//
//      Align two structures.
//

PROC fit1(natoms%,REF set1.(),REF set2.(),REF selection%()) CLOSED
  // Move all set2 coordinates so that the atoms selected by selection%
  // fits the corresponding atoms in set1
  
  FUNC rmsd(natoms%,REF set1.(),REF set2.(),REF selection%()) CLOSED
    rms:=0
    denom:=0
    sumx1:=0
    sumy1:=0
    sumz1:=0
    sumx2:=0
    sumy2:=0
    sumz2:=0
    
    FOR i%:=1 TO natoms% DO
      IF selection%(i%) THEN
        denom:=denom+1 //or use mass
        rms:=rms+(set1.(i%).atc_x#-set2.(i%).atc_x#)^2
        rms:=rms+(set1.(i%).atc_y#-set2.(i%).atc_y#)^2
        rms:=rms+(set1.(i%).atc_z#-set2.(i%).atc_z#)^2
        sumx1:=sumx1+set1.(i%).atc_x#
        sumy1:=sumy1+set1.(i%).atc_y#
        sumz1:=sumz1+set1.(i%).atc_z#
        sumx2:=sumx2+set2.(i%).atc_x#
        sumy2:=sumy2+set2.(i%).atc_y#
        sumz2:=sumz2+set2.(i%).atc_z#
      ENDIF
    NEXT i%
    RETURN SQR(rms/denom)
  ENDFUNC rmsd
  
  PROC jacobi(REF a(,),n%,REF d(),REF v(,),REF nrot%) CLOSED
    // Computes all eigenvalues and eigenvectors of a real symmetric
    // matrix a(,). On output, elements of a above the diagonal are
    // destroyed. d() returns the eigenvalues of a. v(,) is a
    // matrix whose columns contain, on output, the
    // normalized eigenvectors of a. nret% returns the number of
    // Jacobi rotations that were required.
    DIM b(n%),z(n%)
    FOR ip%:=1 TO n% DO
      FOR iq%:=1 TO n% DO
        v(ip%,iq%):=0
      NEXT iq%
      v(ip%,ip%):=1
    NEXT ip%
    FOR ip%:=1 TO n% DO
      b(ip%):=a(ip%,ip%)
      d(ip%):=b(ip%)
      z(ip%):=0
    NEXT ip%
    nrot%:=0
    FOR i%:=1 TO 50 DO
      sm:=0
      FOR ip%:=1 TO n%-1 DO
        FOR iq%:=ip%+1 TO n% DO
          sm:=sm+ABS(a(ip%,iq%))
        NEXT iq%
      NEXT ip%
      IF sm=0 THEN
        // Sort the eigenvalues/vectors:
        FOR ip%:=1 TO n%-1 DO
          FOR iq%:=ip%+1 TO n% DO
            IF d(ip%)>d(iq%) THEN
              tmp:=d(ip%)
              d(ip%):=d(iq%)
              d(iq%):=tmp
              FOR ir%:=1 TO n% DO
                tmp:=v(ir%,ip%)
                v(ir%,ip%):=v(ir%,iq%)
                v(ir%,iq%):=tmp
              NEXT ir%
            ENDIF
          NEXT iq%
        NEXT ip%
        RETURN
      ENDIF
      IF i%<4 THEN
        tresh:=0.2*sm/(n%*n%)
      ELSE
        tresh:=0
      ENDIF
      FOR ip%:=1 TO n%-1 DO
        FOR iq%:=ip%+1 TO n% DO
          g:=100*ABS(a(ip%,iq%))
          IF i%>4 AND (ABS(d(ip%))+g)=ABS(d(ip%)) AND (ABS(d(iq%)+g))=ABS(d(iq%)) THEN
            a(ip%,iq%):=0
          ELIF ABS(a(ip%,iq%))>tresh THEN
            h:=d(iq%)-d(ip%)
            IF (ABS(h)+g)=ABS(h) THEN
              t:=a(ip%,iq%)/h
            ELSE
              theta:=0.5*h/a(ip%,iq%)
              t:=1/(ABS(theta)+SQR(1+theta*theta))
              IF theta<0 THEN
                t:=-t
              ENDIF
            ENDIF
            c:=1/SQR(1+t*t)
            s:=t*c
            tau:=s/(1+c)
            h:=t*a(ip%,iq%)
            z(ip%):=z(ip%)-h
            z(iq%):=z(iq%)+h
            d(ip%):=d(ip%)-h
            d(iq%):=d(iq%)+h
            a(ip%,iq%):=0
            FOR j%:=1 TO ip%-1 DO
              // rotate(a,j,ip,j,iq)
              g:=a(j%,ip%)
              h:=a(j%,iq%)
              a(j%,ip%):=g-s*(h+g*tau)
              a(j%,iq%):=h+s*(g-h*tau)
            NEXT j%
            FOR j%:=ip%+1 TO iq%-1 DO
              // rotate(a,ip,j,j,iq)
              g:=a(ip%,j%)
              h:=a(j%,iq%)
              a(ip%,j%):=g-s*(h+g*tau)
              a(j%,iq%):=h+s*(g-h*tau)
            NEXT j%
            FOR j%:=iq%+1 TO n% DO
              // rotate(a,ip,j,iq,j)
              g:=a(ip%,j%)
              h:=a(iq%,j%)
              a(ip%,j%):=g-s*(h+g*tau)
              a(iq%,j%):=h+s*(g-h*tau)
            NEXT j%
            FOR j%:=1 TO n% DO
              // rotate(v,j,ip,j,iq)
              g:=v(j%,ip%)
              h:=v(j%,iq%)
              v(j%,ip%):=g-s*(h+g*tau)
              v(j%,iq%):=h+s*(g-h*tau)
            NEXT j%
            nrot%:=nrot%+1
          ENDIF
        NEXT iq%
      NEXT ip%
      FOR ip%:=1 TO n% DO
        b(ip%):=b(ip%)+z(ip%)
        d(ip%):=b(ip%)
        z(ip%):=0
      NEXT ip%
    NEXT i%
    PRINT "Too many iterations in JACOBI"
  ENDPROC jacobi
  
  PROC normal(REF v(),n%) CLOSED
    sum:=0
    FOR i%:=1 TO n% DO
      sum:=sum+v(i%)^2
    NEXT i%
    IF sum<1e-12 THEN
      FOR i%:=1 TO n% DO
        v(i%):=0
      NEXT i%
    ELSE
      sum:=1/sum
      FOR i%:=1 TO n% DO
        v(i%):=v(i%)*sum
      NEXT i%
    ENDIF
  ENDPROC normal
  
  PROC frotu(REF r(,),REF u(,)) CLOSED
    DIM w(3,3),a(3,3),b(3,3),scr(3)
    
    det:=0
    FOR i%:=1 TO 3 DO
      i1%:=i%+1
      IF i1%>3 THEN
        i1%:=i1%-3
      ENDIF
      i2%:=i%+2
      IF i2%>3 THEN
        i2%:=i2%-3
      ENDIF
      det:=det+r(i%,1)*(r(i1%,2)*r(i2%,3)-r(i2%,2)*r(i1%,3))
    NEXT i%
    
    ipt%:=0
    FOR i%:=1 TO 3 DO
      FOR j%:=1 TO 3 DO
        w(i%,j%):=0
        FOR k%:=1 TO 3 DO
          w(i%,j%):=w(i%,j%)+r(j%,k%)*r(i%,k%)
        NEXT k%
      NEXT j%
    NEXT i%
    
    trace:=w(1,1)+w(2,2)+w(3,3)
    IF trace<3e-06 THEN
      FOR i%:=1 TO 3 DO
        FOR j%:=1 TO 3 DO
          u(i%,j%):=0
        NEXT j%
        u(i%,i%):=1
      NEXT i%
    ENDIF
    
    jacobi(w(,),3,scr(),a(,),nrot%)
    
    FOR i%:=1 TO 3 DO
      scr(i%):=SQR(ABS(scr(i%)))
      IF scr(i%)<1e-06 THEN
        scr(i%):=1e-06
      ENDIF
    NEXT i%
    
    IF det<0 THEN
      scr(1):=-scr(1)
    ENDIF
    
    FOR j%:=1 TO 3 DO
      evs:=scr(j%)
      FOR i%:=1 TO 3 DO
        b(i%,j%):=0
        FOR k%:=1 TO 3 DO
          b(i%,j%):=b(i%,j%)+r(k%,i%)*a(k%,j%)/evs
        NEXT k%
      NEXT i%
    NEXT j%
    
    det:=0
    FOR i%:=1 TO 3 DO
      i1%:=i%+1
      IF i1%>3 THEN i1%:=i1%-3
      i2%:=i%+2
      IF i2%>3 THEN i2%:=i2%-3
      det:=det+a(i%,1)*(a(i1%,2)*a(i2%,3)-a(i2%,2)*a(i1%,3))
    NEXT i%
    
    FOR j%:=1 TO 3 DO
      IF ABS(scr(j%))<1e-06 THEN
        jp%:=j%+1
        jq%:=j%+2
        IF jp%>3 THEN jp%:=jp%-3
        IF jq%>3 THEN jq%:=jq%-3
        FOR k%:=1 TO 3 DO
          kp%:=k%+1
          kq%:=k%+2
          IF kp%>3 THEN kp%:=kp%-3
          IF kq%>3 THEN kq%:=kq%-3
          b(k%,j%):=b(kp%,jp%)*b(kq%,jq%)-b(kp%,jq%)*b(kq%,jp%)
          IF det<0 THEN b(k%,j%):=-b(k%,j%)
        NEXT k%
      ENDIF
      det:=b(1,j%)^2+b(2,j%)^2+b(3,j%)^2
      IF det<1e-06 THEN
        det:=0
      ELSE
        det:=1/det
      ENDIF
      b(1,j%):=b(1,j%)*det
      b(2,j%):=b(2,j%)*det
      b(3,j%):=b(3,j%)*det
    NEXT j%
    
    FOR j%:=1 TO 3 DO
      FOR i%:=1 TO 3 DO
        u(i%,j%):=0
        FOR k%:=1 TO 3 DO
          u(i%,j%):=u(i%,j%)+a(i%,k%)*b(j%,k%)
        NEXT k%
      NEXT i%
    NEXT j%
    
    FOR j%:=1 TO 3 DO
      det:=u(1,j%)^2+u(2,j%)^2+u(3,j%)^2
      IF det<1e-06 THEN
        det:=0
      ELSE
        det:=1/det
      ENDIF
      u(1,j%):=u(1,j%)*det
      u(2,j%):=u(2,j%)*det
      u(3,j%):=u(3,j%)*det
    NEXT j%
    
    det:=0
    FOR i%:=1 TO 3 DO
      i1%:=i%+1
      IF i1%>3 THEN i1%:=i1%-3
      i2%:=i%+2
      IF i2%>3 THEN i2%:=i2%-3
      det:=det+u(i%,1)*(u(i1%,2)*u(i2%,3)-u(i2%,2)*u(i1%,3))
    NEXT i%
    
    IF ABS(det-1)>0.0001 THEN
      PRINT "Rotation matrix is not unitary";det
    ENDIF
  ENDPROC frotu
  
  // Local dim's:
  
  DIM mass(natoms%),aname$ OF 4
  DIM r(3,3),u(3,3),eu(3),rot(3,3),axis(3),q(4)
  
  // Fill the mass array:
  
  FOR i%:=1 TO natoms% DO
    aname$:=set1.(i%).atc_aname!
    aname$:=aname$(1:1)
    IF aname$="H" THEN
      mass(i%):=1.008
    ELIF aname$="C" THEN
      mass(i%):=12.011
    ELIF aname$="N" THEN
      mass(i%):=14.0067
    ELIF aname$="O" THEN
      mass(i%):=15.9994
    ELIF aname$="S" THEN
      mass(i%):=32.06
    ELIF aname$="P" THEN
      mass(i%):=30.974
    ELSE
      PRINT "Unknown atom type: ",set1.(i%).atc_aname!
      mass(i%):=0.01
    ENDIF
    //PRINT set1.(i%).atc_aname!," Mass: ",mass(i%)
    mass(i%):=1
  NEXT i%
  
  cmx1:=0
  cmy1:=0
  cmz1:=0
  cmx2:=0
  cmy2:=0
  cmz2:=0
  tmass:=0
  FOR k%:=1 TO natoms% DO
    IF selection%(k%) THEN
      cmx1:=cmx1+set1.(k%).atc_x#*mass(k%)
      cmy1:=cmy1+set1.(k%).atc_y#*mass(k%)
      cmz1:=cmz1+set1.(k%).atc_z#*mass(k%)
      cmx2:=cmx2+set2.(k%).atc_x#*mass(k%)
      cmy2:=cmy2+set2.(k%).atc_y#*mass(k%)
      cmz2:=cmz2+set2.(k%).atc_z#*mass(k%)
      tmass:=tmass+mass(k%)
    ENDIF
  NEXT k%
  cmx1:=cmx1/tmass
  cmy1:=cmy1/tmass
  cmz1:=cmz1/tmass
  cmx2:=cmx2/tmass
  cmy2:=cmy2/tmass
  cmz2:=cmz2/tmass
  
  PRINT "RMS before: ",rmsd(natoms%,set1.(),set2.(),selection%())
  
  FOR k%:=1 TO natoms% DO
    set2.(k%).atc_x#:=set2.(k%).atc_x#-cmx2
    set2.(k%).atc_y#:=set2.(k%).atc_y#-cmy2
    set2.(k%).atc_z#:=set2.(k%).atc_z#-cmz2
  NEXT k%
  
  FOR i%:=1 TO 3 DO
    FOR j%:=1 TO 3 DO
      r(i%,j%):=0
    NEXT j%
  NEXT i%
  FOR k%:=1 TO natoms% DO
    IF selection%(k%) THEN
      x2:=set2.(k%).atc_x#*mass(k%)
      y2:=set2.(k%).atc_y#*mass(k%)
      z2:=set2.(k%).atc_z#*mass(k%)
      x1:=set1.(k%).atc_x#-cmx1
      y1:=set1.(k%).atc_y#-cmy1
      z1:=set1.(k%).atc_z#-cmz1
      r(1,1):=r(1,1)+x1*x2
      r(2,1):=r(2,1)+y1*x2
      r(3,1):=r(3,1)+z1*x2
      r(1,2):=r(1,2)+x1*y2
      r(2,2):=r(2,2)+y1*y2
      r(3,2):=r(3,2)+z1*y2
      r(1,3):=r(1,3)+x1*z2
      r(2,3):=r(2,3)+y1*z2
      r(3,3):=r(3,3)+z1*z2
    ENDIF
  NEXT k%
  
  frotu(r(,),u(,))
  
  FOR k%:=1 TO natoms% DO
    cmx3:=u(1,1)*set2.(k%).atc_x#+u(1,2)*set2.(k%).atc_y#+u(1,3)*set2.(k%).atc_z#+cmx1
    cmy3:=u(2,1)*set2.(k%).atc_x#+u(2,2)*set2.(k%).atc_y#+u(2,3)*set2.(k%).atc_z#+cmy1
    cmz3:=u(3,1)*set2.(k%).atc_x#+u(3,2)*set2.(k%).atc_y#+u(3,3)*set2.(k%).atc_z#+cmz1
    set2.(k%).atc_x#:=cmx3
    set2.(k%).atc_y#:=cmy3
    set2.(k%).atc_z#:=cmz3
  NEXT k%
  
  PRINT "RMS after: ",rmsd(natoms%,set1.(),set2.(),selection%())
  
ENDPROC fit1

stc_id%:=0

DIM stc. OF stc_rec

CAT_STC_GET stc_id%,stc.

natoms%:=stc.stc_natoms%
//natoms%:=20

DIM set1.(natoms%) OF atc_rec
DIM set2.(natoms%) OF atc_rec
DIM selection%(natoms%)

PRINT "No. of atoms: ",natoms%

// read 1st structure

st_id%:=CAT_STC_ST_FIRST(stc_id%)

atc_id%:=CAT_ST_ATC_FIRST(st_id%)
FOR i%:=1 TO natoms% DO
  selection%(i%):=TRUE
  CAT_ATC_GET atc_id%,set1.(i%)
  atc_id%:=CAT_ST_ATC_NEXT#(atc_id%,st_id%)
NEXT i%

st_id%:=CAT_STC_ST_NEXT#(st_id%,stc_id%)
WHILE st_id%<>-1 DO
  atc_id%:=CAT_ST_ATC_FIRST(st_id%)
  FOR i%:=1 TO natoms% DO
    CAT_ATC_GET atc_id%,set2.(i%)
    atc_id%:=CAT_ST_ATC_NEXT#(atc_id%,st_id%)
  NEXT i%
  PRINT "st_id%: ",st_id%
  fit1(natoms%,set1.(),set2.(),selection%())
  atc_id%:=CAT_ST_ATC_FIRST(st_id%)
  FOR i%:=1 TO natoms% DO
    CAT_ATC_UPDATE set2.(i%).atc_id%,set2.(i%)
  NEXT i%
  st_id%:=CAT_STC_ST_NEXT#(st_id%,stc_id%)
ENDWHILE