/*
    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 <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\n WARNING: Newton-Raphson root-finding in \'QCProot()\' did not converge \n");

    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 innerprod, 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 * innerprod, 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;
    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 innerprod, 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-14;
//    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 * innerprod, 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);
}


static 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) */
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);
}


#include "FragCds.h"

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          biggest;
    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();

    biggest = 0.0;
    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 == 1)
                {
                    CenterFragCA(frag1);
                    CenterFragCA(frag2);
                }
                else if (center_ca == 2)
                {
                    CenterFrag(frag1);
                    CenterFrag(frag2);
                }

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

                if (pu == 1)
                {
					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);
}

