/*
    Theseus - maximum likelihood superpositioning of macromolecular structures

    Copyright (C) 2004-2015 Douglas L. Theobald

    This file is part of THESEUS.

    THESEUS is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as
    published by the Free Software Foundation, either version 3 of
    the License, or (at your option) any later version.

    THESEUS 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 General Public License for more details.

    You should have received a copy of the GNU General Public
    License along with THESEUS in the file 'COPYING'. If not, see
    <http://www.gnu.org/licenses/>.

    -/_|:|_|_\-
*/

#include <stdlib.h>
#include <string.h>
#include <stdio.h>
#include <math.h>
#include <limits.h>
#include <float.h>
#include <ctype.h>
#include <gsl/gsl_sf_gamma.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_min.h>
#include "DLTutils.h"
#include "CovMat.h"
#include "Error.h"
#include "Cds.h"
#include "PDBCds.h"
#include "pdbStats.h"
#include "distfit.h"
#include "HierarchVars.h"


#define SAFE_FUNC_CALL(f, x, yp) \
do { \
    *yp = GSL_FN_EVAL(f,x); \
    if (!gsl_finite(*yp)) \
    GSL_ERROR("computed function value is infinite or NaN", GSL_EBADFUNC); \
} while(0)


static int
compute_f_values (gsl_function *f, double x_minimum, double *f_minimum,
                  double x_lower, double *f_lower,
                  double x_upper, double *f_upper)
{
    SAFE_FUNC_CALL(f, x_lower, f_lower);
    SAFE_FUNC_CALL(f, x_upper, f_upper);
    SAFE_FUNC_CALL(f, x_minimum, f_minimum);

    return GSL_SUCCESS;
}


static int
dlt_min_fminimizer_set_with_values (gsl_min_fminimizer *s, gsl_function *f,
                                    double x_minimum, double f_minimum,
                                    double x_lower, double f_lower,
                                    double x_upper, double f_upper)
{
    s->function = f;
    s->x_minimum = x_minimum;
    s->x_lower = x_lower;
    s->x_upper = x_upper;

    if (x_lower > x_upper)
    {
        GSL_ERROR("invalid interval (lower > upper)", GSL_EINVAL);
    }

    if (x_minimum >= x_upper || x_minimum <= x_lower)
    {
        GSL_ERROR("x_minimum must lie inside interval (lower < x < upper)", GSL_EINVAL);
    }

    s->f_lower = f_lower;
    s->f_upper = f_upper;
    s->f_minimum = f_minimum;

    if (f_minimum >= f_lower && f_minimum >= f_upper)
    {
        GSL_ERROR("guess is worse than both endpoints", GSL_EINVAL);
    }

    return(s->type->set)(s->state, s->function,
                         x_minimum, f_minimum,
                         x_lower, f_lower,
                         x_upper, f_upper);
}


static int
dlt_min_fminimizer_set(gsl_min_fminimizer *s,
                       gsl_function *f,
                       double x_minimum, double x_lower, double x_upper)
{
    int status ;

    double f_minimum, f_lower, f_upper;

    status = compute_f_values(f, x_minimum, &f_minimum,
                              x_lower, &f_lower,
                              x_upper, &f_upper);

    if (status != GSL_SUCCESS)
    {
        return status ;
    }

    status = dlt_min_fminimizer_set_with_values(s, f, x_minimum, f_minimum,
                                                x_lower, f_lower,
                                                x_upper, f_upper);
    return status;
}


static int
findlargest(double *vec, const int len)
{
    int         i, bgi = 0;
    double      bg;

    bg = DBL_MIN;
    for (i = 0; i < len; ++i)
    {
        if (vec[i] > bg)
        {
            bg = vec[i];
            bgi = i;
        }
    }

    return(bgi);
}


static int
findsmallest(double *vec, const int len)
{
    int         i, smi = 0;
    double      sm;

    sm = DBL_MAX;
    for (i = 0; i < len; ++i)
    {
        if (vec[i] < sm)
        {
            sm = vec[i];
            smi = i;
        }
    }

    return(smi);
}


static int
findsmallest_gt(double *vec, const int len, const double gt)
{
    int         i, smi = 0;
    double      sm;

    sm = DBL_MAX;
    for (i = 0; i < len; ++i)
    {
        if (vec[i] < sm && vec[i] > gt)
        {
            sm = vec[i];
            smi = i;
        }
    }

    return(smi);
}

static double
HarmonicAve(const double *data, const int len)
{
    int             i;
    double          invdata;

    invdata = 0.0;
    for (i = 0; i < len; ++i)
        invdata += 1.0 / data[i];

    return(len / invdata);
}


static void
InvGammaAdjustCov(CdsArray *cdsA, const double b, const double c)
{
    int             i, j;
    const int       vlen = cdsA->vlen;
    const double    nd = 3.0 * cdsA->cnum;
    const double    fact = nd / (nd + 2.0 * (1.0 + c));

    for (i = 0; i < vlen; ++i)
        cdsA->CovMat[i][i] += 2.0 * b / nd;

    for (i = 0; i < vlen; ++i)
        for (j = 0; j < vlen; ++j)
            cdsA->CovMat[i][j] *= fact;
}


static void
InvGammaAdjustVar(double *newvar, const int vlen, const int cnum,
                  double *var, const double b, const double c)
{
    int         i;

    for (i = 0; i < vlen; ++i)
        newvar[i] = (3.0*cnum*var[i] + 2.0*b) / (3.0*cnum + 2.0 + 2.0*c);
        // this is the conditional maximization algorithm
        //newvar[i] = (3.0*cnum*var[i] + 2.0*b) / (3.0*cnum + 2.0*c);
        // this is required for an EM algorithm of the variances
}


// this is required for an EM algorithm of the variances
static void
InvGammaAdjustVarPhi(double *newvar, const int vlen, const int cnum,
                     double *var, const double phi, const double ndf)
{
    int         i;
    double      fact = 1.0 / (3.0 * cnum + ndf);

    for (i = 0; i < vlen; ++i)
    {
        newvar[i] = fact * (3.0 * cnum * var[i] + phi);
        //printf("%3d %26.6f\n", i, var[i]);
    }
}


// this is required for an EM algorithm of the variances
void
InvGammaAdjustVarPhiNu(Cds **cds, double *newvar, const int vlen, const int cnum,
                       double *var, const double phi, const double ndf)
{
    int         i, j;
    double      ri;

    for (i = 0; i < vlen; ++i)
    {
        // could use CalcDf() here
        ri = 0;
        for (j = 0; j < cnum; ++j)
            ri += cds[j]->nu[i];

        newvar[i] = (3.0 * ri * var[i] + phi) / (3.0 * ri + ndf);
        //printf("%3d %26.6e %26.6f %26.6f %26.6f %26.6f\n", i, var[i], phi, ri, ndf, newvar[i]);
    }
    //printf("\n");
}


void
InvGammaAdjustEvals(double *newevals, const int vlen, const int cnum,
                    double *evals, const double phi, const double ndf)
{
    int         i;

    for (i = 0; i < vlen; ++i)
        newevals[i] = (3.0*cnum*evals[i] + phi) / (3.0*cnum + ndf);
        // this is for expected inverse
        // newevals[i] = (3.0*cnum*evals[i] + phi) / (3.0*cnum + ndf - vlen - 1.0);
        // this is for expected covariance matrix (not inverse)

        //printf("%3d %26.6f\n", i, var[i]);
}


static void
InvGammaAdjustVarNu(Cds **cds, double *newvar, const int vlen, const int cnum,
                    double *var, const double b, const double c)
{
    int         i, j;
    double      df;

    for (i = 0; i < vlen; ++i)
    {
        df = 0;
        for (j = 0; j < cnum; ++j)
            df += cds[j]->nu[i];

        df *= 3.0;

        newvar[i] = (df*var[i] + 2.0*b) / (df + 2.0*(1.0 + c));
    }
}


double
LogPrPhi(double phi, void *params)
{
    CdsArray       *cdsA = (CdsArray *) params;
    //double          term;
    const int       vlen = cdsA->vlen;//, cnum = cdsA->cnum;
    double          ndf = algo->ndf;
    double         *var = cdsA->samplevar;
    double          fact, term, ri;
    int             i;

    stats->hierarch_p1 = 0.5*phi;

    if (phi <= 0.0)
        return(DBL_MAX);

    //term = CalcMgLogLNu(cdsA);

    term = 0.0;
    for (i = 0; i < vlen; ++i)
    {
        ri = cdsA->df[i];
        fact = 0.5*(3.0*ri + ndf);
        term += fact*log(phi + 3.0*ri*var[i]);
    }

    term = 0.5*vlen*ndf*log(phi) - term;
    term -= phi; // exponential prior on phi, mean = 1 // must change marg lik too!

//     term += log(phi) - phi/1; // prior on phi

    if (!isfinite(term))
        return(DBL_MAX);
    else
        return(-term);
}


static double
LogPrPhiCov(double phi, void *params)
{
    CdsArray       *cdsA = (CdsArray *) params;
    const int       vlen = cdsA->vlen, cnum = cdsA->cnum;
    double          ndf = algo->ndf;
    double         *evals = cdsA->samplevar;
    double          fact = 0.5*(3.0*cnum + ndf);
    double          term;
    int             i;

    if (phi <= 0.0)
        return(DBL_MAX);

    term = 0.0;
    for (i = 0; i < vlen; ++i)
        term += log(phi + 3.0 * cnum * evals[i]);

    term = 0.5*vlen*ndf*log(phi) - fact*term;
    //term += log(phi) - phi/1; // prior on phi

    term -= phi; // exponential prior on phi, mean = 1

    if (!isfinite(term))
        return(DBL_MAX);
    else
        return(-term);
}


static double
LogPrPhi_f(double phi, void *params)
{
    CdsArray       *cdsA = (CdsArray *) params;
    const int       vlen = cdsA->vlen, cnum = cdsA->cnum;
    double          ndf = algo->ndf;
    double         *var = cdsA->samplevar;
    double          fact, term, ri;
    int             i, j;

    if (phi <= 0.0)
        phi = DBL_EPSILON;

    term = 0.0;
    for (i = 0; i < vlen; ++i)
    {
        ri = 0.0;
        for (j = 0; j < cnum; ++j)
            ri += cdsA->cds[j]->nu[i];

        fact = 0.5*(3.0*ri + ndf);
        term += fact / (phi + 3.0*ri*var[i]);
    }

    return(0.5*vlen*ndf/phi - term);
}


// DLT FIX --- this has to incorporate incomplete info nu
static double
LogPrPhi_df(double phi, void *params)
{
    CdsArray       *cdsA = (CdsArray *) params;
    const int       vlen = cdsA->vlen, cnum = cdsA->cnum;
    double          ndf = algo->ndf;
    double         *var = cdsA->samplevar;
    double          fact = (3.0*cnum + ndf);
    double          term2, tmp;
    int             i;

    if (phi <= 0.0)
        phi = DBL_EPSILON;

    term2 = 0.0;
    for (i = 0; i < vlen; ++i)
    {
        tmp = 1.0 / (phi + 3.0*cnum*var[i]);
        term2 += tmp*tmp;
    }

    return(-vlen/(phi*phi) + fact*term2);
}


// DLT FIX --- this has to incorporate incomplete info nu
static void
LogPrPhi_fdf(double phi, void *params, double *y, double *dy)
{
    CdsArray       *cdsA = (CdsArray *) params;
    const int       vlen = cdsA->vlen, cnum = cdsA->cnum;
    double          ndf = algo->ndf;
    double         *var = cdsA->samplevar;
    double          fact = (3.0*cnum + ndf);
    double          term, term2, tmp;
    int             i;

    if (phi <= 0.0)
        phi = DBL_EPSILON;

    term = term2 = 0.0;
    for (i = 0; i < vlen; ++i)
    {
        tmp = 1.0 / (phi + 3.0*cnum*var[i]);
        term += tmp;
        term2 += tmp*tmp;
    }

    *y = vlen/phi - fact*term;
    *dy = -vlen/(phi*phi) + fact*term2;
}


/* Fit of the variances/eigenvalues to an inverse gamma distribution with
   a fixed c shape parameter.
   c > 1 -> guarantees that the distribution has a finite mean
   c > 2 -> finite variance
   c > 3 -> finite skewness
   c > 4 -> finite kurtosis
*/
void
InvGammaFitMarginalGSLBrent(CdsArray *cdsA)
{
    const int           vlen = cdsA->vlen, cnum = cdsA->cnum;
    int                 status, i;
    int                 iter, max_iter = 100;
    const               gsl_min_fminimizer_type *T = NULL;
    gsl_min_fminimizer *s = NULL;
    double              oldphi = 2.0*stats->hierarch_p1;
    double              a, b, phi;
    double              chi2 = 0.0, logL;
    double              ndf = algo->ndf;
    gsl_function        F;
    double              absprec = FLT_EPSILON*0.1;
    double              relprec = 0.0;

    T = gsl_min_fminimizer_brent;
//    T = gsl_min_fminimizer_goldensection;
//    T = gsl_min_fminimizer_quad_golden;
    s = gsl_min_fminimizer_alloc(T);

    phi = oldphi;

//  printf ("using %s method\n", gsl_min_fminimizer_name(s));
//  printf ("%5s [%9s, %9s] %9s %10s %9s\n",
//          "iter", "lower", "upper", "phi", "err", "err(est)");
//  printf ("%5d [%.7f, %.7f] %.7f %.7f\n", iter, a, b, phi, b - a);

    if (algo->varweight > 0)
    {
        double     *svar = cdsA->samplevar;
        double     *var = cdsA->var;

//         phi = 0.0;
//         for (i = 0; i < vlen; ++i)
//             phi += 1.0 / (svar[i] + oldphi);
//         phi = vlen/phi;

// double marg1, marg2, marg3; // DLT TEST
// if (algo->alignment)
//  marg1 = CalcMgLogLNu(cdsA);
// else
//  marg1 = CalcMgLogL(cdsA);

        phi = 0.0;
        for (i = 0; i < vlen; ++i)
            phi += 1.0/svar[i];
        //phi /= vlen;
        phi = ndf*vlen/(phi+2);

        a = DBL_MIN;
        //b = DBL_MAX;
        // phi should never, ever be larger than the largest variance
        // in fact, it probably shouldn't be larger than the average variance (its approx the harm ave)
        b = svar[findlargest(svar, vlen)] + 1.0;

//         printf("\na: %g phi: %g b: %g \n\n",
//                LogPrPhi(a, (void *) cdsA),
//                LogPrPhi(phi, (void *) cdsA),
//                LogPrPhi(b, (void *) cdsA));
//         fflush(NULL);

        F.function = &LogPrPhi;
        F.params = (void *) cdsA;
        dlt_min_fminimizer_set(s, &F, phi, a, b); // DLTGSL

        iter = 0;
        do
        {
            iter++;
            status = gsl_min_fminimizer_iterate(s);

            phi = gsl_min_fminimizer_x_minimum(s);
            a = gsl_min_fminimizer_x_lower(s);
            b = gsl_min_fminimizer_x_upper(s);

            status = gsl_min_test_interval(a, b, absprec, relprec);

//             if (status == GSL_SUCCESS)
//                 printf ("Converged:\n");
//
//             printf ("%5d [%.7f, %.7f] %.7f %.7f\n", iter, a, b, phi, b - a);

        }
        while(status == GSL_CONTINUE && iter < max_iter);

//         double tmpsum = 0.0;
//         for (i=0; i < vlen; ++i)
//             tmpsum += 1.0 / var[i];
//
//         printf ("%5d [%.7f, %.7f] %.7f %.7f %.7f\n", iter, a, b, phi, b - a, ndf*vlen/(tmpsum+2));
        //phi = ndf*vlen/(tmpsum+2);
        //phi = (ndf*vlen - 2.0)/tmpsum;

// if (algo->alignment) // DLT TEST
//  marg2 = CalcMgLogLNu(cdsA);
// else
//  marg2 = CalcMgLogL(cdsA);

// printf("PhiMargLogL ///////////////////////////////////////////// % e % e % e\n",
//     marg1, marg2, marg2 - marg1);
// fflush(NULL); // DLT TEST

        if (algo->alignment)
            InvGammaAdjustVarPhiNu(cdsA->cds, var, vlen, cnum, svar, phi, ndf);
        else
            InvGammaAdjustVarPhi(var, vlen, cnum, svar, phi, ndf);

//         tmpsum = 0.0;
//         for (i=0; i < vlen; ++i)
//             tmpsum += 1.0 / var[i];
//
//         printf ("      [%.7f, %.7f] %.7f %.7f %.7f\n", a, b, phi, b - a, ndf*vlen/(tmpsum+2));

// if (algo->alignment) // DLT TEST
//  marg3 = CalcMgLogLNu(cdsA);
// else
//  marg3 = CalcMgLogL(cdsA);

// printf("Var2MargLogL ///////////////////////////////////////////// % e % e % e\n",
//     marg2, marg3, marg3 - marg2);
// fflush(NULL); // DLT TEST

        chi2 = chi_sqr_adapt(var, vlen, 0, &logL, 0.5*phi, 0.5*ndf,
                             invgamma_pdf, invgamma_lnpdf, invgamma_int);
    }
    else if (algo->covweight > 0)
    {
        double     *evals = cdsA->evals;
        double     *invevals = cdsA->tmpvecK;
        double    **evecs = cdsA->tmpmatKK2;
        double     *tmpevals = cdsA->samplevar;
        int         i, j;


        ndf = algo->ndf;
        /* must calc evals before minimizer is set */
        /* EigenGSL 0 evals are small to large */
        EigenGSL((const double **) cdsA->CovMat, vlen, tmpevals, evecs, 0);
        //eigensym((const double **) cdsA->CovMat, tmpevals, evecs, vlen);
//        VecPrint(tmpevals, vlen);

// double marg1, marg2, marg3; // DLT TEST
// memcpy(evals, tmpevals, vlen*sizeof(double));
// marg1 = CalcMgLogL(cdsA);

        //tmpevals[0] = phi/2;//ExpXn(phi/2, ndf, vlen);

//printf("phi:%f %f\n", oldphi, tmpevals[0]); fflush(NULL);

        phi = 0.0;
        for (i = 0; i < vlen; ++i)
            phi += tmpevals[i];
        phi /= 3.0*cnum*vlen;

        a = DBL_MIN;
        //b = DBL_MAX;
        b = tmpevals[vlen-1]+1.0; // should never be larger than the largest eval

//         printf("\na: %g phi: %g b: %g \n\n",
//                LogPrPhiCov(a, (void *) cdsA),
//                LogPrPhiCov(phi, (void *) cdsA),
//                LogPrPhiCov(b, (void *) cdsA));
//         fflush(NULL);

        iter = 0;

//         printf ("%5d [%.7f, %.7f] %.7f %.7f\n", iter, a, b, phi, b - a);
//         fflush(NULL);

        F.function = &LogPrPhiCov;
        F.params = (void *) cdsA;
        dlt_min_fminimizer_set(s, &F, phi, a, b); // DLTGSL

        do
        {
            iter++;
            status = gsl_min_fminimizer_iterate(s);

            phi = gsl_min_fminimizer_x_minimum(s);
            a = gsl_min_fminimizer_x_lower(s);
            b = gsl_min_fminimizer_x_upper(s);

            status = gsl_min_test_interval(a, b, absprec, relprec);

//             if (status == GSL_SUCCESS)
//                 printf ("Converged:\n");
//
//             printf ("%5d [%.7f, %.7f] %.7f %.7f\n", iter, a, b, phi, b - a);
//             printf("a: %g phi: %g b: %g\n",
//                    LogPrPhiCov(a, (void *) cdsA),
//                    LogPrPhiCov(phi, (void *) cdsA),
//                    LogPrPhiCov(b, (void *) cdsA));
            fflush(NULL);
        }
        while(status == GSL_CONTINUE && iter < max_iter);

//         printf ("%5d [%.7f, %.7f] %.7f %.7f\n", iter, a, b, phi, b - a);
//         fflush(NULL);

        //phi += FLT_MIN;

// DLT TEST
// stats->hierarch_p1 = 0.5*phi;
// marg2 = CalcMgLogL(cdsA);

// printf("PhiMargLogL ///////////////////////////////////////////// % e % e % e\n",
//     marg1, marg2, marg2 - marg1);
// fflush(NULL); // DLT TEST

        InvGammaAdjustEvals(evals, vlen, cnum, tmpevals, phi, ndf);
        EigenReconSym(cdsA->CovMat, (const double **) evecs, evals, vlen);

// marg3 = CalcMgLogL(cdsA);

// printf("VarMargLogL ///////////////////////////////////////////// % e % e % e\n",
//        marg2, marg3, marg3 - marg2);
// fflush(NULL); // DLT TEST

// stats->hierarch_p1 = 0.5*phi;
// printf("MargLogL ///////////////////////////////////////////// % e\n",
//         CalcMgLogL(cdsA));

        for (i = 0; i < vlen; ++i)
            invevals[i] = 1.0 / evals[i];

        EigenReconSym(cdsA->WtMat, (const double **) evecs, invevals, vlen);

        if (algo->rounds < 3)
        {
            for (i = 0; i < vlen; ++i)
                for (j = 0; j < i; ++j)
                    cdsA->WtMat[i][j] = cdsA->WtMat[j][i] = 0.0;
        }

        for (i = 0; i < vlen; ++i)
            cdsA->var[i] = cdsA->CovMat[i][i];

        chi2 = chi_sqr_adapt(evals, vlen, 0, &logL, 0.5*phi, 0.5*ndf,
                             invgamma_pdf, invgamma_lnpdf, invgamma_int);
    }

    gsl_min_fminimizer_free(s);

    stats->phi = phi;
    stats->hierarch_p1 = 0.5*phi;
    stats->hierarch_p2 = 0.5*ndf;
    stats->hierarch_chi2 = chi2;
}


/* marginal solution, currently the default (2015-04-06) */
/* DEFAULT */
/* Fit of the variances/eigenvalues to an inverse gamma distribution with
   a fixed c shape parameter.
   c > 1 -> guarantees that the distribution has a finite mean
   c > 2 -> finite variance
   c > 3 -> finite skewness
   c > 4 -> finite kurtosis
*/
void
InvGammaFitMarginalMLPhi(CdsArray *cdsA) // DLT FIX: not yet modified for missing data (does it need to be?)
{
    const int           vlen = cdsA->vlen, cnum = cdsA->cnum;
    int                 i, iter;
    double              oldphi = 2.0*stats->hierarch_p1;
    double              phi, sum;
    double              chi2 = 0.0, logL;
    double              ndf = algo->ndf;

    phi = DBL_MAX;

    if (algo->varweight > 0)
    {
        double     *svar = cdsA->samplevar;
        double     *var = cdsA->var;
//VecPrint(var, vlen);
        iter = 0;
        while(fabs(phi - oldphi) > 1e-8)
        {
            oldphi = phi;
            sum = 0.0;
            for (i = 0; i < vlen; ++i)
                sum += 1.0 / var[i];

            phi = ndf*vlen/(sum+2.0);
            //phi = (ndf*vlen - 2.0)/tmpsum;

//             if (iter)
//                 printf ("%5d %.7f [%.7f, %.7f] %.7f\n", iter, sum, oldphi, phi, oldphi-phi);

            ++iter;

// if (algo->alignment) // DLT TEST
//  marg2 = CalcMgLogLNu(cdsA);
// else
//  marg2 = CalcMgLogL(cdsA);

// printf("PhiMargLogL ///////////////////////////////////////////// % e % e % e\n",
//     marg1, marg2, marg2 - marg1);
// fflush(NULL); // DLT TEST

            if (algo->alignment)
                InvGammaAdjustVarPhiNu(cdsA->cds, var, vlen, cnum, svar, phi, ndf);
            else
                InvGammaAdjustVarPhi(var, vlen, cnum, svar, phi, ndf);
        }
        //printf("\n");
// if (algo->alignment) // DLT TEST
//  marg3 = CalcMgLogLNu(cdsA);
// else
//  marg3 = CalcMgLogL(cdsA);

// printf("Var2MargLogL ///////////////////////////////////////////// % e % e % e\n",
//     marg2, marg3, marg3 - marg2);
// fflush(NULL); // DLT TEST

        chi2 = chi_sqr_adapt(var, vlen, 0, &logL, 0.5*phi, 0.5*ndf,
                             invgamma_pdf, invgamma_lnpdf, invgamma_int);
    }
    else if (algo->covweight > 0)
    {
        double     *evals = cdsA->evals;
        double     *invevals = cdsA->tmpvecK;
        double    **evecs = cdsA->tmpmatKK2;
        double     *tmpevals = cdsA->samplevar;
        int         i, j;

        /* EigenGSL 0 evals are small to large */
        EigenGSL((const double **) cdsA->CovMat, vlen, tmpevals, evecs, 0);
//        VecPrint(tmpevals, vlen);

// double marg1, marg2, marg3; // DLT TEST
// memcpy(evals, tmpevals, vlen*sizeof(double));
// marg1 = CalcMgLogL(cdsA);

        //tmpevals[0] = phi/2;//ExpXn(phi/2, ndf, vlen);

//printf("phi:%f %f\n", oldphi, tmpevals[0]); fflush(NULL);

        iter = 0;
        while(fabs(phi - oldphi) > 1e-8)
        {
            oldphi = phi;
            sum = 0.0;
            for (i = 0; i < vlen; ++i)
                sum += 1.0 / evals[i];

            phi = ndf*vlen/(sum+2.0);
            //phi = (ndf*vlen - 2.0)/tmpsum;

            //if (iter)
            //printf ("%5d [%.7f, %.7f] %.7f\n", iter, oldphi, phi, oldphi-phi);
            ++iter;

    // DLT TEST
    // stats->hierarch_p1 = 0.5*phi;
    // marg2 = CalcMgLogL(cdsA);

    // printf("PhiMargLogL ///////////////////////////////////////////// % e % e % e\n",
    //     marg1, marg2, marg2 - marg1);
    // fflush(NULL); // DLT TEST

            InvGammaAdjustEvals(evals, vlen, cnum, tmpevals, phi, ndf);
            EigenReconSym(cdsA->CovMat, (const double **) evecs, evals, vlen);
        }
// marg3 = CalcMgLogL(cdsA);

// printf("VarMargLogL ///////////////////////////////////////////// % e % e % e\n",
//        marg2, marg3, marg3 - marg2);
// fflush(NULL); // DLT TEST

// stats->hierarch_p1 = 0.5*phi;
// printf("MargLogL ///////////////////////////////////////////// % e\n",
//         CalcMgLogL(cdsA));

        for (i = 0; i < vlen; ++i)
            invevals[i] = 1.0 / evals[i];

        EigenReconSym(cdsA->WtMat, (const double **) evecs, invevals, vlen);

        // Normalize the inverse cov mat (for translations); also shouldn't matter for rotations
//         double wtsum = 0.0;
//         for (i = 0; i < vlen; ++i)
//             for (j = 0; j < vlen; ++j)
//                 wtsum += cdsA->WtMat[i][j];
//
//         for (i = 0; i < vlen; ++i)
//             for (j = 0; j < vlen; ++j)
//                 cdsA->WtMat[i][j] /= wtsum;

        if (algo->rounds < 3)
        {
            for (i = 0; i < vlen; ++i)
                for (j = 0; j < i; ++j)
                    cdsA->WtMat[i][j] = cdsA->WtMat[j][i] = 0.0;
        }

        for (i = 0; i < vlen; ++i)
            cdsA->var[i] = cdsA->CovMat[i][i];

//VecPrint(evals, vlen);

        //chi2 = chi_sqr_adapt(cdsA->var, vlen, 0, &logL, 0.5*phi, 0.5*ndf,
        //                     invgamma_pdf, invgamma_lnpdf, invgamma_int);
    }

    stats->phi = phi;
    stats->hierarch_p1 = 0.5*phi;
    stats->hierarch_p2 = 0.5*ndf;
    stats->hierarch_chi2 = chi2;
}


/* Finding the root doesn't seem to work */
#include <gsl/gsl_roots.h>
void
InvGammaFitMarginalGSLRoot(CdsArray *cdsA)
{
    const int           vlen = cdsA->vlen, cnum = cdsA->cnum;
    int                 status, i;
    int                 iter, max_iter = 100;
    const               gsl_root_fdfsolver_type *T = NULL;
    gsl_root_fdfsolver *s = NULL;
    double              oldphi = 2.0*stats->hierarch_p1;
    double              phi, phi0;
    double              chi2 = 0.0, logL;
    double              nu = 1.0;
    gsl_function_fdf    FDF;
    double              prec = FLT_MIN;

    phi = oldphi;

    if (algo->varweight > 0)
    {
        double     *var = cdsA->samplevar;

        nu = 1.0;

        CalcSampleVar(cdsA);

        phi = 0.0;
        for (i = 0; i < vlen; ++i)
            phi += var[i];

        phi /= vlen;
        phi0 = phi;

        FDF.f = &LogPrPhi_f;
        FDF.df = &LogPrPhi_df;
        FDF.fdf = &LogPrPhi_fdf;
        FDF.params = (void *) cdsA;

        T = gsl_root_fdfsolver_newton;
        s = gsl_root_fdfsolver_alloc(T);

        gsl_root_fdfsolver_set(s, &FDF, phi);

        iter = 0;
//         printf ("%5d [%.7f, %.7f] %.7f\n", iter, phi, phi0, phi - phi0);
//         fflush(NULL);
        do
        {
            iter++;
            status = gsl_root_fdfsolver_iterate(s);
            phi0 = phi;
            phi = gsl_root_fdfsolver_root (s);
            status = gsl_root_test_delta (phi, phi0, 0, prec);

//             if (status == GSL_SUCCESS)
//                 printf ("Converged:\n");
//
//             printf ("%5d [%.7f, %.7f] %.7f\n", iter, phi, phi0, phi - phi0);
//             fflush(NULL);
        }
        while(status == GSL_CONTINUE && iter < max_iter);

        InvGammaAdjustVarPhi(cdsA->var, vlen, cnum, var, phi, nu);

        chi2 = chi_sqr_adapt(cdsA->var, vlen, 0, &logL, 0.5*phi, 0.5*nu,
                             invgamma_pdf, invgamma_lnpdf, invgamma_int);
    }

    gsl_root_fdfsolver_free(s);

    stats->hierarch_p1 = 0.5*phi;
    stats->hierarch_p2 = 0.5*nu;
    stats->hierarch_chi2 = chi2;
}


/* DLT 2008-03-28 new */
/* Assumes a known shape param c, real ML-EM fit.
   Estimates smallest 4 eigenvalues as expected (inverse) values given other larger evals.
   Uses expectation of inverse variances.
*/
void
InvGammaFitEvalsEMFixedC(CdsArray *cdsA, const double c, int iterate)
{
    double         *newvar = NULL;
    double         *evals = NULL;
    double         *variance = NULL;
    double        **evecs = NULL;
    int            *missi = NULL;
    double          precision = algo->precision;
    const int       vlen = cdsA->vlen, cnum = cdsA->cnum;
    double          nd, oldb, oldc, b = 0.0, chi2 = 0.0, logL, xn1, expinvx, gt;
    int             i, j, count, newlen, missing;

    newvar = malloc(vlen * sizeof(double));
    nd = 3.0 * cnum;
    oldb = oldc = DBL_MAX;

    if (algo->varweight > 0)
    {
        variance = cdsA->var;
        b = stats->hierarch_p1;
        missing = 4;
        missi = malloc(missing * sizeof(int));

        memcpy(newvar, variance, vlen * sizeof(double));

        if (algo->verbose)
            printf("\n0>>>>>>>>>>>>>");

        count = 0;
        do
        {
            oldb = b;

            /* qsort-dblcmp sorts small to big */
            qsort(newvar, vlen, sizeof(double), dblcmp);
            xn1 = newvar[missing];
            invgamma_fixed_c_EM_fit(newvar, vlen, missing, &b, c, &logL);

            if (algo->alignment)
                InvGammaAdjustVarNu(cdsA->cds, newvar, vlen, cnum, variance, b, c);
            else
                InvGammaAdjustVar(newvar, vlen, cnum, variance, b, c);

            if (algo->verbose)
            {
                printf("\n>>>>> %3d b:% -3.2e c:% -3.2e mode:%-10.5f logL:% -12.6f chi2:%-10.5f",
                       count, b, c, b / (c+1.0), logL, chi2);
                fflush(NULL);
            }

            if (count > 300)
            {
                printf("\n WARNING04: Failed to converge in InvGammaFitEvalsEMFixedC(), round %d\n    ",
                       algo->rounds);
                fflush(NULL);
                break;
            }

            if (iterate == 0)
                break;

            ++count;
        }
        while(fabs(b - oldb) > fabs(b*precision));

        expinvx = 1.0 / ExpInvXn(b, c, xn1);

//         printf("\n%g", 1.0 / expinvx);
//         printf("\n%g", 1 / xn1);
//         printf("\n%g\n", 2*(1/xn1 - 1/newvar[missing+1]) + 1/xn1);

        gt = 0.0;
        for (j = 0; j < missing; ++j)
        {
            missi[j] = findsmallest_gt(variance, vlen, gt);

            if (algo->verbose)
            {
                printf("\n>>>>>>>>>>>>>> %d", j);
                printf("\n>>>>>>>>>>>>>>   pre-var       xn1   expinvx");
                printf("\n1>>>>>>>>>>>>> %8.3e %8.3e %8.3e\n", variance[missi[j]], xn1, expinvx);
                fflush(NULL);
            }

            gt = variance[missi[j]];
            /* this is because the inverse of the variance is always used in other
               calculations/maximizations, yet I store it as the variance (uninverted) */
        }

        for (j = 0; j < missing; ++j)
            variance[missi[j]] = expinvx;

        MyFree(missi);
        chi2 = chi_sqr_adapt(variance, vlen, 0, &logL, b, c, invgamma_pdf, invgamma_lnpdf, invgamma_int);
    }
    else if (algo->covweight > 0)
    {
        double     *invevals = cdsA->tmpvecK;
        evecs = cdsA->tmpmatKK2;
        evals = cdsA->var;

/*         if (vlen - 3 < nd - 6) */
/*             newlen = vlen - 3; */
/*         else */
/*             newlen = nd - 6; */

        if (vlen - 4 < nd - 4)
            newlen = vlen - 4;
        else
            newlen = nd - 4;

        missing = vlen - newlen;

        //eigensym((const double **) cdsA->CovMat, evals, evecs, vlen);
        /* evals are small to large */
        EigenGSL((const double **) cdsA->CovMat, vlen, evals, evecs, 0);
        //printf("missing:%d\n\n", missing);

        for (i = 0; i < missing; ++i)
            evals[i] = 0.0;

        //VecPrint(evals, vlen);

        b = stats->hierarch_p1;

        memcpy(newvar, evals, vlen * sizeof(double));

        if (algo->verbose)
            printf("\n0>>>>>>>>>>>>>");

        count = 0;
        do
        {
            oldb = b;

            qsort(newvar, vlen, sizeof(double), dblcmp);

            xn1 = newvar[missing];
            //chi2 = invgamma_fixed_c_EM_fit(newvar, vlen, missing, &b, c, &logL);

            chi2 = invgamma_fixed_c_EM_fit(newvar, vlen, missing, &b, c, &logL);

            for (i = missing; i < vlen; ++i)
                newvar[i] = (nd*evals[i] + 2.0*b) / (nd + 2.0*(1.0 + c));

            if (algo->verbose)
            {
                printf("\n>>>>> %3d b:% -3.2e c:% -3.2e mode:%-10.5f logL:% -12.6f chi2:%-10.5f",
                       count, b, c, b / (c+1.0), logL, chi2);
                fflush(NULL);
            }

            if (count > 300)
            {
                printf("\n WARNING04: Failed to converge in InvGammaFitEvalsEMFixedC(), round %d\n    ",
                       algo->rounds);
                fflush(NULL);
                break;
            }

            count++;

            if (iterate == 0 || algo->abort)
                break;
        }
        while(fabs(oldb - b) > fabs(b*precision));

        if (algo->verbose)
        {
            printf("\n>>>>> Final: %3d b:% -3.2e c:% -3.2e mode:%-10.5f logL:% -12.6f chi2:%-10.5f minvar:% -3.2e",
                   count, b, c, b / (c+1.0), logL, chi2, 2.0*b / (nd + 2.0*(1.0 + c)));
            fflush(NULL);
        }

        InvGammaAdjustVar(evals+missing, vlen-missing, cnum, evals+missing, b, c);

        xn1 = evals[missing];

        if (algo->verbose)
        {
            printf("\n1>>>> %8.3e %8.3e", evals[0], xn1);
            fflush(NULL);
        }

        expinvx = 1.0 / ExpInvXn(b, c, xn1);

        for (i = 0; i < missing; ++i)
            evals[i] = expinvx;

        if (algo->verbose)
        {
            printf("\n2>>>> %8.3e %8.3e\n", xn1, evals[0]);
            fflush(NULL);
        }

        //chi2 = chi_sqr_adapt(evals, vlen, 0, &logL, b, c, invgamma_pdf, invgamma_lnpdf, invgamma_int);
        //chi2 = chi_sqr_adapt(newvar, newlen, 0, &logL, b, c, invgamma_pdf, invgamma_lnpdf, invgamma_int);

        EigenReconSym(cdsA->CovMat, (const double **) evecs, evals, vlen);

        for (i = 0; i < vlen; ++i)
            invevals[i] = 1.0 / evals[i];

        EigenReconSym(cdsA->WtMat, (const double **) evecs, invevals, vlen);

        for (i = 0; i < vlen; ++i)
            cdsA->var[i] = cdsA->CovMat[i][i];
    }

/*     for (i = 0; i < vlen; ++i) */
/*        printf("\n%3d %e", i, evals[i]); */

    stats->hierarch_p1 = b;
    stats->hierarch_p2 = c;
    stats->hierarch_chi2 = chi2;

    MyFree(newvar);
}


/* ML-EM fit, fitting unknown b and c inverse gamma params (scale and shape, resp.)
   Fits expectation of single smallest eigenvlaue for variance weighting.
*/
void
InvGammaFitEvalsML(CdsArray *cdsA, int iterate)
{
    double         *newvar = NULL;
    double         *evals = NULL;
    double         *variance = NULL;
    double        **evecs = NULL;
    double          precision = algo->precision;
    const int       vlen = cdsA->vlen, cnum = cdsA->cnum;
    double          nd, oldb, oldc, b, c, chi2 = 0.0, logL, xn1, expinvx;
    int             i, count, newlen, missing, smallest;

    newvar = malloc(vlen * sizeof(double));
    nd = 3.0 * cnum;
    oldb = oldc = DBL_MAX;

    b = stats->hierarch_p1;
    c = stats->hierarch_p2;

    if (algo->varweight)
    {
        variance = cdsA->var;

        missing = 1;

        memcpy(newvar, variance, vlen * sizeof(double));

        if (algo->verbose)
            printf("\n0>>>>>>>>>>>>>");

        count = 0;
        do
        {
            oldb = b;

            if (algo->verbose)
            {
                printf("\n>>>>> %3d b:% -3.2e c:% -3.2e mode:%-10.5f logL:% -12.6f chi2:%-10.5f",
                       count, b, c, b / (c+1.0), logL, chi2);
                fflush(NULL);
            }

            if (algo->alignment)
                InvGammaAdjustVarNu(cdsA->cds, newvar, vlen, cnum, variance, b, c);
            else
                InvGammaAdjustVar(newvar, vlen, cnum, variance, b, c);

            /* qsort-dblcmp sorts small to big */
            qsort(newvar, vlen, sizeof(double), dblcmp);
            xn1 = newvar[1];
            invgamma_EMsmall_fit(newvar, vlen, missing, &b, &c, &logL);

            if (count > 300)
            {
                printf("\n WARNING04: Failed to converge in InvGammaFitEvalsML(), round %d\n    ",
                       algo->rounds);
                fflush(NULL);
                break;
            }

            if (iterate == 0)
                break;

            ++count;
        }
        while(fabs(oldb - b) > fabs(b*precision));

        InvGammaAdjustVar(variance, vlen, cnum, variance, b, c);

        smallest = findsmallest(variance, vlen);

        if (algo->verbose)
        {
            printf("\n1>>>> %8.3e %8.3e", variance[smallest], xn1);
            fflush(NULL);
        }

        //variance[smallest] = ExpXn(b, c, xn1);
        variance[smallest] = 1.0 / ExpInvXn(b, c, xn1);
        /* this is because the inverse of the variance is always used in other
           calculations/maximizations, yet I store it as the variance (uninverted) */
        //printf("\n-->-->-->-->--> %8.3e %8.3e\n", variance[smallest], ExpXn(b, c, xn1));

        if (algo->verbose)
        {
            printf("\n2>>>> %8.3e %8.3e\n", xn1, variance[smallest]);
            fflush(NULL);
        }

        chi2 = chi_sqr_adapt(variance, vlen, 0, &logL, b, c, invgamma_pdf, invgamma_lnpdf, invgamma_int);
    }
    else if (algo->covweight)
    {
        evecs = cdsA->tmpmatKK2;
        evals = cdsA->var;

        if (vlen - 3 < nd - 6)
            newlen = vlen - 3;
        else
            newlen = nd - 6;

        missing = vlen - newlen;

        //eigensym((const double **) cdsA->CovMat, evals, evecs, vlen);
        /* eigensym evals are small to large */
        EigenGSL((const double **) cdsA->CovMat, vlen, evals, evecs, 0);

        for (i = 0; i < missing; ++i)
            evals[i] = 0.0;

        memcpy(newvar, evals, vlen * sizeof(double));

        if (algo->verbose)
            printf("\n0>>>>>>>>>>>>>");

        count = 0;
        do
        {
            oldb = b;

            InvGammaAdjustVar(newvar+missing, vlen-missing, cnum, evals+missing, b, c);

            qsort(newvar, vlen, sizeof(double), dblcmp);

            if (algo->verbose)
            {
                printf("\n>>>>> %3d b:% -3.2e c:% -3.2e mode:%-10.5f logL:% -12.6f chi2:%-10.5f",
                       count, b, c, b / (c+1.0), logL, chi2);
                fflush(NULL);
            }

            xn1 = newvar[missing];
            chi2 = invgamma_EMsmall_fit(newvar, vlen, missing, &b, &c, &logL);

//            for (i = missing; i < vlen; ++i)
//                newvar[i] = (nd*evals[i] + 2.0*b) / (nd + 2.0*(1.0 + c));

            if (count > 300)
            {
                printf("\n WARNING04: Failed to converge in InvGammaFitEvalsML(), round %d\n    ",
                       algo->rounds);
                fflush(NULL);
                break;
            }

            count++;

            if (iterate == 0 || algo->abort)
                break;
        }
        while(fabs(oldb - b) > fabs(b*precision));

        if (algo->verbose)
        {
            printf("\n>>>>> Final: %3d b:% -3.2e c:% -3.2e mode:%-10.5f logL:% -12.6f chi2:%-10.5f minvar:% -3.2e",
                   count, b, c, b / (c+1.0), logL, chi2, 2.0*b / (nd + 2.0*(1.0 + c)));
            fflush(NULL);
        }

        InvGammaAdjustVar(evals+missing, vlen-missing, cnum, evals+missing, b, c);

        xn1 = evals[missing];

        if (algo->verbose)
        {
            printf("\n1>>>> %8.3e %8.3e", evals[0], xn1);
            fflush(NULL);
        }

        expinvx = 1.0 / ExpInvXn(b, c, xn1);;

        for (i = 0; i < missing; ++i)
            evals[i] = expinvx;

        if (algo->verbose)
        {
            printf("\n2>>>> %8.3e %8.3e\n", xn1, evals[0]);
            fflush(NULL);
        }

        //chi2 = chi_sqr_adapt(evals, vlen, 0, &logL, b, c, invgamma_pdf, invgamma_lnpdf, invgamma_int);
        //chi2 = chi_sqr_adapt(newvar, newlen, 0, &logL, b, c, invgamma_pdf, invgamma_lnpdf, invgamma_int);

        EigenReconSym(cdsA->CovMat, (const double **) evecs, evals, vlen);

        for (i = 0; i < vlen; ++i)
            evals[i] = cdsA->CovMat[i][i];
    }

//    for (i = 0; i < vlen; ++i)
//       printf("\n%3d %e", i, newvar[i]);

    stats->hierarch_p1 = b;
    stats->hierarch_p2 = c;
    stats->hierarch_chi2 = chi2;

    MyFree(newvar);
}


/* This is the old approximate method, used in versions 1.0-1.1  */
/* inverse gamma fit of variances, excluding the smallest 3 */
/* This accounts for the fact that the smallest three eigenvalues of the covariance
   matrix are always zero, i.e. the covariance matrix is necessarily of rank
   vlen - 3 (or usually less, with inadequate amounts of data 3N-6). */
void
InvGammaFitEvals(CdsArray *cdsA, int iterate)
{
    double         *newvar = NULL;
    double         *variance = cdsA->var;
    //double        **evecs = NULL;
    double          precision = algo->precision;
    const int       vlen = cdsA->vlen, cnum = cdsA->cnum;
    double          nd, oldb, oldc, b, c, chi2 = DBL_MAX, logL, harmave;
    int             i, count, newlen;

    newvar = malloc(vlen * sizeof(double));
    nd = 3.0 * cnum;
    oldb = oldc = DBL_MAX;

    b = stats->hierarch_p1;
    c = stats->hierarch_p2;

    if (algo->varweight)
    {
        newlen = vlen - 3;

        memcpy(newvar, variance, vlen * sizeof(double));

        count = 0;
        do
        {
            oldb = b;
            oldc = c;

            qsort(newvar, vlen, sizeof(double), dblcmp_rev);
            chi2 = invgamma_fit(newvar, newlen, &b, &c, &logL);

            if (2*c + 2 > 1000 * nd)
            {
                harmave = HarmonicAve(newvar + vlen - newlen, newlen);

                for (i = 0; i < vlen; ++i)
                    variance[i] = harmave;

                return;
            }

            if (algo->alignment)
                InvGammaAdjustVarNu(cdsA->cds, newvar, vlen, cnum, variance, b, c);
            else
                InvGammaAdjustVar(newvar, vlen, cnum, variance, b, c);

            /* newvar[findmin(variance, vlen)] = 2.0*b / (nd + 2.0*(1.0 + c)); */

            if (algo->verbose)
            {
                printf(">>>>> %3d b:% -3.2e c:% -3.2e mode:%-10.5f logL:% -12.6f chi2:%-10.5f minvar:% -3.2e\n",
                       count, b, c, b / (c+1.0), logL, chi2, 2.0*b / (nd + 2.0*(1.0 + c)));
                fflush(NULL);
            }

            count++;

            if (iterate == 0 || algo->abort)
                break;
        }
        while(fabs(oldb - b) > fabs(b*precision) &&
              fabs(oldc - c) > fabs(c*precision));

        if (algo->verbose)
        {
            printf(">>>>> Final: %3d b:% -3.2e c:% -3.2e mode:%-10.5f logL:% -12.6f chi2:%-10.5f minvar:% -3.2e\n",
                   count, b, c, b / (c+1.0), logL, chi2, 2.0*b / (nd + 2.0*(1.0 + c)));
            fflush(NULL);
        }

        if (algo->alignment)
            InvGammaAdjustVarNu(cdsA->cds, variance, vlen, cnum, variance, b, c);
        else
            InvGammaAdjustVar(variance, vlen, cnum, variance, b, c);
    }
    else if (algo->covweight)
    {
        if (vlen - 3 < nd - 6)
            newlen = vlen - 3;
        else
            newlen = nd - 6;

        //evecs = cdsA->tmpmatKK2;

        //eigenvalsym((const double **) cdsA->CovMat, variance, evecs, vlen);
        /* eigensym((const double **) cdsA->CovMat, variance, evecs, vlen); */
        EigenvalsGSL((const double **) cdsA->CovMat, vlen, variance);

        /* RevVecIp(variance, vlen); */

        for (i = 0; i < vlen - newlen; ++i)
            variance[i] = 0.0;

/*         b = c = 0.0; */

        for (i = 0; i < vlen; ++i)
            newvar[i] = (nd*variance[i] + 2.0*b) / (nd + 2.0*(1.0 + c));

        count = 0;
        do
        {
            oldb = b;
            oldc = c;

            chi2 = invgamma_fit(newvar + vlen - newlen, newlen, &b, &c, &logL);

            for (i = 0; i < vlen; ++i)
                newvar[i] = (nd*variance[i] + 2.0*b) / (nd + 2.0*(1.0 + c));

            if (algo->verbose)
            {
                printf(">>>>> %3d b:% -3.2e c:% -3.2e mode:%-10.5f logL:% -12.6f chi2:%-10.5f minvar:% -3.2e\n",
                       count, b, c, b / (c+1.0), logL, chi2, 2.0*b / (nd + 2.0*(1.0 + c)));
                fflush(NULL);
            }

            if (count > 100)
            {
                printf("\n WARNING01: Failed to converge in InvGammaFitVars(), round %d\n",
                       algo->rounds);
                fflush(NULL);
                break;
            }

            count++;

            if (iterate == 0 || algo->abort)
                break;
        }
        while(fabs(oldb - b) > fabs(b*precision) &&
              fabs(oldc - c) > fabs(c*precision));

        if (algo->verbose)
        {
            printf(">>>>> Final: %3d b:% -3.2e c:% -3.2e mode:%-10.5f logL:% -12.6f chi2:%-10.5f minvar:% -3.2e\n",
                   count, b, c, b / (c+1.0), logL, chi2, 2.0*b / (nd + 2.0*(1.0 + c)));
            fflush(NULL);
        }

        InvGammaAdjustCov(cdsA, b, c);
/*      printf("\n\n count: %d", count); */
/*      for (i = 0; i < vlen; ++i) */
/*          printf("\n%3d %8.3e %8.3e", i, newvar[i], mode); */
/*         EigenReconSym(cdsA->CovMat, (const double **) evecs, (const double *) newvar, vlen); */

        for (i = 0; i < vlen; ++i)
            variance[i] = cdsA->CovMat[i][i];

/*         printf("\n\n count: %d  harm ave: %8.3e log ave: %8.3e", */
/*                count, HarmonicAve(variance, vlen), exp(LogarithmicAve(variance, vlen))); */
    }

    stats->hierarch_p1 = b;
    stats->hierarch_p2 = c;
    stats->hierarch_chi2 = chi2;

    MyFree(newvar);
}

