/*
    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 "pdbStats_local.h"

static void
Vars2Bfacts(CdsArray *cdsA);

static double
SqrCoordMag(const Cds *cds, const int vec);

static double
RadiusGyration(Cds *cds, const double *weights);

static void
MomentsCds(CdsArray *cdsA);

static void
CalcAIC(CdsArray *cdsA);

static void
CalcBIC(CdsArray *cdsA);


void
CheckVars(CdsArray *cdsA)
{
    int     i;

    for(i = 0; i < cdsA->vlen; ++i)
    {
        if (!isfinite(cdsA->var[i]) || cdsA->var[i] < DBL_EPSILON)
        {
            printf("Bad variance: %4d % e\n", i, cdsA->var[i]);
            fflush(NULL);
        }
    }
}


void
CalcDf(CdsArray *cdsA)
{
    int         i,j;

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


void
WriteTransformations(CdsArray *cdsA, char *outfile_name)
{
    FILE           *transfile = NULL;
    int             i, j;
    int             cnum = cdsA->cnum;
    double          angle;
    double         *v = malloc(3*sizeof(double));

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

    fprintf(transfile, "# Convention: X_sup = (X_orig + 1t')R ,\n");
    fprintf(transfile, "# where the X structures are Kx3 row x col matrices (of K atoms),\n");
    fprintf(transfile, "# R is an orthogonal 3x3 rotation matrix,\n");
    fprintf(transfile, "# t is a 3x1 translation column vector, and\n");
    fprintf(transfile, "# 1 is a Kx1 column vector of ones, such that\n");
    fprintf(transfile, "# 1t' is a Kx3 matrix of identical rows, in which\n");
    fprintf(transfile, "# each row is the row vector t'.\n\n");

    fprintf(transfile, "# Translation vectors\n");

    for (i = 0; i < cnum; ++i)
    {
        fprintf(transfile,
                "MODEL %4d t: %9.4f %9.4f %9.4f\n",
                i+1,
                -cdsA->cds[i]->translation[0],
                -cdsA->cds[i]->translation[1],
                -cdsA->cds[i]->translation[2]);
    }

    fprintf(transfile, "\n# Rotation matrices\n");

    for (i = 0; i < cnum; ++i)
    {
        fprintf(transfile, "MODEL %4d R: ", i+1);

        for (j = 0; j < 3; ++j)
        {
            fprintf(transfile,
                    "% 10.7f % 10.7f % 10.7f    ",
                    cdsA->cds[i]->matrix[j][0],
                    cdsA->cds[i]->matrix[j][1],
                    cdsA->cds[i]->matrix[j][2]);
        }

        fputc('\n', transfile);
    }

    fprintf(transfile, "\n# Rotations, angle-axis representation\n");

    for (i = 0; i < cnum; ++i)
    {
        angle = RotMat2AxisAngle(cdsA->cds[i]->matrix, v);

        fprintf(transfile,
                "MODEL %4d Angle: % 10.7f  Axis: % 10.7f % 10.7f % 10.7f\n",
                i+1, angle, v[0], v[1], v[2]);
    }

    if (algo->scale > 0)
    {
        fprintf(transfile, "\n# Scale factors\n");

        for (i = 0; i < cnum; ++i)
        {
            fprintf(transfile,
                    "MODEL %4d beta: %18.10f\n",
                    i+1,
                    cdsA->cds[i]->scale);
        }
    }

    fprintf(transfile, "\n# Mean\n");

    for (i = 0; i < cdsA->vlen; ++i)
    {
        fprintf(transfile,
                "LM %5d M: % 18.12f % 18.12f % 18.12f\n",
                i+1,
                cdsA->avecds->x[i],
                cdsA->avecds->y[i],
                cdsA->avecds->z[i]);
    }

//     fprintf(transfile, "\n# Phi and n\n");
//
//     fprintf(transfile, "PHI %18.12f\n", );

    fprintf(transfile, "\n\n");
    fflush(NULL);

    MyFree(v);
    fclose(transfile);
}


void
WriteVariance(CdsArray *cdsA, char *outfile_name)
{
    FILE           *varfile = NULL;
    int             i, j, dfi, notcore;
    double          rmsd;

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

    fprintf(varfile, "    #ATOM   resName resSeq     variance      std_dev         RMSD\n");

    if (algo->varweight || algo->leastsquares)
    {
        for (i = 0; i < cdsA->vlen; ++i)
        {
            dfi = cdsA->df[i];
            /* we have to factor in the fact that RMSD is over all axes,
               whereas my variance is *per* axis (must multiple variance
               by a factor of 3) */
            rmsd = sqrt(cdsA->var[i] * 6.0 * dfi / (dfi - 1));

            fprintf(varfile,
                    "RES %-5d       %3s %6d %12.6f %12.6f %12.6f",
                    i+1,
                    cdsA->cds[0]->resName[i],
                    cdsA->cds[0]->resSeq[i],
                    cdsA->var[i],
                    sqrt(cdsA->var[i]),
                    rmsd);

            notcore = 0;
            for (j=0; j < cdsA->cnum; ++j)
                if (cdsA->cds[j]->nu[i] == 0)
                    notcore = 1;

            if (notcore == 0)
                fprintf(varfile, " CORE\n");
            else
                fprintf(varfile, "\n");
        }
    }
    else if (algo->covweight)
    {
        for (i = 0; i < cdsA->vlen; ++i)
        {
            dfi = cdsA->df[i];
            fprintf(varfile,
                    "RES %-5d      %3s %6d %12.6f %12.6f %12.6f\n",
                    i+1,
                    cdsA->cds[0]->resName[i],
                    cdsA->cds[0]->resSeq[i],
                    cdsA->CovMat[i][i],
                    sqrt(cdsA->CovMat[i][i]),
                    sqrt(6.0 * cdsA->CovMat[i][i] * dfi / (dfi - 1)));
        }
    }

    fputc('\n', varfile);
    fflush(NULL);

    /* printf("\nVariance range: %f\n", log(VecBiggest(cdsA->var, cdsA->vlen)/VecSmallest(cdsA->var, cdsA->vlen))/log(10.0)); */

    fclose(varfile);
}


void
WriteResiduals(CdsArray *cdsA, char *outfile_name)
{
    FILE           *residualfile = NULL;
    int             i;

    residualfile = myfopen(outfile_name, "w");
    if (residualfile == NULL)
    {
        perror("\n  ERROR");
        fprintf(stderr,
                "\n  ERROR99: could not open file '%s'. \n", outfile_name);
        PrintTheseusTag();
        exit(EXIT_FAILURE);
    }

    for (i = 0; i < cdsA->cnum * cdsA->vlen * 3; ++i)
    {
        fprintf(residualfile,
                "%-3d %12.6f\n",
                i+1,
                cdsA->residuals[i]);
    }
    fputc('\n', residualfile);

    fclose(residualfile);
}


static void
Vars2Bfacts(CdsArray *cdsA)
{
    int         i;
    double      bfact = 8.0 * MY_PI * MY_PI;

    for (i = 0; i < cdsA->vlen; ++i)
    {
         cdsA->avecds->b[i] = cdsA->var[i] * bfact;

         if (cdsA->avecds->b[i] > 99.99)
             cdsA->avecds->b[i] = 99.99;
    }
}


void
Bfacts2PrVars(CdsArray *cdsA, int coord)
{
    int         i;
    double      bfact = 1.0 / (24.0 * MY_PI * MY_PI);

    for (i = 0; i < cdsA->vlen; ++i)
        cdsA->cds[coord]->prvar[i] = cdsA->cds[coord]->b[i] * bfact;

    for (i = 0; i < cdsA->vlen; ++i)
    {
        myassert(cdsA->cds[coord]->prvar[i] > 0.0);
        if (cdsA->cds[coord]->prvar[i] == 0.0)
            cdsA->cds[coord]->prvar[i] = 0.3;
    }
}


/* average of all possible unique pairwise RMSDs */
/* Eqn 1 and following paragraph, Kearsley, S.K. (1990) "An algorithm for the
   simultaneous superposition of a structural series." Journal of Computational
   Chemistry 11(10):1187-1192. */
double
CalcPRMSD(CdsArray *cdsA)
{
    int             i, j, k, cnt;
    double          sqrdist, x;
    double          wsqrdist;
    double          wtsum;
    double          paRMSD, pawRMSD;
    const int       cnum = cdsA->cnum, vlen = cdsA->vlen;
    const Cds     **cds = (const Cds **) cdsA->cds;
    const Cds      *cdsi = NULL, *cdsj = NULL;
    const double   *w = (const double *) cdsA->w;


    sqrdist = wsqrdist = wtsum = 0.0;

    for (k = 0; k < vlen; ++k)
    {
        //scd = 0.0;
        cnt = 0;
        for (i = 0; i < cnum; ++i)
        {
            cdsi = cds[i];

            if (cdsi->nu[k])
            {
                for (j = 0; j < i; ++j)
                {
                    cdsj = cds[j];

                    if (cdsj->nu[k])
                    {
                        x = SqrCdsDist(cdsi, k, cdsj, k);
                        //scd += x;
                        wsqrdist += (w[k] * SqrCdsDist(cdsi, k, cdsj, k));
                        sqrdist += x;
                        wtsum++;
                        cnt++;
                    }
                }
            }
        }

        //printf("rmsd[%4d]:% f\n", k, sqrt(scd / cnt));
    }

//    paRMSD  = (2.0 *  sqrdist) / (double) (vlen * cnum * (cnum - 1));
//    printf("\npaRMSD = %8.3e\n", sqrt(paRMSD));
    paRMSD  = sqrdist / wtsum;
//    printf("\npaRMSD = %8.3e\n", sqrt(paRMSD));
    pawRMSD = wsqrdist / wtsum;
    stats->ave_pawRMSD = sqrt(pawRMSD);
    stats->ave_paRMSD  = sqrt(paRMSD);

    return (stats->ave_paRMSD);
}


/* Calculate a matrix normal maximum likelihood RMSD,
   based on the traces of the inverse covariance matrices --
   this is an RMSD from the mean (a sigma value),
   _not_ an average pairwise RMSD. */
double
CalcMLRMSD(CdsArray *cdsA)
{
    int             i, vlen = cdsA->vlen;
    double         *variance = cdsA->var;
    double         *evals = cdsA->evals;


    if (algo->covweight)
    {
        double det = 0.0;
        for (i = 0; i < vlen; ++i)
            det += log(evals[i]);
        det /= vlen;
        stats->mlRMSD = sqrt(exp(det));
    }
    else if (algo->varweight)
    {
        stats->mlRMSD = 0.0;
        for (i = 0; i < vlen; ++i)
            stats->mlRMSD += (1.0 / variance[i]);

        stats->mlRMSD = sqrt(vlen / stats->mlRMSD);
    }
    else
    {
        stats->mlRMSD = 0.0;
        for (i = 0; i < vlen; ++i)
            stats->mlRMSD += variance[i];

        stats->mlRMSD = sqrt(stats->mlRMSD / vlen);
    }

    return(stats->mlRMSD);
}


double
SqrCdsDist(const Cds *cds1, const int atom1,
           const Cds *cds2, const int atom2)
{
    double          tmpx, tmpy, tmpz;
//     const double *x1 = cds1->x;
//     const double *y1 = cds1->y;
//     const double *z1 = cds1->z;
//     const double *x2 = cds2->x;
//     const double *y2 = cds2->y;
//     const double *z2 = cds2->z;
//
//     tmpx = x2[atom2] - x1[atom1];
//     tmpy = y2[atom2] - y1[atom1];
//     tmpz = z2[atom2] - z1[atom1];

    tmpx = cds2->x[atom2] - cds1->x[atom1];
    tmpy = cds2->y[atom2] - cds1->y[atom1];
    tmpz = cds2->z[atom2] - cds1->z[atom1];

    #ifdef FP_FAST_FMA
    return(fma(tmpx, tmpx, fma(tmpy, tmpy, tmpz * tmpz)));
    #else
    return(tmpx*tmpx + tmpy*tmpy + tmpz*tmpz);
    #endif
}


double
SqrCdsDistMahal2(const Cds *cds1, const int atom1,
                 const Cds *cds2, const int atom2,
                 const double weight)
{
    return(weight * (mysquare(cds2->x[atom2] - cds1->x[atom1]) +
                     mysquare(cds2->y[atom2] - cds1->y[atom1]) +
                     mysquare(cds2->z[atom2] - cds1->z[atom1])));
}


static double
SqrCoordMag(const Cds *cds, const int vec)
{
    double           xx, yy, zz;
    double           sqrmag;

    xx = cds->x[vec] * cds->x[vec];
    yy = cds->y[vec] * cds->y[vec];
    zz = cds->z[vec] * cds->z[vec];

    sqrmag = xx + yy + zz;

    return(sqrmag);
}


static double
RadiusGyration(Cds *cds, const double *weights)
{
    double          sum;
    int             i;

    sum = 0.0;

    for (i = 0; i < cds->vlen; ++i)
        sum += (weights[i] * SqrCoordMag(cds, i));

    cds->radgyr = sqrt(sum / cds->vlen);
    return(cds->radgyr);
}


static void
MomentsCds(CdsArray *cdsA)
{
    double ave, median, adev, mdev, sdev, var, skew, kurt, hrange, lrange;

    moments((const double *) cdsA->residuals,
            cdsA->cnum * cdsA->vlen * 3, /* data array and length */
            &ave, &median,       /* mean and median */
            &adev, &mdev,        /* ave dev from mean and median */
            &sdev, &var,         /* std deviation, variance */
            &skew, &kurt,        /* skewness and kurtosis */
            &hrange, &lrange);   /* range of data, high and low */

    stats->skewness[3] = skew;
    stats->kurtosis[3] = kurt;
}


void
CalcNormResiduals(CdsArray *cdsA)
{
    int             j, k, m;
    double          logL, rootv;
    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         *normresid = NULL;

    if (cdsA->residuals == NULL)
        cdsA->residuals = calloc(vlen * 3 * cnum, sizeof(double));

    normresid = cdsA->residuals;

    j = 0;
    for (k = 0; k < vlen; ++k)
    {
        rootv = sqrt(1.0 / var[k]);

        myassert(isfinite(rootv));
        myassert(rootv > DBL_EPSILON);

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

            if (cdsm->nu[k])
            {
                normresid[j] = (cdsm->x[k] - avex[k]) * rootv;
                ++j;
                normresid[j] = (cdsm->y[k] - avey[k]) * rootv;
                ++j;
                normresid[j] = (cdsm->z[k] - avez[k]) * rootv;
                ++j;
            }
        }
    }

    //VecPrint(normresid, j-1); exit(1);

    stats->chi2 = chi_sqr_adapt(normresid, j-1, 0, &logL, 0.0, 1.0, normal_pdf, normal_lnpdf, normal_int);
//    printf("\nchi^2: %f\n", stats->chi2);
}


void
CalcNormResidualsLS(CdsArray *cdsA)
{
    int             j, k, m;
    double          logL, avevar;
    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         *normresid = NULL;

    if (cdsA->residuals == NULL)
        cdsA->residuals = calloc(vlen * 3 * cnum, sizeof(double));

    normresid = cdsA->residuals;

    avevar = 0.0;
    for (m = 0; m < vlen; ++m)
        avevar += var[m];
    avevar /= vlen;
//    printf("\n##### %f %f", avevar, stats->stddev*stats->stddev); fflush(NULL);

    j = 0;
    for (k = 0; k < vlen; ++k)
    {
        for (m = 0; m < cnum; ++m)
        {
            cdsm = (Cds *) cds[m];
            // printf("\n%4d %4d %d", k, m, cdsm->nu[k]); fflush(NULL);
            if (cdsm->nu[k])
            {
                normresid[j] = (cdsm->x[k] - avex[k]);
                ++j;
                normresid[j] = (cdsm->y[k] - avey[k]);
                ++j;
                normresid[j] = (cdsm->z[k] - avez[k]);
                ++j;
                // printf("\n%4d %f", j, normresid[j]); fflush(NULL);
            }
        }
    }

    //VecPrint(normresid, j-1); exit(1);

    stats->chi2 = chi_sqr_adapt(normresid, j-1, 0, &logL, 0.0, 1.0, normal_pdf, normal_lnpdf, normal_int);
//    printf("\nchi^2: %f\n", stats->chi2);
}


double
FrobTerm(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
FrobTermDiag(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);
}


double
FrobTerm2(CdsArray *cdsA)
{
    int             i, j, k, m;
    double          trace;
    const int       cnum = cdsA->cnum, vlen = cdsA->vlen;
    const Cds     **cds = (const Cds **) cdsA->cds;
    const Cds      *cdsi = NULL;
    const double   *avex = (const double *) cdsA->avecds->x,
                   *avey = (const double *) cdsA->avecds->y,
                   *avez = (const double *) cdsA->avecds->z;
    double        **ErrMat = MatAlloc(vlen, 3);
    double        **TmpMat = MatAlloc(3, vlen);
    double        **SumMat = MatAlloc(3, 3);
    double        **InvCovMat = MatAlloc(vlen, vlen);

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

    //pseudoinv_sym(cdsA->CovMat, InvCovMat, vlen, DBL_MIN);
    PseudoinvSymGSL((const double **) cdsA->CovMat, InvCovMat, vlen, DBL_MIN);

    for (m = 0; m < cnum; ++m)
    {
        for (i = 0; i < vlen; ++i)
        {
            cdsi = (Cds *) cds[m];
            ErrMat[i][0] = cdsi->x[i] - avex[i];
            ErrMat[i][1] = cdsi->y[i] - avey[i];
            ErrMat[i][2] = cdsi->z[i] - avez[i];
        }

        /* (i x k)(k x j) = (i x j) */
        for (i = 0; i < 3; ++i)
        {
            for (j = 0; j < vlen; ++j)
            {
                TmpMat[i][j] = 0.0;
                for (k = 0; k < vlen; ++k)
                    TmpMat[i][j] += ErrMat[k][i] * InvCovMat[k][j];
            }
        }

        for (i = 0; i < 3; ++i)
        {
            for (j = 0; j < 3; ++j)
            {
                for (k = 0; k < vlen; ++k)
                    SumMat[i][j] += TmpMat[i][k] * ErrMat[k][j];
            }
        }
    }

    trace = SumMat[0][0] + SumMat[1][1] + SumMat[2][2];

    MatDestroy(&ErrMat);
    MatDestroy(&SumMat);
    MatDestroy(&TmpMat);
    MatDestroy(&InvCovMat);

    return(trace);
}


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);
}


double
CalcMgLogL(CdsArray *cdsA)
{
    const int       vlen = cdsA->vlen;
    const int       cnum = cdsA->cnum;
    double         *var = cdsA->samplevar;
    double         *evals = cdsA->evals;
    double          mglogl = 0.0, term, lndet, sum;
    const double    phi = 2.0*stats->hierarch_p1;
    const double    ndf = algo->ndf;
    int             i;

    //AveCdsNu(cdsA);
    CalcSampleVar(cdsA);

    if (algo->leastsquares)
    {
        term = 0.5 * (3.0 * vlen * cnum - 2.0); // with Jeffreys prior on phi this is just 3KN/2
        sum = 0.0;
        for (i = 0; i < vlen; ++i)
            sum += var[i];

        sum *= 0.5;

        mglogl = gsl_sf_lngamma(term)
               - 0.5 * (3.0 * vlen * cnum) * log(2.0*M_PI)
               -term * log(3.0*cnum*sum);
    }
    else if (algo->varweight)
    {
        term = 0.5 * (3.0 * cnum + ndf); /* n = 1 corresponds to c = 0.5 for inverse gamma */

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

        mglogl = vlen * gsl_sf_lngamma(term)
               - term * vlen * gsl_sf_lngamma(0.5 * ndf)
               + 0.5 * ndf * vlen * log(phi)
               - term * lndet;

        mglogl -= phi; // gamma/exponential prior on phi, mean = 1

        //printf("mglogl: % 20.10f % 20.10f % 20.10e % 20.10f\n", term, lndet, phi, mglogl);
        //fflush(NULL);
    }
    else if (algo->covweight)
    {
        term = 0.5 * (3.0 * cnum + ndf);

        mglogl = MultivarLnGamma(vlen, term)
               - MultivarLnGamma(vlen, 0.5 * ndf) // requires ndf >= vlen
               - 3.0 * cnum * vlen * log(M_PI)
               + 0.5 * vlen * ndf * log(phi)
               ;

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

        mglogl -= term * lndet;
        //mglogl += log(phi) - phi/1; // exponential prior on phi, mean = 1
        mglogl -= phi/1;
    }

    stats->mlogL = mglogl;

    return(mglogl);
}


double
CalcMgLogLNu(CdsArray *cdsA)
{
    const int       vlen = cdsA->vlen;
    const int       cnum = cdsA->cnum;
    double         *svar = cdsA->samplevar;
    double         *evals = cdsA->evals;
    double          mglogl, term, lndet, sum, ri;
    const double    phi = 2.0*stats->hierarch_p1;
    const double    ndf = algo->ndf;
    int             i;


    CalcSampleVarNu(cdsA);

    if (algo->leastsquares) // DLT FIX --- this has to incorporate incomplete info nu
    {
        term = 0.5 * (3.0 * vlen * cnum - 2.0); // with Jeffreys prior on phi this is just 3KN/2
        sum = 0.0;
        for (i = 0; i < vlen; ++i)
            sum += svar[i];

        sum *= 0.5;

        mglogl = gsl_sf_lngamma(term)
               - 0.5 * (3.0 * vlen * cnum) * log(2.0*M_PI)
               -term * log(3.0*cnum*sum);
    }
    else if (algo->varweight)
    {
        mglogl = 0.0;
        for (i = 0; i < vlen; ++i)
        {
            ri = cdsA->df[i];
//             ri = 0.0;
//             for (j = 0; j < cnum; ++j)
//             {
//                 nui = cdsA->cds[j]->nu[i];
//                 ri += nui;
//             }

            term = 0.5 * (3.0 * ri + ndf); /* n = 1 corresponds to c = 0.5 for inverse gamma */

            lndet = log(3.0*ri*svar[i] + phi);
            //printf("%3d var:% e\n", i, svar[i]);

            mglogl += gsl_sf_lngamma(term)
                    - gsl_sf_lngamma(0.5 * ndf)
                    - 1.5 * ri * log(M_PI)
                    + 0.5 * ndf * log(phi)
                    - term * lndet;
        }

        mglogl -= phi; // exponential prior on phi

//         printf("mglogl: % 20.10f % 20.10f % 20.10e % 20.10f\n", term, lndet, phi, mglogl);
//         fflush(NULL);
    }
    else if (algo->covweight)
    {
        term = 0.5 * (3.0 * cnum + ndf);

        mglogl = MultivarLnGamma(vlen, term)
               - MultivarLnGamma(vlen, 0.5 * ndf) // requires ndf >= vlen
               - 3.0 * cnum * vlen * log(M_PI)
               + 0.5 * vlen * ndf * log(phi)
               ;

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

        mglogl -= term * lndet;
        //mglogl += log(phi) - phi/1; // exponential prior on phi, mean = 1
        mglogl -= phi/1;
    }
    else
    {
        mglogl = 0.0;
    }

    stats->mlogL = mglogl;

    return(mglogl);
}


/* Calculate number of parameters fit for the specified algorithm and model */
double
CalcParamNum(CdsArray *cdsA)
{

    const double    vlen = cdsA->vlen;
    const double    cnum = cdsA->cnum;
    double          params;

    params = 0.0;

    /* for the atomic covariances/variances */
    if (algo->leastsquares)
        params += 1.0;

//     if (algo->varweight) // no longer needed with marginal likelihood
//         params += vlen;

    if (algo->covweight)
        params += vlen * (vlen + 1.0) / 2.0;

    /* for the hierarchical parameters */
    switch(algo->hierarch)
    {
        case 0:
            break;

        case 1: /* ML fit of variances to an inverse gamma distribution - 1 param */
        case 5:
        case 6:
            params += 1.0;
            break;

        /* ML fit of variances to an inverse gamma distribution - 2 param */
        case 2:
        case 3:
        case 4:
        case 7:
        case 8:
        case 9:
        case 10:
        case 11:
        case 13:
            params += 2.0;
            break;
    }

    /* for the mean */
    if (algo->doave)
        params += 3.0 * vlen;

    /* translations */
    if (algo->dotrans)
        params += 3.0 * cnum;

    /* rotations */
    if (algo->dorot)
        params += 3.0 * cnum;

    return(params);
}


static void
CalcAIC(CdsArray *cdsA)
{
    double         n, p;

    stats->nparams = p = CalcParamNum(cdsA);
    stats->ndata = n = 3.0 * cdsA->cnum * cdsA->vlen;

    stats->AIC = stats->mlogL - p * n / (n - p - 1);
}


static void
CalcBIC(CdsArray *cdsA)
{
    double         n, p;

    p = CalcParamNum(cdsA);
    n = 3.0 * cdsA->cnum * cdsA->vlen;

    stats->BIC = stats->mlogL - (log(n) * p / 2.0);
}


double **
CalcCov(CdsArray *cdsA)
{
    double          newx1, newy1, newz1, newx2, newy2, newz2;
    double          covsum;
    double         *cdskx = NULL, *cdsky = NULL, *cdskz = NULL;
    int             i, j, k;
    const int       cnum = cdsA->cnum;
    const int       vlen = cdsA->vlen;
    const Cds     **cds = (const Cds **) cdsA->cds;
    const Cds      *cdsk = NULL;
    double        **CovMat = MatAlloc(vlen, vlen);
    const double   *avex = (const double *) cdsA->avecds->x,
                   *avey = (const double *) cdsA->avecds->y,
                   *avez = (const double *) cdsA->avecds->z;


    /* calculate covariance matrix of atoms across structures,
       based upon current superposition, put in CovMat */
    for (i = 0; i < vlen; ++i)
    {
        for (j = 0; j <= i; ++j)
        {
            covsum = 0.0;
            for (k = 0; k < cnum; ++k)
            {
                cdsk = cds[k];
                cdskx = cdsk->x;
                cdsky = cdsk->y;
                cdskz = cdsk->z;

                newx1 = cdskx[i] - avex[i];
                newy1 = cdsky[i] - avey[i];
                newz1 = cdskz[i] - avez[i];

                newx2 = cdskx[j] - avex[j];
                newy2 = cdsky[j] - avey[j];
                newz2 = cdskz[j] - avez[j];

                #ifdef FP_FAST_FMA
                covsum += fma(newx1, newx2, fma(newy1, newy2, newz1 * newz2));
                #else
                covsum += (newx1 * newx2 + newy1 * newy2 + newz1 * newz2);
                #endif
            }

            CovMat[i][j] = CovMat[j][i] = covsum; /* sample variance, ML biased not n-1 definition */
        }
    }

    return(CovMat);
}


void
CalcSampleVar(CdsArray *cdsA)
{
    double         newx, newy, newz;
    double         stddev, fact;
    int            i, k;
    const int      cnum = cdsA->cnum;
    const int      vlen = cdsA->vlen;
    const Cds    **cds = (const Cds **) cdsA->cds;
    const Cds     *cdsk = NULL;
    const double   *avex = (const double *) cdsA->avecds->x,
                   *avey = (const double *) cdsA->avecds->y,
                   *avez = (const double *) cdsA->avecds->z;
    double         *svar = cdsA->samplevar;

    memset(svar, 0, vlen * sizeof(double));

	for (k = 0; k < cnum; ++k)
	{
		cdsk = cds[k];
		for (i = 0; i < vlen; ++i)
		{
            newx = cdsk->x[i] - avex[i];
            newy = cdsk->y[i] - avey[i];
            newz = cdsk->z[i] - avez[i];

            #ifdef FP_FAST_FMA
            svar[i] += fma(newx, newx, fma(newy, newy, newz * newz));
            #else
            svar[i] += (newx * newx) + (newy * newy) + (newz * newz);
            #endif
        }
    }

    fact = 1.0 / (3.0 * cnum);

    stddev = 0.0;
    for (i = 0; i < vlen; ++i)
    {
        svar[i] *= fact;
        stddev += svar[i];
    }

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


void
CalcSampleVarNu2(CdsArray *cdsA)
{
    double         newx, newy, newz;
    double         varsum, stddev;
    int            i, k;
    const int      cnum = cdsA->cnum;
    const int      vlen = cdsA->vlen;
    const Cds    **cds = (const Cds **) cdsA->cds;
    const Cds     *cdsk = NULL;
    const double   *avex = (const double *) cdsA->avecds->x,
                   *avey = (const double *) cdsA->avecds->y,
                   *avez = (const double *) cdsA->avecds->z;
    double          nu, nusum;
    double         *svar = cdsA->samplevar;
//    const int   *df = cdsA->df;

    stddev = 0.0;
    for (i = 0; i < vlen; ++i)
    {
        varsum = 0.0;
        nusum = 0.0;
        for (k = 0; k < cnum; ++k)
        {
            cdsk = cds[k];

            nu = cdsk->nu[i];
            nusum += nu;

            newx = cdsk->x[i] - avex[i];
            newy = cdsk->y[i] - avey[i];
            newz = cdsk->z[i] - avez[i];

            #ifdef FP_FAST_FMA
            varsum += nu * fma(newx, newx, fma(newy, newy, newz * newz));
            #else
            varsum += nu * ((newx * newx) + (newy * newy) + (newz * newz));
            #endif
        }

        svar[i] = varsum / (3.0 * nusum); // df[i]);
        stddev += varsum;
    }

// printf("\n");
//     for (i = 0; i < vlen; ++i)
//         printf("ROUND:%5d %4d %16.6f %16.6f\n", algo->rounds, i, svar[i], cdsA->var[i]);
// printf("\n");
// fflush(NULL);

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


// More efficient version than above (~2.5X ?)
void
CalcSampleVarNu(CdsArray *cdsA)
{
    double         newx, newy, newz;
    double         stddev, nu;
    int            i, k;
    const int      cnum = cdsA->cnum;
    const int      vlen = cdsA->vlen;
    const Cds    **cds = (const Cds **) cdsA->cds;
    const Cds     *cdsk = NULL;
    const double   *avex = (const double *) cdsA->avecds->x,
                   *avey = (const double *) cdsA->avecds->y,
                   *avez = (const double *) cdsA->avecds->z;
    double         *svar = cdsA->samplevar;
    const int      *df = (const int *) cdsA->df;

    memset(svar, 0, vlen * sizeof(double));

	for (k = 0; k < cnum; ++k)
	{
		cdsk = cds[k];
		for (i = 0; i < vlen; ++i)
		{
			nu = cdsk->nu[i];
            if (nu > 0.0)
            {
				newx = cdsk->x[i] - avex[i];
				newy = cdsk->y[i] - avey[i];
				newz = cdsk->z[i] - avez[i];

				#ifdef FP_FAST_FMA
				svar[i] += fma(newx, newx, fma(newy, newy, newz * newz));
				#else
				svar[i] += (newx * newx) + (newy * newy) + (newz * newz);
				#endif

// 				if (svar[i] < DBL_MIN) // DLT DEBUG
// 				    printf("^^^ %5d %5d: % 8.3f % 8.3f % 8.3f --- % 8.3f % 8.3f % 8.3f\n",
// 				           k, i, cdsk->x[i], cdsk->y[i], cdsk->z[i], avex[i], avey[i], avez[i]);
			}
		}
	}

    stddev = 0.0;
    for (i = 0; i < vlen; ++i)
    {
        svar[i] /= (3.0 * df[i]);
        stddev += svar[i];
    }

// DLT DEBUG
// printf("\n");
//     for (i = 0; i < vlen; ++i)
//         printf("ROUND:%5d %4d %4d %16.6f %16.6f -- %8.3f %8.3f %8.3f\n", algo->rounds, i, df[i], svar[i], cdsA->var[i], avex[i], avey[i], avez[i]);
// printf("\n");
// fflush(NULL);

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


/* Calculate the trace of the inner product of some coordinates.
   This is the same as the squared radius of gyration without normalization
   for the number of atoms. */
double
TrCdsInnerProd(Cds *cds, const int len)
{
    int             i;
    double          sum, tmpx, tmpy, tmpz;

    sum = 0.0;
    for (i = 0; i < len; ++i)
    {
        tmpx = cds->x[i];
        tmpy = cds->y[i];
        tmpz = cds->z[i];
        sum += (tmpx*tmpx + tmpy*tmpy + tmpz*tmpz);
    }

    return(sum);
}


double
TrCdsInnerProdWt(Cds *cds, const int len, const double *w)
{
    int             i;
    double          sum, tmpx, tmpy, tmpz;

    sum = 0.0;
    for (i = 0; i < len; ++i)
    {
        tmpx = cds->x[i];
        tmpy = cds->y[i];
        tmpz = cds->z[i];
        sum += w[i] * (tmpx*tmpx + tmpy*tmpy + tmpz*tmpz);
    }

    return(sum);
}


double
TrCdsInnerProd2(Cds *cds1, Cds *cds2, const int len)
{
    int             i;
    double          sum;

    sum = 0.0;
    for (i = 0; i < len; ++i)
    {
        sum += (cds1->x[i]*cds2->x[i] + cds1->y[i]*cds2->y[i] + cds1->z[i]*cds2->z[i]);
    }

    return(sum);
}


double
TrCdsInnerProdWt2(Cds *cds1, Cds *cds2, const int len, const double *w)
{
    int             i;
    double          sum;

    sum = 0.0;
    for (i = 0; i < len; ++i)
    {
        sum += w[i] * (cds1->x[i]*cds2->x[i] + cds1->y[i]*cds2->y[i] + cds1->z[i]*cds2->z[i]);
    }

    return(sum);
}


void
UnbiasMean(CdsArray *scratchA)
{
    const int       vlen = scratchA->vlen;
    int             i;
    double          term, trsiginv;
    Cds            *cds = scratchA->avecds;

    trsiginv = 0.0;
    for (i = 0; i < vlen; ++i)
    {
        trsiginv += (cds->x[i] * cds->x[i] +
                     cds->y[i] * cds->y[i] +
                     cds->z[i] * cds->z[i]) / scratchA->var[i];
    }

    term = 1.0 - 6.0 / trsiginv;

    if (term > 0.0)
        term = sqrt(term);
    else
        term = 0.0;

    printf(" bias constant of the mean % f % f\n", trsiginv, term);
    fflush(NULL);

    for (i = 0; i < vlen; ++i)
    {
        cds->x[i] *= term;
        cds->y[i] *= term;
        cds->z[i] *= term;
    }
}


/* Calculate superposition stats */
void
CalcStats(CdsArray *incdsA)
{
    int             i, j;
    int tmph;
    double          smallestRMSD, n;
    const int       cnum = incdsA->cnum;
    const int       vlen = incdsA->vlen;
    double         *evals = malloc(3 * sizeof(double));
    double        **rotmat = MatAlloc(3, 3);
    double        **lastmat = MatAlloc(3,3);
    CdsArray       *cdsA = NULL;

//     cdsA = CdsArrayInit();
//     CdsArrayAlloc(cdsA, cnum, vlen);
//     CdsArraySetup(cdsA);
//     CdsArrayCopy(cdsA, incdsA);

    cdsA = incdsA;

//     if (algo->covweight)
//         SetupCovWeighting(cdsA);
//     else

    if (algo->pca > 0)
        if (cdsA->CovMat == NULL)
            cdsA->CovMat = MatAlloc(vlen, vlen);

    //if (algo->alignment)
    CalcDf(cdsA);

    if (algo->doave)
    {
        if (algo->alignment)
        {
            AveCdsNu(cdsA);
            EM_MissingCds(cdsA);
            //printf("\n\nAveCds\n");
            //PrintCds(scratchA->avecds);
        }
        else
        {
            AveCds(cdsA);
        }

        if (algo->mbias)
            UnbiasMean(cdsA);
    }

    //CalcSampleVar(cdsA);

    if (algo->docovars)
        CalcCovariances(cdsA);

    memcpy(cdsA->var, cdsA->samplevar, vlen * sizeof(double));

    if (algo->dohierarch)
    {
        if (algo->varweight || algo->covweight)
            HierarchVars(cdsA);
    }

    CalcWts(cdsA);

    /* CheckVars(cdsA); */
    if (algo->leastsquares)
        CalcNormResidualsLS(cdsA);
    else
        CalcNormResiduals(cdsA);

    //CalcMgLogL(cdsA);
    CalcMgLogLNu(cdsA);
    CalcAIC(cdsA);
    CalcBIC(cdsA);
    CalcMLRMSD(cdsA);
    CalcPRMSD(cdsA);

    Vars2Bfacts(cdsA);

    MomentsCds(cdsA);

    n = (double) vlen * cnum * 3.0;

    if (algo->leastsquares)
    {
        stats->omnibus_chi2 = stats->chi2; // DLT FIX
        stats->omnibus_chi2_P = 1.0; // DLT FIX
    }
    else
    {
        stats->omnibus_chi2 =
            (vlen * stats->hierarch_chi2 + n * stats->chi2) / (n + vlen);
        stats->omnibus_chi2_P =
            chisqr_sdf(vlen * stats->hierarch_chi2 + n * stats->chi2, n + vlen, 0);
    }

    stats->SES = sqrt((6.0 * n * (n-1)) / ((n-2) * (n+1) * (n+3))); /* exact formulas */
    stats->SEK = sqrt((24.0 * n * (n-1) * (n-1)) / ((n-3) * (n-2) * (n+3) * (n+5)));

    if (algo->covweight && (algo->write_file > 0 || algo->info) && algo->pca == 0)
    {
/*         if (algo->alignment) */
/*             CalcCovMatNu(cdsA); */
/*         else */
/*             CalcCovMat(cdsA); */
        char *cov_name = mystrcat(algo->rootname, "_cov.mat");
        char *cor_name = mystrcat(algo->rootname, "_cor.mat");
        double ndf = algo->ndf;
        double fact = (3.0*cnum+ndf)/(3.0*cnum+ndf-vlen-1.0);

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

        PrintCovMatGnuPlot((const double **) cdsA->CovMat, vlen, cov_name);

        //write_C_mat((const double **) cdsA->CovMat, vlen, 4, 10);

        CovMat2CorMat(cdsA->CovMat, vlen);
        PrintCovMatGnuPlot((const double **) cdsA->CovMat, vlen, cor_name);
        MyFree(cov_name);
        MyFree(cor_name);
    }

    if (algo->fullpca)
    {
        printf("    Calculating anisotropic Principal Components of the superposition ... \n");
        fflush(NULL);

        if (cdsA->FullCovMat == NULL)
            cdsA->FullCovMat = MatAlloc(3 * vlen, 3 * vlen);

        CalcFullCovMat(cdsA);
        Calc3NPCA(cdsA); /* PCA analysis of covariance matrix */
    }
    else if (algo->pca > 0)
    {
        printf("    Calculating isotropic Principal Components of the superposition ... \n");
        fflush(NULL);

        if (algo->alignment)
            CalcCovMatNu(cdsA);
        else
            CalcCovMat(cdsA);

        tmph = algo->hierarch;

        HierarchVars(cdsA);
        algo->hierarch = tmph;
        //#include "internmat.h"
        //memcpy(&cdsA->CovMat[0][0], &internmat[0][0], vlen * vlen * sizeof(double));
        CalcPCA(cdsA); /* PCA analysis of covariance matrix */
    }

    if (algo->stats)
    {
        RadiusGyration(cdsA->avecds, cdsA->w);

        for (i = 0; i < cnum; ++i)
            RadiusGyration(cdsA->cds[i], cdsA->w);
    }

    printf("    Calculating likelihood statistics ... \n");
    fflush(NULL);

    smallestRMSD = DBL_MAX;
    for (i = 0; i < cnum; ++i)
    {
        if (smallestRMSD > cdsA->cds[i]->wRMSD_from_mean)
        {
            smallestRMSD = cdsA->cds[i]->wRMSD_from_mean;
            stats->median = i;
        }
    }

    MyFree(evals);
    MatDestroy(&rotmat);
    MatDestroy(&lastmat);
}


void
CalcPreStats(CdsArray *cdsA)
{
    CalcStats(cdsA);

    stats->starting_stddev = stats->stddev;
    stats->starting_paRMSD = stats->ave_paRMSD;
    stats->starting_pawRMSD = stats->ave_pawRMSD;
    stats->starting_mlRMSD = stats->mlRMSD;
    stats->starting_logL = stats->logL;
}
