--- a
+++ b/R/calc.llk.tree.R
@@ -0,0 +1,50 @@
+#' Function to calculate integrated likelihood for set of variants up tree
+#'
+#' @param pars A list object as returned by the function set.parameters
+#' @param data.sub A 3-dimension array (N. SNPs x N. phenotypes x 3) with state likelihoods.
+#' @param returnForwardMatrix Boolean
+#' @param returnGMatrix Boolean
+#'
+#' @return Either a numeric object with the Tree likelihood or a list with FWD and G matrices
+
+calc.llk.tree <- function(
+    pars,
+    data.sub,
+    returnForwardMatrix = FALSE,
+    returnGMatrix = FALSE
+) {
+
+    ## Get integrated likelihood at nodes - will be overwritten for internal nodes
+    mx <- apply(data.sub, 1, max);
+    d <- exp(data.sub-mx);
+    llk.integrated <- log(d %*% pars$stat.dist)+mx;
+    if (returnGMatrix) g <- array(0, dim(d));
+    
+    for (i in pars$t.path) {
+        ## Emissions at node
+        emiss.node<-data.sub[i,];			
+        data.sub[i,]<-0;
+        for (j in pars$ontology[[i]]) {
+            tmp1 <- cbind(
+                data.sub[j,]-pars$theta.tree,
+                llk.integrated[j]+pars$lp.switch
+            );
+            mx1 <- apply(tmp1, 1, max);
+            tmp2 <- mx1+log(rowSums(exp(tmp1-mx1)));
+            data.sub[i,] <- data.sub[i,]+tmp2;
+            if (returnGMatrix) g[j,] <- tmp2;
+        }
+        data.sub[i,] <- data.sub[i,]+emiss.node;
+        mx <- max(data.sub[i,]);
+        llk.integrated[i] <- mx+log(sum(exp(data.sub[i,]-mx)*pars$stat.dist));
+    }
+    if (returnForwardMatrix) {
+        if (!returnGMatrix) {
+            return(data.sub);
+        } else {
+            return(list(f=data.sub, g=g));
+        }
+    } else {
+        return(llk.integrated[i]);
+    }
+}