#' 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]);

    }

}