! See copyright notice in the COPYRIGHT file.
! This utest program tests the compact quadratic velocity interpolation 2D
program mus_intp_compact2D_test
  use env_module,         only: rk, eps
  use tem_general_module, only: tem_start, tem_finalize, tem_general_type

  use mus_interpolate_d2q9_module, only: mus_interpolate_compact2D

  implicit none

  logical :: error
  real(kind=rk) :: tolerance

  type( tem_general_type ) :: general
  real(kind=rk) :: a(0:5), b(0:5)
  real(kind=rk) :: c
  integer :: iVal

  call tem_start( 'Quadratic compact test', 'utest', general )
  error = .false.
  tolerance = eps * 2500._rk
  write(*,*) 'tolerance = ', tolerance

  do iVal = -2, 2
    c = 1.0_rk * iVal
    a = [   c,  c,  c,  c,  c,  c ]
    b = [   c,  c,  c,  c,  c,  c ]
    call test_interpolation( a, b )
  end do

  a = [   1, -3,  5, -7,  11,  -13 ]
  b = [  -1,  3, -5,  7, -11,   13 ]
  call test_interpolation( a, b )

  call tem_finalize(general)
  write(*,'(A)') 'PASSED'

contains

  function evalPoly2D( c, x, y ) result ( f )
    real(kind=rk), intent(in) :: c(0:5)
    real(kind=rk), intent(in) :: x, y
    real(kind=rk) :: f

    f   =   c(0)         &
      &   + c(1) * x     &
      &   + c(2) * y     &
      &   + c(3) * x * x &
      &   + c(4) * y * y &
      &   + c(5) * x * y

  end function evalPoly2D

  ! Velocity (Ux, Uy) is assumed to be quadratic polynomial function
  ! by coefficients a(0:5) and b(0:5)
  ! Ux = a0 + a1 * x + a2 * y + a3 * x * x + a4 * y * y + a5 * x * y
  ! Uy = b0 + b1 * x + b2 * y + b3 * x * x + b4 * y * y + b5 * x * y
  ! Strain rate (S) thus can be represented by the following:
  ! Sxx = dUx/dx = a1 + 2 * a3 * x + a5 * y
  ! Syy = dUy/dy = b2 + 2 * b4 * y + b5 * x
  ! Sxy = (dUx/dy + dUy/dx) / 2
  !     = ( a2 + 2 * a4 * y + a5 * x + b1 + 2 * b3 * x + b5 + y ) * 0.5
  !
  ! This routine first evaluate the velocity at parent (center point) by
  ! polynomial function. Then it utilizes the mus_quadratic_compact2D to evaluate
  ! its velocity again. Finally compare these two results.
  ! 
  subroutine test_interpolation( a, b )

    real(kind=rk), intent(in) :: a(0:5), b(0:5)

    real(kind=rk) :: cSxx(0:5), cSyy(0:5), cSxy(0:5)
    real(kind=rk) :: vel(2,4), S(3,4)
    ! value of parent
    real(kind=rk) :: velPoly(2), velCompact(2), errorVel(2)
    real(kind=rk) :: x, y
    integer :: iVal, jVal, iElem
    logical :: error

    error = .false.

    cSxx = [ a(1), 2*a(3),   a(5), 0._rk, 0._rk, 0._rk ]
    cSyy = [ b(2),   b(5), 2*b(4), 0._rk, 0._rk, 0._rk ]
    cSxy = [ (a(2)+b(1)), (a(5)+2*b(3)), (2*a(4)+b(5)), 0._rk, 0._rk, 0._rk ] * 0.5_rk

    ! print out polynomial coefficients
    write(*,"(A)") 'Print polynomial coefficients:'
    do iVal = 0, 5
      write(*,"(5A10)")   'a(ux)', 'b(uy)', 'cSxx', 'cSyy','cSxy'
      write(*,"(5F10.5)") a(iVal), b(iVal), cSxx(iVal), cSyy(iVal), cSxy(iVal)
    end do
    write(*,"(A)") ''

    ! evaluate polynomial at four children
    write(*,"(A)") 'Evaluate polynomial at four children position:'
    vel = 0.0_rk
    S   = 0.0_rk
    iElem = 0
    do iVal = 1, 2
      do jVal = 1, 2
        iElem = iElem + 1
        x = real( (jVal - 1), rk )
        y = real( (iVal - 1), rk )
        vel(1,iElem) = evalPoly2D( a,  x, y )
        vel(2,iElem) = evalPoly2D( b,  x, y )
          S(1,iElem) = evalPoly2D( cSxx, x, y )
          S(2,iElem) = evalPoly2D( cSyy, x, y )
          S(3,iElem) = evalPoly2D( cSxy, x, y )
        write(*,"(6A10)")  'iElem', 'vel', 'vel', 'Sxx', 'Syy', 'Sxy'
        write(*,"(I10, 5F10.5)") ielem, vel(1,iElem), vel(2,iElem), S(1,iElem), S(2,iElem), S(3,iElem)
      end do
    end do
    write(*,"(A)") ''

    ! evaluate polynomial at Parent (0.5, 0.5)
    x = 0.5_rk
    y = 0.5_rk
    velPoly(1) = evalPoly2D( a,  x, y )
    velPoly(2) = evalPoly2D( b,  x, y )

    ! evaluate parent velocity by compact interpolation
    velCompact = mus_interpolate_compact2D( vel, S, x,y )

    write(*,*) "Calculating the error of velX velY"
    errorVel = abs( velPoly - velCompact )
    write(*,"(3A13)")   'velPoly x', 'velCompact x', 'error'
    write(*,"(3F13.5)") velPoly(1), velCompact(1), errorVel(1)
    write(*,"(3A13)")   'velPoly y', 'velCompact y', 'error'
    write(*,"(3F13.5)") velPoly(2), velCompact(2), errorVel(2)

    if ( errorVel(1) > tolerance ) then
      error = .true.
      write(*,*) " Discrepency exceeds tolerance!"
      write(*,*) "   Discrepency = ", errorVel(1)
      write(*,*) "   tolerance   = ", tolerance
    end if

    if ( errorVel(2) > tolerance ) then
      error = .true.
      write(*,*) " Discrepency exceeds tolerance!"
      write(*,*) "   Discrepency = ", errorVel(2)
      write(*,*) "   tolerance   = ", tolerance
    end if

    if ( error ) then
      write(*,'(A)') 'Error appears when'
      write(*,'(A,6F6.3)') 'a: ', a(0:5)
      write(*,'(A,6F6.3)') 'b: ', b(0:5)
      write(*,'(A)') 'FAILED'
      stop
    end if

  end subroutine test_interpolation

end program mus_intp_compact2D_test
!******************************************************************************
