/*
    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 <stdlib.h>
#include <string.h>
#include <stdio.h>
#include <math.h>
#include <float.h>
#include "pdbUtils.h"
#include "pdbStats.h"
#include "CovMat.h"
#include "DLTmath.h"
#include "FragCds.h"
#include "ProcGSLSVD.h"


static double
CalcE0Cov(const Cds *cds1, const Cds *cds2);

static void
CalcR(const Cds *cds1, const Cds *cds2, double **Rmat,
      const double *weights);

static void
CalcRCov(const Cds *cds1, const Cds *cds2, double **Rmat, const double **WtMat);

static int
CalcGSLSVD(double **Rmat, double **Umat, double *sigma, double **VTmat);

static int
CalcRotMat(double **rotmat, double **Umat, double **VTmat);


static double
CalcMahFrobInnProd(const Cds *cds, const double *weights)
{
    int             i;
    double          sum;
    const double   *x = (const double *) cds->x,
                   *y = (const double *) cds->y,
                   *z = (const double *) cds->z;
    double          xi, yi, zi;


    sum = 0.0;
    i = cds->vlen;
    while(i-- > 0)
    {
        xi = *x++;
        yi = *y++;
        zi = *z++;

        sum += *weights++ * ((xi * xi) + (yi * yi) + (zi * zi));
    }

    return(sum);
}


static double
CalcInnProdNorm(const Cds *cds)
{
    int             i;
    double          sum;
    const double   *x = (const double *) cds->x,
                   *y = (const double *) cds->y,
                   *z = (const double *) cds->z;
    double          xi, yi, zi;

    sum = 0.0;
    i = cds->vlen;
    while(i-- > 0)
    {
        xi = *x++;
        yi = *y++;
        zi = *z++;

        sum += xi * xi + yi * yi + zi * zi;
    }

    return(sum);
}


static double
CalcInnProdNorm2(const double **cds, const int len)
{
    int             i;
    double          sum;
    const double   *x = (const double *) cds[0],
                   *y = (const double *) cds[1],
                   *z = (const double *) cds[2];
    double          xi, yi, zi;

    sum = 0.0;
    i = len;
    while(i-- > 0)
    {
        xi = *x++;
        yi = *y++;
        zi = *z++;

        sum += xi * xi + yi * yi + zi * zi;
    }

    return(sum);
}


static double
CalcMahFrobInnProdRot(const Cds *cds, const double **rmat, const double *weights)
{
    int             i;
    double          sum;
    const double   *x = (const double *) cds->x,
                   *y = (const double *) cds->y,
                   *z = (const double *) cds->z;
    double          xi, yi, zi, xir, yir, zir;
    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];


    sum = 0.0;
    i = cds->vlen;
    while(i-- > 0)
    {
        xi = *x++;
        yi = *y++;
        zi = *z++;

        xir = xi * rmat00 + yi * rmat10 + zi * rmat20;
        yir = xi * rmat01 + yi * rmat11 + zi * rmat21;
        zir = xi * rmat02 + yi * rmat12 + zi * rmat22;

        sum += *weights++ * ((xir * xir) + (yir * yir) + (zir * zir));
    }

    return(sum);
}


static double
CalcE0Cov(const Cds *cds1, const Cds *cds2)
{
    int             i;
    double          sum;
    const double   *x2 = (const double *) cds2->x,
                   *y2 = (const double *) cds2->y,
                   *z2 = (const double *) cds2->z;
    const double   *x1 = (const double *) cds1->x,
                   *y1 = (const double *) cds1->y,
                   *z1 = (const double *) cds1->z;
    const double   *cx2 = (const double *) cds2->covx,
                   *cy2 = (const double *) cds2->covy,
                   *cz2 = (const double *) cds2->covz;
    const double   *cx1 = (const double *) cds1->covx,
                   *cy1 = (const double *) cds1->covy,
                   *cz1 = (const double *) cds1->covz;
    double          x1i, y1i, z1i, x2i, y2i, z2i,
                    cx1i, cy1i, cz1i, cx2i, cy2i, cz2i;


    sum = 0.0;
    i = cds1->vlen;
    while(i-- > 0)
    {
        x1i = *x1++;
        y1i = *y1++;
        z1i = *z1++;
        x2i = *x2++;
        y2i = *y2++;
        z2i = *z2++;

        cx1i = *cx1++;
        cy1i = *cy1++;
        cz1i = *cz1++;
        cx2i = *cx2++;
        cy2i = *cy2++;
        cz2i = *cz2++;

        sum += (cx1i * x1i + cx2i * x2i) +
               (cy1i * y1i + cy2i * y2i) +
               (cz1i * z1i + cz2i * z2i);
    }

    return(sum);
}


static void
CalcR(const Cds *cds1, const Cds *cds2, double **Rmat,
      const double *weights)
{
    int             i;
    double          weight;
    const double   *x1 = (const double *) cds1->x,
                   *y1 = (const double *) cds1->y,
                   *z1 = (const double *) cds1->z;
    const double   *x2 = (const double *) cds2->x,
                   *y2 = (const double *) cds2->y,
                   *z2 = (const double *) cds2->z;
    double          x2i, y2i, z2i, x1i, y1i, z1i;
    double          Rmat00, Rmat01, Rmat02,
                    Rmat10, Rmat11, Rmat12,
                    Rmat20, Rmat21, Rmat22;

    Rmat00 = Rmat01 = Rmat02 = Rmat10 = Rmat11 = Rmat12 =
    Rmat20 = Rmat21 = Rmat22 = 0.0;

    for (i = 0; i < cds1->vlen; ++i)
    {
        weight = weights[i];

        x1i = x1[i];
        y1i = y1[i];
        z1i = z1[i];

        x2i = weight * x2[i];
        y2i = weight * y2[i];
        z2i = weight * z2[i];

        Rmat00 += x2i * x1i;
        Rmat01 += x2i * y1i;
        Rmat02 += x2i * z1i;

        Rmat10 += y2i * x1i;
        Rmat11 += y2i * y1i;
        Rmat12 += y2i * z1i;

        Rmat20 += z2i * x1i;
        Rmat21 += z2i * y1i;
        Rmat22 += z2i * z1i;
    }

    Rmat[0][0] = Rmat00;
    Rmat[0][1] = Rmat01;
    Rmat[0][2] = Rmat02;
    Rmat[1][0] = Rmat10;
    Rmat[1][1] = Rmat11;
    Rmat[1][2] = Rmat12;
    Rmat[2][0] = Rmat20;
    Rmat[2][1] = Rmat21;
    Rmat[2][2] = Rmat22;
}


static void
CalcRvan(const Cds *cds1, const Cds *cds2, double **Rmat)
{
    int             i;
    const double   *x1 = (const double *) cds1->x,
                   *y1 = (const double *) cds1->y,
                   *z1 = (const double *) cds1->z;
    const double   *x2 = (const double *) cds2->x,
                   *y2 = (const double *) cds2->y,
                   *z2 = (const double *) cds2->z;
    double          x2i, y2i, z2i, x1i, y1i, z1i;
    double          Rmat00, Rmat01, Rmat02,
                    Rmat10, Rmat11, Rmat12,
                    Rmat20, Rmat21, Rmat22;


    Rmat00 = Rmat01 = Rmat02 =
    Rmat10 = Rmat11 = Rmat12 =
    Rmat20 = Rmat21 = Rmat22 = 0.0;

    i = cds1->vlen;
    while(i-- > 0)
    {
        x1i = *x1++;
        y1i = *y1++;
        z1i = *z1++;

        x2i = *x2++;
        y2i = *y2++;
        z2i = *z2++;

        Rmat00 += x2i * x1i;
        Rmat01 += x2i * y1i;
        Rmat02 += x2i * z1i;

        Rmat10 += y2i * x1i;
        Rmat11 += y2i * y1i;
        Rmat12 += y2i * z1i;

        Rmat20 += z2i * x1i;
        Rmat21 += z2i * y1i;
        Rmat22 += z2i * z1i;
    }

    Rmat[0][0] = Rmat00;
    Rmat[0][1] = Rmat01;
    Rmat[0][2] = Rmat02;
    Rmat[1][0] = Rmat10;
    Rmat[1][1] = Rmat11;
    Rmat[1][2] = Rmat12;
    Rmat[2][0] = Rmat20;
    Rmat[2][1] = Rmat21;
    Rmat[2][2] = Rmat22;
}


static void
CalcRvan2(const double **cds1, const double **cds2, const int len, double **Rmat)
{
    int             i;
    const double   *x1 = cds1[0],
                   *y1 = cds1[1],
                   *z1 = cds1[2];
    const double   *x2 = cds2[0],
                   *y2 = cds2[1],
                   *z2 = cds2[2];
    double          x2i, y2i, z2i, x1i, y1i, z1i;
    double          Rmat00, Rmat01, Rmat02,
                    Rmat10, Rmat11, Rmat12,
                    Rmat20, Rmat21, Rmat22;

    Rmat00 = Rmat01 = Rmat02 =
    Rmat10 = Rmat11 = Rmat12 =
    Rmat20 = Rmat21 = Rmat22 = 0.0;

    i = len;
    while(i-- > 0)
    {
        x1i = *x1++;
        y1i = *y1++;
        z1i = *z1++;

        x2i = *x2++;
        y2i = *y2++;
        z2i = *z2++;

        Rmat00 += x2i * x1i;
        Rmat01 += x2i * y1i;
        Rmat02 += x2i * z1i;

        Rmat10 += y2i * x1i;
        Rmat11 += y2i * y1i;
        Rmat12 += y2i * z1i;

        Rmat20 += z2i * x1i;
        Rmat21 += z2i * y1i;
        Rmat22 += z2i * z1i;
    }

    Rmat[0][0] = Rmat00;
    Rmat[0][1] = Rmat01;
    Rmat[0][2] = Rmat02;
    Rmat[1][0] = Rmat10;
    Rmat[1][1] = Rmat11;
    Rmat[1][2] = Rmat12;
    Rmat[2][0] = Rmat20;
    Rmat[2][1] = Rmat21;
    Rmat[2][2] = Rmat22;
}


static void
CalcRCov(const Cds *cds1, const Cds *cds2, double **Rmat, const double **WtMat)
{
    int             i;
    const double   *x2 = (const double *) cds2->covx,
                   *y2 = (const double *) cds2->covy,
                   *z2 = (const double *) cds2->covz;
    const double   *x1 = (const double *) cds1->x,
                   *y1 = (const double *) cds1->y,
                   *z1 = (const double *) cds1->z;
    double          x2i, y2i, z2i, x1i, y1i, z1i;
    double          Rmat00, Rmat01, Rmat02,
                    Rmat10, Rmat11, Rmat12,
                    Rmat20, Rmat21, Rmat22;

    Rmat00 = Rmat01 = Rmat02 = Rmat10 = Rmat11 = Rmat12 =
    Rmat20 = Rmat21 = Rmat22 = 0.0;

    i = cds1->vlen;
    while(i-- > 0)
    {
        x2i = *x2++;
        y2i = *y2++;
        z2i = *z2++;

        x1i = *x1++;
        y1i = *y1++;
        z1i = *z1++;

        Rmat00 += x2i * x1i;
        Rmat01 += x2i * y1i;
        Rmat02 += x2i * z1i;

        Rmat10 += y2i * x1i;
        Rmat11 += y2i * y1i;
        Rmat12 += y2i * z1i;

        Rmat20 += z2i * x1i;
        Rmat21 += z2i * y1i;
        Rmat22 += z2i * z1i;
    }

    Rmat[0][0] = Rmat00;
    Rmat[0][1] = Rmat01;
    Rmat[0][2] = Rmat02;
    Rmat[1][0] = Rmat10;
    Rmat[1][1] = Rmat11;
    Rmat[1][2] = Rmat12;
    Rmat[2][0] = Rmat20;
    Rmat[2][1] = Rmat21;
    Rmat[2][2] = Rmat22;
}


static int
CalcGSLSVD(double **Rmat, double **Umat, double *sigma, double **VTmat)
{
/*     svdcmp(Rmat, 3, 3, sigma, VTmat); */
/*     Mat3Cpy(Umat, (const double **) Rmat); */
/*     Mat3TransposeIp(VTmat); */

/*     double **tmpmat = MatAlloc(3, 3); */
/*     Mat3Cpy(tmpmat, (const double **) Rmat); */

/*     int        info = 0; */
/*     char            jobu = 'A'; */
/*     char            jobvt = 'A'; */
/*     int        m = 3, n = 3, lda = 3, ldu = 3, ldvt = 3; */
/*     int        lwork = 100; */
/*     double         *work; */
/*  */
/*     Mat3TransposeIp(Rmat); */

/*     printf("\n\n Rmat:"); */
/*     write_C_mat((const double **)Rmat, 3, 8, 0); */

    //return(dgesvd_opt_dest(Rmat, 3, 3, Umat, sigma, VTmat));

    // GSL says Jacobi SVD is more accurate the Golub
    svdGSLJacobiDest(Rmat, 3, sigma, VTmat);
    Mat3TransposeIp(VTmat);
    Mat3Cpy(Umat, (const double **) Rmat);

    return(1);

/*     printf("\n\n **********************************************************************:"); */
/*     printf("\n\n GSL:"); */
/*     printf("\n Umat:"); */
/*     write_C_mat((const double **)tmpmat, 3, 8, 0); */
/*     printf("\n\n VTmat:"); */
/*     write_C_mat((const double **)VTmat, 3, 8, 0); */
/*     int i; */
/*     for (i = 0; i < 3; ++i) */
/*         printf("\n sigma[%d] = %8.2f ", i, sigma[i]); */
/*  */
/*     dgesvd_opt_dest(Rmat, 3, 3, Umat, sigma, VTmat); */
/*  */
/*     printf("\n\n LAPACK:"); */
/*     printf("\n Umat:"); */
/*     write_C_mat((const double **)Umat, 3, 8, 0); */
/*     printf("\n\n VTmat:"); */
/*     write_C_mat((const double **)VTmat, 3, 8, 0); */
/*  */
/*     for (i = 0; i < 3; ++i) */
/*         printf("\n sigma[%d] = %8.2f ", i, sigma[i]); */
/*  */
/*     fflush(NULL); */

/*     work = (double *) malloc(lwork * sizeof(double)); */
/*  */
/*     DGESVD(&jobvt, &jobu, &n, &m, */
/*             &Rmat[0][0], &lda, sigma, */
/*             &VTmat[0][0], &ldvt, */
/*             &Umat[0][0], &ldu, */
/*             work, &lwork, &info); */

/*     Mat3TransposeIp(Umat); */
/*     Mat3TransposeIp(VTmat); */

/*  */
/*     char            jobvl = 'V'; */
/*     char            jobvr = 'V'; */
/*     double         *wr = malloc(3 * sizeof(double)); */
/*     double         *wi = malloc(3 * sizeof(double)); */
/*     double        **vl = MatAlloc(3, 3); */
/*     double        **vr = MatAlloc(3, 3); */
/*  */
/*     DGEEV(&jobvl, &jobvr, &n, &tmpmat[0][0], &n,  */
/*            wr, wi, */
/*            &vl[0][0], &n, &vr[0][0], &n, */
/*            work, &lwork, &info); */
/*  */
/*     Mat3Print(vl); */
/*     printf("\n real %f %f %f", wr[0], wr[1], wr[2]); */
/*     printf("\n imag %f %f %f", wi[0], wi[1], wi[2]); */
/*     free(wr); */
/*     free(wi); */
/*     MatDestroy(&vl); */
/*     MatDestroy(&vr); */
/*     MatDestroy(&tmpmat); */

/*     free(work); */
}


/* Takes U and V^t on input, calculates R = VU^t */
static int
CalcRotMat(double **rotmat, double **Umat, double **Vtmat)
{
    int         i, j, k;
    double      det;

    memset(&rotmat[0][0], 0, 9 * sizeof(double));

    det = Mat3Det((const double **)Umat) * Mat3Det((const double **)Vtmat);

    if (det > 0)
    {
        for (i = 0; i < 3; ++i)
            for (j = 0; j < 3; ++j)
                for (k = 0; k < 3; ++k)
                    rotmat[i][j] += (Vtmat[k][i] * Umat[j][k]);

        return(1);
    }
    else
    {
        /* printf("\n * determinant of SVD U or V matrix = %f", det); */

        for (i = 0; i < 3; ++i)
        {
            for (j = 0; j < 3; ++j)
            {
                for (k = 0; k < 2; ++k)
                    rotmat[i][j] += (Vtmat[k][i] * Umat[j][k]);

                rotmat[i][j] -= (Vtmat[2][i] * Umat[j][2]);
            }
        }

        return(-1);
    }
}


/* returns sum of squared residuals, E
   rmsd = sqrt(E/atom_num)  */
double
ProcGSLSVDvan(const Cds *cds1, const Cds *cds2, double **rotmat,
              double **Rmat, double **Umat, double **VTmat, double *sigma)
{
    double          det, sumdev, term1, term2;

    term1 = CalcInnProdNorm(cds2);
    CalcRvan(cds1, cds2, Rmat);
    CalcGSLSVD(Rmat, Umat, sigma, VTmat);
    det = CalcRotMat(rotmat, Umat, VTmat);
    term2 = CalcInnProdNorm(cds1);
    sumdev = term1 + term2;

/*     VerifyRotMat(rotmat, 1e-5); */
/*     printf("\n*************** sumdev = %8.2f ", sumdev); */
/*     printf("\nrotmat:"); */
/*     write_C_mat((const double **)rotmat, 3, 8, 0); */

    if (det < 0)
        sumdev -= 2.0 * (sigma[0] + sigma[1] - sigma[2]);
    else
        sumdev -= 2.0 * (sigma[0] + sigma[1] + sigma[2]);

/*     printf("\nRmat:"); */
/*     write_C_mat((const double **)Rmat, 3, 8, 0); */
/*     printf("\nUmat:"); */
/*     write_C_mat((const double **)Umat, 3, 8, 0); */
/*     printf("\nVTmat:"); */
/*     write_C_mat((const double **)VTmat, 3, 8, 0); */
/*     int i; */
/*     for (i = 0; i < 3; ++i) */
/*         printf("\nsigma[%d] = %8.2f ", i, sigma[i]); */

    return(sumdev);
}


/* returns sum of squared residuals, E
   rmsd = sqrt(E/atom_num)  */
double
ProcGSLSVDvan2(const double **cds1, const double **cds2, const int len, double **rotmat,
               double **Rmat, double **Umat, double **VTmat, double *sigma,
               double *norm1, double *norm2, double *innprod)
{
    double          det;

    *norm1 = CalcInnProdNorm2(cds1, len);
    *norm2 = CalcInnProdNorm2(cds2, len);
    CalcRvan2(cds1, cds2, len, Rmat);
    CalcGSLSVD(Rmat, Umat, sigma, VTmat);
    det = CalcRotMat(rotmat, Umat, VTmat);

//     VerifyRotMat(rotmat, 1e-5);
/*     printf("\n*************** sumdev = %8.2f ", sumdev); */
/*     printf("\nrotmat:"); */
/*     write_C_mat((const double **)rotmat, 3, 8, 0); */

    if (det < 0)
        *innprod = sigma[0] + sigma[1] - sigma[2];
    else
        *innprod = sigma[0] + sigma[1] + sigma[2];

/*     printf("\nRmat:"); */
/*     write_C_mat((const double **)Rmat, 3, 8, 0); */
/*     printf("\nUmat:"); */
/*     write_C_mat((const double **)Umat, 3, 8, 0); */
/*     printf("\nVTmat:"); */
/*     write_C_mat((const double **)VTmat, 3, 8, 0); */
/*     int i; */
/*     for (i = 0; i < 3; ++i) */
/*         printf("\nsigma[%d] = %8.2f ", i, sigma[i]); */

    return(*norm1 + *norm2 - 2.0 * *innprod);
}



static void
CalcRFrag(const FragCds *cds1, const FragCds *cds2, double **Rmat)
{
    int             i;
    const double   *x1 = (const double *) cds1->x,
                   *y1 = (const double *) cds1->y,
                   *z1 = (const double *) cds1->z;
    const double   *x2 = (const double *) cds2->x,
                   *y2 = (const double *) cds2->y,
                   *z2 = (const double *) cds2->z;
    double          x2i, y2i, z2i, x1i, y1i, z1i;
    double          Rmat00, Rmat01, Rmat02,
                    Rmat10, Rmat11, Rmat12,
                    Rmat20, Rmat21, Rmat22;


    Rmat00 = Rmat01 = Rmat02 = Rmat10 = Rmat11 = Rmat12 =
    Rmat20 = Rmat21 = Rmat22 = 0.0;

    i = cds1->fraglen;
    while(i-- > 0)
    {
        x1i = *x1++;
        y1i = *y1++;
        z1i = *z1++;

        x2i = *x2++;
        y2i = *y2++;
        z2i = *z2++;

        Rmat00 += x2i * x1i;
        Rmat01 += x2i * y1i;
        Rmat02 += x2i * z1i;

        Rmat10 += y2i * x1i;
        Rmat11 += y2i * y1i;
        Rmat12 += y2i * z1i;

        Rmat20 += z2i * x1i;
        Rmat21 += z2i * y1i;
        Rmat22 += z2i * z1i;
    }

    Rmat[0][0] = Rmat00;
    Rmat[0][1] = Rmat01;
    Rmat[0][2] = Rmat02;
    Rmat[1][0] = Rmat10;
    Rmat[1][1] = Rmat11;
    Rmat[1][2] = Rmat12;
    Rmat[2][0] = Rmat20;
    Rmat[2][1] = Rmat21;
    Rmat[2][2] = Rmat22;
}


static double
CalcInnProdNormFrag(const FragCds *cds)
{
    int             i;
    double          sum;
    const double   *x = (const double *) cds->x,
                   *y = (const double *) cds->y,
                   *z = (const double *) cds->z;
    double          xi, yi, zi;

    sum = 0.0;
    i = cds->fraglen;
    while(i-- > 0)
    {
        xi = *x++;
        yi = *y++;
        zi = *z++;

        sum += xi * xi + yi * yi + zi * zi;
    }

    return(sum);
}


/* returns sum of squared residuals, E
   rmsd = sqrt(E/atom_num)  */
double
ProcGSLSVDFrag(const FragCds *frag1, const FragCds *frag2, double **rotmat,
               double **Rmat, double **Umat, double **VTmat, double *sigma)
{
    double          det, sumdev, term1, term2;

    term1 = CalcInnProdNormFrag(frag2);
    CalcRFrag(frag1, frag2, Rmat);
    CalcGSLSVD(Rmat, Umat, sigma, VTmat);
    det = CalcRotMat(rotmat, Umat, VTmat);
    term2 = CalcInnProdNormFrag(frag1);
    sumdev = term1 + term2;

    if (det < 0)
        sumdev -= 2.0 * (sigma[0] + sigma[1] - sigma[2]);
    else
        sumdev -= 2.0 * (sigma[0] + sigma[1] + sigma[2]);

    return(sumdev);
}



double
ProcGSLSVD(const Cds *cds1, const Cds *cds2, double **rotmat,
           const double *weights,
           double **Rmat, double **Umat, double **VTmat, double *sigma)
{
    double          det, sumdev, term1, term2;

    term1 = CalcMahFrobInnProd(cds2, weights);
    CalcR(cds1, cds2, Rmat, weights);
    CalcGSLSVD(Rmat, Umat, sigma, VTmat);
    det = CalcRotMat(rotmat, Umat, VTmat);
    term2 = CalcMahFrobInnProdRot(cds1, (const double **) rotmat, weights);
    sumdev = term1 + term2;

/*     VerifyRotMat(rotmat, 1e-5); */
/*     printf("\n*************** sumdev = %8.2f ", sumdev); */
/*     printf("\nrotmat:"); */
/*     write_C_mat((const double **)rotmat, 3, 8, 0); */

    if (det < 0)
        sumdev -= 2.0 * (sigma[0] + sigma[1] - sigma[2]);
    else
        sumdev -= 2.0 * (sigma[0] + sigma[1] + sigma[2]);

/*     printf("\nRmat:"); */
/*     write_C_mat((const double **)Rmat, 3, 8, 0); */
/*     printf("\nUmat:"); */
/*     write_C_mat((const double **)Umat, 3, 8, 0); */
/*     printf("\nVTmat:"); */
/*     write_C_mat((const double **)VTmat, 3, 8, 0); */
/*     int i; */
/*     for (i = 0; i < 3; ++i) */
/*         printf("\nsigma[%d] = %8.2f ", i, sigma[i]); */

    return(sumdev);
}


/* returns sum of squared residuals, E
   rmsd = sqrt(E/atom_num)  */
double
ProcGSLSVDCov(Cds *cds1, Cds *cds2, double **rotmat,
                 const double **WtMat, double **Rmat,
                 double **Umat, double **VTmat, double *sigma)
{
    double          det, sumdev = 0.0;

    CalcCovCds(cds1, WtMat);
    CalcCovCds(cds2, WtMat);

    sumdev = CalcE0Cov(cds1, cds2);
    CalcRCov(cds1, cds2, Rmat, WtMat);
    CalcGSLSVD(Rmat, Umat, sigma, VTmat);
    det = CalcRotMat(rotmat, Umat, VTmat);

    if (det < 0)
        sumdev -= 2.0 * (sigma[0] + sigma[1] - sigma[2]);
    else
        sumdev -= 2.0 * (sigma[0] + sigma[1] + sigma[2]);

    return(sumdev);
}
