/*
    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 <time.h>
#include "pdbStats.h"
#include "pdbIO.h"
#include "Embed.h"
#include "DLTmath.h"
#include "FragCds.h"
#include "ProcGSLSVD.h"
#include "QuarticHornFrag.h"


/* Computes a 3x3 rotation matrix from a quaternion */
/* Based on Horn 1987, p. 641, section A8. */
/* Based on Kearsley 1989, the third equation on page
   209, column 1. Kearsley premultiplies with the rotation,
   whereas I postmultiply, so below is the transpose of
   his. */
void
QuatToRotmat(double **R,      /* 3x3 rotation matrix */
             const double *q) /* rotation eigenvector of Q, a 1x4 quaternion */
{
    double   q0sqr, q1sqr, q2sqr, q3sqr;
    double   q12, q03, q13, q02, q23, q01;

    q0sqr = q[0] * q[0];
    q1sqr = q[1] * q[1];
    q2sqr = q[2] * q[2];
    q3sqr = q[3] * q[3];

    q12 = q[1] * q[2];
    q03 = q[0] * q[3];
    q13 = q[1] * q[3];
    q02 = q[0] * q[2];
    q23 = q[2] * q[3];
    q01 = q[0] * q[1];

    R[1][0] = 2.0 * (q12 - q03);
    R[0][1] = 2.0 * (q12 + q03);
    R[2][0] = 2.0 * (q13 + q02);
    R[0][2] = 2.0 * (q13 - q02);
    R[2][1] = 2.0 * (q23 - q01);
    R[1][2] = 2.0 * (q23 + q01);

    R[0][0] = q0sqr + q1sqr - q2sqr - q3sqr;
    R[1][1] = q0sqr + q2sqr - q1sqr - q3sqr;
    R[2][2] = q0sqr + q3sqr - q1sqr - q2sqr;
}


/* Evaluates the Newton-Raphson correction for the Horn quartic.
   only 11 FLOPs */
static double
eval_horn_NR_corrxn(const double *c, const double x)
{
    double x2 = x*x;
    double b = (x2 + c[2])*x;
    double a = b + c[1];

    return((a*x + c[0])/(2.0*x2*x + b + a));
}


/* Newton-Raphson root finding */
static double
QCProot(double *coeff, double guess, const double delta)
{
    int             i;
    double          oldg;

    for (i = 0; i < 100; ++i)
    {
        oldg = guess;
        /* guess -= (eval_horn_quart(coeff, guess) / eval_horn_quart_deriv(coeff, guess)); */
        guess -= eval_horn_NR_corrxn(coeff, guess);

        if (fabs(guess - oldg) < fabs(delta*guess))
            return(guess);
    }

    fprintf(stderr,
            "\n WARNING: Newton-Raphson root-finding in \'QCProot()\' did not converge: %g %g %g\n",
            guess, oldg, guess-oldg);

    return(guess);
//    exit(EXIT_FAILURE);
}


/* evaluates the horn quartic for coefficients c and given x */
static double
eval_horn_quart(double *c, double x)
{
    return(((x*x+c[2])*x+c[1])*x+c[0]);
}


/* evaluates the derivative of the horn quartic for coefficients c and given x */
static double
eval_horn_quart_deriv(double *c, double x)
{
    return(2.0*(2.0*x*x + c[2])*x + c[1]);
}


/* evaluates the quartic for coefficients c and given x */
double
eval_quart(double *c, double x)
{
    return((((c[4]*x+c[3])*x+c[2])*x+c[1])*x+c[0]);
}


/* evaluates the derivative of the quartic for coefficients c and given x */
double
eval_quart_deriv(double *c, double x)
{
    return(((4.0*c[4]*x + 3.0*c[3])*x + 2.0*c[2])*x + c[1]);
}


/* Newton-Raphson root finding */
double
horn_root(double *coeff, double guess, int n, double delta)
{
    int             i;
    double          x, xold;

    i = 0;
    x = guess;

    while (i < n)
    {
       xold = x;
       x -= (eval_horn_quart(coeff, x) / eval_horn_quart_deriv(coeff, x));

       if (fabs(x - xold) < delta)
          return(x);

       ++i;
    }

    fprintf(stderr, "\n\n ERROR21: newton-raphson root finding in \'root\' did not converge \n");
    return(x);
}


void
ComputeQuartHornFrag(const FragCds *frag1, const FragCds *frag2, double *coeff)
{
    double          Sxx, Sxy, Sxz,
                    Syx, Syy, Syz,
                    Szx, Szy, Szz,
                    Szz2, Syy2, Sxx2,
                    Sxy2, Syz2, Sxz2,
                    Syx2, Szy2, Szx2;
    double          x1, x2, y1, y2, z1, z2;
    const double   *fx1 = frag1->x, *fy1 = frag1->y, *fz1 = frag1->z,
                   *fx2 = frag2->x, *fy2 = frag2->y, *fz2 = frag2->z;
    int             i;

    Sxx = Sxy = Sxz = Syx = Syy = Syz = Szx = Szy = Szz = 0.0;
    for (i = 0; i < frag1->fraglen; ++i)
    {
        x1 = fx1[i];
        y1 = fy1[i];
        z1 = fz1[i];
        x2 = fx2[i];
        y2 = fy2[i];
        z2 = fz2[i];


        Sxx += (x1 * x2);
        Sxy += (x1 * y2);
        Sxz += (x1 * z2);

        Syx += (y1 * x2);
        Syy += (y1 * y2);
        Syz += (y1 * z2);

        Szx += (z1 * x2);
        Szy += (z1 * y2);
        Szz += (z1 * z2);
    }

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

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

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

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

    coeff[0] = mysquare(Sxy2 + Sxz2 - Syx2 - Szx2);
    coeff[0] += (+(Sxy+Syx)*(Syz-Szy)+(Sxz-Szx)*(Sxx-Syy-Szz)) * (-(Sxy-Syx)*(Syz+Szy)+(Sxz-Szx)*(Sxx+Syy-Szz));
    coeff[0] +=  ((-Sxx2 + Syy2 + Syz2 + 2.0*Syz*Szy + Szy2 - 2.0*Syy*Szz + Szz2)
                * (-Sxx2 + Syy2 + Syz2 - 2.0*Syz*Szy + Szy2 + 2.0*Syy*Szz + Szz2));
    coeff[0] += (-(Sxz+Szx)*(Syz-Szy)+(Sxy-Syx)*(Sxx-Syy-Szz)) * ((-Sxz+Szx)*(Syz+Szy)+(Sxy-Syx)*(Sxx-Syy+Szz));
    coeff[0] += (-(Sxz+Szx)*(Syz+Szy)-(Sxy+Syx)*(Sxx+Syy-Szz)) * ((-Sxz+Szx)*(Syz-Szy)-(Sxy+Syx)*(Sxx+Syy+Szz));
    coeff[0] += (+(Sxy+Syx)*(Syz+Szy)+(Sxz+Szx)*(Sxx-Syy+Szz)) * (-(Sxy-Syx)*(Syz-Szy)+(Sxz+Szx)*(Sxx+Syy+Szz));
}


/* A lot of register variables, but this sort of thing scales very well with
   new and improved processors */
double
CalcQuarticCoeffsPu2(const FragCds *frag1, const FragCds *frag2, const int len, double *coeff, const double outerprod, double *quat)
{
    double          Sxx, Sxy, Sxz, Syx, Syy, Syz, Szx, Szy, Szz;
    double          Szz2, Syy2, Sxx2, Sxy2, Syz2, Sxz2, Syx2, Szy2, Szx2,
                    SyzSzymSyySzz2, Sxx2Syy2Szz2Syz2Szy2, Sxy2Sxz2Syx2Szx2,
                    SxzpSzx, SyzpSzy, SxypSyx, SyzmSzy,
                    SxzmSzx, SxymSyx, SxxpSyy, SxxmSyy;
    double          x1, x2, y1, y2, z1, z2;
    const double   *fx1 = frag1->x, *fy1 = frag1->y, *fz1 = frag1->z,
                   *fx2 = frag2->x, *fy2 = frag2->y, *fz2 = frag2->z;
    int             i;
    double          lambdamax;
    double          precision = 1e-7;

    Sxx = Sxy = Sxz = Syx = Syy = Syz = Szx = Szy = Szz = 0.0;
    for (i = 0; i < len; ++i)
    {
        x1 = fx1[i];
        y1 = fy1[i];
        z1 = fz1[i];
        x2 = fx2[i];
        y2 = fy2[i];
        z2 = fz2[i];

        Sxx += (x1 * x2);
        Sxy += (x1 * y2);
        Sxz += (x1 * z2);

        Syx += (y1 * x2);
        Syy += (y1 * y2);
        Syz += (y1 * z2);

        Szx += (z1 * x2);
        Szy += (z1 * y2);
        Szz += (z1 * z2);
    }

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

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

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

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

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

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

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

    lambdamax = QCProot(coeff, 0.5 * outerprod, precision);

    /* Now calculate the optimal rotation from one row of the cofactor matrix */
    double a13, a14, a21, a22, a23, a24;
    double a31, a32, a33, a34, a41, a42, a43, a44;

    a13 = -SxzmSzx;
    a14 = SxymSyx;
    a21 = SyzmSzy;
    a22 = SxxmSyy - Szz - lambdamax;
    a23 = SxypSyx;
    a24 = SxzpSzx;
    a31 = a13;
    a32 = a23;
    a33 = Syy - Sxx - Szz - lambdamax;
    a34 = SyzpSzy;
    a41 = a14;
    a42 = a24;
    a43 = a34;
    a44 = Szz - SxxpSyy - lambdamax;

    double a3344_4334 = a33 * a44 - a43 * a34;
    double a3244_4234 = a32 * a44 - a42 * a34;
    double a3243_4233 = a32 * a43 - a42 * a33;
    double a3143_4133 = a31 * a43 - a41 * a33;
    double a3144_4134 = a31 * a44 - a41 * a34;
    double a3142_4132 = a31 * a42 - a41 * a32;
    double q1 =  a22*a3344_4334 - a23*a3244_4234 + a24*a3243_4233;
    double q2 = -a21*a3344_4334 + a23*a3144_4134 - a24*a3143_4133;
    double q3 =  a21*a3244_4234 - a22*a3144_4134 + a24*a3142_4132;
    double q4 = -a21*a3243_4233 + a22*a3143_4133 - a23*a3142_4132;
    double dq = sqrt(q1*q1 + q2*q2 + q3*q3 + q4*q4);

    quat[0] = q1/dq;
    quat[1] = q2/dq;
    quat[2] = q3/dq;
    quat[3] = q4/dq;

    return(lambdamax);
}


/* A lot of register variables, but this sort of thing scales very well with
   new and improved processors */
/* This version has the conditional to test whether the row of the cofactor matrix is too small.
   It goes through all four rows if necessary to find one that is large enough to avoid floating
   point error. */
static double
CalcQuarticCoeffsPu(const FragCds *frag1, const FragCds *frag2, const int len, double *coeff, const double outerprod, double *quat)
{
    double          Sxx, Sxy, Sxz, Syx, Syy, Syz, Szx, Szy, Szz;
    double          Szz2, Syy2, Sxx2, Sxy2, Syz2, Sxz2, Syx2, Szy2, Szx2,
                    SyzSzymSyySzz2, Sxx2Syy2Szz2Syz2Szy2, Sxy2Sxz2Syx2Szx2,
                    SxzpSzx, SyzpSzy, SxypSyx, SyzmSzy,
                    SxzmSzx, SxymSyx, SxxpSyy, SxxmSyy;
    double          x1, x2, y1, y2, z1, z2;
    const double   *fx1 = frag1->x, *fy1 = frag1->y, *fz1 = frag1->z,
                   *fx2 = frag2->x, *fy2 = frag2->y, *fz2 = frag2->z;
    int             i;
    double          lambdamax;
    double          precision = 1e-8; // 1e-14 is too stringent, can prevent convergence of QCProot.
//    double          precision2 = 1e-7;

    Sxx = Sxy = Sxz = Syx = Syy = Syz = Szx = Szy = Szz = 0.0;
    for (i = 0; i < len; ++i)
    {
        x1 = fx1[i];
        y1 = fy1[i];
        z1 = fz1[i];
        x2 = fx2[i];
        y2 = fy2[i];
        z2 = fz2[i];

        Sxx += (x1 * x2);
        Sxy += (x1 * y2);
        Sxz += (x1 * z2);

        Syx += (y1 * x2);
        Syy += (y1 * y2);
        Syz += (y1 * z2);

        Szx += (z1 * x2);
        Szy += (z1 * y2);
        Szz += (z1 * z2);
    }

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

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

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

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

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

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

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

    lambdamax = QCProot(coeff, 0.5 * outerprod, precision);

    /* Now calculate the optimal rotation from one row of the cofactor matrix */
    double a11, a12, a13, a14, a21, a22, a23, a24;
    double a31, a32, a33, a34, a41, a42, a43, a44;

    a11 = SxxpSyy + Szz - lambdamax;
    a12 = SyzmSzy;
    a13 = -SxzmSzx;
    a14 = SxymSyx;
    a21 = SyzmSzy;
    a22 = SxxmSyy - Szz - lambdamax;
    a23 = SxypSyx;
    a24 = SxzpSzx;
    a31 = a13;
    a32 = a23;
    a33 = Syy - Sxx - Szz - lambdamax;
    a34 = SyzpSzy;
    a41 = a14;
    a42 = a24;
    a43 = a34;
    a44 = Szz - SxxpSyy - lambdamax;

    double a3344_4334 = a33 * a44 - a43 * a34;
    double a3244_4234 = a32 * a44 - a42 * a34;
    double a3243_4233 = a32 * a43 - a42 * a33;
    double a3143_4133 = a31 * a43 - a41 * a33;
    double a3144_4134 = a31 * a44 - a41 * a34;
    double a3142_4132 = a31 * a42 - a41 * a32;
    double q1 =  a22*a3344_4334 - a23*a3244_4234 + a24*a3243_4233;
    double q2 = -a21*a3344_4334 + a23*a3144_4134 - a24*a3143_4133;
    double q3 =  a21*a3244_4234 - a22*a3144_4134 + a24*a3142_4132;
    double q4 = -a21*a3243_4233 + a22*a3143_4133 - a23*a3142_4132;

/*     q1 +=  a12*a3344_4334 - a13*a3244_4234 + a14*a3243_4233; */
/*     q2 += -a11*a3344_4334 + a13*a3144_4134 - a14*a3143_4133; */
/*     q3 +=  a11*a3244_4234 - a12*a3144_4134 + a14*a3142_4132; */
/*     q4 += -a11*a3243_4233 + a12*a3143_4133 - a13*a3142_4132; */
/*      */
/*     double a1324_1423 = a13 * a24 - a14 * a23, a1224_1422 = a12 * a24 - a14 * a22; */
/*     double a1223_1322 = a12 * a23 - a13 * a22, a1124_1421 = a11 * a24 - a14 * a21; */
/*     double a1123_1321 = a11 * a23 - a13 * a21, a1122_1221 = a11 * a22 - a12 * a21; */
/*      */
/*     q1 +=  a42 * a1324_1423 - a43 * a1224_1422 + a44 * a1223_1322; */
/*     q2 += -a41 * a1324_1423 + a43 * a1124_1421 - a44 * a1123_1321; */
/*     q3 +=  a41 * a1224_1422 - a42 * a1124_1421 + a44 * a1122_1221; */
/*     q4 += -a41 * a1223_1322 + a42 * a1123_1321 - a43 * a1122_1221; */
/*      */
/*     q1 +=  a32 * a1324_1423 - a33 * a1224_1422 + a34 * a1223_1322; */
/*     q2 += -a31 * a1324_1423 + a33 * a1124_1421 - a34 * a1123_1321; */
/*     q3 +=  a31 * a1224_1422 - a32 * a1124_1421 + a34 * a1122_1221; */
/*     q4 += -a31 * a1223_1322 + a32 * a1123_1321 - a33 * a1122_1221; */

    double qsqr = q1*q1 + q2 *q2 + q3*q3+q4*q4;

    if (qsqr < precision)
    {
        q1 =  a12*a3344_4334 - a13*a3244_4234 + a14*a3243_4233;
        q2 = -a11*a3344_4334 + a13*a3144_4134 - a14*a3143_4133;
        q3 =  a11*a3244_4234 - a12*a3144_4134 + a14*a3142_4132;
        q4 = -a11*a3243_4233 + a12*a3143_4133 - a13*a3142_4132;
        qsqr = q1*q1 + q2 *q2 + q3*q3+q4*q4;

        if (qsqr < precision)
        {
            double a1324_1423 = a13 * a24 - a14 * a23, a1224_1422 = a12 * a24 - a14 * a22;
            double a1223_1322 = a12 * a23 - a13 * a22, a1124_1421 = a11 * a24 - a14 * a21;
            double a1123_1321 = a11 * a23 - a13 * a21, a1122_1221 = a11 * a22 - a12 * a21;

            q1 =  a42 * a1324_1423 - a43 * a1224_1422 + a44 * a1223_1322;
            q2 = -a41 * a1324_1423 + a43 * a1124_1421 - a44 * a1123_1321;
            q3 =  a41 * a1224_1422 - a42 * a1124_1421 + a44 * a1122_1221;
            q4 = -a41 * a1223_1322 + a42 * a1123_1321 - a43 * a1122_1221;
            qsqr = q1*q1 + q2 *q2 + q3*q3+q4*q4;

            if (qsqr < precision)
            {
                q1 =  a32 * a1324_1423 - a33 * a1224_1422 + a34 * a1223_1322;
                q2 = -a31 * a1324_1423 + a33 * a1124_1421 - a34 * a1123_1321;
                q3 =  a31 * a1224_1422 - a32 * a1124_1421 + a34 * a1122_1221;
                q4 = -a31 * a1223_1322 + a32 * a1123_1321 - a33 * a1122_1221;
                qsqr = q1*q1 + q2 *q2 + q3*q3 + q4*q4;
            }
        }
    }

    double dq = sqrt(qsqr);

    quat[0] = q1/dq;
    quat[1] = q2/dq;
    quat[2] = q3/dq;
    quat[3] = q4/dq;

    return(lambdamax);
}


void
RotMatToQuaternion(const double **rot, double *quat)
{
    double              trace, s, w, x, y, z;

    /* convert to quaternion */
    trace = rot[0][0] + rot[1][1] + rot[2][2] + 1.0;

    if( trace > FLT_EPSILON )
    {
        s = 0.5 / sqrt(trace);
        w = 0.25 / s;
        x = ( rot[2][1] - rot[1][2] ) * s;
        y = ( rot[0][2] - rot[2][0] ) * s;
        z = ( rot[1][0] - rot[0][1] ) * s;
    }
    else
    {
        if (rot[0][0] > rot[1][1] && rot[0][0] > rot[2][2])
        {
            s = 2.0 * sqrt( 1.0 + rot[0][0] - rot[1][1] - rot[2][2]);
            x = 0.25 * s;
            y = (rot[0][1] + rot[1][0] ) / s;
            z = (rot[0][2] + rot[2][0] ) / s;
            w = (rot[1][2] - rot[2][1] ) / s;
        }
        else if (rot[1][1] > rot[2][2])
        {
            s = 2.0 * sqrt(1.0 + rot[1][1] - rot[0][0] - rot[2][2]);
            x = (rot[0][1] + rot[1][0] ) / s;
            y = 0.25 * s;
            z = (rot[1][2] + rot[2][1] ) / s;
            w = (rot[0][2] - rot[2][0] ) / s;
        }
        else
        {
            s = 2.0 * sqrt(1.0 + rot[2][2] - rot[0][0] - rot[1][1]);
            x = (rot[0][2] + rot[2][0] ) / s;
            y = (rot[1][2] + rot[2][1] ) / s;
            z = 0.25 * s;
            w = (rot[0][1] - rot[1][0] ) / s;
        }
    }

    quat[0] = -w;
    quat[1] = x;
    quat[2] = y;
    quat[3] = z;
}


/* returns the sum of the deviations
   rmsd = sqrt(sum_dev/atom_num) */
   /* rotmat checks out vs GSLSVD method */
double
QuarticHornFrag(const FragCds *frag1, const FragCds *frag2, double *coeff)
{
    double          frag1_radgyr;
    double          frag2_radgyr;
    double          rmsd2, lambdamax;
//    int             n = 1000;

    frag1_radgyr = RadGyrSqrFrag(frag1);
    frag2_radgyr = RadGyrSqrFrag(frag2);

    double         *quat = malloc(4 * sizeof(double));
    //double        **rotmat = MatAlloc(3, 3);
    lambdamax = CalcQuarticCoeffsPu(frag1, frag2, frag1->fraglen, coeff, frag1_radgyr + frag2_radgyr, quat);
    rmsd2 = (frag1_radgyr + frag2_radgyr) - (2.0 * lambdamax);
    //printf("\n% -14.6e % -14.6e % -14.6e % -14.6e", quat[0], quat[1], quat[2], quat[3]);
    //QuatToRotmat(rotmat, quat);
    //Mat3Print(rotmat);

//     double        **Rmat = MatAlloc(3, 3);
//     double        **Umat = MatAlloc(3, 3);
//     double        **VTmat = MatAlloc(3, 3);
//     double        *sigma = malloc(3 * sizeof(double));
//     double        **rotmat2 = MatAlloc(3, 3);

    //ProcGSLSVDFrag(frag1, frag2, rotmat2, Rmat, Umat, VTmat, sigma);

    //RotMatToQuaternion((const double **) rotmat2, quat);
    //printf("\n% -14.6e % -14.6e % -14.6e % -14.6e", quat[0], quat[1], quat[2], quat[3]);
    //Mat3Print(rotmat2);

    //if (Mat3Eq((const double **) rotmat, (const double **) rotmat2, 1e-8) == 0)
    //    printf("\n Not equal\n");

//     MatDestroy(&rotmat);
//     MatDestroy(&rotmat2);
//     MatDestroy(&Umat);
//     MatDestroy(&Rmat);
//     MatDestroy(&VTmat);

//    free(sigma);
    free(quat);

    //ComputeQuartHornFrag(frag1, frag2, coeff);
    //rmsd2 = horn_root(coeff, ((frag1_radgyr + frag2_radgyr) / 2.0), n, 1e-6);
    //rmsd2 = (frag1_radgyr + frag2_radgyr) - (2.0 * rmsd2);

    return (rmsd2);
}


void
FragDistPu(CdsArray *cdsA, int fraglen, int center_ca, int pu)
{
    int             coord1, coord2;
    int             i, k, offset, tmp, num, count;
    FragCds     *frag1 = NULL, *frag2 = NULL;
    double         *coeff = NULL;
    double          var;
    FILE           *distfile = NULL, *distfile2 = NULL;
    double         *array = NULL;

    double        **Rmat = MatAlloc(3, 3);
    double        **Umat = MatAlloc(3, 3);
    double        **VTmat = MatAlloc(3, 3);
    double        *sigma = malloc(3 * sizeof(double));
    double        **rotmat2 = MatAlloc(3, 3);

    double         *quat = malloc(4 * sizeof(double));
    double        **rotmat = MatAlloc(3, 3);

    double          frag1_radgyr;
    double          frag2_radgyr;
    double          rmsd2, lambdamax;
//  int             n = 1000;
    clock_t          start_time, end_time;
//    unsigned long   seed = (unsigned long) time(NULL);

    distfile = fopen("frags_dist.txt", "w");
    distfile2 = fopen("frags_dist2.txt", "w");
    coeff = (double *) calloc(5, sizeof(double));

    frag1 = FragCdsAlloc(fraglen);
    frag2 = FragCdsAlloc(fraglen);

    offset = (fraglen - 1) / 2;

    num = cdsA->vlen * cdsA->cnum * (cdsA->cnum - 1) / 2;

    array = (double *) calloc(num,  sizeof(double));

    start_time = clock();

    count = 0;
    for (coord1 = 0; coord1 < cdsA->cnum; ++coord1)
    {
        for (coord2 = coord1 + 1; coord2 < cdsA->cnum; ++coord2)
        {
            for (i = offset; i < cdsA->vlen - offset; ++i)
            {
                for (k = 0; k < fraglen; ++k)
                {
                    tmp = i + k - offset;

                    frag1->x[k] = cdsA->cds[coord1]->x[tmp];
                    frag1->y[k] = cdsA->cds[coord1]->y[tmp];
                    frag1->z[k] = cdsA->cds[coord1]->z[tmp];

                    frag2->x[k] = cdsA->cds[coord2]->x[tmp];
                    frag2->y[k] = cdsA->cds[coord2]->y[tmp];
                    frag2->z[k] = cdsA->cds[coord2]->z[tmp];
                }

                if (center_ca)
                {
                    CenterFragCA(frag1);
                    CenterFragCA(frag2);
                }
                else if (center_ca == 2)
                {
                    CenterFrag(frag1);
                    CenterFrag(frag2);
                }

                //var = QuarticHornFrag((const FragCds *) frag1, (const FragCds *) frag2, coeff);

                if (pu)
                {
                    frag1_radgyr = RadGyrSqrFrag(frag1);
                    frag2_radgyr = RadGyrSqrFrag(frag2);
                    lambdamax = CalcQuarticCoeffsPu(frag1, frag2, frag1->fraglen, coeff, frag1_radgyr + frag2_radgyr, quat);
                    rmsd2 = (frag1_radgyr + frag2_radgyr) - (2.0 * lambdamax);
                    //var = KabschFrag(frag1, frag2, rotmat2, Rmat, Umat, VTmat, sigma);
                    var = ProcGSLSVDFrag(frag1, frag2, rotmat2, Rmat, Umat, VTmat, sigma);
                    printf("\nQCP rmsd = %e %e\n", sqrt(rmsd2/fraglen), sqrt(var/fraglen));
                    QuatToRotmat(rotmat, quat);
                }
                else
                {
    //              printf("\n% -14.6e % -14.6e % -14.6e % -14.6e", quat[0], quat[1], quat[2], quat[3]);
    //              Mat3Print(rotmat);

                    var = ProcGSLSVDFrag(frag1, frag2, rotmat2, Rmat, Umat, VTmat, sigma);
                    //var = KabschFrag(frag1, frag2, rotmat2, Rmat, Umat, VTmat, sigma);
                    //var = ProcJacobiSVDFrag(frag1, frag2, rotmat2, Rmat, Umat, VTmat, sigma);
                    //RotMatToQuaternion((const double **) rotmat2, quat);

    //              printf("\n% -14.6e % -14.6e % -14.6e % -14.6e", quat[0], quat[1], quat[2], quat[3]);
    //              Mat3Print(rotmat2);
                }

//              if (Mat3Eq((const double **) rotmat, (const double **) rotmat2, 1e-8) == 0)
//              {
//                  printf("\n Not equal\n");
//                  Mat3Print(rotmat);
//                  Mat3Print(rotmat2);
//              }

//                fprintf(distfile, "%-16.8e \n", var);
//                fprintf(distfile2, "%-16.8e \n", sqrt(var));

//                if (var > biggest)
//                    biggest = var;

//                array[count] = sqrt(var);
                ++count;
            }
        }
    }

    end_time = clock();
    double milliseconds = (double) (end_time - start_time) / ((double) CLOCKS_PER_SEC * 0.001);

    printf( "\n milliseconds: %-16.8e --  iters = %d \n", milliseconds, count);

    MatDestroy(&rotmat);
    MatDestroy(&rotmat2);
    MatDestroy(&Umat);
    MatDestroy(&Rmat);
    MatDestroy(&VTmat);

    free(sigma);
    free(quat);

    FragCdsFree(&frag1);
    FragCdsFree(&frag2);
    free(coeff);
    fclose(distfile);
    fclose(distfile2);
    free(array);

    exit(EXIT_SUCCESS);
}

