!**********************************************************************
! 有限要素法ルーチン
!**********************************************************************

!----------------------------------------------------------------------
! メッシュデータの作成
!----------------------------------------------------------------------
subroutine femmesh
   use mod_consts
   use mod_fem
   implicit none

   !write(*,*) '**** FEMMESH ****'
   write(1,*) '**** FEMMESH ****'

   call mesh_elem_edge_node_numbers    ! 要素番号・エッジ番号・ノード番号変換表作成
   call mesh_orientation               ! 隣合う要素の向きを揃える
   call mesh_node_to_coord             ! グローバルノード番号→座標変換表作成
   call mesh_boundary_edges            ! 境界のエッジを指定する

   return
end subroutine

!----------------------------------------------------------------------
! 要素番号・エッジ番号・ノード番号変換表作成
!----------------------------------------------------------------------
subroutine mesh_elem_edge_node_numbers
   use mod_consts
   use mod_fem
   implicit none

   integer :: i,j,n,k1,k2,k3,k4,k5
   integer :: memberq

   write(*,*) '**** FEMMESH ****'

   nelem=nx*ny*2               ! 三角要素の数
   nedge=nx*(2*ny+1)+(nx+1)*ny ! 系全体のエッジ数
   nnode=(nx+1)*(ny+1)         ! 系全体のノード数

   ! パラメータ確認
   write(1,*) 'NX=',nx
   write(1,*) 'NY=',ny
   write(1,*) 'TOTAL ELEMENTS=',nelem
   write(1,*) 'TOTAL EDGES=',nedge
   write(1,*) 'TOTAL NODES=',nnode

   ! エラー処理
   if((nx.gt.mx).or.(ny.gt.my)) then
      write(*,*) 'Sub(mesh_elem_edge_node_numbers): mx or my is small!'
      stop
   end if

   ! ******** 要素に関する情報 ********
   ! -------- ポリゴン数 --------
   do i=1,nelem
      elem(i).npoly=3
   end do

   ! -------- エッジの変換 --------
   !
   !                    k5
   !    **********************************
   !    *                            *****
   !    *                        ****    *
   !    *                    ****        *
   ! k1 *                ****            * k2
   !    *            ****                *
   !    *        ****    k4              *
   !    *    ****                        *
   !    *****                            *
   !    **********************************
   !                    k3
   !

   n=0   ! 要素番号を表す
   do i=1,nx
      do j=1,ny
         k1=(i-1)*(ny+(2*ny+1))+j                ! 左
         k2=  (i)*(ny+(2*ny+1))+j                ! 右
         k3=(i-1)*(ny+(2*ny+1))+ny+(2*(j-1)+1)   ! （中、下）
         k4=(i-1)*(ny+(2*ny+1))+ny+(2*(j-1)+1)+1 ! （中、斜）
         k5=(i-1)*(ny+(2*ny+1))+ny+(2*(j-1)+1)+2 ! （中、上）

         n=n+1
         elem(n).edge(1)=k3
         elem(n).edge(2)=k2
         elem(n).edge(3)=k4
         n=n+1
         elem(n).edge(1)=k1
         elem(n).edge(2)=k4
         elem(n).edge(3)=k5
      end do
   end do

   ! -------- ノードの変換 --------
   !
   ! k4                              k3
   ! **********************************
   ! *                            *****
   ! *                        ****    *
   ! *                    ****        *
   ! *                ****            *
   ! *            ****                *
   ! *        ****                    *
   ! *    ****                        *
   ! *****                            *
   ! **********************************
   ! k1                              k2
   !

   n=0   ! 要素番号を表す
   do i=1,nx
      do j=1,ny
         k1=(ny+1)*(i-1)+(j)   ! 左下
         k2=(ny+1)*(i)  +(j)   ! 右下
         k3=(ny+1)*(i-1)+(j+1) ! 左上
         k4=(ny+1)*(i)  +(j+1) ! 右上

         n=n+1
         elem(n).node(1)=k1
         elem(n).node(2)=k2
         elem(n).node(3)=k4
         n=n+1
         elem(n).node(1)=k3
         elem(n).node(2)=k1
         elem(n).node(3)=k4
      end do
   end do

   return
end subroutine

!----------------------------------------------------------------------
! 隣合う要素の向きを揃える
!----------------------------------------------------------------------
subroutine mesh_orientation
   use mod_consts
   use mod_fem
   implicit none

   integer :: i,j,n,npoly

   ! ******** グローバルエッジ情報作成 ********
   ! -------- グローバルエッジの隣接要素, エッジの情報 --------
   do i=1,nedge    ! 0 に初期化
      gedge(i).elem(1)=0
      gedge(i).elem(2)=0

      gedge(i).edge(1)=0
      gedge(i).edge(2)=0

      gedge(i).node(1)=0
      gedge(i).node(2)=0
   end do

   do i=1,nelem
      npoly=elem(i).npoly    ! ポリゴンの数（三角形 or 四角形）
      do j=1,npoly
         n=elem(i).edge(j)    ! 今のグローバルエッジ番号
         if(gedge(n).elem(1).eq.0) then
            ! グローバルエッジ情報に初めて登録されるとき
            gedge(n).elem(1)=i
            gedge(n).edge(1)=j

            gedge(n).node(1)=elem(i).node(mod(j-1,npoly)+1)
            gedge(n).node(2)=elem(i).node(mod(j  ,npoly)+1)

            elem(i).orient(j)=+1    ! 向きを設定
         else
            ! すでにグローバルエッジ情報にもう一つの隣り合う要素の情報が登録されるとき
            gedge(n).elem(2)=i
            gedge(n).edge(2)=j

            ! 向きを設定
            if((gedge(n).node(1)).ne.(elem(i).node(mod(j-1,npoly)+1))) then
               ! 隣要素のエッジの向きと違うとき
               elem(i).orient(j)=-1
            else
               ! 隣要素のエッジの向きと同じ
               elem(i).orient(j)=+1
            end if

            gedge(n).node(1)=elem(i).node(mod(j-1,npoly)+1)
            gedge(n).node(2)=elem(i).node(mod(j  ,npoly)+1)

         end if
      end do
   end do

   return
end subroutine

!----------------------------------------------------------------------
! グローバルノード番号→座標変換表作成
!----------------------------------------------------------------------
subroutine mesh_node_to_coord
   use mod_consts
   use mod_fem
   implicit none

   integer :: i,j,n

   ! ******** TABLE(global node→(x,y)) ********
   n=0   ! 要素番号を表す
   do i=1,nx+1
      do j=1,ny+1
         n=n+1
         gn2x(n)=sizex*dfloat(i-1)/dfloat(nx)
         gn2y(n)=sizey*dfloat(j-1)/dfloat(ny)
      end do
   end do

   return
end subroutine

!----------------------------------------------------------------------
! 要素 e が配列 a のメンバーかどうかのテスト
!
! [Input]
!   e: 要素
!   a: 配列
!   n: 配列の大きさ
!
! [Output]
!   0: メンバーでないとき
!   1: メンバーのとき
!----------------------------------------------------------------------
integer function memberq(e,a,n)
   implicit none

   integer :: e,n
   integer :: a(n)

   integer :: i

   do i=1,n
      if(a(i).eq.e) then
         memberq=1
         return
      end if
   end do

   memberq=0
   return
end function

!----------------------------------------------------------------------
! 境界のエッジを指定する
!----------------------------------------------------------------------
subroutine mesh_boundary_edges
   use mod_consts
   use mod_fem
   implicit none

   integer :: i,j,n
   integer :: memberq

   ! ******** 境界・境界でないエッジの配列を作る ********
   ! -------- 境界(PEC)のエッジのリスト --------
   n=0
   do i=1,ny    ! 左の壁
      n=n+1
      boundary_edge(n)=i
   end do
   do i=1,ny    ! 右の壁
      n=n+1
      boundary_edge(n)=(nedge-ny)+i
   end do
   do i=1,nx    ! 下の壁
      n=n+1
      boundary_edge(n)=(3*ny+1)*(i-1)+(ny+1)
   end do
   do i=1,nx    ! 上の壁
      n=n+1
      boundary_edge(n)=(3*ny+1)*(i-1)+(3*ny+1)
   end do

   n_boundary=n                       ! 境界(PEC)のエッジ数
   n_non_boundary=nedge-n_boundary    ! 境界でないエッジ数（未知数の数となる）

   ! ---- 境界でないのエッジのリスト ----
   n=0
   do i=1,nedge
      if(memberq(i,boundary_edge,n_boundary).eq.0) then
         n=n+1
         non_boundary_edge(n)=i
      end if
   end do

   ! 確認
   !write(*,*) 'Boundary (PEC) edges'
   !do i=1,n_boundary
   !   write(*,*) boundary_edge(i)
   !end do
   !stop

   return
end subroutine

!----------------------------------------------------------------------
! ２点の距離の計算
!
! [Input]
! (x1,y1): 点１の座標
! (x2,y2): 点２の座標
!
! [Return]
! 点１と点２の距離
!----------------------------------------------------------------------
real(8) function dist(x1,y1,x2,y2)
   implicit none
   real(8) :: x1,y1,x2,y2

   dist=dsqrt((x1-x2)**2+(y1-y2)**2)

   return
end function

!----------------------------------------------------------------------
! 要素行列の計算
!
! [Input]
! ie: 対象とする要素
!
! [Output]
! t: [K∇mn]
! u: [Kmn]
!----------------------------------------------------------------------
subroutine emat3(ie,t,u)
   use mod_fem
   implicit none

   integer :: ie
   real(8) :: t(4,4),u(4,4)

   integer :: edge1,edge2,edge3,node1,node2,node3,i,j
   real(8) :: x1,x2,x3,y1,y2,y3,l(3),a(3),b(3),c(3),&
              aa(3),bb(3),cc(3),area,x1int,x2int,y1int,y2int
   real(8) :: dist

   ! グローバルエッジへの変換
   edge1=elem(ie).edge(1)
   edge2=elem(ie).edge(2)
   edge3=elem(ie).edge(3)
   ! グローバルノードへの変換
   node1=elem(ie).node(1)
   node2=elem(ie).node(2)
   node3=elem(ie).node(3)
   ! 頂点座標の取得
   x1=gn2x(node1)
   x2=gn2x(node2)
   x3=gn2x(node3)

   y1=gn2y(node1)
   y2=gn2y(node2)
   y3=gn2y(node3)
   ! エッジの長さの計算
   l(1)=dist(x1,y1,x2,y2)
   l(2)=dist(x2,y2,x3,y3)
   l(3)=dist(x3,y3,x1,y1)
   ! パラメータ a_i,b_i,c_i
   a(1)=x2*y3-x3*y2
   a(2)=x3*y1-x1*y3
   a(3)=x1*y2-x2*y1
   b(1)=y2-y3
   b(2)=y3-y1
   b(3)=y1-y2
   c(1)=x3-x2
   c(2)=x1-x3
   c(3)=x2-x1
   ! パラメータ A_i,B_i,C_i
   aa(1)=a(1)*b(2)-a(2)*b(1)
   aa(2)=a(2)*b(3)-a(3)*b(2)
   aa(3)=a(3)*b(1)-a(1)*b(3)
   bb(1)=b(1)*c(2)-b(2)*c(1)
   bb(2)=b(2)*c(3)-b(3)*c(2)
   bb(3)=b(3)*c(1)-b(1)*c(3)
   cc(1)=c(1)*a(2)-c(2)*a(1)
   cc(2)=c(2)*a(3)-c(3)*a(2)
   cc(3)=c(3)*a(1)-c(1)*a(3)
   ! x,x^2,y,y^2 の三角要素内の積分
   area=(x1*b(1)+x2*b(2)+x3*b(3))/2.0d0 !	三角要素の面積
   x1int=(x1+x2+x3)/3.0d0*area
   x2int=(x1**2+x2**2+x3**2+x1*x2+x2*x3+x3*x1)/6.0d0*area
   y1int=(y1+y2+y3)/3.0d0*area
   y2int=(y1**2+y2**2+y3**2+y1*y2+y2*y3+y3*y1)/6.0d0*area

   ! 準備は整った。やっとこれから要素行列の作成
   do i=1,3
      do j=1,3
         ! T 行列の計算
         t(i,j)=l(i)*l(j)*bb(i)*bb(j)/(4.0d0*area**3)

         ! U 行列の計算
         u(i,j)=(l(i)*l(j)/(16.0d0*area**4))* &
                  ( (aa(i)*aa(j)+cc(i)*cc(j))*area &
                   -(cc(i)*bb(j)+cc(j)*bb(i))*x1int &
                   -(aa(i)*bb(j)+aa(j)*bb(i))*y1int &
                   +bb(i)*bb(j)*(x2int+y2int) )
      end do
   end do

   return
end subroutine

!----------------------------------------------------------------------
! 系行列を作成する
!
! [Output]
! sys2t:
! sys2u:
!----------------------------------------------------------------------
subroutine femsysmat
   use mod_fem
   implicit none

   real(8) :: t(4,4),u(4,4)
   integer :: i,j,i2,j2,ie,nn(4)

!   write(*,*) '**** FEMSYSMAT ****'
   write(1,*) '**** FEMSYSMAT ****'

   ! 系行列を 0 に初期化
   do i=1,medge
      do j=1,medge
         syst(i,j)=0.0d0
         sysu(i,j)=0.0d0
      end do
   end do

   ! 系行列作成
   do ie=1,nelem
      if(elem(ie).npoly.eq.3) then
         call emat3(ie,t,u)
      else if(elem(ie).npoly.eq.4) then
         write(*,*) "Sub(femsysmat): rectangular element is not supported!"
         stop
      else
         write(*,*) 'Sub(femsysmat): npoly is not 3 or 4.'
         stop
      end if

      do i=1,elem(ie).npoly
        nn(i)=elem(ie).edge(i)
      end do

      do i=1,elem(ie).npoly
         do j=1,elem(ie).npoly
            syst(nn(i),nn(j))=syst(nn(i),nn(j)) &
                      +t(i,j)*(elem(ie).orient(i)*elem(ie).orient(j))
            sysu(nn(i),nn(j))=sysu(nn(i),nn(j)) &
                      +u(i,j)*(elem(ie).orient(i)*elem(ie).orient(j))
         end do
      end do
   end do

   ! ディリクレの境界条件を課すために
   ! PEC のエッジに対する方程式を除去する
   do i=1,n_non_boundary
      do j=1,n_non_boundary
         i2=non_boundary_edge(i)
         j2=non_boundary_edge(j)
         syst2(i,j)=syst(i2,j2)
         sysu2(i,j)=sysu(i2,j2)
      end do
   end do

   return
end subroutine

!----------------------------------------------------------------------
! 固有値問題を解く
!
! [Output]
! eigval
! eigvec
!----------------------------------------------------------------------
subroutine eigsolv
   use mod_consts
   use mod_fem
   implicit none

   integer,parameter :: nnn1=medge
   integer,parameter :: nnn2=medge
   ! 固定にしないとヒープ領域にとられて、stack overflow になる
   real(8) :: a(nnn1,nnn2),b(nnn1,nnn2),v(nnn1,nnn1),e(nnn1)
   real(8) :: w(nnn1,7)
   real(8) :: emax,enow
   integer :: nne,nnv,ier,i,j,imineig

!   write(*,*) '**** EIGSOLV ****'
   write(1,*) '**** EIGSOLV ****'
   nne=n_non_boundary
   nnv=nne
   write(1,*) 'Dimension of matrix=',n_non_boundary
   write(1,*) 'Number of Eigenvalues=',nne
   write(1,*) 'Number of Eigenvectors=',nnv

   ! ******** 解くための行列をコピーする ********
   do i=1,n_non_boundary
      do j=1,n_non_boundary
         a(i,j)=syst2(i,j)
         b(i,j)=sysu2(i,j)
      end do
   end do

   ! 実対称行列の固有値問題
   !
   ! 渡部 力, 名取 亮, 小国 力,"Fortran77 による数値計算ソフトウェア",
   ! 丸善株式会社, 1989 より
   !
   ! eigrs(a,b,n,n1,ne,nv,eps,w,e,v,ier)
   !
   ! a(n1,n):  Ax=λBx の A                 (D)
   ! b(n1,n):  Ax=λBx の B                 (D)
   ! n:        行列の次数                   (I)
   ! n1:       配列の行数                   (I)
   ! ne:       求める固有値の数             (I)
   ! nv:       求める固有ベクトルの数       (I)
   ! eps:      収束判定基準                 (D)
   ! w(n1,7):  作業用領域                   (D)
   ! e(ne):    求められた固有値が入る       (D)
   ! v(n1,nv): 求められた固有ベクトルが入る (D)
   ! ier:      リターンコード               (I)
   !
   call eigab(a,b,n_non_boundary,nnn1,-nne,nnv,1.0d-16,w,e,v,ier)

   ! ******** 最小固有値のインデックスを見つける ********
   ! imineig に最小固有値のインデックスを格納する
   imineig=1
   do i=1,nne
      if(dabs(e(i)).gt.1.0d0) then
         imineig=i
         write(1,*) 'Index to the minimum eigenvalue=',imineig
         exit        ! break do loop
      end if
   end do

   ! ******** 結果を格納する ********
   ! ---- 固有値の格納 ----
   do i=1,nne-(imineig-1)
      eigval(i)=e(imineig+(i-1))
   end do

   ! ---- 固有ベクトルの格納 ----
   ! 0 クリアする
   do i=1,n_non_boundary-imineig+1
      do j=1,nedge
         eigvec(i,j)=0.0d0
      end do
   end do
   ! 得られた解を埋め込む
   do i=1,n_non_boundary-imineig+1
      do j=1,n_non_boundary
         eigvec(i,non_boundary_edge(j))=v(j,imineig+(i-1))
      end do
   end do

   ! ******** ファイルにカットオフ周波数 (GHz) を出力する ********
   open(11,file='fcu.dat')
   do i=1,n_non_boundary-imineig+1
      write(11,"(i3,f6.2)") &
         i,(velocity_of_light/(2.0d0*pi))*dsqrt(dabs(eigval(i)))/1.0d9
   end do
   close(11)

   return
end subroutine

!
! End of File
!