/*=======================================================================

    Miscellaneous routines for manipulating and comparing structures.
  
    Copyright 1993 (C) David Hinds - All Rights Reserved
    
    Source file coord.c Version 1.2, Updated 3/15/93
  
=======================================================================*/

#include <math.h>
#include <stdio.h>

#define COORD_C
#include "utypes.h"
#include "errors.h"
#include "umath.h"
#include "vectors.h"
#include "coord.h"

static const char ident_c[] = "@(#) coord.c version 1.2 (3/15/93)";

/*=======================================================================

    Some I/O functions
    
=======================================================================*/

void fputpdb(FILE *f, CoordType *a, const int_t npts)
  {
  int_t i;
  for (i = 0; i < npts; i++)
    fprintf(f, "ATOM %6d  CA  ALA  %4d    %7.3f %7.3f %7.3f\n",
            i+1, i+1, a[i].x, a[i].y, a[i].z);
  } /* fputpdb */
  
/*=======================================================================

    A function to uniformly scale a structure.
    
=======================================================================*/

void resize(CoordType *a, const int_t n, const float_t scale)
  {
  int_t i;
  for (i = 0; i < n; i++) {
    a[i].x *= scale;
    a[i].y *= scale;
    a[i].z *= scale;
    }
  } /* resize */
  
/*=======================================================================

    A function to invert a structure.
    
=======================================================================*/

void invert(CoordType *a, const int_t n)
  {
  int_t i;
  for (i = 0; i < n; i++) a[i].z = -a[i].z;
  } /* invert */
  
/*=======================================================================

    A function to apply a rotation matrix to a structure.
    
=======================================================================*/

void Hinds_rotate(CoordType *a, const int_t n, rotate_t u)
  {
  int_t i;
  float_t x, y, z;
  for (i = 0; i < n; i++) {
    x = u[0][0]*a[i].x + u[0][1]*a[i].y + u[0][2]*a[i].z;
    y = u[1][0]*a[i].x + u[1][1]*a[i].y + u[1][2]*a[i].z;
    z = u[2][0]*a[i].x + u[2][1]*a[i].y + u[2][2]*a[i].z;
    a[i].x = x; a[i].y = y; a[i].z = z;
    }
  } /* rotate */
  
/*=======================================================================

    A function to translate a structure by a vector.
    
=======================================================================*/

void Hinds_translate(CoordType *a, const int_t n, const xlate_t t)
  {
  int_t i;
  for (i = 0; i < n; i++) {
    a[i].x += t[0];
    a[i].y += t[1];
    a[i].z += t[2];
    }
  } /* translate */

/*=======================================================================

    A function to calculate the distance RMS between two structures.
    
=======================================================================*/

float_t distrms(CoordType *a, CoordType *b, const int_t npts)
  {
  int_t i, j;
  float_t ad, bd, sum;

  sum = 0.0;
  for (i = 0; i < npts; i++) {
    for (j = 0; j < i; j++) {
      ad = fsqrt(SQR(a[i].x-a[j].x) + SQR(a[i].y-a[j].y) +
                 SQR(a[i].z-a[j].z));
      bd = fsqrt(SQR(b[i].x-b[j].x) + SQR(b[i].y-b[j].y) +
                 SQR(b[i].z-b[j].z));
      sum += SQR(ad-bd);
      }
    }
  return sqrt(2*sum/(npts*(npts-1)));
  
  } /* distrms */
  
/*=======================================================================

    A function to calculate the lopsidedness of a structure.
    
=======================================================================*/

float_t lopside(CoordType *a, int_t npts, float_t vol)
  {
  int_t i;
  float_t sx[3], sx2, t;
  static const pi = 3.141592653589793;

  sx[0] = sx[1] = sx[2] = sx2 = 0;
  for (i = 0; i < npts; i++) {
    sx[0] += a[i].x; sx[1] += a[i].y; sx[2] += a[i].z;
    sx2 += SQR(a[i].x) + SQR(a[i].y) + SQR(a[i].z);
    }
  t = sx2 - (SQR(sx[0])+SQR(sx[1])+SQR(sx[2]))/npts;
  return sqrt(t/(npts*0.6))/pow(0.75*vol*npts/pi, 1.0/3.0);
  } /* lopsid */
  
/*=======================================================================

    Functions to calculate the coordinate RMS deviation between two
    arbitrary structures.

    This is adapted from Fortran code as described in:
    W. Kabsch (1978).  Acta Cryst. A34, 827-828.
    W. Kabsch (1976).  Acta Cryst. A32, 922-923.

    'fitrms' does a simple RMS calculation.  'bothrms' calculates
    the RMS differences for both a structure and its mirror image.
    'bestrms' returns just the best of the two.
    
=======================================================================*/

static int_t solve(dbl_t e[3],
                   dbl_t e0, dbl_t det, dbl_t spur, dbl_t cof)
  {
  dbl_t d, h, g, cth, sth, sqrth;
#define epsilon 1e-7
#define sqrt3 1.7320508075688772
  
  det *= det;
  d = SQR(spur);
  h = d - cof;
  g = spur * (cof*1.5 - d) - det*0.5;
  if (h > d*epsilon) {
    sqrth = SQRTABS(h);
    d = -g/(h*sqrth);
    if (d > 1-epsilon) {    /* Two identical roots */
      e[0] = spur + 2*sqrth;
      e[2] = e[1] = spur - sqrth;
      return 2;
      }
    else if (d < -1+epsilon) {    /* Two identical roots */ 
      e[1] = e[0] = spur+sqrth;
      e[2] = MAX(0, spur - 2*sqrth);
      return 3;
      }
    else {    /* Three distinct roots */
      d = acos(d)/3.0;
      cth = sqrth*cos(d); sth = sqrth*sqrt3*sin(d);
      e[0] = spur + 2*cth;
      e[1] = spur - cth + sth;
      e[2] = MAX(0, spur - cth - sth);
      return 1;
      }
    }
  else {    /* Three identical roots */ 
    e[0] = e[1] = e[2] = spur;
    return 4;
    }

  } /* solve */

/*=====================================================================*/

static float_t justrms(dbl_t e0, dbl_t det, dbl_t spur, dbl_t cof)
  {
  dbl_t d, e[3];
  
  solve(e, e0, det, spur, cof);
  d = SQRTABS(e[2]);
  if (det < 0) d = -d;
  d += SQRTABS(e[1]) + SQRTABS(e[0]);
  d = e0-2*d;
  return SQRTABS(d);
  } /* justrms */

/*=====================================================================*/

static void justbothrms(float_t *r1, float_t *r2,
                        dbl_t e0, dbl_t det, dbl_t spur, dbl_t cof)
  {
  dbl_t d1, d2, e[3];

  solve(e, e0, det, spur, cof);
  d1 = 2*SQRTABS(e[2]);
  d2 = e0 - 2*(SQRTABS(e[1]) + SQRTABS(e[0]));
  if (det < 0) {
    *r1 = SQRTABS(d2+d1);
    *r2 = SQRTABS(d2-d1);
    }
  else {
    *r1 = SQRTABS(d2-d1);
    *r2 = SQRTABS(d2+d1);
    }
  } /* justbothrms */
  
/*=====================================================================*/

float_t fitrms(CoordType *a, CoordType *b, const int_t npts)
  {
  dbl_t sx[3], sy[3], sxy[3][3], sx2, sy2;
  dbl_t r00, r01, r02, r10, r11, r12, r20, r21, r22;
  dbl_t rr0, rr1, rr2, rr3, rr4, rr5;
  register dbl_t ss, spur, det, cof, e0, inpts;

  sx[0] = vsumby3(&a[0].x, npts);
  sx[1] = vsumby3(&a[0].y, npts);
  sx[2] = vsumby3(&a[0].z, npts);
  sy[0] = vsumby3(&b[0].x, npts);
  sy[1] = vsumby3(&b[0].y, npts);
  sy[2] = vsumby3(&b[0].z, npts);
  sx2 = vsumsqr(&a[0].x, 3*npts);
  sy2 = vsumsqr(&b[0].x, 3*npts);
  sxy[0][0] = vdotby3(&a[0].x, &b[0].x, npts);
  sxy[0][1] = vdotby3(&a[0].x, &b[0].y, npts);
  sxy[0][2] = vdotby3(&a[0].x, &b[0].z, npts);
  sxy[1][0] = vdotby3(&a[0].y, &b[0].x, npts);
  sxy[1][1] = vdotby3(&a[0].y, &b[0].y, npts);
  sxy[1][2] = vdotby3(&a[0].y, &b[0].z, npts);
  sxy[2][0] = vdotby3(&a[0].z, &b[0].x, npts);
  sxy[2][1] = vdotby3(&a[0].z, &b[0].y, npts);
  sxy[2][2] = vdotby3(&a[0].z, &b[0].z, npts);

  inpts = 1.0/((dbl_t)npts);
  e0 = (sx2 - (SQR(sx[0])+SQR(sx[1])+SQR(sx[2]))*inpts +
        sy2 - (SQR(sy[0])+SQR(sy[1])+SQR(sy[2]))*inpts)*inpts;

  ss = sx[0]*inpts;
  r00 = (sxy[0][0] - ss * sy[0]) * inpts;
  r01 = (sxy[0][1] - ss * sy[1]) * inpts;
  r02 = (sxy[0][2] - ss * sy[2]) * inpts;
  ss = sx[1]*inpts;
  r10 = (sxy[1][0] - ss * sy[0]) * inpts;
  r11 = (sxy[1][1] - ss * sy[1]) * inpts;
  r12 = (sxy[1][2] - ss * sy[2]) * inpts;
  ss = sx[2]*inpts;
  r20 = (sxy[2][0] - ss * sy[0]) * inpts;
  r21 = (sxy[2][1] - ss * sy[1]) * inpts;
  r22 = (sxy[2][2] - ss * sy[2]) * inpts;

  det = r00 * (r11*r22 - r21*r12)
      - r10 * (r01*r22 - r21*r02)
      + r20 * (r01*r12 - r11*r02);

  rr0 = SQR(r00) + SQR(r01) + SQR(r02);
  rr2 = SQR(r10) + SQR(r11) + SQR(r12);
  rr5 = SQR(r20) + SQR(r21) + SQR(r22);

  spur = (rr0+rr2+rr5)/3.0;
  cof = rr2*rr5 + rr0*rr5 + rr0*rr2;
  
  rr1 = r00*r10 + r01*r11 + r02*r12;
  rr3 = r00*r20 + r01*r21 + r02*r22;
  rr4 = r10*r20 + r11*r21 + r12*r22;
  
  cof = (cof - SQR(rr1) - SQR(rr3) - SQR(rr4))/3.0;

  return justrms(e0, det, spur, cof);
  
  } /* fitrms */

/*=====================================================================*/

void bothrms(CoordType *a, CoordType *b, const int_t npts,
               float_t *rms, float_t *invrms)
  {
  dbl_t sx[3], sy[3], sxy[3][3], sx2, sy2;
  dbl_t r00, r01, r02, r10, r11, r12, r20, r21, r22;
  dbl_t rr0, rr1, rr2, rr3, rr4, rr5;
  register dbl_t ss, spur, det, cof, e0, inpts;

  /* I think this is a reasonably efficient way of doing this */
  sx[0] = vsumby3(&a[0].x, npts);
  sx[1] = vsumby3(&a[0].y, npts);
  sx[2] = vsumby3(&a[0].z, npts);
  sy[0] = vsumby3(&b[0].x, npts);
  sy[1] = vsumby3(&b[0].y, npts);
  sy[2] = vsumby3(&b[0].z, npts);
  sx2 = vsumsqr(&a[0].x, 3*npts);
  sy2 = vsumsqr(&b[0].x, 3*npts);
  sxy[0][0] = vdotby3(&a[0].x, &b[0].x, npts);
  sxy[0][1] = vdotby3(&a[0].x, &b[0].y, npts);
  sxy[0][2] = vdotby3(&a[0].x, &b[0].z, npts);
  sxy[1][0] = vdotby3(&a[0].y, &b[0].x, npts);
  sxy[1][1] = vdotby3(&a[0].y, &b[0].y, npts);
  sxy[1][2] = vdotby3(&a[0].y, &b[0].z, npts);
  sxy[2][0] = vdotby3(&a[0].z, &b[0].x, npts);
  sxy[2][1] = vdotby3(&a[0].z, &b[0].y, npts);
  sxy[2][2] = vdotby3(&a[0].z, &b[0].z, npts);
  
  inpts = 1.0/((dbl_t)npts);
  e0 = (sx2 - (SQR(sx[0])+SQR(sx[1])+SQR(sx[2]))*inpts +
        sy2 - (SQR(sy[0])+SQR(sy[1])+SQR(sy[2]))*inpts)*inpts;

  ss = sx[0]*inpts;
  r00 = (sxy[0][0] - ss * sy[0]) * inpts;
  r01 = (sxy[0][1] - ss * sy[1]) * inpts;
  r02 = (sxy[0][2] - ss * sy[2]) * inpts;
  ss = sx[1]*inpts;
  r10 = (sxy[1][0] - ss * sy[0]) * inpts;
  r11 = (sxy[1][1] - ss * sy[1]) * inpts;
  r12 = (sxy[1][2] - ss * sy[2]) * inpts;
  ss = sx[2]*inpts;
  r20 = (sxy[2][0] - ss * sy[0]) * inpts;
  r21 = (sxy[2][1] - ss * sy[1]) * inpts;
  r22 = (sxy[2][2] - ss * sy[2]) * inpts;

  det = r00 * (r11*r22 - r21*r12)
      - r10 * (r01*r22 - r21*r02)
      + r20 * (r01*r12 - r11*r02);

  rr0 = SQR(r00) + SQR(r01) + SQR(r02);
  rr2 = SQR(r10) + SQR(r11) + SQR(r12);
  rr5 = SQR(r20) + SQR(r21) + SQR(r22);

  spur = (rr0+rr2+rr5)/3.0;
  cof = rr2*rr5 + rr0*rr5 + rr0*rr2;
  
  rr1 = r00*r10 + r01*r11 + r02*r12;
  rr3 = r00*r20 + r01*r21 + r02*r22;
  rr4 = r10*r20 + r11*r21 + r12*r22;
  
  cof = (cof - SQR(rr1) - SQR(rr3) - SQR(rr4))/3.0;

  justbothrms(rms, invrms, e0, det, spur, cof);
  
  } /* bothrms */

/*=====================================================================*/

float_t bestrms(CoordType *x, CoordType *y, const int_t npts)
  {
  float_t r, ir;
  bothrms(x, y, npts, &r, &ir);
  return MIN(r, ir);
  } /* bestrms */
  
/*=======================================================================

    This calculates rotation and translation matrices for superimposing
    one structure on top of another, in addition to an RMS deviation.
    
=======================================================================*/
  
static void matrix(rotate_t u, xlate_t t,
                   int_t sw, dbl_t e[3], dbl_t rr[6], dbl_t r[3][3],
                   dbl_t sx[3], dbl_t sy[3], int_t npts)
  {
  dbl_t ss[6], d, h, p, a[3][3], b[3][3];
  int_t i, j, l, m, m1, m2, m3;

  if (sw == 4) {
    /* Case of three identical roots */
    for (i = 0; i < 3; i++)
      for (j = 0; j < 3; j++)
        if (i == j) a[i][j] = 1.0; else a[i][j] = 0.0;
    }
  else {
    if (sw == 1) {
      /* Case of three distinct roots */
      for (l=0; l < 2; l++) {
        d = e[l];
        ss[0] = (d-rr[2]) * (d-rr[5]) - SQR(rr[4]);
        ss[1] = (d-rr[5]) * rr[1] + rr[3]*rr[4];
        ss[2] = (d-rr[0]) * (d-rr[5]) - SQR(rr[3]);
        ss[3] = (d-rr[2]) * rr[3] + rr[1]*rr[4];
        ss[4] = (d-rr[0]) * rr[4] + rr[1]*rr[3];
        ss[5] = (d-rr[0]) * (d-rr[2]) - SQR(rr[1]);
        if (FABS(ss[0]) >= FABS(ss[2])) {
          if (FABS(ss[0]) >= FABS(ss[5])) {
            a[0][l] = ss[0]; a[1][l] = ss[1]; a[2][l] = ss[3];
            }
          else {
            a[0][l] = ss[3]; a[1][l] = ss[4]; a[2][l] = ss[5];
            }
          }
        else if (FABS(ss[2]) >= FABS(ss[5])) {
          a[0][l] = ss[1]; a[1][l] = ss[2]; a[2][l] = ss[4];
          }
        else {
          a[0][l] = ss[3]; a[1][l] = ss[4]; a[2][l] = ss[5];
          }
        d = sqrt(SQR(a[0][l]) + SQR(a[1][l]) + SQR(a[2][l]));
        a[0][l] /= d; a[1][l] /= d; a[2][l] /= d;
        }
      m1 = 2; m2 = 0; m3 = 1;
      }
    else {
      /* Cases of two distinct roots */
      if (sw == 2) {
        m = 0; m1 = 2; m2 = 0; m3 = 1;
        }
      else {
        m = 2; m1 = 0; m2 = 1; m3 = 2;
        }
      h = e[2];
      a[0][1] = 1.0; a[1][1] = 1.0; a[2][1] = 1.0;
      if (FABS(rr[0]-h) > FABS(rr[2]-h)) {
        if (FABS(rr[0]-h) > FABS(rr[5]-h)) {
          a[0][m] = rr[0]-h; a[1][m] = rr[1]; a[2][m] = rr[3];
          p = -(rr[0] + rr[1] + rr[3] - h);
          a[0][1] = p/a[0][m];
          }
        else {
          a[0][m] = rr[3]; a[1][m] = rr[4]; a[2][m] = rr[5]-h;
          p = -(rr[3] + rr[4] + rr[5] - h);
          a[2][1] = p/a[2][m];
          }
        }
      else {
        if (FABS(rr[2]-h) > FABS(rr[5]-h)) {
          a[0][m] = rr[1]; a[1][m] = rr[2]-h; a[2][m] = rr[4];
          p = -(rr[1] + rr[2] + rr[4] - h);
          a[1][1] = p/a[1][m];
          }
        else {
          a[0][m] = rr[3]; a[1][m] = rr[4]; a[2][m] = rr[5]-h;
          p = -(rr[3] + rr[4] + rr[5] - h);
          a[2][1] = p/a[2][m];
          }
        }
      d = sqrt(SQR(a[0][m]) + SQR(a[1][m]) + SQR(a[2][m]));
      p = sqrt(SQR(a[0][1]) + SQR(a[1][1]) + SQR(a[2][1]));
      for (i = 0; i < 3; i++) {
        a[i][1] /= p;
        a[i][m] /= d;
        }
      }
    /* Common for either two or three distinct roots */
    a[0][m1] = a[1][m2]*a[2][m3] - a[1][m3]*a[2][m2];
    a[1][m1] = a[2][m2]*a[0][m3] - a[2][m3]*a[0][m2];
    a[2][m1] = a[0][m2]*a[1][m3] - a[0][m3]*a[1][m2];
    }
  
  for (l = 0; l < 2; l++) {
    d = 0;
    for (i = 0; i < 3; i++) {
      b[i][l] = r[0][i]*a[0][l] + r[1][i]*a[1][l] + r[2][i]*a[2][l];
      d += SQR(b[i][l]);
      }
    d = sqrt(d);
    if (d == 0) error("degenerate rotation matrix in matrix()\n");
    for (i = 0; i < 3; i++) b[i][l] /= d;
    }
  b[0][2] = b[1][0]*b[2][1] - b[1][1]*b[2][0];
  b[1][2] = b[2][0]*b[0][1] - b[2][1]*b[0][0];
  b[2][2] = b[0][0]*b[1][1] - b[0][1]*b[1][0];
  
  /* Calculate rotation matrix */
  for (i = 0; i < 3; i++)
    for (j = 0; j < 3; j++)
      u[i][j] = b[i][0]*a[j][0] + b[i][1]*a[j][1] + b[i][2]*a[j][2];
  /* Calculate translation vector */
  for (i = 0; i < 3; i++)
    t[i] = (sy[i] - u[i][0]*sx[0] - u[i][1]*sx[1] - u[i][2]*sx[2])/npts;
  }

/*=====================================================================*/

float_t bestfit(rotate_t u, xlate_t t,
                CoordType *a, CoordType *b, const int_t npts)
  {
  dbl_t sx[3], sy[3], sxy[3][3], sx2, sy2;
  dbl_t r[3][3], rr[6];
  dbl_t d, spur, det, cof, e0, e[3], inpts;
  int_t i, j, m, sw;

  sx[0] = vsumby3(&a[0].x, npts);
  sx[1] = vsumby3(&a[0].y, npts);
  sx[2] = vsumby3(&a[0].z, npts);
  sy[0] = vsumby3(&b[0].x, npts);
  sy[1] = vsumby3(&b[0].y, npts);
  sy[2] = vsumby3(&b[0].z, npts);
  sx2 = vsumsqr(&a[0].x, 3*npts);
  sy2 = vsumsqr(&b[0].x, 3*npts);
  sxy[0][0] = vdotby3(&a[0].x, &b[0].x, npts);
  sxy[0][1] = vdotby3(&a[0].x, &b[0].y, npts);
  sxy[0][2] = vdotby3(&a[0].x, &b[0].z, npts);
  sxy[1][0] = vdotby3(&a[0].y, &b[0].x, npts);
  sxy[1][1] = vdotby3(&a[0].y, &b[0].y, npts);
  sxy[1][2] = vdotby3(&a[0].y, &b[0].z, npts);
  sxy[2][0] = vdotby3(&a[0].z, &b[0].x, npts);
  sxy[2][1] = vdotby3(&a[0].z, &b[0].y, npts);
  sxy[2][2] = vdotby3(&a[0].z, &b[0].z, npts);

  inpts = 1.0/npts;
  e0 = (sx2 - (SQR(sx[0])+SQR(sx[1])+SQR(sx[2]))*inpts +
        sy2 - (SQR(sy[0])+SQR(sy[1])+SQR(sy[2]))*inpts)*inpts;

  for (i = 0; i < 3; i++) {
    for (j = 0; j < 3; j++) {
      r[i][j] = (sxy[i][j] - sx[i]*sy[j]*inpts)*inpts;
      }
    }
    
  for (m = j = 0; j < 3; j++) {
    for (i = 0; i <= j; i++,m++) {
      rr[m] = r[i][0]*r[j][0] + r[i][1]*r[j][1] + r[i][2]*r[j][2];
      }
    }

  det = r[0][0] * (r[1][1]*r[2][2] - r[2][1]*r[1][2])
      - r[1][0] * (r[0][1]*r[2][2] - r[2][1]*r[0][2])
      + r[2][0] * (r[0][1]*r[1][2] - r[1][1]*r[0][2]);

  spur = (rr[0]+rr[2]+rr[5])/3.0;
  cof = (rr[2]*rr[5] - SQR(rr[4]) + rr[0]*rr[5] -
         SQR(rr[3]) + rr[0]*rr[2] - SQR(rr[1]))/3.0;

  sw = solve(e, e0, det, spur, cof);
  matrix(u, t, sw, e, rr, r, sx, sy, npts);
  d = SQRTABS(e[2]);
  if (det < 0) d = -d;
  d += SQRTABS(e[1]) + SQRTABS(e[0]);
  d = e0-2*d;
  return SQRTABS(d);

  } /* bestfit */