/*
    Theseus - maximum likelihood superpositioning of macromolecular structures

    Copyright (C) 2004-2010 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 "Embed_local.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]->innerprod = MatAlloc(cdsA->vlen, cdsA->vlen);

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


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

    MMT = MatAlloc(cdsA->vlen, cdsA->vlen);

    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]->innerprod[i][j] - cdsA->avecds->innerprod[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]); */

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

    MatDestroy(&MMT);
}


/* 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
NNxCds(double **mat, Cds *cds)
{
    int             i, k;

    /* (i x k)(k x j) = (i x j) */
    /* (N x N)(N x 3) = (N x 3) */
    for (i = 0; i < cds->vlen; ++i)
    {
        cds->covx[i] = cds->covy[i] = cds->covz[i] = 0.0;
        for (k = 0; k < cds->vlen; ++k)
        {
            cds->covx[i] += (mat[i][k] * cds->x[k]);
            cds->covy[i] += (mat[i][k] * cds->y[k]);
            cds->covz[i] += (mat[i][k] * cds->z[k]);
        }
    }
}


void
CdsxNN(Cds *cds, const double **mat)
{
    int             j, k;

    /* (i x k)(k x j) = (i x j) */
    /* (3 x N)(N x N) = (3 x N) */
    for (j = 0; j < cds->vlen; ++j)
    {
        cds->covx[j] = cds->covy[j] = cds->covz[j] = 0.0;
        for (k = 0; k < cds->vlen; ++k)
        {
            cds->covx[j] += (cds->x[k] * mat[k][j]);
            cds->covy[j] += (cds->y[k] * mat[k][j]);
            cds->covz[j] += (cds->z[k] * mat[k][j]);
        }
    }
}


/*  double
    RadGyrSqr(Cds *cds)

    computes the unweighted square of the unaveraged radius of gyration
    of a molecule basically the sum of the square of the distances of 
    each atom from the centroid (this function assumes the molecule has
    been centered)
*/
double
RadGyrSqr(const Cds *cds)
{
    int             i;
    double          sum = 0.0;

    for (i = 0; i < cds->vlen; ++i)
        sum += (mysquare(cds->x[i]) + mysquare(cds->y[i]) + mysquare(cds->z[i]));

    return(sum);
}


double
RadGyrSqrW(const Cds *cds, const double *weights)
{
    int             i;
    double          sum = 0.0;

    for (i = 0; i < cds->vlen; ++i)
        sum += weights[i] * (mysquare(cds->x[i]) + mysquare(cds->y[i]) + mysquare(cds->z[i]));

    return(sum);
}


double
RadGyrSqrCov(Cds *cds, const double **weightmat)
{
    int             i;
    double          sum = 0.0;

    CdsxNN(cds, weightmat);

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

    return(sum);
}


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        **innerprod = cds->innerprod;

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

    for (i = 0; i < cds->vlen; ++i)
    {
        for (j = 0; j <= i; ++j)
        {
            innerprod[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->innerprod[j][i] = cds->innerprod[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        **innerprod2 = NULL;
    const double   *x = (const double *) cds->x,
                   *y = (const double *) cds->y,
                   *z = (const double *) cds->z;
    double          xk, yk, zk;

    if (cds->innerprod2 == NULL)
    {
        innerprod2 = cds->innerprod2 = MatAlloc(3, 3);
    }
    else
    {
        innerprod2 = cds->innerprod2;
        memset(&innerprod2[0][0], 0, 9 * sizeof(double));
    }

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

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

    innerprod2[1][0] = innerprod2[0][1];
    innerprod2[2][0] = innerprod2[0][2];
    innerprod2[2][1] = innerprod2[1][2];

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


/* compute the inner product of a symmetrical matrix, in place */
void
MatInnerProdSymIp(double **mat, const int N)
{
    int             i, j, k;
    double        **workmat = NULL;
    
    workmat = MatAlloc(N, N);

    /* InnerProd = M^T M */
    /* (i x k)(k x j) = (i x j) */
    for (i = 0; i < N; ++i)
    {
        for (j = 0; j < N; ++j)
        {
            workmat[i][j] = 0.0;
            for (k = 0; k < N; ++k)
                workmat[i][j] += (mat[i][k] * mat[j][k]);
        }
    }

    for (i = 0; i < N; ++i)
        for (j = 0; j < N; ++j)
             mat[i][j] = workmat[i][j];

    MatDestroy(&workmat);
}


void
MatInnerProdSymOp(double **mato, const double **mati, const int N)
{
    int             i, j, k;

    /* InnerProd = M^T M */
    /* (i x k)(k x j) = (i x j) */
    for (i = 0; i < N; ++i)
    {
        for (j = 0; j < N; ++j)
        {
            mato[i][j] = 0.0;
            for (k = 0; k < N; ++k)
                mato[i][j] += (mati[i][k] * mati[j][k]);
        }
    }
}


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

    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
CalcEDMADistMatOcc(CdsArray *cdsA)
{
    int             i, j, k;
    const int       len = cdsA->vlen, cnum = cdsA->cnum;
    double          off_diagonal, on_diagonal, varsqr;
    double        **H, **distmat;
    double          occ, occsum;

    /* 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;
            occsum = 0.0;
            for (i = 0; i < cnum; ++i)
            {
                occ = cdsA->cds[i]->o[j] * cdsA->cds[i]->o[k];
                occsum += occ;
                cdsA->Dij_matrix[j][k] += occ * cdsA->distmat->matrix[i][j][k];
            }

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

    /* (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)
        {
            occsum = 0.0;
            for (i = 0; i < cnum; ++i)
            {
                occ = cdsA->cds[i]->o[j] * cdsA->cds[i]->o[k];
                occsum += occ;
                cdsA->Var_matrix[j][k] += occ * mysquare(cdsA->distmat->matrix[i][j][k] - distmat[j][k]);
            }

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

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

    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 occupancy */
void
CalcMLDistMatOcc(CdsArray *cdsA)
{
    int             i, j, k, m;
    const int       len = cdsA->vlen;
    const int       cnum = cdsA->cnum;
    double          occsum, occ, tmpx, tmpy, tmpz;
    double        **Dij_matrix = cdsA->Dij_matrix;
    const Cds  **cds = (const Cds **) cdsA->cds;
    const Cds   *cdsi;

    /* (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)
        {
            cdsA->Dij_matrix[j][k] = 0.0;
            occsum = 0.0;
            for (i = 0; i < cnum; ++i)
            {
                cdsi = cds[i];
                occ = cdsi->o[j] * cdsi->o[k];
                occsum += occ;
                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] += occ * (tmpx*tmpx + tmpy*tmpy + tmpz*tmpz);
            }

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

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

            /* printf(" %f %f", occsum, 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 != NULL)
   {
      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
int FastCalcRMSDAndRotation(double *rot, double *A, double *msd, double E0, int len, double minScore)
{
   double          Sxx, Sxy, Sxz, Syx, Syy, Syz, Szx, Szy, Szz;
   double          Szz2, Syy2, Sxx2, Sxy2, Syz2, Sxz2, Syx2, Szy2, Szx2,
                   SyzSzymSyySzz2, Sxx2Syy2Szz2Syz2Szy2, Sxy2Sxz2Syx2Szx2,
                   SxzpSzx, SyzpSzy, SxypSyx, SyzmSzy,
                   SxzmSzx, SxymSyx, SxxpSyy, SxxmSyy;
   double          C[4];
   int i;
   double mxEigenV; 
   double oldg = 0.0;
   double b, a, delta, ms;
   double q1, q2, q3, q4, normq;
   double d11, d12, d13, d14, d21, d22, d23, d24;
   double d31, d32, d33, d34, d41, d42, d43, d44;
   double a2, x2, y2, z2; 
   double xy, az, zx, ay, yz, ax; 
   double d3344_4334, d3244_4234, d3243_4233, d3143_4133,d3144_4134, d3142_4132; 

   Sxx = A[0]; Sxy = A[1]; Sxz = A[2];
   Syx = A[3]; Syy = A[4]; Syz = A[5];
   Szx = A[6]; Szy = A[7]; Szz = A[8];

   Sxx2 = Sxx * Sxx;
   Syy2 = Syy * Syy;
   Szz2 = Szz * Szz;

   Sxy2 = Sxy * Sxy;
   Syz2 = Syz * Syz;
   Sxz2 = Sxz * Sxz;

   Syx2 = Syx * Syx;
   Szy2 = Szy * Szy;
   Szx2 = Szx * Szx;

   SyzSzymSyySzz2 = 2.0*(Syz*Szy - Syy*Szz);
   Sxx2Syy2Szz2Syz2Szy2 = Syy2 + Szz2 - Sxx2 + Syz2 + Szy2;

   C[2] = -2.0 * (Sxx2 + Syy2 + Szz2 + Sxy2 + Syx2 + Sxz2 + Szx2 + Syz2 + Szy2);
   C[1] = 8.0 * (Sxx*Syz*Szy + Syy*Szx*Sxz + Szz*Sxy*Syx - Sxx*Syy*Szz - Syz*Szx*Sxy - Szy*Syx*Sxz);

   SxzpSzx = Sxz + Szx;
   SyzpSzy = Syz + Szy;
   SxypSyx = Sxy + Syx;
   SyzmSzy = Syz - Szy;
   SxzmSzx = Sxz - Szx;
   SxymSyx = Sxy - Syx;
   SxxpSyy = Sxx + Syy;
   SxxmSyy = Sxx - Syy;
   Sxy2Sxz2Syx2Szx2 = Sxy2 + Sxz2 - Syx2 - Szx2;

   C[0] = Sxy2Sxz2Syx2Szx2 * Sxy2Sxz2Syx2Szx2
          + (Sxx2Syy2Szz2Syz2Szy2 + SyzSzymSyySzz2) * (Sxx2Syy2Szz2Syz2Szy2 - SyzSzymSyySzz2)
          + (-(SxzpSzx)*(SyzmSzy)+(SxymSyx)*(SxxmSyy-Szz)) * (-(SxzmSzx)*(SyzpSzy)+(SxymSyx)*(SxxmSyy+Szz))
          + (-(SxzpSzx)*(SyzpSzy)-(SxypSyx)*(SxxpSyy-Szz)) * (-(SxzmSzx)*(SyzmSzy)-(SxypSyx)*(SxxpSyy+Szz))
          + (+(SxypSyx)*(SyzpSzy)+(SxzpSzx)*(SxxmSyy+Szz)) * (-(SxymSyx)*(SyzmSzy)+(SxzpSzx)*(SxxpSyy+Szz))
          + (+(SxypSyx)*(SyzmSzy)+(SxzmSzx)*(SxxmSyy-Szz)) * (-(SxymSyx)*(SyzpSzy)+(SxzmSzx)*(SxxpSyy-Szz));


   mxEigenV = E0;
   for (i = 0; i < 50; ++i)
   {
     oldg = mxEigenV;
     x2 = mxEigenV*mxEigenV;
     b = (x2 + C[2])*mxEigenV;
     a = b + C[1];
     delta = ((a*mxEigenV + C[0])/(2.0*x2*mxEigenV + b + a));
     mxEigenV -= delta;
     if (fabs(mxEigenV - oldg) < fabs((1e-6)*mxEigenV)) {
       break;
     }
   }
   if (i == 50) 
   {
      fprintf(stderr," more than %d iterations needed, something wrong!\b", i);
   }

   ms = 2.0 * (E0 - mxEigenV) / len;
   (*msd) = ms;

   if (minScore > 0) 
   {
        if (ms < minScore)
        {
            // Don't bother with rotation. 
            return -1;
        }   
   }

   d11 = SxxpSyy + Szz-mxEigenV; d12 = SyzmSzy; d13 = - SxzmSzx; d14 = SxymSyx;
   d21 = SyzmSzy; d22 = SxxmSyy - Szz-mxEigenV; d23 = SxypSyx; d24= SxzpSzx;
   d31 = d13; d32 = d23; d33 = Syy-Sxx-Szz - mxEigenV; d34 = SyzpSzy;
   d41 = d14; d42 = d24; d43 = d34; d44 = Szz - SxxpSyy - mxEigenV;
   d3344_4334 = d33 * d44 - d43 * d34; d3244_4234 = d32 * d44-d42*d34;
   d3243_4233 = d32 * d43 - d42 * d33; d3143_4133 = d31 * d43-d41*d33;
   d3144_4134 = d31 * d44 - d41 * d34; d3142_4132 = d31 * d42-d41*d32;
   q1 =  d22*d3344_4334-d23*d3244_4234+d24*d3243_4233;
   q2 = -d21*d3344_4334+d23*d3144_4134-d24*d3143_4133;
   q3 =  d21*d3244_4234-d22*d3144_4134+d24*d3142_4132;
   q4 = -d21*d3243_4233+d22*d3143_4133-d23*d3142_4132;

   normq = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);
   q1 /= normq; q2 /= normq; q3 /= normq; q4 /= normq;

   a2 = q1 * q1;
   x2 = q2 * q2;
   y2 = q3 * q3;
   z2 = q4 * q4;

   xy = q2 * q3;
   az = q1 * q4;
   zx = q4 * q2;
   ay = q1 * q3;
   yz = q3 * q4;
   ax = q1 * q2;

   rot[0] = a2 + x2 - y2 - z2;
   rot[1] = 2 * (xy + az);
   rot[2] = 2 * (zx - ay);
   rot[3] = 2 * (xy - az);
   rot[4] = a2 - x2 + y2 - z2;
   rot[5] = 2 * (yz + ax);
   rot[6] = 2 * (zx + ay);
   rot[7] = 2 * (yz - ax);
   rot[8] = a2 - x2 - y2 + z2;

   return 0;
}


/* static void  */
/* CenterCds(Cds *cds, const int len) */
/* { */
/*     int             i; */
/*     double          xsum, ysum, zsum; */
/*     double         *x = cds->x, *y = cds->y, *z = cds->z; */
/*  */
/*     xsum = ysum = zsum = 0.0; */
/*     for (i = 0; i < len; ++i) */
/*     { */
/*         xsum += x[i]; */
/*         ysum += y[i]; */
/*         zsum += z[i]; */
/*     } */
/*  */
/*     xsum /= len; */
/*     ysum /= len; */
/*     zsum /= len; */
/*  */
/*     for (i = 0; i < len; ++i) */
/*     { */
/*         x[i] -= xsum; */
/*         y[i] -= ysum; */
/*         z[i] -= zsum; */
/*     } */
/* } */


static double
CalcRMSDRotationalMatrix(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->o[i] = 1.0;
    }

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

    /* check to see if the average structure has the wrong chirality,
       since embedding basically randomly reflects the structure */

/*     deviation1 = ProcLAPACKSVDvan(cdsA->cds[0], */
/*                                   avecds, */
/*                                   cdsA->cds[0]->matrix, */
/*                                   cdsA->cds[0]->tmpmat3a, */
/*                                   cdsA->cds[0]->tmpmat3b, */
/*                                   cdsA->cds[0]->tmpmat3c, */
/*                                   cdsA->cds[0]->tmpvec3a); */

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

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

/*     deviation2 = ProcLAPACKSVDvan(cdsA->cds[0], */
/*                                   avecds, */
/*                                   cdsA->cds[0]->matrix, */
/*                                   cdsA->cds[0]->tmpmat3a, */
/*                                   cdsA->cds[0]->tmpmat3b, */
/*                                   cdsA->cds[0]->tmpmat3c, */
/*                                   cdsA->cds[0]->tmpvec3a); */

    deviation2 = CalcRMSDRotationalMatrix(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);
}
