/*
    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 <time.h>
#include <ctype.h>
#include <float.h>
#include <sys/types.h>
#include <unistd.h>
#include "Error.h"
#include "pdbMalloc.h"
#include "pdbStats.h"
#include "pdbUtils.h"
#include "pdbIO.h"
#include "Cds.h"
#include "MultiPose.h"
#include "PDBCds.h"
#include "distfit.h"
#include "DLTmath.h"
#include "RandCds.h"
#include "ProcGSLSVD.h"
#include "DLTmath.h"
#include "libdistfit/vonmises_dist.h"
#include "myassert.h"
#include <gsl/gsl_math.h>
#include <gsl/gsl_sf_hyperg.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>


static double
ScaleMax(const double n, const double gamma, const double phi)
{
    return((gamma + sqrt(gamma*gamma + 4.0*phi*(n-1.0)))/(2.0*phi));
}


/* Parabolic cylinder function D_n(z)
   See Abramowitz and Stegun p 510, 13.6.36
   also see Ch 19, 19.3, 19.5.1, etc.
   The parabolic cylinder function a type of conßuent hypergeometric function,
   deÞned in Gradshteyn and Ryzhik p 1028, section 9.24-9.25.

   http://mathworld.wolfram.com/ParabolicCylinderFunction.html

   gsl_sf_hyperg_U calculates the hypergeometric function of the 2nd kind:

   http://mathworld.wolfram.com/ConfluentHypergeometricFunctionoftheSecondKind.html
*/
/* NB:  THIS IS BROKEN.  gsl_sf_hyperg_U doesn't work for some large arguments --
   I'm unsure exactly which, but it sucks and makes the fxn useless for me.
   It does, however, successfully reproduce ALL the tables in A&S (pp 702-710). */
static double
CalcDnz(const double n, const double z)
{
    return(pow(2.0, 0.5 * n) * exp(-0.25 * z*z ) * gsl_sf_hyperg_U(-0.5 * n, 0.5, 0.5 * z*z));
}


/* NB:  This is broken for large arguments, because CalcDnz is broken. */
static double
CalcUax(const double a, const double x)
{
    return(CalcDnz(-a-0.5, x));
}


static void
AveCdsGibbs(CdsArray *cdsA)
{
    int             i, j;
    double         *avex = cdsA->avecds->x,
                   *avey = cdsA->avecds->y,
                   *avez = cdsA->avecds->z;
    const int       cnum = cdsA->cnum, vlen = cdsA->vlen;
    const Cds     **cds = (const Cds **) cdsA->cds;
    Cds            *cdsj = NULL;
    double          invcnum = 1.0 / (double) cnum;

    memset(avex, 0, vlen * sizeof(double));
    memset(avey, 0, vlen * sizeof(double));
    memset(avez, 0, vlen * sizeof(double));

    for (j = 0; j < cnum; ++j)
    {
        cdsj = (Cds *) cds[j];

        for (i = 0; i < vlen; ++i)
        {
            avex[i] += cdsj->x[i];
            avey[i] += cdsj->y[i];
            avez[i] += cdsj->z[i];
        }
    }

    for (i = 0; i < vlen; ++i)
    {
        avex[i] *= invcnum;
        avey[i] *= invcnum;
        avez[i] *= invcnum;
    }
}


static void
RotMatToQuaternion(const double **rot, double *quat)
{
    double              trace, s, w, x, y, z;

    /* convert to quaternion */
    trace = rot[0][0] + rot[1][1] + rot[2][2] + 1.0;

    if( trace > FLT_EPSILON )
    {
        s = 0.5 / sqrt(trace);
        w = 0.25 / s;
        x = ( rot[2][1] - rot[1][2] ) * s;
        y = ( rot[0][2] - rot[2][0] ) * s;
        z = ( rot[1][0] - rot[0][1] ) * s;
    }
    else
    {
        if (rot[0][0] > rot[1][1] && rot[0][0] > rot[2][2])
        {
            s = 2.0 * sqrt( 1.0 + rot[0][0] - rot[1][1] - rot[2][2]);
            x = 0.25 * s;
            y = (rot[0][1] + rot[1][0] ) / s;
            z = (rot[0][2] + rot[2][0] ) / s;
            w = (rot[1][2] - rot[2][1] ) / s;
        }
        else if (rot[1][1] > rot[2][2])
        {
            s = 2.0 * sqrt(1.0 + rot[1][1] - rot[0][0] - rot[2][2]);
            x = (rot[0][1] + rot[1][0] ) / s;
            y = 0.25 * s;
            z = (rot[1][2] + rot[2][1] ) / s;
            w = (rot[0][2] - rot[2][0] ) / s;
        }
        else
        {
            s = 2.0 * sqrt(1.0 + rot[2][2] - rot[0][0] - rot[1][1]);
            x = (rot[0][2] + rot[2][0] ) / s;
            y = (rot[1][2] + rot[2][1] ) / s;
            z = 0.25 * s;
            w = (rot[0][1] - rot[1][0] ) / s;
        }
    }

    quat[0] = -w;
    quat[1] = x;
    quat[2] = y;
    quat[3] = z;
}


static double
wrap_nPI_pPI(double x)
{
    while(x < -MY_PI)
        x += 2.0*MY_PI;

    while(x > MY_PI)
        x -= 2.0*MY_PI;

    return(x);
}


static double
mardia_gadsden_target_ratio(const double a, const double b, const double x, const double y)
{
    return(exp(a * (cos(y) - cos(x)) + b * (sin(y) - sin(x))) * cos(y) / cos(x));
}


static double
mardia_gadsden_met3(const double a, const double b, const double x, const gsl_rng *r2, const double width)
{
    double          r, y, u;
//    unsigned long   seed = (unsigned long) time(NULL);

    //y = x + (2.0 * width * gsl_rng_uniform(r2) - width);
    //y = normal_dev3(x, width, r2);
    y = x + gsl_ran_gaussian_ziggurat(r2, width);
    y = wrap_nPI_pPI(y);
    r = mardia_gadsden_target_ratio(a, b, x, y);
    u = gsl_rng_uniform(r2);

//    printf("\nMETROPOLIS: % e % e -- % f % f", x, y, r, u);
//    fflush(NULL);

    if (u < r)
    {
//        printf("\nACCEPT: 1");
        return(y);
    }
    else
    {
//        printf("\nACCEPT: 0");
        return(x);
    }
}


static double
scale_log_target_ratio(const double n, const double gamma, const double phi, const double x, const double y)
{
    if (y <= 0.0)
    {
        return(-INFINITY);
    }
    else
    {
        double prior;

        double hngamma = -0.5*phi*(y*y - x*x) + gamma*(y-x) + (n-1.0) * log(y/x);

        // gamma prior
        //double k = 2.0;
        //double theta = 1.0 / 2.0;
        //prior = (k-1.0) * log(y/x) - (y-x)/theta; // gamma prior

        // lognormal prior on beta
        //double sigma = 0.1;
        //prior = -log(y/x) - (log(y)*log(y)-log(x)*log(x))/(2.0*sigma);

        // Jeffrey's prior
        prior = -log(y/x);

        return(hngamma + prior);
    }
}


static double
scale_log_met3(const double n, const double gamma, const double phi, double x, const gsl_rng *r2, const double loc, const double width, const int iters)
{
    double          r, y, u;
    int             i;
    double          jit;

    for (i = 0; i < iters; ++i)
    {
        //y = x + (2.0 * width * gsl_rng_uniform(r2) - width);
        //y = loc + gsl_ran_gaussian_ziggurat(r2, 3.0 * width);
        jit = gsl_ran_gaussian_ziggurat(r2, 3.0 * width);
        //y = x + gsl_ran_gaussian_ziggurat(r2, width);
        //jit = gsl_rng_uniform(r2) * 0.6 - 0.3;
        //jit = gsl_ran_gaussian_ziggurat(r2, 0.7);
        y = x + jit;

        r = scale_log_target_ratio(n, gamma, phi, x, y);
        u = log(gsl_rng_uniform(r2));

        //printf("\nMETROPOLIS: % e % e -- % f % f % f", x, y, r, u, jit);

        if (u < r)
        {
            //printf("\nACCEPT: 1 %e", y);
            x = y;
        }
        else
        {
            //printf("\nACCEPT: 0 %e", x);
        }
    }

    return(x);
}


/*
KVM
For simplicity of presentation take phi = 1
*/
double
scale_rejection(const double r, const double gamma, const double phi, const gsl_rng *r2)
{
    double         sqrtphi = sqrt(phi);
    double         g = gamma / sqrtphi;
    double         R, y, u;
    double         b = 0.5 * (g + sqrt(g*g + 4.0 * r));

    do
    {
        u = gsl_rng_uniform(r2);
        y = gsl_ran_gamma(r2, r, b - g);
//        t = y - gamma - 1.0;
        R = exp(-0.25 * g * g) * pow(b - g, -r) / CalcUax(r - 0.5, -g);

        printf("\nREJ: %e %e %e", u, y, R);
        fflush(NULL);
    }
    while (u >= R);

    return(y/sqrtphi);
}


// Sample from a von Mises distribution
//
// http://phaistos.sourceforge.net/doxygen/vonmises_8cpp_source.html
// This is Hamelryck's version from PHAISTOS, which should be golden ...
// I've checked this against Fisher 1993 myself.
//
// Return a von Mises distribution pseudo-random variate on [-pi, +pi].
// The implementation is similar to the algorithm by Best and Fisher,
// 1979;
// see N.I. Fisher, _Statistical Analysis of Circular Data_,
// Cambridge University Press, 1993, p. 49.
// Also reproduced in Mardia and Jupp, _Directional Statistics_,  2000, p 43.
//
// I added checks for when k is very small or very large.
//
// mu = mean, k = scale
double
vonmises_dev4(double mu, double k, const gsl_rng *r2)
{
    double U1, z, c, U2, U3;

//     if (mu < -M_PI || mu > M_PI)
//     {
//         fprintf(stderr, "VonMises Error: mu must be in the interval [-pi,pi]. mu=%f\n", mu);
//     }

    // von Mises converges to constant mu as k -> inf
    if (k >= (DBL_MAX-1)/2)
        return(fmod(mu, 2.0*M_PI));

    // Set sampling internals
    double a = 1.0 + sqrt(1.0 + 4.0*k*k);
    double b = (a - sqrt(2*a)) / (2*k);
    double r = (1.0 + b*b)/(2*b);

    // von Mises converges to uniform [-pi, pi] as k -> 0
    if (k <= 0.0 || !isfinite(b) || !isfinite(r))
        return((2.0*gsl_rng_uniform(r2)-1.0) * M_PI);

    double f, res;

    while(1)
    {
        U1 = gsl_rng_uniform(r2);
        z = cos(M_PI * U1);
        f = (1.0 + r*z)/(r + z);
        c = k * (r - f);
        U2 = gsl_rng_uniform(r2);

        if (((c*(2.0-c) - U2) > 0.0) || ((log(c/U2) + 1.0 - c >= 0.0)))
        {
            break; // accept
        }
    }

    U3 = gsl_rng_uniform(r2);

    if (U3 > 0.5)
    {
        res = fmod(acos(f)+mu, 2.0*M_PI);
    }
    else
    {
        res = fmod(-acos(f)+mu, 2.0*M_PI);
    }

    return(res);
}


/* http://www.gnu.org/software/gsl/manual/html_node/The-Gamma-Distribution.html
   double gsl_ran_gamma (const gsl_rng * r, double a, double b)
   p(x) dx = {1 \over \Gamma(a) b^a} x^{a-1} e^{-x/b} dx
*/
static double
invgamma_dev4(const double b, const double a, const gsl_rng *r2)
{
    return(1.0 / gsl_ran_gamma(r2, a, 1.0/b));
}


static void
VarCds(CdsArray *cdsA)
{
    int             i, j;
    double          sqrdist;
    double          tmpx, tmpy, tmpz;
    double          variance;
    const int       cnum = cdsA->cnum, vlen = cdsA->vlen;
    double         *var = cdsA->var;
    const Cds     **cds = (const Cds **) cdsA->cds;
    Cds            *cdsj = NULL;
    const double   *avex = (const double *) cdsA->avecds->x,
                   *avey = (const double *) cdsA->avecds->y,
                   *avez = (const double *) cdsA->avecds->z;

    variance = 0.0;
    for (i = 0; i < vlen; ++i)
    {
        sqrdist = 0.0;
        for (j = 0; j < cnum; ++j)
        {
            cdsj = (Cds *) cds[j];
            tmpx = cdsj->x[i] - avex[i];
            tmpy = cdsj->y[i] - avey[i];
            tmpz = cdsj->z[i] - avez[i];
            sqrdist += tmpx*tmpx + tmpy*tmpy + tmpz*tmpz;
        }

        var[i] = sqrdist / (3.0 * cnum);
        variance += var[i];
    }

    variance /= (double) vlen;
    stats->stddev = sqrt(variance);
    stats->var = variance;
}


static void
GibbsTrans(CdsArray *cdsA, const gsl_rng *r2)
{
    int             i, j;
    const int       cnum = cdsA->cnum, vlen = cdsA->vlen;
    double          var = stats->var;
    double          tvar, tvarbeta;
    double          newcen[3];
    Cds            *cdsi = NULL;
    Cds           **cds = cdsA->cds;

    tvar = sqrt(var / vlen);

//     printf("\ntvar: % e", tvar);
//     fflush(NULL);

    for (i = 0; i < cnum; ++i)
    {
        cdsi = cds[i];
        CenMass2((const double **) cdsi->sc,
                 vlen, cdsi->center);

        // For an in-place algorithm where X has already been scaled and rotated,
        // we don't need to scale and rotate the mean center.
        // For out-of-place, we have to apply the inverse scale and inverse
        // rotation to M.

        InvRotVec(&newcen[0], cdsA->avecds->center, cdsi->matrix);

        for (j = 0; j < 3; ++j)
            cdsA->avecds->center[j] = newcen[j] / cdsi->scale;
//
//         printf("\n  trans[%d]", i+1);
//         printf("\nB trans[%d]: % f % f % f", i+1,
//                cdsi->center[0], cdsi->center[1], cdsi->center[2]);
//         fflush(NULL);

        tvarbeta = tvar/cdsi->scale;
        //tvarbeta = tvar;

        cdsi->center[0] -= cdsA->avecds->center[0];
        cdsi->center[1] -= cdsA->avecds->center[1];
        cdsi->center[2] -= cdsA->avecds->center[2];

        cdsi->center[0] += gsl_ran_gaussian_ziggurat(r2, tvarbeta);
        cdsi->center[1] += gsl_ran_gaussian_ziggurat(r2, tvarbeta);
        cdsi->center[2] += gsl_ran_gaussian_ziggurat(r2, tvarbeta);

//         printf("\nA trans[%d]: % f % f % f\n", i+1,
//                cdsA->cds[i]->center[0], cdsA->cds[i]->center[1], cdsA->cds[i]->center[2]);
//         fflush(NULL);

        //ApplyCenterIp(cdsi);
        //NegTransCdsIp(cdsi, cdsi->center);

        TranslateCdsOp2(cdsi->wc,
                        (const double **) cdsi->sc,
                        vlen,
                        (const double *) cdsi->center);
    }
}


static void
GibbsTransDiag(CdsArray *cdsA, const gsl_rng *r2)
{
    int             i, j;
    const int       cnum = cdsA->cnum, vlen = cdsA->vlen;
    double          tvar, invtr, tvarbeta;
    double          newcen[3];
    Cds            *cdsi = NULL;
    Cds           **cds = cdsA->cds;

    invtr = 0.0;
    for (i = 0; i < vlen; ++i)
    {
        //cdsA->w[i] = 1.0 / cdsA->var[i];
        invtr += cdsA->w[i];
    }

    tvar = sqrt(1.0 / invtr);

//    printf("\ntvar: %e", tvar);

/*     for (i = 0; i < vlen; ++i) */
/*         printf("\n  vartrans[%3d]: % 11.5f % 11.5f", i+1, cdsA->var[i], cdsA->w[i]); */

    for (i = 0; i < cnum; ++i)
    {
        cdsi = cds[i];
        CenMassWt2((const double **) cdsi->sc,
                   (const double *) cdsA->w,
                   vlen,
                   cdsi->center);

        // For an in-place algorithm where X has already been scaled and rotated,
        // we don't need to scale and rotate the mean center.
        // For out-of-place, we have to apply the inverse scale and inverse
        // rotation to M.

        InvRotVec(&newcen[0], cdsA->avecds->center, cdsi->matrix);

        for (j = 0; j < 3; ++j)
            cdsA->avecds->center[j] = newcen[j] / cdsi->scale;
//
//         printf("\n  trans[%d]", i+1);
//         printf("\nB trans[%d]: % f % f % f", i+1,
//                cdsi->center[0], cdsi->center[1], cdsi->center[2]);
//         fflush(NULL);

        tvarbeta = tvar/cdsi->scale;
        //tvarbeta = tvar;

        cdsi->center[0] -= cdsA->avecds->center[0];
        cdsi->center[1] -= cdsA->avecds->center[1];
        cdsi->center[2] -= cdsA->avecds->center[2];

        cdsi->center[0] += gsl_ran_gaussian_ziggurat(r2, tvarbeta);
        cdsi->center[1] += gsl_ran_gaussian_ziggurat(r2, tvarbeta);
        cdsi->center[2] += gsl_ran_gaussian_ziggurat(r2, tvarbeta);

/*         printf("\nA trans[%d]: % f % f % f\n", i+1,  */
/*                cdsi->center[0], cdsi->center[1], cdsi->center[2]); */
/*         fflush(NULL); */

        TranslateCdsOp2(cdsi->wc,
                        (const double **) cdsi->sc,
                        vlen,
                        (const double *) cdsi->center);
        //NegTransCdsIp(cdsi, cdsi->center);
    }
}


/* A = (cds1' * cds2)' */
/* Actually retruns the transpose of A */
static void
CdsInnerProduct(double *A, Cds *cds1, Cds *cds2, const int vlen, const double *weight)
{
    double          x1, x2, y1, y2, z1, z2;
    int             i;
    const double   *fx1 = cds1->x, *fy1 = cds1->y, *fz1 = cds1->z;
    const double   *fx2 = cds2->x, *fy2 = cds2->y, *fz2 = cds2->z;

   memset(A, 0, 9 * sizeof(double));

    if (weight)
    {
        for (i = 0; i < vlen; ++i)
        {
             x1 = weight[i] * fx1[i];
             y1 = weight[i] * fy1[i];
             z1 = weight[i] * fz1[i];

             x2 = fx2[i];
             y2 = fy2[i];
             z2 = fz2[i];

             A[0] +=  (x1 * x2);
             A[1] +=  (y1 * x2);
             A[2] +=  (z1 * x2);

             A[3] +=  (x1 * y2);
             A[4] +=  (y1 * y2);
             A[5] +=  (z1 * y2);

             A[6] +=  (x1 * z2);
             A[7] +=  (y1 * z2);
             A[8] +=  (z1 * z2);
        }
    }
    else
    {
        for (i = 0; i < vlen; ++i)
        {
             x1 = fx1[i];
             y1 = fy1[i];
             z1 = fz1[i];

             x2 = fx2[i];
             y2 = fy2[i];
             z2 = fz2[i];

//              A[0] +=  (x1 * x2);
//              A[1] +=  (x1 * y2);
//              A[2] +=  (x1 * z2);
//
//              A[3] +=  (y1 * x2);
//              A[4] +=  (y1 * y2);
//              A[5] +=  (y1 * z2);
//
//              A[6] +=  (z1 * x2);
//              A[7] +=  (z1 * y2);
//              A[8] +=  (z1 * z2);

             A[0] +=  (x1 * x2);
             A[1] +=  (y1 * x2);
             A[2] +=  (z1 * x2);

             A[3] +=  (x1 * y2);
             A[4] +=  (y1 * y2);
             A[5] +=  (z1 * y2);

             A[6] +=  (x1 * z2);
             A[7] +=  (y1 * z2);
             A[8] +=  (z1 * z2);
        }
    }
}


/* A = (cds1' * cds2)' */
/* Actually retruns the transpose of A */
static void
CdsInnerProduct2(double *A, const double **cds1, const double **cds2, const int vlen)
{
    double          x1, x2, y1, y2, z1, z2;
    int             i;
    const double   *fx1 = cds1[0], *fy1 = cds1[1], *fz1 = cds1[2];
    const double   *fx2 = cds2[0], *fy2 = cds2[1], *fz2 = cds2[2];

   memset(A, 0, 9 * sizeof(double));

    for (i = 0; i < vlen; ++i)
    {
         x1 = fx1[i];
         y1 = fy1[i];
         z1 = fz1[i];

         x2 = fx2[i];
         y2 = fy2[i];
         z2 = fz2[i];

         A[0] += (x1 * x2);
         A[1] += (y1 * x2);
         A[2] += (z1 * x2);

         A[3] += (x1 * y2);
         A[4] += (y1 * y2);
         A[5] += (z1 * y2);

         A[6] += (x1 * z2);
         A[7] += (y1 * z2);
         A[8] += (z1 * z2);
    }
}


static void
MardiaRot3(double *R, const double *t)
{
    double  c1, c2, c3, s1, s2, s3;

    c1 = cos(t[0]);
    c2 = cos(t[1]);
    c3 = cos(t[2]);
    s1 = sin(t[0]);
    s2 = sin(t[1]);
    s3 = sin(t[2]);

    R[0] = c1*c2;
    R[1] = c2*s1;
    R[2] = s2;

    R[3] = -c3*s1 - c1*s2*s3;
    R[4] = c1*c3 - s1*s2*s3;
    R[5] = c2*s3;

    R[6] = s1*s3 - c1*c3*s2;
    R[7] = -c3*s1*s2 - c1*s3;
    R[8] = c2*c3;
}


/* See:
   Green and Mardia (2006)
   "Bayesian alignment using hierarchical models, with applications in protein bioinformatics"
   Biometrika 93(2):235Ð254
   Esp. pp 241-242.
*/
void
GibbsRotMardia(CdsArray *cdsA, double **theta, const gsl_rng *r2)
{
    int             i, j, k;
    const int       cnum = cdsA->cnum, vlen = cdsA->vlen;
    double          var = stats->var;
    double        **F = cdsA->tmpmat3a;
    Cds            *ave = cdsA->avecds;
    double          a12, b12, a13, b13, a23, b23;
    double          kap12, kap23, m, t12, t13, t23, width;
    double        **rotmat = NULL;

    for (i = 0; i < cnum; ++i)
    {
        t12 = theta[i][0], t13 = theta[i][1], t23 = theta[i][2];
        //t12 = t13 = t23 = 0.0; /* I'm farily convinced that this is valid, since when I rotate the structures below, */
                               /* I simply offet the structures by a "location parameter". */
                               /* I do the same for the translations. */
                               /* We can't do this if the chains are run out of place */

/*         printf("\n\n****************\nstructure: %d", i+1); */
/*         printf("\nF:"); */
/*         Mat3Print(F); */

        if (algo->varweight > 0)
        {
            CdsInnerProduct(&F[0][0], ave, cdsA->cds[i], vlen, cdsA->w);

            for (j = 0; j < 3; ++j)
                for (k = 0; k < 3; ++k)
                    F[j][k] /= 2.0;
        }
        else
        {
            CdsInnerProduct(&F[0][0], ave, cdsA->cds[i], vlen, NULL);

            for (j = 0; j < 3; ++j)
                for (k = 0; k < 3; ++k)
                    F[j][k] /= (2.0 * var);
        }

        a12 = ( F[1][1] - sin(t13) * F[0][2]) * cos(t23)
            + (-F[1][2] - sin(t13) * F[0][1]) * sin(t23) + cos(t13) * F[0][0];
        b12 = (-F[0][1] - sin(t13) * F[1][2]) * cos(t23)
            + ( F[0][2] - sin(t13) * F[1][1]) * sin(t23) + cos(t13) * F[1][0];

        kap12 = sqrt(a12*a12 + b12*b12);
        m = atan2(b12, a12);
        //printf("\nkap12: % e  m:% e", kap12, m);
        t12 = theta[i][0] = vonmises_dev4(m, kap12, r2);
        //t12 = theta[i][0] = 0.0;

        a23 = ( F[1][1] - sin(t13) * F[0][2]) * cos(t12)
            + (-F[0][1] - sin(t13) * F[1][2]) * sin(t12) + cos(t13) * F[2][2];
        b23 = (-F[1][2] - sin(t13) * F[0][1]) * cos(t12)
            + ( F[0][2] - sin(t13) * F[1][1]) * sin(t12) + cos(t13) * F[2][1];

        kap23 = sqrt(a23*a23 + b23*b23);
        m = atan2(b23, a23);
        //printf("\nkap23: % e  m:% e", kap23, m);
        t23 = theta[i][2] = vonmises_dev4(m, kap23, r2);
        //t23 = theta[i][2] = 0.0;

        a13 = sin(t12) * F[1][0] + cos(t12) * F[0][0]
            + sin(t23) * F[2][1] + cos(t23) * F[2][2];
        b13 = (-sin(t23) * F[0][1] - cos(t23) * F[0][2]) * cos(t12)
            + (-sin(t23) * F[1][1] - cos(t23) * F[1][2]) * sin(t12) + F[2][0];

        width = sqrt(2.0 *(1.0/kap12 + 1.0/kap23));

        for (j = 0; j < 7; ++j)
            t13 = mardia_gadsden_met3(a13, b13, t13, r2, width);

        theta[i][1] = t13;
        //theta[i][1] = 0.0;

/*         printf("\ntheta: % f % f % f\n", theta[i][0], theta[i][1], theta[i][2]); */

        rotmat = cdsA->cds[i]->matrix;
        MardiaRot3(&rotmat[0][0], theta[i]);

/*         printf("\nrotmat:"); */
/*         Mat3Print(rotmat); */
/*         fflush(NULL); */
/*  */
/*         if (VerifyRotMat(rotmat, 1e-6) == 0) */
/*         { */
/*             printf("\nBAD ROTATION MATRIX\n\n"); */
/*             exit(EXIT_FAILURE); */
/*         } */

        Mat3TransposeIp(rotmat);

        RotateCdsIp(cdsA->cds[i], (const double **) rotmat);
    }
}


static void
HabeckRot3(double **R, const double *t)
{
    double  c1, c2, c3, s1, s2, s3;

    c1 = cos(t[0]);
    c2 = cos(t[1]);
    c3 = cos(t[2]);
    s1 = sin(t[0]);
    s2 = sin(t[1]);
    s3 = sin(t[2]);

    R[0][0] = c1*c2*c3 - s1*s3;
    R[0][1] = s1*c2*c3 + c1*s3;
    R[0][2] = -s2*c3;

    R[1][0] = -c1*c2*s3 - s1*c3;
    R[1][1] = -s1*c2*s3 + c1*c3;
    R[1][2] = s2*s3;

    R[2][0] = c1*s2;
    R[2][1] = s1*s2;
    R[2][2] = c2;
}


void
HabeckMF_dev(double **r, const double **f, double *theta,
             double **a, double **u, double **vt, double *lambda,
             const gsl_rng *r2)
{
    int             j;
    double          alpha, beta, gamma, phi, psi, w, q, x;
    double          kappab, kappaphi, kappapsi, tmpa, tmpb, det;

    Mat3Cpy(a, f);
    CalcGSLSVD3(a, u, lambda, vt);

//         printf("lambda% 10.7f % 10.7f % 10.7f\n", lambda[0], lambda[1], lambda[2]);

//         printf("\nU:");
//         Mat3Print(u);
//         printf("U end\n");
//         fflush(NULL);
//
//         printf("\nVt:");
//         Mat3Print(vt);
//         printf("Vt end\n");
//         fflush(NULL);

//         det = Mat3Det((const double **) u);
//         printf("\n * determinant of SVD U matrix = %f\n", det);
//         det = Mat3Det((const double **) vt);
//         printf(" * determinant of SVD V matrix = %f\n", det);

    det = Mat3Det((const double **) u) * Mat3Det((const double **) vt);
//        printf("\n * determinant of SVD UVt matrix = %f\n", det);

    if (det < 0)
    {
//            printf("\nlambda: % f % f % f\n", lambda[0], lambda[1], lambda[2]);
        lambda[2] = -lambda[2];

        for (j = 0; j < 3; ++j)
            u[j][2] = -u[j][2];

//            det = Mat3Det((const double **)U) * Mat3Det((const double **)Vt);
//            printf("\n * ##determinant of SVD UVt matrix = %f\n", det);
    }

    beta = theta[1];

    tmpa = cos(0.5 * beta);
    tmpb = sin(0.5 * beta);

    kappaphi = tmpa*tmpa * (lambda[0] + lambda[1]);
    kappapsi = tmpb*tmpb * (lambda[0] - lambda[1]);

//             printf("\nkappaphi, kappaspi: %e %e\n", kappaphi, kappapsi);
//             fflush(NULL);

    phi = vonmises_dev4(0.0, kappaphi, r2);
    myassert(isfinite(phi));

//             printf("\nphi: %f\n", phi);
//             fflush(NULL);

    psi = vonmises_dev4(M_PI, kappapsi, r2);
    myassert(isfinite(psi));

//             printf("psi: %f\n", psi);
//             fflush(NULL);

    w = gsl_ran_bernoulli(r2, 0.5);

    alpha = 0.5 * (phi + psi) + M_PI * w;
    gamma = 0.5 * (phi - psi) + M_PI * w;

    kappab = (lambda[0] + lambda[1]) * cos(phi)
           + (lambda[0] - lambda[1]) * cos(psi)
           + 2.0 * lambda[2];

    q = gsl_rng_uniform(r2);

    x = 1.0 + (2.0 * log(q + (1.0 - q) * exp(-kappab)) / kappab);
    beta = acos(x);

//         printf("\nalpha, beta, gamma: %f %f %f\n", alpha, beta, gamma);
//         fflush(NULL);

    theta[0] = alpha;
    theta[1] = beta;
    theta[2] = gamma;

//        printf("\ntheta: % f % f % f\n", theta[i][0], theta[i][1], theta[i][2]);

    HabeckRot3(a, theta);

    Mat3MultIp(u, (const double **) a);
    Mat3MultIp(u, (const double **) vt);
    Mat3Cpy(r, (const double **) u);

    if (VerifyRotMat(r, 1e-8) == 0)
    {
        printf("\nBAD ROTATION MATRIX U\n\n");
        exit(EXIT_FAILURE);
    }
}


/* See:
   Habeck (2009)
   "Generation of three-dimensional random rotations in fitting and matching problems."
   Comput Stat 24:719-731

   p 726, 3 Algorithm

   My implementation has been verified against Habeck 2009 Figures 1 & 3 usin
   his A matrix (sxn 4.1), which has singular values 16.17, 4.80, and 0.57,
   max tr(A'R) = 20.4. Conitional dist of beta should have max about 0.34,
   and average about 0.5.
*/
void
GibbsRotHabeck(CdsArray *cdsA, double **theta, const gsl_rng *r2)
{
    int             i, j, k;
    const int       cnum = cdsA->cnum, vlen = cdsA->vlen;
    double          var = stats->var;
    double        **f = cdsA->tmpmat3a;
    double        **a = cdsA->tmpmat3b;
    double        **u = cdsA->tmpmat3c;
    double        **vt = cdsA->tmpmat3d;
    double         *lambda = cdsA->tmpvec3a;
    Cds            *avecds = cdsA->avecds;
    Cds            *tcds = NULL;
    double        **rotmat = NULL;


    if (algo->covweight)
    {
        tcds = cdsA->tcds;
        MatMultCdsMultMatDiag(tcds, (const double **) cdsA->WtMat, avecds);
    }
    else if (algo->varweight)
    {
        tcds = cdsA->tcds;
        MatDiagMultCdsMultMatDiag(tcds, cdsA->w, avecds);
    }
    else if (algo->leastsquares)
    {
        tcds = cdsA->avecds;
    }

    for (i = 0; i < cnum; ++i)
    {
        CdsInnerProduct2(&f[0][0],
                         (const double **) tcds->wc,
                         (const double **) cdsA->cds[i]->wc,
                         vlen);

        if (algo->varweight > 0)
        {
            for (j = 0; j < 3; ++j)
                for (k = 0; k < 3; ++k)
                    f[j][k] *= 0.5;
        }
        else
        {
            for (j = 0; j < 3; ++j)
                for (k = 0; k < 3; ++k)
                    f[j][k] /= (2.0 * var);
        }

        rotmat = cdsA->cds[i]->matrix;

        HabeckMF_dev(rotmat, (const double **) f, theta[i], a, u, vt, lambda, r2);

        RotateCdsIp(cdsA->cds[i], (const double **) rotmat);
    }
}


double
Calc_b(const double *lambda)
{
    int            i, j;
    double         b, fx, dfx, fxdfx, term;
    const int      maxiter = 100;
    const double   tol = FLT_EPSILON;

    b = 1.0;
    for (i = 0; i < maxiter; ++i)
    {
        fx = dfx = 0.0;
        for (j = 0; j < 4; ++j)
        {
             term = 1.0 / (b + 2.0*lambda[j]);
             fx += term;
             dfx -= term*term;
        }

        fx -= 1.0;

        fxdfx = fx / dfx;
        b -= fxdfx; /* Newton-Raphson correction */

        if (b < 0.0)
            b = 0.0;

//         printf("%3d % 10.6e % 10.6e % 10.6e % 10.6e\n",
//                i, b, fx, dfx, fx/dfx);
//         fflush(NULL);

        if (fabs(fxdfx) < tol * b || fabs(fx) < tol)
            break; /* success */
    }

//  printf("\ndone: %3d % 10.6e % 10.6e % 10.6e % 10.6e\n",
//         i, b, fx, dfx, fx/dfx);

    if (i == maxiter)
    {
        printf("\n WARNING02: Newton-Raphson failed to converge in gamma_fit()\n");

        printf("            b            fx           dfx        fx/dfx\n");
        printf("% 10.6e % 10.6e % 10.6e % 10.6e\n",
               b, fx, dfx, fx/dfx);

        fflush(NULL);
    }

    return(b);
}


/* ACG - angular central gaussian distribution variate */
/* omega matrix is assumed to be diagonal */
void
acg_dev(double *x, const int q, const double *omega, const gsl_rng *r2)
{
    int            i;
    double         norm;

    norm = 0.0;
    for (i = 0; i < q; ++i)
    {
        x[i] = gsl_ran_gaussian_ziggurat(r2, sqrt(1.0/omega[i]));
        norm += x[i]*x[i];
    }

    norm = sqrt(norm);

    for (i = 0; i < q; ++i)
        x[i] /= norm;
}


// Equation 5.1
void
KentRot3(double **m, const double *x)
{
    const double x1 = x[0];
    const double x2 = x[1];
    const double x3 = x[2];
    const double x4 = x[3];

    m[0][0] = x1*x1 + x2*x2 - x3*x3 - x4*x4;
    m[0][1] = -2.0*(x1*x4 - x2*x3);
    m[0][2] =  2.0*(x1*x3 + x2*x4);
    m[1][0] =  2.0*(x1*x4 + x2*x3);
    m[1][1] = x1*x1 + x3*x3 - x2*x2 - x4*x4;
    m[1][2] = -2.0*(x1*x2 - x3*x4);
    m[2][0] = -2.0*(x1*x3 - x2*x4);
    m[2][1] =  2.0*(x1*x2 + x3*x4);
    m[2][2] = x1*x1 + x4*x4 - x2*x2 - x3*x3;
}


// Equation 5.2
void
KentDelta2Lambda(double *lambda, const double *delta)
{
    lambda[0] = 0.0;
    lambda[1] = 2.0*(delta[1] + delta[2]);
    lambda[2] = 2.0*(delta[0] + delta[2]);
    lambda[3] = 2.0*(delta[0] + delta[1]);
}


double
CalcEnvelopeBACG(const double *x, const double *lambda, const double *omega, const double b)
{
    double         envf, term;
    int            i;

    envf = 2.0 - 0.5*b;

    for (i = 0; i < 4; ++i)
        envf -= x[i]*x[i]*lambda[i];

    term = 0.0;
    for (i = 0; i < 4; ++i)
        term += x[i]*x[i]*omega[i];

    envf += 2.0 * log(b * term / 4.0);

    return(envf);
}


/* f is actually f', the transpose */
void
KentMF_dev(double **r, const double **f,
           double **a, double **u, double **vt, double *delta,
           double *v1, double *v2, double *v3,
           const gsl_rng *r2)
{
    int            i, j;
    double         w, b, envelope;
    double        *x = v1;
    double        *lambda = v2;
    double        *omega = v3;


    Mat3Cpy(a, f);
    CalcGSLSVD3(a, u, delta, vt);

    double det = Mat3Det((const double **) u) * Mat3Det((const double **) vt);
//  printf("\n * determinant of SVD UVt matrix = %f\n", det);

    if (det < 0)
    {
//            printf("\nlambda: % f % f % f\n", lambda[0], lambda[1], lambda[2]);
        delta[2] = -delta[2];

        for (j = 0; j < 3; ++j)
            u[j][2] = -u[j][2];

//            det = Mat3Det((const double **)U) * Mat3Det((const double **)Vt);
//            printf("\n * ##determinant of SVD UVt matrix = %f\n", det);
    }

//     printf("delta:  %g %g %g\n", delta[0], delta[1], delta[2]);
    KentDelta2Lambda(lambda, delta);
//     printf("lambda: %g %g %g %g\n", lambda[0], lambda[1], lambda[2], lambda[3]);
//     fflush(NULL);

    b = Calc_b(lambda);

    for (i = 0; i < 4; ++i)
        omega[i] = 1.0 + 2.0*lambda[i]/b;

    while(1)
    {
        acg_dev(x, 4, omega, r2);
        w = gsl_rng_uniform(r2);

        envelope = CalcEnvelopeBACG(x, lambda, omega, b);
//         printf("envelope = % 16.8f % 16.8f %d\n", log(w), envelope, log(w) < envelope);
//         fflush(NULL);

        if (log(w) < envelope)
            break;
    }

    KentRot3(a, x);

    Mat3MultIp(u, (const double **) a);
    Mat3MultIp(u, (const double **) vt);
    Mat3Cpy(r, (const double **) u);

    if (VerifyRotMat(r, 1e-8) == 0)
    {
        printf("\nBAD ROTATION MATRIX U\n\n");
        exit(EXIT_FAILURE);
    }
}


void
GibbsRotKent(CdsArray *cdsA, const gsl_rng *r2)
{
    int             i, j, k;
    const int       cnum = cdsA->cnum, vlen = cdsA->vlen;
    double          var = stats->var;
    double        **f = cdsA->tmpmat3a;
    double        **a = cdsA->tmpmat3b;
    double        **u = cdsA->tmpmat3c;
    double        **vt = cdsA->tmpmat3d;
    double         *lambda = cdsA->tmpvec3a;
    double        *v1 = malloc(4 * sizeof(double));
    double        *v2 = malloc(4 * sizeof(double));
    double        *v3 = malloc(4 * sizeof(double));
    Cds            *avecds = cdsA->avecds;
    Cds            *tcds = NULL;
    double        **rotmat = NULL;


    if (algo->covweight)
    {
        tcds = cdsA->tcds;
        MatMultCdsMultMatDiag(tcds, (const double **) cdsA->WtMat, avecds);
    }
    else if (algo->varweight)
    {
        tcds = cdsA->tcds;
        MatDiagMultCdsMultMatDiag(tcds, cdsA->w, avecds);
    }
    else if (algo->leastsquares)
    {
        tcds = cdsA->avecds;
    }

    for (i = 0; i < cnum; ++i)
    {
        /* f' = tcds' * cdsi */
        /* f is the transpose */
        CdsInnerProduct2(&f[0][0],
                         (const double **) tcds->wc,
                         (const double **) cdsA->cds[i]->wc,
                         vlen);

        if (algo->varweight > 0)
        {
            for (j = 0; j < 3; ++j)
                for (k = 0; k < 3; ++k)
                    f[j][k] *= 0.5;
        }
        else
        {
            for (j = 0; j < 3; ++j)
                for (k = 0; k < 3; ++k)
                    f[j][k] /= (2.0 * var);
        }

        rotmat = cdsA->cds[i]->matrix;

        KentMF_dev(rotmat, (const double **) f, a, u, vt, lambda, v1, v2, v3, r2);

        RotateCdsIp(cdsA->cds[i], (const double **) rotmat);
    }

    free(v1);
    free(v2);
    free(v3);
}


static void
MetScale(CdsArray *cdsA, const gsl_rng *r2)
{
    int             i;
    const int       cnum = cdsA->cnum, vlen = cdsA->vlen;
    const int       n = 3.0 * vlen + 1.0;
    double          phi, gamma;
    Cds            *cdsi = NULL;
    Cds            *avecds = cdsA->avecds;
    double          sm, width, oldscale;
    double          var = stats->var;
    int             skip = 7;

    double sum = 0.0;
    for (i = 0; i < cnum; ++i)
        sum += cdsA->cds[i]->scale;

    sum /= cnum;
    //printf("\nave: % f", sum);

    for (i = 0; i < cnum; ++i)
    {
        if (algo->scaleanchor == i)
            continue;

        cdsi = cdsA->cds[i];
        oldscale = cdsi->scale;

        phi = TrCdsInnerProd(cdsi, vlen) / var;
        gamma = TrCdsInnerProd2(cdsi, avecds, vlen) / var;
        sm = ScaleMax(n, gamma, phi);
        width = sqrt(1.0 / (phi + (n-1.0)/(sm*sm)));

        cdsi->scale = scale_log_met3(n, gamma, phi, oldscale, r2, sm, width, skip);
        ScaleCds(cdsi, cdsi->scale);

        //printf("\nscale[%3d]: % f % f % f % f % f", i+1, cdsi->scale, phi, gamma, sm, width);
        //fflush(NULL);
    }
}


static void
MetScaleDiag(CdsArray *cdsA, const gsl_rng *r2)
{
    int             i;
    const int       cnum = cdsA->cnum, vlen = cdsA->vlen;
    const int       n = 3.0 * vlen + 1.0;
    double          phi, gamma;
    Cds            *cdsi = NULL;
    Cds            *avecds = cdsA->avecds;
    double         *wts = cdsA->w;
    double          sm, width, oldscale;
    int             skip = 7;
    double          priorg = 0.0; // set to 1.0 for exponential prior mean = 1

//     double sum = 0.0;
//     for (i = 0; i < cnum; ++i)
//         sum += cdsA->cds[i]->scale;
//
//     sum /= cnum;
//     printf("\nave: % f", sum);
//
//     sum = 0.0;
//     for (i = 0; i < cnum; ++i)
//         sum += log(cdsA->cds[i]->scale);
//
//     sum /= cnum;
//     printf("\nave log: % f", sum);

    for (i = 0; i < cnum; ++i)
    {
        if (algo->scaleanchor == i)
            continue;

        cdsi = cdsA->cds[i];
        oldscale = cdsi->scale;

        phi = TrCdsInnerProdWt(cdsi, vlen, wts);
        gamma = TrCdsInnerProdWt2(cdsi, avecds, vlen, wts) - priorg;
        sm = ScaleMax(n, gamma, phi);
        width = sqrt(1.0 / (phi + (n-1.0)/(sm*sm)));
        //printf("width:% g\n", width);

        cdsi->scale = scale_log_met3(n, gamma, phi, oldscale, r2, sm, width, skip);
        //cdsi->scale = scale_rejection(n, gamma, phi, r2);
        ScaleCds(cdsi, cdsi->scale);

        //printf("\n(diag) scale[%3d]: % f % f % f % f % f", i+1, cdsi->scale, phi, gamma, sm, width);
        //fflush(NULL);
    }
}


static void
CdsInnProd(Cds *cds)
{
    /* (i x k)(k x j) = (i x j) */
    /* (3 x N)(N x 3) = (3 x 3) */
    int             k;
    double        **ip = NULL;
    const double   *x = (const double *) cds->x,
                   *y = (const double *) cds->y,
                   *z = (const double *) cds->z;
    double          xk, yk, zk;

    ip = cds->innerprod;

    memset(ip[0], 0, 9 * sizeof(double));

    for (k = 0; k < cds->vlen; ++k)
    {
        xk = x[k];
        yk = y[k];
        zk = z[k];

        ip[0][0] += (xk * xk);
        ip[1][1] += (yk * yk);
        ip[2][2] += (zk * zk);
        ip[0][1] += (xk * yk);
        ip[0][2] += (xk * zk);
        ip[1][2] += (yk * zk);
    }

    ip[1][0] = ip[0][1];
    ip[2][0] = ip[0][2];
    ip[2][1] = ip[1][2];

    //printf("tr(X'X) = % e\n", ip[0][0] + ip[1][1] + ip[2][2]);

    /* Mat3Print(ip2); */
}


static void
CdsInnProdWt(Cds *cds, const double *wts)
{
    /* (i x k)(k x j) = (i x j) */
    /* (3 x N)(N x 3) = (3 x 3) */
    int             k;
    double        **ip = NULL;
    const double   *x = (const double *) cds->x,
                   *y = (const double *) cds->y,
                   *z = (const double *) cds->z;
    double          xk, yk, zk, wtsi;

    ip = cds->innerprod;

    memset(ip[0], 0, 9 * sizeof(double));

    for (k = 0; k < cds->vlen; ++k)
    {
        wtsi = wts[k];

        xk = x[k];
        yk = y[k];
        zk = z[k];

        ip[0][0] += (xk * xk) * wtsi;
        ip[1][1] += (yk * yk) * wtsi;
        ip[2][2] += (zk * zk) * wtsi;
        ip[0][1] += (xk * yk) * wtsi;
        ip[0][2] += (xk * zk) * wtsi;
        ip[1][2] += (yk * zk) * wtsi;
    }

    ip[1][0] = ip[0][1];
    ip[2][0] = ip[0][2];
    ip[2][1] = ip[1][2];

    //printf("tr(X'X) = % e\n", ip[0][0] + ip[1][1] + ip[2][2]);

    /* Mat3Print(ip2); */
}


void
CalcCdsPrincAxesGibbs(Cds *cds, double **r, double **u, double **vt, double *lambda, const double *wts)
{
    int             j;
    double          det;

    if (algo->leastsquares)
        CdsInnProd(cds);
    else
        CdsInnProdWt(cds, wts);

    CalcGSLSVD3(cds->innerprod, u, lambda, vt);

    det = Mat3Det((const double **) u);
//        printf("\n * determinant of SVD UVt matrix = %f\n", det);

    if (det < 0)
    {
//            printf("\nlambda: % f % f % f\n", lambda[0], lambda[1], lambda[2]);
        printf("\nNEGATIVE DETERMINANT\n");
        lambda[2] = -lambda[2];

        for (j = 0; j < 3; ++j)
            u[j][2] = -u[j][2];
    }

    Mat3Cpy(r, (const double **) u);

    if (VerifyRotMat(r, 1e-8) == 0)
    {
        printf("\nBAD ROTATION MATRIX U\n\n");
        exit(EXIT_FAILURE);
    }
}


void
IdentifyMean(CdsArray *cdsA)
{
    int             i;
    const int       cnum = cdsA->cnum;
    Cds            *avecds = cdsA->avecds;

    ApplyCenterIp(avecds);

//     printf("\na trans: % f % f % f",
//            cdsA->avecds->center[0], cdsA->avecds->center[1], cdsA->avecds->center[2]);
//     fflush(NULL);

    for (i = 0; i < cnum; ++i)
        NegTransCdsIp(cdsA->cds[i]->wc, cdsA->avecds->center, cdsA->vlen);

    CalcCdsPrincAxesGibbs(avecds, avecds->matrix, cdsA->tmpmat3a, cdsA->tmpmat3b, cdsA->tmpvec3a, cdsA->w);
//     printf("\nBEF\n");
//     Mat3Print(avecds->innerprod);
//     double tr = avecds->innerprod[0][0]+avecds->innerprod[1][1]+avecds->innerprod[2][2];
//     printf("tr:%20.8f\n",
//            tr/(3*cdsA->vlen));
    RotateCdsIp(avecds, (const double **) avecds->matrix);
//     CdsInnProdWt(avecds, cdsA->w);
//     printf("tra:%20.10f\n\n", avecds->innerprod[0][0]+avecds->innerprod[1][1]+avecds->innerprod[2][2]);
//     printf("\nAFT\n");
//     Mat3Print(avecds->innerprod);

    for (i = 0; i < cnum; ++i)
        RotateCdsIp(cdsA->cds[i], (const double **) avecds->matrix);
}


static void
GibbsMean(CdsArray *cdsA, const gsl_rng *r2)
{
    int             i;
    double         *avex = cdsA->avecds->x,
                   *avey = cdsA->avecds->y,
                   *avez = cdsA->avecds->z;
    const int       cnum = cdsA->cnum, vlen = cdsA->vlen;
    double          mvar = sqrt(stats->var / cnum);

/*     printf("\nmvar: % e", mvar); */

    AveCdsGibbs(cdsA);

    for (i = 0; i < vlen; ++i)
    {
        avex[i] += gsl_ran_gaussian_ziggurat(r2, mvar);
        avey[i] += gsl_ran_gaussian_ziggurat(r2, mvar);
        avez[i] += gsl_ran_gaussian_ziggurat(r2, mvar);
    }

    CenMass(cdsA->avecds);
    //IdentifyMean(cdsA);
}


static void
GibbsMeanDiag(CdsArray *cdsA, const gsl_rng *r2)
{
    int             i;
    double         *avex = cdsA->avecds->x,
                   *avey = cdsA->avecds->y,
                   *avez = cdsA->avecds->z;
    const int       cnum = cdsA->cnum, vlen = cdsA->vlen;
    double          mvar;

    AveCdsGibbs(cdsA);

    for (i = 0; i < vlen; ++i)
    {
        mvar = sqrt(cdsA->var[i] / cnum);

        avex[i] += gsl_ran_gaussian_ziggurat(r2, mvar);
        avey[i] += gsl_ran_gaussian_ziggurat(r2, mvar);
        avez[i] += gsl_ran_gaussian_ziggurat(r2, mvar);
    }

    CenMassWtIp(cdsA->avecds, cdsA->w);
    //IdentifyMean(cdsA);

//     printf("\ntrans: % f % f % f",
//            cdsA->avecds->center[0], cdsA->avecds->center[1], cdsA->avecds->center[2]);
//     fflush(NULL);

//     ApplyCenterIp(avecds);
//
//     for (i = 0; i < cnum; ++i)
//         NegTransCdsIp(cdsA->cds[i], cdsA->avecds->center);

//    printf("\nmean trans[%d]: % f % f % f", i+1,
//           avecds->center[0], avecds->center[1], avecds->center[2]);
//    fflush(NULL);
}


static void
GibbsVar(CdsArray *cdsA, const gsl_rng *r2)
{
    double          var;
    const int       cnum = cdsA->cnum, vlen = cdsA->vlen;

    VarCds(cdsA);
    var = stats->var;
    //printf("\nB var: % e  % e", var, sqrt(var));

    stats->var = invgamma_dev4(1.5 * cnum * vlen * var, 1.5 * cnum * vlen, r2);

    //printf("\nA var: % e  % e", stats->var, sqrt(stats->var));
    //fflush(NULL);
}


static void
GibbsVarDiag(CdsArray *cdsA, const gsl_rng *r2)
{
    double          phi = stats->phi;
    const int       cnum = cdsA->cnum, vlen = cdsA->vlen;
    int             i;

    VarCds(cdsA);

    for (i = 0; i < vlen; ++i)
    {
        //printf("\nvar[%d]: %e %e %e", i, cdsA->var[i], 3.0 * cnum * cdsA->var[i], (3.0 * cnum * cdsA->var[i] + phi));
        cdsA->var[i] = invgamma_dev4(0.5 * (3.0 * cnum * cdsA->var[i] + phi), (3.0*cnum + 1.0)/2.0, r2);
        //cdsA->var[i] = invgamma_dev4(0.5 * (3.0 * cnum * cdsA->var[i]), 1.5 * cnum + 0.5, r2);
//        printf(" %e", cdsA->var[i]);
    }

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


/* http://www.gnu.org/software/gsl/manual/html_node/The-Gamma-Distribution.html
   double gsl_ran_gamma (const gsl_rng * r, double a, double b)
   p(x) dx = {1 \over \Gamma(a) b^a} x^{a-1} e^{-x/b} dx
*/
static void
GibbsPhi(CdsArray *cdsA, const gsl_rng *r2)
{
    const int       vlen = cdsA->vlen;
    double          invtr, a, b;
    int             i;

    invtr = 0.0;
    for (i = 0; i < vlen; ++i)
        invtr += 1.0 / cdsA->var[i];

    a = 0.5 * (vlen + 2.0);
    b = 2.0 / (invtr + 2.0 / stats->alpha);

//     reference prior = 1/phi
//    a = 0.5*vlen;
//    b = 2.0/invtr;

    stats->phi = gsl_ran_gamma(r2, a, b);

//    printf("\nA phi: %e %e %e %e", stats->phi, b, vlen / invtr, sqrt(vlen / invtr));
//    fflush(NULL);
}


static void
GibbsWrite(CdsArray *cdsA, const gsl_rng *r2, const int iter)
{
    char            filename[256], avename[256];

    sprintf(filename, "%s%05d.pdb", "gibbs_", iter);
    OverWriteTheseusCdsModelFile(cdsA, filename);
    sprintf(avename, "%s%05d.pdb", "gibbs_ave_", iter);
    WriteAveCds(cdsA, avename);
}


static void
GibbsMaxWrite(CdsArray *cdsA, const gsl_rng *r2, const int iter)
{
    char            filename[256];

    sprintf(filename, "%s%05d.pdb", "gibbs_max_", iter);
    OverWriteTheseusCdsModelFile(cdsA, filename);
}


static void
GibbsDiagWrite(CdsArray *cdsA, const gsl_rng *r2, const int iter)
{
    char            filename[256], avename[256];

    sprintf(filename, "%s%05d.pdb", "gibbs_", iter);
    OverWriteTheseusCdsModelFile(cdsA, filename);
    sprintf(avename, "%s%05d.pdb", "gibbs_ave_", iter);
    WriteAveCds(cdsA, avename);
}


static void
WriteSample(FILE *paramfile, CdsArray *cdsA, double **theta, const int iter)
{
    int             j, k;
    double          angle;
    double         *v = malloc(3 * sizeof(double));
    double         *quat = malloc(4 * sizeof(double));
    const int       cnum = cdsA->cnum, vlen = cdsA->vlen;

    fprintf(paramfile, "%-12s %12d\n", "BEG SAMPLE", iter);
    fprintf(paramfile, "%-12s %26.2f\n", "LOGL", stats->logL);
    fprintf(paramfile, "%-12s %26.2f\n", "MLOGL", stats->mlogL);
    fprintf(paramfile, "%-12s %26.6f\n", "RMSD", stats->ave_paRMSD);

    if (algo->varweight > 0)
    {
        fprintf(paramfile, "%-12s %18.9e\n", "ALPHA", stats->alpha);
        fprintf(paramfile, "%-12s %18.9e\n", "PHI", stats->phi);
        fprintf(paramfile, "%-12s %18.9e\n", "SIGMA", sqrt(stats->var));
    }
    else
    {
        fprintf(paramfile, "%-12s %18.9e\n", "PHI", stats->var);
        fprintf(paramfile, "%-12s %18.9e\n", "SIGMA", sqrt(stats->var));
    }

    for (j = 0; j < vlen; ++j)
        fprintf(paramfile, "%-12s %6d %18.9e\n", "VAR", j+1, cdsA->var[j]);

    for (j = 0; j < vlen; ++j)
        fprintf(paramfile, "%-12s %6d % 14.9f % 14.9f % 14.9f\n", "MEAN",
                j+1, cdsA->avecds->x[j], cdsA->avecds->y[j], cdsA->avecds->z[j]);

    for (k = 0; k < cnum; ++k)
        fprintf(paramfile, "%-12s %6d % 18.9e % 18.9e % 18.9e\n", "TRANS",
                k+1, cdsA->cds[k]->center[0], cdsA->cds[k]->center[1], cdsA->cds[k]->center[2]);

    for (k = 0; k < cnum; ++k)
    {
        fprintf(paramfile, "%-12s %6d", "ROT", k+1);

        for (j = 0; j < 3; ++j)
        {
            fprintf(paramfile,
                    //"% 12.3e % 12.3e % 12.3e",
                    "% 12.9f % 12.9f % 12.9f    ",
                    cdsA->cds[k]->matrix[j][0],
                    cdsA->cds[k]->matrix[j][1],
                    cdsA->cds[k]->matrix[j][2]);
        }

        fputc('\n', paramfile);
    }

//     for (k = 0; k < cnum; ++k)
//         fprintf(paramfile, "%-12s %6d % 18.9e % 18.9e % 18.9e\n", "THETA",
//                 k+1, theta[k][0], theta[k][1], theta[k][2]);

    for (k = 0; k < cnum; ++k)
    {
        RotMatToQuaternion((const double **) cdsA->cds[k]->matrix, quat);
        fprintf(paramfile, "%-12s %6d % 18.9e % 18.9e % 18.9e % 18.9e\n", "QUAT",
                k+1, quat[0], quat[1], quat[2], quat[3]);
    }

    for (k = 0; k < cnum; ++k)
    {
        angle = RotMat2AxisAngle(cdsA->cds[k]->matrix, v);
        fprintf(paramfile,
                "%-12s %6d % 10.7f   % 12.9f % 12.9f % 12.9f\n", "ANGLE-AXIS",
                k+1, angle, v[0], v[1], v[2]);
    }

    if (algo->scale > 0)
        for (j = 0; j < cnum; ++j)
            fprintf(paramfile, "%-12s %6d % 26.16f\n", "SCALE", j+1, cdsA->cds[j]->scale);

    fprintf(paramfile, "%-12s %12d\n", "END SAMPLE", iter);

    fflush(NULL);

    free(quat);
    free(v);
}


void
RandInitGibbs(CdsArray *cdsA, const gsl_rng *r2)
{
    const int      cnum = cdsA->cnum, vlen = cdsA->vlen;
    int            slxn, i;

    slxn = (int) (gsl_rng_uniform(r2) * cnum);

    stats->phi = 0.0;
    for (i = 0; i < vlen; ++i)
        cdsA->w[i] = cdsA->var[i] = 1.0;
    RandRotCdsArray(cdsA, r2);
    RandTransCdsArray(cdsA, 1000.0, r2);

    CdsCopyAll(cdsA->avecds, cdsA->cds[slxn]);
}


static double
CalcFrobTerm(CdsArray *cdsA)
{
    int             k, m;;
    const int       cnum = cdsA->cnum, vlen = cdsA->vlen;
    const double   *var = (const double *) cdsA->var;
    const Cds     **cds = (const Cds **) cdsA->cds;
    const Cds      *cdsm = NULL;
    const double   *avex = (const double *) cdsA->avecds->x,
                   *avey = (const double *) cdsA->avecds->y,
                   *avez = (const double *) cdsA->avecds->z;
    double          fterm, tmpx, tmpy, tmpz, invvark;

    fterm = 0.0;
    for (k = 0; k < vlen; ++k)
    {
        invvark = 1.0 / var[k];

        for (m = 0; m < cnum; ++m)
        {
            cdsm = (Cds *) cds[m];

            tmpx = cdsm->x[k] - avex[k];
            tmpy = cdsm->y[k] - avey[k];
            tmpz = cdsm->z[k] - avez[k];

            fterm += (tmpx*tmpx + tmpy*tmpy + tmpz*tmpz) * invvark;
        }
    }

    return(fterm);
}


static double
CalcFrobTermIso(CdsArray *cdsA)
{
    int             k, m;;
    const int       cnum = cdsA->cnum, vlen = cdsA->vlen;
    const Cds     **cds = (const Cds **) cdsA->cds;
    const Cds      *cdsm = NULL;
    const double   *avex = (const double *) cdsA->avecds->x,
                   *avey = (const double *) cdsA->avecds->y,
                   *avez = (const double *) cdsA->avecds->z;
    double          fterm, tmpx, tmpy, tmpz;

    fterm = 0.0;
    for (k = 0; k < vlen; ++k)
    {
        for (m = 0; m < cnum; ++m)
        {
            cdsm = (Cds *) cds[m];

            tmpx = cdsm->x[k] - avex[k];
            tmpy = cdsm->y[k] - avey[k];
            tmpz = cdsm->z[k] - avez[k];

            fterm += (tmpx*tmpx + tmpy*tmpy + tmpz*tmpz);
        }
    }

    return(fterm / stats->var);
}


double
CalcHierarchPrLogL(CdsArray *cdsA)
{
    const double   *var = (const double *) cdsA->var;
    const int       vlen = cdsA->vlen;

    return(invgamma_logL(var, vlen, stats->phi, algo->covnu / 2));
}


static double
CalcLogScaleJacob(CdsArray *cdsA)
{
    double         scales;
    int            i;

    scales = 0.0;
    for (i = 0; i < cdsA->cnum; ++i)
        scales += log(cdsA->cds[i]->scale);

    return(3.0 * cdsA->vlen * scales);
}


/* Calculates the likelihood for a specified Gaussian model, given a
   structural superposition.

     NOTA BENE: This function assumes that the variances, covariance matrices,
     hierarchical model parameters, average coordinates, rotations, and
     translations have all been pre-calculated.

   The first term of the likelihood equation (the Mahalonobius Frobenius
   matrix norm term) is normally equal to NKD/2 at the maximum. However,
   when using shrinkage or hierarchical estimates of the covariance
   matrices, this convenient simplification no longer holds, and the
   double matrix-weighted Frobenius norm must be calculated explicitly.
*/
static double
CalcLogLGibbs(CdsArray *cdsA)
{
    const int       vlen = cdsA->vlen;
    const double    cnum = cdsA->cnum;
    const double    nk = cnum * vlen;
    const double    nd = cnum * 3.0;
    const double    ndk = nk * 3.0;
    const double    ndk2 = 0.5 * ndk;
    const double   *var = (const double *) cdsA->var;
    double          lndet, frobterm, igL, scales;
    int             i;

    lndet = frobterm = igL = 0.0;

    if (algo->leastsquares)
    {
        lndet = vlen * log(stats->var);
        frobterm = CalcFrobTermIso(cdsA);
        igL = 0.0;
    }
    else if (algo->varweight)
    {
        lndet = 0.0;
        for (i = 0; i < vlen; ++i)
            lndet += log(var[i]);

        frobterm = CalcFrobTerm(cdsA);
        igL = CalcHierarchPrLogL(cdsA);
    }

    scales = CalcLogScaleJacob(cdsA);
    stats->logL = scales
                - (0.5 * frobterm)
                - (ndk2 * log(2.0 * M_PI))
                - (0.5 * nd * lndet)
                + igL;

//     printf("\n!      scales    -frobterm/2      -ndk2          igL     lndet\n");
//     printf("! % 12.4f % 12.4f % 12.4f % 12.4f % 12.4f\n",
//            scales, (-0.5 * frobterm), -ndk2, igL, (- 0.5 * nd * lndet));

    return(stats->logL);
}


/* tr(A'B) */
static double
Mat3MultTr2(const double **A, const double **B)
{
    double tr;
    tr = A[0][0]*B[0][0] + A[1][0]*B[1][0] + A[2][0]*B[2][0]
       + A[0][1]*B[0][1] + A[1][1]*B[1][1] + A[2][1]*B[2][1]
       + A[0][2]*B[0][2] + A[1][2]*B[1][2] + A[2][2]*B[2][2];

    return(tr);
}


static double
H_Kent(const double **g, const double **x)
{
    double tr = Mat3MultTr2(g,x);

    return(exp(tr));
}


static void
TestMF(const int burn, gsl_rng *r2)
{
    printf("\n starting rot mat devs\n");
    fflush(NULL);

    double **r = MatAlloc(3,3);
    int i, j, k;

    double    **f  = MatAlloc(3,3);
    double    **a  = MatAlloc(3,3);
    double    **vt = MatAlloc(3,3);
    double    **u  = MatAlloc(3,3);;
    double     *lambda = malloc(4 * sizeof(double));
    double     *v1 = malloc(4 * sizeof(double));
    double     *v2 = malloc(4 * sizeof(double));
    double     *v3 = malloc(4 * sizeof(double));

//         f[0][0] = 85;
//         f[0][1] = 78;
//         f[0][2] = 43;
//         f[1][0] = 11;
//         f[1][1] = 39;
//         f[1][2] = 64;
//         f[2][0] = 41;
//         f[2][1] = 60;
//         f[2][2] = 48;

    f[0][0] = 85;
    f[0][1] = 0;
    f[0][2] = 0;
    f[1][0] = 0;
    f[1][1] = 48;
    f[1][2] = 0;
    f[2][0] = 0;
    f[2][1] = 0;
    f[2][2] = 39;

    for (j = 0; j < 3; ++j)
        for (k = 0; k < 3; ++k)
            f[j][k] *= 0.1;

    double **g = MatAlloc(3,3);
//         for (j = 0; j < 3; ++j)
//             for (k = 0; k < 3; ++k)
//                 g[j][k] = 0.1*ceil(gsl_rng_uniform(r2) * 100);

    for (j = 0; j < 3; ++j)
        //for (k = 0; k < 3; ++k)
            g[j][j] = 0.1*ceil(gsl_rng_uniform(r2) * 100);

    double fv, nfv, nfngv, gv;
    double *theta1 = calloc(3,sizeof(double));
    double *theta2 = calloc(3,sizeof(double));

    double **nf =   MatAlloc(3,3);
    double **nfng = MatAlloc(3,3);
    double **gf =   MatAlloc(3,3);

    for (j = 0; j < 3; ++j)
        for (k = 0; k < 3; ++k)
            nf[j][k] = - f[j][k];

    for (j = 0; j < 3; ++j)
        for (k = 0; k < 3; ++k)
            nfng[j][k] = - f[j][k] - g[j][k];

    for (j = 0; j < 3; ++j)
        for (k = 0; k < 3; ++k)
            gf[j][k] = f[j][k] + g[j][k];

    double delta;
    double nfave = 0.0, nfngave = 0.0, gave = 0.0, nfvar = 0.0;
    for (i = 0; i < burn; ++i)
    {
        KentMF_dev(r, (const double **) f, a, u, vt, lambda, v1, v2, v3, r2);
        //HabeckMF_dev(r, (const double **) f, theta1, a, u, vt, lambda, r2);

        fv  = H_Kent((const double **) f,  (const double **) r);

        gv  = H_Kent((const double **) g,  (const double **) r);
        nfv = H_Kent((const double **) nf, (const double **) r);

        KentMF_dev(r, (const double **) gf, a, u, vt, lambda, v1, v2, v3, r2);
        //HabeckMF_dev(r, (const double **) gf, theta2, a, u, vt, lambda, r2);

        nfngv = H_Kent((const double **) nfng,(const double **) r);

        printf("MF: % 10.8e % 10.8e % 10.8e % 10.8e\n", nfv, log(fv), nfngv, gv);

        delta = nfv - nfave;
        nfave += delta / (i+1);
        nfvar += delta * (nfv - nfave);

        //nfave += nfv;
        nfngave += nfngv;
        gave += gv;

        if (algo->abort)
            break;
    }

    //nfave /= burn;
    nfvar /= burn;
    nfngave /= burn;
    double ratio = nfave/nfngave;
    gave /= burn;

    printf("\n % 10.8e(% 10.8e) % 10.8e % 10.8e % 10.8e:  % 10.8e\n",
           nfave, sqrt(nfvar/burn), nfngave, ratio, gave, ratio/gave);
    Mat3Print(f);
    Mat3Print(g);

    fflush(NULL);
    free(v1);
    free(v2);
    free(v3);
    free(theta1);
    free(theta2);

//int m; double angle; double vc[3];
//double tr;
//         Mat3Print(rotmat);
//         angle = RotMat2AxisAngle(r, vc);
//
//         printf("Angle: % 10.7f  Axis: % 10.7f % 10.7f % 10.7f\n",
//                 angle, vc[0], vc[1], vc[2]);

    exit(EXIT_SUCCESS);
}


void
GibbsMet(CdsArray *cdsA)
{
    int                     i;
    const int               cnum = cdsA->cnum, vlen = cdsA->vlen;
    int                     burn = algo->bayes;
    char                    paramfname[256], maxfname[256];
    //char                    tmpname[256];
    double                **theta = MatAlloc(cnum, 3);
    double                  invtr, mlik, hmlik, blik, amlik, diff, ediff, liksi;
    double                  maxlogL, maxmlogL;
    int                     badsamp, nsamp, burnin = 100;
//    Cds                    *avecds = cdsA->avecds;
    FILE                   *paramfile = NULL;
    FILE                   *maxfile = NULL;
    double                 *liks = malloc(burn * sizeof(double));
    double                 *mliks = malloc(burn * sizeof(double));
    const gsl_rng_type     *T = NULL;
    gsl_rng                *r2 = NULL;

    gsl_rng_env_setup();
    gsl_rng_default_seed = time(NULL) + getpid() + clock();
    T = gsl_rng_ranlxs2;
    r2 = gsl_rng_alloc(T);


    if(0)
    {
        TestMF(burn, r2);
    }

    sprintf(paramfname, "%s_%s.p", algo->rootname, "gibbs");

    paramfile = fopen(paramfname, "w");
    if (paramfile == NULL)
    {
        perror("\n  ERROR");
        fprintf(stderr,
                "\n  ERROR99: could not open file '%s' for writing. \n", paramfname);
        PrintTheseusTag();
        exit(EXIT_FAILURE);
    }

    sprintf(maxfname, "%s_%s.p", algo->rootname, "gibbs_max");

    maxfile = fopen(maxfname, "w");
    if (paramfile == NULL)
    {
        perror("\n  ERROR");
        fprintf(stderr,
                "\n  ERROR99: could not open file '%s' for writing. \n", maxfname);
        PrintTheseusTag();
        exit(EXIT_FAILURE);
    }

    fprintf(paramfile, "%-12s %12d\n", "NUM", cnum);
    fprintf(paramfile, "%-12s %12d\n", "LEN", vlen);

    fprintf(maxfile, "%-12s %12d\n", "NUM", cnum);
    fprintf(maxfile, "%-12s %12d\n", "LEN", vlen);

    stats->alpha = DBL_MAX;

    if (algo->varweight > 0)
    {
        invtr = 0.0;
        for (i = 0; i < vlen; ++i)
        {
            cdsA->w[i] = 1.0 / cdsA->var[i];
            invtr += cdsA->w[i];
        }

        stats->alpha = vlen / invtr;
        stats->phi = 2.0 * vlen / invtr;
        stats->hierarch_p1 = 0.5 * stats->phi;
    }
    else
    {
        for (i = 0; i < vlen; ++i)
            cdsA->w[i] = 1.0 / stats->var;

        stats->phi = stats->var/vlen;
        stats->hierarch_p1 = 0.5 * stats->phi;
    }

    //////////////////////////////////////////
    if (algo->randgibbs > 0)
    {
        printf("\n    Random Gibbs Init\n");
        RandInitGibbs(cdsA, r2);
    }
//
//     double fac, facsum = 0.0;
//     for (i = 1; i < cnum; ++i)
//     {
//         fac = gsl_rng_uniform(r2);
//         facsum += fac;
//         printf("\nfac[%3d]: % 12.6f", i+1, 1.0/fac);
//         //ScaleCds(scratchA->cds[i], 1.0 / (i+1.0));
//         ScaleCds(cdsA->cds[i], fac);
//         //scratchA->cds[i]->scale = 1.0 / (i+1.0);
//     }
//     printf("\nfacsum: %12.6f", facsum+1.0);

    if (algo->varweight > 0)
         GibbsDiagWrite(cdsA, r2, 0);
    else
         GibbsWrite(cdsA, r2, 0);

    //stats->phi = stats->hierarch_p1;
    CalcLogLGibbs(cdsA);
    CalcMgLogL(cdsA);

    printf("\n    Initial LogL: %f", stats->logL);
    printf("\n    Initial mLogL: %f", stats->mlogL);
    fflush(NULL);

    for (i = 0; i < cnum; ++i)
    {
        //ScaleCds(cdsA->cds[i], i+1.0);
        //cdsA->cds[i]->scale = i+1.0;
        cdsA->cds[i]->scale = 1.0;
    }

    VarCds(cdsA);
    printf("\n    var initial: % f", stats->var);
    printf("\n    phi initial: % f", stats->phi);
    fflush(NULL);

    maxlogL = maxmlogL = -DBL_MAX;
    for (i = 1; i <= burn; ++i)
    {
        if (algo->varweight > 0)
        {
            if (algo->dotrans)
                GibbsTransDiag(cdsA, r2);
            if (algo->dorot)
            //    GibbsRotMardia(cdsA, theta, r2);
            //    GibbsRotHabeck(cdsA, theta, r2);
                GibbsRotKent(cdsA, r2);
            if (algo->scale > 0)
                MetScaleDiag(cdsA, r2);
            if (algo->doave)
                GibbsMeanDiag(cdsA, r2);
            GibbsVarDiag(cdsA, r2);
            GibbsPhi(cdsA, r2);
        }
        else
        {
            if (algo->dotrans)
                GibbsTrans(cdsA, r2);
            if (algo->dorot)
            //    GibbsRotMardia(cdsA, theta, r2);
            //    GibbsRotHabeck(cdsA, theta, r2);
                 GibbsRotKent(cdsA, r2);
            if (algo->scale > 0)
                MetScale(cdsA, r2);
            if (algo->doave)
                GibbsMean(cdsA, r2);
            GibbsVar(cdsA, r2);
        }

        liks[i-1] = CalcLogLGibbs(cdsA);
        stats->hierarch_p1 = 0.5 * stats->phi; // DLT this is ungraceful
        mliks[i-1] = CalcMgLogL(cdsA);

        if (i%100 == 0 || i == 1)
        {
            if (i%1000 == 0 || i == 1)
            {
                GibbsWrite(cdsA, r2, i);
            }

            CalcPRMSD(cdsA);
            WriteSample(paramfile, cdsA, theta, i);

            printf("\n%5d LogL: % 20.3f MargLogL: % 20.3f", i, liks[i-1], mliks[i-1]);
            fflush(NULL);
        }

        if (maxlogL < liks[i-1])
        {
            maxlogL = liks[i-1];

            GibbsMaxWrite(cdsA, r2, -1);
            WriteSample(maxfile, cdsA, theta, i);
        }

        if (maxmlogL < mliks[i-1])
        {
            maxmlogL = mliks[i-1];

            GibbsMaxWrite(cdsA, r2, -2);
            WriteSample(maxfile, cdsA, theta, i);
        }

        if (algo->abort)
        {
            burn = i-1;
            break;
        }
    }

    printf("\n    Done with Gibbs-Metropolis ...\n");
    fflush(NULL);


//////////////////////////////////////////////////////////////////////////////////////////
    blik = 0.0;
    for (i = burnin; i < burn; ++i)
        blik += mliks[i];

    nsamp = burn - burnin;
    blik /= nsamp;

    printf("\n    Expected marginal likelihood (covmat): % 26.4f\n", blik);
    fflush(NULL);

    blik = 0.0;
    for (i = burnin; i < burn; ++i)
        blik += liks[i];

    nsamp = burn - burnin;
    blik /= nsamp;

    printf("    Expected log likelihood: % 26.4f\n", blik);
    fflush(NULL);

    mlik = hmlik = amlik = 0.0;
    badsamp = 0;
    for (i = burnin; i < burn; ++i)
    {
        liksi = liks[i];
        diff = liksi - blik;
        ediff = exp(diff);

        if (isfinite(ediff))
        {
            mlik += ediff;
            hmlik += 1.0 / ediff;
            amlik += liksi;
        }
        else
        {
            ++badsamp;
        }
    }

    nsamp -= badsamp;

    printf("\n    Marginal likelihood: % 14.2f % 14.2f % 14.2f\n",
           log(mlik / nsamp) + blik, blik - log(hmlik) + log(nsamp), amlik / nsamp);

    MatDestroy(&theta);
    fclose(paramfile);
    fclose(maxfile);
    free(liks);
    gsl_rng_free(r2);
}

