/*
    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 "DLTmath.h"
#include "pdbUtils.h"
#include "pdbStats.h"
#include "CovMat.h"
#include "FragCds.h"
#include "ProcJacobiSVD.h"

static double
CalcE0(const Cds *cds1, const Cds *cds2, const double *weights);

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 **covmat);

static int
CalcJacobiSVD(double **a, double **U, double *z, double **V, double tol);

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

static double
CalcE0(const Cds *cds1, const Cds *cds2, const double *weights)
{
    int             i;
    double          sum;
    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          x1i, y1i, z1i, x2i, y2i, z2i;
/* #include "pdbIO.h" */
/* PrintCds((Cds *)cds1); */
/* PrintCds((Cds *)cds2); */

    sum = 0.0;
    i = cds1->vlen;
    while(i-- > 0)
    {
        x1i = *x1++;
        y1i = *y1++;
        z1i = *z1++;
        x2i = *x2++;
        y2i = *y2++;
        z2i = *z2++;
        sum +=  *weights++ *
               ((x1i * x1i + x2i * x2i) +
                (y1i * y1i + y2i * y2i) +
                (z1i * z1i + z2i * z2i));
/* printf("\nsum = %d %f %f", i, weight, sum); */
    }
/* exit(0); */
    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);
}


/* This function assumes that the coordinates have been centered previously
   Use CenMass() and ApplyCenter() */
static void
CalcR(const Cds *cds1, const Cds *cds2, double **Rmat, const double *weights)
{
    int             i;
    double          weight;
    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;
    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)
    {
        weight = *weights++;

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

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

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

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

        Rmat20 += weight * z2i * x1i;
        Rmat21 += weight * z2i * y1i;
        Rmat22 += weight * 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
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 void
CalcRCov(const Cds *cds1, const Cds *cds2, double **Rmat, const double **covmat)
{
    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;
}


/* Classic one-sided Jacobi SVD algorithm for 3x3 matrices.
   Calculates the SVD of a, a = UzV^t .

   This is pulled almost verbatim from
   John Nash, _Compact Numerical Methods for Computers_, 1979, Wiley, NY
   Chapter 3, algorithm 1, pp 30-31.

   It is based on the method of Chartres:

   Chartres, B.A. (1962) "Adaptation of the Jacobi method for a computer with
   magnetic tape backing store." Comput. J. 5:51-60.

   Nash, J.C. (1975) "A one-sided transformation method for the Singular Value
   Decomposition and algebraic eigenproblem."  Comput. J. 18:74-76.

   I have added a convergence criterion based on the sum of cross-products
   after a sweep -- it should be 0 if the matrix is orthogonal.
   I also made some adjustments for C-style indexing.
   Unlike Nash's implementation, this function returns the
   transpose of V, same behavior as LAPACK DGESVD.
   Singular values are ordered largest to smallest and stored in z.
   The matrix A is preserved (unlike in Nash 1979 where it is overwritten
   with U) */
static int
CalcJacobiSVD(double **A, double **U, double *z, double **Vt, double tol)
{
    int             i, j, k, count, sweeps;
    double          eps, orth, orthsum;
    double          p, q, r, v, g, h, pp, qr, c = 0.0, s = 1.0;

    /* U = V = I_3 */
    memset(&U[0][0], 0, 9 * sizeof(double));
    for (i = 0; i < 3; i++)
        U[i][i] = 1.0;

    MatCpySqr(Vt, (const double **) A, 3);

    eps = tol * tol * fabs(Vt[0][0] + Vt[1][1] + Vt[2][2]);

    count = sweeps = 0;
    do
    {
        orthsum = 0.0;
        for (j = 0; j < 2; j++)
        {
            for (k = j+1; k < 3; k++)
            {
                p = q = r = 0.0; /* step 4 */
                for (i = 0; i < 3; ++i) /* step 5 */
                {
                    g = Vt[j][i];
                    h = Vt[k][i];
                    p += g * h;
                    q += g * g;
                    r += h * h;
                }

                pp = p*p;
                qr = q*r;
                orth = pp / qr;
                orthsum += orth;

                if (r > eps && qr > eps && orth > eps)
                {
                    if (q < r)
                    {
                        c = 0.0;
                        s = 1.0;
                    }
                    else
                    {
                        /* step 8 - calculation of sine and cosine of rotation angle */
                        q -= r;
                        v = sqrt((4.0 * pp) + (q * q));
                        // v = hypot(2.0*p, q);
                        c = sqrt(0.5 * (v + q) / v);
                        s = p / (v * c);
                    }

                    /* step 9 - apply rotation to a */
                    for (i = 0; i < 3; ++i)
                    {
                        r = Vt[j][i];
                        h = Vt[k][i];
                        Vt[j][i] =  (r * c) + (h * s);
                        Vt[k][i] = -(r * s) + (h * c);
                    }

                    /* step 10 - apply rotation to V */
                    for (i = 0; i < 3; ++i)
                    {
                        r = U[i][j];
                        h = U[i][k];
                        U[i][j] =  (r * c) + (h * s);
                        U[i][k] = -(r * s) + (h * c);
                    }
                }
                /* step 12 */
                count++;
            }
        }

        sweeps++;
        /* printf("\n> [%3d] orthsum: %e   %e", count, orthsum, eps); */
    } while (orthsum > eps && count < 200);

    for (j = 0; j < 3; j++)
    {
        q = 0;
        for (i = 0; i < 3; ++i)
            q += Vt[j][i] * Vt[j][i];

        z[j] = q = sqrt(q);

        if (q <= DBL_EPSILON)
            break;

        for (i = 0; i < 3; ++i)
            Vt[j][i] /= q;
    }

/*     printf("\n> count: %3d   sweeps: %3d", count, sweeps); */
/*     printf("\n singvals:     % f   % f   % f\nJacobi\n", z[0], z[1], z[2]); */
/*     Mat3Print(U); */
/*     Mat3Print(Vt); */
/*  */
/*     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; */
/*   */
/*     work = (double *) malloc(lwork * sizeof(double)); */
/*  */
/*     DGESVD(&jobvt, &jobu, &n, &m, */
/*             &A[0][0], &lda, &z[0], */
/*             &Vt[0][0], &ldvt, */
/*             &U[0][0], &ldu, */
/*             work, &lwork, &info); */
/*  */
/*     printf("\n singvals:     % f   % f   % f\nLAPACK:\n", z[0], z[1], z[2]); */
/*     Mat3Print(U); */
/*     Mat3Print(Vt); */
/*  */
/*     free(work); */
/*     exit(0); */
    return(count);
}


/* Same as CalcJacobiSVD(), but with partial pivoting on row with largest norm.
   Supposedly improves convergence, but probably not for 3x3 matrices.
   Adapted from:

   De Rijk, P.P.M. (1989) "A one-sided Jacobi algorithm for computing the
   singular value decomposition on a vector computer."
   SIAM J. Sci. Stat. Comput. 10(2):359-371.

   See section 2.4 pp. 363-364.
   */
int
CalcJacobiSVDpp(double **A, double **U, double *z, double **Vt, double tol)
{
    int             i, j, k, ss, count, sweeps, pivot;
    double          eps, orth, orthsum, tmp;
    double          p, q, r, v, g, h, pp, qr, c = 0.0, s = 1.0;

    /* U = V = I_3 */
    memset(&U[0][0], 0, 9 * sizeof(double));
    for (i = 0; i < 3; i++)
        U[i][i] = 1.0;

    MatCpySqr(Vt, (const double **) A, 3);

    eps = tol * tol * fabs(Vt[0][0] + Vt[1][1] + Vt[2][2]);

    /* test for orthonormality */
    orthsum = 0.0;
    for (j = 0; j < 2; j++)
    {
        for (k = j+1; k < 3; k++)
        {
            p = q = r = 0.0;
            for (i = 0; i < 3; ++i)
            {
                g = Vt[j][i];
                h = Vt[k][i];
                p += g * h;
                q += g * g;
                r += h * h;
            }

            orthsum += (p*p) / (q*r);
        }
    }

    count = sweeps = pivot = 0;
    while (orthsum > eps && count < 100)
    {
        for (ss = 0; ss < 2; ++ss) /* subsweeps 1 & 2 */
        {
            p = 0.0;
            for (i = 0; i < 3; ++i)
                p += Vt[ss][i] * Vt[ss][i];

            q = 0.0;
            for (i = 0; i < 3; ++i)
                q += Vt[ss+1][i] * Vt[ss+1][i];

            if (p < q)
            {
                for (i = 0; i < 3; ++i)
                {
                    tmp = U[ss][i];
                    U[ss][i] = U[ss+1][i];
                    U[ss+1][i] = tmp;

                    tmp = Vt[ss][i];
                    Vt[ss][i] = Vt[ss+1][i];
                    Vt[ss+1][i] = tmp;
                    pivot++;
                }
            }

            for (j = ss; j < 2; j++)
            {
                for (k = j+1; k < 3; k++)
                {
                    p = q = r = 0.0; /* step 4 */
                    for (i = 0; i < 3; ++i) /* step 5 */
                    {
                        g = Vt[j][i];
                        h = Vt[k][i];
                        p += g * h;
                        q += g * g;
                        r += h * h;
                    }

                    pp = p*p;
                    qr = q*r;
                    orth = pp / qr;
                    /* orthsum += orth; */

                    if (r > eps && qr > eps && orth > eps)
                    {
                        if (q < r)
                        {
                            c = 0.0;
                            s = 1.0;
                        }
                        else
                        {
                            /* step 8 - calculation of sine and cosine of rotation angle */
                            q -= r;
                            v = sqrt((4.0 * pp) + (q * q));
                            c = sqrt(0.5 * (v + q) / v);
                            s = p / (v * c);
                        }

                        /* step 9 - apply rotation to a */
                        for (i = 0; i < 3; ++i)
                        {
                            r = Vt[j][i];
                            h = Vt[k][i];
                            Vt[j][i] =  (r * c) + (h * s);
                            Vt[k][i] = -(r * s) + (h * c);
                        }

                        /* step 10 - apply rotation to V */
                        for (i = 0; i < 3; ++i)
                        {
                            r = U[i][j];
                            h = U[i][k];
                            U[i][j] =  (r * c) + (h * s);
                            U[i][k] = -(r * s) + (h * c);
                        }
                    }
                    /* step 12 */
                    count++;
                }
            } /* end subsweep */
            sweeps++;

            /* test for orthonormality */
            orthsum = 0.0;
            for (j = 0; j < 2; j++)
            {
                for (k = j+1; k < 3; k++)
                {
                    p = q = r = 0.0;
                    for (i = 0; i < 3; ++i)
                    {
                        g = Vt[j][i];
                        h = Vt[k][i];
                        p += g * h;
                        q += g * g;
                        r += h * h;
                    }

                    orthsum += (p*p) / (q*r);
                }
            }

            if (orthsum <= eps)
                break;
        } /* end sweep */
/*         printf("\n> [%3d] orthsum: %e   %e", count, orthsum, eps); */
/*         fflush(NULL); */
    }

    for (j = 0; j < 3; j++)
    {
        q = 0;
        for (i = 0; i < 3; ++i)
            q += Vt[j][i] * Vt[j][i];

        z[j] = q = sqrt(q);

        if (q <= DBL_EPSILON)
            break;

        for (i = 0; i < 3; ++i)
            Vt[j][i] /= q;
    }

/*     printf("\n> count: %3d   sweeps: %3d   pivot: %3d", count, sweeps, pivot); */
/*     fflush(NULL); */

    return(count);
}


/* 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
ProcJacobiSVD(const Cds *cds1, const Cds *cds2, double **rotmat,
                 const double *weights,
                 double **Rmat, double **Umat, double **VTmat, double *sigma)
{
    double          det, sumdev;

    sumdev = CalcE0(cds1, cds2, weights);
    /* printf("\n # sumdev = %8.2f ", sumdev); */
    CalcR(cds1, cds2, Rmat, weights);
    CalcJacobiSVD(Rmat, Umat, sigma, VTmat, 1e-8);
    det = CalcRotMat(rotmat, Umat, VTmat);

/*     VerifyRotMat(rotmat, 1e-5); */
/*     printf("\n\n rotmat:"); */
/*     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("\n\n Rmat:"); */
/*     write_C_mat((const double **)Rmat, 3, 8, 0); */
/*     printf("\n\n Umat:"); */
/*     write_C_mat((const double **)Umat, 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]); */

    return(sumdev);
}


// 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
// CalcInnProdNormRot(const Cds *cds, const double **rmat)
// {
//     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 += xir * xir + yir * yir + zir * zir;
//     }
//
//     return(sum);
// }


double
ProcJacobiSVDvan(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);
    CalcJacobiSVD(Rmat, Umat, sigma, VTmat, 1e-12);
    det = CalcRotMat(rotmat, Umat, VTmat);
    // term2 = CalcInnProdNormRot(cds1, (const double **) rotmat);
    term1 = term2 = 0.0; /* this is just a fudge to make this faster - DLT */
    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);
}


double
ProcJacobiSVDFrag(const FragCds *cds1, const FragCds *cds2, double **rotmat,
                  double **Rmat, double **Umat, double **VTmat, double *sigma)
{
    double          det, sumdev, term1, term2;

    CalcRFrag(cds1, cds2, Rmat);
    CalcJacobiSVD(Rmat, Umat, sigma, VTmat, 1e-12);
    det = CalcRotMat(rotmat, Umat, VTmat);
    term1 = term2 = 0.0; /* this is just a fudge to make this faster - DLT */
    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);
}


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

    CalcCovCds(cds1, covmat);
    CalcCovCds(cds2, covmat);

    sumdev = CalcE0Cov(cds1, cds2);
    CalcRCov(cds1, cds2, Rmat, covmat);
    CalcJacobiSVD(Rmat, Umat, sigma, VTmat, DBL_EPSILON);
    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);
}
