/*
    Theseus - maximum likelihood superpositioning of macromolecular structures

    Copyright (C) 2004-2014 Douglas L. Theobald

    This program 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 2 of the License, or
    (at your option) any later version.

    This program 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 this program; if not, write to the:

    Free Software Foundation, Inc.,
    59 Temple Place, Suite 330,
    Boston, MA  02111-1307  USA

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

#include "MultiPose_local.h"
#include "MultiPose.h"


static double
CalcScaleFactors(CdsArray *cdsA);

static double
CalcScaleFactorsML(CdsArray *cdsA);

static void
*CalcRot_pth(void *rotdata_ptr);

static double
CalcRotations_pth(CdsArray *cdsA, RotData **rotdata, pthread_t *callThd,
                  pthread_attr_t *attr, const int thrdnum);

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


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


double
CalcRotations(CdsArray *cdsA)
{
    Cds           **cds = cdsA->cds;
    const Cds      *avecds = cdsA->avecds;
    const double   *wts = (const double *) cdsA->w;
    Cds            *tcds = NULL;
    double          deviation = 0.0, deviation_sum = 0.0;
    int             i;


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

    for (i = 0; i < cdsA->cnum; ++i)
    {
        if (algo->tenberge)
        {
            AveCdsTB(cdsA, i);
            MatDiagMultCdsMultMatDiag(tcds, wts, avecds);
        }

        /* note that the avecds are already multiplied by the weight matrices */
        if (algo->pu)
        {
            double **qcpcds = MatAlloc(3, cds[i]->vlen);
            double **qcptcds = MatAlloc(3, cds[i]->vlen);
            size_t len = cds[i]->vlen * sizeof(double);
            memcpy(qcpcds[0], cds[i]->x, len);
            memcpy(qcpcds[1], cds[i]->y, len);
            memcpy(qcpcds[2], cds[i]->z, len);
            memcpy(qcptcds[0], tcds->x, len);
            memcpy(qcptcds[1], tcds->y, len);
            memcpy(qcptcds[2], tcds->z, len);
            deviation = CalcRMSDRotationalMatrix(qcpcds, qcptcds, cds[i]->vlen, &cds[i]->matrix[0][0], NULL);
            deviation = deviation * deviation * cds[i]->vlen;
//             printf("\nqcp deviation: %g", deviation);
//             MatPrint(cds[i]->matrix, 3);
            MatDestroy(&qcpcds);
            MatDestroy(&qcptcds);
        }
        else
        {
            deviation = ProcGSLSVDvan(cds[i],
                                      tcds,
                                      cds[i]->matrix,
                                      cdsA->tmpmat3a,
                                      cdsA->tmpmat3b,
                                      cdsA->tmpmat3c,
                                      cdsA->tmpvec3a);
//          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 * cdsA->vlen));
        deviation_sum += deviation;
    }

    return(deviation_sum);
}


double
CalcRotations2(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;
    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 < cdsA->cnum; ++i)
        {
            for (int j = 0; j < cdsA->vlen; ++j)
            {
                if (cdsA->cds[i]->nu[j] == 0)
                {
                    cds[i]->x[j] = 0.0;
                    cds[i]->y[j] = 0.0;
                    cds[i]->z[j] = 0.0;
                }
            }
        }
    }

    deviation = deviation_sum = 0.0;
    for (i = 0; i < cdsA->cnum; ++i)
    {
//         if (algo->alignment)
//         {
//             deviation = ProcGSLSVDvanNu2((const double **) cds[i]->sc,
//                                           (const double **) tcds->wc,
//                                           (const int *) cds[i]->nu,
//                                           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);
//         }
//         else

        if (algo->tenberge)
        {
            AveCdsTB(cdsA, i);
            MatDiagMultCdsMultMatDiag(tcds, wts, avecds);
        }

        /* 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 = 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);
}


static void
*CalcRot_pth(void *rotdata_ptr)
{
    int             i;
    double          deviation = 0.0;
    RotData        *rotdata = (RotData *) rotdata_ptr;
    Cds            *cds = NULL;

    for (i = rotdata->start; i < rotdata->end; ++i)
    {
        cds = rotdata->cds[i];
        /* note that the avecds are already multiplied by the weight matrices */
//        deviation = CalcRMSDRotationalMatrix(cds, rotdata->tcds, cds->vlen, &cds->matrix[0][0], NULL);

        /* rotate the scratch cds with new rotation matrix */
        RotateCdsIp(cds, (const double **) cds->matrix);

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

    pthread_exit((void *) 0);
}


static double
CalcRotations_pth(CdsArray *cdsA, RotData **rotdata, pthread_t *callThd,
                  pthread_attr_t *attr, const int thrdnum)
{
    Cds           **cds = cdsA->cds;
    const Cds      *avecds = cdsA->avecds;
    const double   *wts = (const double *) cdsA->w;
    Cds            *tcds = cdsA->tcds;
    double          deviation_sum = 0.0;
    int             i, rc = 0, incr;
    const int       cnum = cdsA->cnum;

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

    incr = cnum / thrdnum;

    for (i = 0; i < thrdnum - 1; ++i)
    {
        rotdata[i]->cds = cds;
        rotdata[i]->tcds = tcds;
        rotdata[i]->start = i * incr;
        rotdata[i]->end = i*incr + incr;
        rotdata[i]->vlen = cdsA->vlen;

        rc = pthread_create(&callThd[i], attr, CalcRot_pth, (void *) rotdata[i]);

        if (rc)
        {
            printf("ERROR811: return code from pthread_create() %d is %d\n", i, rc);
            exit(EXIT_FAILURE);
        }
    }

    rotdata[thrdnum - 1]->cds = cds;
    rotdata[thrdnum - 1]->tcds = tcds;
    rotdata[thrdnum - 1]->start = (thrdnum - 1) * incr;
    rotdata[thrdnum - 1]->end = cnum;
    rotdata[thrdnum - 1]->vlen = cdsA->vlen;

    rc = pthread_create(&callThd[thrdnum - 1], attr, CalcRot_pth, (void *) rotdata[thrdnum - 1]);

    if (rc)
    {
        printf("ERROR811: return code from pthread_create() %d is %d\n", i, rc);
        exit(EXIT_FAILURE);
    }

    for (i = 0; i < thrdnum; ++i)
    {
        rc = pthread_join(callThd[i], (void **) NULL);

        if (rc)
        {
            printf("ERROR812: return code from pthread_join() %d is %d\n", i, rc);
            exit(EXIT_FAILURE);
        }
    }

    for (i = 0; i < cnum; ++i)
        deviation_sum += 3 * cdsA->vlen * cds[i]->wRMSD_from_mean * cds[i]->wRMSD_from_mean;

    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, var3Ni, phi;

        phi = 2.0*stats->hierarch_p1;

        term = 0.0;
        for (i = 0; i < vlen; ++i)
        {
            var3Ni = cdsA->samplevar3N[i];
            term += var3Ni / (var3Ni + 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 b, const double c)
{
    int         i;

    for (i = 0; i < vlen; ++i)
        newevals[i] = (3.0*cnum*evals[i] + 2.0*b) / (3.0*cnum + 2.0*c);
        // this is required for an EM algorithm of the variances
        //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;

        case 6:
            {
                const int    vlen = cdsA->vlen, cnum = cdsA->cnum;
                double       phi;
                const double nu = algo->covnu;
                double      *evals = cdsA->evals;
                double      *invevals = cdsA->tmpvecK;
                double     **evecs = cdsA->tmpmatKK2;
                double      *tmpevals = cdsA->samplevar3N;
                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, nu);
                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);
}


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

    int             i;

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

    for (i = 0; i < cdsA->cnum; ++i)
    {
        if (Mat3FrobEq((const double **) cdsA->cds[i]->last_matrix,
                       (const double **) cdsA->cds[i]->matrix, precision) == 0)
            return(0);
    }

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

    for (i = 0; i < cnum; ++i)
    {
        MatCpyGen(cds[i]->sc, (const double **) cds[i]->wc, 3, vlen);
        memcpy(cds[i]->so, cds[i]->o, vlen * sizeof(double));
        memcpy(cds[i]->sb, cds[i]->b, vlen * sizeof(double));
    }

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

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

    stats->hierarch_p1 = 1;
    stats->hierarch_p2 = 0.5;

    algo->covnu = 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);
            free(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);
    }

    CalcDf(cdsA);

    gsl_rng_free(r2);
    r2 = NULL;
}


/* The real thing */
int
MultiPose(CdsArray *cdsA)
{
    int             i, round, innerround, maybe;
    double          frobnorm, mlogL, lastmlogL, lastscale, scalesum;
    const int       cnum = cdsA->cnum;
    const int       vlen = cdsA->vlen;
    Cds           **cds = cdsA->cds;

    /* 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, 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);

            // save the translation vector for each coord in the array
            // wait to translate coords right before rotating coords
            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)
				MatCpySym(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 OP // DLT FIX --- maybe have a new vec w/this calc only once?
            if (algo->dotrans)
            {
                for (i = 0; i < cnum; ++i)
                {
                    TranslateCdsOp2(cds[i]->wc,
                                    (const double **) cds[i]->sc,
                                    vlen,
                                    (const double *) cds[i]->center);
                }
            }

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

//                 if (algo->alignment)
//                     CalcRotationsNu2(cdsA);
//                 else
                   CalcRotations2(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 && innerround == 1)
            {
                frobnorm = 0.0;
                for (i = 0; i < cnum; ++i)
                    frobnorm += FrobDiffNormIdentMat((const double **) cds[i]->matrix, 3); // DLT FIX: now obsolete for OP
                frobnorm /= cnum;

                printf("-----<<<<< %3d Frobenius Norm (Outer): % 8.3e ///////\n", round, frobnorm);
                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->alignment)
                {
                    AveCdsNu(cdsA);
                    // EM_MissingCds(cdsA); // DLT OP
                }
                else
                {
                    AveCds(cdsA); //PrintCds(cdsA->avecds);
                }

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

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

            if (algo->verbose)
            {
                frobnorm = 0.0;
                for (i = 0; i < cnum; ++i)
                    frobnorm += FrobDiffNormIdentMat((const double **) cds[i]->matrix, 3);
                frobnorm /= cnum;
                printf("  -->> %3d Frobenius Norm (Inner %d): % e\n", round, innerround, frobnorm);
                printf("  End Inner Round:%d \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\n", innerround);
                fflush(NULL);
            }

            if (algo->noinnerloop)
                break;

            if (algo->abort)
                break;

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

        /* Holding the superposition constant, calculates the covariance matrices  */
        if (algo->docovars)
            CalcCovariances(cdsA);

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

        if (CheckZeroVariances(cdsA))
        {
            algo->varweight = 0;
            algo->covweight = 0;
            algo->leastsquares = 1;
            printf("\n   ----- WARNING: LEAST SQUARES INVOKED [%d] ----- \n", round);
            fflush(NULL);
        }

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

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

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

        lastmlogL = mlogL;
        mlogL = CalcMgLogL(cdsA);

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

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

        if (algo->abort)
            break;

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

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

    return(round);
}


int
MultiPose_pth(CdsArray *baseA)
{
    int             i, round, innerround;
    int             slxn; /* index of random coord to select as first */
    const int       cnum = baseA->cnum;
    const int       vlen = baseA->vlen;
    double         *evals = malloc(3 * sizeof(double));
    Algorithm      *algo = NULL;
    Statistics     *stats = NULL;
    Cds           **cds = NULL;
    Cds            *avecds = NULL;
    CdsArray       *scratchA = NULL;

    gsl_rng               *r2 = NULL;
    const gsl_rng_type    *T = NULL;

    T = gsl_rng_ranlxs2;
    r2 = gsl_rng_alloc(T);

    // THREAD STUFF /////////////////////////////////////////////////////
    const int       thrdnum = algo->threads;
    RotData       **rotdata = malloc(thrdnum * sizeof(RotData *));
    AveData       **avedata = malloc(thrdnum * sizeof(AveData *));
    pthread_t      *callThd = malloc(thrdnum * sizeof(pthread_t));
    pthread_attr_t  attr;

    for (i = 0; i < thrdnum; ++i)
    {
        rotdata[i] = malloc(sizeof(RotData));
        avedata[i] = malloc(sizeof(AveData));
    }

    pthread_attr_init(&attr);
/*     pthread_attr_getstacksize (&attr, &stacksize); */
/*     printf("\nDefault stack size = %d", (int) stacksize); */
    pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
    pthread_attr_setscope(&attr, PTHREAD_SCOPE_SYSTEM);
    // THREAD STUFF /////////////////////////////////////////////////////

    /* setup scratchA */
    scratchA = CdsArrayInit();
    CdsArrayAlloc(scratchA, cnum, vlen);
    CdsArraySetup(scratchA);

    baseA->scratchA = scratchA;

    /* duplicate baseA -- copy to scratchA */
    CdsArrayCopy(scratchA, baseA);

    /* setup local aliases based on scratchA */
    cds = scratchA->cds;
    avecds = scratchA->avecds;

    stats->hierarch_p1 = 1.0;
    stats->hierarch_p2 = 1.0;

    slxn = gsl_rng_uniform_int(r2, cnum);
    CdsCopyAll(avecds, baseA->cds[slxn]);

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

    /* The outer loop:
       (1) First calculates the translations
       (2) Does inner loop -- calc rotations and average till convergence
       (3) Holding the superposition constant, calculates the covariance
           matrices and corresponding weight matrices, looping till
           convergence when using a dimensional/axial covariance matrix
    */
    round = 0;
    while(1)
    {
        if (algo->nullrun)
            break;

        ++round;
        algo->rounds = round;

        /* Find weighted center and translate all cds */
        if (algo->dotrans)
        {
            CalcTranslationsOp(baseA, algo); // DLT OP
            for (i = 0; i < cnum; ++i)
                ApplyCenterIp(cds[i]);

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

        /* 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;

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

            /* find the optimal rotation matrices */
            if (algo->dorot)
            {
                if (algo->alignment)
                {
                    CalcRotationsNu(scratchA);
                }
                else
                {
                    // THREAD STUFF /////////////////////////////////////////////////////
                    CalcRotations_pth(scratchA, rotdata, callThd, &attr, thrdnum);
                    // THREAD STUFF /////////////////////////////////////////////////////
                }
            }

            if (innerround == 1 &&
                CheckConvergenceOuter(scratchA, round, algo->precision))
                   goto outsidetheloops;

            /* find global rmsd and average cds (both held in structure) */
            if (algo->doave)
            {
                if (algo->alignment)
                {
                    AveCdsNu(scratchA);
                    EM_MissingCds(scratchA);
                }
                else
                {
                    // THREAD STUFF /////////////////////////////////////////////////////
                    AveCds_pth(scratchA, avedata, callThd, &attr, thrdnum);
                    // THREAD STUFF /////////////////////////////////////////////////////
                }
            }

            //stats->wRMSD_from_mean = sqrt(deviation_sum / (3 * vlen * cnum));

            if (algo->noinnerloop)
                break;
            else if (innerround > 160)
            {
                putchar(',');
                fflush(NULL);
                break;
            }
        }
        while(CheckConvergenceInner(scratchA, algo->precision) == 0);

        if (algo->docovars)
        {
            CalcCovariances(scratchA);
            if (algo->varweight || algo->covweight)
                HierarchVars(scratchA);
        }

        CalcWts(scratchA);
    }

    outsidetheloops:

    CdsArrayDestroy(&scratchA);
    free(evals);

    // THREAD STUFF /////////////////////////////////////////////////////
    pthread_attr_destroy(&attr);
    for (i = 0; i < thrdnum; ++i)
        free(rotdata[i]);
    for (i = 0; i < thrdnum; ++i)
        free(avedata[i]);
    free(rotdata);
    free(avedata);
    free(callThd);
    // THREAD STUFF /////////////////////////////////////////////////////

    gsl_rng_free(r2);
    r2 = NULL;

    return(round);
}


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

    double         *variance = 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 (variance[i] >= DBL_MAX)
                weight[i] = 0.0;
            else if (variance[i] <= 0.0)
                weight[i] = 0.0;
            else
                weight[i] =  1.0 / variance[i];
        }

//         if (algo->scale > 0)
//         {
//             double sum = 0.0;
//             for (i = 0; i < vlen; ++i)
//             {
//                 sum += weight[i];
//             }
//
//             for (i = 0; i < vlen; ++i)
//             {
//                 variance[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
//     }
}