#include "drawwindow.h"
#include "peptide.h"
#include "math.h"
#include "trace.h"
extern FILE *fpout; // = stdout or logfile, depending whether batch or screen version
extern int verbose; //Defined in trace.c++  if = 1,print some stuff;  = 2 print a lot of stuff


void frtoorth(struct griddata& den1,float fr[3],float orth[3]) 
 /*convert from xi,xj (in units of unit cell grid)
	 to orth co-ord, */
{
  int i;
  float xi[3];

  for(i=0;i<=2;i++)
     xi[i]=fr[i]*den1.cell[i+1];
  orth[0]=xi[0]+xi[1]*den1.cosgam+xi[2]*den1.cosbe;
  orth[1]=xi[1]*den1.singam+xi[2]*den1.cvy;
  orth[2]=xi[2]*den1.cvz;
}


void orthtofr(struct griddata& den1,float xx[3],float fr[3]) 
/*convert from orth co-ord to fractional floating 
    point in unit of unit cell grid*/
{
  int i;

  fr[2]=xx[2]/den1.cvz;
  fr[1]=(xx[1]-fr[2]*den1.cvy)/den1.singam;
  fr[0]=xx[0]-fr[1]*den1.cosgam-fr[2]*den1.cosbe;
  for(i=0;i<=2;i++)
    fr[i]=fr[i]/den1.cell[i+1];
}

void xtof(struct griddata& den1,float xx[3],float ijk[3]) /*convert from world co-ord to floating i,j,k co-ord*/
{
  float x,y,z;
  z=xx[2]/den1.cvz;
  y=(xx[1]-z*den1.cvy)/den1.singam;
  x=xx[0]-y*den1.cosgam-z*den1.cosbe;
  ijk[0]=x/den1.delx[0] - den1.amin[0];
  ijk[1]=y/den1.delx[1] - den1.amin[1];
  ijk[2]=z/den1.delx[2] - den1.amin[2];
}

void xtoi(struct griddata& den1,float xx[3],int ijk[3]) /*convert from world co-ord to interger i,j,k co-ord*/
{
  float x,y,z;
  z=xx[2]/den1.cvz;
  y=(xx[1]-z*den1.cvy)/den1.singam;
  x=xx[0]-y*den1.cosgam-z*den1.cosbe;
  ijk[0]=x/den1.delx[0] - den1.amin[0];
  ijk[1]=y/den1.delx[1] - den1.amin[1];
  ijk[2]=z/den1.delx[2] - den1.amin[2];
}

void imcv(struct griddata& den1,int xi[3],float sx[3])  /*convert from  integer xi,xj (grid point)
	 to world co-ord, identical to cv but uses vectors*/
{
  float x[3];
  int i;
  for(i=0;i<=2;i++)
    x[i]=den1.ximin[i]+xi[i]*den1.delx[i];
  sx[0]=x[0]+x[1]*den1.cosgam+x[2]*den1.cosbe;
  sx[1]=x[1]*den1.singam+x[2]*den1.cvy;
  sx[2]=x[2]*den1.cvz;
}


void mcv(struct griddata& den1,float xi[3],float sx[3])  /*convert from floating xi,xj (grid point)
	 to world co-ord, identical to cv but uses vectors*/
{
  float x[3];
  int i;
  for(i=0;i<=2;i++)
    x[i]=den1.ximin[i]+xi[i]*den1.delx[i];
  sx[0]=x[0]+x[1]*den1.cosgam+x[2]*den1.cosbe;
  sx[1]=x[1]*den1.singam+x[2]*den1.cvy;
  sx[2]=x[2]*den1.cvz;
}


int d3tod1(int cmax[],int i,int j,int k) // convert a 3 dimsional positiion to a 1 dimensional one
{
   int a = cmax[0]+2;
   int b = a+a*cmax[1];
   return  i + j*a + k*b;
}

void d1tod3(int cmax[],int d1,int d3v[3])
{ 
   int a = cmax[0]+2;
   int b = a+a*cmax[1];
   d3v[2] = d1/b;
   d3v[1] = (d1 - d3v[2]*b)/a;
   d3v[0] = d1 - d3v[2]*b -d3v[1]*a;
} 

void d1tow(struct griddata& den1,int cmax[],int ipos,float fvec[3])
/* inputs bone pos (ipos) and return float world co-ord*/
{
   int ivec[3];
   d1tod3(cmax,ipos,ivec);
   imcv(den1,ivec,fvec);// convert int grid ivec to float world co-ord fvec
}

float sqr(float x)
{
   return x*x;
}

void imat2(int** &mat,int n1,int n2)// allocates array 2 dim array (mat)
         // of type int - n1,n2,n3,n4 size of each dimension
{
   int i;
   mat =new int*[n1];
   assert(mat  !=0);
   for ( i = 0; i < n1; ++i){
      mat [i] = new int[n2];
      assert(mat [i] != 0);
   }
}

void fmat2(float** &mat,int n1,int n2)// allocates array 2 dim array (mat)
         // of type float - n1,n2,n3,n4 size of each dimension
{
   int i;
   mat =new float*[n1];
   assert(mat  !=0);
   for ( i = 0; i < n1; ++i){
      mat [i] = new float[n2];
      assert(mat [i] != 0);
   }
}

void freeimat2(int** &mat,int n1,int n2)// free memory array 3 dim array (mat)
         // of type int - n1,n2,n3,n4 size of each dimension
        // should free in reverse order of allocating
{
   int i;
   for ( i = 0; i < n1; ++i){
      delete []mat[i];
   }
   delete []mat;
}

void freefmat2(float** &mat,int n1,int n2)// free memory array 3 dim array (mat)
         // of type int - n1,n2,n3,n4 size of each dimension
        // should free in reverse order of allocating
{
   int i;
   for ( i = 0; i < n1; ++i){
      delete []mat[i];
   }
   delete []mat;
}

void imat3(int*** &mat,int n1,int n2,int n3)// allocates array 3 dim array (mat)
         // of type int - n1,n2,n3,n4 size of each dimension
{
   int i,j;
   mat =new int**[n1];
   assert(mat  !=0);
   for ( i = 0; i < n1; ++i){
      mat [i] = new int*[n2];
      assert(mat [i] != 0);
   }
   for ( i = 0; i < n1; ++i)
      for ( j = 0; j < n2; ++j){
         mat [i][j]=new int[n3];
         assert(mat [i][j] != 0);
      }
}

void freeimat3(int*** &mat,int n1,int n2,int n3)// free memory array 3 dim array (mat)
         // of type int - n1,n2,n3,n4 size of each dimension
        // should free in reverse order of allocating
{
   int i,j;
   for ( i = 0; i < n1; ++i)
      for ( j = 0; j < n2; ++j){
         delete []mat[i][j];
      }
   for ( i = 0; i < n1; ++i){
      delete []mat[i];
   }
   delete []mat;
}


void cmat2(char** &mat,int n1,int n2)// allocates array 2 dim array (mat)
         // of type char - n1,n2,n3,n4 size of each dimension
{
   int i;
   mat =new char*[n1];
   assert(mat  !=0);
   for ( i = 0; i < n1; ++i){
      mat [i] = new char[n2];
      assert(mat [i] != 0);
   }
}


void cmat3(char*** &mat,int n1,int n2,int n3)// allocates array 3 dim array (mat)
         // of type char - n1,n2,n3,n4 size of each dimension
{
   int i,j;
   mat =new char**[n1];
   assert(mat  !=0);
   for ( i = 0; i < n1; ++i){
      mat [i] = new char*[n2];
      assert(mat [i] != 0);
   }
   for ( i = 0; i < n1; ++i)
      for ( j = 0; j < n2; ++j){
         mat [i][j]=new char[n3];
         assert(mat [i][j] != 0);
      }
}
void simat3(short int*** &mat,int n1,int n2,int n3)// allocates array 3 dim array (mat)
         // of type short int - n1,n2,n3,n4 size of each dimension
{
   int i,j;
   mat =new short int**[n1];
   assert(mat  !=0);
   for ( i = 0; i < n1; ++i){
      mat [i] = new short int*[n2];
      assert(mat [i] != 0);
   }
   for ( i = 0; i < n1; ++i)
      for ( j = 0; j < n2; ++j){
         mat [i][j]=new short int[n3];
         assert(mat [i][j] != 0);
      }
}

void scmat3(signed char*** &mat,int n1,int n2,int n3)// allocates array 3 dim array (mat)
         // of type  signed char - n1,n2,n3,n4 size of each dimension
{
   int i,j;
   mat =new signed char**[n1];
   assert(mat  !=0);
   for ( i = 0; i < n1; ++i){
      mat [i] = new signed char*[n2];
      assert(mat [i] != 0);
   }
   for ( i = 0; i < n1; ++i)
      for ( j = 0; j < n2; ++j){
         mat [i][j]=new signed char[n3];
         assert(mat [i][j] != 0);
      }
}

void freescmat3(signed char*** &mat,int n1,int n2,int n3)// free memory array 3 dim array (mat)
         // of type int - n1,n2,n3,n4 size of each dimension
        // should free in reverse order of allocating
{
   int i,j;
   for ( i = 0; i < n1; ++i)
      for ( j = 0; j < n2; ++j){
         delete []mat[i][j];
      }
   for ( i = 0; i < n1; ++i){
      delete []mat[i];
   }
   delete []mat;
}


void unscmat3(unsigned char*** &mat,int n1,int n2,int n3)// allocates array 3 dim array (mat)
         // of type unsigned signed char - n1,n2,n3,n4 size of each dimension
{
   int i,j;
   mat =new unsigned char**[n1];
   assert(mat  !=0);
   for ( i = 0; i < n1; ++i){
      mat [i] = new unsigned char*[n2];
      assert(mat [i] != 0);
   }
   for ( i = 0; i < n1; ++i)
      for ( j = 0; j < n2; ++j){
         mat [i][j]=new unsigned char[n3];
         assert(mat [i][j] != 0);
      }
}

void freeunscmat3(unsigned char*** &mat,int n1,int n2,int n3)// free memory array 3 dim array (mat)
         // of type int - n1,n2,n3,n4 size of each dimension
        // should free in reverse order of allocating
{
   int i,j;
   for ( i = 0; i < n1; ++i)
      for ( j = 0; j < n2; ++j){
         delete []mat[i][j];
      }
   for ( i = 0; i < n1; ++i){
      delete []mat[i];
   }
   delete []mat;
}

void fmat3(float*** &mat,int n1,int n2,int n3)// allocates array 3 dim array (mat)
         // of type  signed char - n1,n2,n3,n4 size of each dimension
{
   int i,j;
   mat =new float**[n1];
   assert(mat  !=0);
   for ( i = 0; i < n1; ++i){
      mat [i] = new float*[n2];
      assert(mat [i] != 0);
   }
   for ( i = 0; i < n1; ++i)
      for ( j = 0; j < n2; ++j){
         mat [i][j]=new float [n3];
         assert(mat [i][j] != 0);
      }
}

void freefmat3(float*** &mat,int n1,int n2,int n3)// free memory array 3 dim array (mat)
         // of type int - n1,n2,n3,n4 size of each dimension
        // should free in reverse order of allocating
{
   int i,j;
   for ( i = 0; i < n1; ++i)
      for ( j = 0; j < n2; ++j){
//         delete mat[i][j];
         delete []mat[i][j];
      }
   for ( i = 0; i < n1; ++i){
      delete []mat[i];
   }
   delete []mat;
}

void scmat4(signed char**** &mat,int n1,int n2,int n3,int n4)// allocates array 4 dim array (mat)
         // of type signed char - n1,n2,n3,n4 size of each dimension
 //NOTE:  I think this is correct - but I really should test it
{
   int i,j,k;
if(verbose>=2)
   fprintf(fpout,"allocating 4d signed char  matrix\n");
   mat =new signed char***[n1];
   assert(mat  !=0);
   for ( i = 0; i < n1; ++i){
      mat [i] = new signed char**[n2];
      assert(mat [i] != 0);
   }
   for ( i = 0; i < n1; ++i)
      for ( j = 0; j < n2; ++j){
         mat [i][j]=new signed char*[n3];
         assert(mat [i][j] != 0);
      }
   for ( i = 0; i < n1; ++i)
      for ( j = 0; j < n2; ++j)
         for(k=0;k<n3;++k){
            mat [i][j][k]=new signed char[n4];
            assert(mat [i][j][k] != 0);
         }
if(verbose>=2)
  fprintf(fpout,"Done\n");
}

void freescmat4(signed char**** &mat,int n1,int n2,int n3)// free memory array 4 dim array (mat)
         // of type int - n1,n2,n3,n4 size of each dimension
        // should free in reverse order of allocating
{
   int i,j,k;
   for ( i = 0; i < n1; ++i)
      for ( j = 0; j < n2; ++j)
         for(k=0;k< n3;++k) {
           delete []mat[i][j][k];
        }
   for ( i = 0; i < n1; ++i)
      for ( j = 0; j < n2; ++j){
          delete []mat[i][j];
      }
   for ( i = 0; i < n1; ++i){
      delete []mat[i];
   }
   delete []mat;
}


void fmat4(float**** &mat,int n1,int n2,int n3,int n4)// allocates array 4 dim array (mat)
         // of type float - n1,n2,n3,n4 size of each dimension
{
   int i,j,k;
if(verbose>=2)
  fprintf(fpout,"allocating 4d floating pt matrix\n");
   mat =new float***[n1];
   assert(mat  !=0);
   for ( i = 0; i < n1; ++i){
      mat [i] = new float**[n2];
      assert(mat [i] != 0);
   }
   for ( i = 0; i < n1; ++i)
      for ( j = 0; j < n2; ++j){
         mat [i][j]=new float*[n3];
         assert(mat [i][j] != 0);
      }
   for ( i = 0; i < n1; ++i)
      for ( j = 0; j < n2; ++j)
         for(k=0;k<n3;++k){
            mat [i][j][k]=new float[n4];
            assert(mat [i][j][k] != 0);
         }
if(verbose>=2)
  fprintf(fpout,"Done\n");
}

void freefmat4(float**** &mat,int n1,int n2,int n3,int n4)// free memory array 4 dim array (mat)
         // of type int - n1,n2,n3,n4 size of each dimension
        // should free in reverse order of allocating
{
   int i,j,k;
   for ( i = 0; i < n1; ++i)
      for ( j = 0; j < n2; ++j)
         for(k=0;k< n3;++k) {
           delete []mat[i][j][k];
        }
   for ( i = 0; i < n1; ++i)
      for ( j = 0; j < n2; ++j){
          delete []mat[i][j];
      }
   for ( i = 0; i < n1; ++i){
      delete []mat[i];
   }
   delete []mat;
}

void fmat5(float***** &mat,int n1,int n2,int n3,int n4,int n5)// allocates array 5 dim array (mat)
         // of type float - n1,n2,n3,n4 n5 size of each dimension
{
   int i,j,k,l;
if(verbose>=2)
  fprintf(fpout,"allocating 5d floating pt matrix\n");
   mat =new float****[n1];
   assert(mat  !=0);
   for ( i = 0; i < n1; ++i){
      mat [i] = new float***[n2];
      assert(mat [i] != 0);
   }
   for ( i = 0; i < n1; ++i)
      for ( j = 0; j < n2; ++j){
         mat [i][j]=new float**[n3];
         assert(mat [i][j] != 0);
      }
   for ( i = 0; i < n1; ++i)
      for ( j = 0; j < n2; ++j)
         for(k=0;k<n3;++k){
            mat [i][j][k]=new float*[n4];
            assert(mat [i][j][k] != 0);
         }
   for ( i = 0; i < n1; ++i)
      for ( j = 0; j < n2; ++j)
         for(k=0;k<n3;++k)
            for(l=0;l<n4;++l){
               mat [i][j][k][l]=new float[n5];
               assert(mat [i][j][k][l] != 0);
            }
if(verbose>=2)
  fprintf(fpout,"Done\n");
}
void simat5(short int***** &mat,int n1,int n2,int n3,int n4,int n5)// allocates array 5 dim array (mat)
         // of type short - n1,n2,n3,n4 n5 size of each dimension
{
   int i,j,k,l;
   mat =new short int****[n1];
   assert(mat  !=0);
   for ( i = 0; i < n1; ++i){
      mat [i] = new short int***[n2];
      assert(mat [i] != 0);
   }
   for ( i = 0; i < n1; ++i)
      for ( j = 0; j < n2; ++j){
         mat [i][j]=new short int**[n3];
         assert(mat [i][j] != 0);
      }
   for ( i = 0; i < n1; ++i)
      for ( j = 0; j < n2; ++j)
         for(k=0;k<n3;++k){
            mat [i][j][k]=new short int*[n4];
            assert(mat [i][j][k] != 0);
         }
   for ( i = 0; i < n1; ++i)
      for ( j = 0; j < n2; ++j)
         for(k=0;k<n3;++k)
            for(l=0;l<n4;++l){
               mat [i][j][k][l]=new short int[n5];
               assert(mat [i][j][k][l] != 0);
            }
}

void planevec(float cn[3],float nca[3],float cdn,float sn,
                float r,float th,float th1,float vorigin[3],float vec[3])
/* finds vector vec that is in same plane as unit vectors cn and nca where
     th is angle between cn and nca; 
     cdn = cn dot nca = cos(th); sn =sqr(sin(th));
     th1 = angle between cn and vec (when both have same origin);
     th2 = th1-th=angle between nca and vec (when both have same origin);
     vorigin = origin of vec,  r is magnitude of vec
Note: th1 has sign:  based on right hand definition of rotation (I think)
*/
{
   float e,f,al,be,th2;
   int i;
   th2=th1-th; /*th1=angle between vec and n, th2=angle between vec and c*/
   e=cos(th1);
   f=cos(th2);
   al=(e-f*cdn)/sn;
   be=(f-e*cdn)/sn;
   for(i=0;i<=2;i++)
     vec[i]=r*(al*cn[i]+be*nca[i])+vorigin[i];
}

float dot(float vec1[],float vec2[]) /*returns v1 dot v2*/
{
   int i;
   float xx;
   xx=0;
   for(i=0;i<3;i++)
     xx=xx+vec1[i]*vec2[i];
   return xx;
}   

float distv(float vec1[],float vec2[]) // Returns the distance between vec 1 and vec 2
{
   int i;
   float vec[3];
   for(i=0;i<=2;i++)
     vec[i]=vec2[i]-vec1[i];
   return sqrt(dot(vec,vec) );
}


void rotatept(struct geometry geo,float rvec[3],float orig[3],float ang,
     float rpt[3])
/*
NOTE: orig can be any point located on original rotation vector axix
 routine that rotates point rpt about unit vector rvec (whose 
   origin in at orig, by ang
 rpt is point that is being rotated -
   its new value is output at end
*/
{
   int i;
   float rom[3][3],tempv[3];
   rotaxis(geo.de,geo.ep,ang,rvec,rom); /* rom is rotation matrix for rotation
         of chi about unit vector u2 which is assumed to be at origin*/  
   tranm(rpt,orig); /* translate c4 so u2 (origin of u2) is at origin */
   multvec(rom,rpt,tempv); /*mult rot matrix times vector rpt */
   for(i=0;i<=2;i++)
      rpt[i]=tempv[i];
   tran(rpt,orig); /* translate back to original positon */
} 

void rotateset(float orig[3],float rpt[3],float rom[3][3])
/*
NOTE: orig can be any point located on original rotation vector axix
 Similar to rotatept  - excempt assumes that rotation matrix
    has already been calculated
*/
{
   int i;
   float tempv[3];

   tranm(rpt,orig); /* translate c4 so u2 (origin of u2) is at origin */
   multvec(rom,rpt,tempv); /*mult rot matrix times vector rpt */
   for(i=0;i<=2;i++)
      rpt[i]=tempv[i];
   tran(rpt,orig); /* translate back to original positon */
}

void rotatesetnew(float orig[3],float rpt[3],float tempv[3],float rom[3][3])
/* Similar to rotatept  - excempt assumes that rotation matrix
    has already been calculated
 Similar to roateset except leaves rpt unchanged and return new vector tempv
*/
{
   float vnew[3];

   trannewm(rpt,tempv,orig); /* translate c4 so u2 (origin of u2) is at origin */
   multvec(rom,tempv,vnew); /*mult rot matrix times vector rpt */
   trannew(vnew,tempv,orig); /* translate back to original positon */
}

void rotaxis(int de[3][3],int ep[3][3][3],float phi,float nca[3],
                       float rotm[3][3]) 
/*rot is the rotation matrix for rotation of phi about 
       unit vector nca; vector nca must pass through origin*/
{
   int i,k,m;
   float cosp,sinp,summ;
   cosp=cos(phi);
   sinp=sin(phi);
   for(i=0;i<=2;i++)
     for(k=0;k<=2;k++) {
        summ=0;
        for(m=0;m<=2;m++)
          summ=summ+ep[i][k][m]*nca[m];
        rotm[i][k]=cosp*de[i][k]+(1-cosp)*nca[i]*nca[k]-sinp*summ;
     }
} 
       
void tranm(float v[3],float t[3]) /*return vector v=v-t*/
{
   int i;
   for(i=0;i<3;i++)
      v[i]=v[i]-t[i];
}
       
void trannewm(float v[3],float vnew[3],float t[3]) /*return vector v=v-t*/
{
   int i;
   for(i=0;i<3;i++)
      vnew[i]=v[i]-t[i];
}
      
void tran(float v[3],float t[3]) /*return vector v=v+t*/
{
   int i;
   for(i=0;i<3;i++)
      v[i]=v[i]+t[i];
}


void trannew(float v[3],float vnew[3],float t[3]) /*return vector v=v+t*/
{
   int i;
   for(i=0;i<3;i++)
      vnew[i]=v[i]+t[i];
}


void crossprod(float va[3],float vb[3],float vcross[3]) /* (float) cross prod. 
         of 2 float  vectors*/
{
  vcross[0]=va[1]*vb[2]-va[2]*vb[1];
  vcross[1]=va[2]*vb[0]-va[0]*vb[2];
  vcross[2]=va[0]*vb[1]-va[1]*vb[0];
}

void imultvec(int m[3][3],float v1[],float v[]) /* returns vector v = m*v1*/
{
   int i,k;
   for(i=0;i<3;i++){
      v[i]=0;
      for(k=0;k<3;k++)
        v[i]=v[i]+m[i][k]*v1[k];
   }
}

void multvec(float m[3][3],float v1[],float v[]) /* returns vector v = m*v1*/
{
   int i,k;
   for(i=0;i<3;i++){
      v[i]=0;
      for(k=0;k<3;k++)
        v[i]=v[i]+m[i][k]*v1[k];
   }
}

void rottranvec(float rot[3][3],float tran[3],float v1[3],float v2[3])
//  v2 = R x v1 + tran
{
   float vnew[3];
   multvec(rot,v1,vnew); /*mult rot matrix times vector rpt */
   trannew(vnew,v2,tran); // translate to final Ca pos
}

float determ(float a[3][3])
/* determinant of 3x3 floating point matrix */
{
  float det;
  det=a[0][0]*(a[1][1]*a[2][2]-a[1][2]*a[2][1])-a[0][1]*(a[1][0]*a[2][2]-a[1][2]*a[2][0]);
  det=det+a[0][2]*(a[1][0]*a[2][1]-a[1][1]*a[2][0]);
  return(det);
}

void nrerror(const char error_text[])
/* Numerical Recipes standard error handler */
{
	fprintf(stderr,"Numerical Recipes run-time error...\n");
	fprintf(stderr,"%s\n",error_text);
	fprintf(stderr,"...now exiting to system...\n");
	exit(1);
}


void findchi(float p0[3],float p1[3],float p2[3],float p3[3],float *chi)
/* finds chi of vector p3 to w, given the 3 previous pooints
i.e. given N,Ca,Cb, the chi for Cg is given by findchi(N,Ca,Cb,Cg,chi)
Procddure:
  define unit vectors u=p0-p1; v=p2-p1,w1=3-p2;
    x is vector in uv plane perp. to v;
       = u/sin(al) -v*cos(al)/sin(al)
     where al is angle between v and u
    wp= perp proj of w1 onto plane perp.to v: = v x (w1 x v)
             w1-(v dot w1)v  (see Sands, p. 45)
  abs. val of chi = acos(wp,x)
  To find sign of chi, define wc= x cross wp
    if chi>0, then wc will be along v
        else along -v
*/
{
   int i;
   float v[3],u[3],w1[3],x[3],wp[3],mag1,mag2;
   float cosal, sinal,wv[3],vdotw,dt,xx;

   for(i=0;i<=2;i++){
      u[i]=p0[i]-p1[i];
      v[i]=p2[i]-p1[i];
      w1[i]=p3[i]-p2[i];
   }
   mag1=sqrt(dot(u,u));
   mag2=sqrt(dot(v,v));
   for(i=0;i<=2;i++){
      u[i]=u[i]/mag1;
      v[i]=v[i]/mag2;
   }
   cosal=dot(u,v); /* cos(al) */
   sinal=sqrt(1-cosal*cosal); /* sin(al) */
   for(i=0;i<=2;i++) /* x will be unit vector */
      x[i]=(u[i] -v[i]*cosal)/sinal;
   vdotw=dot(v,w1);
   for(i=0;i<=2;i++)
      wp[i]=w1[i]-vdotw*v[i]; /* perp. proj of w1 onto plance Perp. to v*/
   mag1=sqrt(dot(wp,wp));
   dt=dot(x,wp);
   xx=dt/mag1;
   if(xx>=1.0)
         *chi=0.0;
   else if(xx<= -1.0)
         *chi=PI;
   else
     *chi = acos(xx);
   crossprod(x,wp,wv); /* find sign of chi */
   vdotw=dot(v,wv)/mag1;
   if(vdotw<0)
      *chi = -(*chi);
} 


void findchivec(float u[3],float v[3],float w1[3],float *chi)
/*
New Version:  =Input the 3 vectors that are used, not the points
 Similar to findchi routine in old maid
  But, assumes there is a break between 3 and fourth pt
  so that last vector formed from point 4 and 5,
     not point 3 and 4 as in old routine
OLD DESCRIPTION:	
finds chi of vector p3 to w, given the 3 previous pooints
i.e. given N,Ca,Cb, the chi for Cg is given by findchi(N,Ca,Cb,Cg,chi)
Procddure:
  define unit vectors u=p0-p1; v=p2-p1,w1=3-p2;
    x is vector in uv plane perp. to v;
       = u/sin(al) -v*cos(al)/sin(al)
     where al is angle between v and u
    wp= perp proj of w1 onto plane perp.to v: = v x (w1 x v)
             w1-(v dot w1)v  (see Sands, p. 45)
  abs. val of chi = acos(wp,x)
  To find sign of chi, define wc= x cross wp
    if chi>0, then wc will be along v
        else along -v
*/
{
   int i;
   float x[3],wp[3],mag1,mag2;
   float cosal, sinal,wv[3],vdotw,dt,xx;

#if 0
   for(i=0;i<=2;i++){
      u[i]=p0[i]-p1[i];
      v[i]=p2[i]-p1[i];
      w1[i]=p4[i]-p3[i];  // This is the new dechange from old routine
   }
#endif
   for(i=0;i<=2;i++) // change sign of u, see above
      u[i] = -u[i]; 
   mag1=sqrt(dot(u,u));
   mag2=sqrt(dot(v,v));
   for(i=0;i<=2;i++){
      u[i]=u[i]/mag1;
      v[i]=v[i]/mag2;
   }
   cosal=dot(u,v); /* cos(al) */
   sinal=sqrt(1-cosal*cosal); /* sin(al) */
   for(i=0;i<=2;i++) /* x will be unit vector */
      x[i]=(u[i] -v[i]*cosal)/sinal;
   vdotw=dot(v,w1);
   for(i=0;i<=2;i++)
      wp[i]=w1[i]-vdotw*v[i]; /* perp. proj of w1 onto plance Perp. to v*/
   mag1=sqrt(dot(wp,wp));
   dt=dot(x,wp);
   xx=dt/mag1;
   if(xx>=1.0)
         *chi=0.0;
   else if(xx<= -1.0)
         *chi=PI;
   else
     *chi = acos(xx);
   crossprod(x,wp,wv); /* find sign of chi */
   vdotw=dot(v,wv)/mag1;
   if(vdotw<0)
      *chi = -(*chi);
} 


void printmat3(char aname[20],float a[3][3])
{
   int i,k,n;
   
   n=3; 
   fprintf(fpout,"mat %s =:\n",aname);
   for(i=0;i<n;i++){
      for(k=0;k<n;k++)
         fprintf(fpout," %f",a[i][k]);
      fprintf(fpout,"\n");
   }
}


void printmat6(char aname[20],float a[6][6])
{
   int i,k,n;
   
   n=6; 
   fprintf(fpout,"mat %s =:\n",aname);
   for(i=0;i<n;i++){
      for(k=0;k<n;k++)
         fprintf(fpout," %f",a[i][k]);
      fprintf(fpout,"\n");
   }
}
void mult11_61(float aa,float bb[6],float cc[6])
/* mult. a constant (1x1 matrix) times a 6x1 or 1x6 vector*/
{
    int i;

    for(i=0;i<6;i++)
      cc[i]=aa*bb[i];
}

void tran66(float aa[6][6],float cc[6][6])
/* transpose a 6x6 matrix */
{
   int i,j;
 
   for(i=0;i<6;i++)
      for(j=0;j<6;j++)
         cc[i][j]=aa[j][i];
}

     
void add66_66(float aa[6][6],float bb[6][6],float cc[6][6])
/* add 6x6 matrix to a 6x6 matrix*/
{
   int i,j;

   for(i=0;i<6;i++)
      for(j=0;j<6;j++)
         cc[i][j]=aa[i][j]+bb[i][j];
}
                    
void mult33_33(float aa[3][3],float bb[3][3],float cc[3][3])
/* multiply a 3x3 matrix times a 3x3 mat */
{
   int i,j,k;

   for(i=0;i<3;i++)
     for(j=0;j<3;j++)
        cc[i][j]=0;
   for(i=0;i<3;i++)
     for(j=0;j<3;j++)
        for(k=0;k<3;k++)
          cc[i][j]=cc[i][j]+aa[i][k]*bb[k][j];
}
         
void mult66_66(float aa[6][6],float bb[6][6],float cc[6][6])
/* multiply a 6x6 matrix times a 6x6 mat */
{
   int i,j,k;

   for(i=0;i<6;i++)
     for(j=0;j<6;j++)
        cc[i][j]=0;
   for(i=0;i<6;i++)
     for(j=0;j<6;j++)
        for(k=0;k<6;k++)
          cc[i][j]=cc[i][j]+aa[i][k]*bb[k][j];
}

void mult33_31(float aa[3][3],float bb[3],float cc[3])
/* multiply a 3x3 matrix times a 3x1 mat */
{
   int i,k;

   for(i=0;i<3;i++)
      cc[i]=0;
   for(i=0;i<3;i++)
      for(k=0;k<3;k++)
         cc[i]=cc[i]+aa[i][k]*bb[k];
}


void mult66_61(float aa[6][6],float bb[6],float cc[6])
/* multiply a 6x6 matrix times a 6x1 mat */
{
   int i,k;

   for(i=0;i<6;i++)
      cc[i]=0;
   for(i=0;i<6;i++)
      for(k=0;k<6;k++)
         cc[i]=cc[i]+aa[i][k]*bb[k];
}

void mult16_66(float aa[6],float bb[6][6],float cc[6])
/* multiply a 1(row) x 6 (column) times a 6x6 matrix*/
{
    int i,k;

    for(i=0;i<6;i++)
       cc[i]=0;
    for(i=0;i<6;i++)
       for(k=0;k<6;k++)
          cc[i]=cc[i]+aa[k]*bb[k][i];
}

void mult16_61(float aa[6],float bb[6],float *cc)
/* mult. a 1 by 6 matrix times a 6 x 1 matrix - retruns a float*/
{
    int k;

    *cc=0;
    for(k=0;k<6;k++)
      *cc= *cc+aa[k]*bb[k];
}

void mult61_16(float aa[6],float bb[6],float cc[6][6])
/* mult. a 6x1 matrix times a 1x6 matrix*/
{
   int i,j;

   for(i=0;i<6;i++)
     for(j=0;j<6;j++)
        cc[i][j]=aa[i]*bb[j];
}
                             
void myludcmp(float a[6][6],int indx[6],float *d)
{
	int i,imax,j,k,n;
	float big,dum,sum,temp,vv[6];

        n=6;
	*d=1.0;
	for (i=0;i<n;i++) {
		big=0.0;
		for (j=0;j<n;j++)
			if ((temp=fabs(a[i][j])) > big) big=temp;
		if (big == 0.0) fprintf(fpout,"Singular matrix in routine LUDCMP\n");
		vv[i]=1.0/big;
	}
	for (j=0;j<n;j++) {
		for (i=0;i<j;i++) {
			sum=a[i][j];
			for (k=0;k<i;k++) sum -= a[i][k]*a[k][j];
			a[i][j]=sum;
		}
		big=0.0;
		for (i=j;i<n;i++) {
			sum=a[i][j];
			for (k=0;k<j;k++)
				sum -= a[i][k]*a[k][j];
			a[i][j]=sum;
			if ( (dum=vv[i]*fabs(sum)) >= big) {
				big=dum;
				imax=i;
			}
		}
		if (j != imax) {
			for (k=0;k<n;k++) {
				dum=a[imax][k];
				a[imax][k]=a[j][k];
				a[j][k]=dum;
			}
			*d = -(*d);
			vv[imax]=vv[j];
		}
		indx[j]=imax;
		if (a[j][j] == 0.0) a[j][j]=TINY;
		if (j != (n-1)) {
			dum=1.0/(a[j][j]);
			for (i=j+1;i<n;i++) a[i][j] *= dum;
		}
	}
}


void mylubksb(float a[6][6],int indx[6],float b[])
{
	int i,ii= -1,ip,j,n;
	float sum;
       
        n=6;
	for (i=0;i<n;i++) {
		ip=indx[i];
		sum=b[ip];
		b[ip]=b[i];
		if (ii+1)
	          for (j=ii;j<=i-1;j++) sum -= a[i][j]*b[j];
		else if (sum) ii=i;
		b[i]=sum;
	}
	for (i=n-1;i>=0;i--) {
		sum=b[i];
		for (j=i+1;j<n;j++) sum -= a[i][j]*b[j];
		b[i]=sum/a[i][i];
	}
}

void mymatinv(float a[6][6], float ai[6][6])
{
   int n,i,j,indx[6];
   float col[6],d;

   n=6;
   myludcmp(a,indx,&d);
   for (j=0;j<n;j++) {
      for( i=0;i<n;i++) col[i]=0.0;
      col[j]=1.0;
      mylubksb(a,indx,col);
      for(i=0;i<n;i++)
          ai[i][j]=col[i];
  }
} 

void myludcmp3(float a[3][3],int indx[3],float *d)
{
	int i,imax,j,k,n;
	float big,dum,sum,temp,vv[3];

        n=3;
	*d=1.0;
	for (i=0;i<n;i++) {
		big=0.0;
		for (j=0;j<n;j++)
			if ((temp=fabs(a[i][j])) > big) big=temp;
		if (big == 0.0) printf("Singular matrix in routine LUDCMP\n");
		vv[i]=1.0/big;
	}
	for (j=0;j<n;j++) {
		for (i=0;i<j;i++) {
			sum=a[i][j];
			for (k=0;k<i;k++) sum -= a[i][k]*a[k][j];
			a[i][j]=sum;
		}
		big=0.0;
		for (i=j;i<n;i++) {
			sum=a[i][j];
			for (k=0;k<j;k++)
				sum -= a[i][k]*a[k][j];
			a[i][j]=sum;
			if ( (dum=vv[i]*fabs(sum)) >= big) {
				big=dum;
				imax=i;
			}
		}
		if (j != imax) {
			for (k=0;k<n;k++) {
				dum=a[imax][k];
				a[imax][k]=a[j][k];
				a[j][k]=dum;
			}
			*d = -(*d);
			vv[imax]=vv[j];
		}
		indx[j]=imax;
		if (a[j][j] == 0.0) a[j][j]=TINY;
		if (j != (n-1)) {
			dum=1.0/(a[j][j]);
			for (i=j+1;i<n;i++) a[i][j] *= dum;
		}
	}
}


void mylubksb3(float a[3][3],int indx[3],float b[])
{
	int i,ii= -1,ip,j,n;
	float sum;
       
        n=3;
	for (i=0;i<n;i++) {
		ip=indx[i];
		sum=b[ip];
		b[ip]=b[i];
		if (ii+1)
	          for (j=ii;j<=i-1;j++) sum -= a[i][j]*b[j];
		else if (sum) ii=i;
		b[i]=sum;
	}
	for (i=n-1;i>=0;i--) {
		sum=b[i];
		for (j=i+1;j<n;j++) sum -= a[i][j]*b[j];
		b[i]=sum/a[i][i];
	}
}

void mymatinv3(float a[3][3], float ai[3][3])
{
   int n,i,j,indx[3];
   float col[3],d;

   n=3;
   myludcmp3(a,indx,&d);
   for (j=0;j<n;j++) {
      for( i=0;i<n;i++) col[i]=0.0;
      col[j]=1.0;
      mylubksb3(a,indx,col);
      for(i=0;i<n;i++)
          ai[i][j]=col[i];
  }
} 

void ludcmp(int ndim,float a[5][5],int indx[],float *d)
{
	int i,imax,j,k,n;
	float big,dum,sum,temp,vv[5];

        n=ndim;
	*d=1.0;
	for (i=1;i<=n;i++) {
		big=0.0;
		for (j=1;j<=n;j++)
			if ((temp=fabs(a[i][j])) > big) big=temp;
		if (big == 0.0) fprintf(fpout,"Singular matrix in routine LUDCMP\n");
		vv[i]=1.0/big;
	}
	for (j=1;j<=n;j++) {
		for (i=1;i<j;i++) {
			sum=a[i][j];
			for (k=1;k<i;k++) sum -= a[i][k]*a[k][j];
			a[i][j]=sum;
		}
		big=0.0;
		for (i=j;i<=n;i++) {
			sum=a[i][j];
			for (k=1;k<j;k++)
				sum -= a[i][k]*a[k][j];
			a[i][j]=sum;
			if ( (dum=vv[i]*fabs(sum)) >= big) {
				big=dum;
				imax=i;
			}
		}
		if (j != imax) {
			for (k=1;k<=n;k++) {
				dum=a[imax][k];
				a[imax][k]=a[j][k];
				a[j][k]=dum;
			}
			*d = -(*d);
			vv[imax]=vv[j];
		}
		indx[j]=imax;
		if (a[j][j] == 0.0) a[j][j]=TINY;
		if (j != n) {
			dum=1.0/(a[j][j]);
			for (i=j+1;i<=n;i++) a[i][j] *= dum;
		}
	}
}



void lubksb(int ndim,float a[5][5],int indx[],float b[])
{
	int i,ii=0,ip,j,n;
	float sum;
       
        n=ndim;
	for (i=1;i<=n;i++) {
		ip=indx[i];
		sum=b[ip];
		b[ip]=b[i];
		if (ii)
	          for (j=ii;j<=i-1;j++) sum -= a[i][j]*b[j];
		else if (sum) ii=i;
		b[i]=sum;
	}
	for (i=n;i>=1;i--) {
		sum=b[i];
		for (j=i+1;j<=n;j++) sum -= a[i][j]*b[j];
		b[i]=sum/a[i][i];
	}
}

   
void solvelineq(int ndim,float a[5][5],float b[])
/* solves linear eqs: Ax = b, solution returnen in b */
{
   int indx[5];
   float d;
   ludcmp(ndim,a,indx,&d);
   lubksb(ndim,a,indx,b);
}

void itoa(int ii,char ch[])
/* converts integer to asci
New verson: uses sprintf routine
*/
{
   sprintf(ch,"%d",ii);
}


void newrankqual(float qual,int ranki[4][MAXRANK],int intprop[4],
           float rankf[MAXRANK])
/* Modified rankqual to handle 4 interger properties associated with qual 
      and to be more general
    MAXRANK = maxinum of stored rankings
    qual = floating point value that is being ranked - associated with rankf
    intprop[0,1,2,3] are integers associated with qual
    ranki[0][0] = total number ranked so far
    ranki[i][1,2,3,4...] = the best, nextbest,etc of intprop[i] associated with qual
    rankf[1,2,3,..] = qual of best qual,second bext  etc. etc.
NOTE: ASSUMES THAT BEFORE ROUTINE IS CALLED:
        ranki[0][0] is set = 0
*/    
{
    int ii,jj,k;
    for(ii =1;ii<=ranki[0][0];ii++){
      if(qual> rankf[ii]){// then insert here
         for(jj=ranki[0][0];jj>=ii;jj--){ // lower ranking of others
           for(k=0;k<=3;k++)
             ranki[k][jj+1] = ranki[k][jj];
           rankf[jj+1] = rankf[jj];
         }
         for(k=0;k<=3;k++)
            ranki[k][ii] = intprop[k];
         rankf[ii] = qual;         
         if(ranki[0][0]<MAXRANK-2) // if has not reached maxrank, than add another
            ranki[0][0] = ranki[0][0]+1;
         goto quitrank;
      }// if
   } // ii
   if(ranki[0][0]<MAXRANK-2){ //if reach here and if has not reached maxrank, than add another
      ranki[0][0] = ranki[0][0]+1; // if reach here then just insert at end
      for(k=0;k<=3;k++)
         ranki[k][ ranki[0][0] ] = intprop[k];
      rankf[ ranki[0][0] ] = qual;
   }
   quitrank:;
}

void rankqual(int maxrank,int ranki[],int ranki2[],float rankf[],int ivalue,float qual,
     int gapnum)
/* General routine for ranking set of data
    maxrank = maxinum of stored rankings
    qual = floating point value that is being ranked - associated with rankf
    ivalue = int  property that is associated with qual - stored in ranki
    gapnum = int  property that is associated with qual - stored in ranki2
    ranki[0] = total number ranked so far
    ranki[1,2,3,4...] = the best, nextbest,etc of ivalue (interger = (i.e.)sequence number) associated with best fit
    ranki2[1,2,3,4...] = the best, nextbest,etc of gapnum ( another integer)  associated with best fit
    rankf[1,2,3,..] = qual of best qual,second bext  etc. etc.
NOTE: ASSUMES THAT BEFORE ROUTINE IS CALLED:
        ranki[0] is set = 0
*/    
{
    int ii,jj;
    for(ii =1;ii<=ranki[0];ii++){
      if(qual> rankf[ii]){// then insert here
         for(jj=ranki[0];jj>=ii;jj--){ // lower ranking of others
             ranki[jj+1] = ranki[jj];
             ranki2[jj+1] = ranki2[jj];
             rankf[jj+1] = rankf[jj];
         }
         ranki[ii] = ivalue;
         ranki2[ii] = gapnum;
         rankf[ii] = qual;         
         if(ranki[0]<maxrank-2) // if has not reached maxrank, than add another
            ranki[0] = ranki[0]+1;
         goto quitrank;
      }// if
   } // ii
   if(ranki[0]<maxrank-2){ //if reach here and if has not reached maxrank, than add another
      ranki[0] = ranki[0]+1; // if reach here then just insert at end
      ranki[ ranki[0] ] = ivalue;
      ranki2[ ranki[0] ] = gapnum;
      rankf[ ranki[0] ] = qual;
   }
   quitrank:;
}

void rankqual3(int maxrank,int ranki[],int ranki2[],int ranki3[],float rankf[],int ivalue,
     float qual,int gapnum1,int gapnum2)
/* 
  Same as rankqual but uses two gap numbers - second gap in ranki3
*/    
{
    int ii,jj;
    for(ii =1;ii<=ranki[0];ii++){
      if(qual> rankf[ii]){// then insert here
         for(jj=ranki[0];jj>=ii;jj--){ // lower ranking of others
             ranki[jj+1] = ranki[jj];
             ranki2[jj+1] = ranki2[jj];
             ranki3[jj+1] = ranki3[jj];
             rankf[jj+1] = rankf[jj];
         }
         ranki[ii] = ivalue;
         ranki2[ii] = gapnum1;
         ranki3[ii] = gapnum2;
         rankf[ii] = qual;         
         if(ranki[0]<maxrank-2) // if has not reached maxrank, than add another
            ranki[0] = ranki[0]+1;
         goto quitrank;
      }// if
   } // ii
   if(ranki[0]<maxrank-2){ //if reach here and if has not reached maxrank, than add another
      ranki[0] = ranki[0]+1; // if reach here then just insert at end
      ranki[ ranki[0] ] = ivalue;
      ranki2[ ranki[0] ] = gapnum1;
      ranki3[ ranki[0] ] = gapnum2;
      rankf[ ranki[0] ] = qual;
   }
   quitrank:;
}

void orderlist(int del[],int order[])
/*
  Inputs an order list of integers in del[] and outputs
the list ordered with lowest value, first
*/
{
   int i,j,k;

   if(del[0]>0){
    order[0]=1;
    order[1]=del[1]; // initialize order
    for(i=2;i<=del[0];i++){
      for(j=1;j<=order[0];j++){
         if(del[i]<order[j]){ // insert here
             for(k=order[0];k>=j;k--) // shiftup
                 order[k+1]=order[k];
             order[j]=del[i];
             order[0]=order[0]+1;
             goto nexti;
         }//insert
      }//j
      order[0]=order[0]+1; // if reach here, insert at end
      order[ order[0] ] = del[i];
      nexti:;
    }//i
   }// del[0]>0
   else
     order[0]=0;   
}
