##The following functions only work for constant order AD models 
###############Theorem 5.8 (a)###############   

REMLE.phi.delta<-function(data, covariate, p, REMLE){
  data <- as.matrix(data)
  Z <- as.matrix(covariate)
  n <- ncol(data)
  m <- ncol(Z)
  V <- matrix(rep(0,n*n), nrow = n)

  if(length(p)==1)  p <- rep(0:p, c(rep(1,p),(n-p)))  
 
  for(i in 1:n){
    Z <- as.matrix(Z[!is.na(data[,i]),])     
    data <- data[!is.na(data[,i]),]          
    N <- nrow(data)         


    if(m==1)  A <- cov(matrix(data[ ,(1:i)], ncol=i))*(N-m)/N   
         
    else{
      Y <- NULL
      for(j in 1:N)
        Y <- cbind(Y, t(data[j,1:i]))
      Y <- t(Y) 

      ## p.147
      beta.bar <- kronecker(solve(t(Z)%*%Z)%*%t(Z), diag(i))%*%Y
      A <- matrix(rep(0,i*i), nrow = i)
      for(s in 1:N){
        M <- matrix(data[s,1:i], nr=i)-kronecker(matrix(Z[s,], nr=1), diag(i))%*%beta.bar
        A <- A + M %*% t(M)
      }
      A <- A/N
    }

    if(REMLE==TRUE)  cov <- (N/(N-m))*A      
    else  cov <- A


    ## innovation variances(2.18) for i=1
    if(i==1)  V[i,i] <- cov

    else{

      ## innovation variances(2.18)
      V[i,i] <- cov[i,i]-t(cov[(i-min(p[i],(i-1))):(i-1), i]) %*%
         solve(cov[(i-min(p[i],(i-1))):(i-1), (i-min(p[i],(i-1))):(i-1)]) %*%
         cov[(i-min(p[i],(i-1))):(i-1), i]    

      ## autoregressive coefficients(2.19)
      phi <- as.vector(t(cov[(i-min(p[i],(i-1))):(i-1),i]) %*% 
         solve(cov[(i-min(p[i],(i-1))):(i-1),(i-min(p[i],(i-1))):(i-1)]))  
      for(j in (i-min(p[i],(i-1))):(i-1))
        V[i,j] <- phi[j+1-(i-min(p[i],(i-1)))]
    }
  }  
  V 
}


###############Theorem 5.8 (b)###############   

## calculate the correlations below and above the main diagonals
cov.cor<-function(V){
  for(i in 2: nrow(V)){
    for(j in 1:(i-1))
      V[j,i] <- V[i,j] <- V[i,j]/sqrt(V[i,i]*V[j,j])
  }
  V
}


REMLE.cov<-function(data, covariate, p, REMLE){
  n <- ncol(data)
  T <- -REMLE.phi.delta(data, covariate, p, REMLE=REMLE)
  D <- matrix(rep(0,n*n), nrow = n)
  diag(D) <- -diag(T)
  diag(T) <- rep(1,n)

  solve(T) %*% D %*% solve(t(T))
}


###############Theorem 5.8 (c)###############   
## inverse
inv <- function(cov){
  solve(cov)
}


## intervenor-adjusted partial correlation

## recursion formula for partial correlations in (2.6)
recur.pcor<-function(index1, index2, index3, V){
  index1 <- index1
  index2 <- index2
  index3 <- index3
  index <- c(index1, index2, index3)

  if(length(index3)==1){
    rxy.z <- (V[index[1], index[2]]-V[index[1], index[3]]*V[index[2], index[3]])/
            (sqrt(1-V[index[1], index[3]]^2)*sqrt(1-V[index[2], index[3]]^2))
    return(rxy.z)
  }
  else{
    index1 <- index[1]
    index2 <- index[2]
    index30 <- index[3]    
    index3c <- index[-c(1,2,3)]

    rxy.zc <- recur.pcor(index1, index2, index3c, V)
    rxz0.zc <- recur.pcor(index1,index30,index3c, V)
    ryz0.zc <- recur.pcor(index2,index30,index3c, V)
    rxy.z <- (rxy.zc - rxz0.zc*ryz0.zc)/(sqrt(1-rxz0.zc^2)*sqrt(1-ryz0.zc^2))
    return(rxy.z)
  }                       
}  

partial.inter<-function(cor){
  V <- cor
  n <- ncol(cor)
  m <- n-2
  for(k in 1:m){
    if(k==m)
       V[1, n] <- V[n, 1] <- recur.pcor(1, n, c(2:(n-1)), cor)

     else
       diag(V[1:(n-k-1), (k+2):n]) <- diag(V[(k+2):n, 1:(n-k-1)]) <- sapply(1:(m+1-k), function(i) if(k==1) 
                    recur.pcor(i, i+k+1, i+1, cor) else recur.pcor(i, i+k+1, c((i+1):(i+k)), cor))
  }
  V
}



###############Theorem 5.8 (d)############### 

beta.hat<-function(data, covariate, p){
  data <- as.matrix(data)
  Z <- as.matrix(covariate)
  n <- ncol(data)
  m <- ncol(covariate)
  
  if(length(p)==1)  p <- rep(0:p, c(rep(1,p),(n-p)))  

  T <- -REMLE.phi.delta(data, covariate, p, REMLE=TRUE)  ## REMLE=TRUE or FALSE 
  diag(T) <- rep(1,n)

  if(m==1){

    muhatstar <- rep(0,n)
    for(i in 1:n){  
      data1 <- data[!is.na(data[,i]),]      

      if(i==1)  muhatstar[i] <- mean(data1[ ,1])
      else{
        Y <- matrix(apply(data1[ ,(1:i)], 2, mean),nrow=1)
        muhatstar[i] <- Y[i]+T[i,(i-min(p[i],(i-1))):(i-1)] %*% Y[(i-min(p[i],(i-1))):(i-1)]
      }
    }
    muhat <- solve(T) %*% muhatstar
    list(muhat=muhat)
  }

  else{
    betahatstar <- NULL
    for(i in 1:n){  
      Z1 <- Z[!is.na(data[,i]),]
      data1 <- data[!is.na(data[,i]),]
      N1 <- nrow(data1)

      if(min(p[i],(i-1))==0)  betahatstar <- rbind(betahatstar, solve(t(Z1) %*% Z1) %*% t(Z1) %*% data1[, i])
      else{
        Q <- NULL
        for(k in 1:min(p[i],(i-1)))
          Q <- cbind(Q, data1[, i-k]) 
        Q <- matrix(Q, nrow=N1)

        M <- diag(N1) - Q %*% solve(t(Q) %*% Q) %*% t(Q)  
        betahatstar <- rbind(betahatstar, solve(t(Z1) %*% M %*% Z1) %*% t(Z1) %*% M %*% data1[, i])
      }
    }
    betahat <- kronecker(solve(T),diag(m)) %*% betahatstar
    list(betahat=betahat)
  }
}