module finite_elements
  use utils
  use user_func
  use g_elimination
  use num_integration
  use ogpf

  implicit none

  abstract interface
    function f_interface(x)
      import :: wp
      real(wp), intent(in) :: x
      real(wp) :: f_interface
    end function f_interface
  end interface

  type, extends(fun) :: inner_product_l_obj
    integer :: i,j,n
    logical :: flag ! To decide weather linear or cubic spline
    real(wp), allocatable :: y(:) ! Nodes on the integration range

    ! Pointers to k and q function, as described by
    ! the formula.
    procedure(f_interface), pointer, nopass :: k,q

    ! Pointers to spline basis function (and derivative)
    procedure(f_interface), pointer, nopass :: s,ds

  contains
    procedure :: eval => inner_product_l_eval
  end type inner_product_l_obj

  type, extends(fun) :: inner_product_obj
    integer :: i,n
    logical :: flag ! To decide weather linear or cubic spline
    real(wp), allocatable :: y(:) ! Nodes on the integration range

    procedure(f_interface), pointer, nopass :: f

    ! Pointers to linear_spline functions
    procedure(f_interface), pointer, nopass :: s

  contains
    procedure :: eval => inner_product_eval
  end type inner_product_obj

contains

  ! Cubic spline calculation
  ! First two and last two basis
  ! need to be calculated in a
  ! different fashion from the
  ! regular ones
  function s_eval(x,y,i,s)
    real(wp) :: s_eval
    real(wp) :: h
    real(wp), intent(in) :: x,y(:)
    real(wp), external :: s
    integer, intent(in) :: i
    integer :: n

    n = size(y) - 2
    h = y(2) - y(1)

    if ( i == 0 ) then
      s_eval = s( x/h ) - 4*s( (x + h)/h )
    elseif ( i == 1 ) then
      s_eval = s( (x - y(2))/h ) - s( (x + h)/h )
    elseif ( i == n ) then
      s_eval = s( (x - y( n + 1 ))/h )&
              - s( (x - (n + 2)*h)/h )
    elseif ( i == n + 1 ) then
      s_eval = s( (x-y( n + 2 ))/h )&
              - 4*s( (x- (n + 2)*h)/h )
    else
      s_eval = s( (x - y(i + 1) )/h )
    end if
  end function s_eval

  function inner_product_l_eval(self, x)
    class(inner_product_l_obj), intent(in) :: self
    real(wp), intent(in) :: x
    real(wp) :: inner_product_l_eval
    real(wp) :: phi_i,phi_j,dphi_i,dphi_j,h
    integer :: k

    h = self%y(2) - self%y(1)

    if ( self%flag ) then
      ! Basis i
      phi_i = s_eval(x,self%y,self%i,self%s)
      ! Basis i
      phi_j = s_eval(x,self%y,self%j,self%s)
      ! Basis i derivative
      dphi_i = s_eval(x,self%y,self%i,self%ds)
      dphi_i = dphi_i/h
      ! Basis j derivative
      dphi_j = s_eval(x,self%y,self%j,self%ds)
      dphi_j = dphi_j/h
    else
      ! Basis i
      phi_i = self%s( (x-self%y( self%i+2 ) )/h )
      ! Basis j
      phi_j = self%s( (x-self%y( self%j+2 ) )/h )

      ! Basis i deriv
      if ( close(x,self%y(1),1.0_wp*1e-3) ) then
        dphi_i = 1.0/h
      elseif ( close(x,self%y(size(self%y)),1.0_wp*1e-3) ) then
        dphi_i = -1.0/h
      elseif ( x <= self%y( self%i+2 ) ) then
        dphi_i = 1.0/h
      elseif ( x <= self%y( self%i+3 ) ) then
        dphi_i = -1.0/h
      else
        dphi_i = 0.0
        !write(*,*) "Error ", x
      end if

      ! Basis j derivative
      if ( close(x,self%y(1),1.0_wp*1e-3) ) then
        dphi_j = 1.0/h
      elseif ( close(x,self%y(size(self%y)),1.0_wp*1e-3) ) then
        dphi_j = -1.0/h
      elseif ( x <= self%y( self%j+2 ) ) then
        dphi_j = 1.0/h
      elseif ( x <= self%y( self%j+3 ) ) then
        dphi_j = -1.0/h
      else
        dphi_j = 0.0/h
        !write(*,*) "Error ", x
      end if

    end if
    !write(*,*) "x ", x
    !write(*,*) "phi_i ", phi_i
    !write(*,*) "phi_j ", phi_j
    !write(*,*) "dphi_j ", dphi_i
    !write(*,*) "dphi_j ", dphi_j

    inner_product_l_eval = &
      self%k(x) * dphi_i * dphi_j &
      + self%q(x) * phi_i * phi_j
  end function inner_product_l_eval

  function inner_product_eval(self, x)
    class(inner_product_obj), intent(in) :: self
    real(wp), intent(in) :: x
    real(wp) :: inner_product_eval,h,phi_i
    integer :: k

    h = self%y(2) - self%y(1)
    if ( self%flag ) then
      phi_i = s_eval(x,self%y,self%i,self%s)

    else
      phi_i = self%s( ( x-self%y( self%i+2 ) )/h )

    end if

    inner_product_eval =  &
      self%f(x)*phi_i
  end function inner_product_eval

  subroutine set_inner_product_l(ipo,y,i,j,n,k,q,s,ds,flag)
    class(inner_product_l_obj), intent(inout) :: ipo
    real(wp), external :: q,k,s,ds
    real(wp), intent(in) :: y(:)
    integer, intent(in) :: i,j,n
    logical, intent(in) :: flag

    ipo%y = y
    ipo%i = i
    ipo%j = j
    ipo%n = n
    ipo%k => k
    ipo%q => q
    ipo%s => s
    ipo%ds => ds
    ipo%flag = flag
  end subroutine set_inner_product_l

  subroutine set_inner_product(ipo,y,i,n,f,s,flag)
    class(inner_product_obj), intent(inout) :: ipo
    real(wp), external :: f,s
    real(wp), intent(in) :: y(:)
    integer, intent(in) :: i,n
    logical, intent(in) :: flag

    ipo%y = y
    ipo%i = i
    ipo%n = n
    ipo%f => f
    ipo%s => s
    ipo%flag = flag
  end subroutine set_inner_product

  function estimated_function(a,y,x,s,flag)
    real(wp), intent(in) :: a(:) ! Coefficient vector
    real(wp), intent(in) :: x
    real(wp), intent(in) :: y(:) ! Integration nodes
    real(wp), external :: s ! Spline basis function
    logical, intent(in) :: flag ! True = Cubic; False = Linear
    real(wp) :: h ! Stepsize
    real(wp) :: estimated_function,phi_i
    integer :: i,n

    h = y(2) - y(1)
    estimated_function = 0.0

    if ( flag ) then
      do i = 0, size(a)-1
        phi_i = s_eval(x,y,i,s)

        ! Test
        !write(*,*) i
        !write(*,*) "phi_i ", phi_i
        !write(*,*) "a_i ", a(i+1)
        estimated_function = estimated_function + phi_i*a(i+1)
      end do
      !write(*,*)
    else
      do i = 0, size(a)-1
        phi_i = s( ( x-y( i+2 ) )/h )
        !write(*,*) "i ", i
        !write(*,*) "x ", y(i+2)
        !write(*,*) "phi_i ", phi_i
        !write(*,*) "a_i ", a(i+1)
        !write(*,*) "u_n ", estimated_function
        !write(*,*)
        estimated_function = estimated_function + phi_i*a(i+1)
      end do
      !write(*,*)
    end if
    !write(*,*)
  end function estimated_function

  subroutine solve_finite_elements(n,base_spline,base_spline_deriv,flag,romberg_iterations,error)
      integer, intent(in) :: n,romberg_iterations
      real(wp), intent(inout) :: error
      real(wp), external :: base_spline,base_spline_deriv
      ! Flag:
      ! If .true. cubic, if .false. linear
      logical, intent(in) :: flag
      integer :: i,j,l,o,m,ub,lb,ms,c
      integer :: sp_dim,total_num_nodes,num_support_nodes,total_num_support_nodes,&
      i_padding,j_padding,node_index,fix_range
      real(wp), parameter :: alpha = 0.0_wp
      real(wp), parameter :: omega = 1.0_wp
      real(wp) :: a,b,h,x,result,aux
      real(wp), allocatable :: integration_range(:),&
                                f_phi(:), coef(:),&
                                plot_expected(:),&
                                plot_calculated(:)
      type(inner_product_l_obj) :: ipol
      type(inner_product_obj) :: ipo
      type(col), allocatable :: Row(:)
      type(gpf):: gp

      write(*,*) "-------------------------"
      write(*,*) "Number of internal nodes:"
      write(*,*) n
      write(*,*)
      write(*,*) "Solving with:"

      ! Number of upper and lower bands
      ! ub = lb = 1 iff linear splines
      ! ub = lb = 3 iff cubic splines
      ! Number of support nodes
      ! Linear Spline = 3
      ! Cubic Spline = 5
      if ( flag ) then
        write(*,*) "Cubic Splines"
        write(*,*)
        ub = 3
        lb = 3
        ! Number of internal nodes
        num_support_nodes = 3
        ! Total number of nodes
        total_num_support_nodes = num_support_nodes+2
        ! Fix nodes range
        fix_range = 2
      else
        write(*,*) "Linear Splines"
        write(*,*)
        ub = 1
        lb = 1
        ! Number of internal nodes
        num_support_nodes = 1
        ! Total number of nodes
        total_num_support_nodes = num_support_nodes+2
        ! Fix nodes range
        fix_range = 1
      end if

      ! Total number of integration nodes
      ! total_num_nodes = internal nodes + 2
      total_num_nodes = n+2

      ! Dimension of the spline
      if ( flag ) then
        sp_dim = n+2
      else
        sp_dim = n
      end if

      ! Saving results for plots
      allocate(plot_expected(total_num_nodes))
      allocate(plot_calculated(total_num_nodes))

      ! Banded matrix structure
      allocate(Row(sp_dim))
      m = ub
      o = -1
      do i = 1, sp_dim
        if ( i <= lb+1 ) then
          o = o + 1
        elseif ( i > sp_dim-ub ) then
          m = m - 1
        end if
        allocate(Row(i)%col(m+o+1))
        Row(i)%col(m+o+1) = 0.0_wp
      end do

      ! Array for the result of the
      ! inner product between f and the splines
      allocate(f_phi(sp_dim))

      ! Coefficients alpha
      allocate(coef(sp_dim))
      coef = 0.0

      ! Stepsize
      h = 1.0_wp/(n+1.0_wp)
      write(*,*) "Step size"
      write(*,*) h
      write(*,*)

      ! Nodes in the integration range
      allocate(integration_range(total_num_nodes))

      do i = 0, total_num_nodes-1
        integration_range(i+1) = i*h
      end do
      write(*,*) "Integration range:"
      write(*,'(F10.5)') integration_range(1:3)
      write(*,*) "..."
      write(*,'(F10.5)') integration_range(total_num_nodes-2:total_num_nodes)

      write(*,*) "Creating banded matrix"
      write(*,*) ". . ."

      ! Index fixing
      i_padding = 0
      j_padding = 0

      do i = 0,sp_dim-1
        ! Define integration limits
        if ( flag ) then
          node_index = (i+1) - fix_range
          if ( node_index <= 0 ) then
            aux = alpha
          else
            aux = integration_range(node_index)
          end if
          a = max(aux, alpha)

          node_index = (i+1) + fix_range
          if ( node_index > total_num_nodes ) then
            aux = omega
          else
            aux = integration_range(node_index)
          end if
          b = min(aux, omega)
        else
          a = integration_range(i+1)
          b = integration_range(i+3)
        end if

        call set_inner_product(ipo,integration_range,i,n,&
                              f,base_spline,flag)
        ! Test
        !write(*,*) "i ", i
        !write(*,*) "node ", node_index
        !write(*,'(A,F10.5)') "a ", a
        !write(*,*) "node ", node_index
        !write(*,'(A,F10.5)') "b ", b
        !write(*,*)

        f_phi(i+1) = romberg(ipo, a, b, romberg_iterations)

        if ( i >= lb ) then
          i_padding = i-lb
        end if
        do j = i,min(i+num_support_nodes,sp_dim-1)

          ! Define integration limits
          if ( flag ) then
            node_index = (j+1) - fix_range
            if ( node_index <= 0 ) then
              aux = alpha
            else
              aux = integration_range(node_index)
            end if
            a = max(aux, alpha)

            node_index = (i+1) + fix_range
            if ( node_index > total_num_nodes ) then
              aux = omega
            else
              aux = integration_range(node_index)
            end if
            b = min(aux, omega)
          else
            a = integration_range(j+1)
            b = integration_range(i+3)
          end if

          ! Set values the inner product L,
          ! according to the given formula
          call set_inner_product_l(ipol,&
                              integration_range,i,j,n,&
                              k,q,&
                              base_spline,base_spline_deriv,&
                              flag)
          ! Test
          write(*,*) "j ", j
          write(*,*) "node ", node_index
          write(*,'(A,F10.5)') "a ", a
          write(*,*) "i ", i
          write(*,*) "node ", node_index
          write(*,'(A,F10.5)') "b ", b
          write(*,*)

          ! Integrate inner product using Romberg's
          ! Save result in the banded matrix structure
          aux = romberg(ipol, a, b, romberg_iterations)

          Row(i+1)%col(j+1-i_padding) = aux
          ! Because of symmetry
          if ( i /= j ) then
            if ( j >= lb ) then
              j_padding = j-lb
            end if
            Row(j+1)%col(i+1-j_padding) = aux
          end if
        end do
        ! Test
        !call pprint_band(Row,lb,ub)
        !call print_band(Row,lb,ub)
      end do
      write(*,*) "Banded matrix creation complete"
      write(*,*)

      ! Test
      !call pprint_band(Row,lb,ub)
      write(*,*) "A"
      call print_band(Row,lb,ub)
      write(*,*)
      !write(*,'(F10.5)') coef
      !write(*,*)
      write(*,*) "b"
      write(*,*)
      write(*,'(F10.5)') f_phi

      write(*,*) "Performing gaussian elimination"
      write(*,*) ". . ."
      call gaussian_elimination_banded(Row,coef,f_phi,lb,ub)
      write(*,*) "Gaussian elimination complete"
      write(*,*)

      ! Test
      !call pprint_band(Row,lb,ub)
      !call print_band(Row,lb,ub)
      !write(*,*)
      !write(*,'(F10.5)') coef
      !write(*,*)
      !write(*,'(F10.5)') f_phi
      !write(*,*)

      write(*,*)
      write(*,*) "Calculating Error`s Infinity Norm"
      write(*,*) ". . ."

      error = 0.0_wp
      do i = 1, total_num_nodes

        x = integration_range(i)
        ! Test
        !write(*,*) x
        plot_expected(i) = u(x)

        plot_calculated(i) = estimated_function(coef,integration_range,x,base_spline,flag)

        if ( abs(plot_calculated(i)-plot_expected(i)) > error ) then
          error = abs(plot_calculated(i)-plot_expected(i))
        end if

      end do
      write(*,*) "Error: ",error
      write(*,*)

      write(*,*) "Now plotting"
      write(*,*) ". . ."
      if ( flag ) then
        call gp%title('Cubic Spline')
      else
        call gp%title('Linear Spline')
      end if
      call gp%options('set key top right; set grid')
      call gp%plot(x1=integration_range,&
              y1=plot_expected,&
              ls1='title "Expected" with lines lt 1 lw 1',&
              x2=integration_range,&
              y2=plot_calculated,&
              ls2='title "Calculated" with lines lt 2 lw 1')

      write(*,*) "Plotting done"
      write(*,*)
      write(*,*) "Problem solved"
      write(*,*) "---------------"
      write(*,*)
    end subroutine solve_finite_elements
end module finite_elements


c
c     -------------------------------------------------------------
c
      subroutine errout19(rect,maxip1,maxjp1,nvar,mptr,irr,
     1     mitot,mjtot,lwidth,lstgrd,
     2     dx,dy,xlow,ylow)
c
      implicit double precision (a-h,o-z)
      include "cirr.i"
      dimension rect(maxip1,maxjp1,nvar),irr(mitot,mjtot)
      dimension err(20), soln(20), berr(20), bsoln(20)
      dimension errmax(20), solnmax(20)
      common /userdt/ cfl,gamma,gamma1,xprob,yprob,alpha,Re,iprob,
     .     ismp,gradThresholdx

c
c     compute error in final solution in rect.
c     note different dimensions for rect and locirr - this version does not
c     use enlarged grid for all amr stuff
c
      do ivar = 1, nvar
         err(ivar)   = 0.d0
         soln(ivar)  = 0.d0
         berr(ivar)  = 0.d0
         bsoln(ivar) = 0.d0
         errmax(ivar) = 0.d0
      end do

      maxi = maxip1 - 1
      maxj = maxjp1 - 1
      xlowb = xlow - lwidth*dx
      ylowb = ylow - lwidth*dy
      write(6,100) mptr
 100  format(//,'L1 error/soln/rel.err for grid ',i5)
      do 15 i = 2, maxi
         do 15 j = 2, maxj
            if (iprob .eq. 19 .or. iprob .eq. 15) then
               iadj = i - 1 + lwidth
               jadj = j - 1 + lwidth
               kirr = irr(iadj,jadj)
               if (kirr .eq. lstgrd) then
                  call makep(poly(1,1,kirr),iadj,jadj,xlowb,ylowb,dx,dy)
                  xcen = xlow + (dfloat(i)-1.5)* dx
                  ycen = ylow + (dfloat(j)-1.5)* dy
               else if (kirr .ne. -1) then
                  xcen = xcirr(kirr)
                  ycen = ycirr(kirr)
               endif
               if (kirr .ne. -1) then
                  if (iprob .eq. 19) then
                    call p19tru(xcen,ycen,rho,u,v,p,poly(1,1,kirr),kirr)
                  elseif (iprob .eq. 15) then
                     call p15tru(xcen,ycen,rho,u,v,p,poly(1,1,kirr))
                  endif
               endif
            endif
            if (irr(i+lwidth-1,j+lwidth-1) .ne. -1) then
              pcomp =.4*(rect(i,j,4)-.5*(rect(i,j,2)**2+rect(i,j,3)**2)/
     .              rect(i,j,1))
              rhocomp = rect(i,j,1)
              err(1)  = err(1) + abs(rhocomp - rho)*ar(kirr)
              err(2)  = err(2) + abs(rect(i,j,2)/rhocomp - u)*ar(kirr)
              err(3)  = err(3) + abs(rect(i,j,3)/rhocomp - v)*ar(kirr)
              err(4)  = err(4) + abs(pcomp - p)*ar(kirr)
              soln(1) = soln(1) + abs(rho)  *ar(kirr)
              soln(2) = soln(2) + abs(u)  *ar(kirr)
              soln(3) = soln(3) + abs(v)*ar(kirr)
              soln(4) = soln(4) + abs(p)  *ar(kirr)
              if (irr(i+lwidth-1,j+lwidth-1) .ne. lstgrd) then ! add to bndry err calc
c     rl1d = sqrt(ar(kirr))
                  do kside=1,10
                     if (poly(kside+2,1,kirr) .eq. -11) then
                        x1 = poly(kside,1,kirr)
                        y1 = poly(kside,2,kirr)
                        x2 = poly(kside+1,1,kirr)
                        y2 = poly(kside+1,2,kirr)
                        go to 25
                     endif
                  end do
 25               rl1d = sqrt((x1-x2)**2 + (y1-y2)**2)
                  berr(1)  = berr(1) + abs(rhocomp - rho)*rl1d
                  berr(2)  = berr(2) + abs(rect(i,j,2)/rhocomp - u)*rl1d
                  berr(3)  = berr(3) + abs(rect(i,j,3)/rhocomp - v)*rl1d
                  berr(4)  = berr(4) + abs(pcomp - p)*rl1d
                  bsoln(1) = bsoln(1) + abs(rho)*rl1d
                  bsoln(2) = bsoln(2) + abs(u)*rl1d
                  bsoln(3) = bsoln(3) + abs(v)*rl1d
                  bsoln(4) = bsoln(4) + abs(p)*rl1d
               endif
c
               errmax(1) = max(errmax(1),abs(rhocomp-rho))
               errmax(2) = max(errmax(2),abs(rect(i,j,2)/rhocomp - u))
               errmax(3) = max(errmax(3),abs(rect(i,j,3)/rhocomp - v))
               errmax(4) = max(errmax(4),abs(pcomp - p))
               solnmax(1) = max(solnmax(1),abs(rho))
               solnmax(2) = max(solnmax(2),abs(u))
               solnmax(3) = max(solnmax(3),abs(v))
               solnmax(4) = max(solnmax(4),abs(p))

            endif
 15      continue
c
         write(6,101) (err(m),soln(m),err(m)/soln(m),m=1,nvar)
 101     format("rho   ",3e15.7,/,"u vel ",3e15.7,/,"v vel ",3e15.7,/,
     .        "press ",3e15.7)
c
         write(6,102)  mptr
 102     format(//,'L1 bndry. error/soln/rel.err for grid ',i5,
     .        ' wghted by bndry seg')
         write(6,101) (berr(m),bsoln(m),err(m)/bsoln(m),m=1,nvar)

         write(6,103) (errmax(m),solnmax(m),m=1,nvar)
 103     format(/,"max         error         soln",/,
     .        " rho   ",2e15.7,/,"  u    ",2e15.7,/,
     .        "  v    ",2e15.7,/," press ",2e15.7,//)
c
         return
         end


!++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++!
!                          Futility Development Group                          !
!                             All rights reserved.                             !
!                                                                              !
! Futility is a jointly-maintained, open-source project between the University !
! of Michigan and Oak Ridge National Laboratory.  The copyright and license    !
! can be found in LICENSE.txt in the head directory of this repository.        !
!++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++!
PROGRAM testGeom
#include "UnitTest.h"
USE ISO_FORTRAN_ENV
USE UnitTest
USE IntrType
USE Constants_Conversion
USE ParameterLists
USE Geom

IMPLICIT NONE

TYPE(ParamType) :: params
TYPE(PointType) :: point,point2,point3
TYPE(LineType) :: line1
TYPE(PlaneType) :: plane1
TYPE(CircleType) :: circle1
TYPE(CylinderType) :: cylinder1
TYPE(OBBoxType) :: box
TYPE(PolygonType) :: poly
REAL(SRK) :: e_3d(3)
REAL(SRK) :: u1_3d(3),u2_3d(3),u3_3d(3)
REAL(SRK) :: n(3)
LOGICAL(SBK) :: bool

CREATE_TEST('Test Geom')
CALL eParams%setQuietMode(.TRUE.)
CALL eParams%setStopOnError(.FALSE.)

REGISTER_SUBTEST('Test Geom',testGeoms)

FINALIZE_TEST()
!
!===============================================================================
CONTAINS
!
!-------------------------------------------------------------------------------
SUBROUTINE testGeoms
  !Test newGeom for OBBoxType
  CALL point%init(DIM=3,X=1._SRK,Y=1._SRK,Z=1._SRK)
  e_3d=(/SQRT(2._SRK),2*SQRT(2._SRK),1._SRK/)
  u1_3d=(/1._SRK,1._SRK,0._SRK/)
  u2_3d=(/-1._SRK,1._SRK,0._SRK/)
  u3_3d=(/0._SRK,0._SRK,2._SRK/)
  CALL newGeom(params,box)
  bool = .NOT.(ANY(box%u /= 0.0_SRK) .OR. ANY(box%e /= 0.0_SRK) &
      .OR. box%p0%dim /= 0 .OR. ALLOCATED(box%p0%coord))
  ASSERT(bool, 'CALL newGeom(params,box) (empty params)')
  CALL params%add('BoxGeom -> CornerPoint',1)
  CALL params%add('BoxGeom -> Vector1',5)
  CALL params%add('BoxGeom -> Vector2',1)
  CALL params%add('BoxGeom -> ExtentVector',0)
  CALL newGeom(params,box)
  bool = .NOT.(ANY(box%u /= 0.0_SRK) .OR. ANY(box%e /= 0.0_SRK) &
      .OR. box%p0%dim /= 0 .OR. ALLOCATED(box%p0%coord))
  ASSERT(bool, 'CALL newGeom(params,box) (bad params)')
  CALL params%clear()
  CALL params%add('BoxGeom -> CornerPoint',point%coord)
  CALL params%add('BoxGeom -> Vector1',u1_3d)
  CALL params%add('BoxGeom -> Vector2',u2_3d)
  CALL params%add('BoxGeom -> Vector3',u3_3d)
  CALL params%add('BoxGeom -> ExtentVector',e_3d)
  CALL newGeom(params,box)
  bool = .NOT.(box%p0 /= point .OR. ANY(box%e /= e_3d) &
      .OR. ANY(.NOT.(box%u(:,1) .APPROXEQ. u1_3d/SQRT(2._SRK))) &
      .OR. ANY(.NOT.(box%u(:,2) .APPROXEQ. u2_3d/SQRT(2._SRK))) &
      .OR. ANY(.NOT.(box%u(:,3) .APPROXEQ. u3_3d*0.5_SRK)) )
  ASSERT(bool, 'CALL newGeom(params,box)')
  CALL params%clear()

  !Test newGeom for circle and cylinder types
  CALL point%clear()
  CALL point%init(DIM=2,X=0.1_SRK,Y=0.2_SRK)
  CALL newGeom(params,circle1)
  bool=.NOT.(circle1%r /= 0.0_SRK .OR. circle1%c%dim /= 0 .OR. &
      ALLOCATED(circle1%c%coord))
  ASSERT(bool, 'CALL newGeom(params,circle1) (empty params)')
  CALL params%add('CircleGeom -> Radius',1)
  CALL params%add('CircleGeom -> Centroid',2)
  CALL params%add('CircleGeom -> StartingAngle',.FALSE.)
  CALL params%add('CircleGeom -> StoppingAngle',(/0.0,0.0/))
  CALL newGeom(params,circle1)
  bool=.NOT.(circle1%r /= 0.0_SRK .OR. circle1%c%dim /= 0 .OR. &
      ALLOCATED(circle1%c%coord))
  ASSERT(bool, 'CALL newGeom(params,circle1) (bad params)')
  CALL params%clear()
  CALL params%add('CircleGeom -> Radius',0.5_SRK)
  CALL params%add('CircleGeom -> Centroid',point%coord)
  CALL newGeom(params,circle1)
  bool=.NOT.(circle1%r /= 0.5_SRK .OR. circle1%c%dim /= 2 .OR. &
      circle1%c%coord(1) /= 0.1_SRK .OR. circle1%c%coord(2) /= 0.2_SRK)
  ASSERT(bool, 'CALL newGeom(params,circle1)')
  CALL params%clear()
  CALL point3%clear()
  CALL point2%clear()
  CALL point2%init(DIM=3,X=0.1_SRK,Y=0.2_SRK,Z=0.3_SRK)
  CALL point3%init(DIM=3,X=0.1_SRK,Y=0.2_SRK,Z=1.3_SRK)
  CALL newGeom(params,cylinder1)
  bool=.NOT.(cylinder1%r /= 0.0_SRK .OR. cylinder1%axis%p1%dim /= 0 .OR. &
      ALLOCATED(cylinder1%axis%p1%coord) .OR. cylinder1%axis%p2%dim /= 0 .OR. &
      ALLOCATED(cylinder1%axis%p2%coord))
  ASSERT(bool,'CALL newGeom(params,cylinder1) (empty params)')
  CALL params%add('CylinderGeom -> Radius',1)
  CALL params%add('CylinderGeom -> BottomCentroid',2)
  CALL params%add('CylinderGeom -> TopCentroid',3)
  CALL params%add('CylinderGeom -> StartingAngle',.FALSE.)
  CALL params%add('CylinderGeom -> StoppingAngle',(/0.0,0.0/))
  CALL newGeom(params,cylinder1)
  bool=.NOT.(cylinder1%r /= 0.0_SRK .OR. cylinder1%axis%p1%dim /= 0 .OR. &
      ALLOCATED(cylinder1%axis%p1%coord) .OR. cylinder1%axis%p2%dim /= 0 .OR. &
      ALLOCATED(cylinder1%axis%p2%coord))
  ASSERT(bool,'CALL newGeom(params,cylinder1) (bad params)')
  CALL params%clear()
  CALL params%add('CylinderGeom -> Radius',1.0_SRK)
  CALL params%add('CylinderGeom -> BottomCentroid',point2%coord)
  CALL params%add('CylinderGeom -> TopCentroid',point3%coord)
  CALL newGeom(params,cylinder1)
  bool=.NOT.(cylinder1%r /= 1.0_SRK .OR. cylinder1%axis%p1%dim /= 3 .OR. &
      ANY(cylinder1%axis%p1%coord /= (/0.1_SRK,0.2_SRK,0.3_SRK/)) .OR. &
      cylinder1%axis%p2%dim /= 3 .OR. &
      ANY(cylinder1%axis%p2%coord /= (/0.1_SRK,0.2_SRK,1.3_SRK/)))
  ASSERT(bool,'CALL newGeom(params,cylinder1)')
  CALL params%clear()
  CALL cylinder1%clear()

  !Test newGeom for line Type
  CALL point2%clear()
  CALL point3%clear()
  CALL point2%init(DIM=3,X=0.1_SRK,Y=0.2_SRK,Z=0.3_SRK)
  CALL point3%init(DIM=3,X=0.4_SRK,Y=0.5_SRK,Z=0.6_SRK)
  CALL newGeom(params,line1)
  bool=.NOT.(line1%p1%dim /= 0 .OR. line1%p2%dim /= 0 .OR. &
      ALLOCATED(line1%p1%coord) .OR. ALLOCATED(line1%p2%coord))
  ASSERT(bool, 'CALL newGeom(params,line1) (empty params)')
  CALL params%add('LineGeom->StartPoint',1)
  CALL params%add('LineGeom->EndPoint',3)
  CALL newGeom(params,line1)
  bool=.NOT.(line1%p1%dim /= 0 .OR. line1%p2%dim /= 0 .OR. &
      ALLOCATED(line1%p1%coord) .OR. ALLOCATED(line1%p2%coord))
  ASSERT(bool, 'CALL newGeom(params,line1) (badparams)')
  CALL params%clear()
  CALL params%add('LineGeom->StartPoint',point2%coord)
  CALL params%add('LineGeom->EndPoint',point3%coord)
  CALL newGeom(params,line1)
  bool=.NOT.(line1%p1%dim /= 3 .OR. line1%p2%dim /= 3 .OR. &
      line1%p1%coord(1) /= 0.1_SRK .OR. line1%p1%coord(2) /= 0.2_SRK .OR. &
      line1%p1%coord(3) /= 0.3_SRK .OR. line1%p2%coord(1) /= 0.4_SRK .OR. &
      line1%p2%coord(2) /= 0.5_SRK .OR. line1%p2%coord(3) /= 0.6_SRK)
  ASSERT(bool, 'CALL newGeom(params,line1)')
  CALL params%clear()
  CALL line1%clear()

  !Test newGeom for plane
  n=(/1.0_SRK,1.0_SRK,1.0_SRK/)
  CALL point%clear()
  CALL point%init(COORD=(/0.5_SRK,0.5_SRK,0.5_SRK/))
  CALL newGeom(params,plane1)
  bool = .NOT.(ANY(plane1%n /= 0.0_SRK) .OR. plane1%v0%dim /= 0 .OR. &
      ALLOCATED(plane1%v0%coord))
  ASSERT(bool, 'CALL newGeom(params,plane1) (empty params)')
  CALL params%add('PlaneGeom->Point',1)
  CALL params%add('PlaneGeom->NormalVector',3)
  CALL newGeom(params,plane1)
  bool = .NOT.(ANY(plane1%n /= 0.0_SRK) .OR. plane1%v0%dim /= 0 .OR. &
      ALLOCATED(plane1%v0%coord))
  ASSERT(bool, 'CALL newGeom(params,plane1) (bad params)')
  CALL params%clear()
  CALL params%add('PlaneGeom->Point',point%coord)
  CALL params%add('PlaneGeom->NormalVector',n)
  CALL newGeom(params,plane1)
  bool = .NOT.(ANY(plane1%v0%coord /= 0.5_SRK) .OR. ANY(.NOT.(plane1%n .APPROXEQ. &
      (/1._SRK/SQRT(3.0_SRK),1._SRK/SQRT(3.0_SRK),1._SRK/SQRT(3.0_SRK)/))))
  ASSERT(bool, 'CALL newGeom(params,plane1)')

  !Test newGeom for Polygon
  CALL params%add('PolygonGeom->nVert',7)
  CALL params%add('PolygonGeom->vertex 1',(/-3.0_SRK,3.0_SRK/))
  CALL params%add('PolygonGeom->vertex 2',(/3.0_SRK,3.0_SRK/))
  CALL params%add('PolygonGeom->vertex 3',(/3.0_SRK,0.0_SRK/))
  CALL params%add('PolygonGeom->vertex 4',(/2.0_SRK,2.0_SRK/))
  CALL params%add('PolygonGeom->vertex 5',(/2.0_SRK,-2.0_SRK/))
  CALL params%add('PolygonGeom->vertex 6',(/-2.0_SRK,-2.0_SRK/))
  CALL params%add('PolygonGeom->vertex 7',(/-2.0_SRK,2.0_SRK/))
  CALL newGeom(params,poly)
  ASSERTFAIL(poly%isinit,'%isinit')
  bool=(poly%nVert == 7) .AND. &
      ALL(poly%vert(1)%coord .APPROXEQA. (/-3.0_SRK,3.0_SRK/)) .AND. &
      ALL(poly%vert(2)%coord .APPROXEQA. (/3.0_SRK,3.0_SRK/)) .AND. &
      ALL(poly%vert(3)%coord .APPROXEQA. (/3.0_SRK,0.0_SRK/)) .AND. &
      ALL(poly%vert(4)%coord .APPROXEQA. (/2.0_SRK,2.0_SRK/)) .AND. &
      ALL(poly%vert(5)%coord .APPROXEQA. (/2.0_SRK,-2.0_SRK/)) .AND. &
      ALL(poly%vert(6)%coord .APPROXEQA. (/-2.0_SRK,-2.0_SRK/)) .AND. &
      ALL(poly%vert(7)%coord .APPROXEQA. (/-2.0_SRK,2.0_SRK/))
  ASSERT(bool,'CALL newGeom(params,poly) (simple)')
ENDSUBROUTINE
ENDPROGRAM testGeom