gt:                     final double diffi = conditionalRootMean[i] - rootPriorMean[i];
context: /*
   * FullyConjugateMultivariateTraitLikelihood.java
   *
   * Copyright (c) 2002-2013 Alexei Drummond, Andrew Rambaut and Marc Suchard
   *
   * This file is part of BEAST.
   * See the NOTICE file distributed with this work for additional
   * information regarding copyright ownership and licensing.
   *
   * BEAST is free software; you can redistribute it and/or modify
   * it under the terms of the GNU Lesser General Public License as
   * published by the Free Software Foundation; either version 2
   * of the License, or (at your option) any later version.
   *
   *  BEAST is distributed in the hope that it will be useful,
   *  but WITHOUT ANY WARRANTY; without even the implied warranty of
   *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   *  GNU Lesser General Public License for more details.
   *
   * You should have received a copy of the GNU Lesser General Public
   * License along with BEAST; if not, write to the
   * Free Software Foundation, Inc., 51 Franklin St, Fifth Floor,
   * Boston, MA  02110-1301  USA
   */
  
  package dr.evomodel.continuous;
  
  import dr.evolution.tree.MultivariateTraitTree;
  import dr.evolution.tree.NodeRef;
  import dr.evomodel.branchratemodel.BranchRateModel;
  import dr.inference.model.CompoundParameter;
  import dr.inference.model.Model;
  import dr.inference.model.Parameter;
  import dr.math.distributions.MultivariateNormalDistribution;
  import dr.math.distributions.WishartSufficientStatistics;
  import dr.math.interfaces.ConjugateWishartStatisticsProvider;
  import dr.math.matrixAlgebra.Matrix;
  import dr.math.matrixAlgebra.Vector;
  
  import java.util.List;
  
  /**
   * Integrated multivariate trait likelihood that assumes a fully-conjugate prior on the root.
   * The fully-conjugate prior is a multivariate normal distribution with a precision scaled by
   * diffusion process
   *
   * @author Marc A. Suchard
   */
  public class FullyConjugateMultivariateTraitLikelihood extends IntegratedMultivariateTraitLikelihood implements ConjugateWishartStatisticsProvider {
  
      public FullyConjugateMultivariateTraitLikelihood(String traitName,
                                                       MultivariateTraitTree treeModel,
                                                       MultivariateDiffusionModel diffusionModel,
                                                       CompoundParameter traitParameter,
                                                       Parameter deltaParameter,
                                                       List<Integer> missingIndices,
                                                       boolean cacheBranches,
                                                       boolean scaleByTime,
                                                       boolean useTreeLength,
                                                       BranchRateModel rateModel,
                                                       Model samplingDensity,
                                                       boolean reportAsMultivariate,
                                                       double[] rootPriorMean,
                                                       double rootPriorSampleSize,
                                                       boolean reciprocalRates) {
  
          super(traitName, treeModel, diffusionModel, traitParameter, deltaParameter, missingIndices, cacheBranches, scaleByTime,
                  useTreeLength, rateModel, samplingDensity, reportAsMultivariate, reciprocalRates);
  
          // fully-conjugate multivariate normal with own mean and prior sample size
          this.rootPriorMean = rootPriorMean;
          this.rootPriorSampleSize = rootPriorSampleSize;
  
          priorInformationKnown = false;
      }
  
  
      public FullyConjugateMultivariateTraitLikelihood(String traitName,
                                                       MultivariateTraitTree treeModel,
                                                       MultivariateDiffusionModel diffusionModel,
                                                       CompoundParameter traitParameter,
                                                       Parameter deltaParameter,
                                                       List<Integer> missingIndices,
                                                       boolean cacheBranches,
                                                       boolean scaleByTime,
                                                       boolean useTreeLength,
                                                       BranchRateModel rateModel,
                                                       List<BranchRateModel> driftModels,
                                                       Model samplingDensity,
                                                       boolean reportAsMultivariate,
                                                       double[] rootPriorMean,
                                                       double rootPriorSampleSize,
                                                       boolean reciprocalRates) {
  
          super(traitName, treeModel, diffusionModel, traitParameter, deltaParameter, missingIndices, cacheBranches, scaleByTime,
                  useTreeLength, rateModel, driftModels, samplingDensity, reportAsMultivariate, reciprocalRates);
  
          // fully-conjugate multivariate normal with own mean and prior sample size
          this.rootPriorMean = rootPriorMean;
          this.rootPriorSampleSize = rootPriorSampleSize;
  
          priorInformationKnown = false;
      }
  
      public FullyConjugateMultivariateTraitLikelihood(String traitName,
                                                       MultivariateTraitTree treeModel,
                                                       MultivariateDiffusionModel diffusionModel,
                                                       CompoundParameter traitParameter,
                                                       Parameter deltaParameter,
                                                       List<Integer> missingIndices,
                                                       boolean cacheBranches,
                                                       boolean scaleByTime,
                                                       boolean useTreeLength,
                                                       BranchRateModel rateModel,
                                                       List<BranchRateModel> optimalValues,
                                                       BranchRateModel strengthOfSelection,
                                                       Model samplingDensity,
                                                       boolean reportAsMultivariate,
                                                       double[] rootPriorMean,
                                                       double rootPriorSampleSize,
                                                       boolean reciprocalRates) {
  
          super(traitName, treeModel, diffusionModel, traitParameter, deltaParameter, missingIndices, cacheBranches, scaleByTime,
                  useTreeLength, rateModel, optimalValues, strengthOfSelection, samplingDensity, reportAsMultivariate, reciprocalRates);
  
          // fully-conjugate multivariate normal with own mean and prior sample size
          this.rootPriorMean = rootPriorMean;
          this.rootPriorSampleSize = rootPriorSampleSize;
  
          priorInformationKnown = false;
      }
  
  
      public double getRescaledLengthToRoot(NodeRef nodeRef) {
  
          double length = 0;
  
          if (!treeModel.isRoot(nodeRef)) {
              NodeRef parent = treeModel.getParent(nodeRef);
              length += getRescaledBranchLengthForPrecision(nodeRef) + getRescaledLengthToRoot(parent);
          }
  
          return length;
      }
  
  
      protected double calculateAscertainmentCorrection(int taxonIndex) {
  
          NodeRef tip = treeModel.getNode(taxonIndex);
          int nodeIndex = treeModel.getNode(taxonIndex).getNumber();
  
          if (ascertainedData == null) { // Assumes that ascertained data are fixed
              ascertainedData = new double[dimTrait];
          }
  
  //        diffusionModel.diffusionPrecisionMatrixParameter.setParameterValue(0,2); // For debugging non-1 values
          double[][] traitPrecision = diffusionModel.getPrecisionmatrix();
          double logDetTraitPrecision = Math.log(diffusionModel.getDeterminantPrecisionMatrix());
  
          double lengthToRoot = getRescaledLengthToRoot(tip);
          double marginalPrecisionScalar = 1.0 / lengthToRoot + rootPriorSampleSize;
          double logLikelihood = 0;
  
          for (int datum = 0; datum < numData; ++datum) {
  
              // Get observed trait value
              System.arraycopy(meanCache, nodeIndex * dim + datum * dimTrait, ascertainedData, 0, dimTrait);
  
              if (DEBUG_ASCERTAINMENT) {
                  System.err.println("Datum #" + datum);
                  System.err.println("Value: " + new Vector(ascertainedData));
                  System.err.println("Cond : " + lengthToRoot);
                  System.err.println("MargV: " + 1.0 / marginalPrecisionScalar);
                  System.err.println("MargP: " + marginalPrecisionScalar);
                  System.err.println("diffusion prec: " + new Matrix(traitPrecision));
              }
  
              double SSE;
              if (dimTrait > 1) {
                  throw new RuntimeException("Still need to implement multivariate ascertainment correction");
              } else {
                  double precision = traitPrecision[0][0] * marginalPrecisionScalar;
  
                  SSE = ascertainedData[0] * precision * ascertainedData[0];
              }
  
              double thisLogLikelihood = -LOG_SQRT_2_PI * dimTrait
                      + 0.5 * (logDetTraitPrecision + dimTrait * Math.log(marginalPrecisionScalar) - SSE);
  
              if (DEBUG_ASCERTAINMENT) {
                  System.err.println("LogLik: " + thisLogLikelihood);
                  dr.math.distributions.NormalDistribution normal = new dr.math.distributions.NormalDistribution(0,
                          Math.sqrt(1.0 / (traitPrecision[0][0] * marginalPrecisionScalar)));
                  System.err.println("TTTLik: " + normal.logPdf(ascertainedData[0]));
                  if (datum >= 10) {
                      System.exit(-1);
                  }
              }
              logLikelihood += thisLogLikelihood;
          }
          return logLikelihood;
      }
  
  //    public double getRootPriorSampleSize() {
  //        return rootPriorSampleSize;
  //    }
  
  //    public double[] getRootPriorMean() {
  //        double[] out = new double[rootPriorMean.length];
  //        System.arraycopy(rootPriorMean, 0, out, 0, out.length);
  //        return out;
  //    }
  
      public WishartSufficientStatistics getWishartStatistics() {
          computeWishartStatistics = true;
          calculateLogLikelihood();
          computeWishartStatistics = false;
          return wishartStatistics;
      }
  
  //    private double getLogPrecisionDetermination() {
  //        return Math.log(diffusionModel.getDeterminantPrecisionMatrix()) + dimTrait * Math.log(rootPriorSampleSize);
  //    }
  
      protected void handleModelChangedEvent(Model model, Object object, int index) {
  
          if (model == diffusionModel) {
              priorInformationKnown = false;
          }
          super.handleModelChangedEvent(model, object, index);
      }
  
      public void restoreState() {
          super.restoreState();
          priorInformationKnown = false;
      }
  
      public void makeDirty() {
          super.makeDirty();
          priorInformationKnown = false;
      }
  
      public double getPriorSampleSize() {
          return rootPriorSampleSize;
      }
  
      public double[] getPriorMean() {
          return rootPriorMean;
      }
  
      @Override
      public boolean getComputeWishartSufficientStatistics() {
          return computeWishartStatistics;
      }
  
      protected void checkLogLikelihood(double loglikelihood, double logRemainders,
                                        double[] conditionalRootMean, double conditionalRootPrecision,
                                        double[][] traitPrecision) {
  
  //        System.err.println("root cmean    : " + new Vector(conditionalRootMean));
  //        System.err.println("root cprec    : " + conditionalRootPrecision);
  //        System.err.println("diffusion prec: " + new Matrix(traitPrecision));
  //
  //        System.err.println("prior mean    : " + new Vector(rootPriorMean));
  //        System.err.println("prior prec    : " + rootPriorSampleSize);
  
          double upperPrecision = conditionalRootPrecision * rootPriorSampleSize / (conditionalRootPrecision + rootPriorSampleSize);
  //        System.err.println("root cprec    : " + upperPrecision);
  
          double[][] newPrec = new double[traitPrecision.length][traitPrecision.length];
  
          for (int i = 0; i < traitPrecision.length; ++i) {
              for (int j = 0; j < traitPrecision.length; ++j) {
                  newPrec[i][j] = traitPrecision[i][j] * upperPrecision;
              }
          }
          MultivariateNormalDistribution mvn = new MultivariateNormalDistribution(rootPriorMean, newPrec);
          double logPdf = mvn.logPdf(conditionalRootMean);
  
          System.err.println("Got here subclass: " + loglikelihood);
          System.err.println("logValue         : " + (logRemainders + logPdf));
          System.err.println("");
  //        System.err.println("logRemainders    : " + logRemainders);
  //        System.err.println("logPDF           : " + logPdf);
  //        System.exit(-1);
      }
  
      protected double integrateLogLikelihoodAtRoot(double[] conditionalRootMean,
                                                    double[] marginalRootMean,
                                                    double[][] notUsed,
                                                    double[][] treePrecisionMatrix, double conditionalRootPrecision) {
          final double square;
          final double marginalPrecision = conditionalRootPrecision + rootPriorSampleSize;
          final double marginalVariance = 1.0 / marginalPrecision;
  
          // square : (Ay + Bz)' (A+B)^{-1} (Ay + Bz)
  
          // A = conditionalRootPrecision * treePrecisionMatrix
          // B = rootPriorSampleSize * treePrecisionMatrix
  
          if (dimTrait > 1) {
  
              computeWeightedAverage(conditionalRootMean, 0, conditionalRootPrecision, rootPriorMean, 0, rootPriorSampleSize,
                      marginalRootMean, 0, dimTrait);
  
              square = computeQuadraticProduct(marginalRootMean, treePrecisionMatrix, marginalRootMean, dimTrait)
                      * marginalPrecision;
  
              if (computeWishartStatistics) {
  
                  final double[][] outerProducts = wishartStatistics.getScaleMatrix();
  
                  final double weight = conditionalRootPrecision * rootPriorSampleSize * marginalVariance;
                  for (int i = 0; i < dimTrait; i++) {
  