/*
    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 <pthread.h>
#include <stdlib.h>
#include <string.h>
#include <stdio.h>
#include <math.h>
#include <float.h>
#include <ctype.h>
#include "CovMat.h"
#include "DLTutils.h"
#include "Error.h"
#include "Embed.h"
#include "DLTmath.h"
#include "pdbIO.h"
#include "Threads.h"
#include "pdbUtils.h"


void
CdsCopyXYZ(Cds *cds1, const Cds *cds2)
{
    memcpy(cds1->x, cds2->x, cds2->vlen * sizeof(double));
    memcpy(cds1->y, cds2->y, cds2->vlen * sizeof(double));
    memcpy(cds1->z, cds2->z, cds2->vlen * sizeof(double));
}


void
CdsCopy(Cds *cds1, const Cds *cds2)
{
    strncpy(cds1->filename, cds2->filename, FILENAME_MAX-1);
    memcpy(cds1->resSeq, cds2->resSeq, cds2->vlen * sizeof(int));
    memcpy(cds1->x, cds2->x, cds2->vlen * sizeof(double));
    memcpy(cds1->y, cds2->y, cds2->vlen * sizeof(double));
    memcpy(cds1->z, cds2->z, cds2->vlen * sizeof(double));
    memcpy(cds1->o, cds2->o, cds2->vlen * sizeof(double));
}


/* copy cds, omitting the given atom */
void
CdsDelete(CdsArray *cdsA, const int omit)
{
    int             i, j;

    if(omit == cdsA->vlen - 1)
    {
        cdsA->vlen--;
        cdsA->avecds->vlen--;

        for (i = 0; i < cdsA->cnum; ++i)
            cdsA->cds[i]->vlen--;
    }
    else
    {
        cdsA->vlen--;
        cdsA->avecds->vlen--;

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

        memmove(&cdsA->avecds->resSeq[omit], &cdsA->avecds->resSeq[omit+1],
                (cdsA->vlen - omit) * sizeof(int));
        memmove(&cdsA->avecds->x[omit], &cdsA->avecds->x[omit+1],
                (cdsA->vlen - omit) * sizeof(double));
        memmove(&cdsA->avecds->y[omit], &cdsA->avecds->y[omit+1],
                (cdsA->vlen - omit) * sizeof(double));
        memmove(&cdsA->avecds->z[omit], &cdsA->avecds->z[omit+1],
                (cdsA->vlen - omit) * sizeof(double));

        for (j = 0; j < cdsA->cnum; ++j)
        {
            memmove(&cdsA->cds[j]->resSeq[omit], &cdsA->cds[j]->resSeq[omit+1],
                    (cdsA->vlen - omit) * sizeof(int));
            memmove(&cdsA->cds[j]->x[omit], &cdsA->cds[j]->x[omit+1],
                    (cdsA->vlen - omit) * sizeof(double));
            memmove(&cdsA->cds[j]->y[omit], &cdsA->cds[j]->y[omit+1],
                    (cdsA->vlen - omit) * sizeof(double));
            memmove(&cdsA->cds[j]->z[omit], &cdsA->cds[j]->z[omit+1],
                    (cdsA->vlen - omit) * sizeof(double));
        }
    }
}


void
CdsArrayCopy(CdsArray *cdsA1, const CdsArray *cdsA2)
{
    int             i;

    cdsA1->vlen = cdsA2->vlen;
    cdsA1->cnum = cdsA2->cnum;
    cdsA1->pdbA = cdsA2->pdbA;

    for (i = 0; i < cdsA2->cnum; ++i)
        CdsCopyAll(cdsA1->cds[i], cdsA2->cds[i]);

    CdsCopyAll(cdsA1->avecds, cdsA2->avecds);

    AlgorithmCopy(cdsA1->algo, cdsA2->algo);
    StatisticsCopy(cdsA1->stats, cdsA2->stats);

    memcpy(cdsA1->var, cdsA2->var, cdsA2->vlen * sizeof(double));
    memcpy(cdsA1->w, cdsA2->w, cdsA2->vlen * sizeof(double));
}


void
AlgorithmCopy(Algorithm *algo1, const Algorithm *algo2)
{
    memcpy(algo1, algo2, sizeof(Algorithm));
    algo1->argv = NULL; /* DLT debug -- these should be copied */
    algo1->infiles = NULL; /* DLT debug -- these should be copied */
    if (algo2->selection != NULL)
    {
        algo1->selection = (char *) malloc((strlen(algo2->selection) + 1) * sizeof(char));
        strcpy(algo1->selection, algo2->selection);
    }
    if (algo2->atomslxn != NULL)
    {
        algo1->atomslxn = (char *) malloc((strlen(algo2->atomslxn) + 1) * sizeof(char));
        strcpy(algo1->atomslxn, algo2->atomslxn);
    }
}


void
StatisticsCopy(Statistics *stats1, const Statistics *stats2)
{
    memcpy(stats1, stats2, sizeof(Statistics));
}


void
CdsCopyAll(Cds *cds1, const Cds *cds2)
{
    int             i;

    cds1->vlen = cds2->vlen;
    cds1->aalen = cds2->aalen;
    cds1->model  = cds2->model;

    strncpy(cds1->filename, cds2->filename, FILENAME_MAX - 1);

    memcpy(cds1->resName_space, cds2->resName_space, cds2->vlen * 4 * sizeof(char));
    memcpy(cds1->chainID, cds2->chainID, cds2->vlen * sizeof(char));
    memcpy(cds1->resSeq, cds2->resSeq, cds2->vlen * sizeof(int));
    memcpy(cds1->x, cds2->x, cds2->vlen * sizeof(double));
    memcpy(cds1->y, cds2->y, cds2->vlen * sizeof(double));
    memcpy(cds1->z, cds2->z, cds2->vlen * sizeof(double));
    memcpy(cds1->o, cds2->o, cds2->vlen * sizeof(double));
    memcpy(cds1->b, cds2->b, cds2->vlen * sizeof(double));
    memcpy(cds1->residual_x, cds2->residual_x, cds2->vlen * sizeof(double));
    memcpy(cds1->residual_y, cds2->residual_y, cds2->vlen * sizeof(double));
    memcpy(cds1->residual_z, cds2->residual_z, cds2->vlen * sizeof(double));

    MatCpySym(cds1->matrix, (const double **) cds2->matrix, 3);

    memcpy(cds1->center, cds2->center, 3 * sizeof(double));
    memcpy(cds1->last_center, cds2->last_center, 3 * sizeof(double));
    memcpy(cds1->translation, cds2->translation, 3 * sizeof(double));

    cds1->RMSD_from_mean  = cds2->RMSD_from_mean;
    cds1->wRMSD_from_mean = cds2->wRMSD_from_mean;

    for (i = 0; i < 2; ++i)
        cds1->evals[i]  = cds2->evals[i];
}


void
PDBCdsCopyAll(PDBCds *cds1, const PDBCds *cds2)
{
    cds1->vlen = cds2->vlen;
    cds1->model  = cds2->model;

    strncpy(cds1->filename, cds2->filename, FILENAME_MAX-1);

    memcpy(cds1->record_space, cds2->record_space, cds2->vlen * 8 * sizeof(char));
    memcpy(cds1->name_space, cds2->name_space, cds2->vlen * 4 * sizeof(char));
    memcpy(cds1->resName_space, cds2->resName_space, cds2->vlen * 4 * sizeof(char));
    memcpy(cds1->segID_space, cds2->segID_space, cds2->vlen * 8 * sizeof(char));
    memcpy(cds1->element_space, cds2->element_space, cds2->vlen * 4 * sizeof(char));
    memcpy(cds1->charge_space, cds2->charge_space, cds2->vlen * 4 * sizeof(char));

    memcpy(cds1->serial, cds2->serial, cds2->vlen * sizeof(int));
    memcpy(cds1->Hnum, cds2->Hnum, cds2->vlen * sizeof(char));
    memcpy(cds1->altLoc, cds2->altLoc, cds2->vlen * sizeof(char));
    memcpy(cds1->xchainID, cds2->xchainID, cds2->vlen * sizeof(char));
    memcpy(cds1->chainID, cds2->chainID, cds2->vlen * sizeof(char));
    memcpy(cds1->resSeq, cds2->resSeq, cds2->vlen * sizeof(int));
    memcpy(cds1->iCode, cds2->iCode, cds2->vlen * sizeof(char));
    memcpy(cds1->x, cds2->x, cds2->vlen * sizeof(double));
    memcpy(cds1->y, cds2->y, cds2->vlen * sizeof(double));
    memcpy(cds1->z, cds2->z, cds2->vlen * sizeof(double));
    memcpy(cds1->occupancy, cds2->occupancy, cds2->vlen * sizeof(double));
    memcpy(cds1->tempFactor, cds2->tempFactor, cds2->vlen * sizeof(double));

    MatCpySym(cds1->matrix, (const double **) cds2->matrix, 3);
    memcpy(cds1->translation, cds2->translation, 3 * sizeof(double));
}


void
CdsAdd(Cds *cds1, const Cds *cds2)
{
    int             i;

    cds1->vlen = cds2->vlen;

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

    cds1->RMSD_from_mean  += cds2->RMSD_from_mean;
    cds1->wRMSD_from_mean += cds2->wRMSD_from_mean;
}


void
RotMatAddIp(double **mat1, const double **mat2)
{
    int             i, j;

    for (i = 0; i < 3; ++i)
        for (j = 0; j < 3; ++j)
            mat1[i][j] += mat2[i][j];
}


void
FillSegIDWithResSeq(PDBCds *cds_to, const Cds *cds_from)
{
    int             i, rn;
    char            resnum[5] ;

    for (i = 0; i < cds_to->vlen; ++i)
    {
         rn = cds_from->resSeq[i];

         if (rn != 0)
         {
             sprintf(resnum, "%04d", rn);
             strncpy(cds_to->segID[i], resnum, 4);
         }
    }
}


/* copies the info for a Cds struct to a PDBCds struct,
   for the most part */
void
CopyCds2PDB(PDBCds *pdbcds, const Cds *cds)
{
    int             i;

    pdbcds->vlen = cds->vlen;
    pdbcds->model = cds->model;
    strncpy(pdbcds->filename, cds->filename, FILENAME_MAX-1);

    MatCpySym(pdbcds->matrix, (const double **) cds->matrix, 3);

    for (i = 0; i < 3; ++i)
        pdbcds->translation[i]  = cds->translation[i];

    for (i = 0; i < cds->vlen; ++i)
    {
        strcpy(pdbcds->record[i], "ATOM");
        pdbcds->serial[i]     = i+1;
        pdbcds->Hnum[i]       = ' ';

        if (strncmp(cds->resName[i], "ADE", 3) == 0 ||
            strncmp(cds->resName[i], "CYT", 3) == 0 ||
            strncmp(cds->resName[i], "GUA", 3) == 0 ||
            strncmp(cds->resName[i], "THY", 3) == 0 ||
            strncmp(cds->resName[i], "URA", 3) == 0 ||
            strncmp(cds->resName[i], "  A", 3) == 0 ||
            strncmp(cds->resName[i], "  C", 3) == 0 ||
            strncmp(cds->resName[i], "  G", 3) == 0 ||
            strncmp(cds->resName[i], "  T", 3) == 0 ||
            strncmp(cds->resName[i], " DA", 3) == 0 || /* remediated PDB residue names */
            strncmp(cds->resName[i], " DC", 3) == 0 || /* remediated PDB residue names */
            strncmp(cds->resName[i], " DG", 3) == 0 || /* remediated PDB residue names */
            strncmp(cds->resName[i], " DT", 3) == 0 || /* remediated PDB residue names */
            strncmp(cds->resName[i], " DI", 3) == 0 || /* remediated PDB residue names */
            strncmp(cds->resName[i], " DU", 3) == 0 || /* remediated PDB residue names */
            strncmp(cds->resName[i], "  U", 3) == 0)
            strcpy(pdbcds->name[i], "P  ");
        else
            strcpy(pdbcds->name[i], "CA ");

        pdbcds->altLoc[i]     = ' ';
        strncpy(pdbcds->resName[i], cds->resName[i], 3);
        pdbcds->xchainID[i]   = ' ';
        pdbcds->chainID[i]    = cds->chainID[i];
        pdbcds->resSeq[i]     = cds->resSeq[i];
        pdbcds->iCode[i]      = ' ';
        pdbcds->x[i]          = cds->x[i];
        pdbcds->y[i]          = cds->y[i];
        pdbcds->z[i]          = cds->z[i];
        pdbcds->occupancy[i]  = cds->o[i];
        pdbcds->tempFactor[i] = cds->b[i];
        strncpy(pdbcds->segID[i], "    ", 4);
        strncpy(pdbcds->element[i], "  ", 2);
        strncpy(pdbcds->charge[i], "  ", 2);
    }
}


int
NMRCheckPDBCdsArray(PDBCdsArray *pdbA)
{
    int             i;
    int             vlen = pdbA->cds[0]->vlen;

    for(i = 1; i < pdbA->cnum; ++i)
    {
        if (pdbA->cds[i]->vlen != vlen)
        {
            fprintf(stderr,
                    "\n  WARNING20: PDB coordinates %d and %d are of unequal length [%d vs %d]. \n\n",
                    0, i, vlen, pdbA->cds[i]->vlen);
            //PrintTheseusTag();
            //exit(EXIT_FAILURE);
        }
    }

    return(vlen);
}


int
NMRCheckCdsArray(CdsArray *pdbA)
{
    int             i;
    int             vlen = pdbA->cds[0]->vlen;

    for(i = 1; i < pdbA->cnum; ++i)
    {
        if (pdbA->cds[i]->vlen != vlen)
        {
            fprintf(stderr,
                    "\n  WARNING20: PDB coordinates %d and %d are of unequal length [%d vs %d]. \n\n",
                    0, i, vlen, pdbA->cds[i]->vlen);
            //PrintTheseusTag();
            //exit(EXIT_FAILURE);
        }
    }

    return(vlen);
}


/* matrix and translation need to be in PDBCds structure */
void
TransformPDBCdsIp(PDBCds *pdbcds)
{
    int             i;
    double          xt, yt, zt;
    double         *x = pdbcds->x, *y = pdbcds->y, *z = pdbcds->z;
    const double    transx = pdbcds->translation[0];
    const double    transy = pdbcds->translation[1];
    const double    transz = pdbcds->translation[2];
    const double  **rmat = (const double **) pdbcds->matrix;
    const double    rmat00 = rmat[0][0], rmat01 = rmat[0][1], rmat02 = rmat[0][2],
                    rmat10 = rmat[1][0], rmat11 = rmat[1][1], rmat12 = rmat[1][2],
                    rmat20 = rmat[2][0], rmat21 = rmat[2][1], rmat22 = rmat[2][2];

    for (i = 0; i < pdbcds->vlen; ++i)
    {
/*         MatPrint(pdbcds->matrix, 3); */
/*             fprintf(stderr, */
/*                     " translation =    %8.3f %8.3f %8.3f \n", */
/*                     pdbcds->translation[0], */
/*                     pdbcds->translation[1], */
/*                     pdbcds->translation[2]); */
/*             fprintf(stderr, */
/*                     " before:         x = %8.3f  y = %8.3f  z = %8.3f \n", */
/*                     pdbcds->x[i], pdbcds->y[i], pdbcds->z[i]); */

        xt = x[i] - transx;
        yt = y[i] - transy;
        zt = z[i] - transz;

        x[i] = (xt * rmat00) + (yt * rmat10) + (zt * rmat20);
        y[i] = (xt * rmat01) + (yt * rmat11) + (zt * rmat21);
        z[i] = (xt * rmat02) + (yt * rmat12) + (zt * rmat22);

/*             fprintf(stderr, */
/*                     " after rotation: x = %8.3f  y = %8.3f  z = %8.3f \n", */
/*                     pdbcds->x[i], pdbcds->y[i], pdbcds->z[i]); */
/*                     exit(0); */
    }
}


void
RotateCdsOp(const Cds *cds1, const double **rmat, Cds *cds2)
{
    int             i;
    double          xt, yt, zt;
    double         *x1 = cds1->x, *y1 = cds1->y, *z1 = cds1->z,
                   *x2 = cds2->x, *y2 = cds2->y, *z2 = cds2->z;
    const double    rmat00 = rmat[0][0], rmat01 = rmat[0][1], rmat02 = rmat[0][2],
                    rmat10 = rmat[1][0], rmat11 = rmat[1][1], rmat12 = rmat[1][2],
                    rmat20 = rmat[2][0], rmat21 = rmat[2][1], rmat22 = rmat[2][2];

    for (i = 0; i < cds1->vlen; ++i)
    {
        xt = x1[i];
        yt = y1[i];
        zt = z1[i];

        x2[i] = xt * rmat00 + yt * rmat10 + zt * rmat20;
        y2[i] = xt * rmat01 + yt * rmat11 + zt * rmat21;
        z2[i] = xt * rmat02 + yt * rmat12 + zt * rmat22;
    }
}


void
RotateCdsIp(Cds *cds, const double **rmat)
{
    int             i;
    double          xt, yt, zt;
    double         *x = cds->x, *y = cds->y, *z = cds->z;
    const double    rmat00 = rmat[0][0], rmat01 = rmat[0][1], rmat02 = rmat[0][2],
                    rmat10 = rmat[1][0], rmat11 = rmat[1][1], rmat12 = rmat[1][2],
                    rmat20 = rmat[2][0], rmat21 = rmat[2][1], rmat22 = rmat[2][2];

    for (i = 0; i < cds->vlen; ++i)
    {
        xt = x[i];
        yt = y[i];
        zt = z[i];

        x[i] = (xt * rmat00) + (yt * rmat10) + (zt * rmat20);
        y[i] = (xt * rmat01) + (yt * rmat11) + (zt * rmat21);
        z[i] = (xt * rmat02) + (yt * rmat12) + (zt * rmat22);
    }
}


void
RotateCdsArrayIp(CdsArray *cdsA, const double **rmat)
{
    int             i;

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

    RotateCdsIp(cdsA->avecds, rmat);
}


void
TransformCdsIp(Cds *cds)
{
    const double  **rmat = (const double **) cds->matrix;
    const double    transx = cds->center[0];
    const double    transy = cds->center[1];
    const double    transz = cds->center[2];
    int             i;
    double          xt, yt, zt;
    double         *x = cds->x, *y = cds->y, *z = cds->z;
    const double    rmat00 = rmat[0][0], rmat01 = rmat[0][1], rmat02 = rmat[0][2],
                    rmat10 = rmat[1][0], rmat11 = rmat[1][1], rmat12 = rmat[1][2],
                    rmat20 = rmat[2][0], rmat21 = rmat[2][1], rmat22 = rmat[2][2];

    for (i = 0; i < cds->vlen; ++i)
    {
        xt = x[i] + transx;
        yt = y[i] + transy;
        zt = z[i] + transz;

        x[i] = (xt * rmat00) + (yt * rmat10) + (zt * rmat20);
        y[i] = (xt * rmat01) + (yt * rmat11) + (zt * rmat21);
        z[i] = (xt * rmat02) + (yt * rmat12) + (zt * rmat22);
    }
}


void
ScaleCds(Cds *cds, const double scale)
{
    int             i;
    double         *x = cds->x, *y = cds->y, *z = cds->z;

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


double
NormalizeWeightsOcc(double *w, double *o, int vlen)
{
    int             i;
    double          weightsum;
    double          normalize;

    weightsum = 0.0;
    for (i = 0; i < vlen; ++i)
        weightsum += o[i] * w[i];

    normalize = vlen / weightsum;

    for (i = 0; i < vlen; ++i)
        w[i] *= normalize;

    return(normalize);
}


double
NormalizeWeights(double *w, int vlen)
{
    int             i;
    double          weightsum;
    double          normalize;

/* normalize by trace of weight matrix */
    weightsum = 0.0;
    for (i = 0; i < vlen; ++i)
        weightsum += w[i];

    /* printf("\nweightsum: %f ", weightsum); */

    normalize = vlen / weightsum;
/* printf("normalize: %f", normalize); */
    for (i = 0; i < vlen; ++i)
        w[i] *= normalize;

    return(normalize);

/* normalize by determinant of weight matrix */
/*     weightsum = 1.0; */
/*     for (i = 0; i < cds->vlen; ++i) */
/*         weightsum *= w[i]; */
/*  */
/*     normalize = pow(weightsum, -1.0 / cds->vlen); */
/*  */
/*     for (i = 0; i < cds->vlen; ++i) */
/*         w[i] *= normalize; */

/* normalize by trace of covariance matrix */
/*     weightsum = 0.0; */
/*     for (i = 0; i < cds->vlen; ++i) */
/*         weightsum += vars[i]; */
/*  */
/*     normalize = cds->vlen / weightsum; */
/*  */
/*     for (i = 0; i < cds->vlen; ++i) */
/*         w[i] *= normalize; */

/* normalize by determinant of covariance matrix */
/*     weightsum = 1.0; */
/*     for (i = 0; i < cds->vlen; ++i) */
/*         weightsum *= vars[i]; */
/*  */
/*     normalize = pow(weightsum, 1.0 / cds->vlen); */
/*  */
/*     for (i = 0; i < cds->vlen; ++i) */
/*         w[i] *= normalize; */
}


void
CenMass(Cds *cds)
{
    int             i;
    const int       vlen = cds->vlen;
    double          xsum, ysum, zsum;
    const double   *x = (const double *) cds->x,
                   *y = (const double *) cds->y,
                   *z = (const double *) cds->z;

    xsum = ysum = zsum = 0.0;
    for (i = 0; i < vlen; ++i)
    {
        xsum += x[i];
        ysum += y[i];
        zsum += z[i];
    }

    cds->center[0] = xsum / vlen;
    cds->center[1] = ysum / vlen;
    cds->center[2] = zsum / vlen;
}


void
CenMassWtOp(Cds *cds, const CdsArray *weights)
{
    int             i;
    double          tempx, tempy, tempz;
    const double   *wts = (const double *) weights->w;
    double          wti, wtsum;
    const double   *x = (const double *) cds->x,
                   *y = (const double *) cds->y,
                   *z = (const double *) cds->z;

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

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

/*     printf("\n********** %f %f %f", cds->center[0], cds->center[1], cds->center[2]); */
/*     fflush(NULL); */
}


void
CenMassWtIp(Cds *cds, const double *wts)
{
    int             i;
    double          tempx, tempy, tempz;
    double          wti, wtsum;
    const double   *x = (const double *) cds->x,
                   *y = (const double *) cds->y,
                   *z = (const double *) cds->z;

    tempx = tempy = tempz = wtsum = 0.0;
    for (i = 0; i < cds->vlen; ++i)
    {
        wti = wts[i];
//        printf("wt[%3d] = %8.3f\n", i, wti);

        wtsum += wti;
        tempx += (wti * x[i]);
        tempy += (wti * y[i]);
        tempz += (wti * z[i]);
    }

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

/*     printf("wtsum = %8.3f\n", wtsum); */
/*     printf("wtocc = %8.3f\n", wtocc); */
/*     fflush(NULL); */

/*      printf("\nDT: % 8.3f % 8.3f % 8.3f\n",  */
/*            cds->center[0], cds->center[1], cds->center[2]);  */
/*      fflush(NULL);  */
}


void
CenMassCovOp(Cds *cds, const CdsArray *weights)
{
    double          tempx, tempy, tempz;
    int             i, j;
    double         *covx = cds->covx,
                   *covy = cds->covy,
                   *covz = cds->covz;
    double          wtsum;

/* #include "internmat.h" */
/* for (i = 0; i < cds->vlen; ++i) */
/*     for (j = 0; j < cds->vlen; ++j) */
/*         weights->CovMat[i][j] = internmat[i][j]; */
/*  */
/* CovInvWeightLAPACK((CdsArray *) weights); */
/* NormalizeCovMat(weights->WtMat, cds->vlen); */

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

    tempx = tempy = tempz = 0.0;
    for (i = 0; i < cds->vlen; ++i)
    {
        tempx += covx[i];
        tempy += covy[i];
        tempz += covz[i];
    }

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

/*     printf("\n% f % f % f", */
/*            cds->center[0], cds->center[1], cds->center[2]); */
/*     fflush(NULL); */
}


void
CenMassCov(Cds *cds, const double **wtmat)
{
    double          tempx, tempy, tempz;
    int             i, j;
    double         *covx = cds->covx,
                   *covy = cds->covy,
                   *covz = cds->covz;
    double          wtsum;

    CalcCovCds(cds, wtmat);

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

    tempx = tempy = tempz = 0.0;
    for (i = 0; i < cds->vlen; ++i)
    {
        tempx += covx[i];
        tempy += covy[i];
        tempz += covz[i];
    }

    cds->center[0] = tempx / wtsum;
    cds->center[1] = tempy / wtsum;
    cds->center[2] = tempz / wtsum;
}


/*void*/
/*CenMassCovOcc(Cds *cds, const double **wtmat)*/
/*{*/
/*    double          tempx, tempy, tempz;*/
/*    int             i, j;*/
/*    const double   *occ = (const double *) cds->o;*/
/*    double         *covx = cds->covx,*/
/*                   *covy = cds->covy,*/
/*                   *covz = cds->covz;*/
/*    double          wtsum;*/
/**/
/*    wtsum = 0.0;*/
/*    for (i = 0; i < cds->vlen; ++i)*/
/*        for (j = 0; j < cds->vlen; ++j)*/
/*            wtsum += occ[i] * occ[j] * wtmat[i][j];*/
/**/
/*    tempx = tempy = tempz = 0.0;*/
/*    for (i = 0; i < cds->vlen; ++i)*/
/*    {*/
/*        tempx += occ[i] * covx[i];*/
/*        tempy += occ[i] * covy[i];*/
/*        tempz += occ[i] * covz[i];*/
/*    }*/
/**/
/*    cds->center[0] = tempx / wtsum;*/
/*    cds->center[1] = tempy / wtsum;*/
/*    cds->center[2] = tempz / wtsum;*/
/*}*/
/**/
/**/
/*void*/
/*CenMassWtOpOcc(Cds *cds, const CdsArray *weights)*/
/*{*/
/*    int             i;*/
/*    double          tempx, tempy, tempz;*/
/*    const double   *wts = (const double *) weights->w;*/
/*    const double   *occ = (const double *) cds->o;*/
/*    double          wti, wtsum;*/
/*    const double   *x = (const double *) cds->x,*/
/*                   *y = (const double *) cds->y,*/
/*                   *z = (const double *) cds->z;*/
/**/
/*    tempx = tempy = tempz = wtsum = 0.0;*/
/*    for (i = 0; i < cds->vlen; ++i)*/
/*    {*/
/*        wti = wts[i] * occ[i];*/
/*        wtsum += wti;*/
/*        tempx += (wti * x[i]);*/
/*        tempy += (wti * y[i]);*/
/*        tempz += (wti * z[i]);*/
/*    }*/
/**/
/*    cds->center[0] = tempx / wtsum;*/
/*    cds->center[1] = tempy / wtsum;*/
/*    cds->center[2] = tempz / wtsum;*/
/*}*/
/**/
/**/
/*void*/
/*CenMassOcc(Cds *cds)*/
/*{*/
/*    int             i;*/
/*    double          tempx, tempy, tempz, occi, occsum;*/
/*    const double   *occ = (const double *) cds->o;*/
/*    const double   *x = (const double *) cds->x,*/
/*                   *y = (const double *) cds->y,*/
/*                   *z = (const double *) cds->z;*/
/**/
/*    tempx = tempy = tempz = occsum = 0.0;*/
/*    for (i = 0; i < cds->vlen; ++i)*/
/*    {*/
/*        occi = occ[i];*/
/*        occsum += occi;*/
/*        tempx += (occi * x[i]);*/
/*        tempy += (occi * y[i]);*/
/*        tempz += (occi * z[i]);*/
/*    }*/
/**/
/*    cds->center[0] = tempx / occsum;*/
/*    cds->center[1] = tempy / occsum;*/
/*    cds->center[2] = tempz / occsum;*/
/*}*/


void
CenMassOccVec(Cds *cds, double *cenmass)
{
    int             i;
    double          tempx, tempy, tempz, occi, occsum;
    const double   *occ = (const double *) cds->o;
    const double   *x = (const double *) cds->x,
                   *y = (const double *) cds->y,
                   *z = (const double *) cds->z;

    tempx = tempy = tempz = occsum = 0.0;
    for (i = 0; i < cds->vlen; ++i)
    {
        occi = occ[i];
        occsum += occi;
        tempx += (occi * x[i]);
        tempy += (occi * y[i]);
        tempz += (occi * z[i]);
    }

    cenmass[0] = tempx / occsum;
    cenmass[1] = tempy / occsum;
    cenmass[2] = tempz / occsum;
}


void
CenMassWtIpOcc(Cds *cds, const double *wts)
{
    int             i;
    double          tempx, tempy, tempz;
    const double   *occ = (const double *) cds->o;
    double          wti, wtsum;
    const double   *x = (const double *) cds->x,
                   *y = (const double *) cds->y,
                   *z = (const double *) cds->z;

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

    cds->center[0] = tempx / wtsum;
    cds->center[1] = tempy / wtsum;
    cds->center[2] = tempz / wtsum;
//    printf("wtsum = %8.3f\n", wtsum);
//    fflush();

/*     printf("\n% 8.3f % 8.3f % 8.3f",  */
/*            cds->center[0], cds->center[1], cds->center[2]);  */
/*     fflush(NULL); */ 
}




/*                 cdsi->x[j] = avex[j]*rmat00 + avey[j]*rmat01 + avez[j]*rmat02; */
/*                 cdsi->y[j] = avex[j]*rmat10 + avey[j]*rmat11 + avez[j]*rmat12; */
/*                 cdsi->z[j] = avex[j]*rmat20 + avey[j]*rmat21 + avez[j]*rmat22; */

void
CenMassWtIpEM(Cds *cds, const Cds *avecds, const double *wts)
{
    int             i;
    double          tempx, tempy, tempz;
    const double   *occ = (const double *) cds->o;
    double          wti, wtsum;
    const double   *x = (const double *) cds->x,
                   *y = (const double *) cds->y,
                   *z = (const double *) cds->z;
    double          rmat00, rmat01, rmat02,
                    rmat10, rmat11, rmat12,
                    rmat20, rmat21, rmat22;
    double        **rmat = cds->matrix;
    double         *avex = avecds->x,
                   *avey = avecds->y,
                   *avez = avecds->z;

    rmat00 = rmat[0][0], rmat01 = rmat[0][1], rmat02 = rmat[0][2],
    rmat10 = rmat[1][0], rmat11 = rmat[1][1], rmat12 = rmat[1][2],
    rmat20 = rmat[2][0], rmat21 = rmat[2][1], rmat22 = rmat[2][2];

    tempx = tempy = tempz = wtsum = 0.0;
    for (i = 0; i < cds->vlen; ++i)
    {
        wti = wts[i];
//        printf("wt[%3d] = %8.3f\n", i, wti);
        
        if (occ[i] == 1.0)
        {
            //wtsum += 1.0;
            wtsum += wti;
            tempx += (wti * x[i]);
            tempy += (wti * y[i]);
            tempz += (wti * z[i]);
        }
        else if (occ[i] == 0.0)
        {
            tempx += (wti * (avex[i]*rmat00 + avey[i]*rmat01 + avez[i]*rmat02));
            tempy += (wti * (avex[i]*rmat10 + avey[i]*rmat11 + avez[i]*rmat12));
            tempz += (wti * (avex[i]*rmat20 + avey[i]*rmat21 + avez[i]*rmat22));

/*             tempx += (wti * (avex[i]*rmat00 + avey[i]*rmat10 + avez[i]*rmat20)); */
/*             tempy += (wti * (avex[i]*rmat01 + avey[i]*rmat11 + avez[i]*rmat21)); */
/*             tempz += (wti * (avex[i]*rmat02 + avey[i]*rmat12 + avez[i]*rmat22)); */
        }
    }

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

/*     printf("wtsum = %8.3f\n", wtsum); */
/*     fflush(NULL); */

/*     printf("\nEM: % 8.3f % 8.3f % 8.3f",  */
/*            cds->center[0], cds->center[1], cds->center[2]);  */
/*     fflush(NULL);  */
}


/*void*/
/*CenMassCovOpOcc(Cds *cds, const CdsArray *weights)*/
/*{*/
/*    double          tempx, tempy, tempz;*/
/*    int             i, j;*/
/*    const double   *occ = (const double *) cds->o;*/
/*    double         *covx = cds->covx,*/
/*                   *covy = cds->covy,*/
/*                   *covz = cds->covz;*/
/*    double          wtsum;*/
/**/
/*    wtsum = 0.0;*/
/*    for (i = 0; i < cds->vlen; ++i)*/
/*        for (j = 0; j < cds->vlen; ++j)*/
/*            wtsum += occ[i] * occ[j] * weights->WtMat[i][j];*/
/**/
/*    tempx = tempy = tempz = 0.0;*/
/*    for (i = 0; i < cds->vlen; ++i)*/
/*    {*/
/*        tempx += occ[i] * covx[i];*/
/*        tempy += occ[i] * covy[i];*/
/*        tempz += occ[i] * covz[i];*/
/*    }*/
/**/
/*    cds->center[0] = tempx / wtsum;*/
/*    cds->center[1] = tempy / wtsum;*/
/*    cds->center[2] = tempz / wtsum;*/
/*}*/


void
ApplyCenter(Cds *cds, const double cenx, const double ceny, const double cenz)
{
    int             i;
    double         *x = cds->x, *y = cds->y, *z = cds->z;

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


void
ApplyCenterIp(Cds *cds)
{
    int             i;
    double         *x = cds->x, *y = cds->y, *z = cds->z;
    const double    cenx = cds->center[0],
                    ceny = cds->center[1],
                    cenz = cds->center[2];

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


void
ApplyNegCenterIp(Cds *cds)
{
    int             i;
    double         *x = cds->x, *y = cds->y, *z = cds->z;
    const double    cenx = cds->center[0],
                    ceny = cds->center[1],
                    cenz = cds->center[2];

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


/* apply the center from 'cds_base' to 'cds' */
void
ApplyCenterOp(Cds *cds1, const Cds *cds2)
{
    int             i;
    double         *x1 = cds1->x, *y1 = cds1->y, *z1 = cds1->z;
    const double   *x2 = cds2->x, *y2 = cds2->y, *z2 = cds2->z;
    const double    cenx = cds2->center[0],
                    ceny = cds2->center[1],
                    cenz = cds2->center[2];

    for (i = 0; i < cds1->vlen; ++i)
    {
        x1[i] = x2[i] - cenx;
        y1[i] = y2[i] - ceny;
        z1[i] = z2[i] - cenz;
    }
}


void
TransCdsIp(Cds *cds, const double *trans)
{
    int             i;
    double         *x = cds->x, *y = cds->y, *z = cds->z;
    const double    transx = trans[0],
                    transy = trans[1],
                    transz = trans[2];

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


void
NegTransCdsIp(Cds *cds, const double *trans)
{
    int             i;
    double         *x = cds->x, *y = cds->y, *z = cds->z;
    const double    transx = trans[0],
                    transy = trans[1],
                    transz = trans[2];

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


/* Copy average cds to atoms with 0 occupancy, part of the EM algorithm's
   E-step for estimating missing data */
void
EM_MissingCds(CdsArray *cdsA)
{
    int             i, j;
    double         *avex = cdsA->avecds->x, *avey = cdsA->avecds->y,
                   *avez = cdsA->avecds->z;
    const Cds  **cds = (const Cds **) cdsA->cds;
    Cds         *cdsi;

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

        for (j = 0; j < cdsA->vlen; ++j)
        {
            if (cdsi->o[j] == 0.0)
            {
                cdsi->x[j] = avex[j];
                cdsi->y[j] = avey[j];
                cdsi->z[j] = avez[j];
            }
        }
    }
}


void
EM_MissingCdsOp(CdsArray *baseA, CdsArray *scratchA)
{
    int             i, j;
    double        **rmat = NULL;
    double         *avex = scratchA->avecds->x,
                   *avey = scratchA->avecds->y,
                   *avez = scratchA->avecds->z;
    const Cds  **cds = (const Cds **) baseA->cds;
    Cds         *cdsi;

    double          rmat00, rmat01, rmat02,
                    rmat10, rmat11, rmat12,
                    rmat20, rmat21, rmat22;

    for (i = 0; i < baseA->cnum; ++i)
    {
        cdsi = (Cds *) cds[i];

        rmat = scratchA->cds[i]->matrix;

        rmat00 = rmat[0][0], rmat01 = rmat[0][1], rmat02 = rmat[0][2],
        rmat10 = rmat[1][0], rmat11 = rmat[1][1], rmat12 = rmat[1][2],
        rmat20 = rmat[2][0], rmat21 = rmat[2][1], rmat22 = rmat[2][2];

        for (j = 0; j < baseA->vlen; ++j)
        {
            if (cdsi->o[j] == 0.0)
            {
                cdsi->x[j] = avex[j]*rmat00 + avey[j]*rmat01 + avez[j]*rmat02;
                cdsi->y[j] = avex[j]*rmat10 + avey[j]*rmat11 + avez[j]*rmat12;
                cdsi->z[j] = avex[j]*rmat20 + avey[j]*rmat21 + avez[j]*rmat22;

                cdsi->x[j] -= scratchA->cds[i]->translation[0];
                cdsi->y[j] -= scratchA->cds[i]->translation[1];
                cdsi->z[j] -= scratchA->cds[i]->translation[2];
            }
        }
    }
}


void
AveCdsNovec(CdsArray *cdsA)
{
    int             i, j;
    double          xtmp, ytmp, ztmp, /* otmp,  */btmp;
    double         *avex = cdsA->avecds->x,
                   *avey = cdsA->avecds->y,
                   *avez = cdsA->avecds->z,
                   *aveo = cdsA->avecds->o,
                   *aveb = cdsA->avecds->b;
    const int       cnum = cdsA->cnum;
    const Cds  **cds = (const Cds **) cdsA->cds;
    Cds         *cdsj;
    double          invcnum = 1.0 / (double) cnum;

    for (i = 0; i < cdsA->vlen; ++i)
    {
        xtmp = ytmp = ztmp = /* otmp = */ btmp = 0.0;
        for (j = 0; j < cnum; ++j)
        {
            cdsj = (Cds *) cds[j];
            xtmp += cdsj->x[i];
            ytmp += cdsj->y[i];
            ztmp += cdsj->z[i];
            /* otmp += cdsj->o[i]; */
            btmp += cdsj->b[i];
        }

        avex[i] = xtmp * invcnum;
        avey[i] = ytmp * invcnum;
        avez[i] = ztmp * invcnum;
        aveo[i] = 1.0;
        aveb[i] = btmp * invcnum;
    }
}


/* Calculate the ML estimate of a hierarchical mean, where the atoms
   are normally distributed with hyper-mean zero */
/* See also below, which is probably more valid (only the weighted mean has zero centroid) */
/* 2009-06-11 */
double
HierAveCds2(CdsArray *cdsA)
{
    int             i, j;
    double         *avex = cdsA->avecds->x,
                   *avey = cdsA->avecds->y,
                   *avez = cdsA->avecds->z,
                   *aveo = cdsA->avecds->o,
                   *aveb = cdsA->avecds->b;
    const int       cnum = cdsA->cnum, vlen = cdsA->vlen;
    const Cds  **cds = (const Cds **) cdsA->cds;
    Cds         *cdsj;
    double          invcnum, psi;


    psi = 0.0;
    for (i = 0; i < vlen; ++i)
        psi += (avex[i]*avex[i] + avey[i]*avey[i] + avez[i]*avez[i]);
    psi /= (3.0 * vlen);

    memset(avex, 0, vlen * sizeof(double));
    memset(avey, 0, vlen * sizeof(double));
    memset(avez, 0, vlen * sizeof(double));
    memset(aveb, 0, vlen * sizeof(double));

    for (j = 0; j < cnum; ++j)
    {
        cdsj = (Cds *) cds[j];

		for (i = 0; i < vlen; ++i)
		{
			avex[i] += cdsj->x[i];
			avey[i] += cdsj->y[i];
			avez[i] += cdsj->z[i];
			aveb[i] += cdsj->b[i];
		}
    }

    for (i = 0; i < vlen; ++i)
    {
        invcnum = 1.0 / ((double) cnum + cdsA->var[i] / psi);
        //printf("\ninvcnum = %e  %e", invcnum, 1.0/cnum);
    
		avex[i] *= invcnum;
		avey[i] *= invcnum;
		avez[i] *= invcnum;
		aveo[i] = 1.0;
		aveb[i] *= invcnum;
    }
    
    return(psi);
}


/* Calculate the ML estimate of a hierarchical mean, where the variance-weighted atoms 
   are normally distributed with hyper-mean zero */
/* 2009-06-11 */
double
HierAveCds(CdsArray *cdsA)
{
    int             i, j;
    double         *avex = cdsA->avecds->x,
                   *avey = cdsA->avecds->y,
                   *avez = cdsA->avecds->z,
                   *aveo = cdsA->avecds->o,
                   *aveb = cdsA->avecds->b;
    const int       cnum = cdsA->cnum, vlen = cdsA->vlen;
    const Cds  **cds = (const Cds **) cdsA->cds;
    Cds         *cdsj;
    double          invcnum, psi, norm;


    psi = norm = 0.0;
    for (i = 0; i < vlen; ++i)
    {
        norm += 1.0 / cdsA->var[i];
        psi += (avex[i]*avex[i]/cdsA->var[i] + 
                avey[i]*avey[i]/cdsA->var[i] + 
                avez[i]*avez[i]/cdsA->var[i]);
    }
    psi /= (3.0 * norm);

    memset(avex, 0, vlen * sizeof(double));
    memset(avey, 0, vlen * sizeof(double));
    memset(avez, 0, vlen * sizeof(double));
    memset(aveb, 0, vlen * sizeof(double));

    for (j = 0; j < cnum; ++j)
    {
        cdsj = (Cds *) cds[j];

		for (i = 0; i < vlen; ++i)
		{
			avex[i] += cdsj->x[i];
			avey[i] += cdsj->y[i];
			avez[i] += cdsj->z[i];
			aveb[i] += cdsj->b[i];
		}
    }

    for (i = 0; i < vlen; ++i)
    {
        invcnum = 1.0 / ((double) cnum + 1.0 / psi);
        //printf("\ninvcnum = %e  %e", invcnum, 1.0/cnum);
    
		avex[i] *= invcnum;
		avey[i] *= invcnum;
		avez[i] *= invcnum;
		aveo[i] = 1.0;
		aveb[i] *= invcnum;
    }
    
    return(psi);
}


void
AveCds(CdsArray *cdsA)
{
    int             i, j;
    double         *avex = cdsA->avecds->x,
                   *avey = cdsA->avecds->y,
                   *avez = cdsA->avecds->z,
                   *aveo = cdsA->avecds->o,
                   *aveb = cdsA->avecds->b;
    const int       cnum = cdsA->cnum, vlen = cdsA->vlen;
    const Cds  **cds = (const Cds **) cdsA->cds;
    Cds         *cdsj;
    double          invcnum = 1.0 / (double) cnum;

    memset(avex, 0, vlen * sizeof(double));
    memset(avey, 0, vlen * sizeof(double));
    memset(avez, 0, vlen * sizeof(double));
    memset(aveb, 0, vlen * sizeof(double));

    for (j = 0; j < cnum; ++j)
    {
        cdsj = (Cds *) cds[j];

		for (i = 0; i < vlen; ++i)
		{
			avex[i] += cdsj->x[i];
			avey[i] += cdsj->y[i];
			avez[i] += cdsj->z[i];
			aveb[i] += cdsj->b[i];
		}
    }

    for (i = 0; i < vlen; ++i)
    {
		avex[i] *= invcnum;
		avey[i] *= invcnum;
		avez[i] *= invcnum;
		aveo[i] = 1.0;
		aveb[i] *= invcnum;
    }
}

void
AveCdsTB(CdsArray *cdsA, int omit)
{
    int             i, j;
    double         *avex = cdsA->avecds->x,
                   *avey = cdsA->avecds->y,
                   *avez = cdsA->avecds->z,
                   *aveo = cdsA->avecds->o,
                   *aveb = cdsA->avecds->b;
    const int       cnum = cdsA->cnum, vlen = cdsA->vlen;
    const Cds  **cds = (const Cds **) cdsA->cds;
    Cds         *cdsj;
    double          invcnum = 1.0 / (double) (cnum-1);

    memset(avex, 0, vlen * sizeof(double));
    memset(avey, 0, vlen * sizeof(double));
    memset(avez, 0, vlen * sizeof(double));
    memset(aveb, 0, vlen * sizeof(double));

    for (j = 0; j < cnum; ++j)
    {
    
        if (j == omit)
            continue;

        cdsj = (Cds *) cds[j];

		for (i = 0; i < vlen; ++i)
		{
			avex[i] += cdsj->x[i];
			avey[i] += cdsj->y[i];
			avez[i] += cdsj->z[i];
			aveb[i] += cdsj->b[i];
		}
    }

    for (i = 0; i < vlen; ++i)
    {
		avex[i] *= invcnum;
		avey[i] *= invcnum;
		avez[i] *= invcnum;
		aveo[i] = 1.0;
		aveb[i] *= invcnum;
    }
}



static void
*AveCdsPth(void *avedata_ptr)
{
    AveData        *avedata = (AveData *) avedata_ptr;
    int             i, j;
    double          xtmp, ytmp, ztmp, /* otmp,  */btmp;
    double         *avex = avedata->cdsA->avecds->x,
                   *avey = avedata->cdsA->avecds->y,
                   *avez = avedata->cdsA->avecds->z,
                   *aveo = avedata->cdsA->avecds->o,
                   *aveb = avedata->cdsA->avecds->b;
    const int       cnum = avedata->cnum;
    double          invcnum = 1.0 / cnum;
    const Cds  **cds = (const Cds **) avedata->cdsA->cds;
    Cds         *cdsj;

    for (i = avedata->start; i < avedata->end; ++i)
    {
        xtmp = ytmp = ztmp = /* otmp = */ btmp = 0.0;
        for (j = 0; j < cnum; ++j)
        {
            cdsj = (Cds *) cds[j];
            xtmp += cdsj->x[i];
            ytmp += cdsj->y[i];
            ztmp += cdsj->z[i];
            /* otmp += cdsj->o[i]; */
            btmp += cdsj->b[i];
        }

        avex[i] = xtmp * invcnum;
        avey[i] = ytmp * invcnum;
        avez[i] = ztmp * invcnum;
        aveo[i] = 1.0;
        aveb[i] = btmp * invcnum;
    }

    pthread_exit((void *) 0);
}


void
AveCds_pth(CdsArray *cdsA, AveData **avedata, pthread_t *callThd,
           pthread_attr_t *attr, const int thrdnum)
{
    const int       cnum = cdsA->cnum, vlen = cdsA->vlen;
    int             i, rc = 0, incr;

    incr = vlen / thrdnum;

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

        rc = pthread_create(&callThd[i], attr, AveCdsPth, (void *) avedata[i]);

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

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

	rc = pthread_create(&callThd[thrdnum - 1], attr, AveCdsPth, (void *) avedata[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);
        }
    }

    return;
}


/* void */
/* AveCdsOcc(CdsArray *cdsA) */
/* { */
/*     int             i, j; */
/*     double          xtmp, ytmp, ztmp, btmp, occ; */
/*     double         *avex = cdsA->avecds->x, */
/*                    *avey = cdsA->avecds->y, */
/*                    *avez = cdsA->avecds->z, */
/*                    *aveo = cdsA->avecds->o, */
/*                    *aveb = cdsA->avecds->b; */
/*     const Cds  **cds = (const Cds **) cdsA->cds; */
/*     Cds         *cdsj; */
/*     const double    cnum = cdsA->cnum; */
/*     double          invdf, occsum; */
/*  */
/*     for (i = 0; i < cdsA->vlen; ++i) */
/*     { */
/*         xtmp = ytmp = ztmp = btmp = 0.0; */
/*         occsum = 0.0; */
/*         for (j = 0; j < cnum; ++j) */
/*         { */
/*             cdsj = (Cds *) cds[j]; */
/*             occ = cdsj->o[i]; */
/*             occsum += occ; */
/*             xtmp += occ * cdsj->x[i]; */
/*             ytmp += occ * cdsj->y[i]; */
/*             ztmp += occ * cdsj->z[i]; */
/*             btmp += occ * cdsj->b[i]; */
/*         } */
/*  */
/*         //invdf = 1.0 / cdsA->df[i]; */
/*         invdf = 1.0 / occsum; */
/*  */
/*         avex[i] = xtmp * invdf; */
/*         avey[i] = ytmp * invdf; */
/*         avez[i] = ztmp * invdf; */
/*         aveo[i] = 1.0; */
/*         aveb[i] = btmp * invdf; */
/*     } */
/* } */


void
AveCdsOcc(CdsArray *cdsA)
{
    int             i, j;
    double         *avex = cdsA->avecds->x,
                   *avey = cdsA->avecds->y,
                   *avez = cdsA->avecds->z,
                   *aveo = cdsA->avecds->o,
                   *aveb = cdsA->avecds->b;
    const int       cnum = cdsA->cnum, vlen = cdsA->vlen;
    const Cds  **cds = (const Cds **) cdsA->cds;
    Cds         *cdsj;
    double          occ, occsum, invocc;

    memset(avex, 0, vlen * sizeof(double));
    memset(avey, 0, vlen * sizeof(double));
    memset(avez, 0, vlen * sizeof(double));
    memset(aveb, 0, vlen * sizeof(double));

    for (i = 0; i < vlen; ++i)
    {
        occsum = 0.0;
        for (j = 0; j < cnum; ++j)
        {
            cdsj = (Cds *) cds[j];
            occ = cdsj->o[i];
            occsum += occ;
			avex[i] += occ * cdsj->x[i];
			avey[i] += occ * cdsj->y[i];
			avez[i] += occ * cdsj->z[i];
			aveb[i] += occ * cdsj->b[i];
        }

        invocc = 1.0 / occsum;

		avex[i] *= invocc;
		avey[i] *= invocc;
		avez[i] *= invocc;
        aveo[i] = 1.0;
		aveb[i] *= invocc;
    }
}


void
CalcCdsPrincAxes(Cds *cds, double **rotmat)
{
    double         *evals = (double *) malloc(3 * sizeof(double));
    double          det;
    int             i, j;

    CdsInnerProd2(cds);
    //eigensym((const double **) cds->innerprod2, evals, rotmat, 3);
    //Mat3TransposeIp(rotmat);
    //printf("\nCalcCdsPrincAxes B:");
    //Mat3Print(rotmat);
    //CdsInnerProd2(cds);
    jacobi3_cyc(cds->innerprod2, evals, rotmat, 1e-8);
    //Mat3Print(rotmat);
    //printf("\nCalcCdsPrincAxes A:");
    det = Mat3Det((const double **) rotmat);

    if (det < 0)
    {
        //printf("\nNEGATIVE DETERMINANT\n");
        for (i = 0; i < 3; ++i)
        {
            if (rotmat[i][i] < 0)
            {
                for (j = 0; j < 3; ++j)
                    rotmat[i][j] *= -1.0;
                    
                break;
            }
        }

        //Mat3Print(rotmat);
    }

/*     Mat3Print(rotmat); */
/*     printf("\n evals %f %f %f  det = %f %f", */
/*            evals[0], evals[1], evals[2], det, Mat3Det((const double **)rotmat)); */

    free(evals);
}


void
SumCdsTB(CdsArray *cdsA, const int exclude)
{
    int             i, j;
    double          xtmp, ytmp, ztmp, otmp, btmp;
    const Cds  **cds = (const Cds **) cdsA->cds;

    for (i = 0; i < cdsA->vlen; ++i)
    {
        xtmp = ytmp = ztmp = otmp = btmp = 0.0;
        for (j = 0; j < exclude; ++j)
        {
            xtmp += cds[j]->x[i];
            ytmp += cds[j]->y[i];
            ztmp += cds[j]->z[i];
            otmp += cds[j]->o[i];
            btmp += cds[j]->b[i];
        }

        for (j = exclude + 1; j < cdsA->cnum; ++j)
        {
            xtmp += cds[j]->x[i];
            ytmp += cds[j]->y[i];
            ztmp += cds[j]->z[i];
            otmp += cds[j]->o[i];
            btmp += cds[j]->b[i];
        }
    }
}


/* multiplies first quaternion by second, puts result in second */
/* when concatenating quats, you multiply the second rotation by the first,
   in that noncommunative order */
double
*ConcatQuatsIp(const double *quat1, double *quat2)
{
    double          ww, xx, yy, zz,
                    wx, xw, yz, zy,
                    wy, xz, yw, zx,
                    wz, xy, yx, zw;

    ww = quat1[0] * quat2[0];
    xx = quat1[1] * quat2[1];
    yy = quat1[2] * quat2[2];
    zz = quat1[3] * quat2[3];
    wx = quat1[0] * quat2[1];
    xw = quat1[1] * quat2[0];
    yz = quat1[2] * quat2[3];
    zy = quat1[3] * quat2[2];
    wy = quat1[0] * quat2[2];
    xz = quat1[1] * quat2[3];
    yw = quat1[2] * quat2[0];
    zx = quat1[3] * quat2[1];
    wz = quat1[0] * quat2[3];
    xy = quat1[1] * quat2[2];
    yx = quat1[2] * quat2[1];
    zw = quat1[3] * quat2[0];

    quat2[0] = ww - xx - yy - zz;
    quat2[1] = wx + xw + yz - zy;
    quat2[2] = wy - xz - yw - zx;
    quat2[3] = wz + xy - yx + zw;

    return(quat2);
}
