/*                                                +++++++++++        */
# define FILENAME                                 "hohvect.c"
/*                                                +++++++++++        */

# include "util.h"

/**
*** mkfrathoh adapted from $XPLOR/source/array.s (MKFRAT)
***

      SUBROUTINE MKFRAT(IMOVE,NATOM,FREEAT,NFREAT)
C
C Given a marker array IMOVE a new array FREEAT[1...NFREAT]
C is constructed which contains the indices of all elements
C with IMOVE(i).eq.0.
C
C Author: Robert Bruccoleri
C
      IMPLICIT NONE
C input/output
      INTEGER IMOVE(*), NATOM, FREEAT(*), NFREAT
C local
      INTEGER I
C begin
      NFREAT=0
      DO I=1,NATOM
      IF (IMOVE(I).EQ.0) THEN
      NFREAT=NFREAT+1
      FREEAT(NFREAT)=I
      END IF
      END DO
      RETURN
      END
***
**/

/*   =========                                                       */
int  mkfrathoh
/*   =========                                                       */
  (int NATOM, int WATBEG, int WATEND, int freeat[])
{
  int i;
  int nfreat=0;
  for (i=1; i<=NATOM; i++) 
    if (i>= WATBEG && i<=WATEND) {
      nfreat++;
      freeat[nfreat]=i;
    }
  printf("%s (mkfrathoh): nfreat=%d\n",FILENAME,nfreat);
  return(nfreat);
}  
    
/*   ========                                                             */
void pdbtohoh(int i, double xx[], double yy[], double zz[], Water_ptr hoh)
/*   ========                                                             */
{
  hoh[0]=xx[i];
  hoh[1]=yy[i];
  hoh[2]=zz[i];
  i++;
  hoh[3]=xx[i];
  hoh[4]=yy[i];
  hoh[5]=zz[i];
  i++;
  hoh[6]=xx[i];
  hoh[7]=yy[i];
  hoh[8]=zz[i];
}

/*   ========                                                             */
void hohtopdb(int i, double xx[], double yy[], double zz[], Water_ptr hoh)
/*   ========                                                             */
{
  xx[i]=    hoh[0];
  yy[i]=    hoh[1];
  zz[i]=    hoh[2];
  xx[++i]=  hoh[3];
  yy[i]=    hoh[4];
  zz[i]=    hoh[5];
  xx[++i]=  hoh[6];
  yy[i]=    hoh[7];
  zz[i]=    hoh[8];
}

/*   =======                                                             */
void hoh2xyz
/*   =======                                                             */
  (Water *hoharr,int watbeg, int watend, 
   double xx[], double yy[], double zz[])
{
  int ihoh;
  for (ihoh=watbeg; ihoh<=watend; ihoh += 3) {
    hohtopdb(ihoh,xx,yy,zz, (Water_ptr) hoharr++);
  }
}

/*   =======                                                             */
void xyz2hoh
/*   =======                                                             */
  (Water *hoharr,int watbeg, int watend, 
   double xx[], double yy[], double zz[],
   PDB_data *P)
{
  int ihoh;
  for (ihoh=watbeg; ihoh<=watend; ihoh += 3) {
    if (!ishoh(P,ihoh)) {
      nonfatal(FILENAME,"xyz2hoh","molecule is not O");
      nprt_pdbrec(P,xx,yy,zz,ihoh);
    }
    pdbtohoh(ihoh,xx,yy,zz, (Water_ptr) hoharr++);
  }
}

/*   =============                                                       */
void hohstereochem( vector9 hoh, vector9 bondlen)
/*   =============                                                       */
{
  bondlen[0]=ndistsq(hoh[0],hoh[1],hoh[2],hoh[3],hoh[4],hoh[5]);
  bondlen[1]=ndistsq(hoh[3],hoh[4],hoh[5],hoh[6],hoh[7],hoh[8]);
  bondlen[2]=ndistsq(hoh[6],hoh[7],hoh[8],hoh[0],hoh[1],hoh[2]);
}


/*       =========                                                      */ 
double * translate(vector9 v, vector9 u)
/*       =========                                                      */ 
{
  v[0] += u[0];
  v[1] += u[1];
  v[2] += u[2];
  return(v);
}
  
/*       ============                                                      */ 
double * vectsubtract (Vector9 v, Vector9 u)
/*       ============                                                      */ 
{
  v[0] -= u[0];
  v[1] -= u[1];
  v[2] -= u[2];
  return(v);
}
  
/*      =====                                                             */
double *mkmat
/*      =====                                                             */
  (double rc11, double rc12, double rc13,
   double rc21, double rc22, double rc23,
   double rc31, double rc32, double rc33, 
   Matrix R)
{
  RC(R,1,1)= rc11;
  RC(R,1,2)= rc12;
  RC(R,1,3)= rc13;
  RC(R,2,1)= rc21;
  RC(R,2,2)= rc22;
  RC(R,2,3)= rc23;
  RC(R,3,1)= rc31;
  RC(R,3,2)= rc32;
  RC(R,3,3)= rc33;
  return(R);
}
 
/*      ================                                                */
double *rotate_to_z_axis(Matrix R, vector9 N0)
/*      ================                                                */
{
  /*  rotang  = acos (N0 . z) */ 
  /*  rotaxis = N0 cross z =  N0[0] x cross z + N[1] y cross z */
  /*                       = -N0[0] y         + N[1] x   */
  double rotang = - acos(N0[2]) ;
  Vector9 rotaxis;
  rotaxis[0] =  N0[1];
  rotaxis[1] = -N0[0];
  rotaxis[2] =  0;
  VTOD(N0);
  FTOD(hypot3(N0[0],N0[1],N0[2]));
  VTOD((R+0));
  VTOD((R+3));
  VTOD((R+6));
  VTOD(rotaxis);
  FTOD(TODEG(rotang));
  xmkrotmat(rotaxis, rotang, R);
  VTOD((R+0));
  VTOD((R+3));
  VTOD((R+6));
  return(R);
}

/*      ======                                                        */
double *rotate(Matrix R, vector9 v0)
/*      ======                                                        */
{
  vector9 u0;
  double 
    *v = v0 -1,
    *u = u0 -1;
  
  u[1] = RC(R,1,1)*v[1] + RC(R,1,2)*v[2] + RC(R,1,3)*v[3];
  u[2] = RC(R,2,1)*v[1] + RC(R,2,2)*v[2] + RC(R,2,3)*v[3];
  u[3] = RC(R,3,1)*v[1] + RC(R,3,2)*v[2] + RC(R,3,3)*v[3];
  v[1] = u[1];
  v[2] = u[2];
  v[3] = u[3];

  return(v0);
}

/*      =======                                                        */
double *qrotate(Vector9 Q, Vector9 v0)
/*      =======                                                        */
{
  /***
   ***  Rotates v0 by quaternion Q. From NTS. See $MC/rotrans.f
   ***/
  vector9 u0;
  double 
    *v = v0 -1,
    *u = u0 -1;
  
  u[1] = 
    v[1] * (   Q[1]*Q[1] - Q[2]*Q[2] - Q[3]*Q[3] + Q[0]*Q[0]) +
    v[2] * (   Q[1]*Q[2] - Q[3]*Q[0]) * 2                     +
    v[3] * (   Q[1]*Q[3] + Q[2]*Q[0]) * 2                     ;

  u[2] = 
    v[1] * (   Q[1]*Q[2] - Q[3]*Q[0]) * 2                     +
    v[2] * ( - Q[1]*Q[1] + Q[2]*Q[2] - Q[3]*Q[3] + Q[0]*Q[0]) +
    v[3] * (   Q[2]*Q[3] - Q[1]*Q[0]) * 2                     ;

  u[1] = 
    v[1] * (   Q[1]*Q[3] - Q[2]*Q[0]) * 2                     +
    v[2] * (   Q[2]*Q[3] + Q[1]*Q[0]) * 2                     +
    v[3] * ( - Q[1]*Q[1] - Q[2]*Q[2] + Q[3]*Q[3] + Q[0]*Q[0]) ;
  
  v[1] = u[1];
  v[2] = u[2];
  v[3] = u[3];
  
  return(v0);
}

/*      =====                                                         */
double *scale(vector9 v, double c)
/*      =====                                                         */
{
  v[0] *= c;
  v[1] *= c;
  v[2] *= c;
  return(v);
}


/*   ========                                                           */
void prt_vect(vector9 v, char *name)
/*   ========                                                           */
{
  printf("%s (prt_vect): %9s = ( %9.4f %9.4f %9.4f )\n",
	 FILENAME,name,v[0],v[1],v[2]);
}

/*   =======                                                           */
void prt_hoh(Water v, char *name)
/*   =======                                                           */
{
  printf("%s (prt_hoh): %9s\n", FILENAME,name);
  printf(" : OX ( %9.4f %9.4f %9.4f )\n",v[0],v[1],v[2]);
  printf(" : H1 ( %9.4f %9.4f %9.4f )\n",H1(v)[0],H1(v)[1],H1(v)[2]);
  printf(" : H2 ( %9.4f %9.4f %9.4f )\n",H2(v)[0],H2(v)[1],H2(v)[2]);
}


/*     ======                                                         */
float  hypot3(float x, float y, float z)
/*     ======                                                         */
{
  return(sqrt(x*x + y*y + z*z));
}

/*     =======                                                            */
double ndistsq
/*     =======                                                            */
  (double xi, double yi, double zi,
   double xj, double yj, double zj)
{
  double rxij = xi - xj;
  double ryij = yi - yj;
  double rzij = zi - zj;
  double x,y,z;
  x = rxij;
  y = ryij;
  z = rzij;
  return( x*x + y*y + z*z );
}


/*   ==========                                                        */
void origin_rot(Matrix R, Vector9 origin, Vector9 v)
/*   ==========                                                        */
{
  v[0] -= origin[0];
  v[1] -= origin[1];
  v[2] -= origin[2];

  rotate(R,v);

  v[0] += origin[0];
  v[1] += origin[1];
  v[2] += origin[2];
}

/*      ========                                                             */
double *mkrotmat(vector9 v0, double theta, vector9 R)
/*      ========                                                             */
{
  double
    c = cos(theta),
    s = sin(theta),
    l = v0[0],
    m = v0[1],
    n = v0[2];
  
  /* Formula from Internation Tables of Crystallography */
  /*              Section 3.3: Modelling and Graphics   */
  /* This doesn't agree with xplor (see below).         */
  
  RC(R,1,1) = l*l + (m*m + n*n)*c ;
  RC(R,2,2) = m*m + (l*l + n*n)*c ;
  RC(R,3,3) = n*n + (m*m + l*l)*c ;

  RC(R,1,2) = l*m*(1-c)-n*s ;
  RC(R,2,1) = l*m*(1-c)+n*s ;

  RC(R,1,3) = n*l*(1-c)+m*s ;
  RC(R,3,1) = n*l*(1-c)-m*s ;

  RC(R,2,3) = m*n*(1-c)-l*s ;
  RC(R,3,2) = m*n*(1-c)+l*s ;

  return (R);
}

/*      =========                                                           */
double *xmkrotmat(vector9 v0, double theta, vector9 R)
/*      =========                                                           */
{

# define AXIS(i) (v0[i-1])
# define RAD (PI/180.0)
# define ZERO 0
# define ONE 1 
# define SMALL 0.0000001
# define MAX(a,b) (a > b ? a : b)
# define MIN(a,b) (a < b ? a : b)
# define ROT(i,j) (RC(R,i,j))
# define RSMALL 0.0000000001

/*
C    T1,T2,T3 are psi (incl. vs. y), phi (azimuthal angle, that is,
C             the angle between the x-axis and the projection of the
C             axis into the x,z plane ), kappa for MODE="SPHE"
C    T3, AXIS(3) are kappa and a 3-D vector specifying the axis for MODE
C*   @="AXIS"
C  Note: all rotations are counter-clockwise
C
C Output:
C    ROT(3,3) contains the rotation matrix.  Should be applied as
C    r'(i)=sum_j ROT(i,j)*r(j)
C
C    AXIS contains the normalized input AXIS vector and T1,T2,T3 will
C    contain the spherical polar angles for MODE="SPHE"
C
.
.
.
C     In axis mode we obtain the psi, phi spherical polar angles from
C     the AXIS vector.
C     The rotation kappa corresonds to an anticlockwise rotation (t3) about
C     the specified axis. The axis is desribed as a vector (AXIS).
*/
  double NN=sqrt(AXIS(1)*AXIS(1)+AXIS(2)*AXIS(2)+AXIS(3)*AXIS(3)) ;
  double T1,T2,T3 = TODEG(theta);

  if (NN < RSMALL) {
    STDERR("rotation vector has zero length");
    T1=ZERO ;
    T2=ZERO ;
    T3=ZERO ;
  } else {
    AXIS(1)=AXIS(1)/NN ;
    AXIS(2)=AXIS(2)/NN ;
    AXIS(3)=AXIS(3)/NN ;
    T1=acos(AXIS(2))/RAD ;
    if (AXIS(1)*AXIS(1)+AXIS(3)*AXIS(3) < SMALL) {
      T2=ZERO ;
    } else {
      T2= acos(MAX(-ONE,
		   MIN(ONE,
		       AXIS(1)/sqrt(AXIS(1)*AXIS(1)+AXIS(3)*AXIS(3)))))
	/RAD ;
      
      if (AXIS(3) > ZERO) T2=-T2 ;
    }
  }
  
/*
C     compute rotation matrix corresponding to the three
C     spherical polar angles psi=t1, phi=t2 and kappa=t3.  The rotation is
C     described by specification of the direction of an axis through
C     phi (azimutal angle between the x axis and the projection of the
C     rotation axis on the x-z plane) and psi (inclination versus y axis).
C     The angle kappa specifies the rotation around the specified axis.
C     The kappa angle is anti-clockwise when looking along the rotation axis
C     The phi angle is anti-clockwise when looking along y.
*/
  {
    double 
      S1=sin(RAD*T1) ,
      S2=sin(RAD*T2) ,
      S3=sin(RAD*T3) ,
      C1=cos(RAD*T1) ,
      C2=cos(RAD*T2) ,
      C3=cos(RAD*T3) ,
      S1SQ = S1*S1 ,
      C3C =1.0 - C3 ;
    ROT(1,1) = C3           + S1SQ*C2*C2*C3C ;
    ROT(1,2) = S1*C1*C2*C3C   - S1*S2*S3 ;
    ROT(1,3) =-S1SQ*C2*S2*C3C - C1*S3 ;
    ROT(2,1) = S1*C1*C2*C3C   + S1*S2*S3 ;
    ROT(2,2) = C3           + C1*C1*C3C ;
    ROT(2,3) =-S1*C1*S2*C3C   + S1*C2*S3 ;
    ROT(3,1) =-S1SQ*S2*C2*C3C + C1*S3 ;
    ROT(3,2) =-S1*C1*S2*C3C   - S1*C2*S3 ;
    ROT(3,3) = C3           + S1SQ*S2*S2*C3C ;
  }
  return (R) ;

# undef AXIS
# undef RAD 
# undef ZERO
# undef ONE 
# undef SMALL 
# undef MAX
# undef MIN
# undef ROT
# undef RSMALL 

}
/*****************************************************************************
 From XPLOR code 
******************************************************************************

C=================================================================
      ELSE IF (MODE.EQ.'SPHE'.OR.MODE.EQ.'AXIS') THEN
C     
         IF (MODE.EQ.'AXIS') THEN
C     
C     In axis mode we obtain the psi, phi spherical polar angles from
C     the AXIS vector.
C     The rotation kappa corresonds to an anticlockwise rotation (t3) about
C     the specified axis. The axis is desribed as a vector (AXIS).
            NN=SQRT(AXIS(1)**2+AXIS(2)**2+AXIS(3)**2)
            IF (NN.LT.RSMALL) THEN
               CALL WRNDIE(-5,'ROTMAT','rotation vector has zero length')
               T1=ZERO
               T2=ZERO
               T3=ZERO
            ELSE
               AXIS(1)=AXIS(1)/NN
               AXIS(2)=AXIS(2)/NN
               AXIS(3)=AXIS(3)/NN
               T1=ACOS(AXIS(2))/RAD
               IF (AXIS(1)**2+AXIS(3)**2.LT.SMALL) THEN
                  T2=ZERO
               ELSE
                  T2=ACOS(MAX(-ONE,MIN(ONE,AXIS(1)/SQRT(AXIS(1)**2+AXIS(3)**2))))
     @                 /RAD
                  IF (AXIS(3).GT.ZERO) T2=-T2
               END IF
            END IF
         END IF
C     
C     compute rotation matrix corresponding to the three
C     spherical polar angles psi=t1, phi=t2 and kappa=t3.  The rotation is
C     described by specification of the direction of an axis through
C     phi (azimutal angle between the x axis and the projection of the
C     rotation axis on the x-z plane) and psi (inclination versus y axis).
C     The angle kappa specifies the rotation around the specified axis.
C     The kappa angle is anti-clockwise when looking along the rotation axis
C*    @.
C     The phi angle is anti-clockwise when looking along y.
         S1=SIN(RAD*T1)
         S2=SIN(RAD*T2)
         S3=SIN(RAD*T3)
         C1=COS(RAD*T1)
         C2=COS(RAD*T2)
         C3=COS(RAD*T3)
         S1SQ=S1*S1
         C3C=1.0-C3
         ROT(1,1) = C3           + S1SQ*C2*C2*C3C
         ROT(1,2) = S1*C1*C2*C3C   - S1*S2*S3
         ROT(1,3) =-S1SQ*C2*S2*C3C - C1*S3
         ROT(2,1) = S1*C1*C2*C3C   + S1*S2*S3
         ROT(2,2) = C3           + C1*C1*C3C
         ROT(2,3) =-S1*C1*S2*C3C   + S1*C2*S3
         ROT(3,1) =-S1SQ*S2*C2*C3C + C1*S3
         ROT(3,2) =-S1*C1*S2*C3C   - S1*C2*S3
         ROT(3,3) = C3           + S1SQ*S2*S2*C3C
C================================================================
*/


/***********************************************************************
 *** Messed up formula ***

 double s,c,q ;
 double 
 *v = v0 - 1;
 s = sin(theta);
 c = cos(theta);
 q = 1 - c;
  
 **  vector v is the direction cosines of a rotation.
 **  theta is magnitude of the rotation in radians. 
 **  R is the resulting rotation matrix. 
 **
 **  R = c   ( 1     ) 
 **          (   1   )
 **          (     1 )
 **
 **    + s   (  0 -v3  v2 )
 **          ( v3   0 -v1 )
 **          (-v2  v1   0 )
 **
 **    + q   ( v1v1  v1v2  v1v3 )
 **          ( v2v1  v2v2  v2v3 )
 **          ( v3v1  v3v2  v3v3 )
 **
  
  RC(R,1,1)= 
    c + 0 +
      q * v[1] * v[1];
  RC(R,1,2)= 
    0 + -s * v[3] +
      q * v[1] * v[2];
  RC(R,1,3)=
    0 + s * v[2] +
      q * v[1] * v[3];
  RC(R,2,1)=
    0 + s * v[3] +
      q * v[2] * v[1];
  RC(R,2,2)=
    c + 0 +
      q * v[2] * v[2];
  RC(R,2,3)=
    0 + -s*v[1] +
      q * v[2] * v[3];
  RC(R,3,1)=
    0 + -s*v[2] +
      q * v[3] * v[1];
  RC(R,3,2)=
    0 + s*v[1] + 
      q * v[3] * v[2];
  RC(R,3,3)=
    c + 0 +
      q * v[3] * v[3];

  return(R);

  ***********************************************************************/


/*     ===============                                                   */
double cos_from_z_axis (const Vector9 head, const Vector9 tail)
/*     ===============                                                   */
{
  Vector9 
    w,
    z = {0, 0, 1};
  SUBTRACT(w,head,tail);
/*
  VTOD(head);
  VTOD(tail);
  VTOD(w);
  { 
    double len = LENVECT(w);
    FTOD(len);
    FTOD(w[2]/len);
    FTOD(TODEG(acos(w[2]/len)));
  }
*/
  return((dotcos(z,w)));
}  

/*     ======                                                              */
double dotcos (const Vector9 v, const Vector9 u)
/*     ======                                                              */
{
  double mv = v[0]*v[0] + v[1]*v[1] + v[2]*v[2];
  double mu = u[0]*u[0] + u[1]*u[1] + u[2]*u[2];
  double vu = v[0]*u[0] + v[1]*u[1] + v[2]*u[2];
  return (vu/(sqrt(mu*mv)));
}

/*   ==================                                                    */
void hohdipole_rad_proj 
/*   ==================                                                    */
  (const int natom,  double xx[], double yy[], double zz[],
   const int sel[], PDB_data *P,
   const double   x0, const double y0, double rpp[])
{
  char subr[] = "hohdipole_rad_proj";
  static bool firstcall = false;
  static bool badsel    = false;
  Vector9 p,dir;
  Water hoh;
  int ihoh;

  if (firstcall) {
    
    for (ihoh=1;ihoh<=natom;ihoh++)
      if (sel[ihoh] && !ishoh(P,ihoh)) {
	badsel = true;
	printf("%s (radial_dipole_proj): ihoh=%d badsel=true\n",
	       FILENAME,ihoh);
      } 
    if (!badsel) 
      printf("%s (%s): firstcall=%d badsel=%d ihoh=%d\n",
	     FILENAME,subr,firstcall,badsel,ihoh);
    firstcall = false;
  } 
  
  if (badsel) 
    return;
  else {
    for (ihoh=1; ihoh<=natom;ihoh++)
      if (sel[ihoh]) {
	pdbtohoh(ihoh,xx,yy,zz,(Water_ptr) hoh);
	hohdipole(hoh,p);
	dir[0]=hoh[0]-x0;
	dir[1]=hoh[1]-y0;
	dir[2]=0;
	rpp[ihoh] = dotcos(p, dir);
      }
  }
}

/*   =====                                                            */
bool ishoh(PDB_data *P, int i0)
/*   =====                                                            */
{
  int i1 = i0+1;
  int i2 = i0+2;
  if (P->atnam[i0][1]=='O' && P->atnam[i0][2]=='H' && P->atnam[i0][3]=='2' &&
      P->atnam[i1][1]=='H' && P->atnam[i1][2]=='1' && 
      P->atnam[i2][1]=='H' && P->atnam[i2][2]=='2' )
    return true;
  else
    return false;
}

/*      =========                                                        */
void    hohdipole(const Water hoh, Vector9 p)
/*      =========                                                        */
{
  p[0]=(hoh[3]+hoh[6])/2 - hoh[0];
  p[1]=(hoh[4]+hoh[7])/2 - hoh[1];
  p[2]=(hoh[5]+hoh[8])/2 - hoh[2];
}

/*      =======                                                          */
double *randlmn(Vector9 v)
/*      =======                                                          */
{
  double m;
  
  v[0]=ranf(-1,1);
  v[1]=ranf(-1,1);
  v[2]=ranf(-1,1);
  
  m=v[0]*v[0]+ v[1]*v[1]+ v[2]*v[2];
  if (m<1) {
    m = sqrt(m);
    v[0] /= m;
    v[1] /= m;
    v[2] /= m;
    return (v);
  } else
    return ( randlmn(v) );
}