!
! This file is part of SACAMOS, State of the Art CAble MOdels for Spice. 
! It was developed by the University of Nottingham and the Netherlands Aerospace 
! Centre (NLR) for ESA under contract number 4000112765/14/NL/HK.
! 
! Copyright (C) 2016-2018 University of Nottingham
! 
! SACAMOS is free software: you can redistribute it and/or modify it under the 
! terms of the GNU General Public License as published by the Free Software 
! Foundation, either version 3 of the License, or (at your option) any later 
! version.
! 
! SACAMOS is distributed in the hope that it will be useful, but 
! WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY 
! or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License 
! for more details.
! 
! A copy of the GNU General Public License version 3 can be found in the 
! file GNU_GPL_v3 in the root or at <http://www.gnu.org/licenses/>.
! 
! SACAMOS uses the EISPACK library (in /SRC/EISPACK). EISPACK is subject to 
! the GNU Lesser General Public License. A copy of the GNU Lesser General Public 
! License version can be found in the file GNU_LGPL in the root of EISPACK 
! (/SRC/EISPACK ) or at <http://www.gnu.org/licenses/>.
! 
! The University of Nottingham can be contacted at: ggiemr@nottingham.ac.uk
!
!
! File Contents:
! 
!     SUBROUTINE Laplace
!     FUNCTION ismember
!
! NAME
!     SUBROUTINE Laplace
!
! DESCRIPTION
!
!     Calculation of Per-Unit-Length Capacitance, Conductance and Inductance matrices 
!     for multi-conductor configutations including lossy dielectric regions.
!     The solution is based on the Finite Element method. 
!
!     The outer boundary of the problem space may be a conducting region or
!     a free space boundary. If the outer boundary is free space then it should
!     be circular and centred on the origin for correct application of the
!     free space boundary condition.
!
!     If a ground plane is present then it must be included in the input cross section
!     geometry as a finite conductor within the free space outer boundary.
!
!     The last conductor specified is always the reference conductor in the per-unit-length 
!     parameter matrices returned
!
!     Lossy dielectrics are taken into account by specifying a frequency and 
!     complex relative permittivity values at that frequency.
!
!     The solution calculates the C and G matrices, The inductance matrix, L is calculated
!     as [L]=[C^-1]*mu0*eps0*epsr where epsr is the background relative permittivity
!     so the inductance value is only correct for cross sections with homogeneous dielectric
!
!     The subroutine requires an existing mesh file created by the open source software gmsh: see http://gmsh.info/
!     This mesh format must be converted into the format required by the NLR Laplace software.
!
!     The underlying theory of the Finite Element formulation may be found in Theory_Manual_Section 4
!
!     The process implemented is as follows:
!  STAGE 1: Read the mesh information and the associated boundary information
!  STAGE 2: Convert gmsh format data to laplace format
!  STAGE 3: Create the boundary element list for PEC or outer boundaries
!  STAGE 4: Create the boundary node list
!  STAGE 5: Set the material information
!  STAGE 6: Filter out the unused nodes from gmsh and renumber the new node list
!  STAGE 7: Determine which of the nodes are unknown 
!  STAGE 8: work out the mapping of node numbers to knowns and unknowns
!  STAGE 9: Create the known voltage vectors
!  STAGE 10: Derive the necessary element properties
!      STAGE 10a: Determine K matrix elements for the unknowns
!      STAGE 10b: Determine K_rhs matrix that will be used to compute the rhs vector from the knowns.
!      STAGE 10c: Determine contribution to the K matrix do the unknowns from the asymptotic boundary condition on the open boundary
!  STAGE 11: Matrix solution of the finite element equations
!      STAGE 11a: fill the K matrix from the array of K matrix elements calculated in STAGE 10a, 10c
!      STAGE 11b: fill the K_rhs matrix from the array of K_rhs matrix elements calculated in STAGE 10b
!      STAGE 11c: Invert the K matrix
!      STAGE 11d loop over all the RHS vectors solving the matrix equation 
!  STAGE 12 Determine the voltage phi in each node of the mesh
!  STAGE 13: Capacitance and Conductance matrix computation from electric energy
!  STAGE 14: Inductance matrix computation from inverse of capacitance matrix
!  STAGE 15: Copy the inductance, capacitance and conductance matrices to the output matrices making them explicitly symmetrical
!  STAGE 16: plot potentials to vtk file for visualisation if required
!  STAGE 17: plot mesh to vtk file for visualisation if required
!  STAGE 18: deallocate memory 
!
!  References in the comments are to the Theory Manual and also the following book:
!      Jian-Ming Jin, "The Finite Element Method in Electromagnetics" 3rd Edition, John Wiley & sons 2014.
!      (Chapter 4.)
!     
! COMMENTS
!     This version uses complex arithmetic to include 
!     lossy dielectrics and hence a conductance matrix may be calculated.
!
!     The finite element equations are soolved using a (slow) direct matrix inverse.
!     Although we need to solve the equations for multiple right hand sides, a sparse matrix method may be better...
!
!     Do we need a contribution to the energy calculation from the field outside the free space boundary condition? See comment in STAGE 13
!
! HISTORY
!    started 5/7/2016 CJS. This subroutineis based on software from NLR and has been 
!                          translated from Matlab to Fortran.
!     8/5/2017         CJS: Include references to Theory_Manual
!     19/6/2018         CJS: Include iterative solver based on the biconjugate gradient method
!
SUBROUTINE Laplace(mesh_filename,dim,first_surface_is_free_space_boundary,   &
                   n_dielectric_regions,dielectric_region_epsr,frequency,Lmat,Cmat,Gmat,ox,oy)
!
! Look for and remove any nodes which do not form part of the mesh. This is required when
! running with gmsh.
!

USE type_specifications
USE constants
USE general_module
USE maths

IMPLICIT NONE

! variables passed to subroutine

character(LEN=filename_length),intent(IN) :: mesh_filename        ! name of the gmsh file for the computational mesh

integer,intent(IN)       :: dim                                              ! dimension of matrix system
logical,intent(IN)       :: first_surface_is_free_space_boundary             ! flag to indicate properties on the first surface defined
integer,intent(IN)       :: n_dielectric_regions                             ! number of dielectric regions
complex(dp),intent(IN)   :: dielectric_region_epsr(0:n_dielectric_regions)   ! complex relative permittivity for each mesh region
                                                                             ! note, include a property for region 0, the 'background' region

real(dp),intent(IN)      :: frequency        ! frequency for L,C,G matrix calculation 

real(dp),intent(OUT)     :: Lmat(dim,dim)    ! Inductance matrix to be calculated
real(dp),intent(OUT)     :: Cmat(dim,dim)    ! Capacitance matrix to be calculated
real(dp),intent(OUT)     :: Gmat(dim,dim)    ! Conductance matrix to be calculated

real(dp),intent(IN)      :: ox    ! mesh centre offset in x used for plotting meshes if required
real(dp),intent(IN)      :: oy    ! mesh centre offset in y used for plotting meshes if required

! local variables

! User defined type for boundary information
type :: boundary_data

  integer :: N_elements_boundary
  integer,allocatable :: boundary_elements(:,:)     ! list of boundary elements, boundary_elements(be,1)= element number, boundary_elements(be,2)= boundary condition number
  
  integer :: N_nodes_boundary
  integer,allocatable :: boundary_nodes(:,:)      ! list of boundary nodes, boundary_nodes(bnode,1)= node number, boundary_nodes(bnode,2)= boundary condition number
  
end type boundary_data

! variables for reading the gmsh mesh file before conversion to the Laplace format
integer :: n_conductors

integer,allocatable  :: point_to_boundary_list(:)                   ! if a point is on an external boundary then put that boundary number in this list
integer,allocatable  :: boundary_segment_to_boundary_number_list(:) ! this list relates the individual boundary segments to the boundary numbers (i.e. conductor numbers)

integer :: bs                                   ! boundary segment number

integer :: first_surface                        ! first_surface=0 if we have a free space outer boundary, 1 otherwise

! Laplace variables

integer :: N_nodes_in                                     ! number of nodes in the gmsh file (this is not necessarily the number of nodes in the FE mesh)
integer :: N_elements_in                                  ! number of elements in the gmsh file (this is not necessarily the number of triangular elements in the FE mesh)

integer :: n_boundaries                                   ! number of boundaries, not including dielectric (internal) boundaries
integer :: N_boundaries_max                               ! maximum boundary number generated by PUL_LC_Laplace and found insolve_real_symm(n, b, x,tol) the mesh file
integer :: N_boundary                                     ! number of viable boundaries in the mesh i.e. boundaries with two or more points on
integer :: boundary_number
integer :: N_elements_boundary_temp
integer :: N_nodes_boundary_temp
integer :: n_boundary_segments

integer,allocatable              :: N_elements_boundary(:)   ! number of elements on each boundary
integer,allocatable              :: N_nodes_boundary(:)      ! number of nodes on each boundary
type(boundary_data),allocatable  :: boundary_info(:)         ! for each boundary the boundary data is a list of elements and nodes on the boundary

real(dp),allocatable :: node_coordinates_in(:,:)          ! Node coordinate list read from gmsh - note that some of these nodes are not used in the finite element solution

integer :: N_nodes                                        ! number of nodes input to Laplace
real(dp),allocatable :: node_coordinates(:,:)             ! Node coordinate list input to the NLR Laplace process

integer :: N_elements                                     ! number of elements input to Laplace
integer,allocatable  :: Element_data(:,:)                 ! Element data input to the NLR Laplace process

integer,allocatable  :: old_node_to_new_node_number(:)    ! node re-numbering array

integer              :: N_materials         ! number of dielectric materials (including the background permittivity)
real(dp),allocatable :: Mat_prop(:,:)       ! material proparties

integer,allocatable :: Node_Type(:)         ! if the node is on a conductor then this holds the conductor number, otherwise it is zero

integer,allocatable :: Node_to_Known_Unknown(:) ! list of all the nodes which points to the appropriate place in the list of knows or unknowns as appropriate
integer,allocatable :: Vector_of_Knowns(:)      ! list of all the boundary nodes i.e. those which will have known potentials

integer     :: N_unknown                     ! number of known node voltages i.e. those on which fixed potential boundary conditions are applied
integer     :: N_known                       ! number of unknown node voltages

integer     :: jmax                          ! maximum boundary number (i.e. number of conductors)
                                             ! this is used to determine the number of finite element solutions (right hand solve_real_symm(n, b, x,tol)sides to solve for) to
                                             ! fill the capacitance/conductance matrix

integer     :: total_n_boundary_nodes        ! total number of boundary nodes i.e. the number of known node values

! array of potentials for each of the finite element vector solutions. This includes both initially known and initially unknown potentials
complex(dp),allocatable     :: V(:,:,:)

! Element properties
complex(dp),allocatable     :: b(:,:)      ! element based array of constants related to the element geometry
complex(dp),allocatable     :: c(:,:)      ! element based array of constants related to the element geometry
complex(dp),allocatable     :: delta(:)    ! element based array with a value related to the element geometry
complex(dp),allocatable     :: eps_r(:)    ! element based array with the complex relative permittivity of the element

!
!integer     :: n_entrysolve_real_symm(n, b, x,tol)
!! 1D arrays used in the construction of the K matrix ( K(i_K(:),j_K(:))=K(i_K(:),j_K(:))+s_K(:) )
!integer,allocatable         :: i_K(:)
!integer,allocatable         :: j_K(:)
!complex(dp),allocatable     :: s_K(:)

! 1D arrays used in the construction of the right hand side vectors ( K_rhs(i_K_rhs(:),j_K_rhs(:))=K_rhs(i_K_rhs(:),j_K_rhs(:))+s_K_rhs(:) )
integer,allocatable         :: i_K_rhs(:)        ! row number
integer,allocatable         :: j_K_rhs(:)        ! col number
complex(dp),allocatable     :: s_K_rhs(:)        ! value

complex(dp),allocatable     :: x(:,:,:)          ! array of solution vectors, one for each each finite element solution required to complete the C/G matrices
complex(dp),allocatable     :: x_tmp(:)          ! single solution vector
complex(dp),allocatable     :: b_tmp(:)          ! single right hand side vector: b_tmp=-matmul(K_rhs,v_tmp)
complex(dp),allocatable     :: v_tmp(:)          ! temporary array with known boundary voltages used for constructing the right hand side 

complex(dp),allocatable     :: K(:,:)         ! Finite element equation coefficient matrix
complex(dp),allocatable     :: KI(:,:)        ! Inverse of the Finite element equation coefficient matrix 
complex(dp),allocatable     :: K_rhs(:,:)     ! array of right hand side vectors, one for each each finite element solution required to complete the C/G matrices

complex(dp),allocatable     :: phi(:,:,:)     ! node based potential values for each finite element solution

complex(dp),allocatable     :: energy(:,:)    ! energy values for each of the finite element solutions

! matrices determined from energy calculations
real(dp),allocatable    :: Inductance_energy(:,:)
real(dp),allocatable    :: Capacitance_energy(:,:)
real(dp),allocatable    :: Conductance_energy(:,:)

! temporary variables used for loops, counters, intermediate results used in calculations etc.
integer :: node,new_node
integer :: element

integer     :: n_entry_K_rhs
integer     :: n1,n2,n3
complex(dp) :: x1,x2,x3,y1,y2,y3
integer     :: Mat_Type

integer     :: n1_el,n2_el,n3_el
integer     :: el
integer     :: n_el_bnd               ! loop variable for elements on the boundary
complex(dp) :: rho,gamma,ls

complex(dp) :: b3,bc_comp,c3,delta_energy,delta_Q,phi_comp
integer     :: nd1,nd2,p

real(dp)    :: x_centre,y_centre

character(LEN=filename_length) :: filename
character(LEN=line_length)     :: line

real(dp)     :: scale

! temporary variables and loop counters
integer :: i,ii,i1,i2,loop,count
integer :: nb1,nb2,nb3,nbcount
integer :: itmp,type,itmp2,itmp3,itmp4
integer :: jj,j1,j2
integer :: n_nodes_bnd
integer :: n_con
integer :: nr

integer      :: ierr   ! error code for matrix inversion 

! variables for iterative solver

integer  itnlim 
real(dp)   rtol   
      
real(dp),allocatable :: bt(:)                ! real vectors for conjugate gradient solution for lossless problems
real(dp),allocatable :: xt(:)

logical :: lossy_dielectric                  ! flag to indicate lossy dielectric i.e. we must solve a complex problem

logical :: wantse
integer :: n,m
real(dp) :: atol, btol, conlim, damp
real(dp) :: Arnorm, xnorm
real(dp),allocatable :: se(:)

real(dp) :: Vout

! START

if (verbose) write(*,*)'CALLED: Laplace'

!  STAGE 1. Read the mesh information and the associated boundary information

n_conductors=dim+1   ! number of conductors, used to establish which boundaries are conductors

! Establish the number of external boundaries evaluated as 
! the number of conducting boundaries + outer boundary if it exists
! note that dielectric boundaries are not external boundaries as they have mesh both sides.

N_boundaries=n_conductors
if (first_surface_is_free_space_boundary) N_boundaries=N_boundaries+1

! open the file with the boundary information produced by PUL_LC_Laplace and written to file there
open(unit=boundary_file_unit,file=boundary_file_name,status='OLD',err=9020)
if (verbose) write(*,*)'Opened file:',boundary_file_name

read(boundary_file_unit,*,end=9030)n_boundary_segments

ALLOCATE( boundary_segment_to_boundary_number_list(1:n_boundary_segments) )

do i=1,n_boundary_segments
  read(boundary_file_unit,*,end=9030)ii,boundary_segment_to_boundary_number_list(i)
end do 

! close the boundary information file
close(unit=boundary_file_unit)
if (verbose) write(*,*)'Closed file:',boundary_file_name

! open and read the mesh file produced by gmsh

open(unit=mesh_file_unit,file=mesh_filename,status='OLD',err=9000)
if (verbose) write(*,*)'opened mesh file:',trim(mesh_filename)

! read to the start of the node list

do
  read(mesh_file_unit,'(A)',end=9010)line
  if (line(1:6).EQ.'$Nodes') exit
end do

read(mesh_file_unit,*)n_nodes_in    

ALLOCATE ( node_coordinates_in(1:N_nodes_in,1:3) )
ALLOCATE( point_to_boundary_list(1:N_nodes_in) )

do i=1,n_nodes_in
  read(mesh_file_unit,*)itmp,node_coordinates_in(i,2),node_coordinates_in(i,3)
  node_coordinates_in(i,1)=i
end do

! read to the start of the element list

do
  read(mesh_file_unit,'(A)',end=9010)line
  if (line(1:9).EQ.'$Elements') exit
end do

! read the element list

read(mesh_file_unit,*)n_elements_in
ALLOCATE ( Element_data(1:n_elements_in,1:5) )  ! note that we allocate data for all the gmesh elements, even though some are line elements and not needed.

Element_data(1:n_elements_in,1:5)=0
point_to_boundary_list(:)=0
n_boundaries_max=0
n_elements=0       ! counter for triangular elements

do i=1,n_elements_in

! The gmsh file includes elements of type 1 (boundary line segments) and type 2 (triangular elements)
! These are dealt with differently so first read the line from the file into a string, then process the string as required
! for these two cases.

  read(mesh_file_unit,'(A256)')line
  read(line,*)itmp,type
  
  if (type.EQ.2) then 
! this is a triangular element so put the data in the element list
     n_elements=n_elements+1                                  ! increase the count of triangular elements
     
     read(line,*)itmp,type,itmp2,itmp3,  &
                 Element_data(n_elements,5),    &
                 Element_data(n_elements,2),    &
                 Element_data(n_elements,3),    &
                 Element_data(n_elements,4)
     Element_data(n_elements,1)=n_elements
     
  else if (type.EQ.1) then 
! this is a boundary segment
     read(line,*)itmp,type,itmp2,itmp3,itmp4,nb1,nb2
     bs=itmp4                                                       ! this is the boundary line segment number (not the boundary number)
     
     boundary_number=boundary_segment_to_boundary_number_list(bs)   ! get the boundary number for this line segment
     n_boundaries_max=max(n_boundaries_max,boundary_number)         ! update the maximum number of boundaries

! only include 'external' boundaries in the point_to_boundary_list, not internal dielectric boundaries
     if (boundary_number.LE.N_boundaries) then
       point_to_boundary_list(nb1)=boundary_number
       point_to_boundary_list(nb2)=boundary_number
     end if
     
  end if  ! element type
  
end do ! next element

! close the mesh file produced by gmsh
close(unit=mesh_file_unit)
if (verbose) write(*,*)'closed mesh file:',trim(mesh_filename)

! Establish whether the first surface is a free space boundary or a conductor
if (first_surface_is_free_space_boundary) then
  first_surface=0
else
  first_surface=1
end if

! STAGE 2: Convert gmsh format data to laplace format
! note that the gmsh node list includes points which are not in the mesh e.g. the points which define the centres of the circular arcs
! these points are removed in the conversion to the Laplace input structures

if (verbose) then
  write(*,*)'Number of nodes in the mesh file=',N_nodes_in
  write(*,*)'Number of elements=',N_elements
  write(*,*)' Allocate and read nodes'
  write(*,*)' Allocate and read boundary information'
  write(*,*)'Number of boundaries including dielectric boundaries=',n_boundaries_max
  write(*,*)'Number of boundaries not including dielectric boundaries=',n_boundaries
end if ! verbose

! check the boundary count
if(n_boundaries.GT.n_boundaries_max) then
  write(run_status,*)'ERROR in Laplace. Inconsistent boundary count ',N_boundaries_max,n_boundaries
  CALL write_program_status()
  STOP 1
end if

ALLOCATE ( N_elements_boundary(1:N_boundaries_max) )
ALLOCATE ( N_nodes_boundary(1:N_boundaries_max) )
ALLOCATE ( boundary_info(1:N_boundaries_max) )

! STAGE 3: Create the boundary element list but only for PEC or outer boundaries
N_boundary=0

do i=1,n_boundaries

  if(verbose) write(*,*)'Setting boundary number',i,'of ',n_boundaries

  count=0
  do loop=1,2
  
! The first time through the loop, count the number of elements with two or more nodes on the current boundary
! The second time through the loop, allocate and fill the boundary_info element information 
! (element number and boundary condition number)
  
    if (loop.eq.2) then
! count is the number of boundary elements
      N_elements_boundary_temp=count
      count=0
      if (N_elements_boundary_temp.GT.0) then
        N_boundary=N_boundary+1
        N_elements_boundary(N_boundary)=N_elements_boundary_temp
        boundary_info(N_boundary)%N_elements_boundary=N_elements_boundary_temp

        if(verbose) then
          write(*,*)'N_boundary=',N_boundary,' N_boundaries_max=',N_boundaries_max
          write(*,*)'Number of boundary elements =',N_elements_boundary_temp,  &
                              ' boundary condition',first_surface+i-1
        end if

        ALLOCATE ( boundary_info(N_boundary)%boundary_elements(1:N_elements_boundary_temp,1:2) )
        
      end if
    end if

! loop over elements    
    do ii=1,n_elements

      nb1=point_to_boundary_list(Element_data(ii,2))
      nb2=point_to_boundary_list(Element_data(ii,3))
      nb3=point_to_boundary_list(Element_data(ii,4))
! work out how many of the nodes on this element are on the current boundary
      nbcount=0
      if(nb1.eq.i) nbcount=nbcount+1
      if(nb2.eq.i) nbcount=nbcount+1
      if(nb3.eq.i) nbcount=nbcount+1
      
      if (nbcount.GE.2) then
! the element has at least one edge on this boundary
        count=count+1
        if (loop.eq.2) then
            
          boundary_info(N_boundary)%boundary_elements(count,1)=ii                 ! element number
          boundary_info(N_boundary)%boundary_elements(count,2)=first_surface+i-1  ! boundary number
        
        end if ! second time round loop so we can record the boundary data
        
      end if ! there are elements on this boundary
      
    end do ! next element in mesh
    
  end do ! next loop (1,2)
    
! create boundary node list
  count=0
  do loop=1,2
  
! The first time through the loop, count the number of nodes on the current boundary
! The second time through the loop, allocate and fill the boundary_info node information 
! (node number and boundary condition number)
  
    if (loop.eq.2) then
! count is the number of boundary nodes 
      N_nodes_boundary_temp=count
      count=0
      if (N_nodes_boundary_temp.GT.0) then  
        N_nodes_boundary(N_boundary)=N_nodes_boundary_temp    
        boundary_info(N_boundary)%N_nodes_boundary=N_nodes_boundary_temp
        
        if(verbose) then
          write(*,*)'N_boundary=',N_boundary,' N_boundaries_max=',N_boundaries_max
          write(*,*)'Number of boundary points   =',N_nodes_boundary_temp,  &
                              ' boundary condition',first_surface+i-1
        end if
        
        ALLOCATE ( boundary_info(N_boundary)%boundary_nodes(1:N_nodes_boundary_temp,1:2) )
       
      end if
    end if
    
    do ii=1,n_nodes_in
    
      if(point_to_boundary_list(ii).eq.i) then
! the node is on this boundary
        count=count+1
        if (loop.eq.2) then
          boundary_info(N_boundary)%boundary_nodes(count,1)=ii                 ! point number
          boundary_info(N_boundary)%boundary_nodes(count,2)=first_surface+i-1  ! boundary condition number
        end if ! second time round loop so we can record the boundary data
        
      end if ! there are nodes on this boundary
      
    end do ! next node in mesh
    
  end do ! next loop (1,2)
  
end do ! next boundary, i

if (verbose) write(*,*)'n_boundaries=',n_boundaries,' N_boundary=',N_boundary

! STAGE 5: Set the material information
if (verbose) write(*,*)' Read material information'

! Set for single material with free space properties and a frequency of 1MHz

N_materials=n_dielectric_regions+1             ! add a free space region
if (verbose) write(*,*)' Number of materials=',N_materials

ALLOCATE ( Mat_prop(1:N_materials,1:4) )

lossy_dielectric=.FALSE.

do i=1,N_materials
  Mat_prop(i,1)=i    ! material number
  Mat_prop(i,2)=dble(dielectric_region_epsr(i-1))   ! Re{epsr}
  Mat_prop(i,3)=aimag(dielectric_region_epsr(i-1))  ! Im{epsr}
  Mat_prop(i,4)=frequency
  
  if ( abs(Mat_prop(i,3)).GT.small ) lossy_dielectric=.TRUE.
    
  if (verbose) then
    count=0
    do element=1,N_elements
      if (Element_data(element,5).EQ.i) count=count+1
    end do
    write(*,*)'Region:',i,' epsr=',Mat_prop(i,2),'+j',Mat_prop(i,3),' n_elements=',count
  end if ! verbose
  
end do ! next material

if (verbose) write(*,*)'frequency=',frequency

! STAGE 6: Filter out the unused nodes from gmsh and renumber the new node list

! work out how many nodes are actually used in the mesh
! then recreate the node list. This is required for meshes produced by gmsh
! which have unused nodes in the coordinate list.

ALLOCATE( old_node_to_new_node_number(1:N_nodes_in) )
old_node_to_new_node_number(1:N_nodes_in)=0            ! set all the elements of the renumbering array to zero - this indicates a node has not been renumberd yet

N_nodes=0

! loop over elements
do element=1,N_elements
  
! loop over the three nodes in this element
  do i=1,3
  
    node=Element_data(element,i+1)
    
! check whether this node has already been found
    if (old_node_to_new_node_number(node).EQ.0) then
    
! new node found so increase the number of nodes and renumber this node to the new node number created
      N_nodes=N_nodes+1
      old_node_to_new_node_number(node)=N_nodes
      
    end if

! renumber the node    
    Element_data(element,i+1)=old_node_to_new_node_number(node)
    
  end do ! next node in this element
  
end do ! next element

if (verbose) then
  write(*,*)'Number of nodes read=',N_nodes_in
  write(*,*)'Number of nodes used=',N_nodes
end if

! renumber the nodes in the boundary list

do i=1,N_boundary

    n_nodes_bnd=N_nodes_boundary(i)
    
    do jj=1,n_nodes_bnd
    
      node=boundary_info(i)%Boundary_nodes(jj,1)
      boundary_info(i)%Boundary_nodes(jj,1)=old_node_to_new_node_number(node)
        
    end do ! next node on this boundary
    
end do ! next boundary

! create the node_coordinate list to be used in the NLR Laplace Finite Element solution

ALLOCATE ( node_coordinates(1:N_nodes,1:3) )

do node=1,N_nodes_in
  new_node=old_node_to_new_node_number(node)
  if (new_node.NE.0) then ! the node exists in the mesh
    node_coordinates(new_node,1:3)=node_coordinates_in(node,1:3)
  end if
end do

DEALLOCATE( old_node_to_new_node_number )
DEALLOCATE( node_coordinates_in )

! STAGE 7: Determine which of the nodes are unknown 

if(verbose) write(*,*)'Determine which of the nodes are unknown'

! Determine which of the nodes are unknown (these will be marked by
! the integer 0) and which are known (these will be marked by integers
! > 0 that indicate the conductor number)

ALLOCATE( Node_Type(1:N_nodes) )

Node_Type(1:N_nodes)=0           ! assume all nodes are unknown to start

do i=1,N_boundary    ! loop over boundaries

    n_nodes_bnd=N_nodes_boundary(i)               ! number of nodes on this boundary
    n_con=boundary_info(i)%boundary_nodes(1,2)    ! number of the boundary condition on this boundary
    
    do jj=1,n_nodes_bnd                           ! loop over the boundary nodes
        nr=boundary_info(i)%Boundary_nodes(jj,1)  ! get the node number of this boundary node
        Node_Type(nr)=n_con                       ! set the node type to the boundary conductor number
    end do
    
end do

! STAGE 8: work out the mapping of node numbers to knowns and unknowns

if(verbose) write(*,*)' Mapping node numbers to unknowns and knowns '

! Mapping node numbers to unknowns and knowns
! Mapping knowns to different boundary values
N_unknown=0
N_known=0
jmax=0

ALLOCATE ( Node_to_Known_Unknown(1:N_nodes) )
Node_to_Known_Unknown(1:N_nodes)=0

! count the total number of boundary nodes (this is the number of knowns)
total_n_boundary_nodes=0
do i=1,N_boundary
  total_n_boundary_nodes=total_n_boundary_nodes+boundary_info(i)%N_nodes_boundary
end do

if (verbose) write(*,*)'Total number of boundary nodes=',total_n_boundary_nodes

ALLOCATE ( Vector_of_Knowns(1:total_n_boundary_nodes) )
Vector_of_Knowns(1:total_n_boundary_nodes)=0

do i=1,N_nodes
    if (Node_Type(i).EQ.0) then
       N_unknown=N_unknown+1
       Node_to_Known_Unknown(i)=N_unknown
    else
       N_known=N_known+1
       Node_to_Known_Unknown(i)=N_known
       do jj=1,N_boundary
           if (Node_Type(i).EQ.jj) then
              Vector_of_Knowns(N_known)=jj
              if (jj.GT.jmax) then
                 jmax=jj
              end if
           end if
       end do
    end if   
end do

if (verbose) write(*,*)'Maximum boundary number, jmax=',jmax, 'N_boundary=',N_boundary

! STAGE 9: Create the known voltage vectors

if (verbose) write(*,*)' Create known voltage vectors'
! Create known voltage vectors with:
! 1) one conductor set to voltage=1V and the
!    other conductors all set to 0V.
! 2) two conductors set to voltage=1V and the
!    other conductors set to 1V.

ALLOCATE ( V(1:jmax-1,1:jmax-1,1:N_known) )
V(1:jmax-1,1:jmax-1,1:N_known)=0d0

! The last conductor is always set to 0V as
! it is taken as the reference. This means that the ground plane, if it exists is always the reference condcutor.

do j1=1,jmax-1
   do j2=j1,jmax-1
      do i=1,N_known
         if (Vector_of_Knowns(i).EQ.(j1)) then
            V(j1,j2,i)=1.0
         end if
         if (Vector_of_Knowns(i).EQ.(j2)) then
            V(j1,j2,i)=1.0   
         end if
      end do
   end do
end do

! Stage 10: Derive the necessary element properties
if (verbose) write(*,*)' Derive the necessary element properties'

ALLOCATE( b(1:N_elements,1:3) )
ALLOCATE( c(1:N_elements,1:3) )
ALLOCATE( delta(1:N_elements) )
ALLOCATE( eps_r(1:N_elements) )

b(1:N_elements,1:3)=0d0
c(1:N_elements,1:3)=0d0
delta(1:N_elements)=0d0
eps_r(1:N_elements)=0d0

! do this twice, the first time to count the numbers of entries and to allocate the memory, the second to fill the memory
do loop=1,2

  n_entry=0
  n_entry_K_rhs=0

! loop over elements
  do i=1,N_elements
  
! get the node numbers for the 3 nodes on this element
    n1=Element_data(i,2)
    n2=Element_data(i,3)
    n3=Element_data(i,4)
    
! get the coordinates of the nodes
    x1=Node_Coordinates(n1,2)
    y1=Node_Coordinates(n1,3)
    x2=Node_Coordinates(n2,2)
    y2=Node_Coordinates(n2,3)
    x3=Node_Coordinates(n3,2)
    y3=Node_Coordinates(n3,3)
    
! Calculate b and c coefficients, delta (element area) (equation 4.24, J-M.Jin)
    b(i,1)=y2-y3
    b(i,2)=y3-y1
    b(i,3)=y1-y2
    c(i,1)=x3-x2
    c(i,2)=x1-x3
    c(i,3)=x2-x1
    
    delta(i)=0.5*(b(i,1)*c(i,2)-b(i,2)*c(i,1))
    
    Mat_Type=Element_data(i,5)     ! material number for this element
        
    eps_r(i)=Mat_Prop(Mat_Type,2)+j*Mat_Prop(Mat_Type,3)   ! relative permittivity in this material region
   
! STAGE 10a: Determine K matrix elements related to the unknowns
! STAGE 10b: Determine K_rhs matrix that will be used to compute the rhs vector from the knowns.
! See section 4.3.3.2 for a description of the assembly of the system of equations (K matrix elements)
! and section 4.3.3.4 for a description of the assembly of the RHS terms.

    if (Node_Type(n1).EQ.0) then        ! node n1 is not on a conducting boundary
        n_entry=n_entry+1
        if (loop.EQ.2) then
          i_K(n_entry)=Node_to_Known_Unknown(n1)
          j_K(n_entry)=Node_to_Known_Unknown(n1)
          s_K(n_entry)=(eps_r(i)/(4.0*delta(i)))*(b(i,1)*b(i,1)+c(i,1)*c(i,1))       ! K11, equation 4.33 with alpha_x=alpha_y=epsr, beta=0, i=j (diagonal element)
        end if
        if (Node_Type(n2).EQ.0) then          ! neither node 1 nor node 2  are on conductors
            n_entry=n_entry+1
            if (loop.EQ.2) then
              i_K(n_entry)=Node_to_Known_Unknown(n1)
              j_K(n_entry)=Node_to_Known_Unknown(n2)
              s_K(n_entry)=(eps_r(i)/(4.0*delta(i)))*(b(i,1)*b(i,2)+c(i,1)*c(i,2))   ! K12, equation 4.33 with alpha_x=alpha_y=epsr, beta=0
            end if
            n_entry=n_entry+1
            if (loop.EQ.2) then
              i_K(n_entry)=Node_to_Known_Unknown(n2)
              j_K(n_entry)=Node_to_Known_Unknown(n1)   
              s_K(n_entry)=s_K(n_entry-1)                                            ! K21=K12
            end if
        else                                  ! node 2 is on a conductor so therefore has a known value so the contribution goes to the right hand side
            n_entry_K_rhs=n_entry_K_rhs+1
            if (loop.EQ.2) then
              i_K_rhs(n_entry_K_rhs)=Node_to_Known_Unknown(n1)
              j_K_rhs(n_entry_K_rhs)=Node_to_Known_Unknown(n2)
              s_K_rhs(n_entry_K_rhs)=(eps_r(i)/(4.0*delta(i)))*(b(i,1)*b(i,2)+c(i,1)*c(i,2))   ! equation 4.33 with alpha_x=alpha_y=epsr, beta=0
            end if
        end if
        if (Node_Type(n3).EQ.0) then          ! neither node 1 nor node 3  are on conductors
            n_entry=n_entry+1
            if (loop.EQ.2) then
              i_K(n_entry)=Node_to_Known_Unknown(n1)
              j_K(n_entry)=Node_to_Known_Unknown(n3)
              s_K(n_entry)=(eps_r(i)/(4.0*delta(i)))*(b(i,1)*b(i,3)+c(i,1)*c(i,3))   ! K13, equation 4.33 with alpha_x=alpha_y=epsr, beta=0
            end if
            n_entry=n_entry+1
            if (loop.EQ.2) then
              i_K(n_entry)=Node_to_Known_Unknown(n3)
              j_K(n_entry)=Node_to_Known_Unknown(n1)
              s_K(n_entry)=s_K(n_entry-1)                                            ! K31=K13
            end if
        else     ! node 3 is on a conductor so therefore has a known value so the contribution goes to the right hand side
            n_entry_K_rhs=n_entry_K_rhs+1
            if (loop.EQ.2) then
              i_K_rhs(n_entry_K_rhs)=Node_to_Known_Unknown(n1)
              j_K_rhs(n_entry_K_rhs)=Node_to_Known_Unknown(n3)
              s_K_rhs(n_entry_K_rhs)=(eps_r(i)/(4.0*delta(i)))*(b(i,1)*b(i,3)+c(i,1)*c(i,3))   ! equation 4.33 with alpha_x=alpha_y=epsr, beta=0
            end if
        end if
    end if
    
    if (Node_Type(n2).EQ.0) then            ! node n2 is not on a conducting boundary
        if (Node_Type(n1).NE.0) then        ! node n1 is on a conductor so therefore has a known value so the contribution goes to the right hand side
            n_entry_K_rhs=n_entry_K_rhs+1
            if (loop.EQ.2) then
              i_K_rhs(n_entry_K_rhs)=Node_to_Known_Unknown(n2)
              j_K_rhs(n_entry_K_rhs)=Node_to_Known_Unknown(n1)
              s_K_rhs(n_entry_K_rhs)=(eps_r(i)/(4.0*delta(i)))*(b(i,1)*b(i,2)+c(i,1)*c(i,2))   ! equation 4.33 with alpha_x=alpha_y=epsr, beta=0
            end if
        end if
        n_entry=n_entry+1
        if (loop.EQ.2) then
          i_K(n_entry)=Node_to_Known_Unknown(n2)
          j_K(n_entry)=Node_to_Known_Unknown(n2)
          s_K(n_entry)=(eps_r(i)/(4.0*delta(i)))*(b(i,2)*b(i,2)+c(i,2)*c(i,2))   ! equation 4.33 with alpha_x=alpha_y=epsr, beta=0, i=j (diagonal element)
        end if
        if (Node_Type(n3).EQ.0) then                  ! node n3 is not on a conducting boundary
            n_entry=n_entry+1
            if (loop.EQ.2) then
              i_K(n_entry)=Node_to_Known_Unknown(n2)
              j_K(n_entry)=Node_to_Known_Unknown(n3)
              s_K(n_entry)=(eps_r(i)/(4.0*delta(i)))*(b(i,2)*b(i,3)+c(i,2)*c(i,3))   ! K23, equation 4.33 with alpha_x=alpha_y=epsr, beta=0
            end if
            n_entry=n_entry+1
            if (loop.EQ.2) then
              i_K(n_entry)=Node_to_Known_Unknown(n3)
              j_K(n_entry)=Node_to_Known_Unknown(n2)
              s_K(n_entry)=s_K(n_entry-1)                                            ! K32=K23
            end if
        else        ! node n3 is on a conductor so therefore has a known value so the contribution goes to the right hand side
            n_entry_K_rhs=n_entry_K_rhs+1
            if (loop.EQ.2) then
              i_K_rhs(n_entry_K_rhs)=Node_to_Known_Unknown(n2)
              j_K_rhs(n_entry_K_rhs)=Node_to_Known_Unknown(n3)
              s_K_rhs(n_entry_K_rhs)=(eps_r(i)/(4.0*delta(i)))*(b(i,2)*b(i,3)+c(i,2)*c(i,3))   ! equation 4.33 with alpha_x=alpha_y=epsr, beta=0
            end if
        end if
     end if
     if (Node_Type(n3).EQ.0) then            ! node n3 is not on a conducting boundary
         if (Node_Type(n1).NE.0) then        ! node n1 is on a conductor so therefore has a known value so the contribution goes to the right hand side
             n_entry_K_rhs=n_entry_K_rhs+1
             if (loop.EQ.2) then
               i_K_rhs(n_entry_K_rhs)=Node_to_Known_Unknown(n3)
               j_K_rhs(n_entry_K_rhs)=Node_to_Known_Unknown(n1)
               s_K_rhs(n_entry_K_rhs)=(eps_r(i)/(4.0*delta(i)))*(b(i,1)*b(i,3)+c(i,1)*c(i,3))   ! equation 4.33 with alpha_x=alpha_y=epsr, beta=0
             end if
         end if
         if (Node_Type(n2).NE.0) then       ! node n2 is on a conductor so therefore has a known value so the contribution goes to the right hand side
             n_entry_K_rhs=n_entry_K_rhs+1
             if (loop.EQ.2) then
               i_K_rhs(n_entry_K_rhs)=Node_to_Known_Unknown(n3)
               j_K_rhs(n_entry_K_rhs)=Node_to_Known_Unknown(n2)
               s_K_rhs(n_entry_K_rhs)=(eps_r(i)/(4.0*delta(i)))*(b(i,2)*b(i,3)+c(i,2)*c(i,3))   ! equation 4.33 with alpha_x=alpha_y=epsr, beta=0
             end if
         end if
         n_entry=n_entry+1
         if (loop.EQ.2) then
           i_K(n_entry)=Node_to_Known_Unknown(n3)
           j_K(n_entry)=Node_to_Known_Unknown(n3)
           s_K(n_entry)=(eps_r(i)/(4.0*delta(i)))*(b(i,3)*b(i,3)+c(i,3)*c(i,3))      ! K33, equation 4.33 with alpha_x=alpha_y=epsr, beta=0, i=j (diagonal element)
         end if
     end if
     
  end do ! next element

  if ( (loop.eq.1).AND.(verbose) ) then
    write(*,*)' Determine contribution to the K matrix due to '
    write(*,*)' the unknowns from the asymptotic boundary condition on the open boundary'
  end if
  
! STAGE 10c: Determine contribution to the K matrix do the unknowns
! from the asymptotic boundary condition on the open boundary

  do i=1,N_boundary   ! loop over the boundaries in the
  
      n_el_bnd=N_elements_boundary(i)                 ! number of elements on this boundary
      n_con=boundary_info(i)%boundary_elements(1,2)   ! condcutor number on this boundary
      
      if (n_con.EQ.0) then     ! boundary condition set to zero indicates an open boundary so work out the open boundary contribution here
      
          do jj=1,n_el_bnd     ! loop over the elements on the boundary
          
              el=boundary_info(i)%boundary_elements(jj,1)        ! get the element number
              
              n1_el=Element_data(el,2)                           ! get the node numbers on this element
              n2_el=Element_data(el,3)
              n3_el=Element_data(el,4)
              
! get the two nodes on the boundary and call them n1 and n2  
              if (ismember(n1_el,boundary_info(i)%boundary_nodes,boundary_info(i)%N_nodes_boundary,1).EQ.1) then
                 n1=n1_el 
                 if (ismember(n2_el,boundary_info(i)%boundary_nodes,boundary_info(i)%N_nodes_boundary,1).EQ.1) then
                    n2=n2_el
                 else   
                    n2=n3_el
                 end if
              else
                  n1=n2_el
                  n2=n3_el
              end if
              
              x1=Node_Coordinates(n1,2)    ! get the coordinats of the boundary nodes
              y1=Node_Coordinates(n1,3)
              x2=Node_Coordinates(n2,2)
              y2=Node_Coordinates(n2,3)
              
              x_centre=(x1+x2)/2           ! get the coordinates of the centre point of the boundary element edge
              y_centre=(y1+y2)/2
              
              rho=sqrt(x_centre**2+y_centre**2)   ! get the distance from the centre of the problem space (assumed to be 0,0) to the boundary element edge centre
              
              ls=sqrt((x2-x1)**2+(y2-y1)**2)      ! l2= boundary element edge length
              
              if (use_ABC) then
! This implements an asymptotic boundary condition. This apears to cause convergence
! issues for the Capacitance matrix calculation so should be used with caution.
! The default is the Neumann boundary condition (gamma=0)
                gamma=-eps_r(el)/(log(rho)*rho)     ! gamma (see equation 4.3 with equation 4.93)
              else
! Neumann boundary condition on the outer boundary. This effectively sets the charge to zero on the outer
! boundary and the convergence of the capacitance matrix is improved compared with the ABC
                gamma=0d0   
              end if
              
              ! K11 contributions according to equation 4.51 (note delta_ij=1 if i=j, 0 otherwise)
              n_entry=n_entry+1
              if (loop.EQ.2) then
                i_K(n_entry)=Node_to_Known_Unknown(n1)
                j_K(n_entry)=Node_to_Known_Unknown(n1)
                s_K(n_entry)=gamma*ls/3.0                    ! i=j=n1
              end if
              ! K12
              n_entry=n_entry+1
              if (loop.EQ.2) then
                i_K(n_entry)=Node_to_Known_Unknown(n1)
                j_K(n_entry)=Node_to_Known_Unknown(n2)
                s_K(n_entry)=gamma*ls/6.0                    ! i=1, j=n2
              end if
              ! K21
              n_entry=n_entry+1
              if (loop.EQ.2) then
                i_K(n_entry)=Node_to_Known_Unknown(n2)
                j_K(n_entry)=Node_to_Known_Unknown(n1)
                s_K(n_entry)=gamma*ls/6.0                    ! i=2, j=n1
              end if
              ! K22
              n_entry=n_entry+1
             if (loop.EQ.2) then
                i_K(n_entry)=Node_to_Known_Unknown(n2)
                j_K(n_entry)=Node_to_Known_Unknown(n2)
                s_K(n_entry)=gamma*ls/3.0                    ! i=j=n2
             end if
          end do    ! next element on the boundary
      end if
  end do ! next boundary
  
  if (loop.eq.1) then
! Allocate memory
     
     ALLOCATE( i_K(1:n_entry) )
     ALLOCATE( j_K(1:n_entry) )
     ALLOCATE( s_K(1:n_entry) )
     
     ALLOCATE( i_K_rhs(1:n_entry_K_rhs) )
     ALLOCATE( j_K_rhs(1:n_entry_K_rhs) )
     ALLOCATE( s_K_rhs(1:n_entry_K_rhs) )
     
  end if
  
end do ! next loop i.e. after counting and allocating the memory required(loop=1) go back and fill the memory with the appropriate values (loop=2)

! STAGE 11: Matrix solution of the finite element equations
! the equation to solve is [s_K](x)=(s_K_rhs)

! allocate memory for the solution vectors
ALLOCATE ( x(1:jmax-1,1:jmax-1,1:N_unknown) )
ALLOCATE ( x_tmp(1:N_unknown) )
ALLOCATE ( b_tmp(1:N_unknown) )
ALLOCATE ( v_tmp(1:N_known) )

! solution based on a full matrix inverse 

ALLOCATE ( K_rhs(1:N_unknown,1:N_known) )

if(verbose) then
  write(*,*)'Number of entries in K    ',n_entry
  write(*,*)'Number of entries in K_rhs',n_entry_K_rhs
end if

! STAGE 11b: fill the K_rhs matrix
K_rhs(1:N_unknown,1:N_known)=0d0
do i=1,n_entry_K_rhs
  K_rhs(i_K_rhs(i),j_K_rhs(i))=K_rhs(i_K_rhs(i),j_K_rhs(i))+s_K_rhs(i)
end do

if(verbose) then
  write(*,*)'Dimension of K     is',N_unknown,N_unknown
  write(*,*)'Dimension of K_rhs is',N_unknown,N_known
else
  write(*,*)'Dimension of K in Laplace is',N_unknown,N_unknown
end if

if (direct_solver) then
  ALLOCATE ( K(1:N_unknown,1:N_unknown) )
  ALLOCATE ( KI(1:N_unknown,1:N_unknown) )

! STAGE 11a: fill the K matrix
  K(1:N_unknown,1:N_unknown)=0d0
  do i=1,n_entry
    K(i_K(i),j_K(i))=K(i_K(i),j_K(i))+s_K(i)
  end do

! STAGE 11c: Invert the K matrix
  if(verbose) write(*,*)'Invert the K matrix'
  ierr=0   ! set ierr=0 to cause an error within cinvert_Gauss_Jordan if there is a problem calculating the inverse
  CALL cinvert_Gauss_Jordan(K,N_unknown,KI,N_unknown,ierr) 

! STAGE 11d loop over all the RHS vectors solving the matrix equation 
  do j1=1,jmax-1
      do j2=j1,jmax-1
          v_tmp(1:n_known)=V(j1,j2,1:n_known)
          b_tmp(1:N_unknown)=-matmul(K_rhs,v_tmp)
          x_tmp=matmul(KI,b_tmp)
          x(j1,j2,1:N_unknown)=x_tmp(1:N_unknown)
      end do
  end do
  
else if(.NOT.lossy_dielectric) then
    
  itnlim = N_unknown * 2  ! maximum number of iterations set to twice the number of unknowns
  rtol   = cg_tol
     
  allocate(xt(1:N_unknown))
  allocate(bt(1:N_unknown))
  
  ALLOCATE( s_K_re(1:n_entry) )
  s_K_re(1:n_entry)=dble(s_K(1:n_entry))
  
! Iterative solution
   do j1=1,jmax-1
      do j2=j1,jmax-1
      
        v_tmp(1:n_known)=V(j1,j2,1:n_known)
        b_tmp(1:N_unknown)=-matmul(K_rhs,v_tmp)
        bt(1:N_unknown)=b_tmp(1:N_unknown)
        xt(1:N_unknown)=0d0   ! initial guess for xt
      
! conjugate gradient solution
        CALL solve_real_symm(N_unknown, bt, xt, rtol, itnlim)

        x(j1,j2,1:N_unknown)=xt(1:N_unknown)
     
      end do
  end do
      
  deallocate(xt)
  deallocate(bt)
  
else if (lossy_dielectric) then

  itnlim = N_unknown * 2  ! maximum number of iterations set to twice the number of unknowns
  rtol   = cg_tol
        
! Iterative solution
   do j1=1,jmax-1
      do j2=j1,jmax-1
      
        v_tmp(1:n_known)=V(j1,j2,1:n_known)
        b_tmp(1:N_unknown)=-matmul(K_rhs,v_tmp)
        x_tmp(1:N_unknown)=(0d0,0d0)   ! initial guess for x_tmp
              
! conjugate gradient solution
        CALL solve_complex_symm(N_unknown, b_tmp, x_tmp, rtol,itnlim )
 
        x(j1,j2,1:N_unknown)=x_tmp(1:N_unknown)
          
      end do
  end do
         
end if

! STAGE 12 Determine the voltage phi in each node of the mesh

ALLOCATE ( phi(1:jmax-1,1:jmax-1,1:N_nodes) )
phi(1:jmax-1,1:jmax-1,1:N_nodes)=0d0

do j1=1,jmax-1
    do j2=j1,jmax-1
        do i=1,N_nodes
            if (Node_Type(i).EQ.0) then
               phi(j1,j2,i)=x(j1,j2,Node_to_Known_Unknown(i))
            else
               phi(j1,j2,i)=V(j1,j2,Node_to_Known_Unknown(i))
            end if
        end do
    end do
end do
   
if(verbose) write(*,*)' Capacitance computation from electric energy'
! STAGE 13: Capacitance computation from electric energy

! CJS comment: Should there be a contribution due to the asymptotic boundary? The boundary is not a 0V equipotential...
! The conclusion is that this contribution should be small and the solution as implemented here converges to
! the correct solution as the outer boundary distance is increased

ALLOCATE ( energy(1:jmax-1,1:jmax-1) )

energy(1:jmax-1,1:jmax-1)=0d0

do j1=1,jmax-1
    do j2=j1,jmax-1    
          do i=1,N_elements
              n1_el=Element_data(i,2)
              n2_el=Element_data(i,3)
              n3_el=Element_data(i,4)
              do nd1=1,3
                  do nd2=1,3 
                      bc_comp=(b(i,nd1)*b(i,nd2)+c(i,nd1)*c(i,nd2))
                      phi_comp=phi(j1,j2,Element_data(i,nd1+1))*(phi(j1,j2,Element_data(i,nd2+1)))
                      delta_energy=eps0*eps_r(i)*bc_comp*phi_comp/(8.0*delta(i))  
                      energy(j1,j2)=energy(j1,j2)+delta_energy
                  end do
              end do
          end do
    end do
end do

ALLOCATE ( Capacitance_energy(1:jmax-1,1:jmax-1) )
Capacitance_energy(1:jmax-1,1:jmax-1)=0d0
ALLOCATE ( Conductance_energy(1:jmax-1,1:jmax-1) )
Conductance_energy(1:jmax-1,1:jmax-1)=0d0

do j1=1,jmax-1

! diagonal elements

    Capacitance_energy(j1,j1)=2.0*dble(energy(j1,j1))
    Conductance_energy(j1,j1)=2.0*(-2d0*pi*frequency*aimag(energy(j1,j1)))
    
! off diagonal elements
    do j2=j1+1,jmax-1

! Theory_Manual_Eqn     4.28
       Capacitance_energy(j1,j2)=dble((energy(j1,j2)-energy(j1,j1)-energy(j2,j2)))
       Capacitance_energy(j2,j1)=Capacitance_energy(j1,j2)  
            
! Theory_Manual_Eqn     4.29
       Conductance_energy(j1,j2)=-2d0*pi*frequency*aimag((energy(j1,j2)-energy(j1,j1)-energy(j2,j2)))
       Conductance_energy(j2,j1)=Conductance_energy(j1,j2)
       
    end do
end do

if(verbose) then
  write(*,*)' '
  write(*,*)'Capacitance matrix from energy'
  do j1=1,jmax-1
    write(*,8000)( Capacitance_energy(j1,j2),j2=1,jmax-1 )
  end do

  write(*,*)' '
  write(*,*)'Conductance matrix from energy'
  do j1=1,jmax-1
    write(*,8000)( Conductance_energy(j1,j2),j2=1,jmax-1 )
  end do
end if

!  STAGE 14: Inductance matrix computation from inverse of capacitance matrix

ALLOCATE ( Inductance_energy(1:jmax-1,1:jmax-1) )

ierr=0   ! set ierr=0 to cause an error if there is a problem calculating the inverse
CALL dinvert_gauss_jordan(Capacitance_energy,jmax-1,Inductance_energy,jmax-1,ierr) 

! Include the relative permittivity of the background medium
Mat_Type=1    
Inductance_energy(:,:)=Inductance_energy(:,:)*mu0*eps0*(Mat_Prop(Mat_Type,2)+j*Mat_Prop(Mat_Type,3))

if (verbose) then
  write(*,*)' '
  write(*,*)'Inductance matrix from energy'
  do j1=1,jmax-1
    write(*,8000)( Inductance_energy(j1,j2),j2=1,jmax-1 )
  end do
end if

8000 format(100E12.4)

! STAGE 15: Copy the inductance, capacitance and conductance matrices to the output matrices
! Making the matrices symmetric explicitly (may not be symmetric to machine precision from the inverse calculation for L)
if (verbose) write(*,*)' Copy L, C, G matrices'

! Make the matrices symmetric explicitly (may not be symmetric to machine precision from the inverse calculation)

do j1=1,jmax-1

  Lmat(j1,j1)=Inductance_energy(j1,j1)
  Cmat(j1,j1)=Capacitance_energy(j1,j1)
  Gmat(j1,j1)=Conductance_energy(j1,j1)

  do j2=j1+1,jmax-1
  
    Lmat(j1,j2)=Inductance_energy(j1,j2)
    Cmat(j1,j2)=Capacitance_energy(j1,j2)
    Gmat(j1,j2)=Conductance_energy(j1,j2)
  
    Lmat(j2,j1)=Inductance_energy(j1,j2)
    Cmat(j2,j1)=Capacitance_energy(j1,j2)
    Gmat(j2,j1)=Conductance_energy(j1,j2)
    
  end do
end do

! STAGE 16: plot potentials to vtk file for visualisation if required
if (plot_potential) then
! We write out the potentials in vtk format here....

! calculate a scale for the problem
  scale=0d0
  do i=1,N_nodes
    scale=max( scale,sqrt(node_coordinates(i,2)**2+node_coordinates(i,3)**2) )
  end do

! write potential as a vtk file

  do j1=1,jmax-1
   do j2=1,jmax-1
  
      write(filename,'(A,A,I1,I1,A4)')trim(mesh_filename),'_V',j1,j2,'.vtk'
      open(unit=10,file=trim(filename))
     
! write header information    
! write vtk header      
      write(10,'(A)')'# vtk DataFile Version 2.0'
      write(10,'(A)')'V.vtk'
      write(10,'(A)')'ASCII'
      write(10,'(A)')'DATASET POLYDATA'
      write(10,'(A,I10,A)')'POINTS',n_nodes,' float'

! write point data 
      do i=1,N_nodes
        write(10,8005)node_coordinates(i,2)+ox,node_coordinates(i,3)+oy,real(phi(j1,j2,i)*scale)
      end do
      
! write element data (note node numbering starts from 0 in vtk format)
      write(10,'(A,2I10)')'POLYGONS',n_elements,n_elements*4
      do i=1,N_elements
        write(10,8010)3,Element_data(i,2)-1,Element_data(i,3)-1,Element_data(i,4)-1
      end do
  
! STAGE 6. write data associated with points

! write point based data
      write(10,'(A,I10)')'POINT_DATA ',n_nodes
      write(10,'(A)')'SCALARS Field_on_cells float 1'
      write(10,'(A)')'LOOKUP_TABLE field_on_cells_table'
    
      do i=1,N_nodes
        write(10,8020)real(phi(j1,j2,i))
      end do
  
    close(unit=10)
  
   end do
 
  end do

end if ! plot_potential

! STAGE 17: plot mesh to vtk file for visualisation if required

if (plot_mesh) then
! We write out the mesh in vtk format here....

! calculate a scale for the problem
  scale=0d0
  do i=1,N_nodes
    scale=max( scale,sqrt(node_coordinates(i,2)**2+node_coordinates(i,3)**2) )
  end do

! write potential as a vtk file
  
  filename=trim(mesh_filename)//'.vtk'
  open(unit=10,file=trim(filename))
     
! write header information    
! write vtk header      
  write(10,'(A)')'# vtk DataFile Version 2.0'
  write(10,'(A)')'V.vtk'
  write(10,'(A)')'ASCII'
  write(10,'(A)')'DATASET POLYDATA'
  write(10,'(A,I10,A)')'POINTS',n_nodes,' float'

! write point data 
  do i=1,N_nodes
    write(10,8005)node_coordinates(i,2)+ox,node_coordinates(i,3)+oy,0d0
  end do
        
! write element data (note node numbering starts from 0 in vtk format)
  write(10,'(A,2I10)')'POLYGONS',n_elements,n_elements*4
  do i=1,N_elements
    write(10,8010)3,Element_data(i,2)-1,Element_data(i,3)-1,Element_data(i,4)-1
  end do
  
! STAGE 6. write data associated with points

! write point based data
  write(10,'(A,I10)')'POINT_DATA ',n_nodes
  write(10,'(A)')'SCALARS Field_on_cells float 1'
  write(10,'(A)')'LOOKUP_TABLE field_on_cells_table'
  
  do i=1,N_nodes
    write(10,8020)real(0d0)
  end do
  
  close(unit=10)
  
end if ! plot_mesh

! format specifications for potential and mesh outputs
8005  format(3E14.5)
8010  format(I3,4I12)
8020  format(E14.5) 

! STAGE 18: deallocate memory 

if (verbose) write(*,*)' Deallocate memory'

DEALLOCATE ( node_coordinates )
DEALLOCATE ( Element_data )
DEALLOCATE ( N_elements_boundary )
DEALLOCATE ( N_nodes_boundary )

do i=1,N_boundaries_max
  if (allocated( boundary_info(i)%boundary_elements )) DEALLOCATE ( boundary_info(i)%boundary_elements )
  if (allocated( boundary_info(i)%boundary_nodes ))    DEALLOCATE ( boundary_info(i)%boundary_nodes)
end do
DEALLOCATE ( boundary_info )

DEALLOCATE( boundary_segment_to_boundary_number_list )

DEALLOCATE ( Mat_Prop )
DEALLOCATE ( Node_Type )

DEALLOCATE ( Node_to_Known_Unknown )
DEALLOCATE ( Vector_of_Knowns )

DEALLOCATE ( V )

DEALLOCATE( b )
DEALLOCATE( c )
DEALLOCATE( delta )
DEALLOCATE( eps_r )
     
DEALLOCATE( i_K )
DEALLOCATE( j_K )
DEALLOCATE( s_K )
if ( ALLOCATED(s_K_re) ) DEALLOCATE( s_K_re )

DEALLOCATE( i_K_rhs )
DEALLOCATE( j_K_rhs )
DEALLOCATE( s_K_rhs )

DEALLOCATE ( x )
DEALLOCATE ( x_tmp )
DEALLOCATE ( b_tmp )
DEALLOCATE ( v_tmp )
if ( ALLOCATED(K) ) DEALLOCATE ( K )
if ( ALLOCATED(KI) ) DEALLOCATE ( KI )
DEALLOCATE ( K_rhs )

DEALLOCATE( phi )

DEALLOCATE ( energy )
DEALLOCATE ( Capacitance_energy )
DEALLOCATE ( Conductance_energy )
DEALLOCATE ( Inductance_energy )

if (verbose) write(*,*)'FINISHED Laplace'

RETURN

9000 write(run_status,*)'ERROR in Laplace opening the mesh file:',trim(mesh_filename)
CALL write_program_status()
STOP 1

9010 write(run_status,*)'ERROR in Laplace reading the mesh file:',trim(mesh_filename)
CALL write_program_status()
STOP 1

9020 write(run_status,*)'ERROR in Laplace opening the boundary file:',boundary_file_name
CALL write_program_status()
STOP 1

9030 write(run_status,*)'ERROR in Laplace reading the boundary file:',boundary_file_name
CALL write_program_status()
STOP 1

END SUBROUTINE Laplace
!
!_________________________________________________________________________________
!
!
FUNCTION ismember(number,array,size,element2) RESULT(res)

! return 1 if number is found in the array, 0 otherwise

integer,intent(IN)     :: number,size,element2
integer,intent(IN)     :: array(1:size,1:2)
integer     :: res

! local variables

integer     :: i

! start

res=0

do i=1,size
  if (number.EQ.array(i,element2)) then
    res=1
    RETURN
  end if
end do

RETURN

END FUNCTION ismember