/*
    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 "Embed_local.h"
#include "Embed.h"


void
SetupLele(CdsArray *cdsA)
{
    int             i;

    /* set up matrices and initialize for Lele distmat covariance calculations */
    for (i = 0; i < cdsA->cnum; ++i)
        cdsA->cds[i]->outerprod = MatAlloc(cdsA->vlen, cdsA->vlen);

    cdsA->avecds->outerprod = MatAlloc(cdsA->vlen, cdsA->vlen);
    DistMatsAlloc(cdsA);
}


void
CalcLeleCovMat(CdsArray *cdsA)
{
    double          idf, cov_sum;
    int             i, j, k;

    idf = 1.0 / (3.0 * (double)(cdsA->cnum)); /* ML, biased, maybe should be n-1 to be unbiased?? */

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

    CdsInnerProd(cdsA->avecds);

    for (i = 0; i < cdsA->vlen; ++i)
    {
        for (j = 0; j < cdsA->vlen; ++j)
        {
            cov_sum = 0.0;
            for (k = 0; k < cdsA->cnum; ++k)
                cov_sum += (cdsA->cds[k]->outerprod[i][j] - cdsA->avecds->outerprod[i][j]);

            cdsA->CovMat[i][j] = cov_sum * idf;
        }
    }

    /* MatPrint(cdsA->CovMat, cdsA->vlen); */

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

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

/*     for (i = 0; i < cdsA->vlen; ++i) */
/*         printf("\n -->> LeleCovVar = %7.3e ", cdsA->CovMat[i][i]); */
}


/* caller must find average structure first */
void
CalcLeleVariances(CdsArray *cdsA)
{
    double          idf;
    double         *var = cdsA->var;
    const Cds     **cds = (const Cds **) cdsA->cds;
    const double   *avex = (const double *) cdsA->avecds->x,
                   *avey = (const double *) cdsA->avecds->y,
                   *avez = (const double *) cdsA->avecds->z;

    int             i, j;

    idf = 1.0 / (double)(cdsA->cnum - 1); /* should be n-1 to be unbiased?? */

    for (i = 0; i < cdsA->vlen; ++i)
    {
        var[i] = 0.0;
        for (j = 0; j < cdsA->cnum; ++j)
        {
            var[i] += cds[j]->x[i] * cds[j]->x[i]
                    + cds[j]->y[i] * cds[j]->y[i]
                    + cds[j]->z[i] * cds[j]->z[i];
        }

        var[i] *= idf;
    }

    for (i = 0; i < cdsA->vlen; ++i)
        var[i] -= (avex[i] * avex[i] + avey[i] * avey[i] + avez[i] * avez[i]) / 3.0;

/*     for (i = 0; i < cdsA->vlen; ++i) */
/*         printf("\n -->> lele var = %12.3 ", var[i]); */
}


void
CdsInnerProd(Cds *cds)
{
    /* (i x k)(k x j) = (i x j) */
    /* (N x 3)(3 x N) = (N x N) */
    int             i, j;
    double        **outerprod = cds->outerprod;

    for (i = 0; i < cds->vlen; ++i)
        for (j = 0; j < cds->vlen; ++j)
            cds->outerprod[i][j] = 0.0;

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

    for (i = 0; i < cds->vlen; ++i)
        for (j = 0; j < i; ++j)
            cds->outerprod[j][i] = cds->outerprod[i][j];
}


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

    innerprod = cds->innerprod;
    memset(&innerprod[0][0], 0, 9 * sizeof(double));

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

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

    innerprod[1][0] = innerprod[0][1];
    innerprod[2][0] = innerprod[0][2];
    innerprod[2][1] = innerprod[1][2];

    /* Mat3Print(cds->innerprod); */
}


/*  Calculates EDMA average of distance cds, with chi^2 variance correction for
    bias. This is an unbiased estimate of the average cds, *assuming that the
    cds are distributed normally*.
*/
void
CalcEDMADistMat(CdsArray *cdsA)
{
    int             i, j, k;
    const int       len = cdsA->vlen;
    double          normalize, off_diagonal, on_diagonal, varsqr;
    double        **H = NULL, **distmat = NULL;

    normalize = 1.0 / (double) cdsA->cnum;

    /* set up H, the centering/normalizing matrix */
    off_diagonal = -1.0 / (double) len;
    on_diagonal = 1.0 + off_diagonal;
    H = MatAlloc(len, len);
    distmat = MatAlloc(len, len);

    for (i = 0; i < len; ++i)
    {
        H[i][i] = on_diagonal;
        for (j = 0; j < i; ++j)
            H[i][j] = H[j][i] = off_diagonal;
    }

    /* The next four steps calculate the Eu(M) matrix, */
    /* which is Lele's matrix of squared distances (Lele 1993, pp. 579-580, Theorem 4)*/
    /* Lele, Subhash (1993) "Euclidean Distance Matrix Analysis (EDMA): Estimation of mean form */
    /* and mean form difference." Mathematical Geology 25(5):573-602 */

    /* (1) calculate the symmetric j x k atom squared distance e^i(l,m) matrix for all structure Cds i */
    for (i = 0; i < cdsA->cnum; ++i)
        for (j = 0; j < len; ++j)
            for (k = 0; k < j; ++k)
                cdsA->distmat->matrix[i][j][k] = SqrCdsDist(cdsA->cds[i], j, cdsA->cds[i], k);

    /* (2) calculate the average squared distance matrix ave{e(l,m)} for the CdsArray */
    for (i = 0; i < len; ++i)
        for (j = 0; j < i; ++j)
            distmat[i][j] = 0.0;

    for (i = 0; i < cdsA->cnum; ++i)
        for (j = 0; j < len; ++j)
            for (k = 0; k < j; ++k)
                distmat[j][k] += cdsA->distmat->matrix[i][j][k];

    for (j = 0; j < len; ++j)
        for (k = 0; k < j; ++k)
            distmat[j][k] *= normalize;

    for (i = 0; i < len; ++i)
        for (j = i+1; j < len; ++j)
            distmat[i][j] = distmat[j][i];

    /* (3) find the difference between each  e^i(l,m)  and the average  ave{e(l,m)}, */
    /*     square it, find the average and put it in Var_matrix, */
    /*     and finally set the diagonal to zero */
    for (i = 0; i < len; ++i)
        for (j = 0; j < i; ++j)
            cdsA->Var_matrix[i][j] = 0.0;

    for (i = 0; i < cdsA->cnum; ++i)
        for (j = 0; j < len; ++j)
            for (k = 0; k < j; ++k)
                cdsA->Var_matrix[j][k] += mysquare(cdsA->distmat->matrix[i][j][k] - distmat[j][k]);

    for (j = 0; j < len; ++j)
        for (k = 0; k < j; ++k)
            cdsA->Var_matrix[j][k] *= normalize;

    /* (4a) calculate ave(delta[l,m]), Lele 1993 Theorem 4, eqn. 2 */
    for (i = 0; i < len; ++i)
    {
        for (j = 0; j < i; ++j)
        {
            varsqr = mysquare(distmat[i][j]) - (1.5 * cdsA->Var_matrix[i][j]);
            if (varsqr > 0.0)
                cdsA->Dij_matrix[i][j] = sqrt(varsqr);
            else
                cdsA->Dij_matrix[i][j] = 0.0;
        }
    }
    /* MatPrint(cdsA->Dij_matrix, len); */

    /* (4b) copy lower left triangle to upper right triangle */
    for (i = 0; i < len; ++i)
        for (j = i+1; j < len; ++j)
            cdsA->Dij_matrix[i][j] = cdsA->Dij_matrix[j][i];

    /* VariancesEDMA((const double *) cdsA->var, cdsA->Dij_matrix, distmat, len); */

    MatDestroy(&H);
    MatDestroy(&distmat);
}


void
CalcEDMADistMatNu(CdsArray *cdsA)
{
    int             i, j, k;
    const int       len = cdsA->vlen, cnum = cdsA->cnum;
    double          off_diagonal, on_diagonal, varsqr;
    double        **H = NULL, **distmat = NULL;
    double          nu, nusum;

    /* set up H, the centering/normalizing matrix */
    off_diagonal = -1.0 / (double) len;
    on_diagonal = 1.0 + off_diagonal;
    H = MatAlloc(len, len);
    distmat = MatAlloc(len, len);

    for (i = 0; i < len; ++i)
    {
        H[i][i] = on_diagonal;
        for (j = 0; j < i; ++j)
            H[i][j] = H[j][i] = off_diagonal;
    }

    /* The next four steps calculate the Eu(M) matrix, */
    /* which is Lele's matrix of squared distances (Lele 1993, pp. 579-580, Theorem 4)*/
    /* Lele, Subhash (1993) "Euclidean Distance Matrix Analysis (EDMA): Estimation of mean form */
    /* and mean form difference." Mathematical Geology 25(5):573-602 */

    /* (1) calculate the symmetric j x k atom squared distance e^i(l,m) matrix for all structure Cds i */
    for (i = 0; i < cnum; ++i)
        for (j = 0; j < len; ++j)
            for (k = 0; k < j; ++k)
                cdsA->distmat->matrix[i][j][k] = SqrCdsDist(cdsA->cds[i], j, cdsA->cds[i], k);

    /* (2) calculate the average squared distance matrix ave{e(l,m)} for the CdsArray */
    for (j = 0; j < len; ++j)
    {
        for (k = 0; k < j; ++k)
        {
            cdsA->Dij_matrix[j][k] = 0.0;
            nusum = 0.0;
            for (i = 0; i < cnum; ++i)
            {
                nu = cdsA->cds[i]->nu[j] * cdsA->cds[i]->nu[k];
                nusum += nu;
                cdsA->Dij_matrix[j][k] += nu * cdsA->distmat->matrix[i][j][k];
            }

            if (nusum == 0.0)
                cdsA->Dij_matrix[j][k] = 0.0;
            else
                cdsA->Dij_matrix[j][k] /= nusum;
        }
    }

    /* (3) find the difference between each  e^i(l,m)  and the average  ave{e(l,m)}, */
    /*     square it, find the average and put it in Var_matrix, */
    /*     and finally set the diagonal to zero */
    for (i = 0; i < len; ++i)
        for (j = 0; j < i; ++j)
            cdsA->Var_matrix[i][j] = 0.0;

    for (j = 0; j < len; ++j)
    {
        for (k = 0; k < j; ++k)
        {
            nusum = 0.0;
            for (i = 0; i < cnum; ++i)
            {
                nu = cdsA->cds[i]->nu[j] * cdsA->cds[i]->nu[k];
                nusum += nu;
                cdsA->Var_matrix[j][k] += nu * mysquare(cdsA->distmat->matrix[i][j][k] - distmat[j][k]);
            }

            if (nusum == 0.0)
                cdsA->Var_matrix[j][k] = 0.0;
            else
                cdsA->Var_matrix[j][k] /= nusum;
        }
    }

    /* (4a) calculate ave(delta[l,m]), Lele 1993 Theorem 4, eqn. 2 */
    for (i = 0; i < len; ++i)
    {
        for (j = 0; j < i; ++j)
        {
            varsqr = mysquare(distmat[i][j]) - (1.5 * cdsA->Var_matrix[i][j]);
            if (varsqr > 0.0)
                cdsA->Dij_matrix[i][j] = sqrt(varsqr);
            else
                cdsA->Dij_matrix[i][j] = 0.0;
        }
    }
    /* MatPrint(cdsA->Dij_matrix, len); */

    /* (4b) copy lower left triangle to upper right triangle */
    for (i = 0; i < len; ++i)
        for (j = i+1; j < len; ++j)
            cdsA->Dij_matrix[i][j] = cdsA->Dij_matrix[j][i];

    /* VariancesEDMA((const double *) cdsA->var, cdsA->Dij_matrix, distmat, len); */

    MatDestroy(&H);
    MatDestroy(&distmat);
}



/*  Center/normalize with H,
    B(M) = -0.5 * H{Eu(M)}H
    based on Lele's three-step PCA algorithm given on page 581 Lele 1993

    This function is highly optimized, using the symmetry of the H and Eu(M)
    matrices, and the fact that H contains only two values.
*/
void
LeleCenterMat(double **mat, const int len)
{
    int             i, j, k;
    double        **tmpmat = NULL;
    double          off_diagonal, tmp;

    /* set up H, the centering/normalizing matrix */
    off_diagonal = -1.0 / (double) len;
    /* on_diagonal = 1.0 + off_diagonal; */
    tmpmat = MatAlloc(len, len);

    memcpy(tmpmat[0], mat[0], len*len*sizeof(double));

    for (i = 0; i < len; ++i)
    {
        tmp = 0.0;
        for (k = 0; k < len; ++k)
            tmp += mat[k][i];

        tmp *= off_diagonal;

        for (j = 0; j < len; ++j)
            tmpmat[j][i] += tmp;
    }

    /* MatPrint(tmpmat, len); */

    memcpy(mat[0], tmpmat[0], len*len*sizeof(double));

    for (i = 0; i < len; ++i)
    {
        tmp = 0.0;
        for (k = 0; k < len; ++k)
            tmp += tmpmat[i][k];

        tmp *= off_diagonal;

        for (j = 0; j < len; ++j)
            mat[i][j] += tmp;
    }

    for (i = 0; i < len; ++i)
        for (j = 0; j < len; ++j)
            mat[i][j] *= -0.5;

    MatDestroy(&tmpmat);
}


void
DoubleCenterMat(double **mat, const int len)
{
    int             i, j;
    double          cen;

    for (j = 0; j < len; ++j)
    {
        cen = 0.0;
        for (i = 0; i < len; ++i)
            cen += mat[i][j];

        cen /= len;

        for (i = 0; i < len; ++i)
            mat[i][j] -= cen;
    }

    for (j = 0; j < len; ++j)
    {
        cen = 0.0;
        for (i = 0; i < len; ++i)
            cen += mat[j][i];

        cen /= len;

        for (i = 0; i < len; ++i)
            mat[j][i] -= cen;
    }

    for (i = 0; i < len; ++i)
        for (j = 0; j < len; ++j)
            mat[i][j] *= -0.5;
}


/*  Calculates straight average of distance cds, without X^2 variance
    correction like CalcEDMADistMat() does. This is a biased estimate of
    the average cds (the squared distances between two cds are too
    large by the sum of the variance for each coord), but I believe it is
    maximum likelihood of some sort.
 */
void
CalcMLDistMat(CdsArray *cdsA)
{
    int             i, j, k;
    const int       len = cdsA->vlen;
    double          normalize, tmpx, tmpy, tmpz;
    double        **Dij_matrix = cdsA->Dij_matrix;
    const Cds     **cds = (const Cds **) cdsA->cds;
    const Cds      *cdsi = NULL;

    normalize = 1.0 / cdsA->cnum;

    /* (1) calculate the symmetric j x k atom squared distance e^i(l,m) matrix for all structure Cds i */
    /* (2) calculate the average squared distance matrix ave{e(l,m)} for the CdsArray */
    for (i = 0; i < len; ++i)
        for (j = 0; j < i; ++j)
            Dij_matrix[i][j] = 0.0;

    for (i = 0; i < cdsA->cnum; ++i)
    {
        cdsi = cds[i];
        for (j = 0; j < len; ++j)
        {
            for (k = 0; k < j; ++k)
            {
                tmpx = cdsi->x[j] - cdsi->x[k];
                tmpy = cdsi->y[j] - cdsi->y[k];
                tmpz = cdsi->z[j] - cdsi->z[k];

                Dij_matrix[j][k] += (tmpx*tmpx + tmpy*tmpy + tmpz*tmpz);
            }
        }
    }

    for (j = 0; j < len; ++j)
        for (k = 0; k < j; ++k)
            Dij_matrix[j][k] *= normalize;
    /*MatPrint(cdsA->Dij_matrix, len);*/

    /* (4b) copy lower left triangle to upper right triangle */
    for (i = 0; i < len; ++i)
        for (j = i+1; j < len; ++j)
            Dij_matrix[i][j] = Dij_matrix[j][i];
}


/* Same as CalcMLDistMat(), but weight by nu */
void
CalcMLDistMatNu(CdsArray *cdsA)
{
    int             i, j, k, m;
    const int       len = cdsA->vlen;
    const int       cnum = cdsA->cnum;
    double          nusum, nu, tmpx, tmpy, tmpz;
    double        **Dij_matrix = cdsA->Dij_matrix;
    const Cds     **cds = (const Cds **) cdsA->cds;
    const Cds      *cdsi = NULL;

    /* (1) calculate the symmetric j x k atom squared distance e^i(l,m) matrix for all structure Cds i */
    /* (2) calculate the average squared distance matrix ave{e(l,m)} for the CdsArray */
    for (j = 0; j < len; ++j)
    {
        for (k = 0; k < j; ++k)
        {
            Dij_matrix[j][k] = 0.0;
            nusum = 0.0;
            for (i = 0; i < cnum; ++i)
            {
                cdsi = cds[i];
                nu = cdsi->nu[j] * cdsi->nu[k];
                nusum += nu;
                tmpx = cdsi->x[j] - cdsi->x[k];
                tmpy = cdsi->y[j] - cdsi->y[k];
                tmpz = cdsi->z[j] - cdsi->z[k];

                Dij_matrix[j][k] += nu * (tmpx*tmpx + tmpy*tmpy + tmpz*tmpz);
            }

            /* printf("\n%f", cdsA->Dij_matrix[j][k]); */

            if (nusum == 0.0)
            {
                Dij_matrix[j][k] = 0.0;
            }
            else
                Dij_matrix[j][k] /= nusum;

            /* printf(" %f %f", nusum, cdsA->Dij_matrix[j][k]); */
        }
    }

    /* DLT debug -- could be made more efficient by saving indices of 0.0s */
    for (j = 0; j < len; ++j)
    {
        for (k = 0; k < j; ++k)
        {
            if (Dij_matrix[j][k] == 0.0)
            {
                for (m = 0; m < len; ++m)
                    if (m != k)
                        Dij_matrix[j][k] += Dij_matrix[j][m];

                for (m = 0; m < len; ++m)
                    if (m != k)
                        Dij_matrix[j][k] += Dij_matrix[m][k];

                Dij_matrix[j][k] /= (len*len - 1);
            }
        }
    }

    /* (4b) copy lower left triangle to upper right triangle */
    for (i = 0; i < len; ++i)
        for (j = 0; j < i; ++j)
            Dij_matrix[j][i] = Dij_matrix[i][j];
}


static double
InnerProduct(double *A, Cds *cds1, Cds *cds2, const int len, const double *weight)
{
    double          x1, x2, y1, y2, z1, z2;
    int             i;
    const double   *fx1 = cds1->x, *fy1 = cds1->y, *fz1 = cds1->z;
    const double   *fx2 = cds2->x, *fy2 = cds2->y, *fz2 = cds2->z;
    double          G1 = 0.0, G2 = 0.0;

    A[0] = A[1] = A[2] = A[3] = A[4] = A[5] = A[6] = A[7] = A[8] = 0.0;

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

             G1 += x1 * fx1[i] + y1 * fy1[i] + z1 * fz1[i];

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

             G2 += weight[i] * (x2 * x2 + y2 * y2 + z2 * z2);

             A[0] +=  (x1 * x2);
             A[1] +=  (x1 * y2);
             A[2] +=  (x1 * z2);

             A[3] +=  (y1 * x2);
             A[4] +=  (y1 * y2);
             A[5] +=  (y1 * z2);

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

            G1 += x1 * x1 + y1 * y1 + z1 * z1;

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

            G2 += (x2 * x2 + y2 * y2 + z2 * z2);

            A[0] +=  (x1 * x2);
            A[1] +=  (x1 * y2);
            A[2] +=  (x1 * z2);

            A[3] +=  (y1 * x2);
            A[4] +=  (y1 * y2);
            A[5] +=  (y1 * z2);

            A[6] +=  (z1 * x2);
            A[7] +=  (z1 * y2);
            A[8] +=  (z1 * z2);
        }
    }

    return (G1 + G2) * 0.5;
}


static double
CalcRMSDRotMat(Cds *cds1, Cds *cds2, const int len, double *rot, const double *weight)
{
    double          A[9];
    double          rmsd;

    /* center the structures */
    //CenterCds(cds1, len);
    //CenterCds(cds2, len);

    /* calculate the (weighted) inner product of two structures */
    double E0 = InnerProduct(A, cds1, cds2, len, weight);

    /* calculate the RMSD & rotational matrix */
    FastCalcRMSDAndRotation(rot, A, &rmsd, E0, len, -1);

    return rmsd;
}


/* Calculate the average coordinates from the average distance matrix
   as calculated in CalcEDMADistMat(CdsArray *cdsA) and
   CalcMLDistMat. This is a distance geometry embedding algorithm.

   See:
   Crippen and Havel (1978) Acta Cryst A34:282
   "Stable calculation of coordinates from distance data."

   Gower, J.C (1966) Biometrika 53:3-4:325-338.
   "Some distance properties of latent root and vector methods used in
   multivariate analysis."

   Both the above refs give equivalent methods. Most mol biologists
   know only the first, statisticians the second.

   First, find the eigenvalues and eigenvectors of the NxN distance
   matrix. Second, order them largest first. The first three
   eigenvectors, multiplied by the sqrt of the corresponding eigenvalue,
   are the x, y, and z coordinate vectors for the structure, respectively.
*/
void
EmbedAveCds(CdsArray *cdsA)
{
    int            i;
    int            vlen = cdsA->vlen;
    double         w0, w1, w2;
    double       **z = NULL;
    double        *w = NULL;
    double         deviation1, deviation2;
    Cds           *avecds = cdsA->avecds;
    Cds           *cds = cdsA->cds[0];

    /* Center/normalize with H, */
    /* B(M) = -0.5 * H{Eu(M)}H */
    /* based on Lele's three-step PCA algorithm given on page 581 Lele 1993 */
    DoubleCenterMat(cdsA->Dij_matrix, vlen);

    w = (double *) calloc(vlen, sizeof(double));
    z = MatAlloc(vlen, vlen);

    EigenGSLDest(cdsA->Dij_matrix, vlen, w, z, 1);

    w0 = sqrt(w[0]);
    w1 = sqrt(w[1]);
    w2 = sqrt(w[2]);

    for (i = 0; i < cdsA->vlen; ++i)
    {
        avecds->x[i] = w2 * z[i][2];
        avecds->y[i] = w1 * z[i][1];
        avecds->z[i] = w0 * z[i][0];
        avecds->nu[i] = 1;
    }

    /* WriteAveCdsFile(cdsA, "test.pdb"); */

    /* check to see if the average structure has the wrong chirality,
       since embedding basically randomly reflects the structure */
    deviation1 = CalcRMSDRotMat(cds, avecds, cds->vlen, &cds->matrix[0][0], NULL);

    for (i = 0; i < cdsA->vlen; ++i)
        avecds->x[i] = -avecds->x[i];

    deviation2 = CalcRMSDRotMat(cds, avecds, cds->vlen, &cds->matrix[0][0], NULL);

    if (deviation1 < deviation2)
        for (i = 0; i < cdsA->vlen; ++i)
            avecds->x[i] = -avecds->x[i];

    /* PrintCds(cdsA->avecds); */
    free(w);
    MatDestroy(&z);
}
