#' @title Calculate Likelihood based on Dirichlet-multinomial distribution estimated parameters
#'
#' @description
#' This function estimates parameters from Dirichlet-multinomial distribution.
#' @param FS_out An object from the FeatureSelection()
#' @param testSet A test set in data frame or matrix form. The colnames should have the same bacteria (features) as in the training set.
#' @param col_start An index indicating at which column is the beginning of bacteria (features) data. The default is the 3rd column.
#' @param type_col An index indicating at which column is group/type variable. The default is the 2nd column.
#' @param HighestRank The top number of features inclueded in model. The default is all the features left after filtering.
#' @return A data frame with 17 columns, each row represents a model estimation output.
#' @export
#' @examples
#' data(training)
#'
#' #### Take one row as testSet ####
#' idx <- sample(1:nrow(training),1)
#' test <- training[idx,]
#' train <- training[-idx,]
#'
#' CalPrb(FS(train),test) # This may take up to one minute

CalPrb <- function(FS_out=FS_out,testSet=testSet,col_start=3,type_col=2,HighestRank=nrow(FS_out$Feature)){

  Genus <- FS_out$CountData
  SortP <- FS_out$Feature

  Disease <- levels(Genus[,type_col])

  TotalType1 <- nrow(Genus[Genus[,type_col] == Disease[1], ])
  TotalType2 <- nrow(Genus[Genus[,type_col] == Disease[2], ])
  Prior1 <- TotalType1/(TotalType1+TotalType2)
  Prior2 <- TotalType2/(TotalType1+TotalType2)

  lh <- list()
  for(rank in 2:HighestRank) {
  #for(rank in 2:5) {
print(rank)
    ######## choose the signature taxa and merge the training data

    #NameList = as.vector(P_table[as.numeric(as.character(P_table$P_Wilcoxon))<0.2,]$Genera)
    #select the NameList based on the rank
    NameList <- as.vector(rownames(SortP)[1:rank])
    NewDF <- Genus[,colnames(Genus) %in% NameList]
    NewDF2 <- Genus[,!colnames(Genus)%in% NameList]

    col_end2 <- dim(NewDF2)[2]
    NewDF2$Others <- rowSums(NewDF2[, col_start:col_end2])
    #NewDFTotal = data.frame(NewDF2[, 1:(col_start-1)],NewDF, NewDF2$Others)
    NewDFTotal <- data.frame(NewDF2[, 1:(col_start-1)],NewDF)
    ###### merge in test row

    NewTestSignature <- testSet[, colnames(Genus) %in% NameList]
    NewTestNonSignature <- testSet[, !colnames(Genus)%in% NameList]
    NewTestNonSignature$Others <- rowSums(NewTestNonSignature[, col_start:col_end2])
    NewTestTotal <- data.frame(NewTestNonSignature[, 1:(col_start-1)],NewTestSignature)
    #NewTestTotal = data.frame(NewTestNonSignature[, 1:(col_start-1)],NewTestSignature, NewTestNonSignature$Others)

    rep2_Type1 <- NewDFTotal[NewDFTotal[,type_col] ==Disease[1],]
    rep2_Type2 <- NewDFTotal[NewDFTotal[,type_col] ==Disease[2],]

    ############Estimate Zero Inflated Generalized Dirchlet-Multinomial parameters
    tol <-0.0001
    max.iter<-1000

     ###### Estimate the parameters from the control data
    Xc <- as.matrix(rep(1, dim(rep2_Type1)[1]))
    Xa <- Xc
    Xb <- Xc
    c0 <- as.matrix(rep(1, dim(rep2_Type1)[2]-col_start))
    alpha0 <- c0
    beta0 <- c0

    ZIGDM_est1 <-ZIGDM.EM.PAR2(rep2_Type1[,-(1:(col_start-1))], Xc, Xa, Xb, c0, alpha0, beta0, tol=0.0001, max.iter=1000)

 #   ZIGDM_est1 <-ZIGDM.EM.PAR2(rep2_Type1[,-c((1:(col_start-1)), ncol(rep2_Type1))], Xc, Xa, Xb, c0, alpha0, beta0, tol=0.0001, max.iter=1000)
    ###p_Type1 <- ZIGDM_prob(rep2_Type1[,-(1:(col_start-1))], Xc, Xa, Xb, c0, alpha0, beta0, tol=0.0001, max.iter=1000)
    #fit3 <- dirmult(rep2_Type1[,-(1:(col_start-1))],epsilon=10^(-4),trace=FALSE)

    ###### Estimate the paramenters from the baseline data
    Xc <- as.matrix(rep(1, nrow(rep2_Type2)))
    Xa <- Xc
    Xb <- Xc
    c0 <- as.matrix(rep(1, ncol(rep2_Type1)-col_start))
    alpha0 <- c0
    beta0 <- c0

    ZIGDM_est2<-ZIGDM.EM.PAR2(rep2_Type2[,-(1:(col_start-1))], Xc, Xa, Xb, c0, alpha0, beta0, tol=0.0001, max.iter=1000)

    ##p_Type2 <- ZIGDM_prob(rep2_Type2[,-(1:(col_start-1))], Xc, Xa, Xb, c0, alpha0, beta0, tol=0.0001, max.iter=1000)
    #fit4 <- dirmult(rep2_Type2[,-(1:(col_start-1))],epsilon=10^(-4),trace=FALSE)

    #########Calculate the likelihood of the test sample being Type1 and Type2
######error p_Type1 has 7 number as 7 taxa? while only 2 taxa in
    #p_Type1 <- ZIGDM_prob(ZIGDM_est1, row = Xc[1,])
    #p_Type2 <- ZIGDM_prob(ZIGDM_est2, row = Xc[1,])

    testlist <- list()

    for (r in 1:nrow(testSet)){
      repeatlist <- list()
      for (rep in 1:10) {
        ##### Calculate zero inflated generalized Dirichlet multinomial probability mass function P(x|Type1)
        p_Type1 <- ZIGDM_prob(ZIGDM_est1, row = Xc[1,])
        p_Type2 <- ZIGDM_prob(ZIGDM_est2, row = Xc[1,])
        normalizeTest = NewTestTotal[r,-(1:(col_start-1))]/sum(NewTestTotal[r,-(1:(col_start-1))])*100
        if(length(normalizeTest) == 2) {
          probability_Type1 = apply(round(normalizeTest[1]),1,dbinom, 100, prob = p_Type1[1])
        }else{
        probability_Type1 = apply(normalizeTest,1,dmultinom, prob = p_Type1)
        }
        lhP_Type1 = probability_Type1*Prior1

        #pdfln_Type1 <- ddirmn(NewTestTotal[r,-(1:(col_start-1))], t(as.matrix(p_Type1)))
        #lh_Type1=exp(pdfln_Type1)
        #lhP_Type1 = lh_Type1*Prior1

        ##### Calculate log of Dirichlet multinomial probability mass function P(x|Type2)
        if(length(normalizeTest) == 2) {
          probability_Type2 = apply(round(normalizeTest[1]),1,dbinom, 100, prob = p_Type2[1])
        }else{
          probability_Type2 = apply(normalizeTest,1,dmultinom, prob = p_Type2)
        }

        lhP_Type2 = probability_Type2*Prior2

        #pdfln_Type2 <- ddirmn(NewTestTotal[r,-(1:(col_start-1))], t(as.matrix(p_Type2)))
        #lh_Type2=exp(pdfln_Type2)
        #lhP_Type2 = lh_Type2*Prior2

        #Pos_Type1 = lh_Type1/(lh_Type1+lh_Type2)
        #Pos_Type2 = lh_Type2/(lh_Type1+lh_Type2)

        PosP_Type1 = lhP_Type1/(lhP_Type1+lhP_Type2)
        PosP_Type2 = lhP_Type2/(lhP_Type1+lhP_Type2)

        #### Create truth labels ####

        if(testSet[r,type_col] == Disease[1]) {
          Type1Label = 1
          Type2Label = 0
        } else if (testSet[r,type_col] == Disease[2]) {
          Type1Label = 0
          Type2Label = 1
        }
      repeatlist[[rep]] <- as.matrix(t(c(Disease[1], Disease[2], rownames(testSet)[r], rank,  PosP_Type1, PosP_Type2, Type1Label, Type2Label,paste(NameList,collapse=";"))))

    }
      RandomRepeat = data.frame(t(sapply(repeatlist,'[')))
      PosP_Type1 = mean(as.numeric(as.character(RandomRepeat$X5)), na.rm = TRUE)
      PosP_Type2 = mean(as.numeric(as.character(RandomRepeat$X6)), na.rm = TRUE)

      testlist[[r]] <- as.matrix(t(c(Disease[1], Disease[2], rownames(testSet)[r], rank,  PosP_Type1, PosP_Type2, Type1Label, Type2Label,paste(NameList,collapse=";"))))

##    lh[[rank]] <- as.matrix(t(c(Disease[1], Disease[2], rownames(testSet), rank, Prior1, Prior2, lh_Type1, lh_Type2, Pos_Type1, Pos_Type2, lhP_Type1, lhP_Type2, PosP_Type1, PosP_Type2, Type1Label, Type2Label,paste(NameList,collapse=";"))))

    lh[[rank]] <- data.frame(t(sapply(testlist,'[')))

  } #end of rank

  lh_table <- data.frame(do.call(rbind,lh))
##  colnames(lh_table) = c("Type1", "Type2", "row", "feature_rank", "Prior1", "Prior2", "lh_Type1", "lh_Type2", "Poster_Type1", "Poster_Type2", "lhP_Type1", "lhP_Type2","Poster_Prio_Type1","Poster_Prio_Type2", "Type1Label", "Type2Label","SelectedFeatures" )

  colnames(lh_table) = c("Type1", "Type2", "row", "feature_rank", "Type1_Posterior_Prb","Type2_Postereior_Prb", "Type1Label", "Type2Label","SelectedFeatures" )
}
  return(lh_table)
}


subroutine addpt(j,nadj,madj,x,y,ntot,eps,ntri,nerror)
# Add point j to the triangulation.
# Called by master, dirseg.

implicit double precision(a-h,o-z)
dimension nadj(-3:ntot,0:madj), x(-3:ntot), y(-3:ntot)
logical didswp

# Put the new point in, joined to the vertices of its
# enclosing triangle.
call initad(j,nadj,madj,x,y,ntot,eps,ntri,nerror)
if(nerror > 0) return

# Look at each `gap', i.e. pair of adjacent segments
# emanating from the new point; they form two sides of a
# quadrilateral; see whether the extant diagonal of this
# quadrilateral should be swapped with its alternative
# (according to the LOP: local optimality principle).
now = nadj(j,1)
nxt = nadj(j,2)
ngap = 0

repeat {
	call swap(j,now,nxt,didswp,nadj,madj,x,y,ntot,eps,nerror)
	if(nerror > 0) return
        n = nadj(j,0)
        if(!didswp) {         # If no swap of diagonals
                now  = nxt    # move to the next gap.
                ngap = ngap+1
        }
        call succ(nxt,j,now,nadj,madj,ntot,nerror)
	if(nerror > 0) return
}
until(ngap==n)

return
end


# 4. faza: Analiza podatkov

## Uvozimo funkcijo za uvoz spletne strani.
#source("lib/xml.r")

# # Preberemo spletno stran v razpredelnico.
# cat("Uvažam spletno stran...\n")
# tabela <- preuredi(uvozi.obcine(), obcine)
#
# # Narišemo graf v datoteko PDF.
# cat("Rišem graf...\n")
# pdf("slike/naselja.pdf", width=6, height=4)
# plot(tabela[[1]], tabela[[4]],
#      main = "Število naselij glede na površino občine",
#      xlab = "Površina (km^2)",
#      ylab = "Št. naselij")
# dev.off()

#1.) Normiran zemljevid Števila športnikov iz posameznih držav na OI 2012

#najprej izračunamo vektor ki vsebuje podatek o športnikih/število prebivalcev
nn<-sportniki*1000000 #število športnikov pomnoženo z 10^6, da vsa števila v legendi niso enaka 0
pop<-svet$pop_est #populacija, podatki iz zemljevida svet

#uredimo
l <- match(svet$name_long, rownames(nn))
norm.sp.svet <- nn[l,]

podatki.za.norm.zemlj<-norm.sp.svet/pop #podatki ki jih bomo uporabili za zemljevid

# Izračunamo max, min, povprečje
#druzine$povprecje <- apply(druzine[1:4], 1, function(x) sum(x*(1:4))/sum(x))
min.norm <- min(podatki.za.norm.zemlj, na.rm=TRUE)
max.norm <- max(podatki.za.norm.zemlj, na.rm=TRUE)
povp.norm<-sum(podatki.za.norm.zemlj,na.rm=TRUE)/length(podatki.za.norm.zemlj)
norm.norm <- (podatki.za.norm.zemlj-min.norm)/(max.norm-min.norm)

# Narišimo zemljevid v PDF.
cat("Rišem normiran zemljevid za OI...\n")
pdf("slike/normiranzemljevid.pdf")

n = 100
barve.norm=rgb(0, 0, 1, (1:n)/n)[unlist(1+(n-1)*norm.norm)]

plot(svet, col = barve.norm)

#naslov
title("Št. športnikov na milijon prebivalcev iz posameznih držav na OI 2012")
#legenda
legend("left", legend = round(seq(min.norm, max.norm, (max.norm-min.norm)/5)),
       fill = rgb(0, 0, 1, (1:6)/6), bg = "white")

#Dodamo imena drzav
#najprej poberemo iz zemljevida svet tiste države ki bi jih radi označili
oznacene<-svet[c(9,23,28,31,136,169),]
#dodamo imena na zemljevid
text(coordinates(oznacene),labels=c("Avstralija","Brazilija","Kanada","Kitajska","Rusija","ZDA"),cex=0.55,col="black")

dev.off()

#2.) Krivulje, ki se najbolj prilegajo številu dogodkov na poletnih in zimskih OI
attach(OI)
pdf("slike/naprednigrafi.pdf",paper="a4r")

#2.1.)modeli: krivulje za št dogodkov poletnih OI

letop<-OI$Leto[Vrsta=="poletne"] #leta poletnih OI
dogp<-OI$Stevilo.dogodkov[Vrsta=="poletne"] #st dogodkov poletnih OI

   #graf
plot(letop,dogp, xlim=c(1896,2050),ylim=c(0,500),
     xlab="Leto",ylab="Število dogodkov",
     main="Napoved za število dogodkov na poletnih OI",pch=20,col="red",type="p",lwd=3.5)

   #premica
linp<-lm(dogp~letop)
abline(linp,col="green")
   #parabola
kvp<-lm(dogp~I(letop^2)+letop)
curve(predict(kvp, data.frame(letop=x)), add = TRUE, col = "purple")
   #loess
loep<-loess(dogp~letop)
curve(predict(loep, data.frame(letop=x)),add=TRUE,col="orange")
   #legenda
legend("topleft", c("Linerana metoda", "Kvadratna metoda","Loess"),lty=c(1,1,1), col = c("green","purple","orange"))
   #Ocenimo prileganje krivulj tako, da izračunamo vsote kvadratov razdalj od napovedanih do dejanskih vrednosti
ostp<-sapply(list(linp, kvp, loep), function(x) sum(x$residuals^2))

#2.2.)modeli: krivulje za št dogodkov zimskih OI

letoz<-OI$Leto[Vrsta=="zimske"] #leta zimskih OI
dogz<-OI$Stevilo.dogodkov[Vrsta=="zimske"] #st dogodkov zimskih OI

   #graf
plot(letoz,dogz,xlim=c(1924,2050),ylim=c(0,200),
     xlab="Leto",ylab="Število dogodkov",
     main="Napoved za število dogodkov na zimskih OI",pch=20,col="blue",type="p",lwd=3.5)

   #premica
linz<-lm(dogz~letoz)
abline(linz,col="green")
   #parabola
kvz<-lm(dogz~I(letoz^2)+letoz)
curve(predict(kvz, data.frame(letoz=x)), add = TRUE, col = "purple")
   #loess
loez<-loess(dogz~letoz)
curve(predict(loez, data.frame(letoz=x)),add=TRUE,col="orange")
   #legenda
legend("topleft", c("Linerana metoda", "Kvadratna metoda","Loess"),lty=c(1,1,1), col = c("green","purple","orange"))
   #Ocenimo prileganje krivulj tako, da izračunamo vsote kvadratov razdalj od napovedanih do dejanskih vrednosti
ostz<-sapply(list(linz, kvz, loez), function(x) sum(x$residuals^2))

detach(OI)
dev.off()

#3.) Razvrstimo države v skupine glede na število športnikov

pdf("slike/skupine.pdf",paper="a4r")

#tabela, ki za države na zemljevidu pove, koliko šprtnikov na milijon prebivalcev so poslale na OI 2012
za10<-data.frame(row.names=svet$name_long,podatki.za.norm.zemlj)
#znebimo se NA vrstic
za10<-za10[-c(7,8,24,39,55,66,89,114,138,141,146,164),]
drz<-svet$name_long[-c(7,8,24,39,55,66,89,114,138,141,146,164)]
#končna tabela, ki jo bom uporabila za razvrščanje v skupine
tab.za.skupine<-data.frame(row.names=drz,za10)

#normaliziramo
X <- scale(as.matrix(tab.za.skupine))
t <- hclust(dist(X),method = "ward")
#narišemo graf
plot(t, hang=-1, cex=0.2, lwd=0.1,main = "Skupine držav")
legend("topleft", c("Skupina 1", "Skupina 2","Skupina 3","Skupina 4","Skupina 5","Skupina 6"),lty=c(1,1,1), col = c("red","yellow","orange","blue","magenta","green"))
text(20,30,"Skupina 1 predstavlja")
text(20,27.7,"državo z najmanj,")
text(20,25.4,"skupina 6 državo z")
text(20,23.1,"največ športniki")
text(20,20.8,"na milijon prebivalcev.")
#določimo skupine
rect.hclust(t,k=6,border=c("red","yellow","magenta","green","orange","blue"))

cutree(t, k=6)

dev.off()


# Wind Manipulation
# 2016-05-05 Ismail SEZEN
# sezenismail@gmail.com

#' rwind: A package for processing wind and related data.
#'
#' The rwind package provides four categories of important functions:
#' \code{\link{deg2comp}}, \code{\link{hd2uv}}, \code{\link{uv2hd}}
#' and \code{\link{rotax}}.
#'
#' @section rwind functions:
#' The rwind functions ...
#'
#' @docType package
#' @name rwind
NULL

index_array <- function(x, dim, value, drop = T) {
  indices <- rep(list(bquote()), length(dim(x)))
  indices[[dim]] <- value
  call <- as.call(c(
    list(as.name("["), quote(x)),
    indices,
    list(drop = drop)))
  eval(call)
}

windhelper_get_args <- function(...) {
  l <- list(...); x <- NULL
  if (length(l) == 1) {
    if (is.array(l[[1]])) x <- l[[1]]
    if (is.data.frame(l[[1]])) x <- as.matrix(l[[1]])
  }
  if (is.null(x)) x <- do.call(c, l)
  if (is.vector(x) && (length(x) %% 2) == 0)
    x <- array(x, dim = c(length(x) / 2, 2))
  return(x)
}

#' Wind direction to compass direction
#'
#' \code{deg2comp} converts horizontal wind speed and direction to uv
#' components of wind.
#'
#' @param x A numeric vector of wind directions in degrees.
#' @return Wind directions converted to compass directions
#'
#' @author Ismail SEZEN, \email{sezenismail@@gmail.com}
#'
#' @examples
#' deg2comp(runif(100, 0, 360))
#' x <- array(runif(1000, 0, 360), dim = c(10, 10, 10))
#' deg2comp(x)
#'
#' @export
deg2comp <- function(x, bins = c("N", "NNE", "NE", "ENE",
                                 "E", "ESE", "SE", "SSE",
                                 "S", "SSW", "SW", "WSW",
                                 "W", "WNW", "NW", "NNW")) {
  x[x == 0] <- 360
  x <- round((x * length(bins) / 360) + .5)
  x[1:length(x)] <- bins[x]
  return(x)
}

#' uv to horizontal speed & direction
#'
#' Convert uv components of wind to horizontal wind speed and direction.
#'
#' @param ... The names of the objects to be converted
#' @param deg Should the resulting wind directions are degree or radian?
#' @param reverse should be added 180 degree (or pi) to the wind directions?
#' @param drop Should be deleted the dimensions of an array which
#'        have only one level?
#' @return Speed and direction of the wind
#'
#' @author Ismail SEZEN, \email{sezenismail@@gmail.com}
#'
#' @examples
#' uv2hd(1, 1)
#' uv2hd(c(1, 1))
#' x <- array(runif(100, -5, 5), dim = c(100, 100, 2))
#' uv2hd(x)
#'
#' @export
uv2hd <- function(..., deg = T, reverse = T, drop = T) {
  x <- windhelper_get_args(...)
  comp_loc <- which(dim(x) == 2, arr.ind = T) # find uv dim order
  if (length(comp_loc) == 0)
    stop("x does not have a dim = 2 represents u and v")
  dx <- dim(x); ldx <- length(dx)
  r <- aperm(x, c(comp_loc, (1:ldx)[-comp_loc])) # get uv dim to start

  num_pi <- if (deg) 180 else pi
  add_pi <- if (reverse) num_pi else 0
  u <- index_array(r, 1, 1)
  v <- index_array(r, 1, 2)
  h <- sqrt(colSums(r ^ 2))
  d <- ((atan2(u / h, v / h) * num_pi / pi) + add_pi)
  if (deg) d <- d  %% 360
  r <- array(c(h, d), dim = dx)

  r[is.nan(r)] <- 0
  dmn <- dimnames(x)
  if (is.null(dmn)) {
    dimnames(r)[[comp_loc]] <- c("h", "d")
  } else {
    dimnames(r) <- dmn
  }
  if (drop) r <- drop(r)
  return(r)
}

#' Horizontal wind speed & direction to uv
#'
#' Convert horizontal wind speed and direction to uv components of wind.
#'
#' @param ... The names of the objects to be converted
#' @param deg Should the resulting wind directions are degree or radian?
#' @param drop Should be deleted the dimensions of an array which
#'        have only one level?
#' @return uv components of the wind
#'
#' @author Ismail SEZEN, \email{sezenismail@@gmail.com}
#'
#' @examples
#' hd2uv(5, 30)
#' hd2uv(c(5, 30))
#' x <- matrix(c(runif(100, -5, 5), round(runif(100, 0, 360), 2)), ncol = 2,
#'             dimnames = list(NULL, c("h","d")))
#' hd2uv(x)
#'
#' @export
hd2uv <- function(..., deg = T, drop = T) {
  x <- windhelper_get_args(...)
  comp_loc <- which(dim(x) == 2, arr.ind = T) # find uv dim order
  if (length(comp_loc) == 0)
    stop("x does not have a dim = 2 represents h and d")
  dx <- dim(x); ldx <- length(dx)
  r <- aperm(x, c(comp_loc, (1:ldx)[-comp_loc])) # get uv dim to start

  num_pi <- if (deg) 180 else pi
  h <- index_array(r, 1, 1)
  d <- index_array(r, 1, 2)
  d <- (d - num_pi) / num_pi
  u <- h * sinpi(d)
  v <- h * cospi(d)
  r <- array(c(u, v), dim = dx)

  r[is.nan(r)] <- 0
  dmn <- dimnames(x)
  if (is.null(dmn)) {
    dimnames(r)[[comp_loc]] <- c("u", "v")
  } else {
    dimnames(r) <- dmn
  }
  if (drop) r <- drop(r)
  return(r)
}

#' Rotate coordinate axis for uv wind components
#'
#' Rotates coordinate axes for uv components of the wind. uv values will be
#' re-calculated based on new axis. For instance, rotating axis 30 degrees
#' in clockwise means new and old positive y-directions will have 30 degrees
#'  between them.
#'
#'
#' @param ... The names of the objects to be converted.
#' @param alfa Rotation angle in degrees or radians.
#' @param deg TRUE if alfa is degree.
#' @param right TRUE if rotation is in clockwise.
#' @param drop Should be deleted the dimensions of an array which have
#'        only one level?
#' @return Rotated uv components
#'
#' @author Ismail SEZEN, \email{sezenismail@@gmail.com}
#'
#' @examples
#' rotax(1, 1)
#' rotax(1, 1, alfa = 120)
#' x <- array(runif(100, -5, 5), dim = c(100, 100, 2))
#' rotax(x)
#'
#' @export
rotax <- function(..., alfa = 45, deg = T, right = T, drop = T) {
  x <- windhelper_get_args(...)
  comp_loc <- which(dim(x) == 2, arr.ind = T) # find uv dim order
  if (length(comp_loc) == 0)
    stop("x does not have a dim = 2 represents u and v")

  u <- index_array(x, comp_loc, 1)
  v <- index_array(x, comp_loc, 2)
  a <- if (deg) alfa / 180 else alfa / pi
  c <- cospi(a); s <- sinpi(a)
  if (right) {
    un <- u * c - v * s
    vn <- u * s + v * c
  } else {
    un <- u * c + v * s
    vn <- v * c - u * s
  }
  dm <- dim(x); dn <- dimnames(x)
  x <- array(c(un, vn), dim = dm)
  dimnames(x) <- dn
  if (drop) x <- drop(x)
  return(x)
}


# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

#' Calculate Kinship matrix by VanRaden method
#'
#' Build date: Dec 12, 2016
#' Last update: Dec 12, 2019
#'
#' @param M Genotype, m * n, m is marker size, n is population size
#' @param priority speed or memory
#' @param cpu the number of cpu
#' @param verbose whether to print detail.
#'
#' @return K, n * n matrix
#' @export
#'
#' @examples
#' genoPath <- system.file("extdata", "06_mvp-impute", "mvp.imp.geno.desc", package = "rMVP")
#' genotype <- attach.big.matrix(genoPath)
#' print(dim(genotype))
#'
#' K <- MVP.K.VanRaden(genotype)
#'
MVP.K.VanRaden <-
function(
    M,
    priority=c("speed", "memory"),
    cpu=1,
    verbose=TRUE
){
    # R.ver <- Sys.info()[['sysname']]
    # wind <- R.ver == 'Windows'
    # linux <- R.ver == 'Linux'
    # mac <- (!linux) & (!wind)
    r.open <- eval(parse(text = "!inherits(try(Revo.version,silent=TRUE),'try-error')"))

    # if(r.open && mac){
    #     Sys.setenv("VECLIB_MAXIMUM_THREADS" = "1")
    # }

    if (!is.big.matrix(M)) stop("Format of Genotype Data must be big.matrix")
    if(hasNA(M@address))   stop("NA is not allowed in genotype, use 'MVP.Data.impute' to impute.")

    # logging.log("Relationship matrix mode in", priority[1], "\n", verbose = verbose)
    # if(is.null(dim(M))) M <- t(as.matrix(M))
    switch(
        match.arg(priority),
        "speed" = {
            # if (!is.matrix(M)) M <- as.matrix(M)
            # n <- ncol(M)
            # m <- nrow(M)
            # Pi <- 0.5 * rowMeans(M)
            # logging.log("Scale the genotype matrix", "\n", verbose = verbose)
            # M <- M - 2 * Pi
            # SUM <- sum(Pi * (1 - Pi))
            # logging.log("Computing Z'Z", "\n", verbose = verbose)

            K <- try(kin_cal_s(M@address, threads = cpu, verbose = verbose, mkl = r.open), silent=TRUE)

            if(inherits(K,"try-error")){
                logging.log("Out of memory, please set parameter (..., priority='memory') and try again.", "\n", verbose = verbose)
                stop(K[[1]])
            }
        },

        "memory" = {
            K <- kin_cal_m(M@address, threads=cpu, verbose = verbose)
            # n <- ncol(M)
            # m <- nrow(M)
            # bac <- paste0("Z", memo, ".temp.bin")
            # des <- paste0("Z", memo, ".temp.desc")
            # if (file.exists(bac)) file.remove(bac)
            # if (file.exists(des)) file.remove(des)
            # #options(bigmemory.typecast.warning=FALSE)
            # Z <- big.matrix(
            #     nrow = m,
            #     ncol = n,
            #     type = "double",
            #     backingfile = bac,
            #     descriptorfile = des,
            #     init = 0.1
            # )
            # Pi <- NULL
            # estimate.memory <- function(dat, integer=FALSE, raw=FALSE){
            #     cells.per.gb <- 2^27  # size of double() resulting in ~1GB of memory use by R 2.15
            #     dimz <- dat
            #     if(length(dimz) == 1) { dimz[2] <- 1 }
            #     if(length(dimz)>1 & length(dimz)<11 & is.numeric(dimz)) {
            #         total.size <- as.double(1)
            #         for(cc in 1:length(dimz)) { total.size <- as.double(total.size * as.double(dimz[cc])) }
            #         memory.estimate <- as.double(as.double(total.size)/cells.per.gb)
            #         memory.estimate <- memory.estimate
            #         if(integer) { memory.estimate <- memory.estimate/2 } else { if(raw) { memory.estimate <- memory.estimate/8 } }
            #         return(memory.estimate)
            #     } else {
            #         # guessing this is a vector
            #         if(!is.list(dimz) & is.vector(dimz)) {
            #             LL <- length(dimz)
            #             return(estimate.memory(LL, integer=integer, raw=raw))
            #         } else {
            #             warning("tried to estimate memory for object which is neither a vector, pair of dimension sizes or a dataframe/matrix")
            #         }
            #     }
            # }
            # if((Sys.info()[['sysname']]) == 'Windows'){
            #     max.gb <- memory.limit()/1000
            # }else{
            #     max.gb <- Inf
            # }
            # maxLines.gb <- estimate.memory(c(maxLine, n))
            # if(maxLines.gb > max.gb) stop("Memory limited! Please reset the 'maxLine'")
            # loop.index <- seq(0, m, maxLine)[-1]
            # if(max(loop.index) < m) loop.index <- c(loop.index, m)
            # loop.len <- length(loop.index)
            # print("Z assignment...")
            # for(cc in 1:loop.len){
            #     if(loop.len == 1){
            #         c1 <- 1
            #     }else{
            #         c1 <- ifelse(cc == loop.len, (loop.index[cc-1]) + 1, loop.index[cc]-maxLine + 1)
            #     }
            #     c2 <- loop.index[cc]
            #     means <-rowMeans(M[c1:c2, 1:n])
            #     if(!is.null(weight)){
            #         Z[c1:c2, 1:n] <- (M[c1:c2, 1:n]-means) * sqrt(weight[c1:c2])
            #     }else{
            #         Z[c1:c2, 1:n] <- M[c1:c2, 1:n]-means
            #     }
            #     Pi <- c(Pi, 0.5 * means);gc()
            # }
            # print("Assignment DONE!")
            # if(is.null(SUM)){
            #     SUM <- sum(Pi * (1-Pi))
            # }
            # fl.suc <- flush(Z)
            # if(!fl.suc){ stop("flush failed\n") }
            # RR <- describe(Z); rm(list=c("Z", "Pi", "means")); gc()
            # Z <- attach.big.matrix(RR)
            # print("Computing Z'Z in big.matrix...")
            # K <- 0.5 * big.crossprod(Z)/SUM
            # rm(Z)
            # gc()
            # unlink(c(paste0("Z", memo, ".temp.bin"), paste0("Z", memo, ".temp.desc")), recursive = TRUE)
        }
    )
    #print("K Preparation is Done!")
    logging.log("Deriving relationship matrix successfully", "\n", verbose = verbose); gc()
    return(K)
}#end of MVP.k.VanRaden function


#########
# Rafael Aparecido Martins Frade - Central European University
# Replication code for paper:
#     Does payroll tax relief increase employment level? Evidence from Brazil - 2020
# This script runs the regressions
#########

setwd('/home/rafael/arquivos/mestrado/2_term/metrics_II/paper/dataset')
set.seed(123456)
require(data.table)
library(stargazer)

# Descriptive table
firmsNS2010 = fread('firms2010.csv')
firmsNS2011 = fread('firms2011.csv')
firmsNS2012 = fread('firms2012.csv')
firmsNS2013 = fread('firms2013.csv')

summary(firmsNS2013[eligible == 1 & Ind.Simples == 0]$Qtd.Vínculos.CLT)
summary(firmsNS2013[eligible == 0 & Ind.Simples == 0]$Qtd.Vínculos.CLT)
summary(firmsNS2013[eligible == 1 & Ind.Simples == 1]$Qtd.Vínculos.CLT)
summary(firmsNS2013[eligible == 0 & Ind.Simples == 1]$Qtd.Vínculos.CLT)

firmsNS2013[eligible == 1 & Ind.Simples == 0, .N]
firmsNS2013[eligible == 0 & Ind.Simples == 0, .N]
firmsNS2013[eligible == 1 & Ind.Simples == 1, .N]
firmsNS2013[eligible == 0 & Ind.Simples == 1, .N]

# Non-SIMPLES
firmsNS2010 = fread('firms2010.csv')[Ind.Simples == 0]
firmsNS2011 = fread('firms2011.csv')[Ind.Simples == 0]
firmsNS2012 = fread('firms2012.csv')[Ind.Simples == 0]
firmsNS2013 = fread('firms2013.csv')[Ind.Simples == 0]

firmsNS2012$year = 1
firmsModel = rbind(firmsNS2010, firmsNS2011, firmsNS2012)
firmsModel$treated = firmsModel$eligible
modelNS12 = lm(log(Qtd.Vínculos.CLT) ~ year*treated, data = firmsModel)

firmsNS2012$year = 0
firmsModel = rbind(firmsNS2010, firmsNS2011, firmsNS2012, firmsNS2013)
firmsModel$treated = firmsModel$eligible
modelNS13 = lm(log(Qtd.Vínculos.CLT) ~ year*treated, data = firmsModel)

summary(firmsModel)

teste[eligible == 1 & Ind.Simples == 1, .N]
teste[eligible == 1 & Ind.Simples == 0, .N]
teste[eligible == 0 & Ind.Simples == 1, .N]
teste[eligible == 0 & Ind.Simples == 0, .N]

#SIMPLES
firmsS2010 = fread('firms2010.csv')[Qtd.Vínculos.CLT <= 100]
firmsS2011 = fread('firms2011.csv')[Qtd.Vínculos.CLT <= 100]
firmsS2012 = fread('firms2012.csv')[Qtd.Vínculos.CLT <= 100]
firmsS2013 = fread('firms2013.csv')[Qtd.Vínculos.CLT <= 100]

firmsS2012$year = 1
firmsModel = rbind(firmsS2010, firmsS2011, firmsS2012)
firmsModel$treated = as.numeric(!firmsModel$Ind.Simples)
modelSNS12 = lm(log(Qtd.Vínculos.CLT) ~ year*treated, data = firmsModel[eligible == 1])

firmsS2012$year = 0
firmsModel = rbind(firmsS2010, firmsS2011, firmsS2012, firmsS2013)
firmsModel$treated = as.numeric(!firmsModel$Ind.Simples)
modelSNS13 = lm(log(Qtd.Vínculos.CLT) ~ year*treated, data = firmsModel[eligible == 1])

stargazer(modelSNS12, modelSNS13,modelNS12, modelNS13,
          title = "Diff in Diff results",
          dep.var.labels = "Formal Jobs",
          column.separate = 1,
          column.labels = c("SIMPLES 12", "SIMPLES 13", "Standard 12", "Standard 13"),
          covariate.labels = c(
            "Year", "Treated", "Year.Treated (ATE)", "Intercept"),
          digits = 2,
          df = FALSE)

data2011 = fread('firms2011.csv')
groups2011NS = data2011[eligible == 1 & Ind.Simples == 0 ,sum(Qtd.Vínculos.CLT), by = CNAE.2.0.Classe]
groups2011Simples = data2011[eligible == 1 & Ind.Simples == 1 ,sum(Qtd.Vínculos.CLT), by = CNAE.2.0.Classe]

data2012 = fread('firms2012.csv')
data2012$year = 1
groups2012NS = data2012[eligible == 1 & Ind.Simples == 0 ,sum(Qtd.Vínculos.CLT), by = CNAE.2.0.Classe]
groups2012Simples = data2012[eligible == 1 & Ind.Simples == 1 ,sum(Qtd.Vínculos.CLT), by = CNAE.2.0.Classe]

data2013 = fread('firms2013.csv')
groups2013NS = data2013[eligible == 1 & Ind.Simples == 0 ,sum(Qtd.Vínculos.CLT), by = CNAE.2.0.Classe]
groups2013Simples = data2013[eligible == 1 & Ind.Simples == 1 ,sum(Qtd.Vínculos.CLT), by = CNAE.2.0.Classe]

differenceFirmsSimples1211 = numeric(0)
differenceFirmsNSimples1211 = numeric(0)

differenceFirmsSimples1312 = numeric(0)
differenceFirmsNSimples1312 = numeric(0)

for (index in 1:154) {

  cnae = groups2012Simples[index]$CNAE.2.0.Classe

  firmsSimples2011 = groups2011Simples[CNAE.2.0.Classe == cnae]$V1
  firmsSimples2012 = groups2012Simples[CNAE.2.0.Classe == cnae]$V1
  firmsSimples2013 = groups2013Simples[CNAE.2.0.Classe == cnae]$V1

  firmsNSimples2011 = groups2011NS[CNAE.2.0.Classe == cnae]$V1
  firmsNSimples2012 = groups2012NS[CNAE.2.0.Classe == cnae]$V1
  firmsNSimples2013 = groups2013NS[CNAE.2.0.Classe == cnae]$V1

  differenceFirmsSimples1211[index] = firmsSimples2012 - firmsSimples2011
  differenceFirmsNSimples1211[index] = firmsNSimples2012 - firmsNSimples2011

  differenceFirmsSimples1312[index] = firmsSimples2013 - firmsSimples2012
  differenceFirmsNSimples1312[index] = firmsNSimples2013 - firmsNSimples2012
  print(paste(cnae, ))
}
$Qtd.Vínculos.CLT
printQtt = function(cnae, diffS1211, diffNS1211, diffS1312, diffNS1312) {

}

sum(differenceFirmsNSimples1211[1:152])
sum(differenceFirmsSimples1211[1:152])
sum(differenceFirmsNSimples1312[1:151])
sum(differenceFirmsSimples1312[1:151])

diffNS1211 = data.frame(jobs = differenceFirmsNSimples1211, year = 0, simples = 0)
diffS1211 =  data.frame(jobs = differenceFirmsSimples1211, year = 0, simples = 1)

diffNS1312 = data.frame(jobs = differenceFirmsNSimples1312, year = 1, simples = 0)
diffS1312 =  data.frame(jobs = differenceFirmsSimples1312, year = 1, simples = 1)

diffData = rbind(diffNS1211, diffS1211, diffNS1312, diffS1312)
diffData$eligible = as.numeric(!diffData$simples)
diffData$year_treated = diffData$treated*diffData$year

modelDiff = lm(log(jobs) ~ eligible*year, data = diffData)
summary(modelDiff)


emma.kinship <- function(snps, method="additive", use="all") {
  n0 <- sum(snps==0,na.rm=TRUE)
  nh <- sum(snps==0.5,na.rm=TRUE)
  n1 <- sum(snps==1,na.rm=TRUE)
  nNA <- sum(is.na(snps))

  stopifnot(n0+nh+n1+nNA == length(snps))

  if ( method == "dominant" ) {
    flags <- matrix(as.double(rowMeans(snps,na.rm=TRUE) > 0.5),nrow(snps),ncol(snps))
    snps[!is.na(snps) & (snps == 0.5)] <- flags[!is.na(snps) & (snps == 0.5)]
  }
  else if ( method == "recessive" ) {
    flags <- matrix(as.double(rowMeans(snps,na.rm=TRUE) < 0.5),nrow(snps),ncol(snps))
    snps[!is.na(snps) & (snps == 0.5)] <- flags[!is.na(snps) & (snps == 0.5)]
  }
  else if ( ( method == "additive" ) && ( nh > 0 ) ) {
    dsnps <- snps
    rsnps <- snps
    flags <- matrix(as.double(rowMeans(snps,na.rm=TRUE) > 0.5),nrow(snps),ncol(snps))
    dsnps[!is.na(snps) & (snps==0.5)] <- flags[!is.na(snps) & (snps==0.5)]
    flags <- matrix(as.double(rowMeans(snps,na.rm=TRUE) < 0.5),nrow(snps),ncol(snps))
    rsnps[!is.na(snps) & (snps==0.5)] <- flags[!is.na(snps) & (snps==0.5)]
    snps <- rbind(dsnps,rsnps)
  }

  if ( use == "all" ) {
    mafs <- matrix(rowMeans(snps,na.rm=TRUE),nrow(snps),ncol(snps))
    snps[is.na(snps)] <- mafs[is.na(snps)]
  }
  else if ( use == "complete.obs" ) {
    snps <- snps[rowSums(is.na(snps))==0,]
  }

  n <- ncol(snps)
  K <- matrix(nrow=n,ncol=n)
  diag(K) <- 1

  for(i in 2:n) {
    for(j in 1:(i-1)) {
      x <- snps[,i]*snps[,j] + (1-snps[,i])*(1-snps[,j])
      K[i,j] <- sum(x,na.rm=TRUE)/sum(!is.na(x))
      K[j,i] <- K[i,j]
    }
  }
  return(K)
}

emma.eigen.L <- function(Z,K,complete=TRUE) {
  if ( is.null(Z) ) {
    return(emma.eigen.L.wo.Z(K))
  }
  else {
    return(emma.eigen.L.w.Z(Z,K,complete))
  }
}

emma.eigen.L.wo.Z <- function(K) {
  eig <- eigen(K,symmetric=TRUE)
  return(list(values=eig$values,vectors=eig$vectors))
}

emma.eigen.L.w.Z <- function(Z,K,complete=TRUE) {
  if ( complete == FALSE ) {
    vids <- colSums(Z)>0
    Z <- Z[,vids]
    K <- K[vids,vids]
  }
  eig <- eigen(K%*%crossprod(Z,Z),symmetric=FALSE,EISPACK=TRUE)
  return(list(values=eig$values,vectors=qr.Q(qr(Z%*%eig$vectors),complete=TRUE)))
}

emma.eigen.R <- function(Z,K,X,complete=TRUE) {
  if ( ncol(X) == 0 ) {
    return(emma.eigen.L(Z,K))
  }
  else if ( is.null(Z) ) {
    return(emma.eigen.R.wo.Z(K,X))
  }
  else {
    return(emma.eigen.R.w.Z(Z,K,X,complete))
  }
}

emma.eigen.R.wo.Z <- function(K, X) {
  n <- nrow(X)
  q <- ncol(X)
  S <- diag(n)-X%*%solve(crossprod(X,X))%*%t(X)
  eig <- eigen(S%*%(K+diag(1,n))%*%S,symmetric=TRUE)
  stopifnot(!is.complex(eig$values))
  return(list(values=eig$values[1:(n-q)]-1,vectors=eig$vectors[,1:(n-q)]))
}

emma.eigen.R.w.Z <- function(Z, K, X, complete = TRUE) {
  if ( complete == FALSE ) {
    vids <-  colSums(Z) > 0
    Z <- Z[,vids]
    K <- K[vids,vids]
  }
  n <- nrow(Z)
  t <- ncol(Z)
  q <- ncol(X)

  SZ <- Z - X%*%solve(crossprod(X,X))%*%crossprod(X,Z)
  eig <- eigen(K%*%crossprod(Z,SZ),symmetric=FALSE,EISPACK=TRUE)
  if ( is.complex(eig$values) ) {
    eig$values <- Re(eig$values)
    eig$vectors <- Re(eig$vectors)
  }
  qr.X <- qr.Q(qr(X))
  return(list(values=eig$values[1:(t-q)],
              vectors=qr.Q(qr(cbind(SZ%*%eig$vectors[,1:(t-q)],qr.X)),
                complete=TRUE)[,c(1:(t-q),(t+1):n)]))
}

emma.delta.ML.LL.wo.Z <- function(logdelta, lambda, etas, xi) {
  n <- length(xi)
  delta <- exp(logdelta)
  return( 0.5*(n*(log(n/(2*pi))-1-log(sum((etas*etas)/(lambda+delta))))-sum(log(xi+delta))) )
}

emma.delta.ML.LL.w.Z <- function(logdelta, lambda, etas.1, xi.1, n, etas.2.sq ) {
  t <- length(xi.1)
  delta <- exp(logdelta)
#  stopifnot(length(lambda) == length(etas.1))
  return( 0.5*(n*(log(n/(2*pi))-1-log(sum(etas.1*etas.1/(lambda+delta))+etas.2.sq/delta))-(sum(log(xi.1+delta))+(n-t)*logdelta)) )
}

emma.delta.ML.dLL.wo.Z <- function(logdelta, lambda, etas, xi) {
  n <- length(xi)
  delta <- exp(logdelta)
  etasq <- etas*etas
  ldelta <- lambda+delta
  return( 0.5*(n*sum(etasq/(ldelta*ldelta))/sum(etasq/ldelta)-sum(1/(xi+delta))) )
}

emma.delta.ML.dLL.w.Z <- function(logdelta, lambda, etas.1, xi.1, n, etas.2.sq ) {
  t <- length(xi.1)
  delta <- exp(logdelta)
  etasq <- etas.1*etas.1
  ldelta <- lambda+delta
  return( 0.5*(n*(sum(etasq/(ldelta*ldelta))+etas.2.sq/(delta*delta))/(sum(etasq/ldelta)+etas.2.sq/delta)-(sum(1/(xi.1+delta))+(n-t)/delta) ) )
}

emma.delta.REML.LL.wo.Z <- function(logdelta, lambda, etas) {
  nq <- length(etas)
  delta <-  exp(logdelta)
  return( 0.5*(nq*(log(nq/(2*pi))-1-log(sum(etas*etas/(lambda+delta))))-sum(log(lambda+delta))) )
}

emma.delta.REML.LL.w.Z <- function(logdelta, lambda, etas.1, n, t, etas.2.sq ) {
  tq <- length(etas.1)
  nq <- n - t + tq
  delta <-  exp(logdelta)
  return( 0.5*(nq*(log(nq/(2*pi))-1-log(sum(etas.1*etas.1/(lambda+delta))+etas.2.sq/delta))-(sum(log(lambda+delta))+(n-t)*logdelta)) )
}

emma.delta.REML.dLL.wo.Z <- function(logdelta, lambda, etas) {
  nq <- length(etas)
  delta <- exp(logdelta)
  etasq <- etas*etas
  ldelta <- lambda+delta
  return( 0.5*(nq*sum(etasq/(ldelta*ldelta))/sum(etasq/ldelta)-sum(1/ldelta)) )
}

emma.delta.REML.dLL.w.Z <- function(logdelta, lambda, etas.1, n, t1, etas.2.sq ) {
  t <- t1
  tq <- length(etas.1)
  nq <- n - t + tq
  delta <- exp(logdelta)
  etasq <- etas.1*etas.1
  ldelta <- lambda+delta
  return( 0.5*(nq*(sum(etasq/(ldelta*ldelta))+etas.2.sq/(delta*delta))/(sum(etasq/ldelta)+etas.2.sq/delta)-(sum(1/ldelta)+(n-t)/delta)) )
}

emma.MLE <- function(y, X, K, Z=NULL, ngrids=100, llim=-10, ulim=10,
  esp=1e-10, eig.L = NULL, eig.R = NULL)
{
  n <- length(y)
  t <- nrow(K)
  q <- ncol(X)

#  stopifnot(nrow(K) == t)
  stopifnot(ncol(K) == t)
  stopifnot(nrow(X) == n)

  if ( det(crossprod(X,X)) == 0 ) {
    warning("X is singular")
    return (list(ML=0,delta=0,ve=0,vg=0))
  }

  if ( is.null(Z) ) {
    if ( is.null(eig.L) ) {
      eig.L <- emma.eigen.L.wo.Z(K)
    }
    if ( is.null(eig.R) ) {
      eig.R <- emma.eigen.R.wo.Z(K,X)
    }
    etas <- crossprod(eig.R$vectors,y)

    logdelta <- (0:ngrids)/ngrids*(ulim-llim)+llim
    m <- length(logdelta)
    delta <- exp(logdelta)
    Lambdas <- matrix(eig.R$values,n-q,m) + matrix(delta,n-q,m,byrow=TRUE)
    Xis <- matrix(eig.L$values,n,m) + matrix(delta,n,m,byrow=TRUE)
    Etasq <- matrix(etas*etas,n-q,m)
    LL <- 0.5*(n*(log(n/(2*pi))-1-log(colSums(Etasq/Lambdas)))-colSums(log(Xis)))
    dLL <- 0.5*delta*(n*colSums(Etasq/(Lambdas*Lambdas))/colSums(Etasq/Lambdas)-colSums(1/Xis))

    optlogdelta <- vector(length=0)
    optLL <- vector(length=0)
    if ( dLL[1] < esp ) {
      optlogdelta <- append(optlogdelta, llim)
      optLL <- append(optLL, emma.delta.ML.LL.wo.Z(llim,eig.R$values,etas,eig.L$values))
    }
    if ( dLL[m-1] > 0-esp ) {
      optlogdelta <- append(optlogdelta, ulim)
      optLL <- append(optLL, emma.delta.ML.LL.wo.Z(ulim,eig.R$values,etas,eig.L$values))
    }

    for( i in 1:(m-1) )
      {
        if ( ( dLL[i]*dLL[i+1] < 0-esp*esp ) && ( dLL[i] > 0 ) && ( dLL[i+1] < 0 ) )
        {
          r <- uniroot(emma.delta.ML.dLL.wo.Z, lower=logdelta[i], upper=logdelta[i+1], lambda=eig.R$values, etas=etas, xi=eig.L$values)
          optlogdelta <- append(optlogdelta, r$root)
          optLL <- append(optLL, emma.delta.ML.LL.wo.Z(r$root,eig.R$values, etas, eig.L$values))
        }
      }
#    optdelta <- exp(optlogdelta)
  }
  else {
    if ( is.null(eig.L) ) {
      eig.L <- emma.eigen.L.w.Z(Z,K)
    }
    if ( is.null(eig.R) ) {
      eig.R <- emma.eigen.R.w.Z(Z,K,X)
    }
    etas <- crossprod(eig.R$vectors,y)
    etas.1 <- etas[1:(t-q)]
    etas.2 <- etas[(t-q+1):(n-q)]
    etas.2.sq <- sum(etas.2*etas.2)

    logdelta <- (0:ngrids)/ngrids*(ulim-llim)+llim

    m <- length(logdelta)
    delta <- exp(logdelta)
    Lambdas <- matrix(eig.R$values,t-q,m) + matrix(delta,t-q,m,byrow=TRUE)
    Xis <- matrix(eig.L$values,t,m) + matrix(delta,t,m,byrow=TRUE)
    Etasq <- matrix(etas.1*etas.1,t-q,m)
    #LL <- 0.5*(n*(log(n/(2*pi))-1-log(colSums(Etasq/Lambdas)+etas.2.sq/delta))-colSums(log(Xis))+(n-t)*log(deltas))
    dLL <- 0.5*delta*(n*(colSums(Etasq/(Lambdas*Lambdas))+etas.2.sq/(delta*delta))/(colSums(Etasq/Lambdas)+etas.2.sq/delta)-(colSums(1/Xis)+(n-t)/delta))

    optlogdelta <- vector(length=0)
    optLL <- vector(length=0)
    if ( dLL[1] < esp ) {
      optlogdelta <- append(optlogdelta, llim)
      optLL <- append(optLL, emma.delta.ML.LL.w.Z(llim,eig.R$values,etas.1,eig.L$values,n,etas.2.sq))
    }
    if ( dLL[m-1] > 0-esp ) {
      optlogdelta <- append(optlogdelta, ulim)
      optLL <- append(optLL, emma.delta.ML.LL.w.Z(ulim,eig.R$values,etas.1,eig.L$values,n,etas.2.sq))
    }

    for( i in 1:(m-1) )
      {
        if ( ( dLL[i]*dLL[i+1] < 0-esp*esp ) && ( dLL[i] > 0 ) && ( dLL[i+1] < 0 ) )
        {
          r <- uniroot(emma.delta.ML.dLL.w.Z, lower=logdelta[i], upper=logdelta[i+1], lambda=eig.R$values, etas.1=etas.1, xi.1=eig.L$values, n=n, etas.2.sq = etas.2.sq )
          optlogdelta <- append(optlogdelta, r$root)
          optLL <- append(optLL, emma.delta.ML.LL.w.Z(r$root,eig.R$values, etas.1, eig.L$values, n, etas.2.sq ))
        }
      }
#    optdelta <- exp(optlogdelta)
  }

  maxdelta <- exp(optlogdelta[which.max(optLL)])
  maxLL <- max(optLL)
  if ( is.null(Z) ) {
    maxva <- sum(etas*etas/(eig.R$values+maxdelta))/n
  }
  else {
    maxva <- (sum(etas.1*etas.1/(eig.R$values+maxdelta))+etas.2.sq/maxdelta)/n
  }
  maxve <- maxva*maxdelta

  return (list(ML=maxLL,delta=maxdelta,ve=maxve,vg=maxva))
}

emma.MLE.noX <- function(y, K, Z=NULL, ngrids=100, llim=-10, ulim=10,
  esp=1e-10, eig.L = NULL)
{
  n <- length(y)
  t <- nrow(K)

#  stopifnot(nrow(K) == t)
  stopifnot(ncol(K) == t)

  if ( is.null(Z) ) {
    if ( is.null(eig.L) ) {
      eig.L <- emma.eigen.L.wo.Z(K)
    }
    etas <- crossprod(eig.L$vectors,y)

    logdelta <- (0:ngrids)/ngrids*(ulim-llim)+llim
    m <- length(logdelta)
    delta <- exp(logdelta)
    Xis <- matrix(eig.L$values,n,m) + matrix(delta,n,m,byrow=TRUE)
    Etasq <- matrix(etas*etas,n,m)
    LL <- 0.5*(n*(log(n/(2*pi))-1-log(colSums(Etasq/Xis)))-colSums(log(Xis)))
    dLL <- 0.5*delta*(n*colSums(Etasq/(Xis*Xis))/colSums(Etasq/Xis)-colSums(1/Xis))

    optlogdelta <- vector(length=0)
    optLL <- vector(length=0)
    #print(dLL)
    if ( dLL[1] < esp ) {
      optlogdelta <- append(optlogdelta, llim)
      optLL <- append(optLL, emma.delta.ML.LL.wo.Z(llim,eig.L$values,etas,eig.L$values))
    }
    if ( dLL[m-1] > 0-esp ) {
      optlogdelta <- append(optlogdelta, ulim)
      optLL <- append(optLL, emma.delta.ML.LL.wo.Z(ulim,eig.L$values,etas,eig.L$values))
    }

    for( i in 1:(m-1) )
      {
        #if ( ( dLL[i]*dLL[i+1] < 0 ) && ( dLL[i] > 0 ) && ( dLL[i+1] < 0 ) )
        if ( ( dLL[i]*dLL[i+1] < 0-esp*esp ) && ( dLL[i] > 0 ) && ( dLL[i+1] < 0 ) )
        {
          r <- uniroot(emma.delta.ML.dLL.wo.Z, lower=logdelta[i], upper=logdelta[i+1], lambda=eig.L$values, etas=etas, xi=eig.L$values)
          optlogdelta <- append(optlogdelta, r$root)
          optLL <- append(optLL, emma.delta.ML.LL.wo.Z(r$root,eig.L$values, etas, eig.L$values))
        }
      }
#    optdelta <- exp(optlogdelta)
  }
  else {
    if ( is.null(eig.L) ) {
      eig.L <- emma.eigen.L.w.Z(Z,K)
    }
    etas <- crossprod(eig.L$vectors,y)
    etas.1 <- etas[1:t]
    etas.2 <- etas[(t+1):n]
    etas.2.sq <- sum(etas.2*etas.2)

    logdelta <- (0:ngrids)/ngrids*(ulim-llim)+llim

    m <- length(logdelta)
    delta <- exp(logdelta)
    Xis <- matrix(eig.L$values,t,m) + matrix(delta,t,m,byrow=TRUE)
    Etasq <- matrix(etas.1*etas.1,t,m)
    #LL <- 0.5*(n*(log(n/(2*pi))-1-log(colSums(Etasq/Lambdas)+etas.2.sq/delta))-colSums(log(Xis))+(n-t)*log(deltas))
    dLL <- 0.5*delta*(n*(colSums(Etasq/(Xis*Xis))+etas.2.sq/(delta*delta))/(colSums(Etasq/Xis)+etas.2.sq/delta)-(colSums(1/Xis)+(n-t)/delta))

    optlogdelta <- vector(length=0)
    optLL <- vector(length=0)
    if ( dLL[1] < esp ) {
      optlogdelta <- append(optlogdelta, llim)
      optLL <- append(optLL, emma.delta.ML.LL.w.Z(llim,eig.L$values,etas.1,eig.L$values,n,etas.2.sq))
    }
    if ( dLL[m-1] > 0-esp ) {
      optlogdelta <- append(optlogdelta, ulim)
      optLL <- append(optLL, emma.delta.ML.LL.w.Z(ulim,eig.L$values,etas.1,eig.L$values,n,etas.2.sq))
    }

    for( i in 1:(m-1) )
      {
        if ( ( dLL[i]*dLL[i+1] < 0-esp*esp ) && ( dLL[i] > 0 ) && ( dLL[i+1] < 0 ) )
        {
          r <- uniroot(emma.delta.ML.dLL.w.Z, lower=logdelta[i], upper=logdelta[i+1], lambda=eig.L$values, etas.1=etas.1, xi.1=eig.L$values, n=n, etas.2.sq = etas.2.sq )
          optlogdelta <- append(optlogdelta, r$root)
          optLL <- append(optLL, emma.delta.ML.LL.w.Z(r$root,eig.L$values, etas.1, eig.L$values, n, etas.2.sq ))
        }
      }
#    optdelta <- exp(optlogdelta)
  }

  maxdelta <- exp(optlogdelta[which.max(optLL)])
  maxLL <- max(optLL)
  if ( is.null(Z) ) {
    maxva <- sum(etas*etas/(eig.L$values+maxdelta))/n
  }
  else {
    maxva <- (sum(etas.1*etas.1/(eig.L$values+maxdelta))+etas.2.sq/maxdelta)/n
  }
  maxve <- maxva*maxdelta

  return (list(ML=maxLL,delta=maxdelta,ve=maxve,vg=maxva))
}

emma.REMLE <- function(y, X, K, Z=NULL, ngrids=100, llim=-10, ulim=10,
  esp=1e-10, eig.L = NULL, eig.R = NULL) {
  n <- length(y)
  t <- nrow(K)
  q <- ncol(X)

#  stopifnot(nrow(K) == t)
  stopifnot(ncol(K) == t)
  stopifnot(nrow(X) == n)

  if ( det(crossprod(X,X)) == 0 ) {
    warning("X is singular")
    return (list(REML=0,delta=0,ve=0,vg=0))
  }

  if ( is.null(Z) ) {
    if ( is.null(eig.R) ) {
      eig.R <- emma.eigen.R.wo.Z(K,X)
    }
    etas <- crossprod(eig.R$vectors,y)

    logdelta <- (0:ngrids)/ngrids*(ulim-llim)+llim
    m <- length(logdelta)
    delta <- exp(logdelta)
    Lambdas <- matrix(eig.R$values,n-q,m) + matrix(delta,n-q,m,byrow=TRUE)
    Etasq <- matrix(etas*etas,n-q,m)
    LL <- 0.5*((n-q)*(log((n-q)/(2*pi))-1-log(colSums(Etasq/Lambdas)))-colSums(log(Lambdas)))
    dLL <- 0.5*delta*((n-q)*colSums(Etasq/(Lambdas*Lambdas))/colSums(Etasq/Lambdas)-colSums(1/Lambdas))

    optlogdelta <- vector(length=0)
    optLL <- vector(length=0)
    if ( dLL[1] < esp ) {
      optlogdelta <- append(optlogdelta, llim)
      optLL <- append(optLL, emma.delta.REML.LL.wo.Z(llim,eig.R$values,etas))
    }
    if ( dLL[m-1] > 0-esp ) {
      optlogdelta <- append(optlogdelta, ulim)
      optLL <- append(optLL, emma.delta.REML.LL.wo.Z(ulim,eig.R$values,etas))
    }

    for( i in 1:(m-1) )
      {
        if ( ( dLL[i]*dLL[i+1] < 0-esp*esp ) && ( dLL[i] > 0 ) && ( dLL[i+1] < 0 ) )
        {
          r <- uniroot(emma.delta.REML.dLL.wo.Z, lower=logdelta[i], upper=logdelta[i+1], lambda=eig.R$values, etas=etas)
          optlogdelta <- append(optlogdelta, r$root)
          optLL <- append(optLL, emma.delta.REML.LL.wo.Z(r$root,eig.R$values, etas))
        }
      }
#    optdelta <- exp(optlogdelta)
  }
  else {
    if ( is.null(eig.R) ) {
      eig.R <- emma.eigen.R.w.Z(Z,K,X)
    }
    etas <- crossprod(eig.R$vectors,y)
    etas.1 <- etas[1:(t-q)]
    etas.2 <- etas[(t-q+1):(n-q)]
    etas.2.sq <- sum(etas.2*etas.2)

    logdelta <- (0:ngrids)/ngrids*(ulim-llim)+llim
    m <- length(logdelta)
    delta <- exp(logdelta)
    Lambdas <- matrix(eig.R$values,t-q,m) + matrix(delta,t-q,m,byrow=TRUE)
    Etasq <- matrix(etas.1*etas.1,t-q,m)
    dLL <- 0.5*delta*((n-q)*(colSums(Etasq/(Lambdas*Lambdas))+etas.2.sq/(delta*delta))/(colSums(Etasq/Lambdas)+etas.2.sq/delta)-(colSums(1/Lambdas)+(n-t)/delta))

    optlogdelta <- vector(length=0)
    optLL <- vector(length=0)
    if ( dLL[1] < esp ) {
      optlogdelta <- append(optlogdelta, llim)
      optLL <- append(optLL, emma.delta.REML.LL.w.Z(llim,eig.R$values,etas.1,n,t,etas.2.sq))
    }
    if ( dLL[m-1] > 0-esp ) {
      optlogdelta <- append(optlogdelta, ulim)
      optLL <- append(optLL, emma.delta.REML.LL.w.Z(ulim,eig.R$values,etas.1,n,t,etas.2.sq))
    }

    for( i in 1:(m-1) )
      {
        if ( ( dLL[i]*dLL[i+1] < 0-esp*esp ) && ( dLL[i] > 0 ) && ( dLL[i+1] < 0 ) )
        {
          r <- uniroot(emma.delta.REML.dLL.w.Z, lower=logdelta[i], upper=logdelta[i+1], lambda=eig.R$values, etas.1=etas.1, n=n, t1=t, etas.2.sq = etas.2.sq )
          optlogdelta <- append(optlogdelta, r$root)
          optLL <- append(optLL, emma.delta.REML.LL.w.Z(r$root,eig.R$values, etas.1, n, t, etas.2.sq ))
        }
      }
#    optdelta <- exp(optlogdelta)
  }

  maxdelta <- exp(optlogdelta[which.max(optLL)])
  maxLL <- max(optLL)
  if ( is.null(Z) ) {
    maxva <- sum(etas*etas/(eig.R$values+maxdelta))/(n-q)
  }
  else {
    maxva <- (sum(etas.1*etas.1/(eig.R$values+maxdelta))+etas.2.sq/maxdelta)/(n-q)
  }
  maxve <- maxva*maxdelta

  return (list(REML=maxLL,delta=maxdelta,ve=maxve,vg=maxva))
}

emma.ML.LRT <- function(ys, xs, K, Z=NULL, X0 = NULL, ngrids=100, llim=-10, ulim=10, esp=1e-10, ponly = FALSE) {
  if ( is.null(dim(ys)) || ncol(ys) == 1 ) {
    ys <- matrix(ys,1,length(ys))
  }
  if ( is.null(dim(xs)) || ncol(xs) == 1 ) {
    xs <- matrix(xs,1,length(xs))
  }
  if ( is.null(X0) ) {
    X0 <- matrix(1,ncol(ys),1)
  }

  g <- nrow(ys)
  n <- ncol(ys)
  m <- nrow(xs)
  t <- ncol(xs)
  q0 <- ncol(X0)
  q1 <- q0 + 1

  if ( !ponly ) {
    ML1s <- matrix(nrow=m,ncol=g)
    ML0s <- matrix(nrow=m,ncol=g)
    vgs <- matrix(nrow=m,ncol=g)
    ves <- matrix(nrow=m,ncol=g)
  }
  stats <- matrix(nrow=m,ncol=g)
  ps <- matrix(nrow=m,ncol=g)
  ML0 <- vector(length=g)

  stopifnot(nrow(K) == t)
  stopifnot(ncol(K) == t)
  stopifnot(nrow(X0) == n)

  if ( sum(is.na(ys)) == 0 ) {
    eig.L <- emma.eigen.L(Z,K)
    eig.R0 <- emma.eigen.R(Z,K,X0)

    for(i in 1:g) {
      ML0[i] <- emma.MLE(ys[i,],X0,K,Z,ngrids,llim,ulim,esp,eig.L,eig.R0)$ML
    }

    x.prev <- vector(length=0)

    for(i in 1:m) {
      vids <- !is.na(xs[i,])
      nv <- sum(vids)
      xv <- xs[i,vids]

      if ( ( mean(xv) <= 0 ) || ( mean(xv) >= 1 ) ) {
        if (!ponly) {
          stats[i,] <- rep(NA,g)
          vgs[i,] <- rep(NA,g)
          ves[i,] <- rep(NA,g)
          ML1s[i,] <- rep(NA,g)
          ML0s[i,] <- rep(NA,g)
        }
        ps[i,] = rep(1,g)
      }
      else if ( identical(x.prev, xv) ) {
        if ( !ponly ) {
          stats[i,] <- stats[i-1,]
          vgs[i,] <- vgs[i-1,]
          ves[i,] <- ves[i-1,]
          ML1s[i,] <- ML1s[i-1,]
          ML0s[i,] <- ML0s[i-1,]
        }
        ps[i,] <- ps[i-1,]
      }
      else {
        if ( is.null(Z) ) {
          X <- cbind(X0[vids,,drop=FALSE],xs[i,vids])
          eig.R1 = emma.eigen.R.wo.Z(K[vids,vids],X)
        }
        else {
          vrows <- as.logical(rowSums(Z[,vids]))
          nr <- sum(vrows)
          X <- cbind(X0[vrows,,drop=FALSE],Z[vrows,vids]%*%t(xs[i,vids,drop=FALSE]))
          eig.R1 = emma.eigen.R.w.Z(Z[vrows,vids],K[vids,vids],X)
        }

        for(j in 1:g) {
          if ( nv == t ) {
            MLE <- emma.MLE(ys[j,],X,K,Z,ngrids,llim,ulim,esp,eig.L,eig.R1)
#            MLE <- emma.MLE(ys[j,],X,K,Z,ngrids,llim,ulim,esp,eig.L,eig.R1)
            if (!ponly) {
              ML1s[i,j] <- MLE$ML
              vgs[i,j] <- MLE$vg
              ves[i,j] <- MLE$ve
            }
            stats[i,j] <- 2*(MLE$ML-ML0[j])

          }
          else {
            if ( is.null(Z) ) {
              eig.L0 <- emma.eigen.L.wo.Z(K[vids,vids])
              MLE0 <- emma.MLE(ys[j,vids],X0[vids,,drop=FALSE],K[vids,vids],NULL,ngrids,llim,ulim,esp,eig.L0)
              MLE1 <- emma.MLE(ys[j,vids],X,K[vids,vids],NULL,ngrids,llim,ulim,esp,eig.L0)
            }
            else {
              if ( nr == n ) {
                MLE1 <- emma.MLE(ys[j,],X,K,Z,ngrids,llim,ulim,esp,eig.L)
              }
              else {
                eig.L0 <- emma.eigen.L.w.Z(Z[vrows,vids],K[vids,vids])
                MLE0 <- emma.MLE(ys[j,vrows],X0[vrows,,drop=FALSE],K[vids,vids],Z[vrows,vids],ngrids,llim,ulim,esp,eig.L0)
                MLE1 <- emma.MLE(ys[j,vrows],X,K[vids,vids],Z[vrows,vids],ngrids,llim,ulim,esp,eig.L0)
              }
            }
            if (!ponly) {
              ML1s[i,j] <- MLE1$ML
              ML0s[i,j] <- MLE0$ML
              vgs[i,j] <- MLE1$vg
              ves[i,j] <- MLE1$ve
            }
            stats[i,j] <- 2*(MLE1$ML-MLE0$ML)
          }
        }
        if ( ( nv == t ) && ( !ponly ) ) {
          ML0s[i,] <- ML0
        }
        ps[i,] <- pchisq(stats[i,],1,lower.tail=FALSE)
      }
    }
  }
  else {
    eig.L <- emma.eigen.L(Z,K)
    eig.R0 <- emma.eigen.R(Z,K,X0)

    for(i in 1:g) {
      vrows <- !is.na(ys[i,])
      if ( is.null(Z) ) {
        ML0[i] <- emma.MLE(ys[i,vrows],X0[vrows,,drop=FALSE],K[vrows,vrows],NULL,ngrids,llim,ulim,esp)$ML
      }
      else {
        vids <- colSums(Z[vrows,]>0)

        ML0[i] <- emma.MLE(ys[i,vrows],X0[vrows,,drop=FALSE],K[vids,vids],Z[vrows,vids],ngrids,llim,ulim,esp)$ML
      }
    }

    x.prev <- vector(length=0)

    for(i in 1:m) {
      vids <- !is.na(xs[i,])
      nv <- sum(vids)
      xv <- xs[i,vids]

      if ( ( mean(xv) <= 0 ) || ( mean(xv) >= 1 ) ) {
        if (!ponly) {
          stats[i,] <- rep(NA,g)
          vgs[i,] <- rep(NA,g)
          ves[i,] <- rep(NA,g)
          ML1s[i,] <- rep(NA,g)
          ML0s[,i] <- rep(NA,g)
        }
        ps[i,] = rep(1,g)
      }
      else if ( identical(x.prev, xv) ) {
        if ( !ponly ) {
          stats[i,] <- stats[i-1,]
          vgs[i,] <- vgs[i-1,]
          ves[i,] <- ves[i-1,]
          ML1s[i,] <- ML1s[i-1,]
        }
        ps[i,] = ps[i-1,]
      }
      else {
        if ( is.null(Z) ) {
          X <- cbind(X0,xs[i,])
          if ( nv == t ) {
            eig.R1 = emma.eigen.R.wo.Z(K,X)
          }
        }
        else {
          vrows <- as.logical(rowSums(Z[,vids]))
          X <- cbind(X0,Z[,vids,drop=FALSE]%*%t(xs[i,vids,drop=FALSE]))
          if ( nv == t ) {
            eig.R1 = emma.eigen.R.w.Z(Z,K,X)
          }
        }

        for(j in 1:g) {
#          print(j)
          vrows <- !is.na(ys[j,])
          if ( nv == t ) {
            nr <- sum(vrows)
            if ( is.null(Z) ) {
              if ( nr == n ) {
                MLE <- emma.MLE(ys[j,],X,K,NULL,ngrids,llim,ulim,esp,eig.L,eig.R1)
              }
              else {
                MLE <- emma.MLE(ys[j,vrows],X[vrows,],K[vrows,vrows],NULL,ngrids,llim,ulim,esp)
              }
            }
            else {
              if ( nr == n ) {
                MLE <- emma.MLE(ys[j,],X,K,Z,ngrids,llim,ulim,esp,eig.L,eig.R1)
              }
              else {
                vtids <- as.logical(colSums(Z[vrows,,drop=FALSE]))
                MLE <- emma.MLE(ys[j,vrows],X[vrows,],K[vtids,vtids],Z[vrows,vtids],ngrids,llim,ulim,esp)
              }
            }

            if (!ponly) {
              ML1s[i,j] <- MLE$ML
              vgs[i,j] <- MLE$vg
              ves[i,j] <- MLE$ve
            }
            stats[i,j] <- 2*(MLE$ML-ML0[j])
          }
          else {
            if ( is.null(Z) ) {
              vtids <- vrows & vids
              eig.L0 <- emma.eigen.L(NULL,K[vtids,vtids])
              MLE0 <- emma.MLE(ys[j,vtids],X0[vtids,,drop=FALSE],K[vtids,vtids],NULL,ngrids,llim,ulim,esp,eig.L0)
              MLE1 <- emma.MLE(ys[j,vtids],X[vtids,],K[vtids,vtids],NULL,ngrids,llim,ulim,esp,eig.L0)
            }
            else {
              vtids <- as.logical(colSums(Z[vrows,])) & vids
              vtrows <- vrows & as.logical(rowSums(Z[,vids]))
              eig.L0 <- emma.eigen.L(Z[vtrows,vtids],K[vtids,vtids])
              MLE0 <- emma.MLE(ys[j,vtrows],X0[vtrows,,drop=FALSE],K[vtids,vtids],Z[vtrows,vtids],ngrids,llim,ulim,esp,eig.L0)
              MLE1 <- emma.MLE(ys[j,vtrows],X[vtrows,],K[vtids,vtids],Z[vtrows,vtids],ngrids,llim,ulim,esp,eig.L0)
            }
            if (!ponly) {
              ML1s[i,j] <- MLE1$ML
              vgs[i,j] <- MLE1$vg
              ves[i,j] <- MLE1$ve
              ML0s[i,j] <- MLE0$ML
            }
            stats[i,j] <- 2*(MLE1$ML-MLE0$ML)
          }
        }
        if ( ( nv == t ) && ( !ponly ) ) {
          ML0s[i,] <- ML0
        }
        ps[i,] <- pchisq(stats[i,],1,lower.tail=FALSE)
      }
    }
  }
  if ( ponly ) {
    return (ps)
  }
  else {
    return (list(ps=ps,ML1s=ML1s,ML0s=ML0s,stats=stats,vgs=vgs,ves=ves))
  }
}

emma.test <- function(ys, xs, K, Z=NULL, x0s = NULL, X0 = NULL, dfxs = 1, dfx0s = 1, use.MLE = FALSE, use.LRT = FALSE, ngrids = 100, llim = -10, ulim = 10, esp=1e-10, ponly = FALSE)
{
  stopifnot (dfxs > 0)

  if ( is.null(dim(ys)) || ncol(ys) == 1 ) {
    ys <- matrix(ys,1,length(ys))
  }

  if ( is.null(dim(xs)) || ncol(xs) == 1 ) {
    xs <- matrix(xs,1,length(xs))
  }
  nx <- nrow(xs)/dfxs

  if ( is.null(x0s) ) {
    dfx0s = 0
    x0s <- matrix(NA,0,ncol(xs))
  }
  # X0 automatically contains intercept. If no intercept is to be used,
  #    X0 should be matrix(nrow=ncol(ys),ncol=0)
  if ( is.null(X0) ) {
    X0 <- matrix(1,ncol(ys),1)
  }

  stopifnot(Z == NULL) # The case where Z is not null is not implemented

  ny <- nrow(ys)
  iy <- ncol(ys)
  ix <- ncol(xs)

  stopifnot(nrow(K) == ix)
  stopifnot(ncol(K) == ix)
  stopifnot(nrow(X0) == iy)

  if ( !ponly ) {
    LLs <- matrix(nrow=m,ncol=g)
    vgs <- matrix(nrow=m,ncol=g)
    ves <- matrix(nrow=m,ncol=g)
  }
  dfs <- matrix(nrow=m,ncol=g)
  stats <- matrix(nrow=m,ncol=g)
  ps <- matrix(nrow=m,ncol=g)

  # The case with no missing phenotypes
  if ( sum(is.na(ys)) == 0 ) {
    if ( ( use.MLE ) || ( !use.LRT ) ) {
      eig.L0 <- emma.eigen.L(Z,K)
    }
    if ( dfx0s == 0 ) {
      eig.R0 <- emma.eigen.R(Z,K,X0)
    }
    x.prev <- NULL

    for(i in 1:ix) {
      x1 <- t(xs[(dfxs*(i-1)+1):(dfxs*i),,drop=FALSE])
      if ( dfxs0 == 0 ) {
        x0 <- X0
      }
      else {
        x0 <- cbind(t(x0s[(dfx0s*(i-1)+1):(dfx0s*i),,drop=FALSE]),X0)
      }
      x <-  cbind(x1,x0)
      xvids <- rowSums(is.na(x) == 0)
      nxv <- sum(xvids)
      xv <- x[xvids,,drop=FALSE]
      Kv <- K[xvids,xvids,drop=FALSE]
      yv <- ys[j,xvids]

      if ( identical(x.prev, xv) ) {
        if ( !ponly ) {
          vgs[i,] <- vgs[i-1,]
          ves[i,] <- ves[i-1,]
          dfs[i,] <- dfs[i-1,]
          REMLs[i,] <- REMLs[i-1,]
          stats[i,] <- stats[i-1,]
        }
        ps[i,] <- ps[i-1,]
      }
      else {
        eig.R1 = emma.eigen.R.wo.Z(Kv,xv)

        for(j in 1:iy) {
          if ( ( use.MLE ) || ( !use.LRT ) ) {
            if ( nxv < t ) {
              # NOTE: this complexity can be improved by avoiding eigen computation for identical missing patterns
              eig.L0v <- emma.eigen.L.wo.Z(Kv)
            }
            else {
              eig.L0v <- eig.L0
            }
          }

          if ( use.MLE ) {
            MLE <- emma.REMLE(yv,xv,Kv,NULL,ngrids,llim,ulim,esp,eig.R1)
            stop("Not implemented yet")
          }
          else {
            REMLE <- emma.REMLE(yv,xv,Kv,NULL,ngrids,llim,ulim,esp,eig.R1)
            if ( use.LRT ) {
              stop("Not implemented yet")
            }
            else {
              U <- eig.L0v$vectors * matrix(sqrt(1/(eig.L0v$values+REMLE$delta)),t,t,byrow=TRUE)
              dfs[i,j] <- length(eig.R1$values)
              yt <- crossprod(U,yv)
              xt <- crossprod(U,xv)
              ixx <- solve(crossprod(xt,xt))
              beta <- ixx%*%crossprod(xt,yt)
              if ( dfxs == 1 ) {
                stats[i,j] <- beta[q1]/sqrt(iXX[q1,q1]*REMLE$vg)
              }
              else {
                model.m <- c(rep(1,dfxs),rep(0,ncol(xv)-dfxs))
                stats[i,j] <-
                  crossprod(crossprod(solve(crossprod(crossprod(iXX,model.m),
                                                      model.m)),
                                      model.m*beta),model.m*beta)

              }
              if ( !ponly ) {
                vgs[i,j] <- REMLE$vg
                ves[i,j] <- REMLE$ve
                REMLs[i,j] <- REMLE$REML
              }
            }
          }
        }
        if ( dfxs == 1 ) {
          ps[i,] <- 2*pt(abs(stats[i,]),dfs[i,],lower.tail=FALSE)
        }
        else {
          ps[i,] <- pf(abs(stats[i,]),dfs[i,],lower.tail=FALSE)
        }
      }
    }
  }
  # The case with missing genotypes - not implemented yet
  else {
    stop("Not implemented yet")
    eig.L <- emma.eigen.L(Z,K)
    eig.R0 <- emma.eigen.R(Z,K,X0)

    x.prev <- vector(length=0)

    for(i in 1:m) {
      vids <- !is.na(xs[i,])
      nv <- sum(vids)
      xv <- xs[i,vids]

      if ( ( mean(xv) <= 0 ) || ( mean(xv) >= 1 ) ) {
        if (!ponly) {
          vgs[i,] <- rep(NA,g)
          ves[i,] <- rep(NA,g)
          REMLs[i,] <- rep(NA,g)
          dfs[i,] <- rep(NA,g)
        }
        ps[i,] = rep(1,g)
      }
      else if ( identical(x.prev, xv) ) {
        if ( !ponly ) {
          stats[i,] <- stats[i-1,]
          vgs[i,] <- vgs[i-1,]
          ves[i,] <- ves[i-1,]
          REMLs[i,] <- REMLs[i-1,]
          dfs[i,] <- dfs[i-1,]
        }
        ps[i,] = ps[i-1,]
      }
      else {
        if ( is.null(Z) ) {
          X <- cbind(X0,xs[i,])
          if ( nv == t ) {
            eig.R1 = emma.eigen.R.wo.Z(K,X)
          }
        }
        else {
          vrows <- as.logical(rowSums(Z[,vids,drop=FALSE]))
          X <- cbind(X0,Z[,vids,drop=FALSE]%*%t(xs[i,vids,drop=FALSE]))
          if ( nv == t ) {
            eig.R1 = emma.eigen.R.w.Z(Z,K,X)
          }
        }

        for(j in 1:g) {
          vrows <- !is.na(ys[j,])
          if ( nv == t ) {
            yv <- ys[j,vrows]
            nr <- sum(vrows)
            if ( is.null(Z) ) {
              if ( nr == n ) {
                REMLE <- emma.REMLE(yv,X,K,NULL,ngrids,llim,ulim,esp,eig.R1)
                U <- eig.L$vectors * matrix(sqrt(1/(eig.L$values+REMLE$delta)),n,n,byrow=TRUE)
              }
              else {
                eig.L0 <- emma.eigen.L.wo.Z(K[vrows,vrows,drop=FALSE])
                REMLE <- emma.REMLE(yv,X[vrows,,drop=FALSE],K[vrows,vrows,drop=FALSE],NULL,ngrids,llim,ulim,esp)
                U <- eig.L0$vectors * matrix(sqrt(1/(eig.L0$values+REMLE$delta)),nr,nr,byrow=TRUE)
              }
              dfs[i,j] <- nr-q1
            }
            else {
              if ( nr == n ) {
                REMLE <- emma.REMLE(yv,X,K,Z,ngrids,llim,ulim,esp,eig.R1)
                U <- eig.L$vectors * matrix(c(sqrt(1/(eig.L$values+REMLE$delta)),rep(sqrt(1/REMLE$delta),n-t)),n,n,byrow=TRUE)
              }
              else {
                vtids <- as.logical(colSums(Z[vrows,,drop=FALSE]))
                eig.L0 <- emma.eigen.L.w.Z(Z[vrows,vtids,drop=FALSE],K[vtids,vtids,drop=FALSE])
                REMLE <- emma.REMLE(yv,X[vrows,,drop=FALSE],K[vtids,vtids,drop=FALSE],Z[vrows,vtids,drop=FALSE],ngrids,llim,ulim,esp)
                U <- eig.L0$vectors * matrix(c(sqrt(1/(eig.L0$values+REMLE$delta)),rep(sqrt(1/REMLE$delta),nr-sum(vtids))),nr,nr,byrow=TRUE)
              }
              dfs[i,j] <- nr-q1
            }

            yt <- crossprod(U,yv)
            Xt <- crossprod(U,X[vrows,,drop=FALSE])
            iXX <- solve(crossprod(Xt,Xt))
            beta <- iXX%*%crossprod(Xt,yt)
            if ( !ponly ) {
              vgs[i,j] <- REMLE$vg
              ves[i,j] <- REMLE$ve
              REMLs[i,j] <- REMLE$REML
            }
            stats[i,j] <- beta[q1]/sqrt(iXX[q1,q1]*REMLE$vg)
          }
          else {
            if ( is.null(Z) ) {
              vtids <- vrows & vids
              eig.L0 <- emma.eigen.L.wo.Z(K[vtids,vtids,drop=FALSE])
              yv <- ys[j,vtids]
              nr <- sum(vtids)
              REMLE <- emma.REMLE(yv,X[vtids,,drop=FALSE],K[vtids,vtids,drop=FALSE],NULL,ngrids,llim,ulim,esp)
              U <- eig.L0$vectors * matrix(sqrt(1/(eig.L0$values+REMLE$delta)),nr,nr,byrow=TRUE)
              Xt <- crossprod(U,X[vtids,,drop=FALSE])
              dfs[i,j] <- nr-q1
            }
            else {
              vtids <- as.logical(colSums(Z[vrows,,drop=FALSE])) & vids
              vtrows <- vrows & as.logical(rowSums(Z[,vids,drop=FALSE]))
              eig.L0 <- emma.eigen.L.w.Z(Z[vtrows,vtids,drop=FALSE],K[vtids,vtids,drop=FALSE])
              yv <- ys[j,vtrows]
              nr <- sum(vtrows)
              REMLE <- emma.REMLE(yv,X[vtrows,,drop=FALSE],K[vtids,vtids,drop=FALSE],Z[vtrows,vtids,drop=FALSE],ngrids,llim,ulim,esp)
              U <- eig.L0$vectors * matrix(c(sqrt(1/(eig.L0$values+REMLE$delta)),rep(sqrt(1/REMLE$delta),nr-sum(vtids))),nr,nr,byrow=TRUE)
              Xt <- crossprod(U,X[vtrows,,drop=FALSE])
              dfs[i,j] <- nr-q1
            }
            yt <- crossprod(U,yv)
            iXX <- solve(crossprod(Xt,Xt))
            beta <- iXX%*%crossprod(Xt,yt)
            if ( !ponly ) {
              vgs[i,j] <- REMLE$vg
              ves[i,j] <- REMLE$ve
              REMLs[i,j] <- REMLE$REML
            }
            stats[i,j] <- beta[q1]/sqrt(iXX[q1,q1]*REMLE$vg)

          }
        }
        ps[i,] <- 2*pt(abs(stats[i,]),dfs[i,],lower.tail=FALSE)
      }
    }
  }
  if ( ponly ) {
    return (ps)
  }
  else {
    return (list(ps=ps,REMLs=REMLs,stats=stats,dfs=dfs,vgs=vgs,ves=ves))
  }
}

emma.REML.t <- function(ys, xs, K, Z=NULL, X0 = NULL, ngrids=100, llim=-10, ulim=10, esp=1e-10, ponly = FALSE) {
  if ( is.null(dim(ys)) || ncol(ys) == 1 ) {
    ys <- matrix(ys,1,length(ys))
  }
  if ( is.null(dim(xs)) || ncol(xs) == 1 ) {
    xs <- matrix(xs,1,length(xs))
  }
  if ( is.null(X0) ) {
    X0 <- matrix(1,ncol(ys),1)
  }

  g <- nrow(ys)
  n <- ncol(ys)
  m <- nrow(xs)
  t <- ncol(xs)
  q0 <- ncol(X0)
  q1 <- q0 + 1

  stopifnot(nrow(K) == t)
  stopifnot(ncol(K) == t)
  stopifnot(nrow(X0) == n)

  if ( !ponly ) {
    REMLs <- matrix(nrow=m,ncol=g)
    vgs <- matrix(nrow=m,ncol=g)
    ves <- matrix(nrow=m,ncol=g)
  }
  dfs <- matrix(nrow=m,ncol=g)
  stats <- matrix(nrow=m,ncol=g)
  ps <- matrix(nrow=m,ncol=g)

  if ( sum(is.na(ys)) == 0 ) {
    eig.L <- emma.eigen.L(Z,K)

    x.prev <- vector(length=0)

    for(i in 1:m) {
      vids <- !is.na(xs[i,])
      nv <- sum(vids)
      xv <- xs[i,vids]

      if ( ( mean(xv) <= 0 ) || ( mean(xv) >= 1 ) ) {
        if ( !ponly ) {
          vgs[i,] <- rep(NA,g)
          ves[i,] <- rep(NA,g)
          dfs[i,] <- rep(NA,g)
          REMLs[i,] <- rep(NA,g)
          stats[i,] <- rep(NA,g)
        }
        ps[i,] = rep(1,g)

      }
      else if ( identical(x.prev, xv) ) {
        if ( !ponly ) {
          vgs[i,] <- vgs[i-1,]
          ves[i,] <- ves[i-1,]
          dfs[i,] <- dfs[i-1,]
          REMLs[i,] <- REMLs[i-1,]
          stats[i,] <- stats[i-1,]
        }
        ps[i,] <- ps[i-1,]
      }
      else {
        if ( is.null(Z) ) {
          X <- cbind(X0[vids,,drop=FALSE],xs[i,vids])
          eig.R1 = emma.eigen.R.wo.Z(K[vids,vids],X)
        }
        else {
          vrows <- as.logical(rowSums(Z[,vids]))
          X <- cbind(X0[vrows,,drop=FALSE],Z[vrows,vids,drop=FALSE]%*%t(xs[i,vids,drop=FALSE]))
          eig.R1 = emma.eigen.R.w.Z(Z[vrows,vids],K[vids,vids],X)
        }

        for(j in 1:g) {
          if ( nv == t ) {
            REMLE <- emma.REMLE(ys[j,],X,K,Z,ngrids,llim,ulim,esp,eig.R1)
            if ( is.null(Z) ) {
              U <- eig.L$vectors * matrix(sqrt(1/(eig.L$values+REMLE$delta)),t,t,byrow=TRUE)
              dfs[i,j] <- nv - q1
            }
            else {
              U <- eig.L$vectors * matrix(c(sqrt(1/(eig.L$values+REMLE$delta)),rep(sqrt(1/REMLE$delta),n-t)),n,n,byrow=TRUE)
              dfs[i,j] <- n - q1
            }
            yt <- crossprod(U,ys[j,])
            Xt <- crossprod(U,X)
            iXX <- solve(crossprod(Xt,Xt))
            beta <- iXX%*%crossprod(Xt,yt)

            if ( !ponly ) {
              vgs[i,j] <- REMLE$vg
              ves[i,j] <- REMLE$ve
              REMLs[i,j] <- REMLE$REML
            }
            stats[i,j] <- beta[q1]/sqrt(iXX[q1,q1]*REMLE$vg)
          }
          else {
            if ( is.null(Z) ) {
              eig.L0 <- emma.eigen.L.wo.Z(K[vids,vids])
              nr <- sum(vids)
              yv <- ys[j,vids]
              REMLE <- emma.REMLE(yv,X,K[vids,vids,drop=FALSE],NULL,ngrids,llim,ulim,esp,eig.R1)
              U <- eig.L0$vectors * matrix(sqrt(1/(eig.L0$values+REMLE$delta)),nr,nr,byrow=TRUE)
              dfs[i,j] <- nr - q1
            }
            else {
              eig.L0 <- emma.eigen.L.w.Z(Z[vrows,vids,drop=FALSE],K[vids,vids])
              yv <- ys[j,vrows]
              nr <- sum(vrows)
              tv <- sum(vids)
              REMLE <- emma.REMLE(yv,X,K[vids,vids,drop=FALSE],Z[vrows,vids,drop=FALSE],ngrids,llim,ulim,esp,eig.R1)
              U <- eig.L0$vectors * matrix(c(sqrt(1/(eig.L0$values+REMLE$delta)),rep(sqrt(1/REMLE$delta),nr-tv)),nr,nr,byrow=TRUE)
              dfs[i,j] <- nr - q1
            }
            yt <- crossprod(U,yv)
            Xt <- crossprod(U,X)
            iXX <- solve(crossprod(Xt,Xt))
            beta <- iXX%*%crossprod(Xt,yt)
            if (!ponly) {
              vgs[i,j] <- REMLE$vg
              ves[i,j] <- REMLE$ve
              REMLs[i,j] <- REMLE$REML
            }
            stats[i,j] <- beta[q1]/sqrt(iXX[q1,q1]*REMLE$vg)
          }
        }
        ps[i,] <- 2*pt(abs(stats[i,]),dfs[i,],lower.tail=FALSE)
      }
    }
  }
  else {
    eig.L <- emma.eigen.L(Z,K)
    eig.R0 <- emma.eigen.R(Z,K,X0)

    x.prev <- vector(length=0)

    for(i in 1:m) {
      vids <- !is.na(xs[i,])
      nv <- sum(vids)
      xv <- xs[i,vids]

      if ( ( mean(xv) <= 0 ) || ( mean(xv) >= 1 ) ) {
        if (!ponly) {
          vgs[i,] <- rep(NA,g)
          ves[i,] <- rep(NA,g)
          REMLs[i,] <- rep(NA,g)
          dfs[i,] <- rep(NA,g)
        }
        ps[i,] = rep(1,g)
      }
      else if ( identical(x.prev, xv) ) {
        if ( !ponly ) {
          stats[i,] <- stats[i-1,]
          vgs[i,] <- vgs[i-1,]
          ves[i,] <- ves[i-1,]
          REMLs[i,] <- REMLs[i-1,]
          dfs[i,] <- dfs[i-1,]
        }
        ps[i,] = ps[i-1,]
      }
      else {
        if ( is.null(Z) ) {
          X <- cbind(X0,xs[i,])
          if ( nv == t ) {
            eig.R1 = emma.eigen.R.wo.Z(K,X)
          }
        }
        else {
          vrows <- as.logical(rowSums(Z[,vids,drop=FALSE]))
          X <- cbind(X0,Z[,vids,drop=FALSE]%*%t(xs[i,vids,drop=FALSE]))
          if ( nv == t ) {
            eig.R1 = emma.eigen.R.w.Z(Z,K,X)
          }
        }

        for(j in 1:g) {
          vrows <- !is.na(ys[j,])
          if ( nv == t ) {
            yv <- ys[j,vrows]
            nr <- sum(vrows)
            if ( is.null(Z) ) {
              if ( nr == n ) {
                REMLE <- emma.REMLE(yv,X,K,NULL,ngrids,llim,ulim,esp,eig.R1)
                U <- eig.L$vectors * matrix(sqrt(1/(eig.L$values+REMLE$delta)),n,n,byrow=TRUE)
              }
              else {
                eig.L0 <- emma.eigen.L.wo.Z(K[vrows,vrows,drop=FALSE])
                REMLE <- emma.REMLE(yv,X[vrows,,drop=FALSE],K[vrows,vrows,drop=FALSE],NULL,ngrids,llim,ulim,esp)
                U <- eig.L0$vectors * matrix(sqrt(1/(eig.L0$values+REMLE$delta)),nr,nr,byrow=TRUE)
              }
              dfs[i,j] <- nr-q1
            }
            else {
              if ( nr == n ) {
                REMLE <- emma.REMLE(yv,X,K,Z,ngrids,llim,ulim,esp,eig.R1)
                U <- eig.L$vectors * matrix(c(sqrt(1/(eig.L$values+REMLE$delta)),rep(sqrt(1/REMLE$delta),n-t)),n,n,byrow=TRUE)
              }
              else {
                vtids <- as.logical(colSums(Z[vrows,,drop=FALSE]))
                eig.L0 <- emma.eigen.L.w.Z(Z[vrows,vtids,drop=FALSE],K[vtids,vtids,drop=FALSE])
                REMLE <- emma.REMLE(yv,X[vrows,,drop=FALSE],K[vtids,vtids,drop=FALSE],Z[vrows,vtids,drop=FALSE],ngrids,llim,ulim,esp)
                U <- eig.L0$vectors * matrix(c(sqrt(1/(eig.L0$values+REMLE$delta)),rep(sqrt(1/REMLE$delta),nr-sum(vtids))),nr,nr,byrow=TRUE)
              }
              dfs[i,j] <- nr-q1
            }

            yt <- crossprod(U,yv)
            Xt <- crossprod(U,X[vrows,,drop=FALSE])
            iXX <- solve(crossprod(Xt,Xt))
            beta <- iXX%*%crossprod(Xt,yt)
            if ( !ponly ) {
              vgs[i,j] <- REMLE$vg
              ves[i,j] <- REMLE$ve
              REMLs[i,j] <- REMLE$REML
            }
            stats[i,j] <- beta[q1]/sqrt(iXX[q1,q1]*REMLE$vg)
          }
          else {
            if ( is.null(Z) ) {
              vtids <- vrows & vids
              eig.L0 <- emma.eigen.L.wo.Z(K[vtids,vtids,drop=FALSE])
              yv <- ys[j,vtids]
              nr <- sum(vtids)
              REMLE <- emma.REMLE(yv,X[vtids,,drop=FALSE],K[vtids,vtids,drop=FALSE],NULL,ngrids,llim,ulim,esp)
              U <- eig.L0$vectors * matrix(sqrt(1/(eig.L0$values+REMLE$delta)),nr,nr,byrow=TRUE)
              Xt <- crossprod(U,X[vtids,,drop=FALSE])
              dfs[i,j] <- nr-q1
            }
            else {
              vtids <- as.logical(colSums(Z[vrows,,drop=FALSE])) & vids
              vtrows <- vrows & as.logical(rowSums(Z[,vids,drop=FALSE]))
              eig.L0 <- emma.eigen.L.w.Z(Z[vtrows,vtids,drop=FALSE],K[vtids,vtids,drop=FALSE])
              yv <- ys[j,vtrows]
              nr <- sum(vtrows)
              REMLE <- emma.REMLE(yv,X[vtrows,,drop=FALSE],K[vtids,vtids,drop=FALSE],Z[vtrows,vtids,drop=FALSE],ngrids,llim,ulim,esp)
              U <- eig.L0$vectors * matrix(c(sqrt(1/(eig.L0$values+REMLE$delta)),rep(sqrt(1/REMLE$delta),nr-sum(vtids))),nr,nr,byrow=TRUE)
              Xt <- crossprod(U,X[vtrows,,drop=FALSE])
              dfs[i,j] <- nr-q1
            }
            yt <- crossprod(U,yv)
            iXX <- solve(crossprod(Xt,Xt))
            beta <- iXX%*%crossprod(Xt,yt)
            if ( !ponly ) {
              vgs[i,j] <- REMLE$vg
              ves[i,j] <- REMLE$ve
              REMLs[i,j] <- REMLE$REML
            }
            stats[i,j] <- beta[q1]/sqrt(iXX[q1,q1]*REMLE$vg)

          }
        }
        ps[i,] <- 2*pt(abs(stats[i,]),dfs[i,],lower.tail=FALSE)
      }
    }
  }
  if ( ponly ) {
    return (ps)
  }
  else {
    return (list(ps=ps,REMLs=REMLs,stats=stats,dfs=dfs,vgs=vgs,ves=ves))
  }
}


#######################################################################################################
#   Kalman filter and smoother function based on Koopman and Durbin's KFAS package
#   y_t = Z_t * alpha_t + eps_t (observation equation)
#   alpha_t+1 = T_t * alpha_t + R_t * eta_t(transition equation)
#   Note the different time indexing for transition equation
#   The y_t,y_{t-1} stacked vector with parameters vectors reconfigured to output the
#     smoothed lag-1 covariances needed by the EM algorithm
#######################################################################################################
MARSSkfas <- function(MLEobj, only.logLik = FALSE, return.lag.one = TRUE, return.kfas.model = FALSE) {
  MODELobj <- MLEobj[["marss"]]
  control <- MLEobj[["control"]]
  diffuse <- MODELobj[["diffuse"]]
  model.dims <- attr(MODELobj, "model.dims")

  n <- model.dims$data[1]
  TT <- model.dims$data[2]
  m <- model.dims$x[1]
  g1 <- model.dims$Q[1]
  h1 <- model.dims$R[1]
  l1 <- model.dims$L[1]
  par.1 <- parmat(MLEobj, t = 1)
  t.B <- matrix(par.1$B, m, m, byrow = TRUE)
  # create the YM matrix
  YM <- matrix(as.numeric(!is.na(MODELobj$data)), n, TT)

  # Make sure the missing vals in yt are NAs if there are any
  yt <- MODELobj$data
  yt[!YM] <- as.numeric(NA)

  # Stack the y so that we can get the lag-1 covariance smoother from the Kalman filter output
  # stack.yt=rbind(yt[,1:TT,drop=FALSE],cbind(NA,yt[,1:(TT-1),drop=FALSE]))
  # stack.yt=t(stack.yt)

  # KFS needs time going down rows so yt can be properly converted to ts object
  yt <- t(yt)

  # Build the Zt matrix which is n x (m+1) or n x (m+1) x T; (m+1) because A is in Z
  if (model.dims$Z[3] == 1 & model.dims$A[3] == 1) {
    # not time-varying
    Zt <- cbind(par.1$Z, par.1$A)
    stack.Zt <- matrix(0, n, 2 * (m + 1))
    stack.Zt[1:n, 1:(m + 1)] <- Zt
  } else {
    Zt <- array(0, dim = c(n, m + 1, TT))
    stack.Zt <- array(0, dim = c(n, 2 * (m + 1), TT))
    pars <- parmat(MLEobj, c("Z", "A"), t = 1:TT)
    Zt[1:n, 1:m, ] <- pars$Z
    Zt[1:n, m + 1, ] <- pars$A
    stack.Zt[1:n, 1:(m + 1), ] <- Zt
  }

  # Build the Tt matrix which is (m+1) x (m+1) or (m+1) x (m+1) x T; (m+1) because x has extra row of 1s (for the A)
  #    Tt=cbind(rbind(parList$B,matrix(0,1,m)),matrix(c(parList$U,1),m+1,1))
  if (model.dims$B[3] == 1 & model.dims$U[3] == 1) {
    # not time-varying
    Tt <- cbind(rbind(par.1$B, matrix(0, 1, m)), matrix(c(par.1$U, 1), m + 1, 1))
    stack.Tt <- matrix(0, 2 * (m + 1), 2 * (m + 1))
    stack.Tt[1:(m + 1), 1:(m + 1)] <- Tt
    stack.Tt[(m + 2):(2 * m + 2), 1:(m + 1)] <- diag(1, m + 1)
  } else {
    Tt <- array(0, dim = c(m + 1, m + 1, TT))
    stack.Tt <- array(0, dim = c(2 * (m + 1), 2 * (m + 1), TT))
    pars <- parmat(MLEobj, c("B", "U"), t = 1:TT)
    # pars uses i+1 because in KFAS x(t)=B(t-1)x(t-1) versus MARSS where x(t)=B(t)x(t-1)
    # KFAS sets prior on x(1) so recursions start at x(2); thus B(0) never appears in the KFAS recursions
    Tt[1:m, 1:m, 1:(TT - 1)] <- pars$B[, , 2:TT]
    Tt[1:m, m + 1, 1:(TT - 1)] <- pars$U[, , 2:TT]
    Tt[m + 1, m + 1, ] <- 1
    # in KFAS T[,,TT] is not used since x(T)=B(t-1)x(t-1) is the last computation
    # Set to 0 since is.SSModel does like NA in any parameters
    Tt[, , TT] <- 0
    stack.Tt[(m + 2):(2 * m + 2), 1:(m + 1), ] <- diag(1, m + 1)
    stack.Tt[1:(m + 1), 1:(m + 1), ] <- Tt
    stack.Tt[, , TT] <- 0 # see comment above re setting TT value
  }

  # Build the Ht (R) matrix which is n x n or n x n x T
  if (model.dims$R[3] == 1) {
    # not time-varying
    Ht <- tcrossprod(par.1$H %*% par.1$R, par.1$H)
  } else {
    Ht <- array(0, dim = c(n, n, TT))
    for (i in 1:TT) {
      Ht[, , i] <- tcrossprod(parmat(MLEobj, "H", t = i)$H %*% parmat(MLEobj, "R", t = i)$R, parmat(MLEobj, "H", t = i)$H)
    }
  }

  # Build the Qt matrix which is g1 x g1 or g1 x g1 x T;
  if (model.dims$Q[3] == 1) {
    # not time-varying
    Qt <- par.1$Q
    stack.Qt <- matrix(0, 2 * g1, 2 * g1)
    stack.Qt[1:g1, 1:g1] <- Qt
  } else {
    # See notes for Tt re differences in parameter indexing for the process equation
    Qt <- array(0, dim = c(g1, g1, TT))
    stack.Qt <- array(0, dim = c(2 * g1, 2 * g1, TT))
    for (i in 1:(TT - 1)) {
      # i+1 since in KFAS my B(t) equals B(t-1)
      Qt[, , i] <- parmat(MLEobj, "Q", t = i + 1)$Q
      stack.Qt[1:g1, 1:g1, i] <- Qt[, , i]
    }
    # see comments on setting of T re TT value; TT value never appears in the KFAS recursions
    # but is.SSModel check does not like any NAs in the parameters
    Qt[, , TT] <- 0
    stack.Qt[, , TT] <- 0
  }

  # build the Rt (G) matrix; process error is G(t)%*%w(t); extra row at bottom for for U (1s row in x)
  if (model.dims$G[3] == 1) {
    # not time-varying
    Rt <- matrix(0, m + 1, g1)
    Rt[1:m, 1:g1] <- par.1$G
    stack.Rt <- matrix(0, 2 * (m + 1), 2 * g1)
    stack.Rt[1:(m + 1), 1:g1] <- Rt
  } else {
    # See notes for Tt re differences in parameter indexing for the process equation
    Rt <- array(0, dim = c(m + 1, g1, TT))
    stack.Rt <- array(0, dim = c(2 * (m + 1), 2 * g1, TT))
    for (i in 1:(TT - 1)) {
      # i+1 since in KFAS my B(t) equals B(t-1)
      Rt[, , i] <- matrix(0, m + 1, g1)
      Rt[1:m, 1:g1, i] <- parmat(MLEobj, "G", t = i + 1)$G
      stack.Rt[1:(m + 1), 1:g1, i] <- Rt[, , i]
    }
    # see comments on setting of T re TT value; TT value never appears in the KFAS recursions
    # but is.SSModel check does not like any NAs in the parameters
    Rt[, , TT] <- 0
    stack.Rt[, , TT] <- 0
  }

  # Build the a1 and P1 matrices
  # First compute x10 and V10 if tinitx=0
  if (MODELobj$tinitx == 0) { # Compute needed x_1 | x_0
    # B(1),U(1), and Q(1) correct here since equivalent to B(0), etc in KFAS terminology
    x00 <- par.1$x0
    V00 <- tcrossprod(par.1$L %*% par.1$V0, par.1$L)
    x10 <- par.1$B %*% x00 + par.1$U # Shumway and Stoffer treatment of initial states
    V10 <- par.1$B %*% V00 %*% t.B + par.1$Q # eqn 6.20
  } else {
    x10 <- par.1$x0
    x00 <- matrix(10, m, 1) # dummy; not defined
    V10 <- par.1$V0
    V00 <- diag(0, m) # dummy; not defined
  }
  a1 <- rbind(x10, 1)
  stack.a1 <- rbind(x10, 1, x00, 1)
  if (diffuse) {
    P1inf <- matrix(0, m + 1, m + 1)
    P1inf[1:m, 1:m] <- V10
    stack.P1inf <- matrix(0, 2 * (m + 1), 2 * (m + 1))
    stack.P1inf[1:m, 1:m] <- V10
    stack.P1inf[(m + 2):(2 * m + 1), (m + 2):(2 * m + 1)] <- V00
    P1 <- matrix(0, m + 1, m + 1)
    stack.P1 <- matrix(0, 2 * (m + 1), 2 * (m + 1))
  } else {
    P1inf <- matrix(0, m + 1, m + 1)
    stack.P1inf <- matrix(0, 2 * (m + 1), 2 * (m + 1))
    P1 <- matrix(0, m + 1, m + 1)
    P1[1:m, 1:m] <- V10
    # P1 is the var-cov matrix for the stacked x1,x0
    # it is matrix(c(V10,B*V00,V00*t(B),V00),2,2)
    # x1=B*x0+U+w; in the var-cov mat looks like
    # E[x1*t(x1)] E[(Bx0+U+w1)*t(x0)] -   E[x1]*E[x1]     E[(Bx0+U+w1)]E[t(x0)]
    # E[x0*t(Bx0+U+w1)] E[x0*t(x0)]       E[(Bx0+U+w1)]E[t(x0)] - E[x0]E[t(x0)]
    stack.P1 <- matrix(0, 2 * (m + 1), 2 * (m + 1))
    stack.P1[1:m, 1:m] <- V10
    stack.P1[(m + 2):(2 * m + 1), (m + 2):(2 * m + 1)] <- V00
    stack.P1[1:m, (m + 2):(2 * m + 1)] <- par.1$B %*% V00
    stack.P1[(m + 2):(2 * m + 1), 1:m] <- V00 %*% t(par.1$B)
  }

  if (!return.lag.one) {
    kfas.model <- SSModel(yt ~ -1 + SSMcustom(Z = Zt, T = Tt, R = Rt, Q = Qt, a1 = a1, P1 = P1, P1inf = P1inf), H = Ht)
  } else {
    kfas.model <- SSModel(yt ~ -1 + SSMcustom(Z = stack.Zt, T = stack.Tt, R = stack.Rt, Q = stack.Qt, a1 = stack.a1, P1 = stack.P1, P1inf = stack.P1inf), H = Ht)
  }

  if (only.logLik) {
    return(list(logLik = logLik(kfas.model)))
  }
  # This works because I do not allow the location of 0s on the diagonal of R or Q to be time-varying
  if (n == 1) {
    diag.R <- unname(par.1$R)
  } else {
    diag.R <- unname(par.1$R)[1 + 0:(n - 1) * (n + 1)]
  }

  if (any(diag.R == 0)) { # because KFAS 1.0.4 added a warning message
    ks.out <- suppressWarnings(KFS(kfas.model, simplify = FALSE))
  } else {
    ks.out <- KFS(kfas.model, simplify = FALSE)
  } # simplify=FALSE so 1.0.0 returns LL
  # because 1.0.0 has time down columns instead of across rows
  if (packageVersion("KFAS") != "0.9.11") {
    ks.out$a <- t(ks.out$a)
    ks.out$alphahat <- t(ks.out$alphahat)
  }

  VtT <- ks.out$V[1:m, 1:m, , drop = FALSE]
  Vtt1 <- ks.out$P[1:m, 1:m, 1:TT, drop = FALSE]
  if (!return.lag.one) {
    Vtt1T <- NULL
  } else {
    Vtt1T <- ks.out$V[1:m, (m + 2):(2 * m + 1), , drop = FALSE]
  }
  # zero out rows cols as needed when R diag = 0
  # Check that if any R are 0 then model is solveable
  if (any(diag.R == 0)) {
    VtT[abs(VtT) < .Machine$double.eps] <- 0
    Vtt1[abs(Vtt1) < .Machine$double.eps] <- 0
    if (return.lag.one) Vtt1T[abs(Vtt1T) < .Machine$double.eps] <- 0
  }

  x10T <- ks.out$alphahat[1:m, 1, drop = FALSE]
  V10T <- matrix(VtT[, , 1], m, m)
  if (MODELobj$tinitx == 1) {
    x00 <- matrix(NA, m, 1)
    V00 <- matrix(NA, m, m)
    x0T <- x10T
    V0T <- V10T
  } else { # MODELobj$tinitx==0
    Vtt1.1 <- sub3D(Vtt1, t = 1)
    Vinv <- pcholinv(Vtt1.1)
    if (m != 1) Vinv <- symm(Vinv) # to enforce symmetry after chol2inv call
    J0 <- V00 %*% t.B %*% Vinv # eqn 6.49 and 1s on diag when Q=0; Here it is t.B[1]
    xtT.1 <- ks.out$alphahat[1:m, 1, drop = FALSE]
    x0T <- x00 + J0 %*% (ks.out$alphahat[1:m, 1, drop = FALSE] - ks.out$a[1:m, 1, drop = FALSE]) # eqn 6.47
    V0T <- V00 + J0 %*% (VtT[, , 1] - Vtt1[, , 1]) * t(J0) # eqn 6.48
  }

  if (!return.kfas.model) kfas.model <- NULL

  # not using ks.out$v (Innov) and ks.out$F (Sigma) since I think there might be a bug (I misunderstand KFAS) when R is not diagonal.
  rtn.list <- list(
    xtT = ks.out$alphahat[1:m, , drop = FALSE],
    VtT = VtT,
    Vtt1T = Vtt1T,
    x0T = x0T,
    V0T = V0T,
    x10T = ks.out$alphahat[1:m, 1, drop = FALSE],
    V10T = V10T,
    x00T = x00,
    V00T = V00,
    Vtt = "Use MARSSkfss to get Vtt",
    Vtt1 = Vtt1,
    J = "Use MARSSkfss to get J",
    J0 = "Use MARSSkfss to get J0",
    Kt = "Use MARSSkfss to get Kt",
    xtt1 = ks.out$a[1:m, 1:TT, drop = FALSE],
    xtt = "Use MARSSkfss to get xtt",
    Innov = "Use MARSSkfss to get Innov",
    Sigma = "Use MARSSkfss to get Sigma",
    kfas.model = kfas.model,
    logLik = ks.out$logLik,
    ok = TRUE,
    errors = NULL
  )
  # apply names
  X.names <- attr(MODELobj, "X.names")
  for(el in c("xtT", "xtt1", "x0T", "x00T", "x10T")) rownames(rtn.list[[el]]) <- X.names
  return(rtn.list)
}