// pmx  Copyright Notice
// ============================
//
// The pmx source code is copyrighted, but you can freely use and
// copy it as long as you don't change or remove any of the copyright
// notices.
//
// ----------------------------------------------------------------------
// pmx is Copyright (C) 2006-2011 by Daniel Seeliger
//
//                        All Rights Reserved
//
// Permission to use, copy, modify, distribute, and distribute modified
// versions of this software and its documentation for any purpose and
// without fee is hereby granted, provided that the above copyright
// notice appear in all copies and that both the copyright notice and
// this permission notice appear in supporting documentation, and that
// the name of Daniel Seeliger not be used in advertising or publicity
// pertaining to distribution of the software without specific, written
// prior permission.
//
// DANIEL SEELIGER DISCLAIMS ALL WARRANTIES WITH REGARD TO THIS
// SOFTWARE, INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND
// FITNESS.  IN NO EVENT SHALL DANIEL SEELIGER BE LIABLE FOR ANY
// SPECIAL, INDIRECT OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER
// RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF
// CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
// CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
// ----------------------------------------------------------------------
#ifndef GEOMETRY_H
#define GEOMETRY_H
#include <pmx.h>


static void Error( char *msg )
{
  fprintf(stderr,"\nERROR in pmx c-layer: %s\n\n", msg);
  exit(1);
}

static inline real sqr(real x)
{
  return (x*x);
}


static inline void rvec_add(const rvec a,const rvec b,rvec c)
{
  real x,y,z;
  
  x=a[XX]+b[XX];
  y=a[YY]+b[YY];
  z=a[ZZ]+b[ZZ];
  
  c[XX]=x;
  c[YY]=y;
  c[ZZ]=z;
}

static inline void ivec_add(const ivec a,const ivec b,ivec c)
{
  int x,y,z;
  
  x=a[XX]+b[XX];
  y=a[YY]+b[YY];
  z=a[ZZ]+b[ZZ];
  
  c[XX]=x;
  c[YY]=y;
  c[ZZ]=z;
}

static inline void rvec_inc(rvec a,const rvec b)
{
  real x,y,z;
  
  x=a[XX]+b[XX];
  y=a[YY]+b[YY];
  z=a[ZZ]+b[ZZ];
  
  a[XX]=x;
  a[YY]=y;
  a[ZZ]=z;
}

static inline void rvec_sub(const rvec a,const rvec b,rvec c)
{
  real x,y,z;
  
  x=a[XX]-b[XX];
  y=a[YY]-b[YY];
  z=a[ZZ]-b[ZZ];
  
  c[XX]=x;
  c[YY]=y;
  c[ZZ]=z;
}

static inline void rvec_dec(rvec a,const rvec b)
{
  real x,y,z;
  
  x=a[XX]-b[XX];
  y=a[YY]-b[YY];
  z=a[ZZ]-b[ZZ];
  
  a[XX]=x;
  a[YY]=y;
  a[ZZ]=z;
}

static inline void copy_rvec(const rvec a,rvec b)
{
  b[XX]=a[XX];
  b[YY]=a[YY];
  b[ZZ]=a[ZZ];
}

static inline void copy_ivec(const ivec a,ivec b)
{
  b[XX]=a[XX];
  b[YY]=a[YY];
  b[ZZ]=a[ZZ];
}

static inline void ivec_sub(const ivec a,const ivec b,ivec c)
{
  int x,y,z;
  
  x=a[XX]-b[XX];
  y=a[YY]-b[YY];
  z=a[ZZ]-b[ZZ];
  
  c[XX]=x;
  c[YY]=y;
  c[ZZ]=z;
}

static inline void copy_mat(matrix a,matrix b)
{
  copy_rvec(a[XX],b[XX]);
  copy_rvec(a[YY],b[YY]);
  copy_rvec(a[ZZ],b[ZZ]);
}

static inline void svmul(real a,const rvec v1,rvec v2)
{
  v2[XX]=a*v1[XX];
  v2[YY]=a*v1[YY];
  v2[ZZ]=a*v1[ZZ];
}

static inline real distance2(const rvec v1,const rvec v2)
{
  return sqr(v2[XX]-v1[XX]) + sqr(v2[YY]-v1[YY]) + sqr(v2[ZZ]-v1[ZZ]);
}

static inline real dist(const rvec v1,const rvec v2)
{
  return sqrt( sqr(v2[XX]-v1[XX]) + sqr(v2[YY]-v1[YY]) + sqr(v2[ZZ]-v1[ZZ]) );
}


static inline void clear_rvec(rvec a)
{
  /* The ibm compiler has problems with inlining this 
   * when we use a const real variable
   */
  a[XX]=0.0;
  a[YY]=0.0;
  a[ZZ]=0.0;
}

static inline void clear_ivec(ivec a)
{
  a[XX]=0;
  a[YY]=0;
  a[ZZ]=0;
}

static inline void clear_mat(matrix a)
{
/*  memset(a[0],0,DIM*DIM*sizeof(a[0][0])); */
  
  const real nul=0.0;
  
  a[XX][XX]=a[XX][YY]=a[XX][ZZ]=nul;
  a[YY][XX]=a[YY][YY]=a[YY][ZZ]=nul;
  a[ZZ][XX]=a[ZZ][YY]=a[ZZ][ZZ]=nul;
}

static inline real iprod(const rvec a,const rvec b)
{
  return (a[XX]*b[XX]+a[YY]*b[YY]+a[ZZ]*b[ZZ]);
}


static inline int iiprod(const ivec a,const ivec b)
{
  return (a[XX]*b[XX]+a[YY]*b[YY]+a[ZZ]*b[ZZ]);
}

static inline real norm2(const rvec a)
{
  return a[XX]*a[XX]+a[YY]*a[YY]+a[ZZ]*a[ZZ];
}

static inline real norm(const rvec a)
{
  return (real)sqrt(a[XX]*a[XX]+a[YY]*a[YY]+a[ZZ]*a[ZZ]);
}

static inline real cos_angle(const rvec a,const rvec b)
{
  /* This version does not need the invsqrt lookup table */
  real   cosval;
  int    m;
  double aa,bb,ip,ipa,ipb; /* For accuracy these must be double! */
  
  ip=ipa=ipb=0.0;
  for(m=0; (m<DIM); m++) {		/* 18		*/
    aa   = a[m];
    bb   = b[m];
    ip  += aa*bb;
    ipa += aa*aa;
    ipb += bb*bb;
  }
  cosval=ip/sqrt(ipa*ipb); 		/* 12		*/
					/* 30 TOTAL	*/
  if (cosval > 1.0) 
    return  1.0; 
  if (cosval <-1.0) 
    return -1.0;
  
  return cosval;
}

static inline real angle_ijk(const rvec a,const rvec b, const rvec c)
{
  rvec ba;
  rvec bc;
  rvec_sub(b,a,ba);
  rvec_sub(b,c,bc);
  return acos (cos_angle(ba,bc) );
}

static inline real cos_angle_ijk(const rvec a,const rvec b, const rvec c)
{
  rvec ba;
  rvec bc;
  rvec_sub(b,a,ba);
  rvec_sub(b,c,bc);
  return cos_angle(ba,bc) ;
}

static inline void cprod(const rvec a,const rvec b,rvec c)
{
  c[XX]=a[YY]*b[ZZ]-a[ZZ]*b[YY];
  c[YY]=a[ZZ]*b[XX]-a[XX]*b[ZZ];
  c[ZZ]=a[XX]*b[YY]-a[YY]*b[XX];
}

static inline real dihedral(const rvec xi,const rvec xj,const rvec xk,const rvec xl)

{
  real ipr,phi,cos_phi,sign;
  rvec r_ij,r_kj,r_kl,m,n;
  
  rvec_sub(xi,xj,r_ij);  
  rvec_sub(xj,xk,r_kj);  
  rvec_sub(xk,xl,r_kl);  

  cprod(r_ij,r_kj,m);        
  cprod(r_kj,r_kl,n);        
  cos_phi=cos_angle(m,n);
  if(cos_phi < -1.) cos_phi = -1.;
  else if(cos_phi > 1.) cos_phi = 1.;
  phi=acos(cos_phi);         
  ipr=iprod(r_ij,n);         
  sign=(ipr>0.0)?-1.0:1.0;
  phi=sign*phi;              
                             
  return phi;
}

static inline void copy_x(rvec *x, rvec *dest, int n)
{
  int i;
  for(i=0;i<n;i++){
    copy_rvec( x[i], dest[i]);
  }
}

static inline void mmul_ur0(matrix a,matrix b,matrix dest)
{
  dest[XX][XX]=a[XX][XX]*b[XX][XX];
  dest[XX][YY]=0.0;
  dest[XX][ZZ]=0.0;
  dest[YY][XX]=a[YY][XX]*b[XX][XX]+a[YY][YY]*b[YY][XX];
  dest[YY][YY]=                    a[YY][YY]*b[YY][YY];
  dest[YY][ZZ]=0.0;
  dest[ZZ][XX]=a[ZZ][XX]*b[XX][XX]+a[ZZ][YY]*b[YY][XX]+a[ZZ][ZZ]*b[ZZ][XX];
  dest[ZZ][YY]=                    a[ZZ][YY]*b[YY][YY]+a[ZZ][ZZ]*b[ZZ][YY];
  dest[ZZ][ZZ]=                                        a[ZZ][ZZ]*b[ZZ][ZZ];
}

static inline void mmul(matrix a,matrix b,matrix dest)
{
  dest[XX][XX]=a[XX][XX]*b[XX][XX]+a[XX][YY]*b[YY][XX]+a[XX][ZZ]*b[ZZ][XX];
  dest[YY][XX]=a[YY][XX]*b[XX][XX]+a[YY][YY]*b[YY][XX]+a[YY][ZZ]*b[ZZ][XX];
  dest[ZZ][XX]=a[ZZ][XX]*b[XX][XX]+a[ZZ][YY]*b[YY][XX]+a[ZZ][ZZ]*b[ZZ][XX];
  dest[XX][YY]=a[XX][XX]*b[XX][YY]+a[XX][YY]*b[YY][YY]+a[XX][ZZ]*b[ZZ][YY];
  dest[YY][YY]=a[YY][XX]*b[XX][YY]+a[YY][YY]*b[YY][YY]+a[YY][ZZ]*b[ZZ][YY];
  dest[ZZ][YY]=a[ZZ][XX]*b[XX][YY]+a[ZZ][YY]*b[YY][YY]+a[ZZ][ZZ]*b[ZZ][YY];
  dest[XX][ZZ]=a[XX][XX]*b[XX][ZZ]+a[XX][YY]*b[YY][ZZ]+a[XX][ZZ]*b[ZZ][ZZ];
  dest[YY][ZZ]=a[YY][XX]*b[XX][ZZ]+a[YY][YY]*b[YY][ZZ]+a[YY][ZZ]*b[ZZ][ZZ];
  dest[ZZ][ZZ]=a[ZZ][XX]*b[XX][ZZ]+a[ZZ][YY]*b[YY][ZZ]+a[ZZ][ZZ]*b[ZZ][ZZ];
}

static inline void transpose(matrix src,matrix dest)
{
  dest[XX][XX]=src[XX][XX];
  dest[YY][XX]=src[XX][YY];
  dest[ZZ][XX]=src[XX][ZZ];
  dest[XX][YY]=src[YY][XX];
  dest[YY][YY]=src[YY][YY];
  dest[ZZ][YY]=src[YY][ZZ];
  dest[XX][ZZ]=src[ZZ][XX];
  dest[YY][ZZ]=src[ZZ][YY];
  dest[ZZ][ZZ]=src[ZZ][ZZ];
}

static inline void tmmul(matrix a,matrix b,matrix dest)
{
  /* Computes dest=mmul(transpose(a),b,dest) - used in do_pr_pcoupl */
  dest[XX][XX]=a[XX][XX]*b[XX][XX]+a[YY][XX]*b[YY][XX]+a[ZZ][XX]*b[ZZ][XX];
  dest[XX][YY]=a[XX][XX]*b[XX][YY]+a[YY][XX]*b[YY][YY]+a[ZZ][XX]*b[ZZ][YY];
  dest[XX][ZZ]=a[XX][XX]*b[XX][ZZ]+a[YY][XX]*b[YY][ZZ]+a[ZZ][XX]*b[ZZ][ZZ];
  dest[YY][XX]=a[XX][YY]*b[XX][XX]+a[YY][YY]*b[YY][XX]+a[ZZ][YY]*b[ZZ][XX];
  dest[YY][YY]=a[XX][YY]*b[XX][YY]+a[YY][YY]*b[YY][YY]+a[ZZ][YY]*b[ZZ][YY];
  dest[YY][ZZ]=a[XX][YY]*b[XX][ZZ]+a[YY][YY]*b[YY][ZZ]+a[ZZ][YY]*b[ZZ][ZZ];
  dest[ZZ][XX]=a[XX][ZZ]*b[XX][XX]+a[YY][ZZ]*b[YY][XX]+a[ZZ][ZZ]*b[ZZ][XX];
  dest[ZZ][YY]=a[XX][ZZ]*b[XX][YY]+a[YY][ZZ]*b[YY][YY]+a[ZZ][ZZ]*b[ZZ][YY];
  dest[ZZ][ZZ]=a[XX][ZZ]*b[XX][ZZ]+a[YY][ZZ]*b[YY][ZZ]+a[ZZ][ZZ]*b[ZZ][ZZ];
}

static inline void mtmul(matrix a,matrix b,matrix dest)
{
  /* Computes dest=mmul(a,transpose(b),dest) - used in do_pr_pcoupl */
  dest[XX][XX]=a[XX][XX]*b[XX][XX]+a[XX][YY]*b[XX][YY]+a[XX][ZZ]*b[XX][ZZ];
  dest[XX][YY]=a[XX][XX]*b[YY][XX]+a[XX][YY]*b[YY][YY]+a[XX][ZZ]*b[YY][ZZ];
  dest[XX][ZZ]=a[XX][XX]*b[ZZ][XX]+a[XX][YY]*b[ZZ][YY]+a[XX][ZZ]*b[ZZ][ZZ];
  dest[YY][XX]=a[YY][XX]*b[XX][XX]+a[YY][YY]*b[XX][YY]+a[YY][ZZ]*b[XX][ZZ];
  dest[YY][YY]=a[YY][XX]*b[YY][XX]+a[YY][YY]*b[YY][YY]+a[YY][ZZ]*b[YY][ZZ];
  dest[YY][ZZ]=a[YY][XX]*b[ZZ][XX]+a[YY][YY]*b[ZZ][YY]+a[YY][ZZ]*b[ZZ][ZZ];
  dest[ZZ][XX]=a[ZZ][XX]*b[XX][XX]+a[ZZ][YY]*b[XX][YY]+a[ZZ][ZZ]*b[XX][ZZ];
  dest[ZZ][YY]=a[ZZ][XX]*b[YY][XX]+a[ZZ][YY]*b[YY][YY]+a[ZZ][ZZ]*b[YY][ZZ];
  dest[ZZ][ZZ]=a[ZZ][XX]*b[ZZ][XX]+a[ZZ][YY]*b[ZZ][YY]+a[ZZ][ZZ]*b[ZZ][ZZ];
}

static inline real det(matrix a)
{
  return ( a[XX][XX]*(a[YY][YY]*a[ZZ][ZZ]-a[ZZ][YY]*a[YY][ZZ])
	  -a[YY][XX]*(a[XX][YY]*a[ZZ][ZZ]-a[ZZ][YY]*a[XX][ZZ])
	  +a[ZZ][XX]*(a[XX][YY]*a[YY][ZZ]-a[YY][YY]*a[XX][ZZ]));
}

static inline void m_add(matrix a,matrix b,matrix dest)
{
  dest[XX][XX]=a[XX][XX]+b[XX][XX];
  dest[XX][YY]=a[XX][YY]+b[XX][YY];
  dest[XX][ZZ]=a[XX][ZZ]+b[XX][ZZ];
  dest[YY][XX]=a[YY][XX]+b[YY][XX];
  dest[YY][YY]=a[YY][YY]+b[YY][YY];
  dest[YY][ZZ]=a[YY][ZZ]+b[YY][ZZ];
  dest[ZZ][XX]=a[ZZ][XX]+b[ZZ][XX];
  dest[ZZ][YY]=a[ZZ][YY]+b[ZZ][YY];
  dest[ZZ][ZZ]=a[ZZ][ZZ]+b[ZZ][ZZ];
}

static inline void m_sub(matrix a,matrix b,matrix dest)
{
  dest[XX][XX]=a[XX][XX]-b[XX][XX];
  dest[XX][YY]=a[XX][YY]-b[XX][YY];
  dest[XX][ZZ]=a[XX][ZZ]-b[XX][ZZ];
  dest[YY][XX]=a[YY][XX]-b[YY][XX];
  dest[YY][YY]=a[YY][YY]-b[YY][YY];
  dest[YY][ZZ]=a[YY][ZZ]-b[YY][ZZ];
  dest[ZZ][XX]=a[ZZ][XX]-b[ZZ][XX];
  dest[ZZ][YY]=a[ZZ][YY]-b[ZZ][YY];
  dest[ZZ][ZZ]=a[ZZ][ZZ]-b[ZZ][ZZ];
}

static inline void msmul(matrix m1,real r1,matrix dest)
{
  dest[XX][XX]=r1*m1[XX][XX];
  dest[XX][YY]=r1*m1[XX][YY];
  dest[XX][ZZ]=r1*m1[XX][ZZ];
  dest[YY][XX]=r1*m1[YY][XX];
  dest[YY][YY]=r1*m1[YY][YY];
  dest[YY][ZZ]=r1*m1[YY][ZZ];
  dest[ZZ][XX]=r1*m1[ZZ][XX];
  dest[ZZ][YY]=r1*m1[ZZ][YY];
  dest[ZZ][ZZ]=r1*m1[ZZ][ZZ];
}

static inline void m_inv_ur0(matrix src,matrix dest)
{
  real tmp = src[XX][XX]*src[YY][YY]*src[ZZ][ZZ];
  if (fabs(tmp) <= 100*1.0e-24)
    Error("Can not invert matrix, determinant is zero");

  dest[XX][XX] = 1/src[XX][XX];
  dest[YY][YY] = 1/src[YY][YY];
  dest[ZZ][ZZ] = 1/src[ZZ][ZZ];
  dest[ZZ][XX] = (src[YY][XX]*src[ZZ][YY]*dest[YY][YY]
		  - src[ZZ][XX])*dest[XX][XX]*dest[ZZ][ZZ];
  dest[YY][XX] = -src[YY][XX]*dest[XX][XX]*dest[YY][YY];
  dest[ZZ][YY] = -src[ZZ][YY]*dest[YY][YY]*dest[ZZ][ZZ];
  dest[XX][YY] = 0.0;
  dest[XX][ZZ] = 0.0;
  dest[YY][ZZ] = 0.0;
}

static inline void m_inv(matrix src,matrix dest)
{
  const real smallreal = (real)1.0e-24;
  const real largereal = (real)1.0e24;
  real  deter,c,fc;

  deter = det(src);
  c     = (real)1.0/deter;
  fc    = (real)fabs(c);
  
  if ((fc <= smallreal) || (fc >= largereal)) 
    Error("Can not invert matrix, determinant");

  dest[XX][XX]= c*(src[YY][YY]*src[ZZ][ZZ]-src[ZZ][YY]*src[YY][ZZ]);
  dest[XX][YY]=-c*(src[XX][YY]*src[ZZ][ZZ]-src[ZZ][YY]*src[XX][ZZ]);
  dest[XX][ZZ]= c*(src[XX][YY]*src[YY][ZZ]-src[YY][YY]*src[XX][ZZ]);
  dest[YY][XX]=-c*(src[YY][XX]*src[ZZ][ZZ]-src[ZZ][XX]*src[YY][ZZ]);
  dest[YY][YY]= c*(src[XX][XX]*src[ZZ][ZZ]-src[ZZ][XX]*src[XX][ZZ]);
  dest[YY][ZZ]=-c*(src[XX][XX]*src[YY][ZZ]-src[YY][XX]*src[XX][ZZ]);
  dest[ZZ][XX]= c*(src[YY][XX]*src[ZZ][YY]-src[ZZ][XX]*src[YY][YY]);
  dest[ZZ][YY]=-c*(src[XX][XX]*src[ZZ][YY]-src[ZZ][XX]*src[XX][YY]);
  dest[ZZ][ZZ]= c*(src[XX][XX]*src[YY][YY]-src[YY][XX]*src[XX][YY]);
}

static inline void mvmul(matrix a,const rvec src,rvec dest)
{
  dest[XX]=a[XX][XX]*src[XX]+a[XX][YY]*src[YY]+a[XX][ZZ]*src[ZZ];
  dest[YY]=a[YY][XX]*src[XX]+a[YY][YY]*src[YY]+a[YY][ZZ]*src[ZZ];
  dest[ZZ]=a[ZZ][XX]*src[XX]+a[ZZ][YY]*src[YY]+a[ZZ][ZZ]*src[ZZ];
}

static inline void mvmul_ur0(matrix a,const rvec src,rvec dest)
{
  dest[ZZ]=a[ZZ][XX]*src[XX]+a[ZZ][YY]*src[YY]+a[ZZ][ZZ]*src[ZZ];
  dest[YY]=a[YY][XX]*src[XX]+a[YY][YY];
  dest[XX]=a[XX][XX]*src[XX];
}

static inline void tmvmul_ur0(matrix a,const rvec src,rvec dest)
{
  dest[XX]=a[XX][XX]*src[XX]+a[YY][XX]*src[YY]+a[ZZ][XX]*src[ZZ];
  dest[YY]=                  a[YY][YY]*src[YY]+a[ZZ][YY]*src[ZZ];
  dest[ZZ]=                                    a[ZZ][ZZ]*src[ZZ];
}

static inline void unitv(const rvec src,rvec dest)
{
  real linv;
  
  linv=1.0/sqrt(norm2(src));
  dest[XX]=linv*src[XX];
  dest[YY]=linv*src[YY];
  dest[ZZ]=linv*src[ZZ];
}

// static void calc_lll(rvec box,rvec lll)
// {
//   lll[XX] = 2.0*M_PI/box[XX];
//   lll[YY] = 2.0*M_PI/box[YY];
//   lll[ZZ] = 2.0*M_PI/box[ZZ];
// }

static inline real trace(matrix m)
{
  return (m[XX][XX]+m[YY][YY]+m[ZZ][ZZ]);
}

// static void m_rveccopy(int dim, rvec *a, rvec *b)
// {
//     /* b = a */
//     int i;

//     for (i=0; i<dim; i++)
//         copy_rvec(a[i],b[i]);
// } 



#define ROTATE(a,i,j,k,l) g=a[i][j];h=a[k][l];a[i][j]=g-s*(h+g*tau);	\
  a[k][l]=h+s*(g-h*tau);

static inline void jacobi6(double **a,double d[],double **v,int *nrot)
{
  int j,i;
  int iq,ip;
  double tresh,theta,tau,t,sm,s,h,g,c;//,*b,*z;
  
  double b[2*DIM];
  double z[2*DIM];

  for (ip=0; ip<2*DIM; ip++) {
    for (iq=0; iq<2*DIM; iq++) v[ip][iq]=0.0;
    v[ip][ip]=1.0;
  }
  for (ip=0; ip<2*DIM;ip++) {
    b[ip]=d[ip]=a[ip][ip];
    z[ip]=0.0;
  }
  *nrot=0;
  for (i=1; i<=50; i++) {
    sm=0.0;
    for (ip=0; ip<2*DIM-1; ip++) {
      for (iq=ip+1; iq<2*DIM; iq++)
        sm += fabs(a[ip][iq]);
    }
    if (sm == 0.0) {
      //      delete[] z;
      //delete[] b;
      //      sfree(z);
      //sfree(b);
      return;
    }
    if (i < 4)
      tresh=0.2*sm/(2*DIM*2*DIM);
    else
      tresh=0.0;
    for (ip=0; ip<2*DIM-1; ip++) {
      for (iq=ip+1; iq<2*DIM; iq++) {
        g=100.0*fabs(a[ip][iq]);
        if (i > 4 && fabs(d[ip])+g == fabs(d[ip])
            && fabs(d[iq])+g == fabs(d[iq]))
          a[ip][iq]=0.0;
        else if (fabs(a[ip][iq]) > tresh) {
          h=d[iq]-d[ip];
          if (fabs(h)+g == fabs(h))
            t=(a[ip][iq])/h;
          else {
            theta=0.5*h/(a[ip][iq]);
            t=1.0/(fabs(theta)+sqrt(1.0+theta*theta));
            if (theta < 0.0) t = -t;
          }
          c=1.0/sqrt(1+t*t);
          s=t*c;
          tau=s/(1.0+c);
          h=t*a[ip][iq];
          z[ip] -= h;
          z[iq] += h;
          d[ip] -= h;
          d[iq] += h;
          a[ip][iq]=0.0;
          for (j=0; j<ip; j++) {
            ROTATE(a,j,ip,j,iq)
	      }
          for (j=ip+1; j<iq; j++) {
            ROTATE(a,ip,j,j,iq)
	      }
          for (j=iq+1; j<2*DIM; j++) {
            ROTATE(a,ip,j,iq,j)
	      }
          for (j=0; j<2*DIM; j++) {
            ROTATE(v,j,ip,j,iq)
	      }
          ++(*nrot);
        }
      }
    }
    for (ip=0; ip<2*DIM; ip++) {
      b[ip] +=  z[ip];
      d[ip]  =  b[ip];
      z[ip]  =  0.0;
    }
  }
  Error("jacobi6: Too many iterations in routine JACOBI\n");
}

static inline void jacobi3(double **a,double d[],double **v,int *nrot)
{
  int j,i;
  int iq,ip;
  double tresh,theta,tau,t,sm,s,h,g,c;//,*b,*z;
  
  double b[DIM];
  double z[DIM];


  for (ip=0; ip<DIM; ip++) {
    for (iq=0; iq<DIM; iq++) v[ip][iq]=0.0;
    v[ip][ip]=1.0;
  }
  for (ip=0; ip<DIM;ip++) {
    b[ip]=d[ip]=a[ip][ip];
    z[ip]=0.0;
  }
  *nrot=0;
  for (i=1; i<=50; i++) {
    sm=0.0;
    for (ip=0; ip<DIM-1; ip++) {
      for (iq=ip+1; iq<DIM; iq++)
        sm += fabs(a[ip][iq]);
    }
    if (sm == 0.0) {
      return;
    }
    if (i < 4)
      tresh=0.2*sm/(DIM*DIM);
    else
      tresh=0.0;
    for (ip=0; ip<DIM-1; ip++) {
      for (iq=ip+1; iq<DIM; iq++) {
        g=100.0*fabs(a[ip][iq]);
        if (i > 4 && fabs(d[ip])+g == fabs(d[ip])
            && fabs(d[iq])+g == fabs(d[iq]))
          a[ip][iq]=0.0;
        else if (fabs(a[ip][iq]) > tresh) {
          h=d[iq]-d[ip];
          if (fabs(h)+g == fabs(h))
            t=(a[ip][iq])/h;
          else {
            theta=0.5*h/(a[ip][iq]);
            t=1.0/(fabs(theta)+sqrt(1.0+theta*theta));
            if (theta < 0.0) t = -t;
          }
          c=1.0/sqrt(1+t*t);
          s=t*c;
          tau=s/(1.0+c);
          h=t*a[ip][iq];
          z[ip] -= h;
          z[iq] += h;
          d[ip] -= h;
          d[iq] += h;
          a[ip][iq]=0.0;
          for (j=0; j<ip; j++) {
            ROTATE(a,j,ip,j,iq)
	      }
          for (j=ip+1; j<iq; j++) {
            ROTATE(a,ip,j,j,iq)
	      }
          for (j=iq+1; j<DIM; j++) {
            ROTATE(a,ip,j,iq,j)
	      }
          for (j=0; j<DIM; j++) {
            ROTATE(v,j,ip,j,iq)
	      }
          ++(*nrot);
        }
      }
    }
    for (ip=0; ip<DIM; ip++) {
      b[ip] +=  z[ip];
      d[ip]  =  b[ip];
      z[ip]  =  0.0;
    }
  }
  Error("jacobi3: Too many iterations in routine JACOBI\n");
}


static inline void princ_comp(int n, rvec x[],
			      matrix trans,rvec d)
{
  
#define NDIM 4
  int  i,j,m,nrot;
  real rx,ry,rz;
  double dd[NDIM],tvec[NDIM];
/*   double inten[NDIM][NDIM]; */
/*   double ev[NDIM][NDIM]; */

  double *inten[NDIM]; 
  double *ev[NDIM]; 
  
/*   inten = new double*[NDIM]; */
/*   ev = new double*[NDIM]; */
  
  real temp;
  for(i=0; (i<NDIM); i++) {
    inten[i] = malloc(sizeof(double)*NDIM);
    ev[i] = malloc(sizeof(double)*NDIM);
    dd[i]=0.0;
  }
  
  for(i=0; (i<NDIM); i++)
    for(m=0; (m<NDIM); m++)
      inten[i][m]=0;
  for(i=0; (i<n); i++) {
    rx=x[i][XX];
    ry=x[i][YY];
    rz=x[i][ZZ];
    inten[0][0]+=(sqr(ry)+sqr(rz));
    inten[1][1]+=(sqr(rx)+sqr(rz));
    inten[2][2]+=(sqr(rx)+sqr(ry));
    inten[1][0]-=(ry*rx);
    inten[2][0]-=(rx*rz);
    inten[2][1]-=(rz*ry);
  }
  inten[0][1]=inten[1][0];
  inten[0][2]=inten[2][0];
  inten[1][2]=inten[2][1];
  
  for(i=0; (i<DIM); i++) {
    for(m=0; (m<DIM); m++)
      trans[i][m]=inten[i][m];
  }
  
  /* Call numerical recipe routines */
  jacobi3(inten,dd,ev,&nrot);
  
  /* Sort eigenvalues in descending order */
#define SWAPPER(i) 			\
  if (fabs(dd[i+1]) > fabs(dd[i])) {	\
    temp=dd[i];					\
    for(j=0; (j<NDIM); j++) tvec[j]=ev[j][i];	\
    dd[i]=dd[i+1];						\
    for(j=0; (j<NDIM); j++) ev[j][i]=ev[j][i+1];		\
    dd[i+1]=temp;						\
    for(j=0; (j<NDIM); j++) ev[j][i+1]=tvec[j];			\
  }
  SWAPPER(0);
  SWAPPER(1);
  SWAPPER(0);
  
  for(i=0; (i<DIM); i++) {
    d[i]=dd[i];
    for(m=0; (m<DIM); m++)
      trans[i][m]=ev[m][i]; 
    
  }
  
   for(i=0;i<NDIM;i++){ 
     free( inten[i] );
     free( ev[i] );
/*     delete[] inten[i]; */
/*     delete[] ev[i]; */
   } 
/*   delete[] inten; */
/*   delete[] ev; */
}


extern void rotate_rvec(int n,rvec x[],matrix trans);
extern real planarity(rvec *x, int n);
extern void set_planar(rvec *x, int n);
extern void center(rvec *, int);
extern void center_and_get_vec(rvec *x, int n, rvec c);
extern void center_vec(rvec *x, int n, rvec c);
extern void do_fit(int natoms,real *w_rls,rvec *xp,rvec *x);
extern void calc_fit_R(int natoms,real *w_rls,rvec *xp,rvec *x,matrix R);
extern real radius_of_gyration(rvec *x, real *mass, int n );
extern void cryst1_to_box( char *line, matrix box); 
extern void box_to_cryst1( matrix box, char *line); 
extern void search_neighbors(rvec *x, int natoms, real cutoff, int **nlist, int *nat);
extern real distance_from_atoms( PyObject *atom1, PyObject *atom2);
extern real distance2_from_atoms( PyObject *atom1, PyObject *atom2);

extern real angle_from_atoms( PyObject *atom1, PyObject *atom2, PyObject *atom3);
extern real dihedral_from_atoms( PyObject *atom1, PyObject *atom2, PyObject *atom3, PyObject *atom4);

extern real calc_bond_energy( PyObject *atom1, PyObject *atom2, real kb, real b0);
extern real calc_angle_energy(PyObject *atom1, PyObject *atom2, PyObject *atom3, real kb, real phi0);
extern real calc_dihedral_energy(PyObject *atom1, PyObject *atom2, PyObject *atom3, PyObject *atom4,  real *params);
extern real calc_improper_energy(PyObject *atom1, PyObject *atom2, PyObject *atom3, PyObject *atom4, real kb, real phi0, int mult);

typedef struct con_table {
  int natoms;
  int *ncon;
  int **con;
} con_table;



#endif

