/* 
On 30mar00:  Reduced random force on side chains by 50%.

16nov99:  Problem with angle >2PI.  MAIN problem in sheets
  Big improvement by commenting out
#if 0
           if (ang > 110.0*PI/180.0)// for sheet allow angle to phi < -180.0 - which is shfifted to -180
               ang = ang-2.0*PI;
#endif
  in findangsign()
I think this cause shift in value of sign so that applied torfor
    causes problem
However, still may occasionally have problem when two neighboring bond
   angles get too big.  Since torsional correction applies opposite force
   to each unit at each end of bond - this can cause problem.  Need some
   procedure for preventing from blowing up 
  Thus, set geo.quit = 1 and exit from tordynbat
*/


#include "peptide.h"
#include "refine.h"
#include "extendfit.h"
#include "trace.h"
#include "tordyn.h"
#include "math.h"
#include "pdbtolev.h"
 
extern int verbose; //Defined in trace.c++  if = 1, print a lot of stuff
extern FILE *fpout; // = stdout or logfile, depending whether batch or screen version
extern struct extbones ext; // All the data for edited bones used to extend one end of a given fit

float maxvel =  0.00;
  
void rndnorm(float rn[2])
/*returns 2  (rn[0,1]) values with random normal distriubiton,
    mean=0;  stddev=1;*/
{
   int i;
   float u[2],v[2],ss;   
   do{
     for(i=0;i<2;i++){
        u[i]= double( rand() ) / double( RAND_MAX );
        v[i]=2*u[i]-1;
     }
     ss=v[0]*v[0]+v[1]*v[1];
   } while(ss>=1);
   for(i=0;i<=1;i++)
     rn[i]=v[i]*sqrt(-2.0*log(ss)/ss);
}

 
void nstep1(int de[3][3],int ep[3][3][3],float delt,
    float ****unit,float ****unitv,float *yy,
    float dyy[3],float dr[4][3],float drca[3])
/*
New -  modified step1 from bonedyn.c   - Assumes first unit is unit2
  Rotate and translate first peptide
*/
{
   int i,k,natoms;
   float wave[3],wcr[3];
   float tranv[3];
//float testv[3];

   natoms=4; /* since for first unit2 (Ca - C - O - N) */
   for(k=0;k<3;k++){/* wave= ave ang. rot. rate during time step*/
         wave[k]=yy[k+3]+0.5*dyy[k+3]*delt;
         tranv[k] = yy[k]*delt+0.5*dyy[k]*delt*delt;
   }
   for(i=0;i<natoms;i++){ /* sum over atoms in first unit2*/
      crossprod(wave,dr[i],wcr);
      for(k=0;k<3;k++){
         unitv[1][1][i][k]=(tranv[k]+wcr[k]*delt)/delt;
         unit[1][1][i][k]=
              unit[1][1][i][k]+delt*unitv[1][1][i][k];
      }
   }
   crossprod(wave,drca,wcr); // roatete and translate next Ca
   for(k=0;k<3;k++){
       unitv[0][1][0][k]= (tranv[k]+wcr[k]*delt)/delt;
       unit[0][1][0][k]=
              unit[0][1][0][k]+delt*unitv[0][1][0][k];
   }
}


   
void makeunitalanine(struct geometry& geo,float ****unit,float ****unitv,
          int ifind,int iend,int iu,float phi,float psi,float delt) 
/*Similar to make alanine - except makes new unit1 and unit2:
  Given previous atoms in iu  - makes new iu+1
OLD:
 Given C1, N2, and Ca2, find next  c2,o2,cb2,n3,ca3etc if  aanum=2
aanum is the number of last a.a, make aa=anmum+1;
NOTE: does not assume ideal length and angles
1=ca,2=c,3=o;4=n,5=cb; 6=cb2 in pos
if ifind=1, do not find o or cb
it iend = 1, means at end, add Ca to last unit2
New:  
  c1 = unit[1][i][0]; N2 = unit[1][i][1];  ca2 = unit[0][i][0];
make new:  c2 = unit[1][i+1][0]; o2 = unit[1][i+1][2];
   cb2 = unit[0][i][1];n3 = unit[1][i+1][1]; ca3=unit[0][i+1][0]
*/
{
   int i,k;
   float uv[3],vv[3],cc[3],cb[3],can[3],cac[3],nt[3];
   float mag1,mag2,udv,sn,th1,th,c[3],n[3],rom[3][3];
   float oldc[3],oldn[3],oldca[3],oldcb[3],oldo[3];

   for(k=0;k<=2;k++){ // store initial positions to find velocity
     oldc[k]=unit[1][iu+1][0][k]; //C
     oldn[k]=unit[1][iu+1][1][k]; //N
     if(iend==1) // add Ca to last unit2
       oldca[k]=unit[1][iu+1][3][k];
     else  // Ca
       oldca[k]=unit[0][iu+1][0][k];
     if(ifind!=1){ // Find O and Cb
       oldcb[k]=unit[0][iu][1][k];//Cb
       oldo[k]=unit[1][iu+1][2][k];
     }
   }//k

   /*here we set phi=psi=0 and find positons of c and n*/
   for(k=0;k<=2;k++) { /*these are the known initial (unit) vectors*/
      uv[k]=unit[1][iu][1][k]-unit[1][iu][0][k]; /*the C to N vector*/
      vv[k]=unit[0][iu][0][k]-unit[1][iu][1][k]; /* the N to Ca vector*/
   }
   mag1=sqrt(dot(uv,uv));
   mag2=sqrt(dot(vv,vv));
   for(k=0;k<=2;k++){
      uv[k]=uv[k]/mag1;
      vv[k]=vv[k]/mag2;
   }
   rotaxis(geo.de,geo.ep,phi,vv,rom); /*rot is the rotation matrix for rotation of phi about 
       unit vector nca, vector nca must pass through origin*/
   udv=dot(uv,vv); /*u dot v */
   th= -acos(udv);/*theta= angle between 2 basis vectors u and cvthis must
          correct sign so that th2=th1-th  <0 is clockwise*/
   sn=1-udv*udv; /*= sqr(sin(theta)*/
   th1=th-PI+geo.tt[4];
   planevec(uv,vv,udv,sn,geo.dsqrt[1],th,th1,unit[0][iu][0],c); 
           /*returns vector c in same plane as u and v, with ca2 as origin*/
   th1=th1-PI+geo.tt[1];
   planevec(uv,vv,udv,sn,geo.dsqrt[2],th,th1,c,n); 
   if(ifind!=1){
     /*for cb need define a third vector to define cbeta which is out of plane*/
     for(i=0;i<=2;i++){
        can[i]= -vv[i]; /* the ca-n vector*/
        cac[i]=(c[i]-unit[0][iu][0][i])/geo.dsqrt[1]; /* new ca-c vector*/
     }
     crossprod(can,cac,cc); /*cc is the component perpendicula to nca,cac*/
     for(i=0;i<=2;i++){ /* the geo.position of cb*/
       cc[i]=cc[i]/sin(geo.tt[4]);
       cb[i]=geo.dsqrt[1]*(-0.5*can[i]-0.5*cac[i]+sqrt(2.0/3.0)*cc[i])
                  +unit[0][iu][0][i];
     }
   /*rotate cb,c,n2 by phi about n-ca=vv with n at origin*/
     tranm(cb,unit[1][iu][1]);multvec(rom,cb,unit[0][iu][1]);
     tran(unit[0][iu][1],unit[1][iu][1]); //new unit[0][iu][1]= Cb
   } /* if ifind!=1*/
   tranm(c,unit[1][iu][1]); /*translates c to c-n, puts n at origin*/
   tranm(n,unit[1][iu][1]);
   multvec(rom,c,unit[1][iu+1][0]);
   multvec(rom,n,nt);
   tran(unit[1][iu+1][0],unit[1][iu][1]);// New unit[1][iu+1][0] = C
   tran(nt,unit[1][iu][1]);
  /*rotate nt by psi about cac*/
   for(i=0;i<=2;i++)
     cac[i]=(unit[1][iu+1][0][i]-unit[0][iu][0][i])/geo.dsqrt[1]; /* this is different than cac above*/
   rotaxis(geo.de,geo.ep,psi,cac,rom);
   tranm(nt,unit[0][iu][0]);
   multvec(rom,nt,unit[1][iu+1][1]); 
   tran(unit[1][iu+1][1],unit[0][iu][0]);//New unit[1][iu+1][1] = N
   /* add ca and o in same plane as ca-c and c-n*/
   for(k=0;k<=2;k++){
     uv[k]=(unit[1][iu+1][0][k]-unit[0][iu][0][k])/geo.dsqrt[1];
     vv[k]=(unit[1][iu+1][1][k]-unit[1][iu+1][0][k])/geo.dsqrt[2];
   }
   th= -PI+geo.tt[1];
   th1=th+PI-geo.tt[3];
   udv=cos(th); /* = cn dot nca = cos(theta)*/
   sn=1-udv*udv; /*= sqr(sin(theta)*/
   if(iend==1) // add Ca to last unit2
      planevec(uv,vv,udv,sn,geo.dsqrt[4],th,th1,unit[1][iu+1][1],
               unit[1][iu+1][3]);// Next Ca 
   else
      planevec(uv,vv,udv,sn,geo.dsqrt[4],th,th1,unit[1][iu+1][1],
               unit[0][iu+1][0]);// Next Ca 
   if(ifind!=1){ // Find O
     th1=PI-geo.tt[2];   
     planevec(uv,vv,udv,sn,geo.dsqrt[3],th,th1,unit[1][iu+1][0],
               unit[1][iu+1][2]); // Add o
   }
   for(k=0;k<=2;k++){ // find velocity
     unitv[1][iu+1][0][k]=(unit[1][iu+1][0][k]-oldc[k])/delt; //C
     unitv[1][iu+1][1][k]=(unit[1][iu+1][1][k]-oldn[k])/delt; //N
     if(iend==1) // add Ca to last unit2
       unitv[1][iu+1][3][k]=(unit[1][iu+1][3][k]-oldca[k])/delt;
     else  // Ca
       unitv[0][iu+1][0][k]=(unit[0][iu+1][0][k]-oldca[k])/delt;
     if(ifind!=1){ // Find O and Cb
       unitv[0][iu][1][k]=(unit[0][iu][1][k]-oldcb[k])/delt;//Cb
       unitv[1][iu+1][2][k]=(unit[1][iu+1][2][k]-oldo[k])/delt;//O
     }
   }//k
}

   
void makecisunitalanine(struct geometry& geo,float ****unit,float ****unitv,
          int ifind,int iend,int iu,float phi,float psi,float delt) 
/*
  ONLY change geo.tt[3] (angle between C - N and new N-Ca) to
    2PI - geo.tt[3]
Similar to make alanine - except makes new unit1 and unit2:
  Given previous atoms in iu  - makes new iu+1
OLD:
 Given C1, N2, and Ca2, find next  c2,o2,cb2,n3,ca3etc if  aanum=2
aanum is the number of last a.a, make aa=anmum+1;
NOTE: does not assume ideal length and angles
1=ca,2=c,3=o;4=n,5=cb; 6=cb2 in pos
if ifind=1, do not find o or cb
it iend = 1, means at end, add Ca to last unit2
New:  
  c1 = unit[1][i][0]; N2 = unit[1][i][1];  ca2 = unit[0][i][0];
make new:  c2 = unit[1][i+1][0]; o2 = unit[1][i+1][2];
   cb2 = unit[0][i][1];n3 = unit[1][i+1][1]; ca3=unit[0][i+1][0]
*/
{
   int i,k;
   float uv[3],vv[3],cc[3],cb[3],can[3],cac[3],nt[3];
   float mag1,mag2,udv,sn,th1,th,c[3],n[3],rom[3][3];
   float oldc[3],oldn[3],oldca[3],oldcb[3],oldo[3];
   float tt3;

   tt3 = 2*PI - geo.tt[3];
   for(k=0;k<=2;k++){ // store initial positions to find velocity
     oldc[k]=unit[1][iu+1][0][k]; //C
     oldn[k]=unit[1][iu+1][1][k]; //N
     if(iend==1) // add Ca to last unit2
       oldca[k]=unit[1][iu+1][3][k];
     else  // Ca
       oldca[k]=unit[0][iu+1][0][k];
     if(ifind!=1){ // Find O and Cb
       oldcb[k]=unit[0][iu][1][k];//Cb
       oldo[k]=unit[1][iu+1][2][k];
     }
   }//k

   /*here we set phi=psi=0 and find positons of c and n*/
   for(k=0;k<=2;k++) { /*these are the known initial (unit) vectors*/
      uv[k]=unit[1][iu][1][k]-unit[1][iu][0][k]; /*the C to N vector*/
      vv[k]=unit[0][iu][0][k]-unit[1][iu][1][k]; /* the N to Ca vector*/
   }
   mag1=sqrt(dot(uv,uv));
   mag2=sqrt(dot(vv,vv));
   for(k=0;k<=2;k++){
      uv[k]=uv[k]/mag1;
      vv[k]=vv[k]/mag2;
   }
   rotaxis(geo.de,geo.ep,phi,vv,rom); /*rot is the rotation matrix for rotation of phi about 
       unit vector nca, vector nca must pass through origin*/
   udv=dot(uv,vv); /*u dot v */
   th= -acos(udv);/*theta= angle between 2 basis vectors u and cvthis must
          correct sign so that th2=th1-th  <0 is clockwise*/
   sn=1-udv*udv; /*= sqr(sin(theta)*/
   th1=th-PI+geo.tt[4];
   planevec(uv,vv,udv,sn,geo.dsqrt[1],th,th1,unit[0][iu][0],c); 
           /*returns vector c in same plane as u and v, with ca2 as origin*/
   th1=th1-PI+geo.tt[1];
   planevec(uv,vv,udv,sn,geo.dsqrt[2],th,th1,c,n); 
   if(ifind!=1){
     /*for cb need define a third vector to define cbeta which is out of plane*/
     for(i=0;i<=2;i++){
        can[i]= -vv[i]; /* the ca-n vector*/
        cac[i]=(c[i]-unit[0][iu][0][i])/geo.dsqrt[1]; /* new ca-c vector*/
     }
     crossprod(can,cac,cc); /*cc is the component perpendicula to nca,cac*/
     for(i=0;i<=2;i++){ /* the geo.position of cb*/
       cc[i]=cc[i]/sin(geo.tt[4]);
       cb[i]=geo.dsqrt[1]*(-0.5*can[i]-0.5*cac[i]+sqrt(2.0/3.0)*cc[i])
                  +unit[0][iu][0][i];
     }
   /*rotate cb,c,n2 by phi about n-ca=vv with n at origin*/
     tranm(cb,unit[1][iu][1]);multvec(rom,cb,unit[0][iu][1]);
     tran(unit[0][iu][1],unit[1][iu][1]); //new unit[0][iu][1]= Cb
   } /* if ifind!=1*/
   tranm(c,unit[1][iu][1]); /*translates c to c-n, puts n at origin*/
   tranm(n,unit[1][iu][1]);
   multvec(rom,c,unit[1][iu+1][0]);
   multvec(rom,n,nt);
   tran(unit[1][iu+1][0],unit[1][iu][1]);// New unit[1][iu+1][0] = C
   tran(nt,unit[1][iu][1]);
  /*rotate nt by psi about cac*/
   for(i=0;i<=2;i++)
     cac[i]=(unit[1][iu+1][0][i]-unit[0][iu][0][i])/geo.dsqrt[1]; /* this is different than cac above*/
   rotaxis(geo.de,geo.ep,psi,cac,rom);
   tranm(nt,unit[0][iu][0]);
   multvec(rom,nt,unit[1][iu+1][1]); 
   tran(unit[1][iu+1][1],unit[0][iu][0]);//New unit[1][iu+1][1] = N
   /* add ca and o in same plane as ca-c and c-n*/
   for(k=0;k<=2;k++){
     uv[k]=(unit[1][iu+1][0][k]-unit[0][iu][0][k])/geo.dsqrt[1];
     vv[k]=(unit[1][iu+1][1][k]-unit[1][iu+1][0][k])/geo.dsqrt[2];
   }
   th= -PI+geo.tt[1];
//   th1=th+PI-geo.tt[3]; // standard trans
   th1=th+PI-tt3; // cis
   udv=cos(th); /* = cn dot nca = cos(theta)*/
   sn=1-udv*udv; /*= sqr(sin(theta)*/
   if(iend==1) // add Ca to last unit2
      planevec(uv,vv,udv,sn,geo.dsqrt[4],th,th1,unit[1][iu+1][1],
               unit[1][iu+1][3]);// Next Ca 
   else
      planevec(uv,vv,udv,sn,geo.dsqrt[4],th,th1,unit[1][iu+1][1],
               unit[0][iu+1][0]);// Next Ca 
   if(ifind!=1){ // Find O
     th1=PI-geo.tt[2];   
     planevec(uv,vv,udv,sn,geo.dsqrt[3],th,th1,unit[1][iu+1][0],
               unit[1][iu+1][2]); // Add o
   }
   for(k=0;k<=2;k++){ // find velocity
     unitv[1][iu+1][0][k]=(unit[1][iu+1][0][k]-oldc[k])/delt; //C
     unitv[1][iu+1][1][k]=(unit[1][iu+1][1][k]-oldn[k])/delt; //N
     if(iend==1) // add Ca to last unit2
       unitv[1][iu+1][3][k]=(unit[1][iu+1][3][k]-oldca[k])/delt;
     else  // Ca
       unitv[0][iu+1][0][k]=(unit[0][iu+1][0][k]-oldca[k])/delt;
     if(ifind!=1){ // Find O and Cb
       unitv[0][iu][1][k]=(unit[0][iu][1][k]-oldcb[k])/delt;//Cb
       unitv[1][iu+1][2][k]=(unit[1][iu+1][2][k]-oldo[k])/delt;//O
     }
   }//k
}
   
void makerevunitalanine(struct geometry& geo,float ****unit,float ****unitv,
          int ifind,int iend,int iu,float phi,float psi,float delt) 
/*Similar to makerevalanine2 - except makes new unit1 and unit2:
 Uses -phi and -psi;
 It is used after have copied geo to unit in reverse direction
   Thus first unit2 corresponds to C terminal end of geo.
 On input, have inverted phipsi, so that phipsi[end] = - (negative of) original phipsi[1]
 Have changed cbond appropriately (in copygeotoauto) so bonds are correct

  Given previous atoms in iu  - makes new iu+1
OLD:
 Given C1, N2, and Ca2, find next  c2,o2,cb2,n3,ca3etc if  aanum=2
aanum is the number of last a.a, make aa=anmum+1;
NOTE: does not assume ideal length and angles
1=ca,2=c,3=o;4=n,5=cb; 6=cb2 in pos
if ifind=1, do not find o or cb
it iend = 1, means at end, add Ca to last unit2
New:  
  c1 = unit[1][i][0]; N2 = unit[1][i][1];  ca2 = unit[0][i][0];
make new:  c2 = unit[1][i+1][0]; o2 = unit[1][i+1][2];
   cb2 = unit[0][i][1];n3 = unit[1][i+1][1]; ca3=unit[0][i+1][0]
*/
{
   int i,k;
   float cc[3],cb[3],cac[3],cca[3],nc[3],cdn,cn[3],ct[3],nca[3];
   float mag1,mag2,sn,th1,th,c[3],n[3],rom[3][3];
   float oldc[3],oldn[3],oldca[3],oldcb[3],oldo[3];

   for(k=0;k<=2;k++){ // store initial positions to find velocity
     oldc[k]=unit[1][iu+1][0][k]; //C
     oldn[k]=unit[1][iu+1][1][k]; //N
     if(iend==1) // add Ca to last unit2
       oldca[k]=unit[1][iu+1][3][k];
     else  // Ca
       oldca[k]=unit[0][iu+1][0][k];
     if(ifind!=1){ // Find O and Cb
       oldcb[k]=unit[0][iu][1][k];//Cb
       oldo[k]=unit[1][iu+1][2][k];
     }
   }//k
   /*here we set phi=psi=0 and find atoms positons*/
   for(k=0;k<=2;k++) { /*these are the known initial (unit) vectors*/
      cca[k]=unit[0][iu][0][k]-unit[1][iu][0][k]; /*the c to ca vector*/
      nc[k]=unit[1][iu][0][k]-unit[1][iu][1][k]; /* the n to c vector*/
   }
   mag1=sqrt(dot(cca,cca));
   mag2=sqrt(dot(nc,nc));
   for(k=0;k<=2;k++){
      cca[k]=cca[k]/mag1;
      cac[k]= -cca[k]; /* the ca-c vector  = psi rotation vector*/
      nc[k]=nc[k]/mag2;
   }
  /*find n and c for phi=psi=0;*/
   cdn=dot(cca,nc);
   th=acos(cdn);/*theta= angle between 2 basis vectors n and c >0 (anticlockwise)*/
   sn=1-cdn*cdn; /*= sqr(sin(theta)*/
   th1=th+PI-geo.tt[4];
   planevec(nc,cca,cdn,sn,geo.dsqrt[4],th,th1,unit[0][iu][0],n);
                        /*return vector n1 in same plane as n2,c and ca*/  
   th1=th-geo.tt[4]-geo.tt[3];                    
   planevec(nc,cca,cdn,sn,geo.dsqrt[2],th,th1,n,c);
   rotaxis(geo.de,geo.ep,psi,cac,rom); /*rom is the rotation matrix for rotation of phi about 
       unit vector nca, vector nca must pass through origin*/
   // Note origin for psi rotation = Ca =pos[aanum][1] 
   if(ifind!=1){
     /*for cb need define a third vector to define cbeta which is out of plane*/
     for(k=0;k<=2;k++){
        cn[k]=(n[k]-unit[0][iu][0][k])/geo.dsqrt[4]; /* the ca-n vector*/
     }
     crossprod(cn,cac,cc); /*cc is the component perpendicula to nca,cac*/
     for(i=0;i<=2;i++){ /* the position of cb*/
       cc[i]=cc[i]/sin(geo.tt[4]);
       cb[i]=geo.dsqrt[1]*(-0.5*cn[i]-0.5*cac[i]+sqrt(2.0/3.0)*cc[i])+unit[0][iu][0][i];
     }
   /*rotate cb,n,c by psi about c-ca*/
     tranm(cb,unit[0][iu][0]); /* puts Cb  at origin  = Ca*/
     multvec(rom,cb,unit[0][iu][1]);
     tran(unit[0][iu][1],unit[0][iu][0]); // new Cb
   }
   tranm(n,unit[0][iu][0]);tranm(c,unit[0][iu][0]);
   multvec(rom,n,unit[1][iu+1][1]);tran(unit[1][iu+1][1],unit[0][iu][0]);//New N
   multvec(rom,c,ct);tran(ct,unit[0][iu][0]);
   /*rotate c, by phi about cn*/
   for(i=0;i<=2;i++) // find phi rotation vector  n to Ca (origin  is at n =pos[aanum][4] )
     nca[i]=(unit[0][iu][0][i]-unit[1][iu+1][1][i])/geo.dsqrt[4]; /* this is different than cn above*/
   rotaxis(geo.de,geo.ep,phi,nca,rom);
   tranm(ct,unit[0][iu][0]);
   multvec(rom,ct,unit[1][iu+1][0]); 
   tran(unit[1][iu+1][0],unit[0][iu][0]); //New C
   /* find o and ca1 in same plane as c,ca2, and n*/
   for(k=0;k<=2;k++) { /*these are the known initial (unit) vectors*/
      cca[k]=unit[1][iu+1][1][k]-unit[0][iu][0][k]; /*the ca to n vector*/
      nc[k]=unit[1][iu+1][0][k]-unit[1][iu+1][1][k]; /* the n to c vector*/
   }
   mag1=sqrt(dot(cca,cca));
   mag2=sqrt(dot(nc,nc));
   for(k=0;k<=2;k++){
      cca[k]=cca[k]/mag1;
      nc[k]=nc[k]/mag2;
   }
   cdn=dot(cca,nc);
   th=acos(cdn);/*theta= angle between 2 basis vectors n and c >0 (anticlockwise)*/
   cdn=cos(th); /* = cn dot nca = cos(theta)*/
   sn=1-cdn*cdn; /*= sqr(sin(theta)*/
   th1=th-PI+geo.tt[1];
   if(iend==1) // add Ca to last unit2
      planevec(cca,nc,cdn,sn,geo.dsqrt[1],th,th1,unit[1][iu+1][0],
             unit[1][iu+1][3]);// Next Ca 
   else
      planevec(cca,nc,cdn,sn,geo.dsqrt[1],th,th1,unit[1][iu+1][0],
             unit[0][iu+1][0]);// Next Ca 
   if(ifind!=1){ // Find O
     th1=th+geo.tt[2]+geo.tt[1]-PI;
     planevec(cca,nc,cdn,sn,geo.dsqrt[3],th,th1,unit[1][iu+1][0],
           unit[1][iu+1][2]); 
   } 
   for(k=0;k<=2;k++){ // find velocity
     unitv[1][iu+1][0][k]=(unit[1][iu+1][0][k]-oldc[k])/delt; //C
     unitv[1][iu+1][1][k]=(unit[1][iu+1][1][k]-oldn[k])/delt; //N
     if(iend==1) // add Ca to last unit2
       unitv[1][iu+1][3][k]=(unit[1][iu+1][3][k]-oldca[k])/delt;
     else  // Ca
       unitv[0][iu+1][0][k]=(unit[0][iu+1][0][k]-oldca[k])/delt;
     if(ifind!=1){ // Find O and Cb
       unitv[0][iu][1][k]=(unit[0][iu][1][k]-oldcb[k])/delt;//Cb
       unitv[1][iu+1][2][k]=(unit[1][iu+1][2][k]-oldo[k])/delt;//O
     }
   }//k
}
   
void makecisrevunitalanine(struct geometry& geo,float ****unit,float ****unitv,
          int ifind,int iend,int iu,float phi,float psi,float delt) 
/*
Makes the cis proline  (See makecisrevalanine2)
 2 changes: geo.tt[1] = 2*PI - geo.tt[1]  and geo.tt[2] = - geo.tt[2] 
Similar to makerevalanine2 - except makes new unit1 and unit2:
 Uses -phi and -psi;
 It is used after have copied geo to unit in reverse direction
   Thus first unit2 corresponds to C terminal end of geo.
 On input, have inverted phipsi, so that phipsi[end] = - (negative of) original phipsi[1]
 Have changed cbond appropriately (in copygeotoauto) so bonds are correct

  Given previous atoms in iu  - makes new iu+1
OLD:
 Given C1, N2, and Ca2, find next  c2,o2,cb2,n3,ca3etc if  aanum=2
aanum is the number of last a.a, make aa=anmum+1;
NOTE: does not assume ideal length and angles
1=ca,2=c,3=o;4=n,5=cb; 6=cb2 in pos
if ifind=1, do not find o or cb
it iend = 1, means at end, add Ca to last unit2
New:  
  c1 = unit[1][i][0]; N2 = unit[1][i][1];  ca2 = unit[0][i][0];
make new:  c2 = unit[1][i+1][0]; o2 = unit[1][i+1][2];
   cb2 = unit[0][i][1];n3 = unit[1][i+1][1]; ca3=unit[0][i+1][0]
*/
{
   int i,k;
   float cc[3],cb[3],cac[3],cca[3],nc[3],cdn,cn[3],ct[3],nca[3];
   float mag1,mag2,sn,th1,th,c[3],n[3],rom[3][3];
   float oldc[3],oldn[3],oldca[3],oldcb[3],oldo[3];
   float tt1,tt2;

   tt1 = 2*PI - geo.tt[1];
   tt2 = -geo.tt[2];
   for(k=0;k<=2;k++){ // store initial positions to find velocity
     oldc[k]=unit[1][iu+1][0][k]; //C
     oldn[k]=unit[1][iu+1][1][k]; //N
     if(iend==1) // add Ca to last unit2
       oldca[k]=unit[1][iu+1][3][k];
     else  // Ca
       oldca[k]=unit[0][iu+1][0][k];
     if(ifind!=1){ // Find O and Cb
       oldcb[k]=unit[0][iu][1][k];//Cb
       oldo[k]=unit[1][iu+1][2][k];
     }
   }//k
   /*here we set phi=psi=0 and find atoms positons*/
   for(k=0;k<=2;k++) { /*these are the known initial (unit) vectors*/
      cca[k]=unit[0][iu][0][k]-unit[1][iu][0][k]; /*the c to ca vector*/
      nc[k]=unit[1][iu][0][k]-unit[1][iu][1][k]; /* the n to c vector*/
   }
   mag1=sqrt(dot(cca,cca));
   mag2=sqrt(dot(nc,nc));
   for(k=0;k<=2;k++){
      cca[k]=cca[k]/mag1;
      cac[k]= -cca[k]; /* the ca-c vector  = psi rotation vector*/
      nc[k]=nc[k]/mag2;
   }
  /*find n and c for phi=psi=0;*/
   cdn=dot(cca,nc);
   th=acos(cdn);/*theta= angle between 2 basis vectors n and c >0 (anticlockwise)*/
   sn=1-cdn*cdn; /*= sqr(sin(theta)*/
   th1=th+PI-geo.tt[4];
   planevec(nc,cca,cdn,sn,geo.dsqrt[4],th,th1,unit[0][iu][0],n);
                        /*return vector n1 in same plane as n2,c and ca*/  
   th1=th-geo.tt[4]-geo.tt[3];                    
   planevec(nc,cca,cdn,sn,geo.dsqrt[2],th,th1,n,c);
   rotaxis(geo.de,geo.ep,psi,cac,rom); /*rom is the rotation matrix for rotation of phi about 
       unit vector nca, vector nca must pass through origin*/
   // Note origin for psi rotation = Ca =pos[aanum][1] 
   if(ifind!=1){
     /*for cb need define a third vector to define cbeta which is out of plane*/
     for(k=0;k<=2;k++){
        cn[k]=(n[k]-unit[0][iu][0][k])/geo.dsqrt[4]; /* the ca-n vector*/
     }
     crossprod(cn,cac,cc); /*cc is the component perpendicula to nca,cac*/
     for(i=0;i<=2;i++){ /* the position of cb*/
       cc[i]=cc[i]/sin(geo.tt[4]);
       cb[i]=geo.dsqrt[1]*(-0.5*cn[i]-0.5*cac[i]+sqrt(2.0/3.0)*cc[i])+unit[0][iu][0][i];
     }
   /*rotate cb,n,c by psi about c-ca*/
     tranm(cb,unit[0][iu][0]); /* puts Cb  at origin  = Ca*/
     multvec(rom,cb,unit[0][iu][1]);
     tran(unit[0][iu][1],unit[0][iu][0]); // new Cb
   }
   tranm(n,unit[0][iu][0]);tranm(c,unit[0][iu][0]);
   multvec(rom,n,unit[1][iu+1][1]);tran(unit[1][iu+1][1],unit[0][iu][0]);//New N
   multvec(rom,c,ct);tran(ct,unit[0][iu][0]);
   /*rotate c, by phi about cn*/
   for(i=0;i<=2;i++) // find phi rotation vector  n to Ca (origin  is at n =pos[aanum][4] )
     nca[i]=(unit[0][iu][0][i]-unit[1][iu+1][1][i])/geo.dsqrt[4]; /* this is different than cn above*/
   rotaxis(geo.de,geo.ep,phi,nca,rom);
   tranm(ct,unit[0][iu][0]);
   multvec(rom,ct,unit[1][iu+1][0]); 
   tran(unit[1][iu+1][0],unit[0][iu][0]); //New C
   /* find o and ca1 in same plane as c,ca2, and n*/
   for(k=0;k<=2;k++) { /*these are the known initial (unit) vectors*/
      cca[k]=unit[1][iu+1][1][k]-unit[0][iu][0][k]; /*the ca to n vector*/
      nc[k]=unit[1][iu+1][0][k]-unit[1][iu+1][1][k]; /* the n to c vector*/
   }
   mag1=sqrt(dot(cca,cca));
   mag2=sqrt(dot(nc,nc));
   for(k=0;k<=2;k++){
      cca[k]=cca[k]/mag1;
      nc[k]=nc[k]/mag2;
   }
   cdn=dot(cca,nc);
   th=acos(cdn);/*theta= angle between 2 basis vectors n and c >0 (anticlockwise)*/
   cdn=cos(th); /* = cn dot nca = cos(theta)*/
   sn=1-cdn*cdn; /*= sqr(sin(theta)*/
//   th1=th-PI+geo.tt[1]; // standard trans
   th1 = th - PI + tt1;  // cis
   if(iend==1) // add Ca to last unit2
      planevec(cca,nc,cdn,sn,geo.dsqrt[1],th,th1,unit[1][iu+1][0],
             unit[1][iu+1][3]);// Next Ca 
   else
      planevec(cca,nc,cdn,sn,geo.dsqrt[1],th,th1,unit[1][iu+1][0],
             unit[0][iu+1][0]);// Next Ca 
   if(ifind!=1){ // Find O
//     th1=th+geo.tt[2]+geo.tt[1]-PI;  // standard trans
     th1=th+tt2+tt1-PI;  // cis
     planevec(cca,nc,cdn,sn,geo.dsqrt[3],th,th1,unit[1][iu+1][0],
           unit[1][iu+1][2]); 
   } 
   for(k=0;k<=2;k++){ // find velocity
     unitv[1][iu+1][0][k]=(unit[1][iu+1][0][k]-oldc[k])/delt; //C
     unitv[1][iu+1][1][k]=(unit[1][iu+1][1][k]-oldn[k])/delt; //N
     if(iend==1) // add Ca to last unit2
       unitv[1][iu+1][3][k]=(unit[1][iu+1][3][k]-oldca[k])/delt;
     else  // Ca
       unitv[0][iu+1][0][k]=(unit[0][iu+1][0][k]-oldca[k])/delt;
     if(ifind!=1){ // Find O and Cb
       unitv[0][iu][1][k]=(unit[0][iu][1][k]-oldcb[k])/delt;//Cb
       unitv[1][iu+1][2][k]=(unit[1][iu+1][2][k]-oldo[k])/delt;//O
     }
   }//k
}



void nextatom(float a1[3],float a2[3],float a3[3],float th,
     float achi,float len,float a[3])
/* Routine to add anohter atom  a to sequence a1,a2,a3 where 
    the angle a2-a3-a = th and a is rotated by achi from
    vector in a1-a2-a3 plane perpend. to a2-a3.  This just 
    the inverse of routine findchi()
  len = lenght of a3-a bond.
*/
{
   int i;
   float u[3],v[3],w[3],wper[3],aa,bb;
   float x[3],y[3];
   float mag1,mag2,cosal,sinal;

   for(i=0;i<=2;i++){
      u[i]=a1[i]-a2[i];
      v[i]=a3[i]-a2[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;
   crossprod(v,x,y);
   mag1=sqrt(dot(y,y));
   for(i=0;i<=2;i++)
      y[i]=y[i]/mag1;
   aa=cos(achi);
   bb=sin(achi);
   for(i=0;i<=2;i++) /* generate w perpendicular - should have correct sign*/
      wper[i]=aa*x[i]+bb*y[i];
   aa=cos(PI-th);
   bb=cos(th-PI/2.0);
   for(i=0;i<=2;i++) /* find w */
      w[i]=aa*v[i]+bb*wper[i];
   mag1=sqrt(dot(w,w));
   for(i=0;i<=2;i++) /* find a output atom */
      a[i]=a3[i]+len*w[i]/mag1;
}   
   

void nsidestep(struct geometry& geo,struct tordyn& tordata)
/*
NOTE: IT NOW IS ASSUMED DOES NOT START WITH Ca  
 builds all side chain atoms after one time step rotation
    new tordata. = chi + dqs*delt
 using chi angles
 If side chain, then given position of Ca, and Cb = uside[first #]
 change chi to chi+ del and build next C, etc.
     autolist[0][0]= # of separate (not connected) auto build traces
     autolist[i][0] = 1 or 2   Indicates have finished trace and
                               whether start trace i with unit 1 or 2
     autolist[i][1]= # of the first unit1 on trace i;
     autolist[i][2]= #  unit1s on trace i;
     autolist[i][3]= # of first unit2 on trace i;
     autolist[i][4]= # of unit2s on trace i;
*/
{
    int i,k,sunit,lunit,count2,ii;
    float nvec[3];
    float old[20][3][3];

    count2=0; /* counts the type 2 atoms, used to find N */
    for(i=1;i<=tordata.nunit1;i++){ /* list of Ca's */
       count2=count2+1;
       if(tordata.sidedata[i][0]!=0){
          sunit=tordata.sidedata[i][1]+1;/*start value ( sidedata[i][1]= Ca )*/
          lunit=tordata.sidedata[i][1]+tordata.sidedata[i][0]-1; /* last value of this sidechian */
          if(lunit>=sunit){          
             for(k=0;k<=2;k++){// Store old positions to find velocity
                for(ii=sunit;ii<=lunit;ii++)
                  old[ii-sunit][0][k]=tordata.uside[ii][0][k];            
                ii=lunit;
                old[ii-sunit][1][k]=tordata.uside[ii][1][k];
             } //k
          }
          for(k=0;k<=2;k++)  /* copy Cb to uside */
               tordata.uside[sunit][0][k]=tordata.unit[0][i][1][k];
          if(geo.dir==0) // N to C direciton
              ii = count2; 
          else if(geo.dir==1)
              ii = count2+1;
          for(k=0;k<=2;k++) // NOTE; THIS ASSUMES DOES NOT START WITH UNIT1
               nvec[k]=tordata.unit[1][ii][1][k];
          if(sunit==lunit)
            nextatom(nvec,tordata.unit[0][i][0],tordata.uside[sunit][0],geo.tt[5],tordata.chi[sunit-1],
                      geo.dsqrt[1],tordata.uside[sunit][1]);
          else
             nextatom(nvec,tordata.unit[0][i][0],tordata.uside[sunit][0],geo.tt[5],tordata.chi[sunit-1],
                      geo.dsqrt[1],tordata.uside[sunit+1][0]);
          if(sunit<lunit){
             if(sunit+1==lunit)
               nextatom(tordata.unit[0][i][0],tordata.uside[sunit][0],
                      tordata.uside[sunit+1][0],geo.tt[5],tordata.chi[sunit],
                      geo.dsqrt[1],tordata.uside[sunit+1][1]);
             else
               nextatom(tordata.unit[0][i][0],tordata.uside[sunit][0],tordata.uside[sunit+1][0],
                      geo.tt[5],tordata.chi[sunit],
                      geo.dsqrt[1],tordata.uside[sunit+2][0]);
          }             
          for(k=sunit+2;k<=lunit;k++){
             if(k==lunit)
               nextatom(tordata.uside[k-2][0],tordata.uside[k-1][0],tordata.uside[k][0],
                       geo.tt[5],tordata.chi[k-1],
                      geo.dsqrt[1],tordata.uside[k][1]);
             else
               nextatom(tordata.uside[k-2][0],tordata.uside[k-1][0],tordata.uside[k][0],
                      geo.tt[5],tordata.chi[k-1],
                      geo.dsqrt[1],tordata.uside[k+1][0]);
          } 
          for(k=0;k<=2;k++){ // find velocity
            for(ii=sunit;ii<=lunit;ii++) 
               tordata.usidev[ii][0][k]=(tordata.uside[ii][0][k]-old[ii-sunit][0][k])/tordata.delt;
            ii=lunit;
            tordata.usidev[ii][1][k]=(tordata.uside[ii][1][k]-old[ii-sunit][1][k])/tordata.delt;
          }
       } /* if sidedata */
    } /* i */
}                                


// Following function is for amino acid side chain
void onesidestep(int iside,int count2,struct geometry& geo,struct tordyn& tordata)
/*
New - if side chain is a.a. residue - then build new by rotation using new deltachi's
    dchi = tordata.dqs[ tordata.sidedata[ii][1] ]*tordata.delt
    Special case for  Proline - where no side chain flexibility
NOTE: IT NOW IS ASSUMED DOES NOT START WITH Ca
If just a sequence of C atoms then  
   builds all side chain atoms after one time step rotation
        new tordata. = chi + dqs*delt
 using chi angles
 If side chain, then given position of Ca, and Cb = uside[first #]
 change chi to chi+ del and build next C, etc.
     autolist[0][0]= # of separate (not connected) auto build traces
     autolist[i][0] = 1 or 2   Indicates have finished trace and
                               whether start trace i with unit 1 or 2
     autolist[i][1]= # of the first unit1 on trace i;
     autolist[i][2]= #  unit1s on trace i;
     autolist[i][3]= # of first unit2 on trace i;
     autolist[i][4]= # of unit2s on trace i;
*/
{
    int i,k,sunit,lunit,ii,jj,kk,iunit,i1,i2;
    float nvec[3];
    float rvec[3],dist,rom[3][3],dchi,pt1[3],pt2[3];
    float old[20][20][3];

    i = iside;
    count2=count2+1;
    if(tordata.sidedata[i][0]!=0){
       sunit=tordata.sidedata[i][1]+1;/*start value ( sidedata[i][1]= Ca )*/
       lunit=tordata.sidedata[i][1]+tordata.sidedata[i][0]-1; /* last value of this sidechian */
       for(k=0;k<=2;k++){// Store old positions to find velocity
          for(i1=sunit;i1<=lunit;i1++)
             old[i1-sunit][0][k]=tordata.uside[i1][0][k];
          i1=lunit;
          for(i2=1;i2<=tordata.sidedata[i][3];i2++)
            old[i1-sunit][i2][k]=tordata.uside[i1][i2][k];
       }//k
       for(k=0;k<=2;k++)  /* copy Cb to uside */
               tordata.uside[sunit][0][k]=tordata.unit[0][i][1][k];
       if(geo.dir==0) // N to C direciton
              ii = count2; 
       else if(geo.dir==1)
              ii = count2+1;
       for(k=0;k<=2;k++) // NOTE; THIS ASSUMES DOES NOT START WITH UNIT1
               nvec[k]=tordata.unit[1][ii][1][k];
       if(tordata.sidedata[i][2]== -1){ // Then side chain just list of C's, build old way by adding each atom
          if(sunit==lunit)
            nextatom(nvec,tordata.unit[0][i][0],tordata.uside[sunit][0],geo.tt[5],tordata.chi[sunit-1],
                      geo.dsqrt[1],tordata.uside[sunit][1]);
          else
             nextatom(nvec,tordata.unit[0][i][0],tordata.uside[sunit][0],geo.tt[5],tordata.chi[sunit-1],
                      geo.dsqrt[1],tordata.uside[sunit+1][0]);
          if(sunit<lunit){
             if(sunit+1==lunit)
               nextatom(tordata.unit[0][i][0],tordata.uside[sunit][0],
                      tordata.uside[sunit+1][0],geo.tt[5],tordata.chi[sunit],
                      geo.dsqrt[1],tordata.uside[sunit+1][1]);
             else
               nextatom(tordata.unit[0][i][0],tordata.uside[sunit][0],tordata.uside[sunit+1][0],
                      geo.tt[5],tordata.chi[sunit],
                      geo.dsqrt[1],tordata.uside[sunit+2][0]);
          }             
          for(k=sunit+2;k<=lunit;k++){
             if(k==lunit)
               nextatom(tordata.uside[k-2][0],tordata.uside[k-1][0],tordata.uside[k][0],
                       geo.tt[5],tordata.chi[k-1],
                      geo.dsqrt[1],tordata.uside[k][1]);
             else
               nextatom(tordata.uside[k-2][0],tordata.uside[k-1][0],tordata.uside[k][0],
                      geo.tt[5],tordata.chi[k-1],
                      geo.dsqrt[1],tordata.uside[k+1][0]);
          } 
      } // if == -1
      else{  // side chain is a.a. residue, rotate units to get new postion
         for(k=0;k<=2;k++) //first rotation vector = Ca to Cb
             pt1[k] = tordata.unit[0][i][0][k];  // CA
         for(ii= 1;ii< tordata.sidedata[i][0];ii++){
            iunit = sunit+ii -1;
            for(k=0;k<=2;k++){ //first rotation vector = Ca to Cb
               pt2[k] = tordata.uside[iunit][0][k];
               rvec[k] = pt2[k]-pt1[k];
            }
            dist = sqrt( dot(rvec,rvec) );
            for(k=0;k<=2;k++) 
               rvec[k] = rvec[k]/dist;
//            dchi = tordata.dqs[iunit-1]*tordata.delt;
            dchi = tordata.chi[iunit-1]-tordata.oldchi[iunit-1];
//dchi = 0;
            rotaxis(geo.de,geo.ep,dchi,rvec,rom); /* rom is rotation matrix for rotation by dchi about unit vector rvec which is assumed to be at origin*/  

#if 1
            for(kk=iunit+1;kk<=lunit;kk++) // Rotate all subsequent single pts
               rotateset(pt2,tordata.uside[kk][0],rom); // pt2 is origin
            for(jj=1;jj<=tordata.sidedata[i][3];jj++) // rotate all points in last unit connected to connect point
                  rotateset(pt1,tordata.uside[lunit][jj],rom); // pt1 is origin
#endif
#if 0
            if(iunit == lunit){// if last unit,- then rotate unit
               for(jj=1;jj<=tordata.sidedata[i][3];jj++) // rotate all points in last unit connected to connect point
                  rotateset(pt1,tordata.uside[iunit][jj],rom); // pt1 is origin
            }
            else{ // not last,rotate all following pts
               rotateset(pt2,tordata.uside[iunit+1][0],rom); // pt1 is origin
               for(jj=1;jj<=tordata.sidedata[i][3];jj++) // rotate all points in last unit connected to connect point
                  rotateset(pt2,tordata.uside[iunit+1][jj],rom); // pt1 is origin
            }
#endif
            for(k=0;k<=2;k++)
               pt1[k] = pt2[k]; 
        }//ii
      } // else side is aa.        
      for(k=0;k<=2;k++){// find velocity
          for(i1=sunit;i1<=lunit;i1++)
             tordata.usidev[i1][0][k]=(tordata.uside[i1][0][k]-old[i1-sunit][0][k])/tordata.delt;
          i1=lunit;
          for(i2=1;i2<=tordata.sidedata[i][3];i2++)
            tordata.usidev[i1][i2][k]=(tordata.uside[i1][i2][k]-old[i1-sunit][i2][k])/tordata.delt;
      }//k
   } /* if sidedata */
} 
                               
void copysidegeotoauto(struct geometry& geo,struct tordyn& tordata)
/* 
Assumes side chain is amino acid
Similar to ncopygeotoauto - but just copies side,
   and assumes stuff like tordata.sidedata is already defined.
*/
{
   int i,ii,jj,k,ns,rottype,sunit,lunit,i1,i2;
   float old[20][20][3];

   ns = 0; // counts total number of side chain atoms
   for(i=1;i<=tordata.nunit1;i++){ // Run through each unit1, set side chain date
      if(tordata.sidedata[i][0]==0){ // this is the Glycine case
         ns = ns+2; // this happens in ncopygeotoauto for glycine case
         goto nexti;
      }
      sunit=tordata.sidedata[i][1]+1;/*start value ( sidedata[i][1]= Ca )*/
      lunit=tordata.sidedata[i][1]+tordata.sidedata[i][0]-1; /* last value of this sidechian */
      for(k=0;k<=2;k++){// Store old positions to find velocity
         for(i1=sunit;i1<=lunit;i1++)
            old[i1-sunit][0][k]=tordata.uside[i1][0][k];
         i1=lunit;
         for(i2=1;i2<=tordata.sidedata[i][3];i2++)
           old[i1-sunit][i2][k]=tordata.uside[i1][i2][k];
      }//k
     if(geo.dir ==0) // N to c direction
         ii = geo.begend[0]+i-1;
     else // C to N
         ii =geo.begend[1]-i-1; // This make ii+1 OK for this direction
     ns=ns+1; 
     tordata.sidedata[i][1]= ns;// start number for this chain
     for(k=0;k<=2;k++)
        tordata.uside[ns][0][k] = geo.pos[ii+1][1][k]; // first atom is Ca
     ns=ns+1; 
     rottype = -geo.iside[ii+1];
     switch(rottype){
        case(ALA): // No new side chain atoms
          break;
        case(ARG): //branched
          for(k=0;k<=2;k++)
            tordata.uside[ns][0][k] =geo.pos[ii+1][5][k];// (Cb)
          ns=ns+1;
          for(k=0;k<=2;k++)
            tordata.uside[ns][0][k] =geo.pos[ii+1][6][k];//(CG)
          ns=ns+1;
          for(k=0;k<=2;k++){
            tordata.uside[ns][0][k] =geo.pos[ii+1][7][k];//
            tordata.uside[ns][1][k] =geo.pos[ii+1][8][k];//
            tordata.uside[ns][2][k] =geo.pos[ii+1][9][k];//
            tordata.uside[ns][3][k] =geo.pos[ii+1][10][k];
            tordata.uside[ns][4][k] =geo.pos[ii+1][12][k];//
          } // k
          break;
        case(ASN): //branched
          for(k=0;k<=2;k++)
            tordata.uside[ns][0][k] =geo.pos[ii+1][5][k];// (Cb)
          ns=ns+1;
          for(k=0;k<=2;k++){
            tordata.uside[ns][0][k] =geo.pos[ii+1][6][k];//
            tordata.uside[ns][1][k] =geo.pos[ii+1][7][k];//
            tordata.uside[ns][2][k] =geo.pos[ii+1][9][k];
          } // k
          break;
        case(ASP): //branched
          for(k=0;k<=2;k++)
            tordata.uside[ns][0][k] =geo.pos[ii+1][5][k];// (Cb)
          ns=ns+1;
          for(k=0;k<=2;k++){
            tordata.uside[ns][0][k] =geo.pos[ii+1][6][k];//
            tordata.uside[ns][1][k] =geo.pos[ii+1][7][k];//
            tordata.uside[ns][2][k] =geo.pos[ii+1][9][k];
          } // k
          break;
        case(CYS): //branched
          for(k=0;k<=2;k++){
            tordata.uside[ns][0][k] =geo.pos[ii+1][5][k];//Cb
            tordata.uside[ns][1][k] =geo.pos[ii+1][6][k];//
          } // k
          break;
        case(GLU): //branched
          for(k=0;k<=2;k++)
            tordata.uside[ns][0][k] =geo.pos[ii+1][5][k];// (Cb)
          ns=ns+1;
          for(k=0;k<=2;k++)
            tordata.uside[ns][0][k] =geo.pos[ii+1][6][k];//(CG)
          ns=ns+1;
          for(k=0;k<=2;k++){
            tordata.uside[ns][0][k] =geo.pos[ii+1][7][k];//
            tordata.uside[ns][1][k] =geo.pos[ii+1][8][k];//
            tordata.uside[ns][2][k] =geo.pos[ii+1][10][k];
          } // k
          break;
        case(AA4): //artificial 4 atom strgint
          for(k=0;k<=2;k++)
            tordata.uside[ns][0][k] =geo.pos[ii+1][5][k];// (Cb)
          ns=ns+1;
          for(k=0;k<=2;k++)
            tordata.uside[ns][0][k] =geo.pos[ii+1][6][k];//(CG)
          ns=ns+1;
          for(k=0;k<=2;k++){
            tordata.uside[ns][0][k] =geo.pos[ii+1][7][k];//
            tordata.uside[ns][1][k] =geo.pos[ii+1][8][k];//
          } // k
          break;
        case(GLN): //branched
          for(k=0;k<=2;k++)
            tordata.uside[ns][0][k] =geo.pos[ii+1][5][k];// (Cb)
          ns=ns+1;
          for(k=0;k<=2;k++)
            tordata.uside[ns][0][k] =geo.pos[ii+1][6][k];//(CG)
          ns=ns+1;
          for(k=0;k<=2;k++){
            tordata.uside[ns][0][k] =geo.pos[ii+1][7][k];//
            tordata.uside[ns][1][k] =geo.pos[ii+1][8][k];//
            tordata.uside[ns][2][k] =geo.pos[ii+1][10][k];
          } // k
          break;
        case(GLY): 
          break;
        case(HIS): // unbranched
          for(k=0;k<=2;k++)
            tordata.uside[ns][0][k] =geo.pos[ii+1][5][k];// (Cb)
          ns=ns+1;
          for(k=0;k<=2;k++){
            tordata.uside[ns][0][k] =geo.pos[ii+1][6][k];//(CG)
            for(jj=1;jj<=tordata.sidedata[i][3];jj++)
               tordata.uside[ns][jj][k] =geo.pos[ii+1][jj+6][k];// (CD1)
          } // k
         break;
#if 1 // NEW  version -treat all terminal atoms as one rigid unit - neglects one degree of freedom
        case(ILE): //SLIGHT ERROR:  (ILE does not fit assume rule that only last group has more than one atom
          for(k=0;k<=2;k++)
            tordata.uside[ns][0][k] =geo.pos[ii+1][5][k];// (Cb)
          for(k=0;k<=2;k++){
            tordata.uside[ns][1][k] =geo.pos[ii+1][6][k];//
            tordata.uside[ns][2][k] =geo.pos[ii+1][7][k];//
            tordata.uside[ns][3][k] =geo.pos[ii+1][8][k];//
          } // k
          break;
#endif
        case(LEU): //Same as ASP
          for(k=0;k<=2;k++)
            tordata.uside[ns][0][k] =geo.pos[ii+1][5][k];// (Cb)
          ns=ns+1;
          for(k=0;k<=2;k++){
            tordata.uside[ns][0][k] =geo.pos[ii+1][6][k];//
            tordata.uside[ns][1][k] =geo.pos[ii+1][7][k];//
            tordata.uside[ns][2][k] =geo.pos[ii+1][9][k];
          } // k
          break;
        case(AA3): //Artificial 3 atom straignt
          for(k=0;k<=2;k++)
            tordata.uside[ns][0][k] =geo.pos[ii+1][5][k];// (Cb)
          ns=ns+1;
          for(k=0;k<=2;k++){
            tordata.uside[ns][0][k] =geo.pos[ii+1][6][k];//
            tordata.uside[ns][1][k] =geo.pos[ii+1][7][k];//
          } // k
          break;
        case(LYS): //unbranched
          for(k=0;k<=2;k++)
            tordata.uside[ns][0][k] =geo.pos[ii+1][5][k];// (Cb)
          ns=ns+1;
          for(k=0;k<=2;k++)
            tordata.uside[ns][0][k] =geo.pos[ii+1][6][k];//(CG)
          ns=ns+1;
          for(k=0;k<=2;k++){
            tordata.uside[ns][0][k] =geo.pos[ii+1][7][k];//
            tordata.uside[ns][1][k] =geo.pos[ii+1][8][k];//
            tordata.uside[ns][2][k] =geo.pos[ii+1][9][k];//
          } // k
          break;
        case(MET): //unbranched
          for(k=0;k<=2;k++)
            tordata.uside[ns][0][k] =geo.pos[ii+1][5][k];// (Cb)
          ns=ns+1;
          for(k=0;k<=2;k++)
            tordata.uside[ns][0][k] =geo.pos[ii+1][6][k];// (Cg)
          ns=ns+1;
          for(k=0;k<=2;k++){
            tordata.uside[ns][0][k] =geo.pos[ii+1][7][k];//SD
            tordata.uside[ns][1][k] =geo.pos[ii+1][8][k];//CE
          } // k
          break;
        case(PHE): // unbranched
          for(k=0;k<=2;k++)
            tordata.uside[ns][0][k] =geo.pos[ii+1][5][k];// (Cb)
          ns=ns+1;
          for(k=0;k<=2;k++){
            tordata.uside[ns][0][k] =geo.pos[ii+1][6][k];//(CG)
            for(jj=1;jj<=tordata.sidedata[i][3];jj++)
               tordata.uside[ns][jj][k] =geo.pos[ii+1][jj+6][k];// (CD1)
          } // k
          break;
        case(PRO): 
          for(k=0;k<=2;k++){
            tordata.uside[ns][0][k] =geo.pos[ii+1][5][k];// (Cb)
            tordata.uside[ns][1][k] =geo.pos[ii+1][6][k];//
            tordata.uside[ns][2][k] =geo.pos[ii+1][7][k];//
          } // k
          break;
        case(SER): 
          for(k=0;k<=2;k++){
            tordata.uside[ns][0][k] =geo.pos[ii+1][5][k];// (Cb)
            tordata.uside[ns][1][k] =geo.pos[ii+1][6][k];//
          } // k
          break;
       case(THR): 
          for(k=0;k<=2;k++){
            tordata.uside[ns][0][k] =geo.pos[ii+1][5][k];// (Cb)
            tordata.uside[ns][1][k] =geo.pos[ii+1][6][k];//
            tordata.uside[ns][2][k] =geo.pos[ii+1][7][k];//NORE CB not countd
          } // k
          break;
        case(TRP): // unbranched
          for(k=0;k<=2;k++)
            tordata.uside[ns][0][k] =geo.pos[ii+1][5][k];// (Cb)
          ns=ns+1;
          for(k=0;k<=2;k++){
            tordata.uside[ns][0][k] =geo.pos[ii+1][6][k];//(CG)
            for(jj=1;jj<=8;jj++)
               tordata.uside[ns][jj][k] =geo.pos[ii+1][jj+6][k];// (CD1)
          } // k
          break;
        case(TYR): // unbranched
          for(k=0;k<=2;k++)
            tordata.uside[ns][0][k] =geo.pos[ii+1][5][k];// (Cb)
          ns=ns+1;
          for(k=0;k<=2;k++){
            tordata.uside[ns][0][k] =geo.pos[ii+1][6][k];//(CG)
            for(jj=1;jj<=4;jj++)
               tordata.uside[ns][jj][k] =geo.pos[ii+1][jj+6][k];// (CD1)
            tordata.uside[ns][5][k] =geo.pos[ii+1][12][k];// (CD1)
            tordata.uside[ns][6][k] =geo.pos[ii+1][13][k];// (CD1)
          } // k
          break;
       case(VAL): // Same as THR
          for(k=0;k<=2;k++){
            tordata.uside[ns][0][k] =geo.pos[ii+1][5][k];// (Cb)
            tordata.uside[ns][1][k] =geo.pos[ii+1][6][k];//
            tordata.uside[ns][2][k] =geo.pos[ii+1][7][k];//NORE CB not countd
          } // k
          break;
        default:
          printf("No sidechain data for rottype = %d\n",rottype);
          break;
      } // rottype
      for(k=0;k<=2;k++){ // find velocity
          for(ii=sunit;ii<=lunit;ii++) 
               tordata.usidev[ii][0][k]=(tordata.uside[ii][0][k]-old[ii-sunit][0][k])/tordata.delt;
          ii=lunit;
          for(i2=1;i2<=tordata.sidedata[i][3];i2++)
              tordata.usidev[ii][1][k]=(tordata.uside[ii][i2][k]-old[ii-sunit][i2][k])/tordata.delt;
      }
      nexti:;
   } //i
}


void checkstruc(struct geometry& geo,struct tordyn& tordata)
/* Checks unit strucutre  (ignoring Cb) - sees if bond lenghts
    are and angles are correct and whether Ca - C - N - Ca2 in 
    same plane. 
    NOW:  Assumes starts and ends with type 2 (i.e. unit2 - unit1 - unit2 - unit1 - unit2)
      Just a checks bonds around each unit 1, does not check ends
  For unit1 - 0 = Ca, 1 = Cb (Note, if use side chain, then do not use Cb??)
  For unit2   0 = C, 1= N, 2 = 0
  Start with unit2: 1, 2, ... geo.nend[1]-1
    then to unit1:  1,2, ... geo.nend[1]-2
*/
{
   int k;
   int icnt[2],count;
   float uv[3],vv[3],mag,mag2,mag3,mag4,pv[3],ang;
   float ev[3],cv[3],err1;

if(verbose==1)
   fprintf(fpout,"\n CHECKING UNIT STRUCURE \n");
   err1= 0.01;
  icnt[0] = 1; // number of first unit1
  icnt[1] = 1;// number of first unit2
  for(count=1;count<=tordata.nunit2;count++){ // check bonds related to each unit2
          if(count==1){
            for(k=0;k<=2;k++)
               uv[k]=tordata.unit[1][icnt[1]][0][k]-tordata.unit[1][icnt[1]][3][k]; /* Ca - C  */
          }
          else{ 
            for(k=0;k<=2;k++)
               uv[k]=tordata.unit[1][icnt[1]][0][k]-tordata.unit[0][icnt[0]-1][0][k]; /* Ca - C  */
          }
          for(k=0;k<=2;k++)
             vv[k]=tordata.unit[1][icnt[1]][1][k]-tordata.unit[1][icnt[1]][0][k]; /* C to N */
          mag=sqrt(dot(uv,uv));
          if(fabs(mag- geo.dsqrt[1])>err1)
             fprintf(fpout,"Error: count=%d  Ca -C length = %6.3f  should be %6.3f\n",
                 count,mag,geo.dsqrt[1]);
          mag2=sqrt(dot(vv,vv));
          if(fabs(mag2- geo.dsqrt[2])>err1)
             fprintf(fpout,"Error: count=%d  C - N  length = %6.3f should be %6.3f\n",
                     count,mag2,geo.dsqrt[2]);
          if(count==tordata.nunit2){
             for(k=0;k<=2;k++)
                pv[k]=tordata.unit[1][icnt[1]][3][k]-tordata.unit[1][icnt[1]][1][k];/* N to Ca */
          }
          else{
            for(k=0;k<=2;k++)
                pv[k]=tordata.unit[0][icnt[0]][0][k]-tordata.unit[1][icnt[1]][1][k];/* N to Ca */
          }
          mag3=sqrt(dot(pv,pv));               
          if(fabs(mag3- geo.dsqrt[4])>err1)
             fprintf(fpout,"Error: count=%d  N - Ca  length = %6.3f should be %6.3f\n",
                 count,mag3,geo.dsqrt[4]);
          for(k=0;k<=2;k++){
             uv[k]= -uv[k]/mag;
             vv[k]=vv[k]/mag2;
             pv[k]= -pv[k]/mag3;
          }
          if(count<tordata.nunit2){//last unit2 
             for(k=0;k<=2;k++)
               cv[k]=tordata.unit[1][icnt[1]+1][0][k]-tordata.unit[0][icnt[0]][0][k]; /* next Ca to C*/
             mag4=sqrt(dot(cv,cv)); 
             for(k=0;k<=2;k++)
               cv[k]=cv[k]/mag4;
             ang=acos(dot(pv,cv));
             if(fabs(ang-geo.tt[4])> err1)
                 fprintf(fpout,"Error: count=%d:   N - Ca -C (deg) = %6.3f should be %6.3f\n",
                       count,ang*180.0/PI,geo.tt[4]*180.0/PI); 
          }
          ang=acos(dot(uv,vv));
          if(fabs(ang-geo.tt[1])>err1)
             fprintf(fpout,"Error: count=%d:   Ca-C-N (deg)= %6.3f should be %6.3f\n",
                  count,ang*180.0/PI,geo.tt[1]*180.0/PI);           
          ang=acos(dot(vv,pv));
          if(fabs(ang-geo.tt[3])>err1)
             fprintf(fpout,"Error: count=%d:   C-N-Ca (deg)= %6.3f should be %6.3f\n",
                     count,ang*180.0/PI,geo.tt[3]*180.0/PI); 
          crossprod(uv,vv,ev);// check if keep planar relation of peptide
          mag3=sqrt(dot(ev,ev));               
          for(k=0;k<=2;k++)
             ev[k] = ev[k]/mag3;
          ang=acos(dot(ev,pv));
          if(fabs(ang-PI/2.0)>err1)
             fprintf(fpout,"Error: count=%d:  Ca -C - N -Ca plane = %6.3f should be %6.3f\n",
                 count,ang*180.0/PI,90.0); 
          icnt[0]=icnt[0]+1;
          icnt[1]=icnt[1]+1;
    }
    fprintf(fpout,"   DONE\n");
}           


void findsignphipsi(float ang,float minang,float maxang,float delang,
      int *angsign,float *mag )
/*
New version:  keep track of sign of force = tordata.angsign:
  1) ang is absolute - ie can get > 180 or less than -180,
       because ang = ang + delta angle where delta is determined
       by dynamic routine 
  2) THIS ASSUMES DOES NOT GO COMPLETELTY AROUND WHICH WOULD SCREW THINGS UP -]
       SINCE TORTIONAL FORCE ADDED - SHOULD PREVENT THIS.
  3) Check is sign from previous step = 0, if not, then keep this sign
       if = 0, then test:  if ang < minang, then sign = 1
          if ang> maxang, then sign = -1;
  4) mag grows from 0 to 1 as ang deviates from min/max bu 0 to delang
Copied from old maid
finds the value of signphipsi for ang given: ang1 (lower limit of box),
     ang2 (upper limit of box) and ang3 value of dividing point
*/
{
    if(*angsign==0){ // sign from previus run = 0, then check if still is 0
       if(ang<minang)
          *angsign = 1;
       else if(ang>maxang)
          *angsign = -1;
    }
    else{ // if not = 0, heck if has now becomes 0
       if( (ang>=minang)&&(ang<=maxang) )
          *angsign = 0;
    }
    if( *angsign !=0){//  find mag
        *mag = fabs(ang - minang)/delang;
#if 1
        if(*mag>1.0)
           *mag = 1.0;
#endif 
    } 
 }
 
void findangsign(struct geometry& geo,struct tordyn& tordata)
/*
NEW- 18feb99.   ACCOUNT FOR POSSIBILITY ang >PI or <  -Pi
    Use dividers so that add or subtract 2PI from ang
NEW: Use correlated phi = slope*phi + intercept to find psi range
 NOTE: mag is required for findsignphipsi, but is then discarded
ttype[count] is constraint for bond preceding unit-
If forward direction - first angle = phi , between unit1[1] and
    unit0[1]
 first  psi = angle between first unit0[1] and second unit1[2] = ttype[2][1]
*/
{
   int i,ii,iang,tcount;
   float phi,ang,mag,psim,psimin,psimax;

   tcount = tordata.nunit1+tordata.nunit2; 
   if(geo.dir ==0) // forward direction - first angle = phi
      ii= 1;
   else  if(geo.dir ==1) // backward direction - first angle = psi
      ii=0;
   for(i=1;i<tcount;i++){ // 
      iang = 1 +( (i-ii) % 2); // = 1 (phi) or 2 (psi)
      ang = tordata.phipsi[i];
      if( tordata.ttype[i]==1 ){ //  helix
        if(iang==1){// phi
           findsignphipsi(ang,geo.sheet[ tordata.ttype[i] ][0],geo.sheet[ tordata.ttype[i] ][1],
              geo.sheet[ tordata.ttype[i] ][4],&tordata.angsign[i],&mag);
        }
        else{ //psi (iang = 2) //NOTE: since psi centered about roughly 0, do not need to consider possibility <> PI,-PI
         phi=tordata.phipsi[1];
         if(phi<geo.sheet[1][0])
            phi = geo.sheet[1][0];
         else if (phi>geo.sheet[1][1])
            phi = geo.sheet[1][1];
         psim = geo.sheet[1][5]*phi + geo.sheet[1][6]; // correalted ideal psi
         psimin = psim -geo.sheet[1][7];// correlated psimin
         psimax = psim +geo.sheet[1][7];
         if(psimin < geo.sheet[1][2]) // absolute allowed min.
             psimin = geo.sheet[1][2];
         if(psimax > geo.sheet[1][3]) // absolute allowed max.
             psimax = geo.sheet[1][3];
         findsignphipsi(ang,psimin,psimax,
              geo.sheet[ tordata.ttype[i] ][4],&tordata.angsign[i],&mag);
       } //psi
      } // helix
      else if(tordata.ttype[i]==2 ){ // sheet
        if(iang==1){ // phi
           findsignphipsi(ang,geo.sheet[ tordata.ttype[i] ][0],geo.sheet[ tordata.ttype[i] ][1],
              geo.sheet[ tordata.ttype[i] ][4],&tordata.angsign[i],&mag);
        }
        else{ //psi
           findsignphipsi(ang,geo.sheet[ tordata.ttype[i] ][2],geo.sheet[ tordata.ttype[i] ][3],
              geo.sheet[ tordata.ttype[i] ][4],&tordata.angsign[i],&mag);
        }
      }// sheet
      else  { //???NOTE THIS HAS NOT YET BEEN MODIFIED TO CORRECT FOR ang <>PI  not helix 
        if(iang==1) // phi
           findsignphipsi(ang,geo.sheet[ tordata.ttype[i] ][0],geo.sheet[ tordata.ttype[i] ][1],
              geo.sheet[ tordata.ttype[i] ][4],&tordata.angsign[i],&mag);
        else //psi
           findsignphipsi(ang,geo.sheet[ tordata.ttype[i] ][2],geo.sheet[ tordata.ttype[i] ][3],
              geo.sheet[ tordata.ttype[i] ][4],&tordata.angsign[i],&mag);
      }// not helix
  }// i
}

void findangtorque(struct geometry& geo,struct tordyn& tordata,int tcount,
      float hbar[NUNIT][3],float tor[NUNIT][3])
/*
NOTe: this assumes starts and ends with unit2 and (for now) forward directon
 finds the torque about bonds to resstrict phi/psi to
   desired region:
  tcount is total number of unit1 + unit2
  i = 1 to tcount:
    i=1 first unit2, i=2 first unit1, etc.
  tor is input torsional force - add angle constraint
  Example: phi for forward direction 
         psi = rotation between unit1 and unit2 (about Ca - Co bond)
         phi = rotation between unit2 and unit1 (about N - Ca bond)
   If want to decrease phi (signval <0):
     Define tv[] as unit vector  in direction of N-Ca bond:
        apply torque = (negative)*tv[] on unit2 and (positive on unit 1)
  count = 1,2:  =phi between unit2[1] and unit1[1] ,psi between unit1[1] and unit2[2]
  count =(3,4),etc. (5,6)  ..  (_,2*nunit1) (last phi/psi pair 
  force on unit = angfor[type 0,1][unit #][x,y,z]
If angforconst = 1 (default)
  then maximum mag of constrint torgue =1.0  (hbar is unit vector)
    while the density component of tor  varies from 0 (For Ca) to 10 or larger 
*/
{
   int i,ii,k,iang,angtype; 
   float angtor[3],tconst,ang,mag;

//tconst = tconst/5.0;
   if(geo.dir ==0){// forward direction
     ii= 1;
   }
   else if (geo.dir ==1){  //back directin
     ii= 0;
   }
   for(i=1;i<tcount;i++){ // 
     iang = 1 +( (i-ii) % 2); // = 1 (phi) or 2 (psi)
     angtype = tordata.ttype[i];
//     if(angtype!=0){ //NEW -ADD 0 CONSTRAINTS   if some constraint
       tconst = tordata.angforconst[angtype]; // constant for torsional constraint
       ang = tordata.phipsi[i];
       if(iang==1){ // phi
          findsignphipsi(ang,geo.sheet[angtype][0],geo.sheet[angtype][1],geo.sheet[angtype][4],
              &tordata.angsign[i],&mag);
//fprintf(fpout,"i=%d phi = %5.3f sign = %d mag = %5.3f\n",i,ang*180.0/PI,tordata.angsign[i],mag);
       }
       else{ //psi
          findsignphipsi(ang,geo.sheet[angtype ][2],geo.sheet[angtype][3],geo.sheet[angtype][4],
              &tordata.angsign[i],&mag);
//fprintf(fpout,"i=%d psi = %5.3f sign = %d mag = %5.3f\n",i,ang*180.0/PI,tordata.angsign[i],mag);
       }
       if(tordata.angsign[i]!=0){
          mag = tconst*mag*tordata.angsign[i];
          for(k=0;k<=2;k++){
            angtor[k] = mag*hbar[i][k];
            tor[i][k]=tor[i][k]-angtor[k];
            tor[i+1][k]=tor[i+1][k]+angtor[k];
         };
       } // angsign !=0 
//     } // type !=0 
   }
}

void fuseforce(struct tordyn& tordata,float fusev[3],float fusefor[3])
/* finds the force that keeps two end residues together
  fusev = vector from tordata towards tordata2
 Atoms move about .001 A per step;
 tordata.fuseconst is Force that keeps two ends togetther when run dynamics in middle
   (fusefor gets mult. by torconst before being added)
    Thus fuse force = density force for distance of 0.01 angstrom
*/
{
   int k;
   float mag;

   mag = sqrt( dot(fusev,fusev) );
   for(k=0;k<=2;k++){
        if(mag>1.0)
            fusefor[k] = tordata.fuseconst*fusev[k]/mag;
        if(mag> 0.3){
           fusefor[k] = tordata.fuseconst*fusev[k];
//printf(" mag = %5.3f  fusefor = %5.3f %5.3f %5.3f\n",mag,fusefor[0],fusefor[1],fusefor[2]);
        }
        else if (mag> 0.02){
           fusefor[k] = 0.3*tordata.fuseconst*fusev[k]/mag;
//printf(" mag = %5.3f  fusefor = %5.3f %5.3f %5.3f\n",mag,fusefor[0],fusefor[1],fusefor[2]);
        }
        else{ // mag < 0.002
            fusefor[k] = 0.0;
//printf(" mag = %5.3f  fusefor = %5.3f %5.3f %5.3f\n",mag,fusefor[0],fusefor[1],fusefor[2]);
        }
   }
}

#if 0 // NOT Used did not work       
void cbforce(struct tordyn& tordata,struct geometry& geo,float cabonefor[NUNIT1][3])
/* Find force in directcion of last Cb (0r Ca) toward tordata.cbfix
  unit[0][i][0,1][x,y,z] = x,y,z pos of ith unit1 of Ca and Cb
   and last Cb corresponds to i = tordata.nunit2
*/
{
    int iend,k;
    float mag;

    iend = tordata.nunit2-1;
    mag = distv(tordata.unit[0][iend][0],tordata.cbfix);
    if(mag> 0.02){
       for(k=0;k<=2;k++)
         cabonefor[iend][k] = tordata.bonefor*(tordata.cbfix[k]-
                       tordata.unit[0][iend][0][k])/mag;
    }
    else
       for(k=0;k<=2;k++)
         cabonefor[iend][k] = 0.0;
}

void boneforce(struct tordyn& tordata,struct geometry& geo,
       float cabonefor[NUNIT1][3],float nbonefor[NUNIT1][3])
/*  Find the force on Ca (and N??) in direction of edited mainbone
  1) Only applied to last nadd residues
  2) If dist > geo.camaindist then apply const force 
NOTE: only good for geo.dir = 0 (N to C direction)
*/
{
   int i,k,ibeg,iend,mpt[3],testpt;
   float pos[3],cadist,ndist,closevec[3],bdist;

   for(i=0;i<NUNIT1;i++){ // Set all forces = 0
     for(k=0;k<=2;k++){
       cabonefor[i][k] = 0.0;
       nbonefor[i][k] = 0.0;
     }
   }
   ibeg = tordata.nunit2-tordata.nadd+1; 
   if(ibeg<=1)
      ibeg = 1;
   iend = tordata.nunit2;
   testpt = tordata.testpt[0]; // first value
   for(i=ibeg;i<=iend;i++){
     if(i<iend){ // not last unit
        maincadist(geo,tordata.unit[0][i][0],1,testpt,tordata.nmax,mpt,
                                 &cadist,pos,&bdist) // Ca dist
        for(k=0;k<=3;k++)
           closevec[k] = (pos[k] -tordata.unit[0][i][0][k])/cadist; 
     }
     else{ // last unit
        maincadist(geo,tordata.unit[1][i][3],1,testpt,tordata.nmax,mpt,
                                 &cadist,pos,&bdist); // Ca dist
        for(k=0;k<=3;k++)
           closevec[k] = (pos[k] -tordata.unit[1][i][3][k])/cadist; 
     }
     if(cadist > geo.camaindist){ // then add constant force
        for(k=0;k<=2;k++)
          cabonefor[i][k] = tordata.bonefor*closevec[k];
     }
     maincadist(geo,tordata.unit[1][i][1],1,testpt,tordata.nmax,mpt,
                                 &ndist,pos,&bdist); // N dist
     for(k=0;k<=3;k++)
           closevec[k] = (pos[k] -tordata.unit[1][i][1][k])/ndist; 
     if(ndist > geo.camaindist){ // then add constant force
        for(k=0;k<=2;k++)
          nbonefor[i][k] = tordata.bonefor*closevec[k];
     }
     testpt = mpt[1];  //  value used for subsequent residues - mpt returned from N dist
//printf("In bonefor:i = %d  ndist = %5.3f nbonefor = %5.3f cadist = %5.3f  x cabonefor = %5.3f\n",
//         i,ndist,nbonefor[i][0],cadist,cabonefor[i][0]);
   }
}   
#endif

void runtordyn(struct griddata& den1,struct geometry& geo,
     struct tordyn& tordata,struct tordyn& tordata2)
/*
NEW - 8feb00 - CORRECTED MAJOR ERROR - OLD VERSION ASSUMED ONLY 
    2 ATOMS IN LAST UNIT - NOT CORRECT FOR AMINO ACIDS
ALSO TRIED USING verlet algorithm for new position:  
  Need to store store old phi/psi and old chi .
New  19may99 - define tordata2 -
 Used if geo.fixend = 3  - apply force to keep end atoms
    in tordata and tordata2 close
  dir = 0 or 1: N to C or C to N direction
 If geo.fixend = 0; then first unit (base) is NOT fixed
 If geo.fixend = 1 or 2 , then first unit is fixed
 If geo.fixend = 3  Then set up two tordatas (tordata (nterm end)
    and tordata2 (cterm end) and overlap one residue
    apply new  force to keep them close.
Modified from old bondyn.c to run mol. dyn. on the geo peptide
New (3jun98)  -  Add angle restraints:
  If new phi (after time step ) is beyond allowed range,
    then arbitrarily set it to limiting value   
New: Add side chain dynamics.
RUN the torsional dynamics routines described by Brunger
    for trace# itrace
Assume dimensionless mass =1 for all atoms.
   Run the dynamics for 
 rij and drij = r between i and j and time derivative of r
 sji  r from c.of mass of j to bond pt in j from i.
 ru[i]  = r to c. of mass of i
 hbar = hij (unit vector) along bond from i to j
 b1 and b2 are the B matrices
 db1 and db2 time der. of B matrices
 dd is the D matrix
 mm,kk and ll and M, K and L matrices.
 iner = inertial matrix
 tor = torque vector
 for = force vector
 qq=Q vector
Modify so that can fix distal end - add new matrices - NOTE - DID NOT WORK - REMOVED
  pm = PHIM   (6x6 matrix)
  pmt = PHIM transpose  (6x6 matrix)
  pl = PHIL   (6x6 matrix)
  rhs = RHS  (1 x 6 vector) 
NOTE: matrices mm[i],kk[i],ll[i],qq[i] corespond to mm[i+1], etc.
NOTE:  gcang and rtor are not used now 
     autolist[0][0]= # of separate (not connected) auto build traces
     autolist[i][0] = 1 or 2   Indicates have finished trace and
                               whether start trace i with unit 1 or 2
     autolist[i][1]= # of the first unit1 on trace i;
     autolist[i][2]= #  unit1s on trace i;
     autolist[i][3]= # of first unit2 on trace i;
     autolist[i][4]= # of unit2s on trace i;

  unit[0][i][0,1][x,y,z] = x,y,z pos of ith unit1 of Ca and Cb
     unit[1][i][0,1][x,y,z] = x,y,z,pos of ith unit2 of Co and N


 uside[i][0,1][x,y,z] = xyz postion of C (0) of side chain i. 
    at End of side chain - have C - C (2 atoms) correspond to 0, and 1. 
 sidedata[i]0]= # of side chains on ith unit1.
 sidedata[i][1]= the # of uside that starts on ith unit1.
 sidedata[0][0]= tot. # of usides that have been currently assigned
  Add side chain dynamics:
     For side, which is just single carbon atoms except for last 2 atoms:
       intertia =0,
       hbar=rij, sij=0, torque=0, M matrix is simply m for i=j (i=0,1,2,) and \
          0 otherwise.  centor of mass = atom pos.
IF there is side chain - do not want to include Cb in unit1, but if at beginning, want N and 
    Ca and if at end want Ca and Co  (the i=0 and i=2 atoms)
   use cstep = 1 or 2 so that jump over Cb in sumation.

*/
{
   int i,ii,j,jj,ix,k,icnt[2],itype,ntype,count,tcount,ifind,iend;
   float ru[NUNIT][3],rij[NUNIT][3],hbar[NUNIT][3],sji[NUNIT][3];
   float rus[NSUNIT][3],rijs[NSUNIT][3],hbars[NSUNIT][3],sjis[NSUNIT][3];
   float ru1,inert[NUNIT][4][3],mag; 
   float dhbar[3],dsji[3],drij[3];
   float b1[NUNIT][6][6],b2[NUNIT][6],dd[NUNIT][6];
   float inerts[NSUNIT][4][3],b1s[NSUNIT][6][6],b2s[NSUNIT][6],dds[NSUNIT][6];
   float db1[6][6],db2[6],kk[6][6],ll[6],mm[NUNIT][6][6];
   float pm[6][6],pmt[NUNIT][6][6];
   float kks[NSUNIT][6][6],lls[NSUNIT][6],mms[NSUNIT][6][6];
   float hcs[3],hcds[3],dhcs[3],forv[3],dr[NUNIT][4][3],qq[NUNIT][6],drca[3];
   float drs[NSUNIT][4][3],qqs[NSUNIT][6];
   float tor[NUNIT][3],force[NUNIT][3],wcf[3],iw[3],wciw[3];
   float forces[NSUNIT][3],tors[NSUNIT][3];
   float invmat[NUNIT],addmat[6][6],tranb1[6][6],temp[NUNIT],wu[3],wu2[3];  
   float invmats[NSUNIT],temps[NSUNIT],vec3s[NSUNIT][6];  
   float vec1[6],vec2[6],vec3[NUNIT][6],vec4[6],vec5[6],vec6s[6],vec6[NUNIT][6],vec7[6],vec8[6]; /* temp vectors*/
   float mat1[6][6],mat2[6][6],mat3[6][6]; /* 6x6 matrix */
   float add0[6][6],invadd0[6][6],for1[3];
   float dyy[NUNIT][6],ddq[NUNIT],xx,rf[2];
   float dyys[NSUNIT][6],ddqs[NSUNIT],phi,psi;
   int iran,natoms,cstep,catoms,cunit1,sidetocount[NSUNIT];
   int phipsiconstraint;
   float fusev[3],fusefor[3];
   int bonebeg,boneend;
   float oldphipsi;
   float origrfor,origtorconst; // used if reduce random force on side chains
//   float dforv[3];  // Diamond force
//float addsav[6][6],tempmat[6][6],testunit[6][6][6],testv[3];
//float tmat3[3][3],tmat6[6][6];
clock_t time0,time1;

//verbose = 2;   
#if 0
i=3;
 printf("At beginning of runtordyn: Ca = %5.3f",tordata.unit[0][i][0][0]);
 printf("  Cb = %5.3f",tordata.uside[8][0][0]);
 for(jj=0;jj<=tordata.sidedata[i][3];jj++) // rotate all points in last unit connected to connect point
   printf(" jj=%d: %5.3f",jj,tordata.uside[9][jj][0]);
 printf("\n");
copyautotogeo(geo,tordata);
#endif       

origrfor = tordata.rfor;
origtorconst = tordata.torconst;
//phipsiconstraint = 0;  // if = 1, apply constraints to angle rotation NOTE: THIS MAY NOT WORK CORRECTLH
phipsiconstraint = 2;  // if = 2, apply tortional angle constraints to angle rotation NOTE: THIS MAY NOT WORK CORRECTLH
/*  itype = 0 or 1 depending on whether unit1 or unit2
  icnt[0,1] = # of keeps track of the unit1,unit 2 that is being used
  geo peptide always begins and ends with Ca (without Cb)
    This means actual begin and end with unit2 with extra atom attached\
     and total number of peptide = geo.nend[1] and geo.nend[0] =1
  todata.nunit1 or 2 = # unit1 or 2
*/
//time0 = clock();
   if(tordata.usediamond==1) // Use diamond force on atoms
      setupdiamondforce(den1,geo,tordata); // This finds qden[] = density from allmving atoms
//time1 = clock();
//printf("time for setupdiamond (microsec) = %d\n",time1-time0);
#if 0
   float cabonefor[NUNIT1][3],nbonefor[NUNIT1][3];
   if(tordata.useboneconstr==1){ // Add Ca (and N?) constraint to keep Ca close to edite dmain
      bonebeg = tordata.nunit2-tordata.nadd+1; // Only use if boneforconst = 1
      if(bonebeg<=1)
         bonebeg = 1;
      boneend = tordata.nunit2;
      boneforce(tordata,geo,cabonefor,nbonefor);
  }
  else if(tordata.useboneconstr==2){ // Add force to bring last Cb close to tordata.cbfix
      cbforce(tordata,geo,cabonefor);
  }
#endif
  itype = 1; // indicates first unit is unit2
  icnt[0] = 1; // number of first unit1
  icnt[1] = 1;// number of first unit2
  tcount = tordata.nunit1+tordata.nunit2; 

   cunit1=icnt[0]-1; /*The unit 1 number minus 1 */
   for(count=1;count<=tcount;count++){ /* find inertia, force ietc for main chain atoms */
     if(itype==0){ /* unit 1 type*/
         if( (count==1)||(count==tcount) ){/* at end - add atoms */
           printf("ERROR:Should not reach here Have assumed in several places that begin with type2\n");
           exit(0);
           natoms=3;
         }
         else{ // not at end
           cunit1=cunit1+1; /* counts unit1 type */
           natoms = 2; // Use Cb always
#if 0 // use Cb only if in density
           if(tordata.cbden[cunit1]==0){ // Cb not in density do not use it
              natoms = 1;
//printf(" Cb not in density for cunit1 = %d\n",cunit1);
           }
           else // Cb in density
             natoms=2; 
#endif
         } // not at end
         sidetocount[cunit1-1 ]=count;
         if( (tordata.sidedata[cunit1][0]>0)||(tordata.sidedata[cunit1][2]>=0)  ){// if side atoms or if is a residue type
            cstep=2; /* skip Cb because it is considered as part of side chain */
/*cstep=1;*/
            jj=tordata.sidedata[cunit1][1]; /*Assign Ca to uside:  jj= # of first side on this unit1 (cunit1)*/
            for(k=0;k<=2;k++)
              tordata.uside[jj][0][k]=tordata.unit[0][cunit1][0][k]; /* The pos. of Ca */
         }
         else{
            cstep=1;
         }
     } // if type 1
     else{ /* type 2 */
         if( (count==1)||(count==tcount) )/* at end - add atoms */
           natoms=4;
         else
           natoms=3; 
         cstep=1;
     }
     for(k=0;k<=2;k++) /* find cm. of mass. vector = ru */
        ru[count][k]=0;
     catoms=0;
     for(ii=0;ii<natoms;ii=ii+cstep){
        catoms=catoms+1;
        for(k=0;k<=2;k++)
           ru[count][k]=ru[count][k]+tordata.unit[itype][ icnt[itype] ][ii][k];
     }
     for(k=0;k<=2;k++)
        ru[count][k]=ru[count][k]/catoms;
     for(k=0;k<=2;k++)
        for(j=0;j<=2;j++)
          inert[count][j][k]=0;
     for(ii=0;ii<natoms;ii=ii+cstep) /* Find dr = vector from c. m. to atom ii */
        for(k=0;k<=2;k++)
          dr[count][ii][k]=tordata.unit[itype][ icnt[itype] ][ii][k]-ru[count][k];
     if(count==1){//Assumes first unit type2, need vector from c.m. to next Ca for nstep1
        for(k=0;k<=2;k++)
          drca[k]=tordata.unit[0][1][0][k]-ru[count][k];        
     }
     for(ii=0;ii<natoms;ii=ii+cstep){/* sum over two atoms in unit - generate inert. ten.*/
        ru1=dot(dr[count][ii],dr[count][ii]); /*= sqr or vector length;  Cannot use above defintion if more then 2 atoms */
        for(k=0;k<=2;k++){
          inert[count][k][k]=inert[count][k][k]+(ru1-sqr(dr[count][ii][k]));/* diagonal tems*/
        }           
        for(k=0;k<=2;k++){
          for(j=k+1;j<=2;j++){
             inert[count][k][j]=inert[count][k][j]-dr[count][ii][k]*dr[count][ii][j];
          }
        }
     } /* ii*/
     for(j=0;j<=2;j++) /* tensor is symetric */
        for(k=j+1;k<=2;k++)
           inert[count][k][j]=inert[count][j][k];
    for(k=0;k<=2;k++){
        force[count][k]=0; /* force on c.m. */
        tor[count][k]=0; /*total torque */
     }
     iran=2;
#if 0       
     for(k=0;k<=2;k++)
        wu[k]=tordata.yy[count][k+3];/* rotational velocity, omega */
#endif 

     for(ii=0;ii<natoms;ii=ii+cstep){/* sum over atoms in unit - generate force and torge vectores ten.*/ 
#if 0       
        crossprod(wu,dr[count][ii],vu); /* vu = rotation vel. comp. of atom ii */
        for(k=0;k<=2;k++) /* add c.m. velocity to ang. vel. = total vel  - used to find frictional drag forece */
           vu[k]=vu[k]+tordata.yy[count][k];
#endif 
        if(geo.quit==1)
           printf("geo.quit=1 before findforden\n");

#if 0 // Find cpu time for diamondforce
time0 = clock();
for(k= 1;k<=1000;k++)
        if(tordata.usediamond!=1)
            findforden(den1,geo,tordata.unit[itype][ icnt[itype] ][ii],forv);
time1 = clock();
printf("time for 1000 calls findforden (microsec) = %d\n",time1-time0);
        else
time0 = clock();
for(k= 1;k<=1000;k++)
            diamondfor(den1,geo,tordata.unit[itype][ icnt[itype] ][ii],dforv);
time1 = clock();
printf("time for diamondfor (microsec) = %d\n",time1-time0);
 printf(" at 1force vs diamondfor: x: (%5.3f  %5.3f)  y: (%5.3f  %5.3f) z:(%5.3f  %5.3f)\n",
    forv[0],dforv[0],forv[1],dforv[1],forv[2],dforv[2]); 
exit(0);
#endif

        if(tordata.usediamond!=1){
            findforden(den1,geo,tordata.unit[itype][ icnt[itype] ][ii],forv);
        }
        else{
            diamondfor(den1,geo,tordata.unit[itype][ icnt[itype] ][ii],forv);
        }
        if(geo.quit==1){
          if(verbose>=2)
              printf("At 1 geo.quit = 1 in findforden: tordata.unit[%d][ %d ][%d][x,y,z]=%5.3f %5.3f %5.3f\n",
                   itype,icnt[itype],ii,tordata.unit[itype][ icnt[itype] ][ii][0],
                       tordata.unit[itype][ icnt[itype] ][ii][1],tordata.unit[itype][ icnt[itype] ][ii][2]);
             goto quitrout;
        }
#if 0  // for debugging
if(geo.quit==1){
  printf("itype = %d  icnt[itype] = %d ii = %d  pos = %5.3f %5.3f %5.3f\n",
              itype,icnt[itype],ii,tordata.unit[itype][ icnt[itype] ][ii][0],
              tordata.unit[itype][ icnt[itype] ][ii][1],tordata.unit[itype][ icnt[itype] ][ii][2]);  
 exit(0);
}
#endif
        if(tordata.fixend==3){ //Add force Then try to keep atoms in tordata and tordata2 close for end residue
           if( (itype==1)&&( icnt[itype] == tordata.fend )&& (ii<=2) ){ // last residue C,O, and N atoms
              for(k=0;k<=2;k++)
                 fusev[k]=tordata2.unit[itype][tordata2.fend][ii][k]-
                               tordata.unit[itype][tordata.fend ][ii][k];  
              fuseforce(tordata,fusev,fusefor);
              for(k=0;k<=2;k++) // add new force between ends
                  forv[k]= forv[k]+ fusefor[k];
           } // if last residue
           if( (itype==1)&&( icnt[itype] == tordata.fend )&& (ii==3) ){ //last res. Ca
              for(k=0;k<=2;k++)
                  fusev[k]=tordata2.unit[0][tordata2.fend-1][0][k]-
                               tordata.unit[1][tordata.fend ][ii][k];
              fuseforce(tordata,fusev,fusefor);
              for(k=0;k<=2;k++) // add new force between ends
                  forv[k]= forv[k]+ fusefor[k];
           } // last Ca
           if((ii==0)&& (itype==0)&& 
                    (icnt[itype] == tordata.fend-1 )  ){ //Next to last -Ca
              for(k=0;k<=2;k++)
                  fusev[k]=tordata2.unit[1][tordata2.fend ][3][k]-
                              tordata.unit[itype][ tordata.fend-1 ][ii][k];
              fuseforce(tordata,fusev,fusefor);
              for(k=0;k<=2;k++) // add new force between ends
                  forv[k]= forv[k]+ fusefor[k];
           } // next to last Ca
        } // fixend= 3
        if(geo.quit==1){
          if(verbose>=2)
             printf("AT 5: geo.quit = 1\n");
          goto quitrout;
        }
float vu[3];

        if(geo.fixend==0){ // add force to both ends
           if( (count == 1)||(count == tcount)){ // NEW - have special force on end residue so that can INCREASE DENSITY FORCE ON END CA  
               tordata.torconst = tordata.torconstend;
           }
           else
               tordata.torconst = origtorconst;
        }
        else{ // add force to free end
           if(count == tcount){ // NEW - have special force on end residue so that can INCREASE DENSITY FORCE ON END CA  
               tordata.torconst = tordata.torconstend;
           }
           else
               tordata.torconst = origtorconst;
        }

        for(k=0;k<=2;k++){
           iran=iran+1;
           if(iran>1){
              rndnorm(rf);/* returns 2 rnad. num.  Normally dist.  mean-0, sig=1 */
              iran=0;
           }
           vu[k] = tordata.unitv[itype][ icnt[itype] ][ii][k];
           for1[k]=tordata.torconst*forv[k]+tordata.rfor*rf[iran]-
                         tordata.gclin*vu[k];
        } /*k*/
#if 0
        if(tordata.useboneconstr==1){ // Add Ca (and N?) constraint to keep Ca close to edite dmain
              if( (itype==0)&&(icnt[itype]>=bonebeg)&&(icnt[itype]<boneend)&&(ii==0) ){//Add Ca force
                 for(k=0;k<=2;k++)
                    for1[k] = for1[k]+ 5.0*cabonefor[ icnt[itype] ][k];
              }
              else if( (itype==1)&&(icnt[itype]==boneend)&&(ii==3) ){
                 for(k=0;k<=2;k++)
                    for1[k] = for1[k]+ 5.0*cabonefor[ icnt[itype] ][k];
              }
              if( (itype==1)&&(icnt[itype]>=bonebeg)&&(icnt[itype]<=boneend)&&(ii==1) ){
//printf("itype = %d icnt=%d ii= %d for1[0] = %5.3f   nbonefor = %5.3f\n",
//      itype,icnt[itype],ii,for1[0],nbonefor[ icnt[itype] ][0]);
                 for(k=0;k<=2;k++){
                    for1[k] = for1[k]+ 5.0*nbonefor[ icnt[itype] ][k];
                 }
              }
         } // use bone constraint
         else if(tordata.useboneconstr==2){ //Add force to bring last Cb close to tordata.cbfi
           if( (itype==0)&&(icnt[itype]==tordata.nunit2-1) &&(ii==0) ){//Add Cb or Ca force
               for(k=0;k<=2;k++)
                  for1[k] = for1[k]+ 5.0*cabonefor[ icnt[itype] ][k];
           }
        }
#endif
        crossprod(dr[count][ii],for1,wcf);
        for(k=0;k<=2;k++){ /* sum force and torque over all atoms */
           force[count][k]=force[count][k]+for1[k];
           tor[count][k]=tor[count][k]+wcf[k];
        }
#if 0
if(count <=2)
   printf("count = %d: ii=%d  forv[0]= %5.3f  vu[0] = %5.3f for1[0]= %5.3f\n",
          count,ii,forv[0],tordata.unitv[itype][ icnt[itype] ][ii][0],for1[0]);
#endif
     } /* ii */
//fprintf(fpout,"count = %d  force[0,1,2]= %6.3f  %6.3f %6.3f  tor =  %6.3f  %6.3f %6.3f \n",
//     count, force[count][0],force[count][1],force[count][2],tor[count][0],tor[count][1],tor[count][2]);
     icnt[itype]=icnt[itype]+1;
     itype=(itype+1)%2;
   }  /* count */

tordata.torconst = tordata.torconstside;  // NEW - 11jan01 - allow possibility of turning off density force on side chain atoms
  for(i=1;i<=tordata.nunit1;i++){ /* Repeat above for sidecha. atoms*/
      if(tordata.sidedata[i][0]>0){
#if 1  //Up to 12dec00 - USED THID - on 12dec00 - tried commenting out - probably made worse - better if use this condition
         if(tordata.sidedata[i][2]!= -1){ // side chain is an amino acid
//            tordata.rfor = origrfor/2.0; // Use smalle random force on side
            tordata.rfor = 2.0*origrfor; // Use larger random force on side
         }
         else{ 
            tordata.rfor = origrfor; 
         }
#endif
         for(j=tordata.sidedata[i][1]+1;j<tordata.sidedata[i][1]+tordata.sidedata[i][0];j++){/*Do not need for Ca */
            if( j==(tordata.sidedata[i][1]+tordata.sidedata[i][0]-1)){  /* last unit - find inertia etc.*/
               for(k=0;k<=2;k++){ // rus is vector to units center of mass
                 rus[j][k]=0.0;
                 for(ii=0;ii<=tordata.sidedata[i][3];ii++) //sidedata[i][3]  = number of atom in last unit
                    rus[j][k]= rus[j][k]+tordata.uside[j][ii][k];
                 rus[j][k] = rus[j][k]/(tordata.sidedata[i][3]+1);
               }
               for(k=0;k<=2;k++) // initialize inert matrix
                  for(jj=0;jj<=2;jj++)
                    inerts[j][jj][k]=0;
               for(ii=0;ii<=tordata.sidedata[i][3];ii++) /* Find dr = vector from c. m. to atom ii */
                  for(k=0;k<=2;k++)
                     drs[j][ii][k]=tordata.uside[j][ii][k]-rus[j][k];
               for(ii=0;ii<=tordata.sidedata[i][3];ii++){/* sum over atoms in unit - generate inert. ten.*/
                   ru1=dot(drs[j][ii],drs[j][ii]); /*= sqr or vector length;  Cannot use above defintion if more then 2 atoms */
                   for(k=0;k<=2;k++){
                      inerts[j][k][k]=inerts[j][k][k]+(ru1-sqr(drs[j][ii][k]));/* diagonal tems*/
                   }           
                   for(k=0;k<=2;k++){
                     for(jj=k+1;jj<=2;jj++){
                      inerts[j][k][jj]=inerts[j][k][jj]-drs[j][ii][k]*drs[j][ii][jj];
                    }
                 }
               } /* ii*/
               for(jj=0;jj<=2;jj++) /* tensor is symetric */
                 for(k=jj+1;k<=2;k++)
                    inerts[j][k][jj]=inerts[j][jj][k];     
               for(k=0;k<=2;k++){
                 forces[j][k]=0; /* force on c.m. */
                 tors[j][k]=0; /*total torque */
               }
               iran=2;
               for(ii=0;ii<=tordata.sidedata[i][3];ii++){/* sum over atoms in unit - generate force and torge vectores ten.*/ 
                  if(tordata.torconst>0.0001){ // do not need to find side force if torconst = 0       
                    if(tordata.usediamond!=1){
                      findforden(den1,geo,tordata.uside[j][ii],forv);
                    }
                    else{
                      diamondfor(den1,geo,tordata.uside[j][ii],forv);
                    }
                  }
// printf(" at 2: force vs diamondfor: x: (%5.3f  %5.3f)  y: (%5.3f  %5.3f) z:(%5.3f  %5.3f)\n",
//    forv[0],dforv[0],forv[1],dforv[1],forv[2],dforv[2]); 
#if 0 // increase density force on side
  for(k=0;k<=2;k++)
     forv[k] = 10.0*forv[k];
#endif
                  if(geo.quit==1){
                     if(verbose>=2)
                         printf("At 2 geo.quit = 1 in findforden\n");
                     goto quitrout;
                  }
                  for(k=0;k<=2;k++){
                     iran=iran+1;
                     if(iran>1){
                       rndnorm(rf);/* returns 2 rnad. num.  Normally dist.  mean-0, sig=1 */
                       iran=0;
                     }
                     for1[k]=tordata.torconst*forv[k]+tordata.rfor*rf[iran]-
                                 tordata.gclin*tordata.usidev[j][ii][k];
                  } /*k*/
                  crossprod(drs[j][ii],for1,wcf);
                  for(k=0;k<=2;k++){ /* sum force and torque over all atoms */
                    forces[j][k]=forces[j][k]+for1[k];
                    tors[j][k]=tors[j][k]+wcf[k];
                 }
              } /* ii */
            } /* if j = last unit*/
            else{ /* j not last unit, just one atom */
               for(k=0;k<=2;k++)
                 rus[j][k]=tordata.uside[j][0][k];
               for(k=0;k<=2;k++)
                  for(jj=0;jj<=2;jj++)
                    inerts[j][jj][k]=0;
               iran=2;
               if(tordata.torconst>0.0001){ // do not need to find side force if torconst = 0       
                 if(tordata.usediamond!=1){
                   findforden(den1,geo,tordata.uside[j][0],forv);
                 }
                 else
                   diamondfor(den1,geo,tordata.uside[j][0],forv);
               }
// printf(" at 3 force vs diamondfor: x: (%5.3f  %5.3f)  y: (%5.3f  %5.3f) z:(%5.3f  %5.3f)\n",
//    forv[0],dforv[0],forv[1],dforv[1],forv[2],dforv[2]); 
               if(geo.quit==1){
                   if(verbose>=2)
                      printf("At 3 geo.quit = 1 in findforden\n");
                    goto quitrout;
                }
               for(k=0;k<=2;k++){
                  iran=iran+1;
                  if(iran>1){
                    rndnorm(rf);/* returns 2 rnad. num.  Normally dist.  mean-0, sig=1 */
                    iran=0;
                  }
                  for1[k]=tordata.torconst*forv[k]+2*tordata.rfor*rf[iran]
                          -tordata.gclin*tordata.usidev[j][0][k];
               } /*k*/
               for(k=0;k<=2;k++){ 
                  forces[j][k]=for1[k];
                  tors[j][k]=0.0;
               }
           } /* if j != last unit*/
         } /* if */
      } /* j */
   } /* i */
tordata.rfor=origrfor; // set back to original value
tordata.torconst=origtorconst;


/* NOTE: rij,sji, and hbar do not have to be stored for entire seqeunce,
           but simpler to do so
Note index in matrix refers to lower number if connects 2 units:
    e.g  rij[3][]  is r34  index runs from 1,2,3, ... tcount-1
 If does not connect then index refers to that one
    e.g.  m[3] is for third unit., index runs f rom 1,2,,,,tcount
*/
  itype = 1; // indicates first unit is unit1
  icnt[0] = 1; // number of first unit1
  icnt[1] = 1;// number of first unit2
  for(i=1;i<=tcount;i++){/* initialize matrices, etc for mainchain */
     count=i;
     for(k=0;k<=2;k++)
        wu[k]=tordata.yy[count][k+3];
     if(i!=tcount){ /* find connect type matrices*/                    
        ntype= (itype+1)%2; /* type of next unit */
        for(k=0;k<=2;k++){
           rij[count][k]=ru[count+1][k]-ru[count][k];          
           sji[count][k]=tordata.unit[ntype][ icnt[ntype] ][ tordata.cbond[itype][1] ][k]
              -ru[count+1][k]; 
           hbar[count][k]=tordata.unit[ntype][ icnt[ntype] ][ tordata.cbond[itype][1] ][k]-
                    tordata.unit[itype][ icnt[itype] ][ tordata.cbond[itype][0] ][k];
        }
        mag=sqrt(dot(hbar[count],hbar[count]));
        for(k=0;k<=2;k++)
          hbar[count][k]=hbar[count][k]/mag;
        for(k=0;k<=2;k++)
           drij[k]=tordata.yy[count+1][k]-tordata.yy[count][k];
        for(j=0;j<6;j++){
          for(k=0;k<6;k++){
            b1[i][j][k]=db1[j][k]=0.0;
          }
          b1[i][j][j]=1.0;
       }
       b1[i][0][4]=rij[i][2];
       b1[i][0][5]= -rij[i][1];
       b1[i][1][3]= -rij[i][2];
       b1[i][1][5]= rij[i][0];
       b1[i][2][3]= rij[i][1];
       b1[i][2][4]= -rij[i][0];
       for(j=0;j<=2;j++){
         wu2[j]=tordata.yy[count+1][j+3];
       }
       crossprod(hbar[i],sji[i],hcs);
       crossprod(wu2,sji[i],dsji);
       crossprod(wu,hbar[i],dhbar);
       crossprod(hbar[i],dsji,hcds);
       crossprod(dhbar,sji[i],dhcs);
       for(k=0;k<=2;k++)
          b2[i][k]= -hcs[k];
       for(k=3;k<=5;k++)
          b2[i][k]= hbar[i][k-3];
       db1[0][4]=drij[2];
       db1[0][5]= -drij[1];
       db1[1][3]= -drij[2];
       db1[1][5]= drij[0];
       db1[2][3]= drij[1];
       db1[2][4]= -drij[0];
       for(k=0;k<=2;k++)
          db2[k]= -hcds[k]-dhcs[k];
       for(k=3;k<=5;k++)
          db2[k]= dhbar[k-3];
       mult66_61(db1,tordata.yy[count],vec5);
       for(k=0;k<=5;k++)
         dd[i][k]=vec5[k]+tordata.dq[count]*db2[k];
     } /* if i<tcount, e.g. connet type matrices*/
     icnt[itype]=icnt[itype]+1;
     itype=(itype+1)%2;
   } /* i */
   if(phipsiconstraint ==2) //Find angle constraints and add to previous torque
         findangtorque(geo,tordata,tcount,hbar,tor);
   for(i=1;i<=tcount;i++){/* initialize matrices, etc for mainchain */
     count=i;
     for(j=0;j<6;j++) /* for the non-connect matrices (M and Q, define also for i=tcount*/
       for(k=0;k<6;k++)
            mm[i][j][k]=0.0;
     for(j=0;j<=2;j++) /* generate M matrix*/
         mm[i][j][j]=1.0; /* = Mass of atom */
     for(j=3;j<=5;j++)
         for(k=3;k<=5;k++){
            mm[i][j][k]=inert[i][j-3][k-3];
         }
     for(j=0;j<=2;j++) /* generate Q vector*/
         qq[i][j]=force[count][j];
     multvec(inert[count],wu,iw);
     crossprod(wu,iw,wciw);
     for(j=3;j<=5;j++)
         qq[i][j]=tor[count][j-3]-wciw[j-3]; 
   }
   for(ix=1;ix<=tordata.nunit1;ix++){ /* Repeat above for sidecha. atoms*/
      if(tordata.sidedata[ix][0]>0){ 
        for(i=tordata.sidedata[ix][1];i<tordata.sidedata[ix][1]+tordata.sidedata[ix][0];i++){
            for(k=0;k<=2;k++)
               wu[k]=tordata.yys[i][k+3];
            if( i!=(tordata.sidedata[ix][1]+tordata.sidedata[ix][0]-1)){ /* Not at end, find connect type matrices*/
               if(i==tordata.sidedata[ix][1]){
                   jj=sidetocount[ix-1];
                   for(k=0;k<=5;k++)
                      tordata.yys[i][k]=tordata.yy[jj][k];
                   for(k=0;k<=2;k++)/* redefine */
                      wu[k]=tordata.yys[i][k+3];
                   for(k=0;k<=2;k++)
                      rijs[i][k]=rus[i+1][k]-ru[jj][k];
               }
               else{
                   for(k=0;k<=2;k++)
                       rijs[i][k]=rus[i+1][k]-rus[i][k];
               }
               for(k=0;k<=2;k++){ /* for i = Ca to end -1 */
                  drij[k]=tordata.yys[i+1][k]-tordata.yys[i][k];
                  hbars[i][k]=tordata.uside[i+1][0][k]-tordata.uside[i][0][k];
                  if(i==(tordata.sidedata[ix][1]+tordata.sidedata[ix][0]-2) ){  /*if i= end-1 then need sji becaue 2 atoms in last unit */       
                     sjis[i][k]=tordata.uside[i+1][0][k]-rus[i+1][k]; 
                  }
                  else{ 
                     sjis[i][k]=0.0;
                  }
               } /* k */

               mag=sqrt(dot(hbars[i],hbars[i]));
               for(k=0;k<=2;k++)
                  hbars[i][k]=hbars[i][k]/mag;
               for(jj=0;jj<6;jj++){
                  for(k=0;k<6;k++){
                     b1s[i][jj][k]=db1[jj][k]=0.0;
                  }
                  b1s[i][jj][jj]=1.0;
               }
               b1s[i][0][4]=rijs[i][2];
               b1s[i][0][5]= -rijs[i][1];
               b1s[i][1][3]= -rijs[i][2];
               b1s[i][1][5]= rijs[i][0];
               b1s[i][2][3]= rijs[i][1];
               b1s[i][2][4]= -rijs[i][0];
               for(jj=0;jj<=2;jj++){
                 wu2[jj]=tordata.yys[i+1][jj+3];
               }
               crossprod(hbars[i],sjis[i],hcs);
               crossprod(wu2,sjis[i],dsji);
               crossprod(wu,hbars[i],dhbar);
               crossprod(hbars[i],dsji,hcds);
               crossprod(dhbar,sjis[i],dhcs);
               for(k=0;k<=2;k++)
                 b2s[i][k]= -hcs[k];
               for(k=3;k<=5;k++)
                 b2s[i][k]= hbars[i][k-3];
              db1[0][4]=drij[2];
              db1[0][5]= -drij[1];
              db1[1][3]= -drij[2];
              db1[1][5]= drij[0];
              db1[2][3]= drij[1];
              db1[2][4]= -drij[0];
              for(k=0;k<=2;k++)
                 db2[k]= -hcds[k]-dhcs[k];
              for(k=3;k<=5;k++)
                 db2[k]= dhbar[k-3];
              mult66_61(db1,tordata.yys[i],vec5);
              for(k=0;k<=5;k++)
                 dds[i][k]=vec5[k]+tordata.dqs[i]*db2[k];
          } /* if j<ende.g. connet type matrices*/
          if(i>tordata.sidedata[ix][1]){
            for(jj=0;jj<6;jj++) /* for the non-connect matrices (M and Q, define also for i=tcount*/
              for(k=0;k<6;k++)
                mms[i][jj][k]=0.0;
            for(jj=0;jj<=2;jj++) /* generate M matrix*/
               mms[i][jj][jj]=1.0; /* = Mass of atom */
            for(jj=3;jj<=5;jj++)
              for(k=3;k<=5;k++)
                mms[i][jj][k]=inerts[i][jj-3][k-3];
            for(jj=0;jj<=2;jj++){ /* generate Q vector*/
              qqs[i][jj]=forces[i][jj];
            }
            multvec(inerts[i],wu,iw);
            crossprod(wu,iw,wciw);
            for(jj=3;jj<=5;jj++){
               qqs[i][jj]=tors[i][jj-3]-wciw[jj-3]; 
            }
          }
       } /*i*/
     } /* if sidepots */
   } /* ix */


/*  Set up matrices for recurrance relation
   NOTE:  e.g. b1[m][i][j] is actually b1[m,m+1][][] in paper by Bae and Haug
       mm[m][][]  is mm[m][][]*/
  /* Start backward recurrance procedure for each side chain*/
   count=0;  /* counts unit1's  that have side chains*/
   for(ix=1;ix<=tordata.nunit1;ix++){ 
      if(tordata.sidedata[ix][0]>0){
         count=count+1;
         for(i=0;i<6;i++){ /*kk and ll are recurance matrices, = 0 for i=N */
           lls[count][i]=0;
           for(j=0;j<6;j++)
             kks[count][i][j]=0;
         }
         for(i=tordata.sidedata[ix][1]+tordata.sidedata[ix][0]-2;i>=tordata.sidedata[ix][1];i--){/* start recurion at end -1 */
         /* generate the inverse matrix which is just a 1x1 float*/
            add66_66(mms[i+1],kks[count],addmat);
            mult16_66(b2s[i],addmat,vec1);/* this is actually b2transpose = 1row by 56 columns*/
            mult16_61(vec1,b2s[i],&invmats[i]); /* invmat is just  scaler */
            if(fabs(invmats[i])>0.00001)
               invmats[i]=1/invmats[i];
            else
               invmats[i]=0.0;  /* if invmat=0, then it is never used */
//printf("invmats[%d] = %5.3f\n",i,invmats[i]);
            tran66(b1s[i],tranb1); /* transpose of b1 */
            mult66_66(tranb1,addmat,mat1);
            mult66_66(mat1,b1s[i],mat2);/* mat2 = first term for kk */
            mult66_61(mat1,b2s[i],vec2);
            mult16_66(vec1,b1s[i],vec3s[i]);
            mult11_61(-invmats[i],vec3s[i],vec4);
            mult61_16(vec2,vec4,mat3); /* mat3 is second term for kk*/
            add66_66(mat2,mat3,kks[count]); /* returns next k matrix */
            mult66_61(addmat,dds[i],vec5);
            for(k=0;k<=5;k++)
               vec6s[k]=qqs[i+1][k]+lls[count][k]-vec5[k];  /* vec6 = first term in brackets in ll expresion*/
            mult66_61(addmat,b2s[i],vec7);
            mult16_61(b2s[i],vec6s,&temps[i]);/* temp is a scaler*/
            for(k=0;k<=5;k++) /* vec8 = term in brackes in ll expression*/
               vec8[k]=vec6s[k]-temps[i]*invmats[i]*vec7[k]; 
            mult66_61(tranb1,vec8,lls[count]); /* returns next ll vector */
       } /* i */
     } /* if */
  } /* ix */


   itype=1;
   cunit1=0; /*The unit 1 number minus 1 */
   ii=0;
#if 1 /* if = 0 debug:  do not add side force to main force */
   for(count=1;count<=tcount;count++){ /* add kks and lls to M and Q respective for each unit 1 that has sidechain*/
     if(itype==0){ /* unit 1 type*/
         cunit1=cunit1+1;
         if(tordata.sidedata[cunit1][0]>0){
           ii=ii+1;
           for(j=0;j<6;j++){
              qq[count][j]=qq[count][j]+lls[ii][j];
              for(k=0;k<6;k++)
                 mm[count][j][k]=mm[count][j][k]+kks[ii][j][k];
//fprintf(fpout,"  ii,j,k: %d %d %d kks = %5.3f",ii,j,k,kks[ii][j][k]);
           }
        }
     }
//fprintf(fpout,"\n"); 
     itype=(itype+1)%2;
   } /* count */
#endif

  /* Start backward recurrance procedure  for mainchain, using sidechain kks and lls */
   for(i=0;i<6;i++){ /*kk, ll, are recurance matrices, = 0 for i=N and pm = I for i=N */
      ll[i]=0;
      for(j=0;j<6;j++){
        kk[i][j]=0;
        pm[i][j]=0; // PhiM 
      }
   }
   for(i=0;i<6;i++) // pm = PHIM= identity matrix
      pm[i][i] =1;
   tran66(pm,pmt[tcount]); // make PhiM tranpose
   for(i=tcount-1;i>=1;i--){ /* start wtih the K(N-1) and L(N-1) equations*/
      /* generate the inverse matrix which is just a 1x1 float*/
      add66_66(mm[i+1],kk,addmat); // addmat = M + K
      mult16_66(b2[i],addmat,vec1);/* vec1 = B2t*(M+K)  (vec1 is tranpose vector= 1 row, 6 columns*/
      mult16_61(vec1,b2[i],&invmat[i]); /* invmat is just  scaler */
      invmat[i]=1/invmat[i]; // the inverse scaler
      tran66(b1[i],tranb1); /* transpose of b1 */
      mult66_66(tranb1,addmat,mat1);// mat1 = B1t*(M + K)
      mult66_66(mat1,b1[i],mat2);/* mat2 = B1t*(M+K)*B1 = first term for kk */
      mult66_61(mat1,b2[i],vec2);//vec2 = B1t*(M+K)*B2 (standard vector 6 rows, 1 columns 
      mult16_66(vec1,b1[i],vec3[i]);// vec3 = B2t*(M+K)*B1(=tranpose vector (1 row, 6 columns)
      mult11_61(-invmat[i],vec2,vec4);//vec4 = -(invscaler)*B1t*(M+K)*B2 (also used in pm , etc.)
      mult61_16(vec4,vec3[i],mat3); /* mat3= -(invscal)*B1t*(M+K)*B2*B2t*(M+K)*B1 is second term for kk*/
      add66_66(mat2,mat3,kk); /*  = new kk returns next k matrix */
      mult66_61(addmat,dd[i],vec5);
      for(k=0;k<=5;k++)
         vec6[i][k]=qq[i+1][k]+ll[k]-vec5[k];  /* vec6 = -(M+K)*D +Q+L =  first term in brackets in ll expresion*/
      mult66_61(addmat,b2[i],vec7);
      mult16_61(b2[i],vec6[i],&temp[i]);/* temp is a scaler= B2t*(  -(M+K)*D + (Q+L) ) */
      for(k=0;k<=5;k++) /* vec8 = term in brackes in ll expression*/
          vec8[k]=vec6[i][k]-temp[i]*invmat[i]*vec7[k]; 
      mult66_61(tranb1,vec8,ll); /* returns next ll vector */
  }
  /* when finish above - return kk and ll for first unit 
    Now - solve for dY[1] and
ddq[i]= -invmat[i]*( vec3[i]*dY[i] - temp[i])
or  ddq[i]= -invmat[i]*( vec3[i]*dY[i] - B2t[i]*( -vec6[i]+pmt[i]*lambda  if distal end fixed 
*/
   
#if 1  /* debug  if = 0  keep main chain fixed */
if(geo.fixend ==0){ // base not fixed, solve for dyy
   add66_66(kk,mm[1],add0);
   mymatinv(add0,invadd0);
   for(k=0;k<6;k++)
     vec1[k]=qq[1][k]+ll[k];
   mult66_61(invadd0,vec1,dyy[1]); 
}
else{  // base body fixed, set dyy = 0
   for(k=0;k<6;k++)
      dyy[1][k] = 0;
}
  
   mult16_61(vec3[1],dyy[1],&xx);
   ddq[1]= -invmat[1]*(xx-temp[1]);
   for(i=2;i<=tcount;i++){ /* generate the rest of dyy and ddq recursively*/
      mult66_61(b1[i-1],dyy[i-1],vec1);
      for(k=0;k<=5;k++){
         dyy[i][k]=vec1[k]+ddq[i-1]*b2[i-1][k]+dd[i-1][k];
      }
      if(i<tcount){/* for last unit, ddq does not exist */ 
        mult16_61(vec3[i],dyy[i],&xx);
        ddq[i]= -invmat[i]*(xx-temp[i]);
      }
   }

//printf("phipsi angles:\n");
   for(i=1;i<=tcount;i++){/* Find new yy and dq for all units*/
      for(k=0;k<6;k++)
         tordata.yy[i][k]=tordata.yy[i][k]+dyy[i][k]*tordata.delt;
      if(i<tcount){
#if 0 // old version
         tordata.dq[i]= tordata.dq[i]+ddq[i]*tordata.delt;
         tordata.phipsi[i] = tordata.phipsi[i]+ tordata.dq[i]*tordata.delt+
              0.5*ddq[i]*tordata.delt*tordata.delt;
#endif
#if 1 // New  - Verlet algorithm
         oldphipsi = tordata.phipsi[i];  
         tordata.phipsi[i] = 2.0*tordata.phipsi[i] - tordata.oldphipsi[i]+
                                 ddq[i]*tordata.delt*tordata.delt;
//printf("  i=%d type=%d: %5.3f",i,tordata.ttype[i],tordata.phipsi[i]);
         tordata.dq[i] = (tordata.phipsi[i]-tordata.oldphipsi[i])/(2.0*tordata.delt);
         tordata.oldphipsi[i] = oldphipsi;
#endif
      }
   } /* i */
//printf("\n");

  itype = 1; // indicates first unit is unit1
  icnt[0] = 1; // number of first unit1
  icnt[1] = 1;// number of first unit2
  ntype= (itype+1)%2; /* type of next unit */
  nstep1(geo.de,geo.ep,tordata.delt,tordata.unit,tordata.unitv,tordata.yy[1],dyy[1],dr[1],drca); // rotates and translates first residue (including nextg Ca)
// Now make the rest of the chain, starting with residue and using new phi/psi angles
  ifind = 0; // indicates find Cb
  for(i=2;i<=tordata.nunit2;i++){
    if(geo.dir==0){
       phi= tordata.phipsi[1+2*(i-2)];
       psi =tordata.phipsi[2+2*(i-2)];
//fprintf(fpout,"i=%d: phi = %5.3f  psi = %5.3f\n",i,phi*180.0/PI,psi*180.0/PI);
       if(i == tordata.nunit2) // at end add Ca to last unit2
          iend =1;
       else iend = 0;
       if(tordata.cis[i+1]!=1) // standard tran
           makeunitalanine(geo,tordata.unit,tordata.unitv,ifind,iend,i-1,phi,
                           psi,tordata.delt);
       else{
//printf("Using makecisunit for i = %d\n",i);
           makecisunitalanine(geo,tordata.unit,tordata.unitv,ifind,iend,i-1,phi,
                           psi,tordata.delt);
       }
     } // dir = 0
     else if(geo.dir==1){ // NOTE: phi/psi must be negative of real phi/psi - first use postitive for constraint conditon, then change sign
        psi= tordata.phipsi[1+2*(i-2)]; //in revrese direction - first angle = phi
        phi = tordata.phipsi[2+2*(i-2)];
//fprintf(fpout,"i=%d: phi = %5.3f  psi = %5.3f\n",i,phi*180.0/PI,psi*180.0/PI);
        phi = -phi;  // change sign for use in makerevunitalanine
        psi = -psi;
        if(i == tordata.nunit2) // at end add Ca to last unit2
           iend =1;
        else iend = 0;
        if(tordata.cis[i-1]!=1) // standard tran
           makerevunitalanine(geo,tordata.unit,tordata.unitv,ifind,iend,i-1,
                   phi,psi,tordata.delt);
        else{
//printf("Using makecisrevunitalnaine for i = %d \n",i);
           makecisrevunitalanine(geo,tordata.unit,tordata.unitv,ifind,iend,i-1,
                   phi,psi,tordata.delt);
        }
     } // dir =1
   } // i
#endif //keeps mainchain fixed if omitted

   ii=0; 
   itype=1; /* run through mainch; copy Ca value to first dyys; etc. */
   for(i=1;i<=tcount;i++){
      if(itype==0){ /* Ca atom type */
        ii=ii+1;
        if(tordata.sidedata[ii][0]>0){/* has side chain */
           if( (tordata.sidedata[ii][2]!=PRO)&
                (tordata.sidedata[ii][2]!=ALA) ){
#if 1 /* if = 0 debug  keep dyys=0 */
            for(k=0;k<6;k++){ /* copy dyy for Ca to dyys[1]*/
               dyys[ tordata.sidedata[ii][1] ][k]=dyy[i][k];
/*               tordata.yys[ tordata.sidedata[ii][1] ][k]=tordata.yy[i][k];*/
/*dyys[ sidedata[ii][1] ][k]=0.0;*/
            }
#endif
#if 0 /*if = 1  debug  keep dyys=0 */
            for(k=0;k<6;k++){ /* copy dyy for Ca to dyys[1]*/
               dyys[ tordata.sidedata[ii][1] ][k]=0.0;
            }
#endif

#if 1 /*debug  DO NOT CHANGE dyys and ddqs */
            mult16_61(vec3s[ tordata.sidedata[ii][1] ],dyys[ tordata.sidedata[ii][1] ],&xx);
            ddqs[ tordata.sidedata[ii][1] ]= -invmats[ tordata.sidedata[ii][1] ]*(xx-temps[ tordata.sidedata[ii][1] ]);            
#if 0 // Old version
            tordata.dqs[ tordata.sidedata[ii][1] ]=tordata.dqs[ tordata.sidedata[ii][1] ]+
                       ddqs[ tordata.sidedata[ii][1] ]*tordata.delt;
            tordata.chi[ tordata.sidedata[ii][1] ]=tordata.chi[ tordata.sidedata[ii][1] ]+
                         tordata.dqs[ tordata.sidedata[ii][1] ]*tordata.delt;
#endif
#if 1 // New  - Verlet algorithm
             oldphipsi = tordata.chi[ tordata.sidedata[ii][1] ];  
             tordata.chi[ tordata.sidedata[ii][1] ]= 2.0*tordata.chi[ tordata.sidedata[ii][1] ] -
                        tordata.oldchi[ tordata.sidedata[ii][1] ]+
                        ddqs[ tordata.sidedata[ii][1] ]*tordata.delt*tordata.delt;
             tordata.dqs[ tordata.sidedata[ii][1] ] =(tordata.chi[ tordata.sidedata[ii][1] ] -
                                  tordata.oldchi[ tordata.sidedata[ii][1] ])/(2.0*tordata.delt);
             tordata.oldchi[ tordata.sidedata[ii][1] ] = oldphipsi;              
#endif
#endif  // debug  DO NOT CHANGE dyys and ddqs
         }// if not PRO
       } // if sidechain
      }// if itype 
      itype=(itype+1)%2;
   } // i
   for(i=1;i<=tordata.nunit1;i++){ /* generate rest of dyys and ddqs recursively */
     if(tordata.sidedata[i][0]>0){
       if( (tordata.sidedata[i][2]!=PRO)&
              (tordata.sidedata[i][2]!=ALA) ){
        for(j=tordata.sidedata[i][1]+1;j<tordata.sidedata[i][1]+tordata.sidedata[i][0];j++){/* start at Cb*/
            mult66_61(b1s[j-1],dyys[j-1],vec1);
            for(k=0;k<=5;k++){
               dyys[j][k]=vec1[k]   +ddqs[j-1]*b2s[j-1][k]  +dds[j-1][k];
            }
            for(k=0;k<6;k++)
                tordata.yys[j][k]=tordata.yys[j][k]+dyys[j][k]*tordata.delt;
#if 1 /*debug  DO NOT CHANGE dyys and ddqs */
            if(j<(tordata.sidedata[i][1]+tordata.sidedata[i][0]-1)){ /* for last atom, dq not defined*/
               mult16_61(vec3s[j],dyys[j],&xx);
               ddqs[j]= -invmats[j]*(xx-temps[j]);
#if 0 // old version
               tordata.dqs[j]=tordata.dqs[j]+ddqs[j]*tordata.delt;
               tordata.chi[j]=tordata.chi[j]+tordata.dqs[j]*tordata.delt;
#endif
#if 1 // New  - Verlet algorithm
               oldphipsi = tordata.chi[j];
               tordata.chi[j] = 2.0*tordata.chi[j] -tordata.oldchi[j] +
                                                     ddqs[j]*tordata.delt*tordata.delt;
               tordata.dqs[j]=(tordata.chi[j]-tordata.oldchi[j])/(2.0*tordata.delt);
               tordata.oldchi[j] = oldphipsi;
#endif
            }// j not last atom
#endif /*debug  DO NOT CHANGE dyys and ddqs */

#if 1 //INSERT NEW  ROUTINE FOR RINGANGLE:
float uv[3],vv[3],wv[3],inertr[3][3],torring[3],rr[3],tempv[3],rmag,lmag;
float domega;
            if(tordata.useringang==1){ //Find rotation angle of last unit if a ring
               if(j== tordata.sidedata[i][1]+tordata.sidedata[i][0]-1){// last unit
                 if( (tordata.sidedata[i][2]==TYR)||(tordata.sidedata[i][2]==HIS)||
                     (tordata.sidedata[i][2]==PHE)||(tordata.sidedata[i][2]==TRP) ){ // if ring compound allow bond angle to  move
                   for(k=0;k<=2;k++){
                     uv[k] = tordata.uside[j-2][0][k]-tordata.uside[j-1][0][k]; // Cb to Ca vector (use i=0 because first atom in unit
                     vv[k] = tordata.uside[j][0][k]-tordata.uside[j-1][0][k]; // Cb to Cb2 vector
                   }
                   crossprod(uv,vv,wv); // w (omega) vector
                   mag= sqrt( dot(wv,wv) );
                   for(k=0;k<=2;k++) 
                     wv[k] = wv[k]/mag; // wv = unit vector in direction of rotation axis
                   for(k=0;k<=2;k++) 
                     rr[k]= rus[j][k]-tordata.uside[j-1][0][k];  // vector from Cb to c.m.  (rotation axis passes through Cb)
                   for(k=0;k<=2;k++) //first find inertia of c.m. about Cb, then add inertia matrx using c.m. cord.
                        inertr[k][k]=0;/* diagonal term = 0 for c.m. pt. rotating about Cb*/
                   for(k=0;k<=2;k++)
                       for(jj=k+1;jj<=2;jj++)
                         inertr[k][jj]= -rr[k]*rr[jj];
                   for(jj=0;jj<=2;jj++) /* tensor is symetric */
                      for(k=jj+1;k<=2;k++)
                         inertr[k][jj]=inertr[jj][k];     
                   for(k=0;k<=2;k++) // Now add inertia matrix for c.m. cordinats
                     for(jj=0;jj<=2;jj++)
                       inertr[jj][k]= inertr[jj][k]+inerts[j][jj][k];
                   multvec(inertr,wv,tempv);  // Mult inert. matrix times unit rotation vector
                   rmag = dot(wv,tempv);  // component of r.h.s. in direction of rotation vector.
                   crossprod(rr,forces[j],torring); // Find torque of c.m. about cb. (forces if sum of forces on last unit
                   for(k=0;k<=2;k++) // add torque of all atoms about c.m. (tors total torque in c.m. cordinaates)
                      torring[k] = torring[k]+tors[j][k];
                   lmag = dot(wv,torring); // component of torque (l.h.s.) along rotation vector.
                   domega = lmag/rmag; //= accelleration of domega/dt = accelleration of rotation angle.
                   oldphipsi = tordata.ringang[i];//verlet algorithm
                   tordata.ringang[i] = 2.0*tordata.ringang[i] -tordata.oldringang[i] +
                                                     domega*tordata.delt*tordata.delt;
                   if(tordata.ringang[i]> (geo.phe + tordata.ringlim) )
                      tordata.ringang[i] = geo.phe + tordata.ringlim;
                   else if(tordata.ringang[i]< (geo.phe - tordata.ringlim) )
                      tordata.ringang[i] = geo.phe - tordata.ringlim; 
                   tordata.omega[i]=(tordata.ringang[i]-tordata.oldringang[i])/(2.0*tordata.delt);
                   tordata.oldringang[i] = oldphipsi;
                 } //j = ring
               }//j= lastunit
            }// useringang = 1
#endif  //END OF NEW INSERTION.

        } /* j */
       } // not PRO
     } /* if >0*/
   } /* i */
   if(tordata.sidedata[1][2]== -1){ // Then side chain just list of C's, build old way by adding each atom
      nsidestep(geo,tordata);
   }
   else{ //NOTE - THIS MEANS CANNOT MIX A.A SIDE AND LIST OF C'S IN SAME FIT side are amino acid - following is crude but works
      copyautotogeo(geo,tordata);
      copysidegeotoauto(geo,tordata);
#if 0
i=3;
 printf("Ca = %5.3f",tordata.unit[0][i][0][0]);
 printf("  Cb = %5.3f",tordata.uside[8][0][0]);
 for(jj=0;jj<=tordata.sidedata[i][3];jj++) // rotate all points in last unit connected to connect point
   printf(" jj=%d: %5.3f",jj,tordata.uside[9][jj][0]);
 printf("\n");
 exit(0);
#endif      
   }
quitrout:;
}


void runonesidetordyn(int iside,struct griddata& den1,struct geometry& geo,
     struct tordyn& tordata)
// Tordyn for just one side chain = iside
{
   int i,ii,j,jj,ij,ix,k,count;
   float ru[NUNIT][3];
   float rus[NSUNIT][3],rijs[NSUNIT][3],hbars[NSUNIT][3],sjis[NSUNIT][3];
   float ru1,mag; 
   float dhbar[3],dsji[3],drij[3];
   float inerts[NSUNIT][4][3],b1s[NSUNIT][6][6],b2s[NSUNIT][6],dds[NSUNIT][6];
   float db1[6][6],db2[6];
   float kks[NUNIT1][6][6],lls[NUNIT1][6],mms[NUNIT1][6][6];
   float hcs[3],hcds[3],dhcs[3],forv[3];
   float drs[NSUNIT][12][3],qqs[NSUNIT][6];
   float wcf[3],iw[3],wciw[3];
   float forces[NSUNIT][3],tors[NSUNIT][3];
   float addmat[6][6],tranb1[6][6],wu[3],wu2[3];  
   float invmats[NSUNIT],temps[NSUNIT],vec3s[NSUNIT][6];  
   float vec1[6],vec2[6],vec6s[6],vec4[6],vec5[6],vec7[6],vec8[6]; /* temp vectors*/
   float mat1[6][6],mat2[6][6],mat3[6][6]; /* 6x6 matrix */
   float for1[3];
   float xx,rf[2];
   float dyys[NSUNIT][6],ddqs[NSUNIT];
   int iran,sidetocount[NSUNIT];
//float dforv[3];


   if(tordata.usediamond==1) // Use diamond force on atoms
      setupdiamondforce(den1,geo,tordata); // This finds qden[] = density from allmving atoms
  i = iside;
      if(tordata.sidedata[i][0]>0){
         for(j=tordata.sidedata[i][1]+1;j<tordata.sidedata[i][1]+tordata.sidedata[i][0];j++){/*Do not need for Ca */
            if( j==(tordata.sidedata[i][1]+tordata.sidedata[i][0]-1)){  /* last unit - find inertia etc.*/
               for(k=0;k<=2;k++){ // center of mass vector
                 rus[j][k]=0.0;
                 for(ij=0;ij<=tordata.sidedata[i][3];ij++)
                    rus[j][k]=rus[j][k]+tordata.uside[j][ij][k];
                 rus[j][k] = rus[j][k]/(1.0+tordata.sidedata[i][3]);
               }
               for(k=0;k<=2;k++)
                  for(jj=0;jj<=2;jj++)
                    inerts[j][jj][k]=0;
               for(ii=0;ii<=tordata.sidedata[i][3];ii++) /* Find dr = vector from c. m. to atom ii */
                  for(k=0;k<=2;k++)
                     drs[j][ii][k]=tordata.uside[j][ii][k]-rus[j][k];
               for(ii=0;ii<=tordata.sidedata[i][3];ii++){/* sum over two atoms in unit - generate inert. ten.*/
                   ru1=dot(drs[j][ii],drs[j][ii]); /*= sqr or vector length;  Cannot use above defintion if more then 2 atoms */
                   for(k=0;k<=2;k++){
                      inerts[j][k][k]=inerts[j][k][k]+(ru1-sqr(drs[j][ii][k]));/* diagonal tems*/
                   }           
                   for(k=0;k<=2;k++){
                     for(jj=k+1;jj<=2;jj++){
                      inerts[j][k][jj]=inerts[j][k][jj]-drs[j][ii][k]*drs[j][ii][jj];
                    }
                 }
               } /* ii*/
               for(jj=0;jj<=2;jj++) /* tensor is symetric */
                 for(k=jj+1;k<=2;k++)
                    inerts[j][k][jj]=inerts[j][jj][k];     
               for(k=0;k<=2;k++){
                 forces[j][k]=0; /* force on c.m. */
                 tors[j][k]=0; /*total torque */
               }
               iran=2;
               for(ii=0;ii<=tordata.sidedata[i][3];ii++){/* sum over atoms in unit - generate force and torge vectores ten.*/        
                  if(tordata.usediamond!=1)
                     findforden(den1,geo,tordata.uside[j][ii],forv);
                  else
                     diamondfor(den1,geo,tordata.uside[j][ii],forv);
// printf(" at 4: force vs diamondfor: x: (%5.3f  %5.3f)  y: (%5.3f  %5.3f) z:(%5.3f  %5.3f)\n",
//    forv[0],dforv[0],forv[1],dforv[1],forv[2],dforv[2]); 
                  if(geo.quit==1){
                     if(verbose>=2)
                          printf("At 4 geo.quit = 1 in findforden\n");
                     goto quitrout;
                   }
                  for(k=0;k<=2;k++){
                     iran=iran+1;
                     if(iran>1){
                       rndnorm(rf);/* returns 2 rnad. num.  Normally dist.  mean-0, sig=1 */
                       iran=0;
                     }
                     for1[k]=tordata.torconst*forv[k]+tordata.rfor*rf[iran]-
                                 tordata.gclin*tordata.usidev[j][ii][k];
                  } /*k*/
                  crossprod(drs[j][ii],for1,wcf);
                  for(k=0;k<=2;k++){ /* sum force and torque over all atoms */
                    forces[j][k]=forces[j][k]+for1[k];
                    tors[j][k]=tors[j][k]+wcf[k];
                 }
             } /* ii */
//printf("Force on last unit =  %5.3f %5.3f %5.3f\n",forces[j][0],forces[j][1],forces[j][2]);
            } /* if j = last unit*/
            else{ /* j not last unit, just one atom */
               for(k=0;k<=2;k++)
                 rus[j][k]=tordata.uside[j][0][k];
               for(k=0;k<=2;k++)
                  for(jj=0;jj<=2;jj++)
                    inerts[j][jj][k]=0;
               iran=2;
               if(tordata.usediamond!=1)
                  findforden(den1,geo,tordata.uside[j][0],forv);
               else
                  diamondfor(den1,geo,tordata.uside[j][0],forv);
// printf(" at 5: force vs diamondfor: x: (%5.3f  %5.3f)  y: (%5.3f  %5.3f) z:(%5.3f  %5.3f)\n",
//    forv[0],dforv[0],forv[1],dforv[1],forv[2],dforv[2]); 
               if(geo.quit==1){
                    if(verbose>=2)
                        printf("At 5 geo.quit = 1 in findforden\n");
                    goto quitrout;
                }
               for(k=0;k<=2;k++){
                  iran=iran+1;
                  if(iran>1){
                    rndnorm(rf);/* returns 2 rnad. num.  Normally dist.  mean-0, sig=1 */
                    iran=0;
                  }
                  for1[k]=tordata.torconst*forv[k]+2*tordata.rfor*rf[iran]
                          -tordata.gclin*tordata.usidev[j][0][k];
               } /*k*/
               for(k=0;k<=2;k++){ 
                  forces[j][k]=for1[k];
                  tors[j][k]=0.0;
               }
           } /* if j != last unit*/
         } /* if */
      } /* j */

  ix = iside;
     if(tordata.sidedata[ix][0]>0){ 
        for(i=tordata.sidedata[ix][1];i<tordata.sidedata[ix][1]+tordata.sidedata[ix][0];i++){
            for(k=0;k<=2;k++)
               wu[k]=tordata.yys[i][k+3];
            if( i!=(tordata.sidedata[ix][1]+tordata.sidedata[ix][0]-1)){ /* Not at end, find connect type matrices*/
               if(i==tordata.sidedata[ix][1]){
                   jj=sidetocount[ix-1];
                   for(k=0;k<=5;k++)
                      tordata.yys[i][k]=tordata.yy[jj][k];
                   for(k=0;k<=2;k++)/* redefine */
                      wu[k]=tordata.yys[i][k+3];
                   for(k=0;k<=2;k++)
                      rijs[i][k]=rus[i+1][k]-ru[jj][k];
               }
               else{
                   for(k=0;k<=2;k++)
                       rijs[i][k]=rus[i+1][k]-rus[i][k];
               }
               for(k=0;k<=2;k++){ /* for i = Ca to end -1 */
                  drij[k]=tordata.yys[i+1][k]-tordata.yys[i][k];
                  hbars[i][k]=tordata.uside[i+1][0][k]-tordata.uside[i][0][k];
                  if(i==(tordata.sidedata[ix][1]+tordata.sidedata[ix][0]-2) ){  /*if i= end-1 then need sji becaue 2 atoms in last unit */       
                     sjis[i][k]=tordata.uside[i+1][0][k]-rus[i+1][k]; 
                  }
                  else{ 
                     sjis[i][k]=0.0;
                  }
               } /* k */

               mag=sqrt(dot(hbars[i],hbars[i]));
               for(k=0;k<=2;k++)
                  hbars[i][k]=hbars[i][k]/mag;
               for(jj=0;jj<6;jj++){
                  for(k=0;k<6;k++){
                     b1s[i][jj][k]=db1[jj][k]=0.0;
                  }
                  b1s[i][jj][jj]=1.0;
               }
               b1s[i][0][4]=rijs[i][2];
               b1s[i][0][5]= -rijs[i][1];
               b1s[i][1][3]= -rijs[i][2];
               b1s[i][1][5]= rijs[i][0];
               b1s[i][2][3]= rijs[i][1];
               b1s[i][2][4]= -rijs[i][0];
               for(jj=0;jj<=2;jj++){
                 wu2[jj]=tordata.yys[i+1][jj+3];
               }
               crossprod(hbars[i],sjis[i],hcs);
               crossprod(wu2,sjis[i],dsji);
               crossprod(wu,hbars[i],dhbar);
               crossprod(hbars[i],dsji,hcds);
               crossprod(dhbar,sjis[i],dhcs);
               for(k=0;k<=2;k++)
                 b2s[i][k]= -hcs[k];
               for(k=3;k<=5;k++)
                 b2s[i][k]= hbars[i][k-3];
              db1[0][4]=drij[2];
              db1[0][5]= -drij[1];
              db1[1][3]= -drij[2];
              db1[1][5]= drij[0];
              db1[2][3]= drij[1];
              db1[2][4]= -drij[0];
              for(k=0;k<=2;k++)
                 db2[k]= -hcds[k]-dhcs[k];
              for(k=3;k<=5;k++)
                 db2[k]= dhbar[k-3];
              mult66_61(db1,tordata.yys[i],vec5);
              for(k=0;k<=5;k++)
                 dds[i][k]=vec5[k]+tordata.dqs[i]*db2[k];
          } /* if j<ende.g. connet type matrices*/
          if(i>tordata.sidedata[ix][1]){
            for(jj=0;jj<6;jj++) /* for the non-connect matrices (M and Q, define also for i=tcount*/
              for(k=0;k<6;k++)
                mms[i][jj][k]=0.0;
            for(jj=0;jj<=2;jj++) /* generate M matrix*/
               mms[i][jj][jj]=1.0; /* = Mass of atom */
            for(jj=3;jj<=5;jj++)
              for(k=3;k<=5;k++)
                mms[i][jj][k]=inerts[i][jj-3][k-3];
            for(jj=0;jj<=2;jj++) /* generate Q vector*/
              qqs[i][jj]=forces[i][jj];
            multvec(inerts[i],wu,iw);
            crossprod(wu,iw,wciw);
            for(jj=3;jj<=5;jj++)
               qqs[i][jj]=tors[i][jj-3]-wciw[jj-3]; 
          }
       } /*i*/
     } /* if sidepots */

/*  Set up matrices for recurrance relation
   NOTE:  e.g. b1[m][i][j] is actually b1[m,m+1][][] in paper by Bae and Haug
       mm[m][][]  is mm[m][][]*/
  /* Start backward recurrance procedure for each side chain*/
   count=0;  /* counts unit1's  that have side chains*/
  ix=iside;
      if(tordata.sidedata[ix][0]>0){
         count=count+1;
         for(i=0;i<6;i++){ /*kk and ll are recurance matrices, = 0 for i=N */
           lls[count][i]=0;
           for(j=0;j<6;j++)
             kks[count][i][j]=0;
         }
         for(i=tordata.sidedata[ix][1]+tordata.sidedata[ix][0]-2;i>=tordata.sidedata[ix][1];i--){/* start recurion at end -1 */
         /* generate the inverse matrix which is just a 1x1 float*/
            add66_66(mms[i+1],kks[count],addmat);
            mult16_66(b2s[i],addmat,vec1);/* this is actually b2transpose = 1row by 56 columns*/
            mult16_61(vec1,b2s[i],&invmats[i]); /* invmat is just  scaler */
            if(fabs(invmats[i])>0.00001)
               invmats[i]=1/invmats[i];
            else
               invmats[i]=0.0;  /* if invmat=0, then it is never used */
            tran66(b1s[i],tranb1); /* transpose of b1 */
            mult66_66(tranb1,addmat,mat1);
            mult66_66(mat1,b1s[i],mat2);/* mat2 = first term for kk */
            mult66_61(mat1,b2s[i],vec2);
            mult16_66(vec1,b1s[i],vec3s[i]);
            mult11_61(-invmats[i],vec3s[i],vec4);
            mult61_16(vec2,vec4,mat3); /* mat3 is second term for kk*/
            add66_66(mat2,mat3,kks[count]); /* returns next k matrix */
            mult66_61(addmat,dds[i],vec5);
            for(k=0;k<=5;k++)
               vec6s[k]=qqs[i+1][k]+lls[count][k]-vec5[k];  /* vec6 = first term in brackets in ll expresion*/
            mult66_61(addmat,b2s[i],vec7);
            mult16_61(b2s[i],vec6s,&temps[i]);/* temp is a scaler*/
            for(k=0;k<=5;k++) /* vec8 = term in brackes in ll expression*/
               vec8[k]=vec6s[k]-temps[i]*invmats[i]*vec7[k]; 
            mult66_61(tranb1,vec8,lls[count]); /* returns next ll vector */
       } /* i */
     } /* if */
 
   ii =iside;
         if(tordata.sidedata[ii][0]>0){/* has side chain */

#if 1 /*if = 1  debug  keep dyys=0 */
            for(k=0;k<6;k++){ /* copy dyy for Ca to dyys[1]*/
               dyys[ tordata.sidedata[ii][1] ][k]=0.0;
            }
#endif
#if 1 /*debug  DO NOT CHANGE dyys and ddqs */
            mult16_61(vec3s[ tordata.sidedata[ii][1] ],dyys[ tordata.sidedata[ii][1] ],&xx);
            ddqs[ tordata.sidedata[ii][1] ]= -invmats[ tordata.sidedata[ii][1] ]*(xx-temps[ tordata.sidedata[ii][1] ]);            
            tordata.dqs[ tordata.sidedata[ii][1] ]=tordata.dqs[ tordata.sidedata[ii][1] ]+
                       ddqs[ tordata.sidedata[ii][1] ]*tordata.delt;
            tordata.chi[ tordata.sidedata[ii][1] ]=tordata.chi[ tordata.sidedata[ii][1] ]+
                         tordata.dqs[ tordata.sidedata[ii][1] ]*tordata.delt;
#endif
         } // if has side chian
 
  i = iside; 
     if(tordata.sidedata[i][0]>0){
        for(j=tordata.sidedata[i][1]+1;j<tordata.sidedata[i][1]+tordata.sidedata[i][0];j++){/* start at Cb*/
            mult66_61(b1s[j-1],dyys[j-1],vec1);
            for(k=0;k<=5;k++){
               dyys[j][k]=vec1[k]   +ddqs[j-1]*b2s[j-1][k]  +dds[j-1][k];
            }
            for(k=0;k<6;k++)
                tordata.yys[j][k]=tordata.yys[j][k]+dyys[j][k]*tordata.delt;
            if(j<(tordata.sidedata[i][1]+tordata.sidedata[i][0]-1)){ /* for last atom, dq not defined*/
               mult16_61(vec3s[j],dyys[j],&xx);
               ddqs[j]= -invmats[j]*(xx-temps[j]);
               tordata.dqs[j]=tordata.dqs[j]+ddqs[j]*tordata.delt;
               tordata.chi[j]=tordata.chi[j]+tordata.dqs[j]*tordata.delt;
            }
        } /* j */
      } /* if */
   int count2;
   count2 = 0;
   onesidestep(iside,count2,geo,tordata);
quitrout:;
}
