/*
    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 "MultiPose_local.h"
#include "MultiPose.h"


static double
CalcScaleFactors(CdsArray *cdsA);

static double
CalcScaleFactorsML(CdsArray *cdsA);

static void
InvGammaAdjustEvals(double *newevals, const int vlen, const int cnum,
                    double *evals, const double b, const double c);


void
PrintMeanCenMassWt2(const double **cds, const double *wts, const int vlen)
{
    int             i;
    double          center[3];
    double          tempx, tempy, tempz;
    double          wti, wtsum;
    const double   *x = cds[0],
                   *y = cds[1],
                   *z = cds[2];

    tempx = tempy = tempz = wtsum = 0.0;
    for (i = 0; i < vlen; ++i)
    {
        wti = wts[i];
        wtsum += wti;
        tempx += (wti * x[i]);
        tempy += (wti * y[i]);
        tempz += (wti * z[i]);
    }

    center[0] = tempx / wtsum;
    center[1] = tempy / wtsum;
    center[2] = tempz / wtsum;

    printf("CENTER: %12.2f %12.2f %12.2f\n", center[0], center[1], center[2]);
}


void
CalcTranslationsOp(CdsArray *cdsA, Algorithm *algo)
{
    int             i;

    for (i = 0; i < cdsA->cnum; ++i)
    {
        if (algo->covweight == 0)
        {
            if (algo->alignment)
            {
                CenMassWtNu2((const double **) cdsA->cds[i]->sc,
                             (const double **) cdsA->avecds->wc,
                             (const int *) cdsA->cds[i]->nu,
                             (const double *) cdsA->w,
                             cdsA->vlen,
                             (const double **) cdsA->cds[i]->matrix,
                             cdsA->cds[i]->center);
            }
            else
            {
                CenMassWt2((const double **) cdsA->cds[i]->sc,
                           (const double *) cdsA->w,
                           cdsA->vlen,
                           cdsA->cds[i]->center);
            }
        }
        else
        {
            CenMassCov2((const double **) cdsA->cds[i]->sc,
                        cdsA->cds[i]->cc,
                        (const double **) cdsA->WtMat,
                        cdsA->vlen,
                        cdsA->cds[i]->center);
        }
    }

    //PrintMeanCenMassWt2((const double **) cdsA->avecds->wc, (const double *) cdsA->w, cdsA->vlen);
}


double
CalcRotations(CdsArray *cdsA)
{
    Cds           **cds = cdsA->cds;
    const Cds      *avecds = cdsA->avecds;
    const double   *wts = (const double *) cdsA->w;
    const double  **wtmat = (const double **) cdsA->WtMat;
    Cds            *tcds = NULL;
    double          deviation, deviation_sum;
    double          norm1, norm2, innprod;
    const int       vlen = cdsA->vlen;
    const int       cnum = cdsA->cnum;
    int             i;


    if (algo->covweight)
    {
        tcds = cdsA->tcds;
        MatMultCdsMultMatDiag(tcds, wtmat, avecds);
    }
    else if (algo->varweight)
    {
        tcds = cdsA->tcds;
        MatDiagMultCdsMultMatDiag(tcds, wts, avecds);
    }
    else if (algo->leastsquares)
    {
        tcds = cdsA->avecds;
    }

    // Assumes Cds have been wt centered previously
    if (algo->alignment)
    {
        for (i = 0; i < cnum; ++i)
        {
            for (int j = 0; j < vlen; ++j)
            {
                if (cds[i]->nu[j] == 0) // working coords, ->wc
                {
                    cds[i]->x[j] = cds[i]->y[j] = cds[i]->z[j] = 0.0;
                }
            }
        }
    }

    deviation = deviation_sum = 0.0;
    for (i = 0; i < cnum; ++i)
    {
       /* note that the avecds are already multiplied by the weight matrices */
        if (algo->pu)
        {
            deviation = CalcRMSDRotationalMatrix(cds[i]->wc, tcds->wc, vlen, &cds[i]->matrix[0][0], NULL);
            deviation = deviation * deviation * vlen;
//             printf("\nqcp deviation: %g", deviation);
//             MatPrint(cds[i]->matrix, 3);
        }
        else
        {
//             deviation = ProcGSLSVDvanNu((const double **) cds[i]->wc, // sc works for non-missing data
//                                        (const double **) tcds->wc,
//                                        cds[i]->nu,
//                                        vlen,
//                                        cds[i]->matrix,
//                                        cdsA->tmpmat3a,
//                                        cdsA->tmpmat3b,
//                                        cdsA->tmpmat3c,
//                                        cdsA->tmpvec3a,
//                                        &norm1, &norm2, &innprod);

            deviation = ProcGSLSVDvan2((const double **) cds[i]->wc, // sc works for non-missing data
                                       (const double **) tcds->wc,
                                       vlen,
                                       cds[i]->matrix,
                                       cdsA->tmpmat3a,
                                       cdsA->tmpmat3b,
                                       cdsA->tmpmat3c,
                                       cdsA->tmpvec3a,
                                       &norm1, &norm2, &innprod);
//             printf("\nSVD deviation: %g", deviation);
//             MatPrint(cds[i]->matrix, 3);
        }

        /* find global rmsd and average cds (both held in structure) */
        cds[i]->wRMSD_from_mean = sqrt(deviation / (3 * vlen));
        deviation_sum += deviation;
    }

    return(deviation_sum);
}


/* This is the classic iterative (not eigendecomp) solution given by Gower 1975 and in
   Gower and Dijksterhuis 2004, Ch 9, page 113, Eqn 9.21 */
static double
CalcScaleFactors(CdsArray *cdsA)
{
    Cds         *cdsi = NULL;
    Cds        **cds = cdsA->cds;
    Cds         *avecds = cdsA->avecds;
    double         *wts = cdsA->w;
    int             i;
    const int       cnum = cdsA->cnum, vlen = cdsA->vlen;
    double          scalesum, selfprod, innprod, norm, avecdstr;


    if (algo->leastsquares)
    {
        avecdstr = TrCdsInnerProd(avecds, vlen);

        norm = 0.0;
        for (i = 0; i < cnum; ++i)
            norm += TrCdsInnerProd(cds[i], vlen);
    }
    else if (algo->varweight)
    {
        avecdstr = TrCdsInnerProdWt(avecds, vlen, wts);

        norm = 0.0;
        for (i = 0; i < cnum; ++i)
            norm += TrCdsInnerProdWt(cds[i], vlen, wts);
    }
    else
    {
        norm = avecdstr = 1.0;
    }

//  for (i = 0; i < vlen; ++i)  // DLT OP
//      wts[i] = 1.0 / cdsA->var[i];  // DLT OP

    scalesum = 0.0;
    for (i = 0; i < cnum; ++i)
    {
        cdsi = cdsA->cds[i];

        if (algo->leastsquares)
        {
            selfprod = TrCdsInnerProd(cdsi, vlen);
            innprod = TrCdsInnerProd2(cdsi, avecds, vlen);

        }
        else if (algo->varweight)
        {
            selfprod = TrCdsInnerProdWt(cdsi, vlen, wts);
            innprod = TrCdsInnerProdWt2(cdsi, avecds, vlen, wts) - 1.0;
        }
        else
        {
            innprod = selfprod = 1.0;
        }

        cdsi->scale = norm * innprod / (cnum * avecdstr * selfprod);
        //cdsi->scale = (sqrt(innprod*innprod + 12.0 * (double) vlen * selfprod) + innprod) / (2.0 * selfprod);
        //cds[i]->scale = innprod / selfprod;
        scalesum += log(cds[i]->scale);
        //printf("\nscale[%3d] = %12.6e", i+1, cdsi->scale);
    }

    scalesum = exp(scalesum / (double) cnum);

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

    //for (i = 0; i < cnum; ++i)
    //    printf("\nscale[%3d]: %12.6f", i+1, 15.5 * 30.0 * cdsA->cds[i]->scale / bsum);

    for (i = 0; i < cnum; ++i)
        ScaleCds(cdsi, cdsA->cds[i]->scale);

    return(scalesum);
}


/* ML solution, scale of structure #1 constrained to be 1 */
static double
CalcScaleFactorsML(CdsArray *cdsA)
{
    Cds           **cds = cdsA->cds;
    Cds            *avecds = cdsA->avecds;
    Cds            *cdsi = NULL;
    int             i;
    const int       cnum = cdsA->cnum, vlen = cdsA->vlen;
    const double   *wts = (const double *) cdsA->w;
    double          scalesum, ft, gt, sigma2, lagrangian;
    //int             scaleanchor = algo->scaleanchor;

    if (algo->leastsquares)
    {
        sigma2 = 0.0;
        for (i = 0; i < vlen; ++i)
            sigma2 += cdsA->var[i];
        sigma2 /= (double) vlen;

        //printf("\nsigma2 = %12.6e \n", sigma2);

        for (i = 0; i < cnum; ++i)
        {
            cdsi = cdsA->cds[i];
            ft = TrCdsInnerProd(cdsi, vlen);
            gt = TrCdsInnerProd2(cdsi, avecds, vlen);

//             if (i == scaleanchor)
//                 cdsi->scale = 1.0;
//             else
                cdsi->scale = (sqrt(gt*gt + 12.0 * vlen * sigma2 * ft) + gt) / (2.0 * ft);
        }
    }
    else if (algo->varweight)
    {
        double      term, vari, phi;

        phi = 2.0*stats->hierarch_p1;

        term = 0.0;
        for (i = 0; i < vlen; ++i)
        {
            vari = cdsA->samplevar[i];
            term += 3.0 * cnum * vari / (3.0 * cnum * vari + phi);
        }

        //printf("term =  % 12.4e\n", term);

        lagrangian = (3.0 * cnum + 1.0) * term / cnum - 3.0 * vlen;
        //printf("term2 = % 12.4e\n", term);

        for (i = 0; i < cnum; ++i)
        {
            cdsi = cdsA->cds[i];
            ft = TrCdsInnerProdWt(cdsi, vlen, wts);
            gt = TrCdsInnerProdWt2(cdsi, avecds, vlen, wts);
            // printf("gt = % 12.4e\n", gt);
            gt += lagrangian;

//             if (i == scaleanchor)
//                 cdsi->scale = 1.0;
//             else
               cdsi->scale = (sqrt(gt*gt + 12.0 * vlen * ft) + gt) / (2.0 * ft);
               //cdsi->scale = (sqrt(gt*gt + 4.0 * (3.0 * vlen + 1.0) * ft) + gt) / (2.0 * ft);

//             printf("scale[%3d] = % 12.4e\n", i+1, cdsi->scale);
        }

//         printf("%5d\n", algo->rounds);
    }
    else
    {
        for (i = 0; i < cnum; ++i)
            cds[i]->scale = 1.0;
    }

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

//     printf("scalesum: % 12.6e % 12.6e\n", scalesum, aveb);

    for (i = 0; i < cnum; ++i)
        cdsA->cds[i]->scale /= aveb;

    for (i = 0; i < cnum; ++i)
        ScaleCds(cdsi, cdsA->cds[i]->scale);

    /* This is to verify that our implicit constraint is actually in effect. */
//     double bsum, ftsum, gtsum;
//     double          nkd = 3.0 * cnum * vlen;
//     bsum = 0.0;
//     for (i = 0; i < cnum; ++i)
//         bsum += TrCdsInnerProdWt(cds[i], vlen, wts) - TrCdsInnerProdWt2(cds[i], avecds, vlen, wts);
//
//     printf("bsum = % 12.6e % 12.6e % 12.6e % 12.6e\n", bsum, nkd, cnum*lagrangian, bsum - nkd - cnum*lagrangian);
//
//     ftsum = 0.0;
//     for (i = 0; i < cnum; ++i)
//         ftsum += TrCdsInnerProdWt(cds[i], vlen, wts);
//
//     gtsum = 0.0;
//     for (i = 0; i < cnum; ++i)
//         gtsum += TrCdsInnerProdWt2(cds[i], avecds, vlen, wts);
//
//     printf("ftsum, gtsum: % 12.6e % 12.6e % 12.6e\n", ftsum, gtsum, 3.0*vlen);

    return(scalesum);
}


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

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

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


void
HierarchVars(CdsArray *cdsA)
{
    switch(algo->hierarch)
    {
        case 0:
            break;

        /* Assuming a known shape param c, real ML-EM fit */
        case 1:
            if (algo->rounds > 4)
                InvGammaFitEvalsEMFixedC(cdsA, 0.5, 1);
            else
                InvGammaFitEvalsEMFixedC(cdsA, 0.5, 0);
            break;

        /* real ML-EM fit, fitting unknown b and c inverse gamma params (scale and shape, resp.) */
        case 2:
            if (algo->rounds > 4)
                InvGammaFitEvalsML(cdsA, 1);
            else
                InvGammaFitEvalsML(cdsA, 0);
            break;

        case 3:
            /* This is the old approximate method, used in versions 1.0-1.1 */
            /* inverse gamma fit of variances, excluding the smallest 3 */
            /* This accounts for the fact that the smallest three eigenvalues of the covariance
               matrix are always zero, i.e. the covariance matrix is necessarily of rank
               vlen - 3 (or usually less, with inadequate amounts of data 3N-6). */
            if (algo->rounds > 4)
                InvGammaFitEvals(cdsA, 1);
            else
                InvGammaFitEvals(cdsA, 0);
            break;

        case 4:
            if (algo->rounds < 15)
                InvGammaFitEvalsEMFixedC(cdsA, 0.5, 1);
            else
                InvGammaFitMarginalGSLBrent(cdsA);
            //InvGammaMarginalFitEvals(cdsA);
            break;

        case 5:
            InvGammaFitMarginalGSLBrent(cdsA);
            break;

        /* marginal solution, currently the default (2015-01-01) */
        /* DEFAULT */
        case 6:
            InvGammaFitMarginalMLPhi(cdsA);
            break;

        case 7:
            {
                const int    vlen = cdsA->vlen, cnum = cdsA->cnum;
                double       phi;
                const double ndf = algo->ndf;
                double      *evals = cdsA->evals;
                double      *invevals = cdsA->tmpvecK;
                double     **evecs = cdsA->tmpmatKK2;
                double      *tmpevals = cdsA->samplevar;
                int          i, j;

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

                //VecPrint(tmpevals, vlen);

                stats->hierarch_p1 = algo->minc;
                phi = 2.0 * algo->minc;

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

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

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

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

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

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

        default:
            printf("\n  ERROR:  Bad -g option \"%d\" \n", algo->hierarch);
            Usage(0);
            exit(EXIT_FAILURE);
            break;
    }

    if (algo->verbose)
        printf("    HierarchVars() chi2:%f\n", stats->hierarch_chi2);
}


/* Calculates weights corresponding to the atomic, row-wise covariance matrix only */
void
CalcWts(CdsArray *cdsA)
{
    int             i;

    double         *var = cdsA->var;
    double         *weight = cdsA->w;
    const int       vlen = cdsA->vlen;

    if (algo->leastsquares)
    {
        for (i = 0; i < vlen; ++i)
            weight[i] = 1.0;
    }
    else if (algo->varweight)
    {
        for (i = 0; i < vlen; ++i)
        {
//             if (var[i] >= DBL_MAX)
//                 weight[i] = 0.0;
//             else if (var[i] <= 0.0)
//                 weight[i] = 0.0;
//             else
                weight[i] =  1.0 / var[i];
                //printf("%4d: % 16.3f % 16.3f \n", i, weight[i], var[i]); //DLT DEBUG
        }

//         if (algo->scale > 0)
//         {
//             double sum = 0.0;
//             for (i = 0; i < vlen; ++i)
//             {
//                 sum += weight[i];
//             }
//
//             for (i = 0; i < vlen; ++i)
//             {
//                 var[i] *= sum;
//                 weight[i] /= (sum/vlen);
//             }
//         }
    }

//     else if (algo->covweight)
//     //  WtMat is calculated in HeriarchVars
//     {
// //         if (algo->rounds < 3)
// //         {
// //             for (i = 0; i < vlen; ++i)
// //                 for (j = 0; j < i; ++j)
// //                     cdsA->CovMat[i][j] = cdsA->CovMat[j][i] = 0.0;
// //         }
//
//         /* CovInvWeightLAPACK(cdsA); */
//         /* pseudoinv_sym(cdsA->CovMat, cdsA->WtMat, vlen, DBL_MIN); */
//         //InvSymEigenOp(cdsA->WtMat, (const double **) cdsA->CovMat, vlen, cdsA->tmpvecK, cdsA->tmpmatKK1, DBL_MIN);
//
//         // WtMat is calculated in HeriarchVars
//     }
}



int
CheckConvergenceInner(CdsArray *cdsA, const double precision)
{

    int             i;
    double          frobnorm = 0.0;

    if (algo->abort)
        return(1);

    for (i = 0; i < cdsA->cnum; ++i)
    {
        frobnorm = Mat3FrobDiff((const double **) cdsA->cds[i]->last_matrix,
                                (const double **) cdsA->cds[i]->matrix);

        if (frobnorm > precision)
            return(0);
    }

    if (algo->verbose)
        printf("\n    -->> Frobenius Norm (Inner %d): %e\n", algo->innerrounds, frobnorm);

    return(1);
}


int
CheckConvergenceOuter(CdsArray *cdsA, int round, const double precision)
{
    int             i;

    if (round >= algo->iterations)
    {
        return(1);
    }
    else if (algo->abort)
    {
        return(1);
    }
    else if (round > 6)
    {
        stats->precision = 0.0;
        for (i = 0; i < cdsA->cnum; ++i)
        {
            stats->precision += Mat3FrobDiff((const double **) cdsA->cds[0]->matrix,
                                             (const double **) cdsA->cds[0]->last_matrix);
        }

        stats->precision /= cdsA->cnum;

        if (algo->verbose)
        {
            printf("    -->> Frobenius Norm (Outer %d): % e\n", round, stats->precision);
            fflush(NULL);
        }

        if (stats->precision > precision)
            return(0);
        else
            return(1);
    }
    else
    {
        return(0);
    }
}


void
InitializeStates(CdsArray *cdsA)
{
    int             i;
    int             slxn; /* index of random coords to select as first */
    const int       cnum = cdsA->cnum;
    const int       vlen = cdsA->vlen;
    Cds           **cds = cdsA->cds;
    Cds            *avecds = cdsA->avecds;

    const gsl_rng_type    *T = gsl_rng_ranlxs2;
    gsl_rng               *r2 = gsl_rng_alloc(T);

    if (algo->covweight && algo->ndf == 0)
    {
        algo->ndf = cdsA->vlen + 2; // minimal value to ensure finite mean of inverse wishart
    }
    else if (algo->ndf == 0)
    {
        algo->ndf = 3;
    }

    for (i = 0; i < cnum; ++i)
    {
        MatCpyGen(cds[i]->sc, (const double **) cds[i]->wc, 5, vlen);
    }

    if (algo->covweight)
    {
        SetupCovWeighting(cdsA);
    }

    memsetd(cdsA->evals, 1.0, vlen);
    memsetd(cdsA->w, 1.0, vlen);

    /* Initialize the algorithm -- we need a centered mean structure as first guess */
    if (algo->embedave)
    {
        printf("    Calculating distance matrix for embedding average ... \n");
        fflush(NULL);

        CdsCopyAll(avecds, cds[0]);
        DistMatsAlloc(cdsA);

        if (algo->alignment)
            CalcMLDistMatNu(cdsA);
        else
            CalcMLDistMat(cdsA);

        printf("    Embedding average structure (ML) ... \n");
        fflush(NULL);

        EmbedAveCds(cdsA);

        for (i = 0; i < vlen; ++i)
            avecds->resSeq[i] = i+1;

        DistMatsDestroy(cdsA);

        printf("    Finished embedding \n");
        fflush(NULL);

        if (algo->write_file)
        {
            char *embed_ave_name = mystrcat(algo->rootname, "_embed_ave.pdb");
            WriteAveCdsFile(cdsA, embed_ave_name);
            MyFree(embed_ave_name);
        }
    }
    else
    {
        slxn = gsl_rng_uniform_int(r2, cnum);
        CdsCopyAll(avecds, cds[slxn]);
    }

    if (algo->dotrans)
    {
        CenMass(avecds);
        ApplyCenterIp(avecds);
    }

    if (algo->seed)
    {
        CalcStats(cdsA);
    }

    if (!algo->doave)
    {
        AveCds(cdsA); // get average from initial superposition in file
    }

    if (!algo->docovars)
    {
        CalcCovariances(cdsA);
    }

    CalcDf(cdsA);

    gsl_rng_free(r2);
    r2 = NULL;
}


/* The real deal */
int
MultiPose(CdsArray *cdsA)
{
    int             i, round, innerround, maybe;
    double          mlogL, lastmlogL, lastscale, scalesum;
    const int       cnum = cdsA->cnum;
    const int       vlen = cdsA->vlen;
    Cds           **cds = cdsA->cds;
    double        **invmat = MatAlloc(3,3);
    double          marg1, marg2, marg3, marg4, marg5, marg6, marg7, marg8;

    marg1 = marg2 = marg3 = marg4 = marg5 = marg6 = marg7 = marg8 = 0.0;

    /* The EM algorithm */
    // Calculate, in this order:
    //   translations
    //   rotations
    //   scale
    //   mean
    //   covariance
    //   hierarchical params (phi)

    /* The outer loop:
       (1) First calculate the translations
       (2) Inner loop -- calc rotations, scales, and mean, until convergence
       (3) Holding the superposition constant, calculate the covariance
           matrix (with corresponding weight matrix) and hierarchical params */
    round = 0;
    maybe = 0;
    mlogL = lastmlogL = lastscale = -DBL_MAX;
    scalesum = 1.0;
    while(1)
    {
        if (algo->nullrun)
            break;

        ++round;
        algo->rounds = round;

        if (algo->verbose)
        {
            printf("\n\n\nNew Outer Round:%3d ///////////////////////////////////////////", round);
            fflush(NULL);
        }

        /* Find weighted center of all cds, for translation vectors */
        if (algo->dotrans)
        {
            CalcTranslationsOp(cdsA, algo);  // VecPrint(cds[0]->center, 3);
            // wait to translate coords right before rotating coords

            // save the translation vector for each coord in the array
            for (i = 0; i < cnum; ++i)
                memcpy(cds[i]->translation, cds[i]->center, 3 * sizeof(double));
        }

        if (algo->dorot)
        {
            /* save the old rotation matrices to test convergence at bottom of outer loop */
            for (i = 0; i < cnum; ++i)
                MatCpySqr(cds[i]->last_outer_matrix, (const double **) cds[i]->matrix, 3);
        }

        /* Inner loop:
           (1) Calc rotations given weights/weight matrices
           (2) Rotate cds with new rotations
           (3) Recalculate average

           Loops till convergence, holding constant the weights, variances, and covariances
           (and thus the translations too) */
        innerround = 0;
        do
        {
            ++innerround;
            algo->innerrounds += innerround;

            if (algo->verbose)
            {
                printf("\n  New Inner Round:%d \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\n", innerround);
                fflush(NULL);
            }

            // translate/center static cds with new translation vector from static coords,
            // put in working coords
            // this is inefficient // DLT FIX --- maybe have a new vec w/this calc only once?
            if (algo->dotrans)
            {
                if (algo->verbose)
                {
                    for (i = 0; i < cnum; ++i)
                    {
                        TranslateCdsOp2(cds[i]->wc,
                                        (const double **) cds[i]->sc,
                                        vlen,
                                        (const double *) cds[i]->last_center);

                        RotateCdsIp2(cds[i]->wc, vlen, (const double **) cds[i]->matrix);

                        memcpy(cds[i]->last_center, cds[i]->center, 3 * sizeof(double));
                    }

                    if (algo->alignment)
                        marg7 = CalcMgLogLNu(cdsA);
                    else
                        marg7 = CalcMgLogL(cdsA);
                }

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

                if (algo->verbose)
                {
                    for (i = 0; i < cnum; ++i)
                        RotateCdsIp2(cds[i]->wc, vlen, (const double **) cds[i]->matrix);

                    if (algo->alignment)
                        marg8 = CalcMgLogLNu(cdsA);
                    else
                        marg8 = CalcMgLogL(cdsA);

                    printf("%7d TransMargLogL ///////////////////////////////////////////// % e % e % e\n",
                           round, marg8, marg7, marg8 - marg7);
                    fflush(NULL);

                    for (i = 0; i < cnum; ++i)
                    {
                        memcpy(invmat[0], cds[i]->matrix[0], 9*sizeof(double));
                        MatTransIp(invmat, 3);
                        RotateCdsIp2(cds[i]->wc, vlen, (const double **) invmat);
                    }
                }
            }

            if (algo->dorot)
            {
                if (algo->verbose)
                {
                    for (i = 0; i < cnum; ++i)
                        RotateCdsIp2(cds[i]->wc, vlen, (const double **) cds[i]->matrix);

                    if (algo->alignment)
                        marg5 = CalcMgLogLNu(cdsA);
                    else
                        marg5 = CalcMgLogL(cdsA);

                    for (i = 0; i < cnum; ++i)
                    {
                        memcpy(invmat[0], cds[i]->matrix[0], 9*sizeof(double));
                        MatTransIp(invmat, 3);
                        RotateCdsIp2(cds[i]->wc, vlen, (const double **) invmat);
                    }
                }

                // save the old rotation matrices to test convergence at bottom of inner loop
                for (i = 0; i < cnum; ++i)
                {
                    MatCpySqr(cds[i]->last_matrix, (const double **) cds[i]->matrix, 3);
                }

                CalcRotations(cdsA); // DLT OP

                // rotate working cds with new rotation matrix from static coords, put in wrking coords
                for (i = 0; i < cnum; ++i)
                {
                    RotateCdsIp2(cds[i]->wc, vlen, (const double **) cds[i]->matrix);
                }

                if (algo->verbose)
                {
                    if (algo->alignment)
                        marg6 = CalcMgLogLNu(cdsA);
                    else
                        marg6 = CalcMgLogL(cdsA);

                    printf("%7d RotMargLogL   ///////////////////////////////////////////// % e % e % e\n",
                           round, marg6, marg5, marg6 - marg5);
                    fflush(NULL);
                }
            }

            if (algo->verbose)
            {
                printf("%7d TrRtMargLogL  ///////////////////////////////////////////// % e % e % e\n",
                       round, marg6, marg7, marg6 - marg7);
                fflush(NULL);
            }

            if (algo->scale > 0)
            {
                lastscale = scalesum;

                if (algo->scale)
                    scalesum = CalcScaleFactorsML(cdsA);
                else if (algo->scale == 2)
                    scalesum = CalcScaleFactors(cdsA);
            }

            /* find global rmsd and average cds (both held in structure) */
            if (algo->doave)
            {
                if (algo->verbose)
                {
                    if (algo->alignment)
                        marg3 = CalcMgLogLNu(cdsA);
                    else
                        marg3 = CalcMgLogL(cdsA);
                }

                if (algo->alignment)
                {
                    AveCdsNu(cdsA);
                    //PrintCds(cdsA->avecds);
                    // EM_MissingCds(cdsA); // DLT OP
                }
                else
                {
                    AveCds(cdsA); //PrintCds(cdsA->avecds);
                }

                if (algo->dotrans)
                {
                    /* center up the mean cds (translations assume wtd center = 0,0,0) */
                    CenMassWt2((const double **) cdsA->avecds->wc, (const double *) cdsA->w, vlen,
                                cdsA->avecds->center);
                    NegTransCdsIp(cdsA->avecds->wc, (const double *) cdsA->avecds->center, vlen);

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

                //PrintMeanCenMassWt2((const double **) cdsA->avecds->wc, cdsA->w, vlen);

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

                if (algo->verbose)
                {
                    if (algo->alignment)
                        marg4 = CalcMgLogLNu(cdsA);
                    else
                        marg4 = CalcMgLogL(cdsA);

                    printf("%7d AveMargLogL   ///////////////////////////////////////////// % e % e % e\n",
                           round, marg4, marg3, marg4 - marg3);
                    fflush(NULL);
                }
            }

            if ((innerround == 1) && // DLT
                CheckConvergenceOuter(cdsA, round, algo->precision)) // DLT
                maybe = 1; // DLT

            if (algo->noinnerloop)
                break;

            if (algo->abort)
                break;

            if (innerround > 100)
            {
                putchar('.');
                fflush(NULL);
                break;
            }
        }
        while((CheckConvergenceInner(cdsA, algo->precision) == 0) &&
              (fabs(scalesum - lastscale) > algo->precision/cnum));

//         WriteTransformations(cdsA, "bug_trans");
//         printf("PHI[%4d]: %18.12f\n", round, 2.0*stats->hierarch_p1);

        /* Holding the superposition constant, calculates the sample covariance matrix */
        if (algo->verbose)
        {
            if (algo->alignment)
                marg1 = CalcMgLogLNu(cdsA);
            else
                marg1 = CalcMgLogL(cdsA);
        }

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

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

        if (algo->verbose)
        {
            if (algo->alignment)
                marg2 = CalcMgLogLNu(cdsA);
            else
                marg2 = CalcMgLogL(cdsA);

            printf("%7d VarMargLogL   ///////////////////////////////////////////// % e % e % e\n",
                   round, marg2, marg2, marg2 - marg1);
            fflush(NULL);
        }

        if (ConditionNumber(cdsA->var, vlen) < FLT_EPSILON || CheckZeroVariances(cdsA))
        {
//             algo->varweight = 0;
//             algo->covweight = 0;
//             algo->leastsquares = 1;

//            printf("\n    ----- WARNING: LEAST SQUARES INVOKED [%d] ----- \n\n", round);
            if (algo->ndf < vlen)
            {
                printf("\n    ----- WARNING: PRIOR DOF increased [%d]: n = %d ----- \n\n", round, vlen);
                fflush(NULL);

                algo->ndf = vlen;
            }
        }

        /* calculate the weights or weight matrices */
        CalcWts(cdsA);

        if (algo->dotrans)
        {
            /* center up the mean cds (translations assume wtd center = 0,0,0) */
            CenMassWt2((const double **) cdsA->avecds->wc, (const double *) cdsA->w, vlen,
                        cdsA->avecds->center);
            NegTransCdsIp(cdsA->avecds->wc, (const double *) cdsA->avecds->center, vlen);

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

//PrintCds(cdsA->avecds); // DLT DEBUG

        if (algo->instfile)
            WriteInstModelFile("_inst.pdb", cdsA);

        if (algo->verbose)
        {
            printf("END Outer Round:%3d /////////////////////////////////////////////\n\n", round);
            fflush(NULL);
        }

        lastmlogL = mlogL;
        if (algo->alignment)
            mlogL = CalcMgLogLNu(cdsA);
        else
            mlogL = CalcMgLogL(cdsA);

        if (algo->printlogL)
        {
            printf("----> %6d mlogL: % 22.3f % e % e %e <----\n",
                   round, mlogL, lastmlogL, mlogL - lastmlogL, stats->hierarch_p1);
        }

        if (round >= algo->iterations)
            break;

        if (algo->abort)
            break;

        if (algo->verbose)
        {
            printf("%7d MargLogL      ///////////////////////////////////////////// % e % e % e\n",
                   round, mlogL, lastmlogL, mlogL - lastmlogL);
            fflush(NULL);
        }

        if (maybe && (fabs(lastmlogL - mlogL) < 0.00001 /* algo->precision */))
            break;
    }

    if (algo->instfile)
        WriteInstModelFile("_inst_final.pdb", cdsA);

    MatDestroy(&invmat);

// printf("MargLogL ///////////////////////////////////////////// % .2f\n",
//         CalcMgLogLNu(cdsA)/(vlen*3.0*cnum));

    return(round);
}

