module evaluate
use config, only: ptol
use compare
use math
implicit none

private
public :: basis_funs, ders_basis_funs
public :: decasteljau1, decasteljau2, bezier_curve_points, bezier_surface_points
public :: curve_point, curve_points, rat_curve_derivs, curve_derivs_alg1
public :: surface_point, surface_points, rat_surface_derivs, surface_derivs_alg1
public :: find_span, find_span_mult, find_mult_knots, find_mult
public :: check_param

contains

subroutine decasteljau1(cpw, n, u, pnt)
    !> Compute a point on a Bezier curve using deCasteljau algorithm.
    !> n - Number of control points - 1.
    !> cpw - Control points.
    !> u - Parameter (0. <= u <= 1.).
    !> pnt - Point on Bezier curve.

    !f2py inent(in) cpw, n, u
    !f2py intent(out) pnt
    !f2py depend(n) cpw

    ! Input
    integer, intent(in) :: n
    double precision, intent(in) :: u
    double precision, intent(in) :: cpw(0:n, 0:3)

    ! Output
    double precision, intent(out) :: pnt(0:3)

    ! Working
    integer :: i, k
    double precision :: u_
    double precision :: temp(0:n, 0:3)

    u_ = check_bounds(u, 0.0d0, 1.0d0)
    temp = cpw
    do k = 1, n
        do i = 0, n - k
            temp(i, :) = (1.0d0 - u_) * temp(i, :) + u_ * temp(i + 1, :)
        end do
    end do
    pnt = temp(0, :)

end subroutine decasteljau1

subroutine decasteljau2(cpw, n, m, u, v, pnt)
    !> Compute a point on a Bezier surface using deCasteljau algorithm.
    !> cpw - Control points.
    !> n - Number of control points - 1 in u-direction.
    !> m - Number of control points - 1 in v-direction.
    !> u - Parameter in u-direction (0. <= u <= 1.).
    !> v - Parameter in v-direction (0. <= v <= 1.).
    !> pnt - Point on Bezier surface.

    !f2py intent(in) cpw, n, m, u, v
    !f2py intent(out) pnt
    !f2py depend(n, m) cpw

    ! Input
    integer, intent(in) :: n, m
    double precision, intent(in) :: u, v
    double precision, intent(in) :: cpw(0:n, 0:m, 0:3)

    ! Output
    double precision, intent(out) :: pnt(0:3)

    ! Working
    integer :: i, j, k
    double precision :: temp1(0:min(n, m), 0:3)
    double precision :: temp2(0:max(n, m), 0:3)

    temp1(:, :) = 0.0d0
    temp2(:, :) = 0.0d0
    if (n .le. m) then
        do j = 0, m
            temp1 = cpw(0:n, j, :)
            call decasteljau1(temp1, n, u, pnt)
            temp2(j, :) = pnt
        end do
        call decasteljau1(temp2, m, v, pnt)
    else
        do i = 0, n
            temp1 = cpw(i, 0:m, :)
            call decasteljau1(temp1, m, v, pnt)
            temp2(i, :) = pnt
        end do
        call decasteljau1(temp2, n, u, pnt)
    end if

end subroutine decasteljau2

subroutine find_span(n, p, u, uk, mid)
    !> Determine the knot span index.
    !> n - Number of control points - 1.
    !> p - Degree.
    !> u - Parameter .
    !> uk - Knot vector.
    !> mid - Knot span.

    !f2py inent(in) n, p, u, uk
    !f2py intent(out) mid
    !f2py depend(n, p) uk

    ! Input
    integer, intent(in) :: n, p
    double precision, intent(in) :: u
    double precision, intent(in) :: uk(0:n + p + 1)

    ! Output
    integer, intent(out) :: mid

    ! Working
    integer :: low, high

    ! Special case
    if (GreaterThanEq(u, uk(n + 1), ptol)) then
        mid = n
        return
    end if
    if (LessThanEq(u, uk(p), ptol)) then
        mid = p
        return
    end if

    low = p
    high = n + 1
    mid = (low + high) / 2
    do while ((LessThan(u, uk(mid), ptol)) .or. (GreaterThanEq(u, uk(mid + 1), ptol)))
        if (LessThan(u, uk(mid), ptol)) then
            high = mid
        else
            low = mid
        end if
        mid = (low + high) / 2
    end do

end subroutine find_span

subroutine basis_funs(i, u, p, uk, m, bf)
    !> Compute the non-vanishing basis functions.
    !> i - Knot span index.
    !> u - Parameter.
    !> p - Degree.
    !> uk - Knot vector.
    !> m - Size of knot vector  - 1.
    !> bf - Non-vanishing basis functions.

    !f2py intent(in) i, u, p, uk, m
    !f2py intent(out) bf
    !f2py depend(m) uk
    !f2py depend(p) bf

    ! Input
    integer, intent(in) :: i, p, m
    double precision, intent(in) :: u
    double precision, intent(in) :: uk(0:m)

    ! Output
    double precision, intent(out) :: bf(0:p)

    ! Working
    integer :: j, r
    double precision :: temp, saved, u_
    double precision :: left(0:p), right(0:p)

    bf(:) = 0.0d0
    left(:) = 0.0d0
    right(:) = 0.0d0

    u_ = check_bounds(u, uk(0), uk(m))
    bf(0) = 1.0d0
    do j = 1, p
        left(j) = u_ - uk(i + 1 - j)
        right(j) = uk(i + j) - u_
        saved = 0.0d0
        do r = 0, j - 1
            temp = bf(r) / (right(r + 1) + left(j - r))
            bf(r) = saved + right(r + 1) * temp
            saved = left(j - r) * temp
        end do
        bf(j) = saved
    end do

end subroutine basis_funs

subroutine ders_basis_funs(i, u, p, n, uk, m, ders)
    !> Compute nonzero basis functions and their derivatives.
    !> i - Knot span index.
    !> u - Parameter.
    !> p - Degree.
    !> n - Number of derivatives to compute (n <= p).
    !> uk - Knot vector.
    !> m - Size of knot vector - 1.
    !> ders - Basis functions and derivatives ders(k, j) where k is
    !> the k-th derivative.

    !f2py intent(in) i, u, p, n, uk, m
    !f2py intent(out) ders
    !f2py depend(m) uk
    !f2py depend(n, p) ders

    ! Input
    integer, intent(in) :: i, p, n, m
    double precision, intent(in) :: u
    double precision, intent(in) :: uk(0:m)

    ! Output
    double precision, intent(out) :: ders(0:n, 0:p)

    ! Working
    integer :: j, k, r, j1, j2, rk, pk, s1, s2
    double precision :: saved, temp, d, u_
    double precision :: left(0:p), right(0:p)
    double precision :: ndu(0:p, 0:p), a(0:1, 0:p)

    ders(:, :) = 0.0d0
    left(:) = 0.0d0
    right(:) = 0.0d0
    ndu(:, :) = 0.0d0
    a(:, :) = 0.0d0

    u_ = check_bounds(u, uk(0), uk(m))
    ndu(0, 0) = 1.0d0
    do j = 1, p
        left(j) = u_ - uk(i + 1 - j)
        right(j) = uk(i + j) - u_
        saved = 0.d0
        do r = 0, j - 1
            ndu(j, r) = right(r + 1) + left(j - r)
            temp = ndu(r, j - 1) / ndu(j, r)
            ndu(r, j) = saved + right(r + 1) * temp
            saved = left(j - r) * temp
        end do
        ndu(j, j) = saved
    end do
    do j = 0, p
        ders(0, j) = ndu(j, p)
    end do
    do r = 0, p
        s1 = 0
        s2 = 1
        a(0, 0) = 1.d0
        do k = 1, n
            d = 0.d0
            rk = r - k
            pk = p - k
            if (r .ge. k) then
                a(s2, 0) = a(s1, 0) / ndu(pk + 1, rk)
                d = a(s2, 0) * ndu(rk, pk)
            end if
            if (rk .ge. -1) then
                j1 = 1
            else
                j1 = -rk
            end if
            if (r - 1 .le. pk) then
                j2 = k - 1
            else
                j2 = p - r
            end if
            do j = j1, j2
                a(s2, j) = (a(s1, j) - a(s1, j - 1)) / ndu(pk + 1, rk + j)
                d = d + a(s2, j) * ndu(rk + j, pk)
            end do
            if (r .le. pk) then
                a(s2, k) = -a(s1, k - 1) / ndu(pk + 1, r)
                d = d + a(s2, k) * ndu(r, pk)
            end if
            ders(k, r) = d
            j = s1
            s1 = s2
            s2 = j
        end do
    end do
    r = p
    do k = 1, n
        do j = 0, p
            ders(k, j) = ders(k, j) * r
        end do
        r = r * (p - k)
    end do
end subroutine ders_basis_funs

subroutine curve_point(n, p, uk, cpw, u, pnt)
    !> Compute curve point.
    !> n - Number of control points - 1.
    !> p - Degree.
    !> uk - Knot vector.
    !> cpw - Control points.
    !> u - Parameter.
    !> pnt - Point on curve.

    !f2py intent(in) n, p, uk, cpw, u
    !f2py intent(out) pnt
    !f2py depend(n, p) uk
    !f2py depend(n) cpw

    ! Input
    integer, intent(in) :: n, p
    double precision, intent(in) :: u
    double precision, intent(in) :: uk(0:n + p + 1)
    double precision, intent(in) :: cpw(0:n, 0:3)

    ! Output
    double precision, intent(out) :: pnt(0:2)

    ! Working
    integer :: i, span, m
    double precision :: bf(0:p), temp(0:3)

    m = n + p + 1
    call find_span(n, p, u, uk, span)
    call basis_funs(span, u, p, uk, m, bf)
    temp(:) = 0.0d0
    do i = 0, p
        temp = temp + bf(i) * cpw(span - p + i, :)
    end do
    pnt = temp(0:2) / temp(3)

end subroutine curve_point

subroutine curve_derivs_alg1(n, p, uk, cpw, u, d, ck)
    !> Compute curve derivatives.
    !> n - Number of control points - 1.
    !> p - Degree.
    !> uk - Knot vector.
    !> cpw - Control points.
    !> u - Parameter.
    !> d - Maximum derivative to compute.
    !> ck - Curve derivatives ck(k, :) where k is the k-th derivative and
    !> 0 <= k <= d.

    !f2py intent(in) n, p, uk, cpw, u, d
    !f2py intent(out) ck
    !f2py depend(n, p) uk
    !f2py depend(n) cpw
    !f2py depend(d) ck

    ! Input
    integer, intent(in) :: n, p, d
    double precision, intent(in) :: u
    double precision, intent(in) :: uk(0:n + p + 1)
    double precision, intent(in) :: cpw(0:n, 0:3)

    ! Output
    double precision, intent(out) :: ck(0:d, 0:3)

    ! Working
    integer :: du, k, span, j, m
    double precision :: nders(0:min(d, p), 0:p)

    ck(:, :) = 0.0d0
    du = min(d, p)
    m = n + p + 1
    call find_span(n, p, u, uk, span)
    call ders_basis_funs(span, u, p, du, uk, m, nders)
    do k = 0, du
        do j = 0, p
            ck(k, :) = ck(k, :) + nders(k, j) * cpw(span - p + j, :)
        end do
    end do

end subroutine curve_derivs_alg1

subroutine rat_curve_derivs(n, p, uk, cpw, u, d, ck)
    !> Compute rational curve derivative.
    !> n - Number of control points - 1.
    !> p - Degree.
    !> uk - Knot vector.
    !> cpw - Control points.
    !> u - Parameter.
    !> d - Maximum derivative to compute.
    !> ck - Curve derivatives ck(k, :) where k is the k-th derivative and
    !> 0 <= k <= d.

    !f2py intent(in) n, p, uk, cpw, u, d
    !f2py intent(out) ck
    !f2py depend(n, p) uk
    !f2py depend(n) cpw
    !f2py depend(d) ck

    ! Input
    integer, intent(in) :: n, p, d
    double precision, intent(in) :: u
    double precision, intent(in) :: uk(0:n + p + 1)
    double precision, intent(in) :: cpw(0:n, 0:3)

    ! Output
    double precision, intent(out) :: ck(0:d, 0:2)

    integer :: i, k
    double precision :: bc
    double precision :: cders(0:d, 0:3), aders(0:d, 0:2), wders(0:d)
    double precision :: v(0:2)

    cders(:, :) = 0.0d0
    aders(:, :) = 0.0d0
    wders(:) = 0.0d0

    call curve_derivs_alg1(n, p, uk, cpw, u, d, cders)
    aders = cders(:, 0:2)
    wders = cders(:, 3)
    ck(:, :) = 0.0d0
    do k = 0, d
        v = aders(k, :)
        do i = 1, k
            bc = binomial(k, i)
            v = v - bc * wders(i) * ck(k - i, :)
        end do
        ck(k, :) = v / wders(0)
    end do

end subroutine rat_curve_derivs

subroutine surface_point(n, p, uk, m, q, vk, cpw, u, v, pnt)
    !> Compute surface point.
    !> n - Number of control points - 1 in u-direction.
    !> p - Degree in u-direction.
    !> uk - Knot vector in u-direction.
    !> m - Number of control points - 1 in v-direction.
    !> q - Degree in v-direction.
    !> vk - Knot vector in v-direction.
    !> cpw - Control points.
    !> u - Parameter in u-direction.
    !> v - Parameter in v-direction.
    !> pnt - Point on surface.

    !f2py intent(in) n, p, uk, m, q, vk, cpw, u, v
    !f2py intent(out) pnt
    !f2py depend(n, p) uk
    !f2py depend(m, q) vk
    !f2py depend(n, m) cpw

    ! Input
    integer, intent(in) :: n, p, m, q
    double precision, intent(in) :: u, v
    double precision, intent(in) :: uk(0:n + p + 1), vk(0:m + q + 1)
    double precision, intent(in) :: cpw(0:n, 0:m, 0:3)

    ! Output
    double precision, intent(out) :: pnt(0:2)

    ! Working
    integer :: uspan, vspan, uind, vind, l, k, r, s
    double precision :: nu(0:p), nv(0:q)
    double precision :: temp1(0:3), temp2(0:3)

    call find_span(n, p, u, uk, uspan)
    r = n + p + 1
    call basis_funs(uspan, u, p, uk, r, nu)
    call find_span(m, q, v, vk, vspan)
    s = m + q + 1
    call basis_funs(vspan, v, q, vk, s, nv)
    uind = uspan - p
    temp2(:) = 0.0d0
    do l = 0, q
        temp1(:) = 0.0d0
        vind = vspan - q + l
        do k = 0, p
            temp1 = temp1 + nu(k) * cpw(uind + k, vind, :)
        end do
        temp2 = temp2 + nv(l) * temp1
    end do
    pnt = temp2(0:2) / temp2(3)

end subroutine surface_point

subroutine surface_derivs_alg1(n, p, uk, m, q, vk, cpw, u, v, d, skl)
    !> Compute surface derivatives.
    !> n - Number of control points - 1 in u-direction.
    !> p - Degree in u-direction.
    !> uk - Knot vector in u-direction.
    !> m - Number of control points - 1 in v-direction.
    !> q - Degree in v-direction.
    !> vk - Knot vector in v-direction.
    !> cpw - Control points.
    !> u - Parameter in u-direction.
    !> v - Parameter in v-direction.
    !> d - Maximum derivative to compute.
    !> skl - Surface derivatives skl(k, l) with respect to u k times
    !> and v l times and 0 <= k, l <= d.

    !f2py intent(in) n, p, uk, m, q, vk, cpw, u, v, d
    !f2py intent(out) skl
    !f2py depend(n, p) uk
    !f2py depend(m, q) vk
    !f2py depend(n, m) cpw
    !f2py depend(d) skl

    ! Input
    integer, intent(in) :: n, p, m, q, d
    double precision, intent(in) :: u, v
    double precision, intent(in) :: uk(0:n + p + 1), vk(0:m + q + 1)
    double precision, intent(in) :: cpw(0:n, 0:m, 0:3)

    ! Output
    double precision, intent(out) :: skl(0:d, 0:d, 0:3)

    ! Working
    integer :: du, dv, uspan, vspan, k, s, r, dd, l
    double precision :: temp(0:q, 0:3)
    double precision, allocatable :: nu(:, :), nv(:, :)

    du = min(d, p)
    dv = min(d, q)
    allocate(nu(0:du, 0:p), nv(0:dv, 0:q))

    call find_span(n, p, u, uk, uspan)
    r = n + p + 1
    call ders_basis_funs(uspan, u, p, du, uk, r, nu)
    call find_span(m, q, v, vk, vspan)
    s = m + q + 1
    call ders_basis_funs(vspan, v, q, dv, vk, s, nv)

    skl(:, :, :) = 0.0d0
    temp(:, :) = 0.0d0
    do k = 0, du
        do s = 0, q
            temp(s, :) = 0.0d0
            do r = 0, p
                temp(s, :) = temp(s, :) + nu(k, r) * cpw(uspan - p + r, &
                                                         vspan - q + s, :)
            end do
        end do
        dd = min(d - k, dv)
        do l = 0, dd
            do s = 0, q
                skl(k, l, :) = skl(k, l, :) + nv(l, s) * temp(s, :)
            end do
        end do
    end do

end subroutine surface_derivs_alg1

subroutine rat_surface_derivs(n, p, uk, m, q, vk, cpw, u, v, d, skl)
    !> Compute rational surface derivatives.
    !> n - Number of control points - 1 in u-direction.
    !> p - Degree in u-direction.
    !> uk - Knot vector in u-direction.
    !> m - Number of control points - 1 in v-direction.
    !> q - Degree in v-direction.
    !> vk - Knot vector in v-direction.
    !> cpw - Control points.
    !> u - Parameter in u-direction.
    !> v - Parameter in v-direction.
    !> d - Maximum derivative to compute.
    !> skl - Surface derivatives skl(k, l) with respect to u k times
    !> and v l times and 0 <= k, l <= d.

    !f2py intent(in) n, p, uk, m, q, vk, cpw, u, v, d
    !f2py intent(out) skl
    !f2py depend(n, p) uk
    !f2py depend(m, q) vk
    !f2py depend(n, m) cpw
    !f2py depend(d) skl

    ! Input
    integer, intent(in) :: n, p, m, q, d
    double precision, intent(in) :: u, v
    double precision, intent(in) :: uk(0:n + p + 1), vk(0:m + q + 1)
    double precision, intent(in) :: cpw(0:n, 0:m, 0:3)

    ! Output
    double precision, intent(out) :: skl(0:d, 0:d, 0:2)

    ! Working
    integer :: k, l, i, j
    double precision :: bc
    double precision :: v1(0:2), v2(0:2)
    double precision :: sders(0:d, 0:d, 0:3)
    double precision :: aders(0:d, 0:d, 0:2)
    double precision :: wders(0:d, 0:d)

    skl(:, : ,:) = 0.0d0
    sders(:, :, :) = 0.0d0
    aders(:, :, :) = 0.0d0
    wders(:, :) = 0.0d0

    call surface_derivs_alg1(n, p, uk, m, q, vk, cpw, u, v, d, sders)
    do i = 0, d
        do j = 0, d
            aders(i, j, :) = sders(i, j, 0:2)
            wders(i, j) = sders(i, j, 3)
        end do
    end do

    do k = 0, d
        do l = 0, d - k
            v1 = aders(k, l, :)
            do j = 1, l
                bc = binomial(l, j)
                v1 = v1 - bc * wders(0, j) * skl(k, l - j, :)
            end do
            do i = 1, k
                bc = binomial(k, i)
                v1 = v1 - bc * wders(i, 0) * skl(k - i, l, :)
                v2(:) = 0.0d0
                do j = 1, l
                    bc = binomial(l, j)
                    v2 = v2 + bc * wders(i, j) * skl(k - i, l - j, :)
                end do
                bc = binomial(k, i)
                v1 = v1 - bc * v2
            end do
            skl(k, l, :) = v1 / wders(0, 0)
        end do
    end do

end subroutine rat_surface_derivs

subroutine find_span_mult(n, p, u, uk, span, s)
    !> Determine the knot span index and multiplicity.
    !> n - Number of control points - 1.
    !> p - Degree.
    !> u - Parameter.
    !> uk - Knot vector.
    !> span - Knot span index.
    !> s - Multiplicity of knot.

    !f2py intent(in) n, p, u, uk
    !f2py intent(out) span, s
    !f2py depend(n, p) uk

    ! Input
    integer, intent(in) :: n, p
    double precision, intent(in) :: u
    double precision, intent(in) :: uk(0:n + p + 1)

    ! Output
    integer, intent(out) :: span, s

    ! Working
    integer :: i, first, last, step

    call find_span(n, p, u, uk, span)

    if ((GreaterThan(u, uk(span), ptol)) .and. (LessThan(u, uk(span + 1), ptol))) then
        s = 0
        return
    end if

    if ((GreaterThan(u, uk(span), ptol)) .and. (LessThanEq(u, uk(span + 1), ptol))) then
        first = span + 1
        last = n + p + 1
        step = 1
    else
        first = span
        last = 0
        step = -1
    end if

    s = 0
    do i = first, last, step
        ! if (u .eq. uk(i)) then
        if (EqualTo(u, uk(i), ptol)) then
            s = s + 1
        else
            return
        end if
    end do

end subroutine find_span_mult

function find_mult(n, p, uk, u) result(mult)
    !> Determine the multiplicity of the parameter.
    !> n - Number of control points - 1.
    !> p - Degree.
    !> uk - Knot vector.
    !> u - Parameter.
    !> mult - Multiplicity of parameter.

    ! Input
    integer, intent(in) :: n, p
    double precision, intent(in) :: u
    double precision, intent(in) :: uk(0: n + p + 1)

    ! Output
    integer :: mult

    ! Working
    integer :: k

    call find_span_mult(n, p, u, uk, k, mult)

end function find_mult

subroutine find_mult_knots(n, p, uk, nu, um, uq)
    !> Find the multiplicities and the unique knots of the knot vector.
    !> n - Number of control points - 1.
    !> p - Degree.
    !> uk - Knot vector.
    !> nu - Number of unique knots.
    !> um - Multiplicity of each unique knot.
    !> uq - Unique knot vector.

    !f2py intent(in) n, p, uk
    !f2py intent(out) nu, um, uq
    !f2py depend(n, p) uk, um, uq

    ! Input
    integer, intent(in) :: n, p
    double precision, intent(in) :: uk(0:n + p + 1)

    ! Output
    integer, intent(out) :: nu
    integer, intent(out) :: um(0:n + p + 1)
    double precision, intent(out) :: uq(0:n + p + 1)

    ! Working
    integer :: i, m, mult

    um(:) = 0
    uq(:) = 0.0d0

    i = 0
    m = n + p + 1
    nu = 0
    do while (i .le. m)
        uq(nu) = uk(i)
        mult = 0
        ! do while ((i .le. m) .and. (dabs(uk(i) .eq. uq(nu))))
        do while ((i .le. m) .and. (EqualTo(uk(i), uq(nu), ptol)))
            i = i + 1
            mult = mult + 1
            if ( i .gt. m) then
                exit
            end if
        end do
        um(nu) = mult
        nu = nu + 1
    end do

end subroutine find_mult_knots

subroutine check_param(n, p, uk, u, is_closed)
    !> Check that the parameter is within the knot vector or is within
    !> tolerance of a unique knot value.
    !> n - Number of control points - 1.
    !> p - Degree.
    !> uk - Knot vector.
    !> u - Parameter.
    !> Option to specify a closed knot vector.

    ! Input
    integer, intent(in) :: n, p
    double precision, intent(in) :: uk(0:n + p + 1)

    ! In/Out
    double precision, intent(inout) :: u

    ! Optional
    logical, intent(in), optional :: is_closed

    ! Working
    logical :: is_closed_
    integer :: k

    if (present(is_closed)) then
        is_closed_ = is_closed
    else
        is_closed_ = .false.
    end if

    if (is_closed_) then
        if (u .gt. uk(n + 1)) then
            u = u - uk(n + 1) + uk(p)
        elseif (u .lt. uk(p)) then
            u = uk(n + 1) + u - uk(p)
        end if
    end if

    call find_span(n, p, u, uk, k)

    if (LessThanEq(u, uk(k), ptol)) then
        u = uk(k)
        return
    end if

    if (GreaterThanEq(u, uk(k + 1), ptol)) then
        u = uk(k + 1)
        return
    end if

end subroutine check_param

subroutine bezier_curve_points(n, cpw, nu, arr_u, pnts)
    !> Compute Bezier curve points.
    !> n - Number of control points - 1.
    !> cpw - Control points.
    !> nu - Number of parameters.
    !> arr_u - Parameters.
    !> pnts - Points on curve.

    !f2py intent(in) n, cpw, nu, arr_u
    !f2py intent(out) pnts
    !f2py depend(n) cpw
    !f2py depend(nu) arr_u
    !f2py depend(nu) pnts

    ! Input
    integer, intent(in) :: n, nu
    double precision, intent(in) :: arr_u(nu)
    double precision, intent(in) :: cpw(0:n, 0:3)

    ! Output
    double precision, intent(out) :: pnts(nu, 3)

    ! Working
    integer :: i
    double precision :: pi(4)

    pnts(:, :) = 0.0d0
    do i = 1, nu
        call decasteljau1(cpw, n, arr_u(i), pi)
        pnts(i, :) = pi(1:3) / pi(4)
    end do

end subroutine bezier_curve_points

subroutine curve_points(n, p, uk, cpw, nu, arr_u, pnts)
    !> Compute curve points.
    !> n - Number of control points - 1.
    !> p - Degree.
    !> uk - Knot vector.
    !> cpw - Control points.
    !> nu - Number of parameters.
    !> arr_u - Parameters.
    !> pnts - Points on curve.

    !f2py intent(in) n, p, uk, cpw, nu, arr_u
    !f2py intent(out) pnts
    !f2py depend(n, p) uk
    !f2py depend(n) cpw
    !f2py depend(nu) arr_u
    !f2py depend(nu) pnts

    ! Input
    integer, intent(in) :: n, p, nu
    double precision, intent(in) :: arr_u(nu)
    double precision, intent(in) :: uk(0:n + p + 1)
    double precision, intent(in) :: cpw(0:n, 0:3)

    ! Output
    double precision, intent(out) :: pnts(nu, 3)

    ! Working
    integer :: i
    double precision :: pi(3)

    pnts(:, :) = 0.0d0
    do i = 1, nu
        call curve_point(n, p, uk, cpw, arr_u(i), pi)
        pnts(i, :) = pi
    end do

end subroutine curve_points

subroutine bezier_surface_points(n, m, cpw, np, arr_u, arr_v, pnts)
    !> Compute points on a Bezier surface.
    !> n - Number of control points - 1 in u-direction.
    !> m - Number of control points - 1 in v-direction.
    !> cpw - Control points.
    !> np - Number of parameters.
    !> arr_u - Parameters in u-direction.
    !> arr_v - Parameters in v-direction.
    !> pnts - Points on Bezier surface.

    !f2py intent(in) cpw, n, m, arr_u, arr_v, np
    !f2py intent(out) pnts
    !f2py depend(n, m) cpw
    !f2py depend(np) arr_u, arr_v, pnts

    ! Input
    integer, intent(in) :: n, m, np
    double precision, intent(in) :: arr_u(np), arr_v(np)
    double precision, intent(in) :: cpw(0:n, 0:m, 0:3)

    ! Output
    double precision, intent(out) :: pnts(np, 3)

    ! Working
    integer :: i
    double precision :: pi(4)

    pnts(:, :) = 0.0d0
    do i = 1, np
        call decasteljau2(cpw, n, m, arr_u(i), arr_v(i), pi)
        pnts(i, :) = pi(1:3) / pi(4)
    end do

end subroutine bezier_surface_points

subroutine surface_points(n, p, uk, m, q, vk, cpw, np, arr_u, arr_v, pnts)
    !> Compute surface points.
    !> n - Number of control points - 1 in u-direction.
    !> p - Degree in u-direction.
    !> uk - Knot vector in u-direction.
    !> m - Number of control points - 1 in v-direction.
    !> q - Degree in v-direction.
    !> vk - Knot vector in v-direction.
    !> cpw - Control points.
    !> np - Number of parameters.
    !> arr_u - Parameters in u-direction.
    !> arr_v - Parameters in v-direction.
    !> pnt - Points on surface.

    !f2py intent(in) n, p, uk, m, q, vk, cpw, np, arr_u, arr_v
    !f2py intent(out) pnts
    !f2py depend(n, p) uk
    !f2py depend(m, q) vk
    !f2py depend(n, m) cpw
    !f2py depend(np) arr_u, arr_v, pnts

    ! Input
    integer, intent(in) :: n, p, m, q, np
    double precision, intent(in) :: arr_u(np), arr_v(np)
    double precision, intent(in) :: uk(0:n + p + 1), vk(0:m + q + 1)
    double precision, intent(in) :: cpw(0:n, 0:m, 0:3)

    ! Output
    double precision, intent(out) :: pnts(np, 3)

    ! Working
    integer :: i
    double precision :: pi(3)

    pnts(:, :) = 0.0d0
    do i = 1, np
        call surface_point(n, p, uk, m, q, vk, cpw, arr_u(i), arr_v(i), pi)
        pnts(i, :) = pi
    end do

end subroutine surface_points

end module evaluate


*DECK PCHDF
      REAL FUNCTION PCHDF (K, X, S, IERR)
C***BEGIN PROLOGUE  PCHDF
C***SUBSIDIARY
C***PURPOSE  Computes divided differences for PCHCE and PCHSP
C***LIBRARY   SLATEC (PCHIP)
C***TYPE      SINGLE PRECISION (PCHDF-S, DPCHDF-D)
C***AUTHOR  Fritsch, F. N., (LLNL)
C***DESCRIPTION
C
C          PCHDF:   PCHIP Finite Difference Formula
C
C     Uses a divided difference formulation to compute a K-point approx-
C     imation to the derivative at X(K) based on the data in X and S.
C
C     Called by  PCHCE  and  PCHSP  to compute 3- and 4-point boundary
C     derivative approximations.
C
C ----------------------------------------------------------------------
C
C     On input:
C        K      is the order of the desired derivative approximation.
C               K must be at least 3 (error return if not).
C        X      contains the K values of the independent variable.
C               X need not be ordered, but the values **MUST** be
C               distinct.  (Not checked here.)
C        S      contains the associated slope values:
C                  S(I) = (F(I+1)-F(I))/(X(I+1)-X(I)), I=1(1)K-1.
C               (Note that S need only be of length K-1.)
C
C     On return:
C        S      will be destroyed.
C        IERR   will be set to -1 if K.LT.2 .
C        PCHDF  will be set to the desired derivative approximation if
C               IERR=0 or to zero if IERR=-1.
C
C ----------------------------------------------------------------------
C
C***SEE ALSO  PCHCE, PCHSP
C***REFERENCES  Carl de Boor, A Practical Guide to Splines, Springer-
C                 Verlag, New York, 1978, pp. 10-16.
C***ROUTINES CALLED  XERMSG
C***REVISION HISTORY  (YYMMDD)
C   820503  DATE WRITTEN
C   820805  Converted to SLATEC library version.
C   870813  Minor cosmetic changes.
C   890411  Added SAVE statements (Vers. 3.2).
C   890411  REVISION DATE from Version 3.2
C   891214  Prologue converted to Version 4.0 format.  (BAB)
C   900315  CALLs to XERROR changed to CALLs to XERMSG.  (THJ)
C   900328  Added TYPE section.  (WRB)
C   910408  Updated AUTHOR and DATE WRITTEN sections in prologue.  (WRB)
C   920429  Revised format and order of references.  (WRB,FNF)
C   930503  Improved purpose.  (FNF)
C***END PROLOGUE  PCHDF
C
C**End
C
C  DECLARE ARGUMENTS.
C
      INTEGER  K, IERR
      REAL  X(K), S(K)
C
C  DECLARE LOCAL VARIABLES.
C
      INTEGER  I, J
      REAL  VALUE, ZERO
      SAVE ZERO
      DATA  ZERO /0./
C
C  CHECK FOR LEGAL VALUE OF K.
C
C***FIRST EXECUTABLE STATEMENT  PCHDF
      IF (K .LT. 3)  GO TO 5001
C
C  COMPUTE COEFFICIENTS OF INTERPOLATING POLYNOMIAL.
C
      DO 10  J = 2, K-1
         DO 9  I = 1, K-J
            S(I) = (S(I+1)-S(I))/(X(I+J)-X(I))
    9    CONTINUE
   10 CONTINUE
C
C  EVALUATE DERIVATIVE AT X(K).
C
      VALUE = S(1)
      DO 20  I = 2, K-1
         VALUE = S(I) + VALUE*(X(K)-X(I))
   20 CONTINUE
C
C  NORMAL RETURN.
C
      IERR = 0
      PCHDF = VALUE
      RETURN
C
C  ERROR RETURN.
C
 5001 CONTINUE
C     K.LT.3 RETURN.
      IERR = -1
      CALL XERMSG ('SLATEC', 'PCHDF', 'K LESS THAN THREE', IERR, 1)
      PCHDF = ZERO
      RETURN
C------------- LAST LINE OF PCHDF FOLLOWS ------------------------------
      END


!!## MODULE: FUNCTION  xyPLANE
MODULE FUN_xyPLANE
!!### PURPOSE
!! Create a plane with three different interfaces.

!!### USAGE
!! Three different versions are provided:
!
!       Pn = xyPLANE_C( C )
!
!!  where <C> is a rank 1 array of coefficients, <C = [a,b,c,d]> which define
!!  the plane by equation $ ax + by + c = 0 $.
!
!       Pn = xyPLANE_P2( P2 )
!
!!  where <P2> is a rank 2 array of 2 points <P2(1,:) = P_a>, <P2(2,:) = P_b>.
!
!       Pn = xyPLANE_PV( P , V )
!
!
!!  where <P> is a point and <V> is a vector in the direction of the
!!  plane normal.

!!### METHOD
!! Uses the Hessian normal form for a plane.

!!### EXTERNAL KINDS
USE KND_IntrinsicTypes,ONLY: KIND_Rsp,KIND_Rdp !!((01-A-KND_IntrinsicTypes.f90))

!!### EXTERNAL PARAMETERS
USE PAR_Constants_Rsp,ONLY: c_0_Rsp => c_0     !!((02-A-PAR_Constants_Rsp.f90))
USE PAR_Constants_Rdp,ONLY: c_0_Rdp => c_0     !!((02-A-PAR_Constants_Rdp.f90))

!!### EXTERNAL PROCEDURES
USE FUN_xyCROSS  ,ONLY: xyCROSS_VV             !!((03-A-FUN_xyCROSS.f90))
USE FUN_xyPERPCCW,ONLY: xyPERPCCW_V            !!((03-A-FUN_xyPERPCCW.f90))
USE FUN_xyDOT    ,ONLY: xyDOT_VV               !!((03-A-FUN_xyDOT.f90))
USE FUN_xyNORM   ,ONLY: xyNORM_V               !!((03-A-FUN_xyNORM.f90))
USE FUN_xyVECTOR ,ONLY: xyVECTOR_PP            !!((03-A-FUN_xyVECTOR.f90))

!!### DEFAULT IMPLICIT
IMPLICIT NONE

!!### DEFAULT ACCESS
PRIVATE

!!### PROCEDURE OVERLOADING
INTERFACE xyPLANE_P2
 MODULE PROCEDURE xyPLANE_P2_Rsp
 MODULE PROCEDURE xyPLANE_P2_Rdp
END INTERFACE

INTERFACE xyPLANE_PV
 MODULE PROCEDURE xyPLANE_PV_Rsp
 MODULE PROCEDURE xyPLANE_PV_Rdp
END INTERFACE

INTERFACE xyPLANE_C
 MODULE PROCEDURE xyPLANE_PAR_Rsp
 MODULE PROCEDURE xyPLANE_PAR_Rdp
END INTERFACE

!!### PUBLIC ACCESS LIST
PUBLIC :: xyPLANE_P2
PUBLIC :: xyPLANE_PV
PUBLIC :: xyPLANE_C

!!## MODULE PROcEDURES
CONTAINS

PURE FUNCTION xyPLANE_PV_Rsp( P , V ) RESULT(Pn)
!!#### LOCAL KINDS
USE KND_IntrinsicTypes,ONLY: KIND_R=>KIND_Rsp  !!((01-A-KND_IntrinsicTypes.f90))
!!#### LOCAL PARAMETERS
REAL(KIND_R),PARAMETER :: c_0 = c_0_Rsp
INCLUDE "05-B-FUN_xyPLANE_PV.f90.hdr"
!!--begin--
INCLUDE "05-B-FUN_xyPLANE_PV.f90.bdy"
!!--end--
END FUNCTION

PURE FUNCTION xyPLANE_PV_Rdp( P , V ) RESULT(Pn)
!!#### LOCAL KINDS
USE KND_IntrinsicTypes,ONLY: KIND_R=>KIND_Rdp  !!((01-A-KND_IntrinsicTypes.f90))
!!#### LOCAL PARAMETERS
REAL(KIND_R),PARAMETER :: c_0 = c_0_Rdp
INCLUDE "05-B-FUN_xyPLANE_PV.f90.hdr"
!!--begin--
INCLUDE "05-B-FUN_xyPLANE_PV.f90.bdy"
!!--end--
END FUNCTION

PURE FUNCTION xyPLANE_PAR_Rsp( C ) RESULT(Pn)
!!#### LOCAL KINDS
USE KND_IntrinsicTypes,ONLY: KIND_R=>KIND_Rsp  !!((01-A-KND_IntrinsicTypes.f90))
INCLUDE "05-B-FUN_xyPLANE_C.f90.hdr"
!!--begin--
INCLUDE "05-B-FUN_xyPLANE_C.f90.bdy"
!!--end--
END FUNCTION

PURE FUNCTION xyPLANE_PAR_Rdp( C ) RESULT(Pn)
!!#### LOCAL KINDS
USE KND_IntrinsicTypes,ONLY: KIND_R=>KIND_Rdp  !!((01-A-KND_IntrinsicTypes.f90))
INCLUDE "05-B-FUN_xyPLANE_C.f90.hdr"
!!--begin--
INCLUDE "05-B-FUN_xyPLANE_C.f90.bdy"
!!--end--
END FUNCTION

PURE FUNCTION xyPLANE_P2_Rsp( P2 ) RESULT(Pn)
!!#### LOCAL KINDS
USE KND_IntrinsicTypes,ONLY: KIND_R=>KIND_Rsp  !!((01-A-KND_IntrinsicTypes.f90))
INCLUDE "05-B-FUN_xyPLANE_P2.f90.hdr"
!!--begin--
INCLUDE "05-B-FUN_xyPLANE_P2.f90.bdy"
!!--end--
END FUNCTION

PURE FUNCTION xyPLANE_P2_Rdp( P2 ) RESULT(Pn)
!!#### LOCAL KINDS
USE KND_IntrinsicTypes,ONLY: KIND_R=>KIND_Rdp  !!((01-A-KND_IntrinsicTypes.f90))
INCLUDE "05-B-FUN_xyPLANE_P2.f90.hdr"
!!--begin--
INCLUDE "05-B-FUN_xyPLANE_P2.f90.bdy"
!!--end--
END FUNCTION

END MODULE


C NCLFORTSTART
      SUBROUTINE KRONSQM (n,n2,A,B,C)
        IMPLICIT NONE
c                                                ! input
        INTEGER              n, n2
        DOUBLE PRECISION     A(n,n), B(n,n)
c                                                ! output
        DOUBLE PRECISION     C(n2,n2)
C NCLEND
c computes the Kronecker product of square matrices A and B (dim=d),
c puts into C (dim=n^2)
        INTEGER              i, j, k, l, row, col

        DO i=1,n
          DO k=1,n
            row=n*(i-1)+k
            DO j=1,n
              DO l=1,n
                col=n*(j-1)+l
                C(row,col)=A(i,j)*B(k,l)
              END DO
            END DO
          END DO
        END DO

        RETURN
      END


module nrutils_modules
  integer(kind=4) :: iseed
contains
  ! Calculation of integrals...
  SUBROUTINE qromb(func,a,b,ss)
    implicit none
    INTEGER JMAX,JMAXP,K,KM
    REAL*8 a,b,func,ss,EPS
    EXTERNAL func
    PARAMETER (EPS=1.e-6, JMAX=30, JMAXP=JMAX+1, K=5, KM=K-1)
    INTEGER j
    REAL*8 dss,h(JMAXP),s(JMAXP)
    h(1)=1.
    do 11 j=1,JMAX
       call trapzd(func,a,b,s(j),j)
       if (j.ge.K) then
          call polint(h(j-KM),s(j-KM),K,0.d0,ss,dss)
          if (abs(dss).le.EPS*abs(ss)) return
       endif
       s(j+1)=s(j)
       h(j+1)=0.25*h(j)
11  continue
    pause 'too many steps in qromb'
  END SUBROUTINE qromb

  SUBROUTINE qromo(func,a,b,ss,choose)
    implicit none
    INTEGER JMAX,JMAXP,K,KM
    REAL*8 a,b,func,ss,EPS
    EXTERNAL func,choose
    PARAMETER (EPS=1.e-6, JMAX=20, JMAXP=JMAX+1, K=5, KM=K-1)
    INTEGER j
    REAL*8 dss,h(JMAXP),s(JMAXP)
    h(1)=1.
    do 11 j=1,JMAX
       call choose(func,a,b,s(j),j)
       if (j.ge.K) then
          call polint(h(j-KM),s(j-KM),K,0.d0,ss,dss)
          if (abs(dss).le.EPS*abs(ss)) return
       endif
       s(j+1)=s(j)
       h(j+1)=h(j)/9.
11  continue
    pause 'too many steps in qromo'
  END SUBROUTINE qromo

  SUBROUTINE qqromb(func,a,b,c,ss)
    implicit none
    INTEGER JMAX,JMAXP,K,KM
    REAL*8 a,b,c,func,ss,EPS
    EXTERNAL func
    PARAMETER (EPS=1.d-6, JMAX=30, JMAXP=JMAX+1, K=5, KM=K-1)
    INTEGER j
    REAL*8 dss,h(JMAXP),s(JMAXP)

    h(1)=1.
    do 11 j=1,JMAX
       call trapzd3(func,a,b,c,s(j),j)
       if (j.ge.K) then
          call polint(h(j-KM),s(j-KM),K,0.d0,ss,dss)
          if (abs(dss).le.EPS*abs(ss)) return
       endif
       s(j+1)=s(j)
       h(j+1)=0.25*h(j)
11  continue
    pause 'too many steps in qromb'
  END SUBROUTINE qqromb

  SUBROUTINE mqromb(func,a,b,xp,yp,np,ss)
    implicit none
    integer np
    real*8 xp(np),yp(np)
    INTEGER JMAX,JMAXP,K,KM
    REAL*8 a,b,func,ss,EPS
    EXTERNAL func
    PARAMETER (EPS=1.d-6, JMAX=30, JMAXP=JMAX+1, K=5, KM=K-1)
    INTEGER j
    REAL*8 dss,h(JMAXP),s(JMAXP)

    h(1)=1.
    do 11 j=1,JMAX
       call trapzd2(func,a,b,xp,yp,np,s(j),j)
       if (j.ge.K) then
          call polint(h(j-KM),s(j-KM),K,0.d0,ss,dss)
          if (abs(dss).le.EPS*abs(ss)) return
       endif
       s(j+1)=s(j)
       h(j+1)=0.25*h(j)
11  continue
    pause 'too many steps in qromb'
  END SUBROUTINE mqromb

  SUBROUTINE polint(xa,ya,n,x,y,dy)
    implicit none
    INTEGER n,NMAX
    REAL*8 dy,x,y,xa(n),ya(n)
    PARAMETER (NMAX=10)
    INTEGER i,m,ns
    REAL*8 den,dif,dift,ho,hp,w,c(NMAX),d(NMAX)
    ns=1
    dif=abs(x-xa(1))
    do 11 i=1,n
       dift=abs(x-xa(i))
       if (dift.lt.dif) then
          ns=i
          dif=dift
       endif
       c(i)=ya(i)
       d(i)=ya(i)
11  continue
    y=ya(ns)
    ns=ns-1
    do 13 m=1,n-1
       do 12 i=1,n-m
          ho=xa(i)-x
          hp=xa(i+m)-x
          w=c(i+1)-d(i)
          den=ho-hp
          if(den.eq.0.)pause 'failure in polint'
          den=w/den
          d(i)=hp*den
          c(i)=ho*den
12     continue
       if (2*ns.lt.n-m)then
          dy=c(ns+1)
       else
          dy=d(ns)
          ns=ns-1
       endif
       y=y+dy
13  continue
    return
  END SUBROUTINE polint

  SUBROUTINE trapzd(func,a,b,s,n)
    implicit none
    INTEGER n
    REAL*8 a,b,s,func
    EXTERNAL func
    INTEGER it,j
    REAL*8 del,sum,tnm,x
    if (n.eq.1) then
       s=0.5*(b-a)*(func(a)+func(b))
    else
       it=2**(n-2)
       tnm=it
       del=(b-a)/tnm
       x=a+0.5*del
       sum=0.
       do 11 j=1,it
          sum=sum+func(x)
          x=x+del
11     continue
       s=0.5*(s+(b-a)*sum/tnm)
    endif
    return
  END SUBROUTINE trapzd

  SUBROUTINE trapzd2(func,a,b,xp,yp,np,s,n)
    implicit none
    integer np
    real*8 xp(np),yp(np)
    INTEGER n
    REAL*8 a,b,s,func
    EXTERNAL func
    INTEGER it,j
    REAL*8 del,sum,tnm,x
    if (n.eq.1) then
       s=0.5*(b-a)*(func(a,xp,yp,np)+func(b,xp,yp,np))
    else
       it=2**(n-2)
       tnm=it
       del=(b-a)/tnm
       x=a+0.5*del
       sum=0.
       do 11 j=1,it
          sum=sum+func(x,xp,yp,np)
          x=x+del
11     continue
       s=0.5*(s+(b-a)*sum/tnm)
    endif
    return
  END SUBROUTINE trapzd2

  SUBROUTINE trapzd3(func,a,b,c,s,n)
    implicit none
    INTEGER n
    REAL*8 a,b,c,s,func
    EXTERNAL func
    INTEGER it,j
    REAL*8 del,sum,tnm,x
    if (n.eq.1) then
       s=0.5*(b-a)*(func(a,c)+func(b,c))
    else
       it=2**(n-2)
       tnm=it
       del=(b-a)/tnm
       x=a+0.5*del
       sum=0.
       do 11 j=1,it
          sum=sum+func(x,c)
          x=x+del
11     continue
       s=0.5*(s+(b-a)*sum/tnm)
    endif
    return
  END SUBROUTINE trapzd3

  SUBROUTINE midpnt(func,a,b,s,n)
    implicit none
    INTEGER n
    REAL*8 a,b,s,func
    EXTERNAL func
    INTEGER it,j
    REAL*8 ddel,del,sum,tnm,x
    if (n.eq.1) then
       s=(b-a)*func(0.5*(a+b))
    else
       it=3**(n-2)
       tnm=it
       del=(b-a)/(3.*tnm)
       ddel=del+del
       x=a+0.5*del
       sum=0.
       do 11 j=1,it
          sum=sum+func(x)
          x=x+ddel
          sum=sum+func(x)
          x=x+del
11     continue
       s=(s+(b-a)*sum/tnm)/3.
    endif
    return
  END SUBROUTINE midpnt

  ! Calculation of Bessel functions...

  REAL*8 FUNCTION bessj0(x)
    implicit none
    REAL*8 x
    REAL*8 ax,xx,z
    REAL*8 p1,p2,p3,p4,p5,q1,q2,q3,q4,q5,r1,r2,r3,r4,r5,r6,&
         & s1,s2,s3,s4,s5,s6,y
    SAVE p1,p2,p3,p4,p5,q1,q2,q3,q4,q5,r1,r2,r3,r4,r5,r6,s1,s2,s3,s4,&
         & s5,s6
    DATA p1,p2,p3,p4,p5/1.d0,-.1098628627d-2,.2734510407d-4,&
         & -.2073370639d-5,.2093887211d-6/, q1,q2,q3,q4,q5/-.1562499995d-1,&
         & .1430488765d-3,-.6911147651d-5,.7621095161d-6,-.934945152d-7/
    DATA r1,r2,r3,r4,r5,r6/57568490574.d0,-13362590354.d0,&
         & 651619640.7d0,-11214424.18d0,77392.33017d0,-184.9052456d0/,s1,s2,&
         & s3,s4,s5,s6/57568490411.d0,1029532985.d0,9494680.718d0,&
         & 59272.64853d0,267.8532712d0,1.d0/
    if(abs(x).lt.8.)then
       y=x**2
       bessj0=(r1+y*(r2+y*(r3+y*(r4+y*(r5+y*r6)))))/(s1+y*(s2+y*(s3+y*&
            & (s4+y*(s5+y*s6)))))
    else
       ax=abs(x)
       z=8./ax
       y=z**2
       xx=ax-.785398164
       bessj0=sqrt(.636619772/ax)*(cos(xx)*(p1+y*(p2+y*(p3+y*(p4+y*&
            & p5))))-z*sin(xx)*(q1+y*(q2+y*(q3+y*(q4+y*q5)))))
    endif
    return
  END FUNCTION bessj0

  REAL*8 FUNCTION bessi0(x)
    implicit none
    REAL*8 x
    REAL*8 ax
    REAL*8 p1,p2,p3,p4,p5,p6,p7,q1,q2,q3,q4,q5,q6,q7,q8,q9,y
    SAVE p1,p2,p3,p4,p5,p6,p7,q1,q2,q3,q4,q5,q6,q7,q8,q9
    DATA p1,p2,p3,p4,p5,p6,p7/1.0d0,3.5156229d0,3.0899424d0,&
         & 1.2067492d0,0.2659732d0,0.360768d-1,0.45813d-2/
    DATA q1,q2,q3,q4,q5,q6,q7,q8,q9/0.39894228d0,0.1328592d-1,&
         & 0.225319d-2,-0.157565d-2,0.916281d-2,-0.2057706d-1,0.2635537d-1,&
         & -0.1647633d-1,0.392377d-2/
    if (abs(x).lt.3.75) then
       y=(x/3.75)**2
       bessi0=p1+y*(p2+y*(p3+y*(p4+y*(p5+y*(p6+y*p7)))))
    else
       ax=abs(x)
       y=3.75/ax
       bessi0=(exp(ax)/sqrt(ax))*(q1+y*(q2+y*(q3+y*(q4+y*(q5+y*(q6+y*&
            & (q7+y*(q8+y*q9))))))))
    endif
    return
  END FUNCTION bessi0

  REAL*8 FUNCTION bessk0(x)
    implicit none
    REAL*8 x
    REAL*8 p1,p2,p3,p4,p5,p6,p7,q1,q2,q3,q4,q5,q6,q7,y
    SAVE p1,p2,p3,p4,p5,p6,p7,q1,q2,q3,q4,q5,q6,q7
    DATA p1,p2,p3,p4,p5,p6,p7/-0.57721566d0,0.42278420d0,0.23069756d0,&
         & 0.3488590d-1,0.262698d-2,0.10750d-3,0.74d-5/
    DATA q1,q2,q3,q4,q5,q6,q7/1.25331414d0,-0.7832358d-1,0.2189568d-1,&
         & -0.1062446d-1,0.587872d-2,-0.251540d-2,0.53208d-3/
    if (x.le.2.0) then
       y=x*x/4.0
       bessk0=(-log(x/2.0)*bessi0(x))+(p1+y*(p2+y*(p3+y*(p4+y*(p5+y*&
            & (p6+y*p7))))))
    else
       y=(2.0/x)
       bessk0=(exp(-x)/sqrt(x))*(q1+y*(q2+y*(q3+y*(q4+y*(q5+y*(q6+y*&
            & q7))))))
    endif
    return
  END FUNCTION bessk0

  REAL*8 FUNCTION bessi1(x)
    implicit none
    REAL*8 x
    REAL*8 ax
    REAL*8 p1,p2,p3,p4,p5,p6,p7,q1,q2,q3,q4,q5,q6,q7,q8,q9,y
    SAVE p1,p2,p3,p4,p5,p6,p7,q1,q2,q3,q4,q5,q6,q7,q8,q9
    DATA p1,p2,p3,p4,p5,p6,p7/0.5d0,0.87890594d0,0.51498869d0,&
         & 0.15084934d0,0.2658733d-1,0.301532d-2,0.32411d-3/
    DATA q1,q2,q3,q4,q5,q6,q7,q8,q9/0.39894228d0,-0.3988024d-1,&
         & -0.362018d-2,0.163801d-2,-0.1031555d-1,0.2282967d-1,-0.2895312d-1,&
         & 0.1787654d-1,-0.420059d-2/
    if (abs(x).lt.3.75) then
       y=(x/3.75)**2
       bessi1=x*(p1+y*(p2+y*(p3+y*(p4+y*(p5+y*(p6+y*p7))))))
    else
       ax=abs(x)
       y=3.75/ax
       bessi1=(exp(ax)/sqrt(ax))*(q1+y*(q2+y*(q3+y*(q4+y*(q5+y*(q6+y*&
            &(q7+y*(q8+y*q9))))))))
       if(x.lt.0.)bessi1=-bessi1
    endif
    return
  END FUNCTION bessi1

   REAL*8 FUNCTION bessk1(x)
     implicit none
     REAL*8 x
     REAL*8 p1,p2,p3,p4,p5,p6,p7,q1,q2,q3,q4,q5,q6,q7,y
     SAVE p1,p2,p3,p4,p5,p6,p7,q1,q2,q3,q4,q5,q6,q7
     DATA p1,p2,p3,p4,p5,p6,p7/1.0d0,0.15443144d0,-0.67278579d0,&
          & -0.18156897d0,-0.1919402d-1,-0.110404d-2,-0.4686d-4/
     DATA q1,q2,q3,q4,q5,q6,q7/1.25331414d0,0.23498619d0,-0.3655620d-1,&
          & 0.1504268d-1,-0.780353d-2,0.325614d-2,-0.68245d-3/
     if (x.le.2.0) then
        y=x*x/4.0
        bessk1=(log(x/2.0)*bessi1(x))+(1.0/x)*(p1+y*(p2+y*(p3+y*(p4+y*&
             & (p5+y*(p6+y*p7))))))
     else
        y=2.0/x
        bessk1=(exp(-x)/sqrt(x))*(q1+y*(q2+y*(q3+y*(q4+y*(q5+y*(q6+y*&
             & q7))))))
     endif
     return
   END FUNCTION bessk1

   ! Generate random numbers...

   REAL*8 FUNCTION ran3(idum)
     implicit none
     INTEGER idum
     INTEGER MBIG,MSEED,MZ
     REAL*8 FAC
     PARAMETER (MBIG=1000000000,MSEED=161803398,MZ=0,FAC=1./MBIG)
     INTEGER i,iff,ii,inext,inextp,k
     INTEGER mj,mk,ma(55)
     SAVE iff,inext,inextp,ma
     DATA iff /0/
     if(idum.lt.0.or.iff.eq.0)then
        iff=1
        mj=MSEED-iabs(idum)
        mj=mod(mj,MBIG)
        ma(55)=mj
        mk=1
        do 11 i=1,54
           ii=mod(21*i,55)
           ma(ii)=mk
           mk=mj-mk
           if(mk.lt.MZ)mk=mk+MBIG
           mj=ma(ii)
11      continue
        do 13 k=1,4
          do 12 i=1,55
            ma(i)=ma(i)-ma(1+mod(i+30,55))
            if(ma(i).lt.MZ)ma(i)=ma(i)+MBIG
12        continue
13      continue
        inext=0
        inextp=31
        idum=1
      endif
      inext=inext+1
      if(inext.eq.56)inext=1
      inextp=inextp+1
      if(inextp.eq.56)inextp=1
      mj=ma(inext)-ma(inextp)
      if(mj.lt.MZ)mj=mj+MBIG
      ma(inext)=mj
      ran3=mj*FAC
      return
   END FUNCTION ran3

   ! Solve ODE

   SUBROUTINE odeint(ystart,nvar,x1,x2,eps,h1,hmin,nok,nbad,derivs,&
        &     rkqs,xx,yy,n)
     use structure
     implicit none

     INTEGER nbad,nok,nvar,MAXSTP,KMAXX,NMAX
     PARAMETER (MAXSTP=1000,NMAX=2,KMAXX=2000)

     REAL*8 eps,h1,hmin,x1,x2,ystart(nvar),TINY
     parameter(TINY=1.d-30)
     EXTERNAL derivs,rkqs

     INTEGER i,kmax,kount,nstp
     REAL*8 dxsav,h,hdid,hnext,x,xsav,dydx(NMAX),xp(KMAXX),y(NMAX),&
          &     yp(NMAX,KMAXX),yscal(NMAX)
     COMMON /path/ kmax,kount,dxsav,xp,yp

     integer n
     real*8 xx(*),yy(*)

     real*8 phiz,fintrp
     external phiz,fintrp
     real*8 sum

     kmax=KMAXX

     x=x1
     h=sign(h1,x2-x1)
     nok=0
     nbad=0
     kount=0

     do i=1,nvar
        y(i)=ystart(i)
     end do

     if (kmax.gt.0) xsav=x-2.*dxsav

     do 16 nstp=1,MAXSTP
        call derivs(x,y,dydx)

        do i=1,nvar
           yscal(i)=abs(y(i))+abs(h*dydx(i))+TINY
        end do

        if(kmax.gt.0)then

           if(abs(x-xsav).gt.abs(dxsav)) then

              if(kount.lt.kmax-1)then
                 kount=kount+1
                 xp(kount)=x
                 do i=1,nvar
                    yp(i,kount)=y(i)
                 end do
                 xsav=x
              endif            ! if(kount.lt.kmax-1) then

           endif               ! if(abs(x-xsav).gt.abs(dxsav)) then
        endif                  ! if(kmax.gt.0) then

        if((x+h-x2)*(x+h-x1).gt.0.) h=x2-x

        call rkqs(y,dydx,nvar,x,h,eps,yscal,hdid,hnext,derivs)

        if(hdid.eq.h)then
           nok=nok+1
        else
           nbad=nbad+1
        endif

        if((x-x2)*(x2-x1).ge.0.)then
           do 14 i=1,nvar
              ystart(i)=y(i)
14         continue
           if(kmax.ne.0)then
              kount=kount+1
              xp(kount)=x
              do 15 i=1,nvar
                 yp(i,kount)=y(i)
15            continue
           endif

           n=kount
           do i=1,n
              xx(i)=xp(i)
              yy(i)=yp(1,i)
           end do

           return
        endif
        if(abs(hnext).lt.hmin) pause 'stepsize smaller than minimum in odeint'
        h=hnext
16      continue
        pause 'too many steps in odeint'
        return
      END SUBROUTINE odeint

      SUBROUTINE rkqs(y,dydx,n,x,htry,eps,yscal,hdid,hnext,derivs)
!        implicit none
        INTEGER n,NMAX
        REAL*8 eps,hdid,hnext,htry,x,dydx(n),y(n),yscal(n)
        PARAMETER (NMAX=50)
        INTEGER i
        external derivs
        REAL*8 errmax,h,xnew,yerr(NMAX),ytemp(NMAX),SAFETY,PGROW,PSHRNK,&
             & ERRCON
        PARAMETER (SAFETY=0.9,PGROW=-.2,PSHRNK=-.25,ERRCON=1.89e-4)
        h=htry
1       call rkck(y,dydx,n,x,h,ytemp,yerr,derivs)
        errmax=0.
        do 11 i=1,n
           errmax=max(errmax,abs(yerr(i)/yscal(i)))
11      continue

        errmax=errmax/eps
        if(errmax.gt.1.)then
           h=SAFETY*h*(errmax**PSHRNK)
           if(h.lt.0.1*h)then
              h=.1*h
           endif
           xnew=x+h
           if(xnew.eq.x)pause 'stepsize underflow in rkqs'
           goto 1
        else
           if(errmax.gt.ERRCON)then
              hnext=SAFETY*h*(errmax**PGROW)
           else
              hnext=5.*h
           endif
           hdid=h
           x=x+h
           do 12 i=1,n
              y(i)=ytemp(i)
12         continue
        return
     endif
  END SUBROUTINE rkqs

  SUBROUTINE rkck(y,dydx,n,x,h,yout,yerr,derivs)
!    implicit none
    INTEGER n,NMAX
    REAL*8 h,x,dydx(n),y(n),yerr(n),yout(n)
    PARAMETER (NMAX=50)
    external derivs
    INTEGER i
    REAL*8 ak2(NMAX),ak3(NMAX),ak4(NMAX),ak5(NMAX),ak6(NMAX),&
         & ytemp(NMAX),A2,A3,A4,A5,A6,B21,B31,B32,B41,B42,B43,B51,B52,B53,&
         & B54,B61,B62,B63,B64,B65,C1,C3,C4,C6,DC1,DC3,DC4,DC5,DC6
    PARAMETER (A2=.2,A3=.3,A4=.6,A5=1.,A6=.875,B21=.2,B31=3./40.,&
         & B32=9./40.,B41=.3,B42=-.9,B43=1.2,B51=-11./54.,B52=2.5,&
         & B53=-70./27.,B54=35./27.,B61=1631./55296.,B62=175./512.,&
         & B63=575./13824.,B64=44275./110592.,B65=253./4096.,C1=37./378.,&
         & C3=250./621.,C4=125./594.,C6=512./1771.,DC1=C1-2825./27648.,&
         & DC3=C3-18575./48384.,DC4=C4-13525./55296.,DC5=-277./14336.,&
         & DC6=C6-.25)
    do 11 i=1,n
       ytemp(i)=y(i)+B21*h*dydx(i)
11  continue
    call derivs(x+A2*h,ytemp,ak2)
    do 12 i=1,n
       ytemp(i)=y(i)+h*(B31*dydx(i)+B32*ak2(i))
12  continue
    call derivs(x+A3*h,ytemp,ak3)
    do 13 i=1,n
       ytemp(i)=y(i)+h*(B41*dydx(i)+B42*ak2(i)+B43*ak3(i))
13  continue
    call derivs(x+A4*h,ytemp,ak4)
    do 14 i=1,n
       ytemp(i)=y(i)+h*(B51*dydx(i)+B52*ak2(i)+B53*ak3(i)+B54*ak4(i))
14  continue
    call derivs(x+A5*h,ytemp,ak5)
    do 15 i=1,n
       ytemp(i)=y(i)+h*(B61*dydx(i)+B62*ak2(i)+B63*ak3(i)+B64*ak4(i)+&
            & B65*ak5(i))
15  continue
    call derivs(x+A6*h,ytemp,ak6)
    do 16 i=1,n
       yout(i)=y(i)+h*(C1*dydx(i)+C3*ak3(i)+C4*ak4(i)+C6*ak6(i))
16  continue
    do 17 i=1,n
       yerr(i)=h*(DC1*dydx(i)+DC3*ak3(i)+DC4*ak4(i)+DC5*ak5(i)+DC6*ak6(i))
17  continue
    return
  END SUBROUTINE rkck

  ! Sorting routine

  SUBROUTINE indexx(n,arr,indx)
    implicit none
    INTEGER n,indx(n),M,NSTACK
    REAL arr(n)
    PARAMETER (M=7,NSTACK=50)
    INTEGER i,indxt,ir,itemp,j,jstack,k,l,istack(NSTACK)
    REAL a
    do 11 j=1,n
       indx(j)=j
11  continue
    jstack=0
    l=1
    ir=n
1   if(ir-l.lt.M)then
       do 13 j=l+1,ir
          indxt=indx(j)
          a=arr(indxt)
          do 12 i=j-1,1,-1
             if(arr(indx(i)).le.a)goto 2
             indx(i+1)=indx(i)
12        continue
          i=0
2         indx(i+1)=indxt
13     continue
       if(jstack.eq.0)return
       ir=istack(jstack)
       l=istack(jstack-1)
       jstack=jstack-2
    else
       k=(l+ir)/2
       itemp=indx(k)
       indx(k)=indx(l+1)
       indx(l+1)=itemp
       if(arr(indx(l+1)).gt.arr(indx(ir)))then
          itemp=indx(l+1)
          indx(l+1)=indx(ir)
          indx(ir)=itemp
       endif
       if(arr(indx(l)).gt.arr(indx(ir)))then
          itemp=indx(l)
          indx(l)=indx(ir)
          indx(ir)=itemp
       endif
       if(arr(indx(l+1)).gt.arr(indx(l)))then
          itemp=indx(l+1)
          indx(l+1)=indx(l)
          indx(l)=itemp
       endif
       i=l+1
       j=ir
       indxt=indx(l)
       a=arr(indxt)
3      continue
       i=i+1
       if(arr(indx(i)).lt.a)goto 3
4      continue
       j=j-1
       if(arr(indx(j)).gt.a)goto 4
       if(j.lt.i)goto 5
       itemp=indx(i)
       indx(i)=indx(j)
       indx(j)=itemp
       goto 3
5      indx(l)=indx(j)
       indx(j)=indxt
       jstack=jstack+2
       if(jstack.gt.NSTACK)pause 'NSTACK too small in indexx'
       if(ir-i+1.ge.j-l)then
          istack(jstack)=ir
          istack(jstack-1)=i
          ir=j-1
       else
          istack(jstack)=j-1
          istack(jstack-1)=l
          l=i
       endif
    endif
    goto 1
  END SUBROUTINE indexx

end module nrutils_modules


program time_independent
! Compile as ```gfortran time_independent.f90 -o time_independent finite_difference.o -llapack```
! After compiling ```gfortran -c finite_difference.f90 -llapack```
use finite_diff
implicit none

interface
    function potential_V(N_grid,x_pos,election)
    implicit none
    integer     :: N_grid
    real*8      :: x_pos(1:N_grid)
    character*2 :: election
    real*8      :: potential_V(1:N_grid)
    end function
end interface

integer :: NN, ii, n_eigen, INFO, N_grid
real*8, parameter :: x_0 = -10, x_n = 10
real*8 :: dx
real*8, allocatable :: energies(:), psi_states(:,:), V_pot(:), x_pos(:)
character*42 :: energy_file, states_file
character*2 :: election

write(*,*) "Currently enabled potential choices:"
write(*,'(A,/,A,/,A,/,A)') " - 'ho': harmonic oscillator"," - 'qs': V=5 step at x = 0",&
" - 'qb': V = 5 barrier when x in (-5,5)", " - 'qw': V = -5 well when x in (-5,5)"
write(*,*) "Any option not listed leads to the (-10,10) quantum box."
write(*,*) "Enter the potential type, grid size and the number of desired energy levels"
read(*,*) election, NN, n_eigen

if (NN.le.n_eigen) stop "The grid size must be higher than the number of energy levels"

! The grid size determines the increment value
dx = (x_n-x_0)/NN

if (dx.ge.1) stop "Grid size too small. Minumum should be 20 (advised 400 and above)."

! Since we assume x_0=x_n=0, we reduce the problem to a (NN-1)x(NN-1) matrix
N_grid = NN - 1

! Compute the potential energy array
allocate(V_pot(1:N_grid))
allocate(x_pos(1:N_grid))
x_pos = (/(ii*dx + x_0, ii = 1,N_grid)/)
V_pot = potential_V(N_grid,x_pos,election)

! Calling single particle routine for diagonal potentials and vanishing boundaries
call finite_diff_1D(x_0,x_n,NN,n_eigen,energies,psi_states,V_pot,INFO)

! Validate there are no routine errors
if (INFO.ne.0) then
    write(*,'(A,I4)') "Unsuccessful calculation. Status = ",INFO
stop "Not storing results. Exiting program..."
end if

write(energy_file,'(A,A,A,I0,A)') "energies_",election,"_",NN,".dat"
write(states_file,'(A,A,A,I0,A)') "psi_states_",election,"_",NN,".dat"
open(15, file = energy_file)
open(16, file = states_file)
do ii = 1, n_eigen
    write(15,*) energies(ii)
    write(16,*) psi_states(:,ii)! Storing as row vectors
end do
close(16)
close(15)

end program time_independent

! Catallogue of potentials
function potential_V(N_grid,x_pos,election)
implicit none
integer     :: N_grid, ii
real*8      :: x_pos(1:N_grid)
character*2 :: election
real*8      :: potential_V(1:N_grid)
    select case (election)
    case ('ho')
        ! Harmonic oscillator potential
        potential_V = x_pos**2/2
    case ('qs')
        ! Step potential
        do ii = 1, N_grid
            if (x_pos(ii).lt.0) then
                potential_V(ii) = 0
            else
                potential_V(ii) = 1
            end if
        end do
    case ('qb','qw')
        ! Barrier potential
        do ii = 1, N_grid
            if ((x_pos(ii).lt.-5).or.(x_pos(ii).gt.5)) then
                potential_V(ii) = 0
            else
                potential_V(ii) = 1
            end if
        end do
        ! Potential well
        if (election.eq.'we') then
            potential_V = -potential_V
        end if
    case default
        potential_V = (/(0,ii = 1,N_grid)/)
        election = 'bx'
    end select
end function


!======================== primitive_to_conserved_1D ==========================80
!>
!! Converts from the primitive state to the conserved state.
!<
!=============================================================================80
pure function primitive_to_conserved_1D(qp)

  use set_precision,   only : dp
  use set_constants,   only : half
  use fluid_constants, only : xgm1

  implicit none

  real(dp), dimension(3), intent(in) :: qp
  real(dp), dimension(3)             :: primitive_to_conserved_1D

  primitive_to_conserved_1D(1) = qp(1)
  primitive_to_conserved_1D(2) = qp(1)*qp(2)
  primitive_to_conserved_1D(3) = qp(3)*xgm1 + half*qp(1)*qp(2)*qp(2)

end function primitive_to_conserved_1D


!=====================================================================!
! Run gradient-based descent algorithms to solve linear algebra
!=====================================================================!

program test

  implicit none

  call check_conjugate

end program test

!---------------------------------------------------------------------!
!
!---------------------------------------------------------------------!

subroutine check_conjugate

  use linear_algebra

  implicit none

  dirichlet : block

    integer, parameter :: npts = 64
    real(8), parameter :: max_tol = 1.0d-8
    integer, parameter :: max_it = 100000
    real(8) :: x(npts,3), b(npts), A(npts,npts), P(npts, npts)
    integer :: iter, flag, i, j
    real(8) :: tol

    ! Solve using LU factorization
    call assemble_system_dirichlet(0.0d0, 1.0d0, npts, A, b, x(:,1), P)
    x(:,1) = solve(A,b)

    ! solve using CG
    call assemble_system_dirichlet(0.0d0, 1.0d0, npts, A, b, x(:,2), P)
    call dcg(A, b, max_it, max_tol, x(:,2), iter, tol, flag)
    print *, 'cg', tol, iter

    ! Solve using preconditioned CG
    call assemble_system_dirichlet(0.0d0, 1.0d0, npts, A, b, x(:,3), P)
    call dpcg(A, P,  b, max_it, max_tol, x(:,3), iter, tol, flag)
    print *, 'pcg', tol, iter

    open(11, file='dirichlet.dat')
    do i = 1, npts
       ! x, exact, xcg, xpcg
       write(11, *) dble(i)/dble(npts+1), x(i,1), x(i,2), x(i,3)
    end do
    close(11)

  end block dirichlet

  mixed : block

    integer, parameter :: npts = 256
    real(8), parameter :: max_tol = 1.0d-8
    integer, parameter :: max_it = 100000
    real(8) :: x(npts+1,3), b(npts+1), A(npts+1,npts+1), P(npts+1, npts+1)
    integer :: iter, flag, i, j
    real(8) :: tol

    ! Solve using LU factorization
    call assemble_system_mixed(0.0d0, 1.0d0, npts, A, b, x(:,1), P)
    x(:,1) = solve(A,b)

    ! Solve using CG
    call assemble_system_mixed(0.0d0, 1.0d0, npts, A, b, x(:,2), P)
    call dcg(A, b, max_it, max_tol, x(:,2), iter, tol, flag)
    print *, 'cg', tol, iter

    ! Solve using preconditioned CG
    call assemble_system_mixed(0.0d0, 1.0d0, npts, A, b, x(:,3), P)
    call dpcg(A, P,  b, max_it, max_tol, x(:,3), iter, tol, flag)
    print *, 'pcg', tol, iter

    open(11, file='mixed.dat')
    do i = 1, npts + 1
       write(11, *) dble(i)/dble(npts+1), x(i,1), x(i,2), x(i,3)
    end do
    close(11)

  end block mixed

end subroutine check_conjugate

!---------------------------------------------------------------------!
! Assemble -U_xx = 2x - 0.5, U(0) = 1;  U(1)= 0, x in [0,1]
!---------------------------------------------------------------------!

subroutine assemble_system_dirichlet(a, b, npts, V, rhs, u, P)

  use linear_algebra, only : inv

  implicit none

  real(8), intent(in)  :: a, b ! bounds of the domain
  integer, intent(in)  :: npts ! number of interior points
  real(8), intent(out) :: V(npts,npts) ! banded matrix
  real(8), intent(out) :: rhs(npts)
  real(8), intent(out) :: u(npts)
  real(8), intent(out) :: P(npts,npts)
  real(8) :: S(npts,npts), D(npts, npts)

  real(8), parameter :: PI = 3.141592653589793d0
  real(8)            :: h, alpha
  integer            :: M, N
  integer            :: i, j, k

  ! h = width / num_interior_pts + 1
  h = (b-a)/dble(npts+1)
  V = 0.0d0

  ! Size of the linear system = unknowns (interior nodes)
  M = npts ! nrows
  N = npts ! ncols

  ! Set the inner block
  rows: do i = 1, M
     cols: do j = 1, N
        if (j .eq. i-1) then
           ! lower triangle
           V(j,i) = -1.0d0
        else if (j .eq. i+1) then
           ! upper triangle
           V(j,i) = -1.0d0
        else if (j .eq. i) then
           ! diagonal
           V(j,i) = 2.0d0
        else
           ! skip
        end if
     end do cols
  end do rows

  ! Assemble the RHS
  do i = 1, M
     rhs(i) = h*h*(2.0d0*dble(i)*h - 0.5d0)
  end do
  rhs(1) = rhs(1) + 1.0d0
  rhs(M) = rhs(M)

  ! Initial solution profile use sin function as a first guess
  do i = 1, M
     u(i) =  sin(dble(i)*h*PI)
  end do

  ! Find the sine transform matrix
  alpha = sqrt(2.0d0/dble(npts+1))
  do j = 1, M
     do k = 1, N
        S(k,j) = alpha*sin(PI*dble(j*k)/dble(npts+1))
     end do
  end do

  ! Find the diagonal matrix
  D = matmul(S, matmul(V, S))

  ! Invert the digonal matrix easily
  do j = 1, M
     D(j,j) = 1.0d0/D(j,j)
  end do

  ! Define the preconditioner
  p = matmul(S, matmul(D, S))

end subroutine assemble_system_dirichlet

!---------------------------------------------------------------------!
! Assemble -U_xx = 2x - 0.5, U(0) = U_x(1)= 0, x in [0,1]
!---------------------------------------------------------------------!

subroutine assemble_system_mixed(a, b, npts, V, rhs, u, P)

  use linear_algebra, only : inv

  implicit none

  real(8), intent(in)  :: a, b ! bounds of the domain
  integer, intent(in)  :: npts ! number of interior points
  real(8), intent(out) :: V(npts+1,npts+1) ! banded matrix
  real(8), intent(out) :: rhs(npts+1)
  real(8), intent(out) :: u(npts+1)
  real(8), intent(out) :: P(npts+1,npts+1)
  real(8) :: S(npts+1, npts+1), D(npts+1, npts+1)
  real(8), parameter :: PI = 3.141592653589793d0
  real(8)            :: h, alpha
  integer            :: M, N
  integer            :: i, j, k

  ! h = width / num_interior_pts + 1
  h = (b-a)/dble(npts+1)
  V = 0.0d0

  ! Size of the linear system = unknowns
  M = npts + 1 ! nrows
  N = npts + 1 ! ncols

  ! Set the inner block
  rows: do i = 1, M
     cols: do j = 1, npts
        if (j .eq. i-1) then
           ! lower triangle
           V(j,i) = -1.0d0
        else if (j .eq. i+1) then
           ! upper triangle
           V(j,i) = -1.0d0
        else if (j .eq. i) then
           ! diagonal
           V(j,i) = 2.0d0
        else
           ! skip
        end if
     end do cols
  end do rows
  V(M,M) = 1.0d0
  V(M,N-1) = -1.0d0

  ! Assemble the RHS
  do i = 1, M
     rhs(i) = h*h*(2.0d0*dble(i)*h - 0.5d0)
  end do
  rhs(1) = rhs(1) + 1.0d0
  rhs(M) = rhs(M)/2.0d0

  ! Initial solution profile use sin function as a first guess
  do i = 1, M
     u(i) = sin(dble(i)*h*PI)
  end do
  ! Find the sine transform matrix
  alpha = sqrt(2.0d0/dble(npts+1))
  do j = 1, M
     do k = 1, N
        S(k,j) = alpha*sin(PI*dble(j*k)/dble(npts+1))
     end do
  end do

  ! Find the diagonal matrix
  D = matmul(S, matmul(V, S))

  ! Invert the digonal matrix easily
  do j = 1, M
     D(j,j) = 1.0d0/D(j,j)
  end do

  ! Define the preconditioner
  p = matmul(S, matmul(D, S))

end subroutine assemble_system_mixed


program tsunami

  ! Tsunami simulator.
  !
  ! Solves the non-linear 2-d shallow water equation system:
  !
  !     du/dt + u du/dx + v du/dy + g dh/dx = 0
  !     dv/dt + u dv/dx + v dv/dy + g dh/dy = 0
  !     dh/dt + d(hu)/dx + d(hv)/dy = 0
  !
  ! This version is parallelized and uses derived types.

  use iso_fortran_env, only: int32, real32, event_type, team_type
  use mod_field, only: Field, diffx, diffy

  implicit none

  integer(int32) :: n
  integer(int32) :: time_step_count[*]

  integer(int32), parameter :: im = 201 ! grid size in x
  integer(int32), parameter :: jm = 201 ! grid size in y
  integer(int32), parameter :: num_time_steps = 1000 ! number of time steps

  real(real32), parameter :: dt = 0.02 ! time step [s]
  real(real32), parameter :: dx = 1 ! grid spacing in x [m]
  real(real32), parameter :: dy = 1 ! grid spacing in y [m]
  real(real32), parameter :: g = 9.8 ! gravitational acceleration [m/s^2]

  integer(int32), parameter :: ic = im / 2 + 1, jc = jm / 2 + 1
  real(real32), parameter :: decay = 0.02

  type(Field) :: h, hm, u, v

  type(team_type) :: new_team
  integer(int32) :: team_num

  type(event_type) :: time_step_event[*]

  real(real32) :: hmin, hmax, hmean

  team_num = 1
  if (this_image() == 1) team_num = 2
  form team(team_num, new_team)

  change team(new_team)

    if (team_num == 1) then

    u = Field('u', [im, jm])
    v = Field('v', [im, jm])
    h = Field('h', [im, jm])
    hm = Field('h_mean', [im, jm])

    ! initialize a gaussian blob in the center
    call h % init_gaussian(decay, ic, jc)

    hm = 10.

    call h % write(0)

    time_loop: do n = 1, num_time_steps

      ! compute u at next time step
      u = u - (u * diffx(u) / dx + v * diffy(u) / dy &
        + g * diffx(h) / dx) * dt

      ! compute v at next time step
      v = v - (u * diffx(v) / dx + v * diffy(v) / dy &
        + g * diffy(h) / dy) * dt

      ! compute h at next time step
      h = h - (diffx(u * (hm + h)) / dx &
             + diffy(v * (hm + h)) / dy) * dt

      hmin = minval(h % data)
      call co_min(hmin, 1)

      hmax = maxval(h % data)
      call co_max(hmax, 1)

      hmean = sum(h % data(h % lb(1):h % ub(1),h % lb(2):h % ub(2))) &
           / size(h % data(h % lb(1):h % ub(1),h % lb(2):h % ub(2)))
      call co_sum(hmean, 1)
      hmean = hmean / num_images()

      if (this_image() == 1) then
        event post(time_step_event[1, team_number=2]) ! not implemented as of gcc-9.2.0 and OpenCoarrays-2.8.0
        print '(a, i5, 3(f10.6))', 'step, min(h), max(h), mean(h):', &
          n, hmin, hmax, hmean
      end if

      call h % write(n)

    end do time_loop

    else if (team_num == 2) then

      n = 0
      do
        call event_query(time_step_event, time_step_count)
        if (time_step_count > n) then
          n = time_step_count
          print *, 'tsunami logger: step ', n, 'of', num_time_steps, 'done'
        end if
        if (n == num_time_steps) exit
      end do

    end if

  end team

end program tsunami


real, dimension(1000) :: a = (/ (1.0/(i*i), i=1, 1000) /)
real :: result

result = sum(a);


      SUBROUTINE ATXPXA(A,U,C,N,NA,NU,NC,EPS,FAIL)
C
C   PURPOSE:
C      Solve the real matrix equation A'X + XA = C, where A and C are
C      constant matrices of dimension n x n with C=C'.  The matrix A
C      is transformed into upper Schur form and the transformed system
C      is solved by back substitution.  The option is provided to input
C      the Schur form directly and bypass the Schur decomposition.
C   REFERENCES:
C      Bartels, R.H.; and Stewart, G.W.: Algorithm 432 - Solution of
C        the Matrix Equation AX + XB = C.  Commun. ACM, vol. 15, no. 9,
C        Sept. 1972, pp. 820-826.
C
C   Subroutines employed by ATXPXA: BCKMLT, HSHLDR, SCHUR, SYMSLV
C   Subroutines employing ATXPXA: BARSTW
C
      IMPLICIT REAL*8 (A-H,O-Z)
      REAL*8
     1A(NA,1),U(NU,1),C(NC,1),EPS
      INTEGER
     1N,NA,NU,NC,FAIL,N1,NM1,I,J,K
      N1 = N+1
      NM1 = N-1
C
C IF REQUIRED, REDUCE A TO LOWER REAL SCHUR FORM.
C
      IF(EPS .LT. 0.) GO TO 15
      CALL HSHLDR(A,N,NA)
      CALL BCKMLT(A,U,N,NA,NU)
      DO 10 I=1,NM1
        A(I+1,I) = A(I,N1)
   10 CONTINUE
      CALL SCHUR(A,U,N,NA,NU,EPS,FAIL)
      IF(FAIL .NE. 0) RETURN
C
C TRANSFORM C.
C
   15 DO 20 I=1,N
          C(I,I)=C(I,I)/2.
   20 CONTINUE
      DO 40 I=1,N
        DO 30 J=1,N
          A(N1,J) = 0.
          DO 30 K=I,N
            A(N1,J) = A(N1,J) + C(I,K)*U(K,J)
   30   CONTINUE
          DO 40 J=1,N
          C(I,J) = A(N1,J)
   40 CONTINUE
      DO 60 J=1,N
        DO 50 I=1,N
          A(I,N1) = 0.
          DO 50 K=1,N
            A(I,N1) = A(I,N1) + U(K,I)*C(K,J)
   50   CONTINUE
        DO 60 I=1,N
          C(I,J) = A(I,N1)
   60 CONTINUE
      DO 70 I=1,N
        DO 70 J=I,N
          C(I,J) = C(I,J) + C(J,I)
          C(J,I) = C(I,J)
   70 CONTINUE
C
C SOLVE THE TRANSFORMED SYSTEM.
C
      CALL SYMSLV(A,C,N,NA,NC)
C
C TRANSFORM C BACK TO THE SOLUTION.
C
      DO 80 I=1,N
        C(I,I) = C(I,I)/2.
   80 CONTINUE
      DO 100 I=1,N
        DO 90 J=1,N
          A(N1,J) = 0.
          DO 90 K=I,N
            A(N1,J) = A(N1,J) + C(I,K)*U(J,K)
   90   CONTINUE
        DO 100 J=1,N
          C(I,J) = A(N1,J)
  100 CONTINUE
      DO 120 J=1,N
        DO 110 I=1,N
          A(I,N1) = 0.
          DO 110 K=1,N
            A(I,N1) = A(I,N1) + U(I,K)*C(K,J)
  110   CONTINUE
        DO 120 I=1,N
          C(I,J) = A(I,N1)
  120 CONTINUE
      DO 130 I=1,N
        DO 130 J=I,N
          C(I,J) = C(I,J) + C(J,I)
          C(J,I) = C(I,J)
  130 CONTINUE
      RETURN
      END


Real Function fkaon(p, pmax)
  Save
  fmax = 0.148
  If (pmax==0.) pmax = 0.000001
  fkaon = (1.-p/pmax)*(p/pmax)**2
  If (fkaon>fmax) fkaon = fmax
  fkaon = fkaon/fmax
  Return
End Function fkaon


!>-----------------------------------------------------------------------------------------------
!!
!> @brief       Solution of linear algebraic equations
!!
!> @author      Christopher Kebschull
!> @author      Vitali Braun
!> @author      Eduard Gamper
!> @author      Volker Schaus
!! @version     1.0
!!
!> @date

!!
VB:  26.07.2013 (initial design)

!!
ChK: 10.07.2015 (assimilated into libslam)

!!
VB:  31.01.2016 (fixed issue, where routines would checkOut upon an error)

!!
VB:  08.02.2016 (introduced Cholesky decomposition for matrix inversion)

!!
VS:  16.08.2018 (Adding additional functions linear_comb1, linear_comb2)

!!
ChK: 13.10.2019 (Enforcing camel case and adding copyright)

!!

!!
!! @details     This module contains functions, parameters and routines for the solution of
!!              linear algebraic equations.
!!
!! @anchor      linAlgebra
!!
!> @copyright   Institute of Space Systems / TU Braunschweig
!!
!!------------------------------------------------------------------------------------------------
module slam_linAlgebra

  use slam_types
  use slam_math, only: outerproduct, eps20, identity_matrix
  use slam_error_handling

  implicit none

  private

  integer, parameter :: INVERT_CHOLESKY  = 1  ! matrix inversion via Cholesky decomposition (only for positive definite matrices)
  integer, parameter :: INVERT_LU_DECOMP = 2  ! matrix inversion via LU decomposition

  public :: cholesky_decomposition
  public :: invert_matrix
  public :: forward_substitution
  public :: back_substitution
  public :: inverse2
  public :: linear_comb1
  public :: linear_comb2
  public :: add_vec
  public :: sub_vec

contains

! -------------------------------------------------------------------------------------
!
!> @brief       Matrix inversion via different methods
!!
!> @author      Vitali Braun
!!
!> @date

!!
 15.01.2016 (Re-implementation with new methods)

!!

!!
!> @param[in]   A       input matrix A
!> @param[in]   method  inversion method (either CHOLESKY or LU_DECOMP)
!> @param[out]  A_inv    inverted matrix
!!
!! @anchor      invert_matrix
!!
!> @copyright   Institute of Space Systems / TU Braunschweig
! -------------------------------------------------------------------------------------
  subroutine invert_matrix(A, A_inv, method)

    real(dp), dimension(:,:),                 intent(in)  :: A
    real(dp), dimension(size(A,1),size(A,1)), intent(out) :: A_inv
    character(len=*), intent(in) :: method

    character(len=*), parameter :: csubid = 'invert_matrix'
    real(dp), dimension(size(A,1),size(A,1)) :: A_loc                           ! local copy of matrix A
    real(dp), dimension(size(A,1),size(A,1)) :: A_chol                          ! Cholesky decomposition of matrix A
    real(dp), dimension(size(A,1),size(A,1)) :: A_chol_inv                      ! Inverse of (lower triangular) Cholesky decomposition of matrix A
    real(dp), dimension(size(A,1),size(A,1)) :: id_matrix                        ! identity matrix
    real(dp), dimension(size(A,1))           :: b_temp                           ! temporary vector
    integer,  dimension(size(A,1))           :: idx                             ! row interchange index vector for LU decomposition
    integer  :: n,i,k
    integer  :: invMethod ! inversion method to be used
    real(dp) :: d

    real(dp), dimension(6,6) :: tempMat

    if(isControlled()) then
      if(hasToReturn()) return
      call checkIn(csubid)
    end if

    if(size(a,1) == size(a,2)) then
      n = size(a,1)
    else
      call setError(E_MATRIX_MATCHING, FATAL) ! Matrix is not invertible
      return
    end if

    ! check method to be used
    if(index(method, 'CHOLESKY') /= 0) then
      invMethod = INVERT_CHOLESKY
    else if(index(method, 'LU_DECOMP') /= 0) then
      invMethod = INVERT_LU_DECOMP
    else
      call setError(E_UNKNOWN_PARAMETER, FATAL, (/method/))
      return
    end if

    !** create local copy of matrix A
    A_loc = A
    !** set up identity matrix
    call identity_matrix(id_matrix)

    select case(invMethod)
      case(INVERT_CHOLESKY)
        ! compute Cholesky decomposition, which will result in a lower triangular matrix
        call cholesky_decomposition(A,A_chol)
        if(hasFailed()) return

        ! now perform forward substitution column-wise
        call identity_matrix(id_matrix)
        do i=1,n
          b_temp = id_matrix(:,i)
          call forward_substitution(A_chol, b_temp)
          A_chol_inv(:,i) = b_temp
        end do

        ! the inverse is now simply: A^{-1} = (A_chol_inv^T)*A_chol_inv
        A_inv = matmul(transpose(A_chol_inv), A_chol_inv)

      case(INVERT_LU_DECOMP)
        !** perform LU decomposition of matrix A
        call LU_decomposition(A_loc, idx, d)

        call identity_matrix(A_inv)
        !** now find inverse by columns using LU back_substitution
        do i=1,n
          call lu_back_substitution(A_loc, idx, A_inv(:,i))
        end do

    end select

    if(isControlled()) then
      call checkOut(csubid)
    end if

    return

  end subroutine invert_matrix

  ! -------------------------------------------------------------------------------------
  !
  !> @brief       Back substitution
  !!
  !> @param[in]     a       input matrix A
  !! @param[inout]  b       output matrix B
  !!
  !! @anchor      back_substitution
  !!
  ! -------------------------------------------------------------------------------------
  subroutine back_substitution(a,b)

    real(dp), dimension(:,:), intent(in)    :: a
    real(dp), dimension(:),   intent(inout) :: b

    integer :: i,j
    real(dp), dimension(size(b)) :: x

    do i=size(a,1),1,-1
      x(i) = b(i)
      do j=i+1,size(a,1)
        x(i) = x(i) - a(i,j)*x(j)
      end do
      x(i) = x(i)/a(i,i)
    end do

    b = x
    return

  end subroutine back_substitution

  ! -------------------------------------------------------------------------------------
  !
  !> @brief       Forward substitution
  !!
  !> @param[in]     a       input matrix A
  !! @param[inout]  b       output matrix B
  !!
  !! @anchor      forward_substitution
  !!
  ! -------------------------------------------------------------------------------------
  subroutine forward_substitution(a,b)

    real(dp), dimension(:,:), intent(in)    :: a
    real(dp), dimension(:),   intent(inout) :: b

    integer :: i,j
    real(dp), dimension(size(b)) :: x

    do i=1,size(a,1)
      x(i) = b(i)
      do j=1,i-1
        x(i) = x(i) - a(i,j)*x(j)
      end do
      x(i) = x(i)/a(i,i)
    end do

    b = x
    return

  end subroutine

!>-----------------------------------------------------------------------------------------------
!!
!> @brief       Back substitution based on LU decomposed matrix A
!!
!> @author      Vitali Braun (based on Numerical Recipes in Fortran 90, Vol.2)
!!
!> @date

!!
 26.07.2013 (initial design)

!!

!!
!! @details     Solves the set of N linear equations A ·X = B. Here the N × N matrix A is input,
!!              not as the original matrix A, but rather as its LU decomposition, determined by
!!              the routine "LU_decomposition". "idx" is input as the permutation vector of
!!              length N returned by "LU_decomposition". "b" is input as the right-hand-side
!!              vector B,also of length N, and returns with the solution vector X. "a" and "idx"
!!              are not modified by this routine and can be left in place for successive calls
!!              with different right-hand sides b. This routine takes into account the possibility
!!              that b will begin with many zero elements, so it is efficient for use in matrix inversion.
!!
!> @param[in]   A       input matrix A in LU decomposition form (NxN)
!!
!> @param[in]   idx     array containing the row interchanges
!> @param[out]  b       right hand side of vector B, returns solution vector X.
!!
!!
!! @see         lu_decomposition
!! @anchor      lu_back_substitution
!!------------------------------------------------------------------------------------------------
  subroutine lu_back_substitution(a,idx,b)

    real(dp), dimension(:,:), intent(in)    :: a
    integer,  dimension(:),   intent(in)    :: idx
    real(dp), dimension(:),   intent(inout) :: b

    character(len=*), parameter :: csubid = 'lu_back_substitution'
    integer  :: i,n,ii,ll
    real(dp) :: summ

    if(isControlled()) then
      if(hasToReturn()) return
      call checkIn(csubid)
    end if

    if(size(a,1) == size(a,2) .and. size(a,2) == size(idx)) then
      n = size(a,1)
    else
      call setError(E_MATRIX_MATCHING, FATAL) ! There is a row of zeros.
      if(isControlled()) then
        call checkOut(csubid)
      end if
      return
    end if

    ii=0

    do i=1,n
      ll    = idx(i)
      summ  = b(ll)
      b(ll) = b(i)

      if (ii /= 0) then
        summ = summ - dot_product(a(i,ii:i-1),b(ii:i-1))
      else if (summ /= 0.0) then
        ii=i
      end if

      b(i) = summ

    end do

    do i=n,1,-1
      b(i) = (b(i) - dot_product(a(i,i+1:n),b(i+1:n)))/a(i,i)
    end do

    if(isControlled()) then
      call checkOut(csubid)
    end if
    return

  end subroutine lu_back_substitution

!>--------------------------------------------------------------------------------------
!!
!> @brief       LU decomposition
!!
!> @author      Vitali Braun (based on Numerical Recipes in Fortran 90, Vol.2)
!!
!> @date

!!
 26.07.2013 (initial design)

!!

!!
!! @details     Given an N × N input matrix A, this routine replaces it by the LU decomposition of
!!              a rowwise permutation of itself. "idx" is an output vector of length N that records
!!              the row permutation effected by the partial pivoting. "d" is output as +-1 depending
!!              on whether the number of row interchanges was even or odd, respectively. This
!!              routine is used in combination with "back substitution" to solve linear equations or
!!              invert a matrix.
!!
!> @param[in]   A       input matrix A (NxN)
!!
!> @param[out]  idx     array containing the row interchanges
!> @param[out]  d       indicates whether the number of row interchanges was even (+1) or odd (-1)
!!
!! @anchor      lu_decomposition
!!--------------------------------------------------------------------------------------
  subroutine lu_decomposition(A,idx,d)

    real(dp), dimension(:,:), intent(inout) :: a
    integer,  dimension(:),   intent(out)   :: idx
    real(dp),                 intent(out)   :: d

    character(len=*), parameter    :: csubid = 'LU_decomposition'
    integer, dimension(1)          :: itemp
    real(dp), dimension(size(a,1))  :: swap
    integer                        :: j,n,imax
    real(dp), dimension(size(a,1)) :: vv        ! stores the implicit scaling of each row.

    if(isControlled()) then
      if(hasToReturn()) return
      call checkIn(csubid)
    end if

    if(size(a,1) == size(a,2) .and. size(a,2) == size(idx)) then
      n = size(a,1)
    else
      call setError(E_MATRIX_MATCHING, FATAL) ! There is a row of zeros.
      if(isControlled()) then
        call checkOut(csubid)
      end if
      return
    end if

    d  = 1.0                      ! No row interchanges yet.
    vv = maxval(abs(a),dim=2)     ! Loop over rows to get the implicit scaling

    if (any(vv == 0.d0)) then
      call setError(E_SINGULAR_MATRIX, FATAL) ! There is a row of zeros.
      if(isControlled()) then
        call checkOut(csubid)
      end if
      return
    end if

    vv=1.d0/vv  ! Save the scaling.

    do j=1,n
      itemp = maxloc(vv(j:n)*abs(a(j:n,j)))
      imax  = (j-1) + itemp(1)

      if (j /= imax) then                        ! Do we need to interchange rows?
        swap      = a(imax,:)
        a(imax,:) = a(j,:)
        a(j,:)    = swap
        d         = -d                           ! ...and change the parity of d.
        vv(imax)  = vv(j)                        ! also interchange the scale factor.
      end if

      idx(j)=imax

      if (a(j,j) == 0.d0) a(j,j)=eps20
        !If the pivot element is zero the matrix is singular (at least to the precision of the al-
        !gorithm). For some applications on singular matrices, it is desirable to substitute eps20
        !for zero.

      a(j+1:n,j)     = a(j+1:n,j)/a(j,j)  ! Divide by the pivot element.
      a(j+1:n,j+1:n) = a(j+1:n,j+1:n) - outerproduct(a(j+1:n,j),a(j,j+1:n)) ! Reduce remaining submatrix.

    end do

    if(isControlled()) then
      call checkOut(csubid)
    end if

  end subroutine lu_decomposition

!>-------------------------------------------------------------------------------------
!!
!> @brief       Cholesky decomposition
!!
!> @author      Vitali Braun (based on Numerical Recipes in Fortran 90, Vol.2)
!!
!> @date

!!
 13.03.2014 (initial design)

!!

!!
!! @details     Given an N × N positive-definite symmetric matrix A, this routine constructs its
!!              cholesky decomposition A = LL*. On input, only the upper triangle of A need to
!!              be given. The Cholesky factor L is returned in lower triangular form.
!!
!> @param[in]   A       input matrix A (NxN)
!> @param[out]  L       cholesky decomposition of A in lower triangular form (NxN)
!!
!! @anchor      cholesky_decomposition
!!-------------------------------------------------------------------------------------
  subroutine cholesky_decomposition(a,l)

    implicit none

    real(dp), dimension(:,:), intent(in)  :: a
    real(dp), dimension(:,:), intent(out) :: l

    character(len=*), parameter :: csubid = 'cholesky_decomposition'

    integer  :: i,j,n
    real(dp), dimension(size(a,1), size(a,2)) :: aloc
    real(dp) :: summ

    if(isControlled()) then
      if(hasToReturn()) return
      call checkIn(csubid)
    end if

    aloc = a
    l    = 0.d0
    n    = size(a,1)

    do i = 1,n

      summ = aloc(i,i)-dot_product(aloc(i,1:i-1),aloc(i,1:i-1))

      if (summ <= 0.0) then
        call setError(E_MATRIX_NOT_POSITIVE_DEFINITE, FATAL)
        return
      end if

      l(i,i) = sqrt(summ)
      aloc(i+1:n,i) = (aloc(i,i+1:n)-matmul(aloc(i+1:n,1:i-1),aloc(i,1:i-1)))/l(i,i)

    end do

    do i=1,size(l,1)
      l(i,i+1:size(l,1)) = 0.d0
      forall(j = 1:i-1) l(i,j) = aloc(i,j)
    end do

    if(isControlled()) then
      call checkOut(csubid)
    end if
    return

  end subroutine cholesky_decomposition

  !>-------------------------------------------------------------------------------------
  !!
  !> @brief       Calculation of inverse
  !!
  !! @author      Eduard Gamper (EG), based on
  !!                 Alex G (http://ww2.odu.edu/~agodunov/computing/programs/book2/Ch06/Inverse.f90, accessed 03.02.2015)
  !!
  !> @date

  !!
 01.12.2009 (initial design)

  !!
 22.11.2017 EG: Input corrected. Now, original matrix won't be destroyed.

  !!
 22.11.2017 EG: Changed in code values to double (d0).

  !!

  !!
  !> @details     Subroutine calculates the inverse of a nxn matrix. Based on Doolittle LU
  !!              factorization for Ax=b
  !!
  !> @param[in]   original_matrix     original matrix (NxN)
  !> @param[in]   n                   dimension of original matrix
  !> @param[out]  c                   inverse of original matrix (NxN)
  !!
  !> @anchor      inverse2
  !>-------------------------------------------------------------------------------------
  subroutine inverse2(original_matrix,c,n)
    implicit none

    integer, intent(in)                     :: n
    real(dp), dimension(n,n), intent(in)    :: original_matrix
    real(dp), dimension(n,n), intent(out)   :: c

    real(dp), dimension(n,n)                :: a
    real(dp), dimension(n,n)                :: L
    real(dp), dimension(n,n)                :: U
    real(dp), dimension(n)                  :: b
    real(dp), dimension(n)                  :: d
    real(dp), dimension(n)                  :: x
    real(dp)                                :: coeff
    integer                                 :: i
    integer                                 :: j
    integer                                 :: k

    ! Copy original matrix to working matrix "a"
    a = original_matrix

    ! step 0: initialization for matrices L and U and b
    ! Fortran 90/95 aloows such operations on matrices
    L=0.d0
    U=0.d0
    b=0.d0
!
    ! step 1: forward elimination
    do k=1, n-1
       do i=k+1,n
          coeff=a(i,k)/a(k,k)
          L(i,k) = coeff
          do j=k+1,n
             a(i,j) = a(i,j)-coeff*a(k,j)
          end do
       end do
    end do

    ! Step 2: prepare L and U matrices
    ! L matrix is a matrix of the elimination coefficient
    ! + the diagonal elements are 1.0
    do i=1,n
      L(i,i) = 1.d0
    end do
    ! U matrix is the upper triangular part of A
    do j=1,n
      do i=1,j
        U(i,j) = a(i,j)
      end do
    end do

    ! Step 3: compute columns of the inverse matrix C
    do k=1,n
      b(k)=1.d0
      d(1) = b(1)
    ! Step 3a: Solve Ld=b using the forward substitution
      do i=2,n
        d(i)=b(i)
        do j=1,i-1
          d(i) = d(i) - L(i,j)*d(j)
        end do
      end do
    ! Step 3b: Solve Ux=d using the back substitution
      x(n)=d(n)/U(n,n)
      do i = n-1,1,-1
        x(i) = d(i)
        do j=n,i+1,-1
          x(i)=x(i)-U(i,j)*x(j)
        end do
        x(i) = x(i)/u(i,i)
      end do
    ! Step 3c: fill the solutions x(n) into column k of C
      do i=1,n
        c(i,k) = x(i)
      end do
      b(k)=0.d0
    end do

  end subroutine inverse2

  !---------------------------------------------------------------------------
  !!
  !> @anchor  linear_comb1
  !!
  !> @brief   linear combination of one vector with a scalar
  !!
  !> @author  Volker Schaus (VS)
  !!
  !> @details this subroutine calculates the linear combination of a vector
  !!            multiplied by a constants.
  !> @date

  !!
VS: 16.08.2018 initial implementation based on Vallado

  !!

  !!
  !---------------------------------------------------------------------------
  function linear_comb1(a, vec) result (out_vec)

    real(dp), intent(in)                :: a
    real(dp), dimension(3), intent(in)  :: vec
    real(dp), dimension(3)              :: out_vec
    integer i

    do i= 1, 3
        out_vec(i)= a * vec(i)
    end do

    return
  end function linear_comb1

  !---------------------------------------------------------------------------
  !!
  !> @anchor  linear_comb2
  !!
  !> @brief   linear combination of two vectors
  !!
  !> @author  Volker Schaus (VS)
  !!
  !> @details this subroutine calculates the linear combination of two vectors
  !!            multiplied by two different constants
  !!
  !> @date

  !!
VS: 15.08.2018 initial implementation based on Vallado

  !!

  !!
  !---------------------------------------------------------------------------
  function linear_comb2 (a1, a2, vec1, vec2) result (out_vec)

    real(dp) :: a1
    real(dp) :: a2
    real(dp), dimension(3), intent(in)  :: vec1
    real(dp), dimension(3), intent(in)  :: vec2
    real(dp), dimension(3)              :: out_vec

    integer i

    do i= 1, 3
        out_vec(i)= a1*vec1(i) + a2*vec2(i)
    end do

    return
  end function linear_comb2

  !---------------------------------------------------------------------------
  !!
  !> @anchor  add_vec
  !!
  !> @brief   adds two vectors
  !!
  !>  @param[in] real(dp),  dimension (3) vec1
  !>  @param[in] real(dp),  dimension (3) vec2
  !>  @param[out] real(dp), dimension (3) out_vec
  !!
  !> @author  Volker Schaus (VS)
  !!
  !> @details this subroutine adds two vectors
  !!
  !> @date

  !!
VS: 17.08.2018 initial implementation based on Vallado

  !!

  !!
  !---------------------------------------------------------------------------
  function add_vec (vec1,vec2) result (out_vec)

    real(dp), dimension(3), intent(in) :: vec1
    real(dp), dimension(3), intent(in) :: vec2
    real(dp), dimension(3) :: out_vec
    integer i

    do i = 1,3
        out_vec(i) = vec1(i) + vec2(i)
      end do

  end function add_vec

  !---------------------------------------------------------------------------
  !!
  !> @anchor  sub_vec
  !!
  !> @brief   subtracts two vectors
  !!
  !>  @param[in] real(dp),  dimension (3) vec1
  !>  @param[in] real(dp),  dimension (3) vec2
  !>  @param[out] real(dp), dimension (3) out_vec
  !!
  !> @author  Volker Schaus (VS)
  !!
  !> @details this subroutine subtracts two vectors
  !!
  !> @date

  !!
VS: 20.08.2018 initial implementation based on Vallado

  !!

  !!
  !---------------------------------------------------------------------------
  function sub_vec(Vec1, Vec2) result(out_vec)

    real(dp), dimension(3), intent(in)  :: vec1
    real(dp), dimension(3), intent(in)  :: vec2
    real(dp), dimension(3)              :: out_vec

    integer i

    do i=1, 3
        out_vec(i) = vec1(i) - vec2(i)
    end do

  end function sub_vec

end module slam_linAlgebra


      SUBROUTINE newt(x,n,check)
      INTEGER n,nn,NP,MAXITS
      LOGICAL check
      REAL x(n),fvec,TOLF,TOLMIN,TOLX,STPMX
      PARAMETER (NP=40,MAXITS=200,TOLF=1.e-4,TOLMIN=1.e-6,TOLX=1.e-7,
     *STPMX=100.)
      COMMON /newtv/ fvec(NP),nn
      SAVE /newtv/
CU    USES fdjac,fmin,lnsrch,lubksb,ludcmp
      INTEGER i,its,j,indx(NP)
      REAL d,den,f,fold,stpmax,sum,temp,test,fjac(NP,NP),g(NP),p(NP),
     *xold(NP),fmin
      EXTERNAL fmin
      nn=n
      f=fmin(x)
      test=0.
      do 11 i=1,n
        if(abs(fvec(i)).gt.test)test=abs(fvec(i))
11    continue
      if(test.lt..01*TOLF)then
        check=.false.
        return
      endif
      sum=0.
      do 12 i=1,n
        sum=sum+x(i)**2
12    continue
      stpmax=STPMX*max(sqrt(sum),float(n))
      do 21 its=1,MAXITS
        call fdjac(n,x,fvec,NP,fjac)
        do 14 i=1,n
          sum=0.
          do 13 j=1,n
            sum=sum+fjac(j,i)*fvec(j)
13        continue
          g(i)=sum
14      continue
        do 15 i=1,n
          xold(i)=x(i)
15      continue
        fold=f
        do 16 i=1,n
          p(i)=-fvec(i)
16      continue
        call ludcmp(fjac,n,NP,indx,d)
        call lubksb(fjac,n,NP,indx,p)
        call lnsrch(n,xold,fold,g,p,x,f,stpmax,check,fmin)
        test=0.
        do 17 i=1,n
          if(abs(fvec(i)).gt.test)test=abs(fvec(i))
17      continue
        if(test.lt.TOLF)then
          check=.false.
          return
        endif
        if(check)then
          test=0.
          den=max(f,.5*n)
          do 18 i=1,n
            temp=abs(g(i))*max(abs(x(i)),1.)/den
            if(temp.gt.test)test=temp
18        continue
          if(test.lt.TOLMIN)then
            check=.true.
          else
            check=.false.
          endif
          return
        endif
        test=0.
        do 19 i=1,n
          temp=(abs(x(i)-xold(i)))/max(abs(x(i)),1.)
          if(temp.gt.test)test=temp
19      continue
        if(test.lt.TOLX)return
21    continue
      pause 'MAXITS exceeded in newt'
      END

NR-Functions/Numerical Recipes- Example & Functions/Functions/newt.for