!----------------------------------------------------------------------
! Copyright (c) 2009 SPMODEL Development Group. All rights reserved.!
!----------------------------------------------------------------------
!
!表題  lumatrix テストプログラム
!
!履歴  2009/08/06  竹広真一
!      2009/10/24  佐々木洋平 幾つかの数値代入を倍精度に変更
!                  dc_test を使用するように修正
!
program lumatrix_test

  use lumatrix
  use dc_types, only: DP
  use dc_message, only: MessageNotify
  use dc_test, only: AssertEqual, AssertGreaterThan


  integer,parameter  :: idim=2, jdim=2, ndim=2

  real(DP) :: alu3(jdim,ndim,ndim)
  real(DP) :: b3(idim,jdim,ndim)
  real(DP) :: b3sol(idim,jdim,ndim)
  integer :: kp2(jdim,ndim)
  real(DP) :: b2(jdim,ndim)
  real(DP) :: b2sol(jdim,ndim)

  real(DP) :: alu2(ndim,ndim)
  integer :: kp1(ndim)
  real(DP) :: bb2(idim,ndim)
  real(DP) :: bb2sol(idim,ndim)
  real(DP) :: bb1(ndim)
  real(DP) :: bb1sol(ndim)

  real(DP) :: alu3_A0(jdim,ndim,ndim)
  real(DP) :: b3_B0(idim,jdim,ndim)
  real(DP) :: b2_B0(jdim,ndim)
  real(DP) :: alu2_A0(ndim,ndim)
  real(DP) :: bb2_B0(idim,ndim)
  real(DP) :: bb1_B0(ndim)

  real(DP), parameter :: tol = 1.0d-12

  ! 判定誤差設定
  integer, parameter :: check_digits = 15
  integer, parameter :: ignore = -16

  call MessageNotify('M','lumatrix_test','lumatrix test')

  alu3(1,1,:) = (/1.0d0,1.0d0/)
  alu3(1,2,:) = (/1.0d0,0.0d0/)
  alu3(2,1,:) = (/0.0d0,1.0d0/)
  alu3(2,2,:) = (/1.0d0,-1.0d0/)

  b3(1,1,:)     = (/2.0d0,0.0d0/)
  b3(2,1,:)     = (/4.0d0,0.0d0/)
  b3(1,2,:)     = (/2.0d0,0.0d0/)
  b3(2,2,:)     = (/4.0d0,0.0d0/)

  b3sol(1,1,:)     = (/0.0d0,2.0d0/)
  b3sol(2,1,:)     = (/0.0d0,4.0d0/)
  b3sol(1,2,:)     = (/2.0d0,2.0d0/)
  b3sol(2,2,:)     = (/4.0d0,4.0d0/)

  b2(1,:)     = (/2.0d0,0.0d0/)
  b2(2,:)     = (/2.0d0,0.0d0/)

  b2sol(1,:)     = (/0.0d0,2.0d0/)
  b2sol(2,:)     = (/2.0d0,2.0d0/)

  alu3_A0 = alu3
  b3_B0   = b3
  b2_B0   = b2

  call ludecomp(alu3,kp2)

  b3=lusolve(alu3,kp2,b3)

  call AssertEqual(&
    & message = 'Test 323',                                        &
    & answer  = b3sol,                                             &
    & check   = b3,                                                &
    & significant_digits = check_digits, ignore_digits = ignore    &
    )

  call Assert_Residual_323(&
    & 'Test 323', alu3_A0, b3, b3_B0, tol)

  b2=lusolve(alu3,kp2,b2)

  call AssertEqual(&
    message = 'Test 322',                                         &
    answer  = b2sol,                                              &
    check   = b2,                                                 &
    significant_digits = check_digits, ignore_digits = ignore     &
    )

  call Assert_Residual_322('Test 322', alu3_A0, b2, b2_B0, tol)

  alu2(1,:) = (/1.0d0,1.0d0/)
  alu2(2,:) = (/1.0d0,0.0d0/)

  bb2(1,:)     = (/2.0d0,0.0d0/)
  bb2(2,:)     = (/4.0d0,0.0d0/)

  bb2sol(1,:)  = (/0.0d0,2.0d0/)
  bb2sol(2,:)  = (/0.0d0,4.0d0/)

  bb1(:)     = (/2.0d0,0.0d0/)
  bb1sol(:)  = (/0.0d0,2.0d0/)

  alu2_A0 = alu2
  bb2_B0  = bb2
  bb1_B0  = bb1

  call ludecomp(alu2,kp1)

  bb2=lusolve(alu2,kp1,bb2)

  call AssertEqual(&
    message = 'Test 212',                                         &
    answer  = bb2sol,                                             &
    check   = bb2,                                                &
    significant_digits = check_digits, ignore_digits = ignore     &
    )

  call Assert_Residual_212('Test 212', alu2_A0, bb2, bb2_B0, tol)

  bb1=lusolve(alu2,kp1,bb1)

  call AssertEqual(&
    message = 'Test 211',                                         &
    answer  = bb1sol,                                             &
    check   = bb1,                                                &
    significant_digits = check_digits, ignore_digits = ignore     &
    )
  call Assert_Residual_211('Test 211', alu2_A0, bb1, bb1_B0, tol)

  call MessageNotify('M','lumatrix_test','lumatrix test succeeded')

  ! ---- pivot必須ケース（先頭ピボット0） ----
  alu2 = 0.0_DP
  alu2(1,:) = (/0.0d0, 1.0d0/)
  alu2(2,:) = (/1.0d0, 1.0d0/)
  alu2_A0 = alu2

  bb1 = (/1.0d0, 2.0d0/)   ! 例：解が [1,1] になる
  bb1_B0 = bb1

  call ludecomp(alu2, kp1)
  bb1 = lusolve(alu2, kp1, bb1)

  call Assert_Residual_211('PivotCase 211', alu2_A0, bb1, bb1_B0, tol)

  ! ---- 同じLUで別RHSを2回解く ----
  bb1 = (/2.0d0, 0.0d0/)
  bb1_B0 = bb1
  bb1 = lusolve(alu2, kp1, bb1)
  call Assert_Residual_211('ReuseLU 1', alu2_A0, bb1, bb1_B0, tol)

  bb1 = (/3.0d0, 1.0d0/)
  bb1_B0 = bb1
  bb1 = lusolve(alu2, kp1, bb1)
  call Assert_Residual_211('ReuseLU 2', alu2_A0, bb1, bb1_B0, tol)


contains

  pure real(DP) function norm2_vec(x) result(n2)
    real(DP), intent(in) :: x(:)
    n2 = sqrt(sum(x*x))
  end function norm2_vec

  subroutine Assert_Residual_211(msg, A, x, b, tol)
    character(*), intent(in) :: msg
    real(DP), intent(in) :: A(:,:), x(:), b(:)
    real(DP), intent(in) :: tol
    real(DP) :: r(size(b)), rel

    r = matmul(A, x) - b
    rel = norm2_vec(r) / max(norm2_vec(b), tiny(1.0_DP))

    call AssertEqual(&
      & message=trim(msg)//' residual', &
      & answer=0.0_DP, check=rel,       &
      & significant_digits=check_digits, ignore_digits=ignore)
    call AssertGreaterThan(&
      & message=trim(msg)//' residual', &
      & answer = rel,                   &
      & check  = tol        )
  end subroutine Assert_Residual_211

  subroutine Assert_Residual_212(msg, A, x, b, tol)
    character(*), intent(in) :: msg
    real(DP), intent(in) :: A(:,:), x(:,:), b(:,:)
    real(DP), intent(in) :: tol
    integer :: j
    real(DP) :: r(size(b,2)), rel

    do j = 1, size(b,1)
      r = matmul(A, x(j,:)) - b(j,:)
      rel = norm2_vec(r) / max(norm2_vec(b(j,:)), tiny(1.0_DP))
      call AssertGreaterThan( &
        & message=trim(msg)//' residual', &
        & answer = rel,                   &
        & check  = tol        )
    end do
  end subroutine Assert_Residual_212

  subroutine Assert_Residual_322(msg, A, x, b, tol)
    character(*), intent(in) :: msg
    real(DP), intent(in) :: A(:,:,:), x(:,:), b(:,:)
    real(DP), intent(in) :: tol
    integer :: j
    real(DP) :: r(size(b,2)), rel

    do j = 1, size(b,1)
      r = matmul(A(j,:,:), x(j,:)) - b(j,:)
      rel = norm2_vec(r) / max(norm2_vec(b(j,:)), tiny(1.0_DP))
      call AssertGreaterThan( &
        & message=trim(msg)//' residual', &
        & answer = rel,                   &
        & check  = tol        )
    end do
  end subroutine Assert_Residual_322

  subroutine Assert_Residual_323(msg, A, x, b, tol)
    character(*), intent(in) :: msg
    real(DP), intent(in) :: A(:,:,:), x(:,:,:), b(:,:,:)
    real(DP), intent(in) :: tol
    integer :: i, j
    real(DP) :: r(size(b,3)), rel

    do i = 1, size(b,1)
      do j = 1, size(b,2)
        r = matmul(A(j,:,:), x(i,j,:)) - b(i,j,:)
        rel = norm2_vec(r) / max(norm2_vec(b(i,j,:)), tiny(1.0_DP))
        call AssertGreaterThan( &
          & message=trim(msg)//' residual', &
          & answer = rel,                   &
          & check  = tol        )
      end do
    end do
  end subroutine Assert_Residual_323


end program lumatrix_test

