text:
module BcdV3

using StaticArrays
using LinearAlgebra
using Printf
using Base.Threads

export
    bcd!,
    bcd,
    get_callback,
    get_callback_maxdiff_param,
    Ω,
    inverse_affine_Q,
    inverse_affine,
    dynamics

function nlogp(y::Matrix{SVector{3,type}}, u, T, b, Q_inv, Ns) where {type}
    # C = Atomic{type}(0.0)
    N = size(y,2)
    N_triads = size(y,1)
    N_orientions = length(Ns)
    e2 = zeros(type, N_triads, N)
    @threads for n = 1:N
        for k = 1:N_triads
            e = T[k]*u[k,n] + b[k] - y[k,n]
            c_k_n = dot(e, Q_inv[k], e)
            if n <= N_orientions
                c_k_n *= Ns[n]
            end
            # C += c_k_n
            # atomic_add!(C,c_k_n)
            e2[k,n] = c_k_n
        end
    end

    return sum(e2)
end
function nlogp(y::AbstractMatrix{type}, u, T, b, Q_inv, Ns) where {type}

    N = size(y,2)
    inds = reshape(1:size(y,1),3,:)
    N_triads = size(inds,2)
    N_orientions = length(Ns)
    e2 = zeros(type, N_triads, N)
    for n = 1:N
        for k = 1:N_triads
            kk = inds[:,k]
            e = T[:,:,k]*u[kk,n] + b[:,k] - y[kk,n]
            c_k_n = dot(e, Q_inv[kk,kk], e)
            if n <= N_orientions
                c_k_n *= Ns[n]
            end
            e2[k,n] = c_k_n
        end
    end

    return sum(e2)
end
function nlogp_naive(y::Matrix{SVector{3,type}}, Q_inv, Ns, r, T, b, g, w, w_dot, s) where {type}
    C = zeros(type, size(y))
    Na = length(r)
    N = size(y, 2)
    N_triads = size(y,1)
    N_orientions = length(Ns)
    for n = 1:N
        for k = 1:N_triads
            if n <= N_orientions
                if k <= Na
                    u = -g[n]
                else
                    u = zero(SVector{3, type})
                end
            else
                n_d = n - N_orientions
                if k <= Na
                    u = Ω(w[n_d])^2*r[k] + Ω(w_dot[n_d])*r[k] + s[n_d]
                else
                    u = w[n_d]
                end
            end

            e = T[k]*u + b[k] - y[k,n]
            c_k_n = dot(e, Q_inv[k], e)

            if n <= N_orientions
                c_k_n *= Ns[n]
            end

            C[k,n] = c_k_n
        end
    end
    return sum(C)
end
function nlogp_naive(y::Matrix{type}, Q_inv::Matrix{type}, Ns, r::Matrix{type}, T, b::Matrix{type}, g::Matrix{type}, w::Matrix{type}, w_dot::Matrix{type}, s::Matrix{type}) where {type}

    inds = reshape(1:size(y,1), 3, :)
    Na = size(r, 2)
    N = size(y, 2)
    N_triads = size(inds,2)
    N_orientions = length(Ns)
    C = zeros(type,N_triads, N)
    for n = 1:N
        for k = 1:N_triads
            kk = inds[:,k]
            if n <= N_orientions
                if k <= Na
                    u = -g[:,n]
                else
                    u = zeros(type, 3)
                end
            else
                n_d = n - N_orientions
                if k <= Na
                    u = Ω(w[:,n_d])^2*r[:,k] + Ω(w_dot[:,n_d])*r[:,k] + s[:,n_d]
                else
                    u = w[:,n_d]
                end
            end

            e = T[:,:,k]*u + b[:,k] - y[kk,n]
            c_k_n = dot(e, Q_inv[kk,kk], e)

            if n <= N_orientions
                c_k_n *= Ns[n]
            end

            C[k,n] = c_k_n
        end
    end
    return sum(C)
end
################################################################################
# Means and jacobians
################################################################################
function Ω(a::SVector{3,T}) where {T}
    A = zero(MMatrix{3,3,T, 9})

    A[1,2] = -a[3]
    A[2,1] =  a[3]

    A[1,3] =  a[2]
    A[3,1] = -a[2]

    A[2,3] = -a[1]
    A[3,2] =  a[1]

    return SMatrix(A)
end
function Ω(a::AbstractVector{T}) where {T}
    A = zeros(T,3,3)

    A[1,2] = -a[3]
    A[2,1] =  a[3]

    A[1,3] =  a[2]
    A[3,1] = -a[2]

    A[2,3] = -a[1]
    A[3,2] =  a[1]

    return A
end
J_r(w, w_dot) = Ω(w)^2 + Ω(w_dot)

function J_Tb(u::SVector{3,T}) where {T}
    A = zero(MMatrix{3,12,T})

    for i = 1:3, j = 1:3
        A[j,j + 3(i-1)] = u[i]
    end
    for i = 1:3
        A[i,i+9] = one(T)
    end
    SMatrix(A)
end
function J_Tb(u::AbstractVector{T}) where {T}
    A = zeros(T,3,12)

    for i = 1:3, j = 1:3
        A[j,j + 3(i-1)] = u[i]
    end
    for i = 1:3
        A[i,i+9] = one(T)
    end
    A
end
function J_Tb_ref(u::SVector{3,T}) where {T}
    A = zero(MMatrix{3,9,T})
    A[1,1] = u[1]

    A[1,2] = u[2]
    A[2,3] = u[2]

    A[1,4] = u[3]
    A[2,5] = u[3]
    A[3,6] = u[3]

    A[1,7] = one(T)
    A[2,8] = one(T)
    A[3,9] = one(T)
    SMatrix(A)
end
function J_Tb_ref(u::AbstractVector{T}) where {T}
    A = zeros(T,3,9)
    A[1,1] = u[1]

    A[1,2] = u[2]
    A[2,3] = u[2]

    A[1,4] = u[3]
    A[2,5] = u[3]
    A[3,6] = u[3]

    A[1,7] = one(T)
    A[2,8] = one(T)
    A[3,9] = one(T)
    A
end
function T_ref(x::SVector{6,type}) where {type}
    T = zero(MMatrix{3,3,type})
    T[1,1] = x[1]
    T[1,2] = x[2]
    T[1,3] = x[4]

    T[2,2] = x[3]
    T[2,3] = x[5]

    T[3,3] = x[6]
    SMatrix(T)
end
function T_ref(x::AbstractVector{TT}) where {TT}
    T = zeros(TT,3,3)
    T[1,1] = x[1]
    T[1,2] = x[2]
    T[1,3] = x[4]

    T[2,2] = x[3]
    T[2,3] = x[5]

    T[3,3] = x[6]
    T
end
function H!(H::AbstractMatrix{T}, r::Vector{SVector{3,T}}) where {T}
    Na = length(r)
    for k = 1:Na
        H[1+3(k-1):3*k,1:3] = -Ω(r[k])
    end
    nothing
end
function H!(H::AbstractMatrix{T}, r::AbstractMatrix{T}) where {T}
    Na = size(r,2)
    inds = reshape(1:length(r), 3,:)
    for k = 1:Na
        kk = inds[:,k]
        H[kk,1:3] = -Ω(r[:,k])
    end
    nothing
end
function fill_H_constants!(H::AbstractMatrix{T}, Na) where {T}
    eye = SMatrix{3,3, T}(I)
    for k = 1:Na
        H[1+3(k-1):3*k, 4:6] = eye
    end
    nothing
end
function fill_H_constants_2!(H::AbstractMatrix{T}, Na) where {T}
    for k = 1:Na
        H[1+3(k-1):3*k, 4:6] = Matrix{T}(I,3,3)
    end
    nothing
end
function h!(y::AbstractVector, w::AbstractVector, r::AbstractMatrix, Ng::Int)

    Na = size(r, 2)
    @inbounds for k = 1:Na
        o = 3(k-1)
        y[1+o] = -w[2]^2*r[1,k]
        y[1+o] -= w[3]^2*r[1,k]
        y[1+o] += w[1]*w[2]*r[2,k]
        y[1+o] += w[1]*w[3]*r[3,k]

        y[2+o] = w[1]*w[2]*r[1,k]
        y[2+o] -= w[1]^2*r[2,k]
        y[2+o] -= w[3]^2*r[2,k]
        y[2+o] += w[2]*w[3]*r[3,k]

        y[3+o] = w[1]*w[3]*r[1,k]
        y[3+o] += w[2]*w[3]*r[2,k]
        y[3+o] -= w[1]^2*r[3,k]
        y[3+o] -= w[2]^2*r[3,k]
    end
    @inbounds for k = 1:Ng
        o = 3(k-1) + 3*Na
        y[1+o] = w[1]
        y[2+o] = w[2]
        y[3+o] = w[3]
    end
    nothing
end
function J_h!(J::AbstractMatrix{T}, w, r, Ng) where {T}
    # fill!(J, zero(T))
    Na = size(r, 2)
    @inbounds for i = 1:3
        @inbounds for k = 1:Na
            o = 3(k-1)
            if i == 1
                J[1+o, i]  = w[3]*r[3,k] +   w[2]*r[2,k]
                J[2+o, i]  = w[2]*r[1,k] - 2.0*w[1]*r[2,k]
                J[3+o, i]  = w[3]*r[1,k] - 2.0*w[1]*r[3,k]
            elseif i == 2
                J[1+o, i]  = w[1]*r[2,k] - 2.0*w[2]*r[1,k]
                J[2+o, i]  = w[3]*r[3,k] +   w[1]*r[1,k]
                J[3+o, i]  = w[3]*r[2,k] - 2.0*w[2]*r[3,k]
            else
                J[1+o, i]  = w[1]*r[3,k] - 2.0*w[3]*r[1,k]
                J[2+o, i]  = w[2]*r[3,k] - 2.0*w[3]*r[2,k]
                J[3+o, i]  = w[2]*r[2,k] +   w[1]*r[1,k]
            end
        end
    end
    nothing
end
function fill_J_h_buff!(J::AbstractMatrix{T}, Na, Ng) where {T}
    @inbounds for i = 1:3
        @inbounds for k = i:3:3*Ng
            J[3*Na + k,i] = one(T)
        end
    end
    nothing
end
################################################################################
# Data manipulation
################################################################################
function inverse_affine(y, T, b::Vector{SVector{3,TT}}) where {TT}
    u = [similar_type(y[k,n]) for k = 1:size(y,1), n = 1:size(y,2)]
    inverse_affine!(u, y, T, b)
    return u
end
function inverse_affine!(u, y, T, b::Vector{SVector{3,TT}}) where {TT}
    @threads for n = 1:size(y,2)
        for k = 1:size(y,1)
            u[k,n] = T[k]\(y[k,n] - b[k])
        end
    end
    nothing
end
function inverse_affine!(u, y, T, b)
    inds = reshape(1:size(y,1), 3,:)
    for k = 1:size(inds,2)
        kk = inds[:,k]
        u[kk,:] = T[:,:,k]\(y[kk,:] .- b[:,k])
    end
    nothing
end
function inverse_affine_Q(Q_inv, T::Vector{SMatrix{3,3,TT}}) where {TT}
    Qu_inv = [similar_type(Q_k) for Q_k in Q_inv]
    inverse_affine_Q!(Qu_inv, Q_inv, T)
    return Qu_inv
end
function inverse_affine_Q!(Qu_inv, Q_inv, T::Vector{SMatrix{3,3,TT,9}}) where {TT}
    for n = 1:length(Q_inv)
        Qu_inv[n] = T[n]'*Q_inv[n]*T[n]
    end
    nothing
end
function inverse_affine_Q!(Qu_inv::AbstractMatrix{TT}, Q_inv::AbstractMatrix{TT}, T::AbstractArray{TT,3}) where {TT}
    inds = reshape(1:size(Q_inv,1), 3,:)
    for k = 1:size(inds,2)
        kk = inds[:,k]
        Qu_inv[kk,kk] = T[:,:,k]'*Q_inv[kk,kk]*T[:,:,k]
    end
    nothing
end

################################################################################
# Gravity estimator
################################################################################
"""
    Should multiply D and d with number of measurements, but they take out each other.
"""
function gravity!(g::Vector{SVector{3,T}}, u, Qu_inv, Na, Nt, g_mag, tol, i_max) where {T}
    inds = @SVector [1,3]

    D::SMatrix{3,3,T} = mapreduce(k -> Qu_inv[k], +, 1:Na)
    (sig_max, sig_min) = svdvals(D)[inds]
    for n = 1:Nt
        d::SVector{3, T} = mapreduce(k -> Qu_inv[k]*u[k,n], +, 1:Na)
        g[n] = estimate_gravity(d, D, sig_min, sig_max, g_mag, tol, i_max)
    end
    nothing
end
function gravity!(g, u, Qu_inv, g_mag::T, tol, i_max) where {T}
    inds = reshape(1:size(u,1),3,:)
    Na = size(inds,2)
    D = mapreduce(k -> Qu_inv[inds[:,k], inds[:,k]], +, 1:Na)
    sigs = svdvals(D)
    (sig_max, sig_min) = sigs[1], sigs[3]
    for n = 1:size(u,2)
        d = mapreduce(k -> Qu_inv[inds[:,k], inds[:,k]]*u[inds[:,k],n], +, 1:Na)
        g[:,n] = estimate_gravity(SVector{3, T}(d), SMatrix{3,3,T}(D), sig_min, sig_max, g_mag, tol, i_max) |> Array
    end
    nothing
end
function estimate_gravity(d::SVector{3,T}, D::SMatrix{3,3,T}, s_min, s_max, g_mag,
    tol, iter_max) where {T}

    g_mid = zeros(SVector{3,T})
    d_norm = norm(d)
    lambda_up = d_norm/g_mag - s_min
    g_low = norm((D + lambda_up*I)\d)

    lambda_low = d_norm/g_mag - s_max
    g_upper = norm((D + lambda_low*I)\d)

    k = 0
    converged = false
    while !converged
        k += 1
        lambda_mid = (lambda_up + lambda_low)/2.0
        g_mid = -(D + lambda_mid*I)\d
        g_mid_norm = norm(g_mid)

        if g_mid_norm > g_mag
            (lambda_low, g_upper) = (lambda_mid, g_mid_norm)
        else
            (lambda_up, g_low) = (lambda_mid, g_mid_norm)
        end
        # lower can be higher than upper, why?
        converged = k == iter_max || abs(g_upper - g_low) < tol

        # @printf(
        # "Bisection: k = %3d |g_up - g_low| = %.5e < %.1e = tol \n", k, abs(g_upper - g_low), tol
        # )
    end
    if k == iter_max
        @warn @sprintf(
            "Bisection: Max iterations %d reached. Criterion |g_up - g_low| = %.2e < %.1e = tol \n",
            k, abs(g_upper - g_low), tol
        )
    end
    return g_mid::SVector{3,T}
end
################################################################################
# Gauss Newton
################################################################################
"""
    Need to put e, y_pred and J_m in arguments so that the type is concrete.
"""
function gauss_newton(w::SVector{3,T}, e::SVector{N,T}, y, P, r, Ng::Int, g_tol, i_max,
    y_pred, J_m) where {N, T}

    H = zero(SMatrix{3, 3, T})
    g = zero(SVector{3, T})
    p = zero(SVector{3, T})

    k = 0::Int
    converged = false

    while !converged
        k += 1

        h!(y_pred, w, r, Ng)

        e = y - SVector(y_pred)

        J_h!(J_m, w, r, Ng)
        J = SMatrix(J_m)

        J_T_P = J'*P
        H = J_T_P*J
        g = J_T_P*e

        p = H\g
        w += p

        # ||Jp||/(1+||r||
        # Should acutally be  ||RJp||/(1+||Rr||  where R'R = P
        converged = k == i_max || norm(J*p, Inf) <  g_tol*(1.0 + norm(e, Inf))
    end
    if k == i_max
        crit = norm(SMatrix(J_m)*p, Inf)/(1.0 + norm(e, Inf))

        @warn @sprintf(
            "GS: Max iterations %d reached. Criterion ||Jp||/(1+||r||) = %.2e < %.1e = tol \n",
            k, crit, g_tol
        )
    end

    w::SVector{3,T}
end
"""
    min f(w)
    over w

    Using Gauss Newton

    f(w) = (y - h(w,r))^T * P*(y - h(w,r))
"""
function gauss_newton(w::AbstractVector{T}, y::AbstractVector{T}, P, r, Ng::Int, g_tol, i_max;
                      warn = true) where {T}

    N = length(y)

    H = zeros(T,3,3)   # Hessian
    g = zeros(T,3)     # Gradient
    p = zeros(T,3)     # Step

    J = zeros(T,N,3)
    y_pred = zeros(T,N)
    e = zeros(T,N)

    J_T_P = zeros(T, 3, N)

    Na = size(r,2)
    fill_J_h_buff!(J, Na, Ng)

    k = 0::Int
    converged = false

    while !converged
        k += 1

        h!(y_pred, w, r, Ng)
        e .= y - y_pred

        J_h!(J, w, r, Ng)

        J_T_P .= J' * P
        H .= J_T_P * J
        g .= J_T_P * e

        p .= H \ g
        w += p

        # ||Jp||/(1+||r||
        # Should acutally be  ||RJp||/(1+||Rr||  where R'R = P
        converged = k == i_max || norm(J * p, Inf) <  g_tol * (1.0 + norm(e, Inf))
    end
    if warn && k == i_max
        crit = norm(J * p, Inf) / (1.0 + norm(e, Inf))

        @warn @sprintf(
            "GS: Max iterations %d reached. Criterion ||Jp||/(1+||r||) = %.2e < %.1e = tol \n",
            k, crit, g_tol
        )
        converged = false
    end

    w, converged
end
function dynamics(w0::Vector{SVector{3,T}}, u, Qu_inv, r, Ng, ::Val{N_sens},
    tol_gs, i_max_w) where {T, N_sens}

    N_dynamics = length(w0)

    w_dot = [zero(SVector{3,T}) for _ = 1:N_dynamics]
    s = [zero(SVector{3,T}) for _ = 1:N_dynamics]

    Qu_inv_tall::SMatrix{N_sens, N_sens, T, N_sens^2} =
        SMatrix{N_sens, N_sens, T, N_sens^2}(cat(Qu_inv...; dims = (1,2)))
    u_n = [zero(SVector{N_sens,T}) for _ = 1:N_dynamics]
    for n = 1:N_dynamics
        u_n[n] = vcat(u[:,n]...)
    end
    w = [copy(w0_n) for w0_n in w0]
    dynamics!!(w, w_dot, s, u_n, Qu_inv_tall, r, Ng, tol_gs, i_max_w)

    return w, w_dot, s
end
function dynamics!!(w::Vector{SVector{3,T}}, w_dot::Vector{SVector{3,T}}, s::Vector{SVector{3,T}},
    u::Vector{SVector{N,T}}, Qu_inv::SMatrix{N,N,T},
    r, Ng, g_tol, i_max) where{N,T}
    N_dynamics = length(w)
    Na = size(r, 2)

    H_m = zero(MMatrix{N, 6, T})
    fill_H_constants!(H_m, Na)
    H!(H_m, r)
    H = SMatrix(H_m)
    e = zero(SVector{N, T})

    phi = zero(SVector{6, T})
    inds_w_dot = @SVector [1,2,3]
    inds_s = @SVector [4,5,6]

    # Buffs
    y_pred = zero(MVector{N, T})
    J_m = zero(MMatrix{N, 3, T})
    fill_J_h_buff!(J_m, Na, Ng)

    H_T_Q_inv = H'*Qu_inv
    WLS = (H_T_Q_inv*H)\H_T_Q_inv
    P = Qu_inv - H_T_Q_inv'*WLS

    r_m = hcat(r...)
    for n = 1:N_dynamics
        w[n] = gauss_newton(w[n], e, u[n], P, r_m, Ng, g_tol, i_max, y_pred, J_m)
        h!(y_pred, w[n], r_m, Ng)
        e = u[n] - SVector(y_pred)
        phi = WLS*e
        w_dot[n] = phi[inds_w_dot]
        s[n] = phi[inds_s]
    end
    w, w_dot, s
end
################################################################################
# Callbacks
################################################################################
function get_callback(show_every = 1)
    function callback(k::Int, dx, dlogp, log_p, r, T, b)
        if mod(k,show_every) == 0
            @printf("%9d logp %.18e  dlogp % .5e dx % .5e\n", k, log_p, dlogp, dx)
        end
    end
    return callback
end
function get_callback_maxdiff_param(r_true, T_true, b_true, show_every = 1)

    callback(k::Int, dlogp, log_p, r, T, b) = begin
        if mod(k, show_every) == 0
            r_e = norm(r - r_true, Inf)
            T_e = [norm(T[k] - T_true[k]) for k = 1:length(T)] |> x  -> norm(x, Inf)
            b_e = [norm(b[k] - b_true[k]) for k = 1:length(b)] |> x  -> norm(x, Inf)
            @printf("%15d logp %.3e  dlogp %.3e r_e %.4e T_e %.4e b_e %.4e \n", k, log_p, dlogp,
            r_e, T_e, b_e)
            # @printf(" C_norm %.8e\n", tr(T[4])/3)
        end
        # println(k, " ", log_p)
    end

    return callback

end

function maxdiff(x::AbstractArray, y::AbstractArray)
    res = real(zero(x[1] - y[1]))
    @inbounds for i in 1:length(x)
        delta = abs(x[i] - y[i])
        if delta > res
            res = delta
        end
    end
    return res
end
function maxdiff(r, T, b, r_p, T_p, b_p)

    dr = (maxdiff(r[k], r_p[k]) for k in eachindex(r))
    dT = (maxdiff(T[k], T_p[k]) for k in eachindex(T))
    db = (maxdiff(b[k], b_p[k]) for k in eachindex(b))

    d = reduce(max, dr)
    d = max(d, reduce(max, dT))
    max(d, reduce(max, db))
end
################################################################################
# BCD loop
################################################################################
function bcd!(r,T,b, y, Q_inv, Ns, g_mag::TT, ::Val{N_sens}, ::Val{Na},
              tol_interval, i_max_g, tol_gs, i_max_w, tol_bcd_dlogp, tol_bcd_dx, i_max_bcd;
              callback = nothing, warn = true, w0 = nothing, verbose = false,
              update_Ta = true, update_Tg = true) where {TT, N_sens, Na}

    display(T[1])
    @assert istriu(T[1])
    @assert r[1] == zeros(3)
    @assert length(T) == length(b) == size(y,1) == length(Q_inv)
    @assert length(r) == Na

    N = size(y, 2)
    N_triads = size(y, 1)
    Ng = N_triads - Na
    N_orientions = length(Ns)
    N_dynamics = N - N_orientions

    println("N dynamics: ", N_dynamics)
    r_m = zero(MMatrix{3,Na,TT})
    for k = 2:Na
        r_m[:,k] = r[k]
    end
    u = [zero(SVector{3,TT}) for i = 1:N_triads, j = 1:N]
    Qu_inv = [zero(SMatrix{3,3,TT}) for i = 1:N_triads]

    # Statics
    g = [zero(SVector{3,TT}) for _ = 1:N_orientions]

    # Dynamics
    u_m = zero(MVector{N_sens,TT})
    u_n = [zero(SVector{N_sens,TT}) for _ = 1:N_dynamics]
    Qu_inv_tall = zero(MMatrix{N_sens, N_sens, TT, N_sens^2})
    # Initial w0 from gyroscopes
    if w0 == nothing
        w = map(1:N_dynamics) do n
            mapreduce(k -> y[Na + k, n + N_orientions], +, 1:Ng)/Ng
        end
    else
        w = copy(w0)
    end

    w_dot = [zero(SVector{3,TT}) for _ = 1:N_dynamics]
    s = [zero(SVector{3,TT}) for _ = 1:N_dynamics]

    u_pred_all = [zero(MVector{N_sens, TT}) for _ = 1:N_dynamics]
    # WLS = zero(SMatrix{6, N_sens, TT})
    H_m = zero(MMatrix{N_sens, 6, TT})
    fill_H_constants!(H_m, Na)

    # Buffs
    u_pred = [zero(MVector{N_sens, TT}) for _ = 1:nthreads()]
    J_m = map(1:nthreads()) do n
        J = zero(MMatrix{N_sens, 3, TT})
        fill_J_h_buff!(J, Na, Ng)
        J
    end

    Jrs = [zero(SMatrix{3,3,TT}) for _ = 1:N_dynamics]
    Tg = zero(MMatrix{3,3,TT})

    ii_T_1 = SVector{6,Int}([1,2,3,4,5,6])
    ii_b_1 = SVector{3,Int}([7,8,9])

    ii_T = SVector{9,Int}([1,2,3,4,5,6,7,8,9])
    ii_b = SVector{3,Int}([10,11,12])

    inds_w_dot = SVector{3,Int}([1,2,3])
    inds_s = SVector{3,Int}([4,5,6])

    if update_Ta && update_Tg
        slice = 2:N_triads
    elseif update_Ta && !update_Tg
        slice = 2:Na
    elseif !update_Ta && update_Tg
        slice = Na+1:N_triads
    else
        slice = 2:0
    end

    k = 0
    r_p = copy(r)
    T_p = copy(T)
    b_p = copy(b)
    log_p_prev::TT = Inf
    dlogp = zero(TT)
    logp = zero(TT)
    dx = zero(TT)
    converged = false
    while !converged
        k += 1

        inverse_affine!(u, y, T, b)
        inverse_affine_Q!(Qu_inv, Q_inv, T)

        if verbose
            lc = nlogp_naive(y, Q_inv, Ns, r, T, b, g, w, w_dot,s)
            @printf "1: %.3e\n" lc
            lp = lc
        end

        gravity!(g, u, Qu_inv, Na, N_orientions, g_mag, tol_interval, i_max_g)

        if verbose
            lc = nlogp_naive(y, Q_inv, Ns, r, T, b, g, w, w_dot,s)
            @printf "2: %.3e diff %.3e\n" lc lp-lc
            lp = lc
        end

        # Dynamics
        for k = 1:N_triads
            kk = 1+3*(k-1):3*k
            Qu_inv_tall[kk,kk] = Qu_inv[k]
        end
        for n = 1:N_dynamics
            for k = 1:N_triads
                u_m[1+3(k-1):3*k] = u[k,n+N_orientions]
            end
            u_n[n] = SVector(u_m)
        end

        H!(H_m, r)
        H = SMatrix(H_m)
        H_T_Q_inv = H'*SMatrix(Qu_inv_tall)
        H_T_Q_inv_H = H_T_Q_inv*H
        WLS = H_T_Q_inv_H\H_T_Q_inv
        P2 = H_T_Q_inv'*WLS
        P = SMatrix(Qu_inv_tall) - P2

        @threads for n = 1:N_dynamics
            e = zero(SVector{N_sens, TT})
            phi = zero(SVector{6, TT})
            u_pred_i = u_pred[threadid()]
            J_m_i = J_m[threadid()]

            w[n] = gauss_newton(w[n], e, u_n[n], P, r_m, Ng, tol_gs, i_max_w,
                                u_pred_i, J_m_i)
            h!(u_pred_i, w[n], r_m, Ng)
            u_pred_all[n] = copy(u_pred_i)
            e = u_n[n] - SVector(u_pred_i)
            phi = WLS*e
            w_dot[n] = phi[inds_w_dot]
            s[n] = phi[inds_s]
        end

        if verbose
            lc = nlogp_naive(y, Q_inv, Ns, r, T, b, g, w, w_dot,s)
            @printf "3: %.3e diff %.3e\n" lc lp-lc
            lp = lc
        end

        # Positions
        @threads for n = 1:N_dynamics
            Jrs[n] = J_r(w[n], w_dot[n])
        end
        @threads for k = 2:Na
            A_r, a_r = zero(SMatrix{3,3, TT}), zero(SVector{3, TT})
            for n = 1:N_dynamics
                J = Jrs[n]'*Qu_inv[k]
                A_r += J*Jrs[n]
                a_r += J*(u[k,n + N_orientions] - s[n])
            end
            r[k] = A_r\a_r
            r_m[:,k] = r[k]
        end

        if verbose
            lc = nlogp_naive(y, Q_inv, Ns, r, T, b, g, w, w_dot,s)
            @printf "4: %.3e diff %.3e\n" lc lp-lc
            lp = lc
        end

        # Update u
        @threads for n = 1:N
            for k = 1:N_triads
                # Static
                if n <= N_orientions
                    if k <= Na
                        u[k,n] = -g[n]
                    else
                        u[k,n] = zero(SVector{3, TT})
                    end
                else
                    n_d = n - N_orientions
                    if k <= Na
                        u[k,n] = Jrs[n_d]*r[k] + s[n_d]
                    else
                        u[k,n] = w[n_d]
                    end
                end
            end
        end

        # Ref Tb
        if update_Ta
            A_Tb_1, a_Tb_1 = zero(SMatrix{9,9, TT}), zero(SVector{9, TT})
            for n = 1:N
                A_1 = J_Tb_ref(u[1,n])
                A_T_Q_inv_1 = A_1'*Q_inv[1]
                if n <= N_orientions
                    A_T_Q_inv_1 *= Ns[n]
                end
                A_Tb_1 += A_T_Q_inv_1*A_1
                a_Tb_1 += A_T_Q_inv_1*y[1,n]
            end
            x_1 = A_Tb_1\a_Tb_1
            T[1] = T_ref(x_1[ii_T_1])
            b[1] = x_1[ii_b_1]
        end
        if verbose
            lc = nlogp_naive(y, Q_inv, Ns, r, T, b, g, w, w_dot,s)
            @printf "5: %.3e diff %.3e\n" lc lp-lc
            lp = lc
        end

        # T and b estimation
        # Dont update T_g if only static measurements
        @threads for k = slice
            A_Tb, a_Tb = zero(SMatrix{12,12, TT}), zero(SVector{12, TT})

            for n = 1:N
                A = J_Tb(u[k,n])
                A_T_Q_inv = A'*Q_inv[k]
                if n <= N_orientions
                    A_T_Q_inv *= Ns[n]
                end
                A_Tb += A_T_Q_inv*A
                a_Tb += A_T_Q_inv*y[k,n]
            end

            x = A_Tb\a_Tb
            T[k] = SMatrix{3,3, TT}(reshape(x[ii_T], 3,3))
            b[k] = x[ii_b]
        end

        if verbose
            lc = nlogp_naive(y, Q_inv, Ns, r, T, b, g, w, w_dot,s)
            @printf "6: %.3e diff %.3e\n" lc lp-lc
            lp = lc
        end

        # Normalize Tg
        if update_Tg
            C::TT = 0.0
            for k = 1:Ng
                C += tr(T[k+Na])
            end
            C /= (3*Ng)
            for k = 1:Ng
                Tg .= MMatrix(T[k+Na])
                for i = 1:3
                    Tg[i,i] /= C
                end
                T[k+Na] = SMatrix(Tg)
            end
        end

        if verbose
            lc = nlogp_naive(y, Q_inv, Ns, r, T, b, g, w, w_dot,s)
            @printf "7: %.3e diff %.3e\n" lc lp-lc
            lp = lc
        end

        # Logp
        logp = nlogp(y, u, T, b, Q_inv, Ns)
        dlogp = log_p_prev - logp
        log_p_prev = logp

        dx = maxdiff(r, T, b, r_p, T_p, b_p)

        r_p .= r
        T_p .= T
        b_p .= b

        if callback != nothing
            callback(k, dx, dlogp, logp, r, T, b)
        end

        converged = k == i_max_bcd || abs(dlogp) < tol_bcd_dlogp || dx < tol_bcd_dx
    end
    if k == i_max_bcd && warn
        @warn join(
        [@sprintf("BCD: Max iterations %d reached", k),
         @sprintf("Criterion |dlopp| = %.2e < %.1e = tol",abs(dlogp), tol_bcd_dlogp),
         @sprintf("Criterion ||dx||_∞ = %.2e < %.1e = tol", dx, tol_bcd_dx)],
        "\n"
        )
    end
    eta = Dict(
        :w => w,
        :w_dot => w_dot,
        :s => s,
        :g => g
    )
    return r, T, b, eta
end

function bcd(r, T, b, y, Q_inv, Ns, g_mag,
    tol_interval, i_max_g, tol_gs, i_max_w, tol_bcd_dlogp, tol_bcd_dx, i_max_bcd;
    kwargs...)

    TT = Float64

    Na = size(r,2)
    Nt = size(T,3)

    rs = [SVector{3,TT}(r[:,k]) for k = 1:Na]
    bs = [SVector{3,TT}(b[:,k]) for k = 1:Nt]
    Ts = [SMatrix{3,3,TT}(T[:,:,k]) for k = 1:Nt]

    ys = [SVector{3,TT}(y[1+3(k-1):3*k, n]) for k = 1:Nt, n = 1:size(y,2)]
    Q_inv_s = [SMatrix{3,3,TT}(Q_inv[:,:,k]) for k = 1:Nt]

    r_hat, T_hat, b_hat, eta_hat = bcd!(rs, Ts, bs, ys, Q_inv_s, Ns, g_mag,
                                        Val{3*Nt}(),Val{Na}(),
                                        tol_interval, i_max_g,
                                        tol_gs, i_max_w,
                                        tol_bcd_dlogp, tol_bcd_dx, i_max_bcd;
                                        kwargs...)

    eta = map(eta_hat |> collect) do (k,v)
        a = hcat(Array.(v)...)
        (k,a)
    end |> Dict
    theta = Dict(
        :r => hcat(Array.(r_hat)...),
        :b => hcat(Array.(b_hat)...),
        :T => cat(Array.(T_hat)...; dims = 3)
    )

    theta, eta
end
function bcd!(r::AbstractMatrix{type}, T::AbstractArray{type,3}, b::AbstractMatrix{type},
              y::AbstractMatrix{type}, Q_inv::AbstractMatrix{type},
              Ns, g_mag::type,
              tol_interval, i_max_g, tol_gs, i_max_w, tol_bcd_dlogp, tol_bcd_dx, i_max_bcd;
              callback = nothing, warn = true, w0 = nothing,
              update_Ta = true, update_Tg = true,
              verbose = false) where {type}

    N_sens = size(y,1)
    Na = size(r,2)
    N_triads = Int(N_sens/3)
    Ng =  N_triads - Na

    @assert istriu(T[:,:,1])
    @assert r[:,1] == zeros(3)
    @assert size(y,1) == size(Q_inv,1) == size(Q_inv,2) == length(b)

    N = size(y,2)
    N_orientions = length(Ns)
    N_dynamics = N - N_orientions
    if verbose
        println("N dynamics: ", N_dynamics)
        println("N orientations: ", N_orientions)
    end

    inds = reshape(1:3*N_triads, 3, :)

    # Initial w0 from gyroscopes
    if w0 == nothing
        @assert Ng > 0
        y_g = y[3*Na + 1:end, N_orientions + 1:end]

        # Mean value
        w = map(1:3) do k
            sum(y_g[k:3:end, :]; dims = 1)/Ng
        end |> x -> vcat(x...)
    else
        w = copy(w0)
    end
    w_dot = zeros(type,3, N_dynamics)
    s = zeros(type,3, N_dynamics)
    g = zeros(type,3, N_orientions)

    u = zeros(type, size(y))
    Qu_inv = zeros(type, size(Q_inv))

    H = zeros(type, N_sens, 6)
    fill_H_constants_2!(H, Na)

    Jrs = zeros(type, 3, 3, N_dynamics)
    u_dyn_pred = zeros(type,3*N_triads, N_dynamics)

    if update_Ta && update_Tg
        slice = 2:N_triads
    elseif update_Ta && !update_Tg
        slice = 2:Na
    elseif !update_Ta && update_Tg
        slice = Na+1:N_triads
    else
        slice = 2:0
    end

    # Convergence stuff
    k = 0
    theta_p = vcat(r[:], T[:], b[:])
    log_p_prev::type = Inf
    dlogp = zero(type)
    logp = zero(type)
    dx = zero(type)
    converged = false
    converged_bisection = zeros(Bool, N_orientions, i_max_bcd)
    converged_gauss_newton = zeros(Bool, N_dynamics, i_max_bcd)

    while !converged
        k += 1
        if verbose
            println("BCD iteration: $(k)")
        end

        inverse_affine!(u, y, T, b)
        inverse_affine_Q!(Qu_inv, Q_inv, T)

        if verbose
            lc = nlogp_naive(y, Q_inv, Ns, r, T, b, g, w, w_dot,s)
            @printf "1: %.3e\n" lc
            lp = lc
        end

        gravity!(g, u[1:3*Na,1:N_orientions], Qu_inv, g_mag, tol_interval, i_max_g)

        if verbose
            lc = nlogp_naive(y, Q_inv, Ns, r, T, b, g, w, w_dot,s)
            @printf "2: %.3e diff %.3e\n" lc lp-lc
            lp = lc
        end

        # Dynamics
        H!(H, r)
        # Since H is zero for gyro scope measurements
        # P = Q_inv - Q_inv*H*(H'*Q_inv*H)^{-1}*H'*Q_inv
        WLS = (H'*Qu_inv*H)\(H'*Qu_inv)
        P = Qu_inv  - Qu_inv*H*WLS
        u_dyn = u[:,1 + N_orientions:end]
        @threads for n = 1:N_dynamics

            if verbose
                println("GS iteration: $(n)")
            end

            w[:,n], converged_gauss_newton[n,k] = gauss_newton(
                w[:,n], u_dyn[:,n], P, r, Ng, tol_gs, i_max_w;
                warn = warn
            )
            if !converged_gauss_newton[n,k]
                @warn @sprintf("BCD Iteration %d, time sample %d did not converge in GS", k, n)
            end

            @views h!(u_dyn_pred[:,n], w[:,n], r, Ng)
        end

        phi = WLS*(u_dyn - u_dyn_pred)
        w_dot[:,:] = phi[1:3,:]
        s[:,:] = phi[4:6,:]

        if verbose
            lc = nlogp_naive(y, Q_inv, Ns, r, T, b, g, w, w_dot,s)
            @printf "3: %.3e diff %.3e\n" lc lp-lc
            lp = lc
        end

        # Positions
        @threads for n = 1:N_dynamics
            Jrs[:,:,n] = J_r(w[:,n], w_dot[:,n])
        end
        @threads for k = 2:Na
            kk = inds[:,k]
            A_r, a_r = zeros(type,3,3), zeros(type,3)
            for n = 1:N_dynamics
                J = Jrs[:,:,n]'*Qu_inv[kk,kk]
                A_r += J*Jrs[:,:,n]
                a_r += J*(u[kk,n + N_orientions] - s[:,n])
            end
            r[:,k] = A_r\a_r
        end

        if verbose
            lc = nlogp_naive(y, Q_inv, Ns, r, T, b, g, w, w_dot,s)
            @printf "4: %.3e diff %.3e\n" lc lp-lc
            lp = lc
        end
        # Update u
        @threads for n = 1:N
            for k = 1:N_triads
                kk = inds[:,k]
                # Static
                if n <= N_orientions
                    if k <= Na
                        u[kk,n] = -g[:,n]
                    else
                        u[kk,n] = zeros(type,3)
                    end
                else
                    n_d = n - N_orientions
                    if k <= Na
                        u[kk,n] = Jrs[:,:,n_d]*r[:,k] + s[:,n_d]
                    else
                        u[kk,n] = w[:,n_d]
                    end
                end
            end
        end

        # Ref Tb
        if update_Ta
            A_Tb_1, a_Tb_1 = zeros(type,9,9), zeros(type,9)
            for n = 1:N
                A_1 = J_Tb_ref(u[1:3,n])
                A_T_Q_inv_1 = A_1'*Q_inv[1:3,1:3]
                if n <= N_orientions
                    A_T_Q_inv_1 *= Ns[n]
                end
                A_Tb_1 += A_T_Q_inv_1*A_1
                a_Tb_1 += A_T_Q_inv_1*y[1:3,n]
            end
            x_1 = A_Tb_1\a_Tb_1
            T[:,:,1] = T_ref(x_1[1:6])
            b[:,1] = x_1[7:9]
        end
        if verbose
            lc = nlogp_naive(y, Q_inv, Ns, r, T, b, g, w, w_dot,s)
            @printf "5: %.3e diff %.3e\n" lc lp-lc
            lp = lc
        end

        # T and b estimation
        @threads for k = slice
            A_Tb, a_Tb = zeros(type,12,12), zeros(type,12)
            kk = inds[:,k]
            for n = 1:N
                A = J_Tb(u[kk,n])
                A_T_Q_inv = A'*Q_inv[kk,kk]
                if n <= N_orientions
                    A_T_Q_inv *= Ns[n]
                end
                A_Tb += A_T_Q_inv*A
                a_Tb += A_T_Q_inv*y[kk,n]
            end

            x = A_Tb\a_Tb
            T[:,:,k] = reshape(x[1:9], 3,3)
            b[:,k] = x[10:12]
        end

        if verbose
            lc = nlogp_naive(y, Q_inv, Ns, r, T, b, g, w, w_dot,s)
            @printf "6: %.3e diff %.3e\n" lc lp-lc
            lp = lc
        end
        # Normalize Tg
        if update_Tg
            C::type = 0.0
            for k = 1:Ng
                C += tr(T[:,:,k+Na])
            end
            C /= (3*Ng)
            for k = 1:Ng
                T[:,:,k+Na] ./= C
            end
        end

        if verbose
            lc = nlogp_naive(y, Q_inv, Ns, r, T, b, g, w, w_dot,s)
            @printf "7: %.3e diff %.3e\n" lc lp-lc
            lp = lc
        end

        # Logp
        logp = nlogp(y, u, T, b, Q_inv, Ns)
        dlogp = log_p_prev - logp
        log_p_prev = logp
        theta = vcat(r[:], T[:], b[:])
        dx = maxdiff(theta, theta_p)

        theta_p .= theta

        if callback != nothing
            params = Dict(
                :r => r,
                :T => T,
                :b => b,
                :w => w,
                :w_dot => w_dot,
                :s => s,
                :g => g,
            )
            callback(k, dx, dlogp, logp, params)
        end

        converged = k == i_max_bcd || abs(dlogp) < tol_bcd_dlogp || dx < tol_bcd_dx
    end
    if k == i_max_bcd && warn
        @warn join(
            [@sprintf("BCD: Max iterations %d reached", k),
             @sprintf("Criterion |dlopp| = %.2e < %.1e = tol",abs(dlogp), tol_bcd_dlogp),
             @sprintf("Criterion ||dx||_∞ = %.2e < %.1e = tol", dx, tol_bcd_dx)],
            "\n"
        )
    end
    theta = Dict(
        :r => r,
        :T => T,
        :b => b,
    )
    eta = Dict(
        :w => w,
        :w_dot => w_dot,
        :s => s,
        :g => g,
    )
    convergence_assessment = Dict(
        :gs_iterations => converged_gauss_newton
    )
    return theta, eta, convergence_assessment
end

end # module


text:
import Setfield
using StaticArrays: @SVector, normalize
using Setfield: setproperties
export position
export direction

function position end
function direction end

Setfield.set(o, ::typeof(@lens position(_)), val) = set_position(o, val)
Setfield.set(o, ::typeof(@lens direction(_)), val) = set_direction(o, val)

function direction(p)
    @SVector [p.u, p.v, p.w]
end

function position(p::EGSParticle; z=nothing)
    if z === nothing
        @SVector[p.x, p.y]
    else
        @SVector[p.x, p.y, z]
    end
end

function position(p::AbstractIAEAParticle)
    @SVector[p.x, p.y, p.z]
end

function set_direction(p, dir)
    u,v,w = dir
    setproperties(p, (u=u,v=v,w=w))
end

function set_position(p::IAEAParticle, pos)
    x,y,z = pos
    setproperties(p, (x=x,y=y,z=z))
end

function set_position(p::EGSParticle, pos)
    x,y = pos
    setproperties(p, (x=x,y=y))
end

function set_position_direction(p::EGSParticle, pos, dir)
    x,y = pos
    u,v,w = dir
    setproperties(p, (x=x,y=y,u=u,v=v,w=w))
end

function set_position_direction(p::IAEAParticle, pos, dir)
    x,y,z = pos
    u,v,w = dir
    setproperties(p, (x=x,y=y,z=z,u=u,v=v,w=w))
end

ext: jl


text:
abstract type AbstractDirichletBC <: BCType end
abstract type AbstractNeumannBC <: BCType end
export isDirichlet, isNeumann

mutable struct DirichletAllZero <: AbstractDirichletBC
end
function (obj::DirichletAllZero)(
                          xy::AbstractArray{Tmsh},
                          gD::AbstractArray{Tsol, 1}) where {Tmsh, Tsol}  # (numDofPerNode))
  gD[:] = 0.0
  return nothing
end

mutable struct DirichletAllOne <: AbstractDirichletBC
end
function (obj::DirichletAllOne)(
                          xy::AbstractArray{Tmsh},
                          gD::AbstractArray{Tsol, 1}) where {Tmsh, Tsol}  # (numDofPerNode)
  gD[:] = 1.0
  return nothing
end

mutable struct DirichletTrig <: AbstractDirichletBC
end
function (obj::DirichletTrig)(
                          xy::AbstractArray{Tmsh},
                          q::AbstractArray{Tsol, 1}) where {Tmsh, Tsol}  # (numDofPerNode))
  k = 2.0
  q[:] = sin(2*k*pi*xy[1])*sin(2*k*pi*xy[2])
  if abs(q[1]) > 1.0E-10
    error("some problem!")
  end
  return nothing
end

mutable struct DirichletPolynial2nd <: AbstractDirichletBC
end
function (obj::DirichletPolynial2nd)(
                          xy::AbstractArray{Tmsh},
                          gD::AbstractArray{Tsol, 1}) where {Tmsh, Tsol}  # (numDofPerNode)
  a = 1.0
  b = 1.0
  c = 1.0
  gD[:] =  a*xy[1]*xy[1] + b*xy[1]*xy[2] + c*xy[2]*xy[2]
  # gD[:] =  -0.25*xy[1]*xy[1] - 0.25*xy[2]*xy[2]
end

mutable struct NeumannAllZero <: AbstractNeumannBC
end

function (obj::NeumannAllZero)(
                          xy::AbstractArray{Tmsh, 1},
                          gN::AbstractArray{Tsol, 1}) where {Tmsh, Tsol}  # (numDofPerNode)
  gN[:] = 1.0
  return nothing
end

mutable struct NeumannAllOne <: AbstractNeumannBC
end

function (obj::NeumannAllOne)(
                          xy::AbstractArray{Tmsh, 1},
                          gN::AbstractArray{Tsol, 1}) where {Tmsh, Tsol}  # (numDofPerNode)
  gN[:] = 1.0
  return nothing
end

global const BCDict = Dict{String, BCType}(
                                                "DirichletAllZero" => DirichletAllZero(),
                                                "DirichletAllOne" => DirichletAllOne(),
                                                "NeumannAllZero" => NeumannAllZero(),
                                                "NeumannAllOne" => NeumannAllOne(),
                                                "DirichletPolynial2nd" => DirichletPolynial2nd(),
                                                "DirichletTrig" => DirichletTrig()
                                               )

function isDirichlet(dBC::AbstractDirichletBC)
  return true
end
function isDirichlet(dBC::AbstractNeumannBC)
  return false
end

function isNeumann(dBC::AbstractDirichletBC)
  return false
end
function isNeumann(dBC::AbstractNeumannBC)
  return true
end

function getBCFunctors(mesh::AbstractMesh, sbp::AbstractOperator, eqn::EllipticData, opts)
  for i = 1:mesh.numBC
    key = string("BC", i, "_name")
    val = opts[key]
    mesh.bndry_funcs[i] = BCDict[val]
  end
end
function interpolateBoundary(mesh::AbstractDGMesh,
                             sbp::AbstractOperator,
                             eqn::AbstractEllipticData{Tsol, Tres},
                             opts,
                             q::AbstractArray{Tsol,3},
                             q_bndry::AbstractArray{Tsol,3}) where {Tsol, Tres}
  boundaryinterpolate!(mesh.sbpface, mesh.bndryfaces, q, q_bndry)
end

function interpolateBoundary(mesh::AbstractDGMesh,
                             sbp::AbstractOperator,
                             eqn::AbstractEllipticData{Tsol, Tres},
                             opts,
                             grad::AbstractArray{Tsol,4},
                             grad_bndry::AbstractArray{Tsol,4}) where {Tsol, Tres}
  @assert(size(grad, 4) == size(grad_bndry, 4))   # Tdim
  @assert(size(grad, 1) == size(grad_bndry, 1))   # numDofPerNode

  dim = size(grad, 4)
  for d=1:dim
    g = sview(grad, :, :, :, d)
    g_bndry = sview(grad_bndry, : , : , :, d)
    boundaryinterpolate!(mesh.sbpface, mesh.bndryfaces, g, g_bndry)
  end
end

function getBCFluxes(mesh::AbstractMesh{Tmsh},
                            sbp::AbstractOperator,
                            eqn::EllipticData{Tsol, Tres, Tdim},
                            opts) where {Tmsh, Tsol, Tres, Tdim}
  #
  # The boundary integrals are categorized into 3 classes.
  # The first is numerical and applies to all boundaries (both Dirichlet
  # and Neumann). The second class is `physical` Dirichlet and the third
  # one is `physical` Neumann. These 3 classes are dealed with separatedly
  # in 3 loops.
  #

  #
  # Take into account Direclet and Neumann boundary conditions
  #
  p = opts["order"]
  Cip = opts["Cip"]
  penalty_method = opts["Flux_name"]
  sbpface = mesh.sbpface
  dq = Array{Tsol}(mesh.numDofPerNode, mesh.numNodesPerFace)
  penalty_factor_shahbazi = 0.5*Float64(p + 1.0)*Float64(p + Tdim)/Float64(Tdim)
  penalty = Array{Tsol}(mesh.numDofPerNode, mesh.numNodesPerFace)
  penalty_shahbazi = Array{Tsol}(mesh.numDofPerNode, mesh.numNodesPerFace)
  sbpface = mesh.sbpface
  numFacesPerElem = 3
  nrm = Array{Tmsh}(Tdim, mesh.numNodesPerFace)
  nrm1 = Array{Tmsh}(Tdim, mesh.numNodesPerFace)
  area = Array{Tmsh}(mesh.numNodesPerFace)
  numFacesPerElem = 3

  eigMax = Array{Tmsh}(mesh.numDofPerNode)
  lambda_dqdx = Array{Tsol}(Tdim, mesh.numDofPerNode, mesh.numNodesPerFace)

  Sat = Array{Tmsh}(mesh.numDofPerNode, mesh.numNodesPerFace, mesh.numNodesPerFace)
  RLR = Array{Tmsh}(mesh.numDofPerNode, mesh.numNodesPerFace, mesh.numNodesPerFace)
  R = sview(sbpface.interp, :,:)
  RR = R.'*R
  area_sum = sview(eqn.area_sum, :)

  perm = zeros(Tmsh, sbp.numnodes, sbpface.stencilsize)
  Hinv = zeros(Tmsh, sbp.numnodes, sbp.numnodes)
  Bsqrt = zeros(Tmsh, sbpface.numnodes, sbpface.numnodes)
  for s = 1:sbpface.stencilsize
    perm[sbpface.perm[s, 1], s] = 1.0
  end
  for i = 1:sbp.numnodes
    Hinv[i,i] = 1.0/sbp.w[i]
  end
  for i = 1:sbpface.numnodes
    Bsqrt[i,i] = sqrt(sbpface.wface[i])
  end
  BsqrtRHinvRtBsqrt = Bsqrt*R.'*perm.'*Hinv*perm*R*Bsqrt
  sigma = eigmax(BsqrtRHinvRtBsqrt)

  relax_coef = 1.0
  if haskey(opts, "unstable_coef")
    relax_coef = opts["unstable_coef"]
  end

  for iBC = 1:mesh.numBC
    indx0 = mesh.bndry_offsets[iBC]
    indx1 = mesh.bndry_offsets[iBC+1] - 1
    gD = Array{Tsol}(mesh.numDofPerNode, mesh.numNodesPerFace)
    gg = Array{Tsol}(mesh.numDofPerNode)
    gN = Array{Tsol}(mesh.numDofPerNode)

    for f = indx0:indx1
      xflux = sview(eqn.xflux_bndry, :,:,f)
      yflux = sview(eqn.yflux_bndry, :,:,f)
      flux  = sview(eqn.flux_bndry, :,:,f)
      bndry = mesh.bndryfaces[f]
      elem = bndry.element

      #
      # Compute geometric info on face
      #
      nrm = ro_sview(mesh.nrm_bndry, :, :, f)
      for n=1:mesh.numNodesPerFace
        area[n] = norm(ro_sview(nrm, :, n))
        nrm1[:,n] = nrm[:,n] / area[n]
      end

      dqdx = sview(eqn.q_grad_bndry, :, :, f, 1)
      dqdy = sview(eqn.q_grad_bndry, :, :, f, 2)

      lambda = sview(eqn.lambda_bndry, :,:,:,:,f)

      for n = 1:mesh.numNodesPerFace
        for var = 1:mesh.numDofPerNode
          lambda_dqdx[1, var, n] = lambda[1, 1, var, n]*dqdx[var, n] + lambda[1, 2, var, n]*dqdy[var, n]
          lambda_dqdx[2, var, n] = lambda[2, 1, var, n]*dqdx[var, n] + lambda[2, 2, var, n]*dqdy[var, n]
        end
      end

      for n = 1:mesh.numNodesPerFace
        xy = sview(mesh.coords_bndry, :, n, f)
        q = sview(eqn.q_bndry, :, n, f)
        #
        # term-2
        #
        xflux[:, n] = -q[:]*nrm[1, n]
        yflux[:, n] = -q[:]*nrm[2, n]
        #
        # term-3
        #
        flux[:, n] = -lambda_dqdx[1, :, n]*nrm[1, n] - lambda_dqdx[2, :, n]*nrm[2, n]
      end
    end

    #
    # physical Dirichlet
    #
    bc_func = mesh.bndry_funcs[iBC]
    if isDirichlet(bc_func) # Dirichlet BC, term-5 and term-7
      for f = indx0:indx1
        xflux = sview(eqn.xflux_bndry, :,:,f)
        yflux = sview(eqn.yflux_bndry, :,:,f)
        flux  = sview(eqn.flux_bndry, :,:,f)
        bndry = mesh.bndryfaces[f]
        elem = bndry.element
        face = bndry.face
        perm = sview(sbpface.perm, :, face)
        stencilSize = sbpface.stencilsize
        #
        # Compute geometric info on face
        #
        nrm = ro_sview(mesh.nrm_bndry, :, :, f)
        for n = 1 : mesh.numNodesPerFace
          # area[n] = norm(view(nrm, :, n))
          area[n] = norm(ro_sview(nrm, :, n))
          nrm1[1, n] = nrm[1, n]/area[n]
          nrm1[2, n] = nrm[2, n]/area[n]
        end
        #
        # compute element size `he`
        #
        elem_vol = 0.0
        for n = 1:mesh.numNodesPerElement
          elem_vol += sbp.w[n]/mesh.jac[n, elem]
        end

        face_area = 0.0
        for n = 1:mesh.numNodesPerFace
          face_area += sbpface.wface[n]*area[n]
        end
        #
        # On boundary, the area is weighted twice
        #
        area_weight = 0.5*area_sum[elem]/face_area

        if penalty_method == "Shahbazi" || penalty_method == "SAT0"
          he = elem_vol/eqn.area_sum[elem]
        end

        lambda = sview(eqn.lambda, :,:,:,:,elem)

        if penalty_method == "SAT0"
          eigMax[:] = -1.0

          for dof = 1 : mesh.numDofPerNode
            # left element
            for n = 1 : mesh.numNodesPerElement
              b = real(lambda[1,1,dof,n]) + real(lambda[2,2,dof,n])
              ac = real(lambda[1,1,dof,n]) * real(lambda[2,2,dof,n]) - real(lambda[1,2,dof,n]) * real(lambda[2,1,dof,n])
              root = 0.5*(b + sqrt(b*b - 4*ac))*mesh.jac[n, elem]
              eigMax[dof] = max(eigMax[dof], root)
            end

            for n = 1:mesh.numNodesPerFace
              # 0.25 = 0.5*0.5.
              # One 1/2 comes from double weight for boundary face;
              # the other comes from 1/sum(sbpface.wface[i])
              penalty[dof, n] = eigMax[dof]*sigma*area_weight*area[n]*area[n]
            end
          end
        elseif penalty_method == "Shahbazi"
          lambda_face = sview(eqn.lambda_bndry, :,:, :, :, f)

          for n = 1:mesh.numNodesPerFace
            for dof = 1:mesh.numDofPerNode
              penalty[dof, n] =  nrm1[1, n]*nrm1[1, n]*lambda_face[1, 1, dof, n]
              penalty[dof, n] += nrm1[1, n]*nrm1[2, n]*lambda_face[1, 2, dof, n]
              penalty[dof, n] += nrm1[2, n]*nrm1[1, n]*lambda_face[2, 1, dof, n]
              penalty[dof, n] += nrm1[2, n]*nrm1[2, n]*lambda_face[2, 2, dof, n]
              penalty[dof, n] *= penalty_factor_shahbazi*area[n]/he
            end
          end
        elseif penalty_method == "SAT"

          lambda = sview(eqn.lambda, :,:,:,:,elem)
          Sat[:,:,:] = 0.0

          # RLR_L and RLR_R are reused for each (d1, d2)
          for d1 = 1:Tdim
            for d2 = 1:Tdim
              # Compute R Λ{d1,d2}/H R^T
              RLR[:,:,:] = 0.0
              for n1 = 1:mesh.numNodesPerFace
                for n2 = 1:mesh.numNodesPerFace
                  for k = 1:stencilSize
                    RLR[:, n2, n1] += lambda[d1, d2, :, perm[k]] / eqn.w[perm[k], elem] * R[k, n2] * R[k, n1]
                  end
                end
              end
              #
              # N_{d1}i} RLR N_{d2}
              #
              for n1 = 1:mesh.numNodesPerFace
                for n2 = 1:mesh.numNodesPerFace
                  Sat[:, n2, n1] += nrm1[d1, n2]*RLR[:, n2, n1]*nrm1[d2, n12]
                end
              end
            end
          end

          #
          # Left multiply by numFacesPerElem*B
          #
          for row = 1:mesh.numNodesPerFace
            for col = 1:mesh.numNodesPerFace
              Sat[:, row, col] *= area_weight*sbpface.wface[col]*area[col]
            end
          end

        else
          for n = 1:mesh.numNodesPerFace
            for dof = 1:mesh.numDofPerNode
              penalty[dof, n] = Cip*p*p/he
            end
          end
        end

        q = sview(eqn.q_bndry, :, :, f)
        for n = 1:mesh.numNodesPerFace
          xy = sview(mesh.coords_bndry, :, n, f)
          # bc_func(xy, sview(gD, :, n))
          bc_func(xy, gg)
          gD[:, n] = gg[:]
          for dof = 1:mesh.numDofPerNode
            dq[dof, n] = q[dof, n] - gD[dof, n]
          end
        end

        if penalty_method == "Shahbazi" || penalty_method == "SAT0"
          for n = 1:mesh.numNodesPerFace
            for dof = 1 : mesh.numDofPerNode
              flux[dof, n] += penalty[dof, n]*dq[dof, n] * relax_coef
            end
          end
        elseif penalty_method == "SAT"
          for n = 1:mesh.numNodesPerFace
            for n1 = 1:mesh.numNodesPerFace
              for dof = 1:mesh.numDofPerNode
                flux[dof, n] += area[n]*Sat[dof, n, n1]*dq[dof, n1] * relax_coef
              end
            end
          end
        else
          error("We should never get here")
        end

        #
        # term-7
        #
        for n = 1:mesh.numNodesPerFace
          xflux[:, n] += gD[:,n]*nrm[1, n]
          yflux[:, n] += gD[:,n]*nrm[2, n]
        end
        #
      end
      # physical Neumann
      #
    elseif isNeumann(bc_func)   # Neumann BC, term-8
      for f = indx0:indx1
        flux  = sview(eqn.flux_bndry, :,:,f)
        bndry = mesh.bndryfaces[f]
        #
        # Compute geometric info on face
        #
        nrm = ro_sview(mesh.nrm_bndry, :, :, f)
        for n = 1 : mesh.numNodesPerFace
          # area[n] = norm(view(nrm, :, n))
          area[n] = norm(ro_sview(nrm, :, n))
          nrm1[1, n] = nrm[1, n]/area[n]
          nrm1[2, n] = nrm[2, n]/area[n]
        end

        for j = 1:mesh.numNodesPerFace
          xy = sview(mesh.coords_bndry, :, j, f)
          bc_func(xy, gN)
          # term-8
          flux[:, j] -= gN*area[n]
        end
      end
    else
      error("We should never get here!")
    end
  end

  return nothing
end


text:
module DAWG
using LegacyStrings
export DictionaryMatch, SparseMatrixDAWG, DoubleArrayDAWG,
        makeSparseMatrixDAWG, makeDoubleArrayDAWG

#declare sparse matrix based DAWG type
type SparseMatrixDAWG
  m::SparseMatrixCSC
  rownum::Int64
  colnum::Int64
  edgedict::Dict{Char,Int32}
end

#declare double array based DAWG type
type DoubleArrayDAWG
  b::Array{Int32,1}
  check::Array{Int32,1}
  rownum::Int64
  colnum::Int64
  edgedict::Dict{Char,Int32}
end

#DAWG object definition
type DawgNode
    id::Int32
    final::Bool
    edges::Dict
    count::Int32

    function DawgNode(id=0)
        self = new()

        self.id = id
        self.final = false
        self.edges = Dict{Char,DawgNode}()
        self.count = -1
        return self
    end

end

function str(self::DawgNode)
    arr = IOBuffer()
    if self.final
        write(arr,'1')
    else
        write(arr,'0')
    end
    for (label, node) in sort( collect(zip(keys(self.edges), values(self.edges))) )
        write(arr,label,'_',node.id,'_')
    end
    return takebuf_string(arr)
end

function numReachable(self::DawgNode)
    if self.count > 0
        return self.count
    end
    count = 0
    if self.final
        count += 1
    end
    for (label, node) in sort( collect(zip(keys(self.edges), values(self.edges))) )
        count += numReachable(node)
    end
    self.count = count
    return count
end

#declare DAWG type
type Dawg
    NextId::Int32
    previousWord::AbstractString
    root::DawgNode
    uncheckedNodes::Array
    data::Array
    minimizedNodes::Dict
    insert::Function
    _minimize::Function
    finish::Function
    lookup::Function
    nodeCount::Function
    edgeCount::Function

    function Dawg()
        self = new()
        self.NextId = 0
        self.previousWord = ""
        self.root = DawgNode(0)
        self.NextId += 1
        self.uncheckedNodes = []
        self.data = []
        self.minimizedNodes = Dict()

        self.insert = function(word,data)
            word
            word = utf32(word)
            if(word <= self.previousWord)
                println("Error: Words must be inserted in alphabetical order: ",word)
                return
            end
            commonPrefix = 0
            for i = 1:min(length(word),length(self.previousWord))
                word[i] == self.previousWord[i] ? commonPrefix += 1 : break
            end
            self._minimize(commonPrefix)
            node = length(self.uncheckedNodes) == 0 ? self.root : self.uncheckedNodes[end][3]
            for letter in word[(commonPrefix+1):end]
                nextNode = DawgNode(self.NextId)
                self.NextId += 1
                node.edges[letter] = nextNode
                push!(self.uncheckedNodes, (node,letter,nextNode) )
                node=nextNode
            end

            node.final = true
            self.previousWord = word
        end

        self._minimize = function(downTo)
            if(length(self.uncheckedNodes)<1)
                return
            end
            for i = length(self.uncheckedNodes):-1:(downTo+1)
                (parent, letter, child) = self.uncheckedNodes[i]
                if haskey(self.minimizedNodes,str(child))
                    parent.edges[letter] = self.minimizedNodes[str(child)]
                else
                    self.minimizedNodes[str(child)] = child
                end
                pop!(self.uncheckedNodes)
            end
        end

        self.finish = function()
            self._minimize( 0 )
            numReachable(self.root)
            self.minimizedNodes[str(self.root)] = self.root
        end

        self.lookup = function(word)
            word = utf32(word)
            node = self.root
            skipped = 0
            for letter in word
                if !haskey(node.edges,letter)
                    return ""
                end
                for (label, child) in sort( collect(zip(keys(node.edges), values(node.edges))) )
                    if label == letter
                        if node.final
                            skipped += 1
                        end
                        node=child
                        break
                    end
                    skipped += child.count
                end
            end

            if(node.final)
                println("<--- This is word. ($skipped)")
            end
        end

        self.nodeCount = function()
            return length(self.minimizedNodes)
        end

        self.edgeCount = function()
            count=0
            for (key,node) in zip(keys(self.minimizedNodes),values(self.minimizedNodes))
                count += length(node.edges)
            end
            return count
        end

        return self
    end
end

#recursively collect states with paths in DAWG
#used when constructing sparse matrix DAWG
#returns nothing but fills statedict, edgedict, donepathdict, and statecollect
function travrec(node,statedict,edgedict,donepathdict,statecollect)
    for path in keys(node.edges)
        if !haskey(statedict,node.id)
            statedict[node.id] = length(statedict)+1
            statecollect[node.id] = node
        end
        if !haskey(edgedict,path)
            edgedict[path] = length(edgedict)+1
        end

        if get(donepathdict,"$(node.id)$path",0) == node.edges[path].id
            return
        end
        donepathdict["$(node.id)$path"] = node.edges[path].id

        if(node.edges[path].final)
            if length(node.edges[path].edges) > 0
                travrec(node.edges[path],statedict,edgedict,donepathdict,statecollect)
            end
        else
            travrec(node.edges[path],statedict,edgedict,donepathdict,statecollect)
        end
    end
end

#recursively collect states with paths in DAWG
#used when constructing double array DAWG
#returns nothing but fills statedict, edgedict, donepathdict, and statecollect
function travrecBC(node,statedict,edgedict,donepathdict,statecollect,termnode)
    if !haskey(statedict,node.id)
        statedict[node.id] = length(statedict)+1
        statecollect[node.id] = node
    end

    if node.final && length(node.edges) == 0
        termnode[node.id] = node.id
        return
    end

    for path in keys(node.edges)
        if !haskey(edgedict,path)
            edgedict[path] = length(edgedict)+1
        end

        if get(donepathdict,(node.id,path),0) == node.edges[path].id
           return
        end
        donepathdict[(node.id,path)] = node.edges[path].id

        travrecBC(node.edges[path],statedict,edgedict,donepathdict,statecollect,termnode)
    end
    return termnode
end

#directly fill sparse matrix with DAWG states and transition information
function fillmatrixStraightIJV(rownum,colnum,statedict,edgedict,statecollect)
    const block = colnum
    I = zeros(Int32,block)
    J = zeros(Int32,block)
    V = zeros(Int32,block)

    cnt = 0
    element = 0
    statesize = length(statecollect)
    for k in sort(collect(keys(statecollect)))
        node = statecollect[k]
        cnt += 1
        if cnt%100000 == 0
            println("Done: $cnt / $statesize")
        end

        if length(I) < (element+block)
            I = vcat(I,zeros(Int32,block))
            J = vcat(J,zeros(Int32,block))
            V = vcat(V,zeros(Int32,block))
        end

        for path in keys(node.edges)
            element += 1
            I[element] = statedict[node.id]
            J[element] = edgedict[path]

            if(node.edges[path].final)
                if length(node.edges[path].edges) > 0
                    V[element] = -statedict[node.edges[path].id]
                else
                    V[element] = -rownum
                end
                continue
            end
            V[element] = statedict[node.edges[path].id]
        end
    end
    len = findlast(I)
    return sparse(I[1:len],J[1:len],V[1:len],rownum,colnum)
end

#perform dictionary match using sparse matrix DAWG
#input: sentence as a valid string, dawgdict as a sparse matrix based DAWG
#set extract = true (default) will display matched word as progressing
function DictionaryMatch(sentence,dawgdict::SparseMatrixDAWG; extract::Bool=true)
  rettuple = tokenizetuple(sentence,dawgdict.m,dawgdict.edgedict,dawgdict.rownum,dawgdict.colnum)
  if extract
    usentence = utf32(sentence)
    for (from,to) in rettuple
      println(usentence[from:to])
    end
  end
  rettuple
end

#perform dictionary matching scan with spare matrix based DAWG(inner use only)
function tokenizetuple(sentence,m::SparseMatrixCSC,edgedict,rownum,def)
    curstate = 1
    cutat = []
    curpos = 1
    lastbegin = 1
    lastgood = 1
    sentence = utf32(sentence)
    senlen = length(sentence)

    while curpos <= senlen
        @inbounds col = get(edgedict,sentence[curpos],def)
        @inbounds curstate = m[curstate,col]

        if (curstate > 0)
            curpos += 1
        elseif (curstate == 0)
             if (lastgood>lastbegin)
                push!(cutat,(lastbegin,lastgood))
            end
            curstate = 1
            lastbegin = curpos = lastgood+=1
        elseif (curstate<0)
            if (curstate==-rownum)
                push!(cutat,(lastbegin,curpos))
                curstate = 1
                lastbegin = lastgood = curpos+=1
            else
                curstate = -curstate
                lastgood = curpos
                curpos += 1
            end
        end
    end

    ##flush
    if (lastgood>lastbegin)
        push!(cutat,(lastbegin,lastgood))
    end

    return cutat
end

#perform dictionary match using double array DAWG
#input: sentence as a valid string, dawgdict as a sparse matrix based DAWG
#set extract = true (default) will display matched word as progressing
function DictionaryMatch(sentence,dawgdict::DoubleArrayDAWG; extract::Bool=true)
  rettuple = tokenizedoublearrayBC(sentence,dawgdict.edgedict,dawgdict.b,dawgdict.check)
  if extract
    usentence = utf32(sentence)
    for (from,to) in rettuple
      println(usentence[from:to])
    end
  end
  rettuple
end

#perform dictionary matching scan with double array based DAWG(inner use only)
function tokenizedoublearrayBC(sentence,edgedict,b,check)
    curstate = 2
    cutat = []
    curpos = 1
    lastbegin = 1
    lastgood = 1
    sentence = utf32(sentence)
    senlen = length(sentence)

    while curpos <= senlen
        @inbounds col = get(edgedict,sentence[curpos],0)
        if col == 0
            curstate = -1
        else
            if curstate > 1
                @inbounds p = b[curstate] + col
                @inbounds curstate = check[p] == b[curstate] ? p : -1
            end
        end

        if(curstate < 1)
            if(lastgood>lastbegin)
                push!(cutat,(lastbegin,lastgood))
            end
            curstate = 2
            lastbegin = curpos = lastgood+=1
        elseif(b[b[curstate]]==-1)
            push!(cutat,(lastbegin,curpos))
            curstate = 2
            lastbegin = lastgood = curpos+=1
        elseif(b[b[curstate]]==0)
            lastgood = curpos
            curpos += 1
        else
            curpos += 1
        end
    end

    ##flush
    if(lastgood>lastbegin)
        push!(cutat,(lastbegin,lastgood))
    end

    return cutat
end

#constrct a sparse matrix based DAWG from word list(dictionary)
#return SparseMatrixDAWG type
function makeSparseMatrixDAWG(list)
   println("Making DAWG... Please wait...")
   d = Dawg()
    for w in sort(list)
    d.insert(w,"hello")
   end
   d.finish()
   statedict = Dict()
   statenum = d.nodeCount()

   edgedict = Dict{Char,Int32}()
   edgenum = d.edgeCount()

   donepathdict = Dict()
   statecollect = Dict{Int32,DawgNode}()

   travrec(d.root,statedict,edgedict,donepathdict,statecollect)
   colnum = 1+length(edgedict)
   rownum = 1+length(statedict)

   m = fillmatrixStraightIJV(rownum,colnum,statedict,edgedict,statecollect)
   println("Done!")
   return  SparseMatrixDAWG(m,rownum,colnum,edgedict)
end

#find vacant space in double array (inner use only)
function x_checkBC(check,c_set)
    minc = minimum(c_set)
    firstvac = findnext(check,0,minc+2)
    bf = firstvac-minc
    runningbf = bf
    if maximum(bf .+ c_set) >= length(check)
        return 0
    end

    while sum(abs(check[ bf .+ c_set])) > 0  || bf<1
      firstvac = findnext(check,0,runningbf)
      if firstvac == 0
          return 0
      end
      #runningbf += 1
      runningbf = firstvac+1
      bf = firstvac-minc
      if maximum(bf .+ c_set) >= length(check)
          return 0
      end
    end
    check[ bf .+ c_set ] = bf
    return bf
end

#constrct double array based on data collected from tree based DAWG
function getdoublearrayBC(statedict,edgedict,statecollect,termnode)

    invn = Dict()
    fv = Dict()
    maxchmap = 5*(1+length(edgedict))
    check = zeros(Int32,maxchmap)
    b = zeros(Int32,maxchmap)

    check[1] = 1
    b[1] = -1
    check[2] = 0
    b[2] = 2

    for k in keys(termnode)
      fv[abs(statedict[k])] = 1
    end

    #filling check
    cnt = 0
    statesize = length(statecollect)
    for k in sort(collect(keys(statecollect))) #values(statecollect)

        node = statecollect[k]
        if haskey(termnode,node.id)
            continue
        end
        cnt += 1
        if cnt%10000 == 0
            println("1st Done: $cnt / $statesize")
        end

        c_set = [0]
        if(length(node.edges)>0)
            c_set = [c_set; map(x->edgedict[x],collect(keys(node.edges)))]
        end

        firstvac = 0
        while firstvac <= 0
            firstvac = x_checkBC(check,c_set)
            if firstvac <= 0
                b = vcat(b,zeros(Int32,maxchmap))
                check = vcat(check,zeros(Int32,maxchmap))
            end
        end

        fv[abs(statedict[node.id])] = firstvac
        if !node.final
          b[firstvac] = 2
        end
    end

    #filling base

    cnt = 0
    for node in values(statecollect)
        cnt += 1
        if cnt%10000 == 0
            println("2nd Done: $cnt / $statesize")
        end

        bf = fv[abs(statedict[node.id])]

        for path in keys(node.edges)
            if haskey(fv,abs(statedict[node.edges[path].id]))
              b[bf+edgedict[path]] = fv[abs(statedict[node.edges[path].id])]
            end
        end
    end

    maxb = maximum(b)+length(edgedict)
    sizediff = maxb-length(b)
    if maxb > length(b)
        b = [b;zeros(Int32,sizediff)]
        check = [check;zeros(Int32,sizediff)]
    end
    return (b,check,fv)
end

#constrct a sparse matrix based DAWG from word list(dictionary)
#return DoubleArrayDAWG type
function makeDoubleArrayDAWG(list)
   println("Making DAWG... Please wait...")
   d = Dawg()
   for w in sort(list)
     d.insert(w,"hello")
   end
   d.finish()
   statedict = Dict()
   statenum = d.nodeCount()

   edgedict = Dict{Char,Int32}()
   edgenum = d.edgeCount()

   donepathdict = Dict()
   statecollect = Dict{Int32,DawgNode}()

   termnode = Dict()
   travrecBC(d.root,statedict,edgedict,donepathdict,statecollect,termnode)
   colnum = 1+length(edgedict)
   rownum = 1+length(statedict)

   (b,check,fv) = getdoublearrayBC(statedict,edgedict,statecollect,termnode);
   println("Done!")
   return DoubleArrayDAWG(b,check,rownum,colnum,edgedict)
end

end # module

max_stars_repo_name: hottolink/DAWG

max_issues_repo_name: hottolink/DAWG

max_forks_repo_name: hottolink/DAWG


text:
using Test, YaoArrayRegister, YaoBase

@testset "select" begin
    reg = product_state(4, 6; nbatch = 2)
    # println(focus!(reg, [1,3]))
    r1 = select!(focus!(copy(reg), [2, 3]), 0b11) |> relax!(to_nactive = 2)
    r2 = select(focus!(copy(reg), [2, 3]), 0b11) |> relax!(to_nactive = 2)
    r3 = copy(reg) |> focus!(2, 3) |> select!(0b11) |> relax!(to_nactive = 2)

    @test r1' * r1 ≈ ones(2)
    @test r1 ≈ r2
    @test r3 ≈ r2
end

@testset "measure and collapseto/remove" begin
    reg = rand_state(4)
    res = measure_collapseto!(reg, (4,))
    @test isnormalized(reg)
    result = measure(reg; nshots = 10)
    @test all(result .< 8)

    reg = rand_state(6) |> focus!(1, 4, 3)
    reg0 = copy(reg)
    res = measure_remove!(reg)
    select(reg0, res)
    @test select(reg0, res) |> normalize! ≈ reg

    r = rand_state(10)
    r1 = copy(r) |> focus!(1, 4, 3)
    res = measure_remove!(r, (1, 4, 3))
    r2 = select(r1, res)
    r2 = relax!(r2, (); to_nactive = nqubits(r2))
    @test normalize!(r2) ≈ r

    reg = rand_state(6, nbatch = 5) |> focus!((1:5)...)
    measure_collapseto!(reg, 1)
    @test nactive(reg) == 5
end


text:
#=
Compose objects into tree structures to represent part-whole hierarchies.
Composite lets clients treat individual objects and compositions of objects uniformly.
=#

# portfolio management example - fund of funds

module CompositeExample

# export Portfolio, Holding, sample_portfolio, market_value

struct Holding
    symbol::String
    qty::Int
    price::Float64
end

Base.show(io::IO, h::Holding) =
    print(io, h.symbol, " ", h.qty, " shares @ \$", h.price)

struct Portfolio
    symbol::String
    name::String
    stocks::Vector{Holding}
    subportfolios::Vector{Portfolio}
end

Base.show(io::IO, p::Portfolio) = myshow(io, p)

function myshow(io::IO, p::Portfolio, level = 0)
    println("  " ^ level, p.name, " (", p.symbol, ")")
    if length(p.stocks) > 0
        println(io, "  ", "  " ^ level, "Holdings:")
        foreach(h -> println(io, "  " ^ level, "    ", h), p.stocks)
    end
    if length(p.subportfolios) > 0
        foreach(subpf -> myshow(io, subpf, level + 1), p.subportfolios)
    end
end

Portfolio(symbol::String, name::String, Holdings::Vector{Holding}) =
    Portfolio(symbol, name, Holdings, Portfolio[])

Portfolio(symbol::String, name::String, subportfolios::Vector{Portfolio}) =
    Portfolio(symbol, name, Holding[], subportfolios)

function sample_portfolio()
    large_cap = Portfolio("TOMKA", "Large Cap Portfolio", [
        Holding("AAPL", 100, 275.15),
        Holding("IBM", 200, 134.21),
        Holding("GOOG", 300, 1348.83)])

    small_cap = Portfolio("TOMKB", "Small Cap Portfolio", [
        Holding("ATO", 100, 107.05),
        Holding("BURL", 200, 225.09),
        Holding("ZBRA", 300, 257.80)])

    p1 = Portfolio("TOMKF", "Fund of Funds Sleeve", [large_cap, small_cap])
    p2 = Portfolio("TOMKG", "Special Fund Sleeve", [Holding("C", 200, 76.39)])
    return Portfolio("TOMZ", "Master Fund", [p1, p2])
end

# Contractual functions

# Every investment should have a market value regarless of whether
# it is a portfolio or stock.

market_value(s::Holding) = s.qty * s.price

market_value(p::Portfolio) =
    mapreduce(market_value, +, p.stocks, init = 0.0) +
    mapreduce(market_value, +, p.subportfolios, init = 0.0)
    # length(p.stocks) > 0 ? sum(market_value.(p.stocks)) : 0.0 +
    # length(p.subportfolios) > 0 ? sum(market_value.(p.subportfolios)) : 0.0

# Every investment should have a trade symbols regarless of whether
# it is a portfolio or stock.

trade_symbol(s::Holding) = s.symbol
trade_symbol(p::Portfolio) = p.symbol

# ---- Testing ----
function test()
    portfolio = CompositeExample.sample_portfolio()
    mv = market_value(portfolio)
    println(portfolio)
    println("market value = ", mv)
end

end # module

using .CompositeExample
CompositeExample.test()

#= REPL
julia> using .CompositeExample

julia> CompositeExample.test()
Master Fund (TOMZ)
  Fund of Funds Sleeve (TOMKF)
    Large Cap Portfolio (TOMKA)
      Holdings:
        AAPL 100 shares @ $275.15
        IBM 200 shares @ $134.21
        GOOG 300 shares @ $1348.83
    Small Cap Portfolio (TOMKB)
      Holdings:
        ATO 100 shares @ $107.05
        BURL 200 shares @ $225.09
        ZBRA 300 shares @ $257.8
  Special Fund Sleeve (TOMKG)
    Holdings:
      C 200 shares @ $76.39

market value = 607347.0
=#


text:
using Symbolics

# this symbolic structure corresponds to a symbolic transfer operator
# (L_n f)(x) = \sum_{y\in T^{-1}(x)}\frac{f(y)}{|T'(y)|^n}
struct SymbL
    n
    f
end

@variables x D(x)

∂ = Differential(x)

Diff(P::SymbL) = [SymbL(P.n+1, ∂(P.f)), SymbL(P.n, P.n*P.f*D)]

function Diff(v::Vector{SymbL})
    w = SymbL[]
    for P in v
        append!(w, Diff(P))
    end
    return w
end

function compute_dfly_k_fi_DDi(k::Int)
    @variables x
    @variables f[0:k](x)
    @variables DD[0:k](x)
    ∂ = Differential(x)

    der_dict = Dict([[D => DD[1] ]; [∂(f[i]) => f[i+1] for i in 1:k]; [∂(DD[i]) => DD[i+1] for i in 1:k]])

    P = SymbL(1, f[1]) #Lf
    v = P
    for i in 1:k
        v = Diff(v)
        for k in 1:length(v)
            l = expand_derivatives(v[k].f, true)
            l = substitute(l, der_dict)
            l = simplify(l; expand = true)
            v[k] = SymbL(v[k].n, l)
        end
    end

    return v
end

using SymbolicUtils

function _optimize_mult(k, n, h::SymbolicUtils.Mul, vals)

    @variables x

    @variables f[0:k](x)
    boolf = [(symb in keys(h.dict)) for symb in f]

    @variables DD[0:k](x)
    boolDD = [(symb in keys(h.dict)) for symb in DD]

    # I start with a simple version, where I use the n on
    # the highest derivative of DD, in further versions
    # I will try to optimize over all possibilities

    i = findlast(boolDD)
    pow = h.dict[DD[i]]

    # This uses the computed bounds to compute
    # ||DDᵢ/(T')^n-1||_{∞}*||DD_i||_{∞}^{pow-1}

    bound = vals[i+1, n]*vals[i+1, 1]^(pow-1)

    for j in 1:i-1
        if boolDD[j]!= false
            pow = h.dict[DD[j]]
            bound *= vals[j+1, 1]*pow
        end
    end

    i = findlast(boolf)
    return h.coeff*bound*f[i]

end

function optimize_coefficients(k, v::Vector{SymbL}, vals)
    out = Num(0)
    for x in v
        n = x.n
        #@info "x" x
        if typeof(x.f.val) <: SymbolicUtils.Mul
            y = x.f.val
            val = _optimize_mult(k, n, y, vals)
            out+= _optimize_mult(k, n, x.f.val, vals)
        elseif typeof(x.f.val) <: SymbolicUtils.Add
            for y in keys(x.f.val.dict)
                val = _optimize_mult(k, n, y, vals)
                out+= x.f.val.dict[y]*val
            end
        end
    end
    return out
end

function substitute_values(k, v::Vector{SymbL}, vals)
    λ = vals[1]
    DDs = vals[2:end]
    @variables x
    @variables f[0:k](x)
    @variables DD[0:k](x)

    subsdict = Dict([DD[i]=>DDs[i] for i in 1:k])

    w = zeros(Num, length(v))

    for k in 1:length(v)
        n = v[k].n
        f = v[k].f
        #w[k] = substitute(f, subsdict)
        #w[k] = λ^(n-1)*w[k]
    end
    return w
end


text:
export psos

"""
    psos(bd::Billiard, plane::InfiniteWall, t, particles)
Compute the Poincaré section of the `particles` with the given `plane`, by evolving
each one for time `t` (either integer or float) inside `bd`.

The `plane` can be an [`InfiniteWall`](@ref) of *any* orientation, however only
crossings of the `plane` such that `dot(velocity, normal) < 0` are allowed, with
`normal` the normal unit vector of the `plane`.

`particles` can be:
* A single particle.
* A `Vector` of particles.
* An integer `n` optionally followed by an angular velocity `ω`.

Return the positions `poss` and velocities `vels` at the instances of crossing
the `plane`. If given more than one particle, the result is a vector of vectors
of vectors.

*Notice* - This function can handle pinned particles.
If a pinned particle can intersect with the `plane`, then an intersection is returned.
If however it can't then empty vectors are returned.
"""
function psos(
    bd::Billiard{T}, plane::InfiniteWall{T}, t, par::AbstractParticle{T}) where {T}

    p = copy(par)
    rpos = SV{T}[]
    rvel = SV{T}[]
    count = zero(t)

    periodic = isperiodic(bd)
    check_for_pinned = typeof(p) <: MagneticParticle
    check_for_pinned && (cyclotron_time = 2π/abs(p.omega))
    t_to_write = zero(T)

    while count < t
        # compute collision times
        i::Int, tmin::T, cp = next_collision(p, bd)

        tplane, = collision(p, plane)

        # if tplane is smaller, I intersect the section
        if tplane ≥ 0.0 && tplane < tmin && tplane != Inf
            psos_pos, psos_vel = propagate_posvel(p.pos, p, tplane)
            if dot(psos_vel, plane.normal) < 0 # only write crossings with 1 direction
                push!(rpos, psos_pos); push!(rvel, psos_vel)
            end
        end

        if check_for_pinned
            if isperiodic(i, bd)
                t_to_write += tmin
            else
                t_to_write = zero(T)
            end
        end

        tmin == Inf && break

        # Now "bounce" the particle normally:
        relocate!(p, bd[i], tmin, cp)
        resolvecollision!(p, bd[i])
        typeof(par) <: MagneticParticle && (p.center = find_cyclotron(p))

        if check_for_pinned && t_to_write ≥ cyclotron_time
            tplane, = collision(p, plane)
            if tplane == Inf
                if length(rpos) > 0
                    return [rpos[1]], [rvel[1]]
                else
                    return rpos, rvel
                end
            else
                psos_pos, psos_vel = propagate_posvel(p.pos, p, tplane)
                return [psos_pos], [psos_vel]
            end
        end

        count += increment_counter(t, tmin)
    end
    return rpos, rvel
end

function psos(bd::Billiard{T}, plane::InfiniteWall{T}, t,
    pars::Vector{<:AbstractParticle{T}}) where {T}

    retpos = Vector{SV{T}}[]
    retvel = Vector{SV{T}}[]

    for p ∈ pars
        rpos, rvel = psos(bd, plane, t, p)
        push!(retpos, rpos); push!(retvel, rvel)
    end
    return retpos, retvel
end

psos(bd, plane, t, n::Integer) = psos(bd, plane, t, [randominside(bd) for i=1:n])
psos(bd, plane, t, n::Integer, ω::Real) =
psos(bd, plane, t, [randominside(bd, ω) for i=1:n])

propagate_posvel(pos, p::Particle{T}, t) where {T} =
    (SV{T}(pos[1] + p.vel[1]*t, pos[2] + p.vel[2]*t), p.vel)

function propagate_posvel(pos, p::MagneticParticle{T}, t) where {T}
    ω = p.omega
    φ0 = atan(p.vel[2], p.vel[1])
    s0, c0 = sincos(φ0)
    sωφ0, cωφ0 = sincos(ω*t + φ0)
    ppos = SV{T}(sωφ0/ω - s0/ω, -cωφ0/ω + c0/ω)
    psos_vel = SV{T}(cωφ0, sωφ0)
    return pos + ppos, psos_vel
end


text:
# Code taken from
# https://thirld.com/blog/2015/05/30/julia-profiling-cheat-sheet/

using Profile
using ProfileView
using Serialization

function benchmark()
    # Any setup code goes here.
        function code_to_profile()
            let grad=grad, data=data, theta=theta, sim=sim, dtv=dtv
                simloglik_drill_data!(grad, data, theta, sim, dtv, true)
            end
        end

    # Run once, to force compilation.
    println("======================= First run:")
    @time code_to_profile()

    # Run a second time, with profiling.
    println("\n\n======================= Second run:")
    Profile.init(delay=0.01)
    Profile.clear()
    Profile.clear_malloc_data()
    @profile @time code_to_profile()

    # Write profile results to profile.bin.
    r = Profile.retrieve()
    f = open("profile.bin", "w")
    serialize(f, r)
    close(f)
end

benchmark()
f = open("profile.bin")
r = deserialize(f);
ProfileView.view(r[1], lidict=r[2])


text:
"""
    crossover_classic(population::AbstractMatrix,mutated::AbstractMatrix,Cr::Real)

Perform the crossover operation between a **`population`** matrix and **`mutated`** matrix.
The crossover probability, **`Cr`** ∈ [0,1], is a user-defined value that controls
the fraction of parameter values that are copied from the mutant.

To determine which source contributes a given parameter, uniform crossover
compares **`Cr`** to the output of a uniform random number generator, **`rand()`**.
If the random number is less than or equal to **`Cr`**, the trial parameter
is inherited from the **`mutated`** matrix; otherwise, the parameter is copied
from the **`population`** matrix. In addition, the trial parameter with randomly
chosen index, **`j`**, is taken from the mutant to ensure that the trial vector
does not duplicate.

(*Price, K., Storn, R. M., & Lampinen, J. A. (2006).
Differential evolution: a practical approach to global optimization.
Springer Science & Business Media.*)

# Parameters

## **`population`**:

Population matrix, each row is an individue; each individue has some parameters.

## **`mutated`**:

The mutated population matrix.

## **`Cr`**:

Crossover probability.

# Examples

```julia-repl
julia> A = rand(5,3)
5×3 Matrix{Float64}:
 0.896444   0.493249   0.51672
 0.705792   0.0381215  0.48208
 0.298154   0.491878   0.658559
 0.635572   0.342714   0.443062
 0.0671616  0.553378   0.903124

julia> B = rand(5,3)
5×3 Matrix{Float64}:
 0.946939  0.684977   0.848902
 0.390751  0.0296715  0.789489
 0.772502  0.351496   0.889189
 0.187194  0.451319   0.943257
 0.264035  0.635716   0.477502

julia> crossover_classic(A,B,0.5)
5×3 Matrix{Float64}:
 0.896444   0.684977   0.51672
 0.390751   0.0296715  0.789489
 0.298154   0.351496   0.889189
 0.187194   0.451319   0.943257
 0.0671616  0.635716   0.477502
```
"""
function crossover_classic(population::AbstractMatrix,mutated::AbstractMatrix,Cr::Real)
#Error check
    if size(population) != size(mutated)
        throw(ErrorException("Size of population matrix $(size(population)) must be the same as the mutated matrix $(size(mutated))"))
    end

    if (Cr < 0) | (Cr > 1)
        throw(ErrorException("Cr must be in the interval [0,1]"))
    end

    crossed = similar(population)
    population_size = size(population,1)
    number_of_parameters = size(population,2)

    for individue in 1:population_size
        for parameter in 1:number_of_parameters
            if (rand() <= Cr) | (rand(1:number_of_parameters) == parameter)
                @inbounds crossed[individue,parameter] = mutated[individue,parameter]
            else
                @inbounds crossed[individue,parameter] = population[individue,parameter]
            end
        end
    end

    return crossed
end

function crossover_JADE(individue::AbstractVector,mutated_individue::AbstractVector,Cr::Real)
    number_of_parameters = length(individue)
    cross = copy(individue)

    @inbounds for parameter in 1:number_of_parameters
        if (rand() <= Cr) | (rand(1:number_of_parameters) == parameter)
            cross[parameter] = mutated_individue[parameter]
        end
        return cross
    end
end


text:
module FixPA
export rev_add

using SmoothedParticles

"""
    FixPA_eps

    Epsilon in the fixed-point arithmetic
"""
const FixPA_eps = 1/(2^30)

"""
    nom(x::Float64)::Int64

Value of ``x`` in the fixed-point arithmetic.
"""
function nom(x::Float64)::Int64
    return Int64(round(x/FixPA_eps))
end

"""
    rev_add(x::Float64, y::Float64)::Float64

Reversible addition of two floats.
"""
function rev_add(x::Float64, y::Float64)::Float64
    return FixPA_eps*(nom(x) + nom(y))
end

"""
    rev_add(x::RealVector, y::RealVector)::RealVector

Reversible addition of two vectors.
"""
function rev_add(x::RealVector, y::RealVector)::RealVector
    v1 = FixPA_eps*(nom(x[1]) + nom(y[1]))
    v2 = FixPA_eps*(nom(x[2]) + nom(y[2]))
    v3 = FixPA_eps*(nom(x[3]) + nom(y[3]))
    return RealVector(v1,v2,v3)
end

end


text:
# Contour + heatmap

using CairoMakie
let
    f(x,y) = (x + 2y^2) * abs(sin(y) + cos(x))
    x = y = 1:0.2:20
    z = [f(x,y) for x in x, y in y]

    fig = Figure(resolution=(800,400), fontsize = 22)
    axs = [Axis(fig[1, j], aspect = 1, xlabel = "x", ylabel = j ==1 ? "y" : "")
        for j in 1:2]
    p1 = CairoMakie.heatmap!(axs[1], x, y, z, colormap = :plasma)
#     CairoMakie.contour!(axs[2], x, y, z; color = :black, levels = 100:1:101)
    CairoMakie.heatmap!(axs[2], x, y, z; colormap = (:plasma, 0.5))
    CairoMakie.contour!(axs[2], x, y, z; color = :white, levels = 100:1:101)
    Colorbar(fig[1,3], p1, width = 20, ticksize=20, tickalign = 1)
    [limits!(axs[i], 1,20,1,20) for i in 1:2]
    display(fig)
end

max_stars_repo_name: Ellon-M/visualizations

max_issues_repo_name: Ellon-M/visualizations

max_forks_repo_name: Ellon-M/visualizations


text:
# Note: The "EHR Launch" workflow is also known as the "Embedded Launch" workflow.

const _default_scope = "launch"

"""
    provider_ehr_launch(config::ProviderEHRLaunchConfig, uri_string::String; kwargs...)

## Optional Keyword Arguments:
- `scope::AbstractString`. Default value: `$(_default_scope)`.
"""
function provider_ehr_launch(config::ProviderEHRLaunchConfig, uri_string::AbstractString; kwargs...)
    uri = URIs.URI(uri_string)::URIs.URI
    return provider_ehr_launch(config, uri; kwargs...)
end

"""
    provider_ehr_launch(config::ProviderEHRLaunchConfig, uri::URIs.URI; kwargs...)

## Optional Keyword Arguments:
- `scope::AbstractString`. Default value: `$(_default_scope)`.
"""
function provider_ehr_launch(config::ProviderEHRLaunchConfig,
                             uri::URIs.URI;
                             kwargs...)
    queryparams = URIs.queryparams(uri)::Dict{String, String}
    return provider_ehr_launch(config, queryparams; kwargs...)
end

"""
    provider_ehr_launch(config::ProviderEHRLaunchConfig, queryparams::Dict; kwargs...)

## Optional Keyword Arguments:
- `scope::String`. Default value: `$(_default_scope)`.
"""
function provider_ehr_launch(config::ProviderEHRLaunchConfig,
                             queryparams::Dict{String, String};
                             scope::AbstractString = _default_scope)
    iss          = queryparams["iss"]::String
    launch_token = queryparams["launch"]::String
    return provider_ehr_launch(config; iss, launch_token, scope)
end

"""
    provider_ehr_launch(config::ProviderEHRLaunchConfig; iss::String, launch_token::String, kwargs...)

## Required Keyword Arguments:
- `iss::String`
- `launch_token::String`

## Optional Keyword Arguments:
- `scope::String`. Default value: `$(_default_scope)`.
"""
function provider_ehr_launch(config::ProviderEHRLaunchConfig;
                             iss::String,
                             launch_token::String,
                             scope::AbstractString = _default_scope)
    iss_metadata_endpoint = "$(iss)/Metadata"
    metadata_response = HTTP.request(
        "GET",
        iss_metadata_endpoint;
        headers = Dict(
            "Accept" => "application/fhir+json",
            "Epic-Client-ID" => config.client_id,
        ),
    )
    metadata_response_json = JSON3.read(String(metadata_response.body))
    extension = metadata_response_json.rest[1].security.extension[1].extension
    endpoints = _get_endpoints_from_extension(extension)
    authorize_endpoint = endpoints["authorize"]
    token_endpoint = endpoints["token"]
    authorize_uri_with_querystring_params = URIs.URI(
        URIs.URI(authorize_endpoint);
        query = Dict(
            "aud"           => iss,
            "client_id"     => config.client_id,
            "launch"        => launch_token,
            "redirect_uri"  => config.redirect_uri,
            "response_type" => "code",
            "scope"         => scope,
        )
    )
    authorize_response = HTTP.request(
        "GET",
        authorize_uri_with_querystring_params;
        redirect = false,
    )
    headers = Dict(authorize_response.headers)

    let
        error_msg = string(
            "Something when wrong when authenticating to the EHR. ",
            "One possible explanation is that your `redirect_uri` value does not exactly ",
            "match any of the `redirect_uri` values that are on file with the EHR.",
        )
        haskey(headers, "Location") || throw(ErrorException(error_msg))
    end
    location = headers["Location"]
    location_uri = URIs.URI(location)
    location_queryparams = URIs.queryparams(location_uri)

    let
        error_msg = "Encountered an error while trying to authenticate to the EHR."
        haskey(location_queryparams, "error")             && @error "Error: $(location_queryparams["error"])"
        haskey(location_queryparams, "error_description") && @error "Error: $(location_queryparams["error_description"])"
        haskey(location_queryparams, "code")              || throw(ErrorException(error_msg))
    end
    authorization_code = location_queryparams["code"]

    authz_code_info = _AuthorizationCodeInformation(;
        authorization_code = authorization_code,
        client_id          = config.client_id,
        redirect_uri       = config.redirect_uri,
        token_endpoint     = token_endpoint,
    )

    access_token_info = authorization_code_to_access_token(authz_code_info)

    launch_token_is_jwt, launch_token_jwt_decoded = try_decode_jwt(launch_token)

    ehr_launch_result = ProviderEHRLaunchResult(;
        authorization_code             = access_token_info.authorization_code,
        authorization_code_is_jwt      = access_token_info.authorization_code_is_jwt,
        authorization_code_jwt_decoded = access_token_info.authorization_code_jwt_decoded,
        access_token                   = access_token_info.access_token,
        access_token_is_jwt            = access_token_info.access_token_is_jwt,
        access_token_jwt_decoded       = access_token_info.access_token_jwt_decoded,
        access_token_response          = access_token_info.access_token_response,
        launch_token                   = launch_token,
        launch_token_is_jwt            = launch_token_is_jwt,
        launch_token_jwt_decoded       = launch_token_jwt_decoded,
    )::ProviderEHRLaunchResult

    return ehr_launch_result
end

function _get_endpoints_from_extension(extension)
    endpoints = Dict{String, String}()
    for e ∈ extension
        value_uri = e.valueUri
        url = e.url
        endpoints[url] = value_uri
    end
    return endpoints
end


text:
module RegressionAndOtherStories

using Reexport

@reexport using CSV, DelimitedFiles, Unicode
@reexport using DataFrames, CategoricalArrays
@reexport using StatsBase, Statistics

# Direct access to the R repository "ROS-Examples"

"""

ros_data()

Base part of the path to datafiles in the R package ROS-Examples.
Return "" if `env_var` is not present in ENV.

## Positional argument
```julia
* `dataset::Union{AbstractString, Missing}` # Path to data in ROS-Examples
```

### Keyword arguments
```julia
* `env_var = "JULIA_ROS_HOME"` # Environment variable name
```

### Returns
```julia
* `path::AbstractString` # Path or "".
```

# Extended help

Examples:
```julia
ros_path()
ros_path("HDI")
```
"""
function ros_path(dataset::Union{AbstractString, Missing} = missing;
    env_var="JULIA_ROS_HOME")

    if haskey(ENV, env_var)
        ros_src_path = ENV["JULIA_ROS_HOME"]
    else
        @warn "JULIA_ROS_HOME environment variable not defined."
        return ""
    end
    if ismissing(dataset)
        normpath(ros_src_path)
    else
        normpath(joinpath(ros_src_path, dataset))
    end
end

"""

ros_data()

Construct the path to a datafile in the R package ROS-Examples.
Return "" if `env_var` is not present in ENV.

## Positional argument
```julia
* `dataset::AbstractString`
* `parts::Vector{AbstractString}` # Path to data file in
```

### Keyword arguments
```julia
* `env_var = "JULIA_ROS_HOME"` # Environment variable name
```

### Returns
```julia
* `path::AbstractString` # Path or "".
```

# Extended help

Examples:
```julia
ros_data()
ros_data("HDI", "hdi.dat")
```
"""
function ros_data(dataset, parts...; env_var="JULIA_ROS_HOME")
   if haskey(ENV, env_var)
        ros_src_path = ENV["JULIA_ROS_HOME"]
    else
        @warn "JULIA_ROS_HOME environment variable not defined."
        return ""
    end
    normpath(joinpath(ros_path(dataset), "data", parts...))
end

export
    ros_path,
    ros_data

# Access RegressionAndOtherStories.jl data files (.csv) using ros_datadir()
"""

# ros_datadir()

Path to the RegressionAndOtherStories.jl data files.

Construct the path to a dataset in RegressionAndOtherStories.jl.

## Positional argument
```julia
* `parts::Vector{AbstractString}` # Path to data file in
```

### Returns
```julia
* `path::AbstractString` # Path or "".
```

# Extended help

Examples:
```julia
ros_datadir("ElectionsEconomy", "hibbs.dat")
```
or, to read in as a DataFrame:
```julia
hibbs = CSV.read(ros_datadir("ElectionsEconomy", "hibbs.csv"), DataFrame)
```

"""
function ros_datadir(parts...)
    normpath(@__DIR__, "..", "data", parts...)
end

export
    ros_datadir

end # module