program TRUSS

integer :: NNOD,NELM,IC
integer :: NDF(100,4),ELM(200,2)
real :: XY(100,2),MAT(200,7)
real :: BMAT(200,4),KMAT(200,4,4),GSMAT(200,200),FVEC(200)
real :: ADISP(100,2),AFORCE(100,2),NDISP(100,2),NFORCE(100,2)
real :: A(200,200),B(200),X(200)
integer :: i,j

call INPUT(NNOD,NELM,XY,ELM,MAT,NDF,ADISP,AFORCE)

call MKBMAT(NELM,BMAT,ELM,XY,MAT)
call MKEMAT(NELM,BMAT,MAT,KMAT)
call MKGSMAT(NNOD,NELM,ELM,KMAT,GSMAT)

call MKFVEC(NNOD,NDF,AFORCE,FVEC)

do i=1,2*NNOD
  do j=1,2*NNOD
    A(i,j)=GSMAT(i,j)
  end do
  B(i)=FVEC(i)
end do

call BOUND(NNOD,NDF,ADISP,A,B)

call SOLVE(NNOD,A,B,X,IC)

if (IC==0) then
  call RESULTS(NNOD,NELM,ELM,MAT,GSMAT,X,BMAT,NDISP,NFORCE)
end if

call OUTPUT(NNOD,NELM,XY,ELM,MAT,NDF,ADISP,AFORCE,NDISP,NFORCE,IC)

call OUTPUT2(NNOD,NELM,KMAT,BMAT,GSMAT,FVEC)

end program TRUSS

!-------------------------------------------------------------sub. INPUT
subroutine INPUT(NNOD,NELM,XY,ELM,MAT,NDF,ADISP,AFORCE)

integer :: NNOD,NELM
real :: XY(100,2),MAT(200,7)
real :: ADISP(100,2),AFORCE(100,2)
integer :: NDF(100,4),ELM(200,2)
integer :: n,m,k

read(*,*) NNOD,NELM
do n=1,NNOD
  read(*,*) (XY(n,k),k=1,2),(NDF(n,k),k=1,4),(ADISP(n,k),k=1,2),(AFORCE(n,k),k=1,2)
end do
do m=1,NELM
  read(*,*) (ELM(m,k),k=1,2),(MAT(m,k),k=1,2)
end do

return
end subroutine INPUT

!-------------------------------------------------------------sub. MKBMAT
subroutine MKBMAT(NELM,BMAT,ELM,XY,MAT)

integer :: NELM
real :: XY(100,2),MAT(200,7),BMAT(200,4)
integer :: ELM(200,2)
integer :: m,n1,n2
real :: x(2),y(2)

do m=1,NELM

  n1=ELM(m,1)
  n2=ELM(m,2)

  x(1)=XY(N1,1)
  y(1)=XY(N1,2)
  x(2)=XY(N2,1)
  y(2)=XY(N2,2)

  MAT(m,3)=sqrt((x(1)-x(2))**2 + (y(1)-y(2))**2)
  
  if (abs(x(2)-x(1))>0.0) then
     MAT(m,4)=atan((y(2)-y(1))/(x(2)-x(1)))
  elseif ((y(2)-y(1))>0.0) then
     MAT(m,4)=3.141592654/2.0
  else
     MAT(m,4)=-3.141592654/2.0
  end if

  BMAT(m,1)=-cos(MAT(m,4))/MAT(m,3)
  BMAT(m,2)=-sin(MAT(m,4))/MAT(m,3)
  BMAT(m,3)=cos(MAT(m,4))/MAT(m,3)
  BMAT(m,4)=sin(MAT(m,4))/MAT(m,3)

end do

return
end subroutine MKBMAT

!-------------------------------------------------------------sub. MKEMAT
subroutine MKEMAT(NELM,BMAT,MAT,KMAT)

integer :: NELM
real :: MAT(200,7),BMAT(200,4),KMAT(200,4,4)
integer :: m,i,j

do m=1,NELM

  do i=1,4
    do j=1,4
      KMAT(m,i,j)=(MAT(m,1)*MAT(m,2)*MAT(m,3))*BMAT(m,i)*BMAT(m,j)
    end do
  end do

end do

return
end subroutine MKEMAT

!-------------------------------------------------------------sub. MKGSMAT
subroutine MKGSMAT(NNOD,NELM,ELM,KMAT,GSMAT)

integer :: NNOD,NELM
real :: MAT(200,7),BMAT(200,4),KMAT(200,4,4),GSMAT(200,200)
integer :: NDF(100,4),ELM(200,2)
integer :: nd(4),m,i,j,n1,n2

do i=1,2*NNOD
  do j=1,2*NNOD
    GSMAT(i,j)=0.0
  end do
end do

do m=1,NELM

  n1=ELM(m,1)
  n2=ELM(m,2)

  nd(1)=2*n1-1
  nd(2)=2*n1
  nd(3)=2*n2-1
  nd(4)=2*n2

  do i=1,4
    do j=1,4
      GSMAT(nd(i),nd(j))=GSMAT(nd(i),nd(j))+KMAT(m,i,j)
    end do
  end do

end do

return
end subroutine MKGSMAT

!-------------------------------------------------------------sub. MKFVEC
subroutine MKFVEC(NNOD,NDF,AFORCE,FVEC)

integer :: NNOD,NDF(100,4)
real :: AFORCE(100,2),FVEC(200)
integer :: n,k,i

do i=1,2*NNOD
  FVEC(i)=0.0
end do

do n=1,NNOD
  do k=1,2
    if (NDF(n,k+2)==1) then
      i=2*n+(k-2)
      FVEC(i)=FVEC(i)+AFORCE(n,k)
    end if
  end do
end do

return
end subroutine MKFVEC

!-------------------------------------------------------------sub. BOUDN
subroutine BOUND(NNOD,NDF,ADISP,A,B)

integer :: NNOD,NDF(100,4)
real :: ADISP(100,2),A(200,200),B(200)
integer :: n,k,i,j

do n=1,NNOD
  do k=1,2
    if (NDF(n,k)==1) then
      i=2*n+(k-2)
      do j=1,2*NNOD
        B(i)=B(i)-A(j,i)*ADISP(n,k)
      end do
      do j=1,2*NNOD
        A(j,i)=0.0
        A(i,j)=0.0
      end do
      A(i,i)=1.0
      B(i)=ADISP(n,k)
    end if
  end do
end do

return
end subroutine BOUND

!-------------------------------------------------------------sub. RESULTS
subroutine RESULTS(NNOD,NELM,ELM,MAT,GSMAT,X,BMAT,NDISP,NFORCE)

integer :: NNOD,NELM,ELM(200,2)
real :: XY(100,2),MAT(200,7),BMAT(200,4),GSMAT(200,200),X(200)
real :: NDISP(100,2),NFORCE(100,2)
integer :: n,k,i,j,n1,n2
real :: tmp,d(4)

do n=1,NNOD
  do k=1,2

    i=2*n+(k-2)

    NDISP(n,k)=X(i)

    tmp=0.0
    do j=1,2*NNOD
      tmp=tmp+GSMAT(i,j)*X(j)
    end do
    NFORCE(n,k)=tmp

  end do
end do

do m=1,NELM

 n1=ELM(m,1)
 n2=ELM(m,2)

 d(1)=NDISP(n1,1)
 d(2)=NDISP(n1,2)
 d(3)=NDISP(n2,1)
 d(4)=NDISP(n2,2)
 
 tmp=0.0
 do i=1,4
   tmp=tmp+BMAT(m,i)*d(i)
 end do
 MAT(m,5)=tmp
 MAT(m,6)=MAT(m,1)*MAT(m,5)
 MAT(m,7)=MAT(m,2)*MAT(m,6)

end do

return
end subroutine RESULTS

!-------------------------------------------------------------sub. OUTPUT
subroutine OUTPUT(NNOD,NELM,XY,ELM,MAT,NDF,ADISP,AFORCE,NDISP,NFORCE,IC)

integer :: NNOD,NELM,IC
integer :: NDF(100,4),ELM(200,2)
real :: XY(100,2),MAT(200,7),ADISP(100,2),AFORCE(100,2),NDISP(100,2),NFORCE(100,2)
integer :: n,m,k

if (IC==1) then
  write(*,*) "Calculation Error!!!!!!!!!"
end if

write(*,*) 'Number of Nordal Points : ',NNOD
write(*,*) 'Number of Elements : ',NELM
write(*,*) 
write(*,1000)
do n=1,NNOD
  write(*,1001) n,(XY(n,k),k=1,2),(NDF(n,k),k=1,4),(ADISP(n,k),k=1,2),(AFORCE(n,k),k=1,2),(NDISP(n,k),k=1,2),(NFORCE(n,k),k=1,2)
end do
write(*,*)
write(*,2000)
do m=1,NELM
  write(*,2001) m,(ELM(m,k),k=1,2),(MAT(m,k),k=1,7)
end do

1000 format(3X,'n',6X,'X',9X,'Y      NDF',6X,'ADISPx',7X,'ADISPy',7X,'AFORCEx',6X,'AFORCEy',6X,'DISPx',8X,'DISPy',10X,'Fx',11X,'Fy')
1001 format(I4,2F10.5,4I2,8E13.5)
2000 format(3X,'m  n1  n2',6X,'E',12X,'A',12X,'L',11X,'TH',11X,'EPS',10X,'SIG',11X,'P')
2001 format(3I4,7E13.5)

return

end subroutine OUTPUT
!-------------------------------------------------------------sub. OUTPUT2
subroutine OUTPUT2(NNOD,NELM,KMAT,BMAT,GSMAT,FVEC)

integer :: NNOD,NELM
real :: KMAT(200,4,4),BMAT(200,4),GSMAT(200,200),FVEC(200)
integer :: n,m,k

write(*,*) '[B]-Matrix//////////////////////'
do m=1,NELM
  write(*,1000) m
  write(*,1001) (BMAT(m,k),K=1,4)
end do

write(*,*)
write(*,*) '[K]-Matrix//////////////////////'
do m=1,NELM
    write(*,2000) m
  do i=1,4
    write(*,2001) (KMAT(m,i,k),k=1,4)
  end do
end do

write(*,*)
write(*,*) '[GS]-Matrix//////////////////////'
do i=1,2*NNOD
    write(*,*) (GSMAT(i,j),j=1,2*NNOD),' | ',FVEC(i)
end do

1000 format('Element No.:',I4)
1001 format(4E13.5)
2000 format('Element No.:',I4)
2001 format(4E13.5)

return
end subroutine OUTPUT2
!-------------------------------------------------------------sub. SOLVE
subroutine SOLVE(NNOD,A,B,X,IC)

integer :: NNOD,Num,IC
real :: A(200,200),B(200),X(200)

Num=2*NNOD

call GAUSS1(Num,A,B,IC)

if (IC==0) then
  call GAUSS2(Num,A,B,X)
end if

return
end subroutine SOLVE


!-------------------------------------------------------------sub. GAUSS1
subroutine GAUSS1(n,A,b,ic)

integer :: n,ic
real :: A(200,200),B(200)
real :: eps,tmp
integer :: itmp,i,j,k

eps=1.0E-10
ic=0

do k=1,n

 itmp=k
 do i=K+1,n
   if (abs(A(i,k))>abs(A(itmp,k))) then
      itmp=i
   end if
 end do
 
 imax=itmp
 
 do j=k,n
   tmp=A(imax,j)
   A(imax,j)=A(k,j)
   A(k,j)=tmp
 end do
 tmp=b(imax)
 b(imax)=b(k)
 b(k)=tmp
 
 if (abs(A(k,k))<eps) then
   ic=1
   return
 end if
      
 do j=k+1,n
   A(k,j)=A(k,j)/A(k,k)
 end do
 b(k)=b(k)/A(k,k)
 A(k,k)=1.0
 
 do i=k+1,n
   do j=k+1,n
     A(i,j)=A(i,j)-A(i,k)*A(k,j)
   end do
   b(i)=b(i)-A(i,k)*b(k)
   A(i,k)=0.0
 end do
 
end do

return
end subroutine GAUSS1
!-------------------------------------------------------------sub. GAUSS2
subroutine GAUSS2(n,A,b,x)

real :: A(200,200),b(200),x(200),SUM,tmp
integer :: m,n,k

do k=n,1,-1
  
  tmp=0.0
  do m=k+1,n
    tmp=tmp+A(k,m)*x(m)
  end do
  SUM=tmp
  
  x(k)=b(k)-SUM
  
end do
return
end subroutine GAUSS2