C
C NAZ10  WAFF5-1 Finite Element Method
C @Zitny 98
C Algorithms test - Waffle expansion 
C
      parameter (num=50,nem=50)
      common /grid/x(num),y(num),nc(3,nem),beta(nem)
      common am(10000),fm(100)
C Read grid data
      open(1,file='naz10.dat')
      read(1,*)
      read(1,*)nu,ne
      read(1,*)(x(i),y(i),i=1,nu)
      read(1,*)((nc(j,i),j=1,3),beta(i),i=1,ne)
      neq=2*nu+ne
C Assembly of stiffness matrix and R.H.S. vector
      call gassem(neq,ne,nu,nc,x,y,beta,am,fm)
C Solution of linear equations by Gauss elimination (routine IBM)
C      call gelg(fm,am,neq,1,1e-6,ier)
      call gaussj(am,neq,neq,fm,1,1)
C Output Results
      open(2,file='naz10.out')
      write(2,1)nu,(x(i),y(i),i=1,nu)
      write(2,2)ne,((nc(j,i),j=1,3),beta(i),i=1,ne)
      write(2,3)ier,(i,fm(2*i-1),fm(2*i),i=1,nu)
1     format(i3/(2e10.3))
2     format(i3/(3i5,e10.3))
3     format(' IERROR=',i3/(i3,2e10.3))
      end

      subroutine gassem(neq,ne,nu,nc,x,y,beta,am,fm)
      dimension x(nu),y(nu),beta(ne),nc(3,ne),am(neq,neq),fm(neq),
     /          al(7,7),fl(7),xl(3),yl(3),ieq(7)
C AM - stiffness matrix of system
C FM - R.H.S. vector of system      
      do i=1,neq
      do j=1,neq
        am(i,j)=0
      enddo
        fm(i)=0
      enddo
C Element loop (NC(1..3, 1..NE)-connectivity matrix)
      do ie=1,ne
         do i=1,3
            iu=nc(i,ie)
            xl(i)=x(iu)
            yl(i)=y(iu)
            ieq(2*i-1)=2*iu-1
            ieq(2*i)=2*iu
         enddo
         ieq(7)=2*nu+ie
         call kbeta(xl,yl,beta(ie),al,fl)
C ASSEMBLY
         do i=1,7
C Add local R.H.S. vector FL(1..7) to the global vector FM(1..NEQ)
            fm(ieq(i))=fm(ieq(i))+fl(i)
            do j=1,7
C Add local matrix (7 x 7) to the global matrix (NEQ x NEQ)
               am(ieq(i),ieq(j))=am(ieq(i),ieq(j))+al(i,j)
            enddo
         enddo
      enddo
      end
      
      subroutine kbeta(x,y,beta,al,fl)
      dimension x(3),y(3),al(7,7),fl(7)
C Local STIFFNESS MATRIX AL(7 x 7)
      det=x(1)*(y(2)-y(3))+x(2)*(y(3)-y(1))+x(3)*(y(1)-y(2))
      al=0
      fl=0
      do i=1,3
         i1=2*i-1
C New trick: IP1=i+1, IP2=i+2 in a cyclic list 1,2,3 
         ip1=mod(i,3)+1
         ip2=mod(ip1,3)+1
C Last column and row are formed by X,Y (indegrals of derivatives)
         al(i1,7)=(y(ip1)-y(ip2))/2
         al(i1+1,7)=(x(ip2)-x(ip1))/2
         al(7,i1)=al(i1,7)
         al(7,i1+1)=al(i1+1,7)
C Submatrix 6 x 6
         do j=1,3
            j1=2*j-1
            if(i.eq.j)then
                 v=det/6
            else
                 v=det/12
            endif
            al(i1,j1)=v
            al(i1+1,j1+1)=v
         enddo
      enddo
      fl(7)=beta*det/2.
      end
      
      subroutine gaussj(a,n,np,b,m,mp)
C
C Numerical Recipes p. 28
C
      parameter (nmax=100)
      dimension a(np,np),b(np,mp),ipiv(nmax),indxr(nmax),indxc(nmax)
      do i=1,n
         ipiv(i)=0
      enddo
      do i=1,n
         big=0
         do j=1,n
             if(ipiv(j).ne.1)then
                   do k=1,n
                         if(ipiv(k).eq.0)then
                               if(abs(a(j,k)).ge.big)then
                                    big=abs(a(j,k))
                                    irow=j
				    icol=k
                               endif
                         elseif(ipiv(k).gt.1)then
                               pause 'Singular matrix'
                         endif
                    enddo
               endif
          enddo
          ipiv(icol)=ipiv(icol)+1
          if(irow.ne.icol)then
              do l=1,n
                 dum=a(irow,l)
                 a(irow,l)=a(icol,l)
                 a(icol,l)=dum
              enddo
              do l=1,m
                 dum=b(irow,l)
                 b(irow,l)=b(icol,l)
                 b(icol,l)=dum
              enddo
          endif
          indxr(i)=irow
          indxc(i)=icol
          if(a(icol,icol).eq.0.) pause 'Singular'
          pivinv=1/a(icol,icol)
          a(icol,icol)=1
          do l=1,n
             a(icol,l)=a(icol,l)*pivinv
          enddo
          do l=1,m
             b(icol,l)=b(icol,l)*pivinv
          enddo
          do ll=1,n
              if(ll.ne.icol)then
                  dum=a(ll,icol)   
                  a(ll,icol)=0
                  do l=1,n
                     a(ll,l)=a(ll,l)-a(icol,l)*dum
                  enddo
                  do l=1,m
                     b(ll,l)=b(ll,l)-b(icol,l)*dum
                  enddo
               endif
           enddo
      enddo
      do l=n,1,-1
         if(indxr(l).ne.indxc(l))then
            do k=1,n
               dum=a(k,indxr(l))
               a(k,indxr(l))=a(k,indxc(l))
               a(k,indxc(l))=dum
            enddo
         endif
      enddo
      end
