Base.setindex!(v::Array{Complex{ArbFloat{P}},1}, c::ArbComplex{P}, idx::Int64) where {P} =
         setindex!(v, Complex{ArbFloat{P}}(ArbFloat{P}(real(c)), ArbFloat{P}(imag(c))), idx)

Base.setindex!(v::Array{Complex{ArbReal{P}},1}, c::ArbComplex{P}, idx::Int64) where {P} =
    setindex!(v, Complex{ArbReal{P}}(real(c), imag(c)), idx)

Base.convert(::ArbFloatMatrix{P}, x::Array{ArbFloat{P}, 2}) where {P} = ArbFloatMatrix(x)
Base.convert(::ArbRealMatrix{P}, x::Array{ArbReal{P}, 2})  where {P} = ArbRealMatrix(x)
Base.convert(::ArbComplexMatrix{P}, x::Array{ArbComplex{P}, 2}) where {P} = ArbComplexMatrix(x)

svdvals(x::Array{ArbComplex{N},2}) where {N} = throw(ErrorException("not supported"))
svdvals(x::Array{ArbComplex{N},M}) where {N,M} = throw(ErrorException("not supported"))
svdvals(x::Array{Complex{ArbFloat{N}},2}) where {N} = throw(ErrorException("not supported"))
svdvals(x::Array{Complex{ArbReal{N}},2}) where {N} = throw(ErrorException("not supported"))
svdvals(x::Array{ArbComplex,2}) = throw(ErrorException("not supported"))
svdvals(x::Array{ArbComplex,M}) where {M} = throw(ErrorException("not supported"))
svdvals(x::Array{Complex{ArbFloat},2}) = throw(ErrorException("not supported"))
svdvals(x::Array{Complex{ArbReal},2}) = throw(ErrorException("not supported"))

eigvals(x::Array{ArbReal{N},2}) where {N} = ArbReal{N}.(eigvals(ArbFloat{N}.(midpoint.(x))))
eigvals(x::Array{ArbReal,2})  = ArbReal{workingprecision{ArbReal}}.(eigvals(ArbFloat{workingprecision(ArbFloat)}.(midpoint.(x))))

eigvals(x::Array{ArbComplex{N},2}) where {N} = throw(ErrorException("not supported"))
eigvals(x::Array{Complex{ArbFloat{N}},2}) where {N} = throw(ErrorException("not supported"))
eigvals(x::Array{Complex{ArbReal{N}},2}) where {N} = throw(ErrorException("not supported"))

eigvecs(x::Array{ArbFloat{N},2}) where {N} = throw(ErrorException("not supported"))
eigvecs(x::Array{ArbReal{N},2}) where {N} = throw(ErrorException("not supported"))
eigvecs(x::Array{ArbComplex{N},2}) where {N} = throw(ErrorException("not supported"))
eigvecs(x::Array{Complex{ArbFloat{N}},2}) where {N} = throw(ErrorException("not supported"))
eigvecs(x::Array{Complex{ArbReal{N}},2}) where {N} = throw(ErrorException("not supported"))
eigvecs(x::Array{ArbFloat{N},M}) where {N,M} = throw(ErrorException("not supported"))
eigvecs(x::Array{ArbReal{N},M}) where {N,M} = throw(ErrorException("not supported"))
eigvecs(x::Array{ArbComplex{N},M}) where {N,M} = throw(ErrorException("not supported"))

eigvecs(x::Array{ArbFloat,2}) = throw(ErrorException("not supported"))
eigvecs(x::Array{ArbReal,2}) = throw(ErrorException("not supported"))
eigvecs(x::Array{ArbComplex,2}) = throw(ErrorException("not supported"))
eigvecs(x::Array{Complex{ArbFloat},2}) = throw(ErrorException("not supported"))
eigvecs(x::Array{Complex{ArbReal},2}) = throw(ErrorException("not supported"))
eigvecs(x::Array{ArbFloat,M}) where {M} = throw(ErrorException("not supported"))
eigvecs(x::Array{ArbReal,M}) where {M} = throw(ErrorException("not supported"))
eigvecs(x::Array{ArbComplex,M}) where {M} = throw(ErrorException("not supported"))

eigen(x::Array{ArbFloat{N},2}) where {N} = throw(ErrorException("not supported"))
eigen(x::Array{ArbReal{N},2}) where {N} = throw(ErrorException("not supported"))
eigen(x::Array{ArbComplex{N},2}) where {N} = throw(ErrorException("not supported"))
eigen(x::Array{Complex{ArbFloat{N}},2}) where {N} = throw(ErrorException("not supported"))
eigen(x::Array{Complex{ArbReal{N}},2}) where {N} = throw(ErrorException("not supported"))
eigen(x::Array{ArbFloat{N},M}) where {N,M} = throw(ErrorException("not supported"))
eigen(x::Array{ArbReal{N},M}) where {N,M} = throw(ErrorException("not supported"))
eigen(x::Array{ArbComplex{N},M}) where {N,M} = throw(ErrorException("not supported"))

eigen(x::Array{ArbFloat,2}) = throw(ErrorException("not supported"))
eigen(x::Array{ArbReal,2}) = throw(ErrorException("not supported"))
eigen(x::Array{ArbComplex,2}) = throw(ErrorException("not supported"))
eigen(x::Array{Complex{ArbFloat},2}) = throw(ErrorException("not supported"))
eigen(x::Array{Complex{ArbReal},2}) = throw(ErrorException("not supported"))
eigen(x::Array{ArbFloat,M}) where {M} = throw(ErrorException("not supported"))
eigen(x::Array{ArbReal,M}) where {M} = throw(ErrorException("not supported"))
eigen(x::Array{ArbComplex,M}) where {M} = throw(ErrorException("not supported"))


module Vmrs
using CUDA
using DocStringExtensions
using Adapt

export AbstractVmr, Vmr, VmrGM, init_vmr, get_vmr

"""
    AbstractVmr{FT}
"""
abstract type AbstractVmr{FT <: AbstractFloat} end
"""
    VmrGM{FT,FTA1D,FTA2D} <: AbstractVmr{FT}

Volume mixing ratios for various gases in the atmosphere. This struct can be used
when only H₂O and O₃ concentrations vary spatially and a global mean is used
for all other gases.

# Fields
$(DocStringExtensions.FIELDS)
"""
struct VmrGM{FT <: AbstractFloat, FTA1D <: AbstractArray{FT, 1}, FTA2D <: AbstractArray{FT, 2}} <: AbstractVmr{FT}
    "volume mixing ratio of H₂O"
    vmr_h2o::FTA2D
    "volume mixing ratio of Ozone"
    vmr_o3::FTA2D
    "volume mixing ratio of all other gases, which are independent of location and column"
    vmr::FTA1D
end
VmrGM(vmr_h2o, vmr_o3, vmr) = VmrGM{eltype(vmr_h2o), typeof(vmr), typeof(vmr_h2o)}(vmr_h2o, vmr_o3, vmr)
Adapt.@adapt_structure VmrGM

function VmrGM(
    ::Type{FT},
    ::Type{DA},
    vmr_h2o::FTA2D,
    vmr_o3::FTA2D,
    vmr::FTA1D,
) where {FT <: AbstractFloat, FTA1D <: AbstractArray{FT, 1}, FTA2D <: AbstractArray{FT, 2}, DA}
    return VmrGM{FT, typeof(vmr), typeof(vmr_h2o)}(vmr_h2o, vmr_o3, vmr)
end

"""
    Vmr{FT,FTA3D} <: AbstractVmr{FT}

Volume mixing ratios for various gases in the atmosphere. This struct can be used
when concentrations vary spatially for all gases.

# Fields
$(DocStringExtensions.FIELDS)
"""
struct Vmr{FT <: AbstractFloat, FTA3D <: AbstractArray{FT, 3}} <: AbstractVmr{FT}
    "volume mixing ratio of all gases as a function of location and column"
    vmr::FTA3D
end
Vmr(vmr) = Vmr{eltype(vmr), typeof(vmr)}(vmr)
Adapt.@adapt_structure Vmr
"""
    get_vmr(
        vmr::VmrGM{FT},
        ig::Int,
        ilay::Int,
        icol::Int,
    ) where {FT<:AbstractFloat}

Obtain volume mixing ratio of gas `ig` for layer `ilay` of column `icol`.
"""
@inline function get_vmr(vmr::VmrGM{FT}, ig::Int, ilay::Int, icol::Int) where {FT <: AbstractFloat}
    if ig == 0
        return FT(1)
    elseif ig == 1 # h2o / h2o_foreign / h2o_self-continua
        return @inbounds vmr.vmr_h2o[ilay, icol]
    elseif ig == 3 # ozone
        return @inbounds vmr.vmr_o3[ilay, icol]
    else # other gases
        return @inbounds vmr.vmr[ig]
    end
end

"""
    get_vmr(
        vmr::Vmr{FT},
        ig::Int,
        ilay::Int,
        icol::Int,
    ) where {FT<:AbstractFloat}

Obtain volume mixing ratio of gas `ig` for layer `ilay` of column `icol`.
"""
@inline function get_vmr(vmr::Vmr{FT}, ig::Int, ilay::Int, icol::Int) where {FT <: AbstractFloat}
    if ig == 0
        return FT(1)
    else
        return @inbounds vmr.vmr[ilay, icol, ig]
    end
end

"""
    init_vmr(
        ngas,
        nlay,
        ncol,
        ::Type{FT},
        ::Type{DA};
        gm::Bool = true,
    ) where {FT<:AbstractFloat,DA}

Initialize the Vmr struct.
"""
function init_vmr(
    ngas::Int,
    nlay::Int,
    ncol::Int,
    ::Type{FT},
    ::Type{DA};
    gm::Bool = true,
) where {FT <: AbstractFloat, DA}
    if gm
        FTA1D = DA{FT, 1}
        FTA2D = DA{FT, 2}
        return VmrGM{FT, FTA1D, FTA2D}(FTA2D(zeros(nlay, ncol)), FTA2D(zeros(nlay, ncol)), FTA1D(zeros(ngas)))
    else
        FTA3D = DA{FT, 3}
        return Vmr{FT, FTA3D}(FTA3D(zeros(nlay, ncol, ngas)))
    end
end

end


struct TimeSeriesConstraintSpecInternal
    constraint_infos::Vector{DeviceTimeSeriesConstraintInfo}
    constraint_name::Symbol
    variable_name::Symbol
    bin_variable_name::Union{Nothing, Symbol}
    param_reference::Union{Nothing, UpdateRef}
end

function lazy_lb!(psi_container::PSIContainer, inputs::TimeSeriesConstraintSpecInternal)
    time_steps = model_time_steps(psi_container)
    names = [get_component_name(x) for x in inputs.constraint_infos]
    variable = get_variable(psi_container, inputs.variable_name)
    lb_name = middle_rename(inputs.constraint_name, PSI_NAME_DELIMITER, "lb")
    con_lb = add_cons_container!(psi_container, lb_name, names, time_steps)

    for constraint_info in inputs.constraint_infos
        ci_name = get_component_name(constraint_info)
        if constraint_info.range.limits.min > -Inf &&
           !isempty(constraint_info.range.additional_terms_lb)
            con_lb[ci_name, :].data .= JuMP.AffExpr(0.0)
            continue
        end
        for t in time_steps
            expression_lb = JuMP.AffExpr(0.0, variable[ci_name, t] => 1.0)
            for val in constraint_info.range.additional_terms_lb
                JuMP.add_to_expression!(
                    expression_lb,
                    get_variable(psi_container, val)[ci_name, t],
                    -1.0,
                )
            end
            lb_val = max(0.0, constraint_info.range.limits.min)
            con_lb[ci_name, t] =
                JuMP.@constraint(psi_container.JuMPmodel, expression_lb >= lb_val)
        end
    end
    return
end

@doc raw"""
Constructs upper bound for given variable and time series data and a multiplier.

# Constraint

```variable[name, t] <= constraint_infos[name].multiplier * ts_data[name].timeseries[t] ```

# LaTeX

`` x_t \leq r^{val} r_t, \forall t ``
"""
function device_timeseries_ub!(
    psi_container::PSIContainer,
    inputs::TimeSeriesConstraintSpecInternal,
)
    time_steps = model_time_steps(psi_container)
    names = [get_component_name(x) for x in inputs.constraint_infos]
    variable = get_variable(psi_container, inputs.variable_name)
    ub_name = middle_rename(inputs.constraint_name, PSI_NAME_DELIMITER, "ub")
    con_ub = add_cons_container!(psi_container, ub_name, names, time_steps)
    lazy_add_lb = false

    for constraint_info in inputs.constraint_infos
        ci_name = get_component_name(constraint_info)
        for t in time_steps
            expression_ub = JuMP.AffExpr(0.0, variable[ci_name, t] => 1.0)
            for val in constraint_info.range.additional_terms_ub
                JuMP.add_to_expression!(
                    expression_ub,
                    get_variable(psi_container, val)[ci_name, t],
                )
            end
            con_ub[ci_name, t] = JuMP.@constraint(
                psi_container.JuMPmodel,
                expression_ub <= constraint_info.multiplier * constraint_info.timeseries[t]
            )
        end
        if constraint_info.range.limits.min > -Inf ||
           !isempty(constraint_info.range.additional_terms_lb)
            lazy_add_lb = true
        end
    end

    @debug lazy_add_lb
    lazy_add_lb && lazy_lb!(psi_container, inputs)

    return
end

@doc raw"""
Constructs lower bound for given variable subject to time series data and a multiplier.

# Constraint

``` constraint_infos[name].multiplier * ts_data[name].timeseries[t] <= variable[name, t] ```

# LaTeX

`` r^{val} r_t \leq x_t, \forall t ``

where (name, data) in range_data.
"""
function device_timeseries_lb!(
    psi_container::PSIContainer,
    inputs::TimeSeriesConstraintSpecInternal,
)
    time_steps = model_time_steps(psi_container)
    variable = get_variable(psi_container, inputs.variable_name)
    lb_name = middle_rename(inputs.constraint_name, PSI_NAME_DELIMITER, "lb")
    names = [get_component_name(x) for x in inputs.constraint_infos]
    constraint = add_cons_container!(psi_container, lb_name, names, time_steps)

    for constraint_info in inputs.constraint_infos
        ci_name = get_component_name(constraint_info)
        for t in time_steps
            expression_lb = JuMP.AffExpr(0.0, variable[ci_name, t] => 1.0)
            for val in constraint_info.range.additional_terms_lb
                JuMP.add_to_expression!(
                    expression_lb,
                    get_variable(psi_container, val)[ci_name, t],
                    -1.0,
                )
            end
            constraint[ci_name, t] = JuMP.@constraint(
                psi_container.JuMPmodel,
                expression_lb >= constraint_info.multiplier * constraint_info.timeseries[t]
            )
        end
    end
    return
end

@doc raw"""
Constructs upper bound for given variable using a parameter. The constraint is
    built with a time series data vector and a multiplier

# Constraint

``` variable[name, t] <= constraint_infos[name].multiplier * param[name, t] ```

# LaTeX

`` x^{var}_t \leq r^{val} x^{param}_t, \forall t ``
"""
function device_timeseries_param_ub!(
    psi_container::PSIContainer,
    inputs::TimeSeriesConstraintSpecInternal,
)
    time_steps = model_time_steps(psi_container)
    names = [get_component_name(x) for x in inputs.constraint_infos]
    variable = get_variable(psi_container, inputs.variable_name)
    ub_name = middle_rename(inputs.constraint_name, PSI_NAME_DELIMITER, "ub")
    con_ub = add_cons_container!(psi_container, ub_name, names, time_steps)
    container =
        add_param_container!(psi_container, inputs.param_reference, names, time_steps)
    multiplier = get_multiplier_array(container)
    param = get_parameter_array(container)
    lazy_add_lb = false

    for constraint_info in inputs.constraint_infos
        ci_name = get_component_name(constraint_info)
        for t in time_steps
            expression_ub = JuMP.AffExpr(0.0, variable[ci_name, t] => 1.0)
            for val in constraint_info.range.additional_terms_ub
                JuMP.add_to_expression!(
                    expression_ub,
                    get_variable(psi_container, val)[ci_name, t],
                )
            end
            param[ci_name, t] =
                PJ.add_parameter(psi_container.JuMPmodel, constraint_info.timeseries[t])
            con_ub[ci_name, t] = JuMP.@constraint(
                psi_container.JuMPmodel,
                expression_ub <= constraint_info.multiplier * param[ci_name, t]
            )
            multiplier[ci_name, t] = constraint_info.multiplier
        end
        if constraint_info.range.limits.min > -Inf ||
           !isempty(constraint_info.range.additional_terms_lb)
            lazy_add_lb = true
        end
    end

    @debug lazy_add_lb
    lazy_add_lb && lazy_lb!(psi_container, inputs)
    return
end

@doc raw"""
Constructs lower bound for given variable using a parameter. The constraint is
    built with a time series data vector and a multiplier

# Constraint

``` constraint_infos[name].multiplier * param[name, t] <= variable[name, t] ```

# LaTeX

`` r^{val} x^{param}_t \leq x^{var}_t, \forall t ``
"""
function device_timeseries_param_lb!(
    psi_container::PSIContainer,
    inputs::TimeSeriesConstraintSpecInternal,
)
    time_steps = model_time_steps(psi_container)
    variable = get_variable(psi_container, inputs.variable_name)
    lb_name = middle_rename(inputs.constraint_name, PSI_NAME_DELIMITER, "lb")
    names = [get_component_name(x) for x in inputs.constraint_infos]
    constraint = add_cons_container!(psi_container, lb_name, names, time_steps)
    container =
        add_param_container!(psi_container, inputs.param_reference, names, time_steps)
    multiplier = get_multiplier_array(container)
    param = get_parameter_array(container)

    for constraint_info in inputs.constraint_infos
        ci_name = get_component_name(constraint_info)
        for t in time_steps
            expression_lb = JuMP.AffExpr(0.0, variable[ci_name, t] => 1.0)
            for val in constraint_info.range.additional_terms_lb
                JuMP.add_to_expression!(
                    expression_lb,
                    get_variable(psi_container, val)[ci_name, t],
                    -1.0,
                )
            end
            param[ci_name, t] =
                PJ.add_parameter(psi_container.JuMPmodel, constraint_info.timeseries[t])
            constraint_info[ci_name, t] = JuMP.@constraint(
                psi_container.JuMPmodel,
                expression_lb >= constraint_info.multiplier * param[ci_name, t]
            )
            multiplier[ci_name, t] = constraint_info.multiplier
        end
    end

    return
end

@doc raw"""
Constructs upper bound for variable and time series or confines to 0 depending on binary variable.
    The upper bound is defined by a time series and a multiplier.

# constraint_infos

``` varcts[name, t] <= varbin[name, t]* constraint_infos[name].multiplier * ts_data[name].timeseries[t] ```

where (name, data) in range_data.

# LaTeX

`` x^{cts}_t \leq r^{val} r_t x^{bin}_t, \forall t ``
"""
function device_timeseries_ub_bin!(
    psi_container::PSIContainer,
    inputs::TimeSeriesConstraintSpecInternal,
)
    time_steps = model_time_steps(psi_container)
    ub_name = middle_rename(inputs.constraint_name, PSI_NAME_DELIMITER, "ub")
    varcts = get_variable(psi_container, inputs.variable_name)
    varbin = get_variable(psi_container, inputs.bin_variable_name)
    names = [get_component_name(x) for x in inputs.constraint_infos]
    con_ub = add_cons_container!(psi_container, ub_name, names, time_steps)
    for constraint_info in inputs.constraint_infos
        ci_name = get_component_name(constraint_info)
        for t in time_steps
            forecast = constraint_info.timeseries[t]
            multiplier = constraint_info.multiplier
            expression_ub = JuMP.AffExpr(0.0, varcts[ci_name, t] => 1.0)
            for val in constraint_info.range.additional_terms_ub
                JuMP.add_to_expression!(
                    expression_ub,
                    get_variable(psi_container, val)[ci_name, t],
                )
            end
            con_ub[ci_name, t] = JuMP.@constraint(
                psi_container.JuMPmodel,
                expression_ub <= varbin[ci_name, t] * multiplier * forecast
            )
        end
    end
    return
end

@doc raw"""
Constructs upper bound for variable and time series and a multiplier or confines to 0 depending on binary variable.
    Uses BigM constraint type to allow for parameter since ParameterJuMP doesn't support var*parameter

# constraint_infos

``` varcts[name, t] - constraint_infos[name].multipliers * param[name, t] <= (1 - varbin[name, t]) * M_value ```

``` varcts[name, t] <= varbin[name, t]*M_value ```

# LaTeX

`` x^{cts}_t - r^{val} x^{param}_t \leq M(1 - x^{bin}_t ), forall t ``

`` x^{cts}_t \leq M x^{bin}_t, \forall t ``
"""
function device_timeseries_ub_bigM!(
    psi_container::PSIContainer,
    inputs::TimeSeriesConstraintSpecInternal,
)
    time_steps = model_time_steps(psi_container)
    ub_name = middle_rename(inputs.constraint_name, PSI_NAME_DELIMITER, "ub")
    key_status = middle_rename(inputs.constraint_name, PSI_NAME_DELIMITER, "status")

    varcts = get_variable(psi_container, inputs.variable_name)
    varbin = get_variable(psi_container, inputs.bin_variable_name)
    names = [get_component_name(x) for x in inputs.constraint_infos]
    con_ub = add_cons_container!(psi_container, ub_name, names, time_steps)
    con_status = add_cons_container!(psi_container, key_status, names, time_steps)
    container =
        add_param_container!(psi_container, inputs.param_reference, names, time_steps)
    multiplier = get_multiplier_array(container)
    param = get_parameter_array(container)

    for constraint_info in inputs.constraint_infos
        ci_name = get_component_name(constraint_info)
        for t in time_steps
            expression_ub = JuMP.AffExpr(0.0, varcts[ci_name, t] => 1.0)
            for val in constraint_info.range.additional_terms_ub
                JuMP.add_to_expression!(
                    expression_ub,
                    get_variable(psi_container, val)[ci_name, t],
                )
            end
            param[ci_name, t] =
                PJ.add_parameter(psi_container.JuMPmodel, constraint_info.timeseries[t])
            con_ub[ci_name, t] = JuMP.@constraint(
                psi_container.JuMPmodel,
                expression_ub - param[ci_name, t] * constraint_info.multiplier <=
                (1 - varbin[ci_name, t]) * M_VALUE
            )
            con_status[ci_name, t] = JuMP.@constraint(
                psi_container.JuMPmodel,
                expression_ub <= varbin[ci_name, t] * M_VALUE
            )
            multiplier[ci_name, t] = constraint_info.multiplier
        end
    end
    return
end


const MANTISSA_MASK = Base.significand_mask(Float64)
const EXP_MASK = Base.exponent_mask(Float64) >> Base.significand_bits(Float64)

memcpy(d, doff, s, soff, n) = ccall(:memcpy, Cvoid, (Ptr{Cvoid}, Ptr{Cvoid}, Int), d + doff - 1, s + soff - 1, n)
memmove(d, doff, s, soff, n) = ccall(:memmove, Cvoid, (Ptr{Cvoid}, Ptr{Cvoid}, Int), d + doff - 1, s + soff - 1, n)

pow5_bitcount(::Type{Float16}) = 30
pow5_bitcount(::Type{Float32}) = 61
pow5_bitcount(::Type{Float64}) = 121

pow5_inv_bitcount(::Type{Float16}) = 30
pow5_inv_bitcount(::Type{Float32}) = 59
pow5_inv_bitcount(::Type{Float64}) = 122

qinvbound(::Type{Float16}) = 4
qinvbound(::Type{Float32}) = 9
qinvbound(::Type{Float64}) = 21

qbound(::Type{Float16}) = 15
qbound(::Type{Float32}) = 31
qbound(::Type{Float64}) = 63

"""
    Ryu.log10pow2(e::Integer)

Computes `floor(log10(2^e))`. This is valid for all `e < 1651`.
"""
log10pow2(e) = (e * 78913) >> 18

"""
    Ryu.log10pow5(e::Integer)

Computes `floor(log10(5^e))`. This is valid for all `e < 2621`.
"""
log10pow5(e) = (e * 732923) >> 20

"""
    Ryu.pow5bits(e)

Computes `e == 0 ? 1 : ceil(log2(5^e))`. This is valid for `e < 3529` (if performend in `Int32` arithmetic).
"""
pow5bits(e) = ((e * 1217359) >> 19) + 1

""""
     Ryu.mulshift(m::U, mula, j) where {U<:Unsigned}

Compute `(m * mul) >> j`, where `j >= 8*sizeof(U)`. The type of the results is the larger of `U` or `UInt32`.
"""
@inline function mulshift(m::U, mul, j) where {U<:Unsigned}
    W = widen(U)
    nbits = 8*sizeof(U)
    return ((((W(m) * (mul % U)) >> nbits) + W(m) * (mul >> nbits)) >> (j - nbits)) % promote_type(U,UInt32)
end

indexforexp(e) = div(e + 15, 16)
pow10bitsforindex(idx) = 16 * idx + 120
lengthforindex(idx) = div(((Int64(16 * idx) * 1292913986) >> 32) + 1 + 16 + 8, 9)

"""
    Ryu.pow5(x, p)

Return `true` if `5^p` is a divisor of `x`.
"""
@inline function pow5(x, p)
    count = 0
    while true
        q = div(x, 5)
        r = x - 5 * q
        r != 0 && return count >= p
        x = q
        count += 1
    end
end

"""
    Ryu.pow2(x, p)

Return `true` if `2^p` is a divisor of `x`. In other words, if the trailing `p` bits of `x` are zero.
"""
pow2(x, p) = (x & ((Int64(1) << p) - 1)) == 0

"""
    Ryu.decimallength(v)

The number of decimal digits of the integer `v`.
"""
@inline function decimallength(v)
    v >= 10000000000000000 && return 17
    v >= 1000000000000000 && return 16
    v >= 100000000000000 && return 15
    v >= 10000000000000 && return 14
    v >= 1000000000000 && return 13
    v >= 100000000000 && return 12
    v >= 10000000000 && return 11
    v >= 1000000000 && return 10
    v >= 100000000 && return 9
    v >= 10000000 && return 8
    v >= 1000000 && return 7
    v >= 100000 && return 6
    v >= 10000 && return 5
    v >= 1000 && return 4
    v >= 100 && return 3
    v >= 10 && return 2
    return 1
end
@inline function decimallength(v::UInt32)
    v >= 100000000 && return 9
    v >= 10000000 && return 8
    v >= 1000000 && return 7
    v >= 100000 && return 6
    v >= 10000 && return 5
    v >= 1000 && return 4
    v >= 100 && return 3
    v >= 10 && return 2
    return 1
end
@inline function decimallength(v::UInt16)
    v >= 10000 && return 5
    v >= 1000 && return 4
    v >= 100 && return 3
    v >= 10 && return 2
    return 1
end

@inline function mulshiftinvsplit(::Type{Float64}, mv, mp, mm, i, j)
    @inbounds mul = DOUBLE_POW5_INV_SPLIT[i + 1]
    vr = mulshift(mv, mul, j)
    vp = mulshift(mp, mul, j)
    vm = mulshift(mm, mul, j)
    return vr, vp, vm
end

@inline function mulshiftinvsplit(::Type{Float32}, mv, mp, mm, i, j)
    @inbounds mul = FLOAT_POW5_INV_SPLIT[i + 1]
    vr = mulshift(mv, mul, j)
    vp = mulshift(mp, mul, j)
    vm = mulshift(mm, mul, j)
    return vr, vp, vm
end

@inline function mulshiftinvsplit(::Type{Float16}, mv, mp, mm, i, j)
    @inbounds mul = HALF_POW5_INV_SPLIT[i + 1]
    vr = mulshift(mv, mul, j)
    vp = mulshift(mp, mul, j)
    vm = mulshift(mm, mul, j)
    return vr, vp, vm
end

@inline function mulshiftsplit(::Type{Float64}, mv, mp, mm, i, j)
    @inbounds mul = DOUBLE_POW5_SPLIT[i + 1]
    vr = mulshift(mv, mul, j)
    vp = mulshift(mp, mul, j)
    vm = mulshift(mm, mul, j)
    return vr, vp, vm
end

@inline function mulshiftsplit(::Type{Float32}, mv, mp, mm, i, j)
    @inbounds mul = FLOAT_POW5_SPLIT[i + 1]
    vr = mulshift(mv, mul, j)
    vp = mulshift(mp, mul, j)
    vm = mulshift(mm, mul, j)
    return vr, vp, vm
end

@inline function mulshiftsplit(::Type{Float16}, mv, mp, mm, i, j)
    @inbounds mul = HALF_POW5_SPLIT[i + 1]
    vr = mulshift(mv, mul, j)
    vp = mulshift(mp, mul, j)
    vm = mulshift(mm, mul, j)
    return vr, vp, vm
end

"""
    Ryu.umul256(a::UInt128, bHi::UInt64, bLo::UInt64)::Tuple{UInt128, UInt128}

Compute `p = a*b` where `b = bLo + bHi<<64`, returning the result as `pLo, pHi` where `p = pLo + pHi<<128`.
"""
@inline function umul256(a, bHi, bLo)
    aLo = a % UInt64
    aHi = (a >> 64) % UInt64

    b00 = UInt128(aLo) * bLo
    b01 = UInt128(aLo) * bHi
    b10 = UInt128(aHi) * bLo
    b11 = UInt128(aHi) * bHi

    b00Lo = b00 % UInt64
    b00Hi = (b00 >> 64) % UInt64

    mid1 = b10 + b00Hi
    mid1Lo = mid1 % UInt64
    mid1Hi = (mid1 >> 64) % UInt64

    mid2 = b01 + mid1Lo
    mid2Lo = mid2 % UInt64
    mid2Hi = (mid2 >> 64) % UInt64

    pHi = b11 + mid1Hi + mid2Hi
    pLo = (UInt128(mid2Lo) << 64) | b00Lo
    return pLo, pHi
end

"""
    Ryu.umul256_hi(a::UInt128, bHi::UInt64, bLo::UInt64)::UInt128

Compute `pHi = (a*b)>>128` where `b = bLo + bHi<<64`.
"""
@inline umul256_hi(a, bHi, bLo) = umul256(a, bHi, bLo)[2]

"""
    Ryu.mulshiftmod1e9(m, mula, mulb, mulc, j)::UInt32

Compute `(m * mul) >> j % 10^9` where `mul = mula + mulb<<64 + mulc<<128`, and `j >= 128`.
"""
@inline function mulshiftmod1e9(m, mula, mulb, mulc, j)
    b0 = UInt128(m) * mula
    b1 = UInt128(m) * mulb
    b2 = UInt128(m) * mulc
    mid = b1 + ((b0 >> 64) % UInt64)
    s1 = b2 + ((mid >> 64) % UInt64)
    v = s1 >> (j - 128)
    multiplied = umul256_hi(v, 0x89705F4136B4A597, 0x31680A88F8953031)
    shifted = (multiplied >> 29) % UInt32
    return (v % UInt32) - UInt32(1000000000) * shifted
end

@inline function append_n_digits(olength, digits, buf, pos)
    i = 0
    while digits >= 10000
        c = digits % 10000
        digits = div(digits, 10000)
        c0 = (c % 100) << 1
        c1 = div(c, 100) << 1
        unsafe_copyto!(buf, pos + olength - i - 2, DIGIT_TABLE, c0 + 1, 2)
        unsafe_copyto!(buf, pos + olength - i - 4, DIGIT_TABLE, c1 + 1, 2)
        i += 4
    end
    if digits >= 100
        c = (digits % 100) << 1
        digits = div(digits, 100)
        unsafe_copyto!(buf, pos + olength - i - 2, DIGIT_TABLE, c + 1, 2)
        i += 2
    end
    if digits >= 10
        c = digits << 1
        unsafe_copyto!(buf, pos + olength - i - 2, DIGIT_TABLE, c + 1, 2)
        i += 2
    else
        buf[pos] = UInt8('0') + digits
        i += 1
    end
    return pos + i
end

@inline function append_d_digits(olength, digits, buf, pos, decchar)
    i = 0
    while digits >= 10000
        c = digits % 10000
        digits = div(digits, 10000)
        c0 = (c % 100) << 1
        c1 = div(c, 100) << 1
        unsafe_copyto!(buf, pos + olength + 1 - i - 2, DIGIT_TABLE, c0 + 1, 2)
        unsafe_copyto!(buf, pos + olength + 1 - i - 4, DIGIT_TABLE, c1 + 1, 2)
        i += 4
    end
    if digits >= 100
        c = (digits % 100) << 1
        digits = div(digits, 100)
        unsafe_copyto!(buf, pos + olength + 1 - i - 2, DIGIT_TABLE, c + 1, 2)
        i += 2
    end
    if digits >= 10
        c = digits << 1
        buf[pos] = DIGIT_TABLE[c + 1]
        buf[pos + 1] = decchar
        buf[pos + 2] = DIGIT_TABLE[c + 2]
        i += 3
    else
        buf[pos] = UInt8('0') + digits
        buf[pos + 1] = decchar
        i += 2
    end
    return pos + i
end

@inline function append_c_digits(count, digits, buf, pos)
    i = 0
    while i < count - 1
        c = (digits % 100) << 1
        digits = div(digits, 100)
        unsafe_copyto!(buf, pos + count - i - 2, DIGIT_TABLE, c + 1, 2)
        i += 2
    end
    if i < count
        buf[pos + count - i - 1] = UInt8('0') + (digits % 10)
        i += 1
    end
    return pos + i
end

@inline function append_nine_digits(digits, buf, pos)
    if digits == 0
        for _ = 1:9
            buf[pos] = UInt8('0')
            pos += 1
        end
        return pos
    end
    i = 0
    while i < 5
        c = digits % 10000
        digits = div(digits, 10000)
        c0 = (c % 100) << 1
        c1 = div(c, 100) << 1
        unsafe_copyto!(buf, pos + 7 - i, DIGIT_TABLE, c0 + 1, 2)
        unsafe_copyto!(buf, pos + 5 - i, DIGIT_TABLE, c1 + 1, 2)
        i += 4
    end
    buf[pos] = UInt8('0') + digits
    i += 1
    return pos + i
end

const BIG_MASK = (big(1) << 64) - 1

const POW10_SPLIT = collect(Iterators.flatten(map(0:63) do idx
    pow10bits = pow10bitsforindex(idx)
    map(0:lengthforindex(idx)-1) do i
        v = (div(big(1) << pow10bits, big(10)^(9 * i)) + 1) % ((big(10)^9) << 136)
        return (UInt64(v & BIG_MASK), UInt64((v >> 64) & BIG_MASK), UInt64((v >> 128) & BIG_MASK))
    end
end))

function generateinversetables()
    POW10_OFFSET_2 = Vector{UInt16}(undef, 68 + 1)
    MIN_BLOCK_2 = fill(0xff, 68 + 1)
    POW10_SPLIT_2 = Tuple{UInt64, UInt64, UInt64}[]
    lowerCutoff = big(1) << (54 + 8)
    for idx = 0:67
        POW10_OFFSET_2[idx + 1] = length(POW10_SPLIT_2)
        i = 0
        while true
            v = ((big(10)^(9 * (i + 1)) >> (-(120 - 16 * idx))) % (big(10)^9) << (120 + 16))
            if MIN_BLOCK_2[idx + 1] == 0xff && ((v * lowerCutoff) >> 128) == 0
                i += 1
                continue
            end
            if MIN_BLOCK_2[idx + 1] == 0xff
                MIN_BLOCK_2[idx + 1] = i
            end
            v == 0 && break
            push!(POW10_SPLIT_2, ((v & BIG_MASK) % UInt64, ((v >> 64) & BIG_MASK) % UInt64, ((v >> 128) & BIG_MASK) % UInt64))
            i += 1
        end
    end
    POW10_OFFSET_2[end] = length(POW10_SPLIT_2)
    MIN_BLOCK_2[end] = 0x00

    return POW10_OFFSET_2, MIN_BLOCK_2, POW10_SPLIT_2
end

const POW10_OFFSET_2, MIN_BLOCK_2, POW10_SPLIT_2 = generateinversetables()

"""
    Ryu.pow5invsplit(T, i)

Compute `floor(2^k/5^i)+1`, where `k = pow5bits(i) - 1 + pow5_inv_bitcount(T)`. The result
is an unsigned integer twice as wide as `T` (i.e. a `UInt128` if `T == Float64`), with
`pow5_inv_bitcount(T)` significant bits.
"""
function pow5invsplit(::Type{T}, i) where {T<:AbstractFloat}
    W = widen(uinttype(T))
    pow = big(5)^i
    inv = div(big(1) << (ndigits(pow, base=2) - 1 + pow5_inv_bitcount(T)), pow) + 1
    return W(inv)
end

"""
    Ryu.pow5split(T, i)

Compute `floor(5^i/2^k)`, where `k = pow5bits(i) - pow5_bitcount(T)`. The result is an
unsigned integer twice as wide as `T` (i.e. a `UInt128` if `T == Float64`), with
`pow5_bitcount(T)` significant bits.
"""
function pow5split(::Type{T}, i) where {T<:AbstractFloat}
    W = widen(uinttype(T))
    pow = big(5)^i
    return W(pow >> (ndigits(pow, base=2) - pow5_bitcount(T)))
end

const DOUBLE_POW5_INV_SPLIT = map(i->pow5invsplit(Float64, i), 0:291)
const FLOAT_POW5_INV_SPLIT = map(i->pow5invsplit(Float32, i), 0:30)
const HALF_POW5_INV_SPLIT = map(i->pow5invsplit(Float16, i), 0:17)

const DOUBLE_POW5_SPLIT = map(i->pow5split(Float64, i), 0:325)
const FLOAT_POW5_SPLIT = map(i->pow5split(Float32, i), 0:46)
const HALF_POW5_SPLIT = map(i->pow5split(Float16, i), 0:23)

const DIGIT_TABLE = UInt8[
  '0','0','0','1','0','2','0','3','0','4','0','5','0','6','0','7','0','8','0','9',
  '1','0','1','1','1','2','1','3','1','4','1','5','1','6','1','7','1','8','1','9',
  '2','0','2','1','2','2','2','3','2','4','2','5','2','6','2','7','2','8','2','9',
  '3','0','3','1','3','2','3','3','3','4','3','5','3','6','3','7','3','8','3','9',
  '4','0','4','1','4','2','4','3','4','4','4','5','4','6','4','7','4','8','4','9',
  '5','0','5','1','5','2','5','3','5','4','5','5','5','6','5','7','5','8','5','9',
  '6','0','6','1','6','2','6','3','6','4','6','5','6','6','6','7','6','8','6','9',
  '7','0','7','1','7','2','7','3','7','4','7','5','7','6','7','7','7','8','7','9',
  '8','0','8','1','8','2','8','3','8','4','8','5','8','6','8','7','8','8','8','9',
  '9','0','9','1','9','2','9','3','9','4','9','5','9','6','9','7','9','8','9','9'
]

const POW10_OFFSET = UInt16[
  0, 2, 5, 8, 12, 16, 21, 26, 32, 39,
  46, 54, 62, 71, 80, 90, 100, 111, 122, 134,
  146, 159, 173, 187, 202, 217, 233, 249, 266, 283,
  301, 319, 338, 357, 377, 397, 418, 440, 462, 485,
  508, 532, 556, 581, 606, 632, 658, 685, 712, 740,
  769, 798, 828, 858, 889, 920, 952, 984, 1017, 1050,
  1084, 1118, 1153, 1188
]


## Functions to handle input files.

using Chemfiles

include("clustering.jl")

"""
    parse_cif(file_path)

Parse a CIF file and return a dictionary where each identifier (without the
starting '_' character) is linked to its value.
Values are either a string or a vector of string (if defined in a loop).
"""
function parse_cif(file)
    all_data = Dict{String, Union{String, Vector{String}}}()
    inloop = false
    loopisspecified = false
    loopspec = String[]
    loop_n = 0

    l = read(file, String)
    i, j, x = nextword(l, 0)
    while i != 0

        if inloop
            if !loopisspecified
                if l[i] != '_' # This indicates the start of the values
                    loop_n = length(loopspec)
                    loopisspecified = true
                else # The identifier is part of the loop specification
                    push!(loopspec, l[i+1:j])
                    all_data[l[i+1:j]] = String[]
                    i, j, x = nextword(l, x); continue
                end
            end

            # From this point, the loop has been specified
            @assert loopisspecified
            if l[i] != '_' && l[i:j] != "loop_"
                for k in 1:loop_n
                    push!(all_data[loopspec[k]], l[i:j])
                    i, j, x = nextword(l, x)
                end
                continue
            end
            if l[i:j] == "loop_"
                loopisspecified = false
                loopspec = String[]
                i, j, x = nextword(l, x); continue
            end

            # This point can only be reached if we just quitted a loop
            inloop = false
        end

        @assert !inloop
        if l[i] == '_' # Simple identifier definition
            next_i, next_j, next_x = nextword(l, x)
            @assert next_i != 0
            all_data[l[i+1:j]] = l[next_i:next_j]
            x = next_x
        else
            if l[i:j] == "loop_"
                inloop = true
                loopisspecified = false
                loopspec = String[]
            elseif j-i > 4 && l[i:i+4] == "data_"
                @assert !haskey(all_data, "data")
                all_data["data"] = l[i+5:j]
            else
                k::Int = findprev(isequal('\n'), l, i)
                n::Int = count("\n", l[1:k])
                error("Unkown word \"$(l[i:j])\" at line $(n+1), position $(i-k):$(j-k)")
            end
        end

        i, j, x = nextword(l, x)
    end

    return all_data
end

function parsestrip(s)
    s = s[end] == ')' ? s[1:prevind(s, findlast('(', s))] : s
    return parse(BigFloat, s)
end

"""
    CIF(file_path::AbstractString)

Make a CIF object out of the parsed file.
"""
CIF(file::AbstractString) = CIF(parse_cif(file))
function CIF(parsed::Dict{String, Union{Vector{String},String}})
    natoms = length(parsed["atom_site_label"])
    equivalentpositions = pop!(parsed,
        haskey(parsed, "symmetry_equiv_pos_as_xyz") ?
            "symmetry_equiv_pos_as_xyz" : "space_group_symop_operation_xyz")
    refid = find_refid(equivalentpositions)
    cell = Cell(Symbol(haskey(parsed, "symmetry_cell_setting") ?
                            pop!(parsed, "symmetry_cell_setting") : ""),
                pop!(parsed, "symmetry_space_group_name_H-M"),
                haskey(parsed, "symmetry_Int_Tables_number") ?
                    parse(Int, pop!(parsed, "symmetry_Int_Tables_number")) : 0,
                parsestrip(pop!(parsed, "cell_length_a")),
                parsestrip(pop!(parsed, "cell_length_b")),
                parsestrip(pop!(parsed, "cell_length_c")),
                parsestrip(pop!(parsed, "cell_angle_alpha")),
                parsestrip(pop!(parsed, "cell_angle_beta")),
                parsestrip(pop!(parsed, "cell_angle_gamma")),
                parse.(EquivalentPosition, equivalentpositions, Ref(refid)))

    haskey(parsed, "symmetry_equiv_pos_site_id") && pop!(parsed, "symmetry_equiv_pos_site_id")
    removed_identity = false
    for i in eachindex(cell.equivalents)
        eq = cell.equivalents[i]
        if isone(eq.mat) && iszero(eq.ofs)
            deleteat!(cell.equivalents, i)
            removed_identity = true
            break
        end
    end
    @assert removed_identity

    labels =  pop!(parsed, "atom_site_label")
    _symbols = haskey(parsed, "atom_site_type_symbol") ?
                    pop!(parsed, "atom_site_type_symbol") : copy(labels)
    symbols = String[]
    @inbounds for x in _symbols
        i = findfirst(!isletter, x)
        push!(symbols, isnothing(i) ? x : x[1:i-1])
    end
    pos_x = pop!(parsed, "atom_site_fract_x")
    pos_y = pop!(parsed, "atom_site_fract_y")
    pos_z = pop!(parsed, "atom_site_fract_z")

    types = Symbol[]
    pos = Matrix{Float64}(undef, 3, natoms)
    correspondence = Dict{String, Int}()
    for i in 1:natoms
        @assert !haskey(correspondence, labels[i])
        correspondence[labels[i]] = i
        push!(types, Symbol(symbols[i]))
        pos[:,i] = parsestrip.([pos_x[i], pos_y[i], pos_z[i]])
        pos[:,i] .-= floor.(Int, pos[:,i])
    end

    invids = sortperm(types)
    types = types[invids]
    ids = invperm(invids)
    bonds = falses(natoms, natoms)
    if haskey(parsed, "geom_bond_atom_site_label_1") &&
       haskey(parsed, "geom_bond_atom_site_label_2")
        bond_a = pop!(parsed, "geom_bond_atom_site_label_1")
        bond_b = pop!(parsed, "geom_bond_atom_site_label_2")
        for i in 1:length(bond_a)
            x = correspondence[bond_a[i]]
            y = correspondence[bond_b[i]]
            bonds[x,y] = bonds[y,x] = 1
        end
    end

    return CIF(parsed, cell, ids, types, pos, bonds)
end

"""
    parse_arc(file)

Parse a .arc Systre archive such as the one used by the RCSR.
Return a pair `(flag, pairs)`.

`flag` is set if the archive corresponds to one generated by a compatible release
of CrystalNets. If unset, the genomes of the archive may not be the same as those
computed by CrystalNets for the same nets.
`pairs` is a `Dict{String,String}` whose entries have the form `genome => id` where `id`
is the name of the net and `genome` is the topological genome corresponding to this net
(given as a string of whitespace-separated values parseable by `PeriodicGraph`).
"""
function parse_arc(file)
    pairs = Dict{String,String}()
    curr_key = ""
    counter = 1
    firstline = readline(file)
    flag = startswith(firstline, "Made by CrystalNets.jl v")
    if flag
        flag = CRYSTAL_NETS_VERSION == VersionNumber(firstline[25:end])
    end
    for l in eachline(file)
        if length(l) > 3 && l[1:3] == "key"
            @assert isempty(curr_key)
            i = 4
            while isspace(l[i])
                i += 1
            end
            curr_key = l[i:end]
            @assert !haskey(pairs, curr_key)
        elseif length(l) > 2 && l[1:2] == "id"
            @assert !isempty(curr_key)
            i = 3
            while isspace(l[i])
                i += 1
            end
            pairs[curr_key] = l[i:end]
            curr_key = ""
        end
    end
    return flag, pairs
end

"""
    parse_arcs(file)

Parse a folder containing .arc Systre archives such as the one used by the RCSR.
Return a pair `(flag, pairs)` with the same convention than `parse_arc`
"""
function parse_arcs(path)
    combine(x, y) = x * ", " * y
    dict = Dict{String,String}()
    flag = true
    for f in readdir(path; sort=true)
        _flag, _dict = parse_arc(path*f)
        flag &= _flag
        mergewith!(combine, dict, _dict)
    end
    return flag, dict
end

function parse_atom_name(name::AbstractString)
    firstsep = findfirst(x -> ispunct(x) || isspace(x) || isnumeric(x), name)
    if firstsep isa Nothing
        return String(name)
    else
        firstsep::Int
        if firstsep != 1 && !any(isuppercase, @view name[nextind(name, firstsep):end])
            return String(name[1:prevind(firstsep)])
        else
            return String(name)
        end
    end
end

function parse_atom(name)
    atom = Chemfiles.Atom(name)
    set_type!(atom, parse_atom_name(name))
    return atom
end

function set_unique_bond_type!(frame::Frame, types, bond_length, bonded_atoms::Tuple{Symbol, Symbol}, onlykeep, tol)
    @ifwarn @info "To avoid guessing bonds, use a file format that contains the bonds."
    n = Int(size(frame))
    pos = Chemfiles.positions(frame)
    mat = Chemfiles.matrix(UnitCell(frame))'
    indices = [i for i in 1:n if types[i] ∈ onlykeep]
    #=@inbounds=# for _i in 1:length(indices)
        i = indices[_i]
        for _j in _i+1:length(indices)
            j = indices[_j]
            if minmax(types[i], types[j]) == bonded_atoms
                bonded = abs2(periodic_distance(pos[:,i], pos[:,j], mat) - bond_length) <= tol
                if bonded
                    Chemfiles.add_bond!(frame, i-1, j-1)
                end
            end
        end
    end
    nothing
end

function try_guess_bonds!(frame::Frame)
    # n = Int(size(frame))
    # types = Vector{Symbol}(undef, n)
    # for i in 1:n
    #     types[i] = Symbol(parse_atom_name(Chemfiles.type(Chemfiles.Atom(frame, i-1))))
    # end
    # unique_types = unique!(sort(types))
    # if unique_types == [:C] || unique_types == [:C, :H]
    #     if !(NOWARN::Bool)
    #         @warn "Guessing bonds. The structure seems to be made of only carbons (and possibly hydrogens): using an interatomic distance of 1.54±0.3 Å to assign edges between C atoms."
    #     end
    #     set_unique_bond_type!(frame, types, 1.54, (:C, :C), (:C,), 0.3)
    # elseif unique_types == [:O, :Si] || unique_types == [:Si]
    #     if !(NOWARN::Bool)
    #         @warn "Guessing bonds. The structure seems to be a zeolite: using an interatomic distance of 3.1±0.2 Å to assign edges between Si atoms."
    #     end
    #     set_unique_bond_type!(frame, types, 1.63, (:O, :Si), (:O, :Si), 0.1)
    # else
        @ifwarn begin
            @warn "Guessing bonds through Chemfiles. This may take a while for big structures and may be inexact."
            @info "To avoid guessing bonds, use a file format that contains the bonds."
        end
        guess_bonds!(frame)
    # end
end

function attribute_residues(topology, assert_use_existing_residues)
    m = Int(count_residues(topology))
    n = Int(size(topology))
    atoms_in_residues = m == 0 ? 0 : sum(length(atoms(Residue(topology, i))) for i in 0:(m-1))
    @assert atoms_in_residues <= n

    if atoms_in_residues < n
        if assert_use_existing_residues
            throw(ArgumentError("""
            Cannot use existing residues as vertices because not all atoms have an associated residue.
            To fix this, either assign a residue to each atom or provide another way to detect the vertices.
            """))
        end
        @ifwarn begin
            if atoms_in_residues > 0
                @warn "Some but not all atoms have an associated residue, so we cannot rely on existing residues"
            end
        end
        attributions = Int[]
    else
        attributions = zeros(Int, n)
        for i_residue in 1:m
            for atom in atoms(Residue(topology, i_residue-1))
                attributions[atom+1] = i_residue
            end
        end
    end
    return attributions
end

@static if !isdefined(Chemfiles, :atoms) # up to 0.9.3 included
    function atoms(residue::Residue)
        count = size(residue)
        result = Array{UInt64}(undef, count)
        Chemfiles.__check(Chemfiles.lib.chfl_residue_atoms(Chemfiles.__const_ptr(residue), pointer(result), count))
        return result
    end
end

function check_collision(pos, mat)
    _n, n = size(pos)
    @assert _n == 3
    invmat = inv(mat)
    toremove = Int[]
    for i in 1:n, j in (i+1):n
        if periodic_distance(invmat*pos[:,i], invmat*pos[:,j], mat) < 0.55
            push!(toremove, i)
            push!(toremove, j)
        end
    end
    tokeep = collect(1:n)
    if !isempty(toremove)
        @ifwarn @warn "This file contains multiple colliding atoms. All colliding atoms will be removed."
        unique!(sort!(toremove))
    end
    return toremove
end

hasHneighbor(types, graph, i) = any(x -> types[x.v] === :H, neighbors(graph,i))
function check_valence(types, graph)
    n = length(types)

    ## Small atoms valence check
    invalidatoms = Set{Symbol}()
    for i in 1:n
        d = degree(graph, i)
        t = types[i]
        if t === :O && d > 2 && (d > 3 || !hasHneighbor(types, graph, i))
            push!(invalidatoms, :O)
        elseif t === :C && d > 4
            push!(invalidatoms, :C)
        elseif t === :N && d > 3 && (d > 4 || !hasHneighbor(types, graph, i))
            push!(invalidatoms, :N)
        end
    end
    if !isempty(invalidatoms)
        s = String.(collect(invalidatoms))
        @ifwarn @warn "Found $(join(s, ',', " and ")) with too many bonds."
        return true
    end
    return false
end

"""
    @enum BondingMode

Selection mode for the detection of bonds. The choices are:
-   `InputBonds`: use the input bonds. Fail if those are not specified.
-   `ChemfilesBonds`: use chemfiles built-in bond detection mechanism.
-   `AutoBonds`: if the input specifies bonds, use them unless they look suspicious (too or too
    large according to a heuristic). Otherwise, fall back to `ChemfilesBonds`.
"""
@enum BondingMode begin
    InputBonds
    ChemfilesBonds
    AutoBonds
end

function sanity_checks!(pos, types, graph, mat, bondingmode)
    ## Bond length check
    removeedges = PeriodicEdge3D[]
    for e in edges(graph)
        s, d = e.src, e.dst.v
        bondlength = norm(pos[:,d] .+ (mat * e.dst.ofs) .- pos[:,s])
        if bondlength > 3
            @ifwarn @warn "Suspiciously large bond found."
            return true
        elseif bondlength < 0.85 && types[s] !== :H && types[d] !== :H
            push!(removeedges, e)
        end
    end
    if bondingmode == AutoBonds
        @ifwarn begin
            if !isempty(removeedges)
                @warn "Suspiciously small bond found. Such bonds are probably spurious and will be deleted."
                @info "To force retaining these bonds, use --bond-detect=input or --bond-detect=chemfiles"
            end
        end
        for e in removeedges
            rem_edge!(graph, e)
        end
    end
    return false
end

"""
       parse_chemfiles(path)

Parse a file given in any reckognised chemical format and extract the topological
information.
Such format can be .cif or any file format reckognised by Chemfiles.jl that
contains all the necessary topological information.
"""
function parse_chemfile(_path, exportto=tempdir(), bondingmode::BondingMode=AutoBonds, assert_use_existing_residues=false ; ignore_atoms=[])
    # Separate the cases unhandled by Chemfiles from the others
    path = expanduser(_path)
    cell = Cell()
    frame::Frame = if lowercase(last(splitext(path))) == ".cif"
        framecif = Frame()
        cif = expand_symmetry(CIF(path))
        a, b, c, α, β, γ = cell_parameters(cif.cell)
        set_cell!(framecif, UnitCell(a, b, c, α, β, γ))
        n = length(cif.ids)
        ignored = 0
        for i in 1:n
            typ = cif.types[cif.ids[i]]
            if typ ∈ ignore_atoms
                ignored += 1
                continue
            end
            pos = cif.cell.mat * cif.pos[:,i]
            atom = parse_atom(string(typ))
            add_atom!(framecif, atom, Vector{Float64}(pos))
        end
        n -= ignored
        if iszero(cif.bonds)
            try_guess_bonds!(framecif)
        else
            for i in 1:n, j in (i+1):n
                if cif.bonds[i,j]
                    add_bond!(framecif, i-1, j-1)
                end
            end
        end
        attributions = Int[]

        framecif
    else # The rest is handled by Chemfiles

        frameelse = read(Trajectory(path))
        for i in Int(size(frameelse))-1:-1:0
            if Symbol(type(Chemfiles.Atom(frameelse, i))) ∈ ignore_atoms
                Chemfiles.remove_atom!(frameelse, i)
            end
        end

        toremove = reverse(check_collision(positions(frameelse), matrix(UnitCell(frame))))
        for j in toremove
            Chemfiles.remove_atom!(frameelse, j-1)
        end

        topology = Topology(frameelse)
        n = Int(size(topology))

        if bonds_count(topology) == 0
            try_guess_bonds!(frameelse)
            topology = Topology(frameelse) # safer but useless since the underlying pointer is the same
        end

        attributions = attribute_residues(topology, assert_use_existing_residues)

        frameelse
    end

    cell = Cell(Cell(), SMatrix{3,3,BigFloat}(matrix(UnitCell(frame)))')
    if !all(isfinite, cell.mat) || iszero(det(cell.mat))
        @ifwarn @warn "Suspicious unit cell of matrix $(Float64.(cell.mat)). Is the input really periodic? Using a cubic unit cell instead"
        cell = Cell()
    end

    types = [Symbol(type(Chemfiles.Atom(frame, i))) for i in 0:(n-1)]
    poss = copy(positions(frame))

    adjacency = zeros(Bool, n, n)
    for (a,b) in eachcol(bonds(Topology(frame)))
        adjacency[a+1,b+1] = true
        adjacency[b+1,a+1] = true
    end
    mat = Float64.(cell.mat)
    graph = PeriodicGraph3D(n, edges_from_bonds(adjacency, mat, poss))

    if bondingmode != InputBonds
        bad_valence = check_valence(types, graph)
        recompute_bonds = bad_valence || sanity_checks!(poss, types, graph, mat, bondingmode)
        if recompute_bonds
            @ifwarn begin
                @warn "Disregarding all bonds from the input file."
                @info "To force retaining the initial bonds, use --bond-detect=input"
            end
            try_guess_bonds!(frame)
            topology = Topology(frame)
            adjacency = zeros(Bool, n, n)
            for (a,b) in eachcol(bonds(topology))
                adjacency[a+1,b+1] = true
                adjacency[b+1,a+1] = true
            end
            graph = PeriodicGraph3D(n, edges_from_bonds(adjacency, cell.mat, poss))
            @ifwarn if check_valence(types, graph)
                @warn "Remaining atoms with invalid valence. Proceeding anyway."
            end
            recompute_bonds = sanity_checks!(poss, types, graph, cell.mat, bondingmode)
            @ifwarn if recompute_bonds
                 @warn "Remaining bonds of suspicious lengths. Proceeding anyway."
            end
            attributions = attribute_residues(topology, assert_use_existing_residues)
        end
    end

    if isempty(attributions)
        crystalnothing = Crystal{Nothing}(cell, types, nothing, poss, graph)
        ifexport(crystalnothing, splitext(splitdir(path)[2])[1], exportto)
        return crystalnothing
    else
        crystalclusters = Crystal{Clusters}(cell, types, regroup_sbus(graph, attributions), poss, graph)
        ifexport(crystalclusters, splitext(splitdir(path)[2])[1], exportto)
        return crystalclusters
    end
end


export datachunk
export split_into_n, split_tod_mpi, get_chunk_properties

import Healpix
import CorrNoise
using Random
using FITSIO

try
    import MPI
catch
end

"""
This structure holds a number of parameters relative to a certain chunk of data.

Field              | Type           | Meaning
:----------------- |:-------------- |:------------------------------------------------------------
`pol_number`       | Int            | ID number of the polarimeter
`first_idx`        | Int            | Index of the first element of the chunk
`last_idx`         | Int            | Index of the last element of the chunk
`num_of_elements`  | Int            | Total number of elements of the chunk

# Example
given the following array of data: [1, 10, 20, 3, 4, 5, 7]
measured by the polarimeter number 2

the chunk: [20, 3, 4]
will correspond to the following structure:
datachunk(2, 3, 5, 3)
"""
struct datachunk
    pol_number ::Int
    first_idx::Int
    last_idx::Int
    num_of_elements::Int
end

"""
    function split_into_n(length, num_of_segments)
        Given the `length` of the array, it convenientely
        splits it into `num_of_segments` sections of as similar length as possible.
        It returns an array containing the number of elements of each section.

        # Example
        julia> split_into_n(20, 3)
        3-element Array{Int64,1}:
        6
        7
        7
"""
function split_into_n(length, num_of_segments)
    @assert num_of_segments >0
    @assert length >= num_of_segments
    start_pos = zeros(Int, num_of_segments+1)

    for i in 1:num_of_segments+1
        start_pos[i] =  floor(((i-1)*length/num_of_segments))
    end

    return start_pos[2:end]-start_pos[1:end-1]
end

"""
    function split_tod_mpi(total_time, baseline_length_s, baselines_per_process, num_of_MPI_proc)

        This function can be used to split the TOD production of many polarimeters among MPI processes.

        It requires in input:

        -the total time (in seconds) of the simulated observation
        -the length (in seconds) of each 1/f noise baseline
        -the array containing the number of 1/f baselines to simulate for each process.
            It can be obtained by using the function `plit_into_n` in the following way:

            split_into_n(num_of_polarimeters*baselines_per_pol, num_of_MPI_proc)

            where baselines_per_pol = Int64(total_time/baseline_length_s) is the number of baselines of each polarimeter

        -the number of MPI processes used

        It returns an array of arrays of `datachunk` instances, of length == num_of_MPI_proc
        where each element tells the chunk of data that each process should simulate (see Example)

        # Example
        ```julia-repl
        julia> num_of_polarimeters = 4
        julia> num_of_MPI_proc = 3
        julia> total_time = 50
        julia> baseline_length_s = 10
        julia> baselines_per_pol = Int64(total_time/baseline_length_s)
        5

        julia> baselines_per_process = split_into_n(20, 3)
        3-element Array{Int64,1}:
        6
        7
        7

        julia> chunks = split_tod_mpi(total_time, baseline_length_s, baselines_per_process, num_of_MPI_proc)
        3-element Array{Any,1}:
        Any[datachunk(1, 1, 5, 5), datachunk(2, 1, 1, 1)]
        Any[datachunk(2, 2, 5, 4), datachunk(3, 1, 3, 3)]
        Any[datachunk(3, 4, 5, 2), datachunk(4, 1, 5, 5)]
        ```

        which means:

        - process number 0 should simulate:
        polarimeter number 1 from baseline 1 to baseline 5,  total number of baselines = 5
        polarimeter number 2 from baseline 1 to baseline 1 , total number of baselines = 1

        - process number 1 should simulate:
        polarimeter number 2 from baseline 2 to baseline 5,  total number of baselines = 4
        polarimeter number 3 from baseline 1 to baseline 3 , total number of baselines = 3

        - process number 2 should simulate:
        polarimeter number 3 from baseline 4 to baseline 5,  total number of baselines = 2
        polarimeter number 4 from baseline 1 to baseline 5,  total number of baselines = 5

"""
function split_tod_mpi(total_time, baseline_length_s, baselines_per_process, num_of_MPI_proc)

    duration = Int64(total_time/baseline_length_s)

    #initialization
    detector_num = 1
    sample_idx = 0
    samples_in_det = duration
    result = []

    for rank_num in 0:(num_of_MPI_proc-1)  #loop on MPI processes

        samples_for_this_process = baselines_per_process[rank_num+1]
        samples_left = samples_for_this_process
        data_this_rank = []

        while samples_left > 0  #loop on detectors

            #if the current detector has more samples than needed to fill the current MPI process
            if samples_in_det > samples_left

                first_idx = sample_idx+1
                last_idx = sample_idx+samples_left
                data = datachunk(detector_num, first_idx, last_idx, samples_left)
                data_this_rank = append!(data_this_rank, [data])

                sample_idx = sample_idx + samples_left
                samples_in_det = samples_in_det - samples_left
                samples_left = 0

            #if the current detector has not enough samples to provide the current MPI process
            #with the required number of samples. In this case we need to increase "detector_num" before the next iteration
            else

                first_idx = sample_idx+1
                last_idx = sample_idx+samples_in_det
                data = datachunk(detector_num, first_idx, last_idx, samples_in_det)
                data_this_rank = append!(data_this_rank, [data])

                samples_left = samples_left - samples_in_det
                samples_in_det = 0
            end

            if samples_in_det == 0
                detector_num +=1
                sample_idx = 0
                samples_in_det = duration
            end

        end

        result = append!(result, [data_this_rank])

    end

    return result

end

"""
    function get_chunk_properties(chunks, baseline_length_s, fsamp_hz, rank)

        Given:
        - the data chunks (which can be obtained by using the function `split_tod_mpi`)
        - the length (in seconds) of each 1/f noise baseline
        - the sampling frequency (in Hz)
        - the number of current MPI rank

        this function extracts useful information to perform the TOD simulation in the current rank.

        It returns a tuple containing 5 arrays:
        - the ID number of the polarimeters that the current rank will simulate
        - the start time of the acquisition portion for each polarimeter
        - the stop time of the acquisition portion for each polarimeter
        - the number of 1/f baselines for each polarimeter
        - the total number of samples for each polarimeter

        # Example
        ```julia-repl
        julia> baseline_length_s = 10
        julia> fsamp_hz = 10
        julia> rank = 1

        julia> chunks = [[datachunk(1, 1, 5, 5), datachunk(2, 1, 1, 1)], [datachunk(2, 2, 5, 4), datachunk(3, 1, 3, 3)], [datachunk(3, 4, 5, 2), datachunk(4, 1, 5, 5)]]

        get_chunk_properties(chunks, baseline_length_s, fsamp_hz, rank)
        ([2, 3], [10.0, 0.0], [50.0, 30.0], [4, 3], [400, 300])

        which means that rank 1 will simulate:
        - polarimeter number 2 from 10 s (from the starting of the acquisition) to 50 s,
          with a total of 4 1/f baselines and 400 samples.
        - polarimeter number 3 from 0 s (from the starting of the acquisition) to 30 s,
          with a total of 3 1/f baselines and 300 samples.

    """
    function get_chunk_properties(chunks, baseline_length_s, fsamp_hz, rank)

        this_rank_chunk = chunks[rank+1]
        first_time, last_time = [Array{Float64}(undef, length(this_rank_chunk)) for i in (1:2)]
        detector_number, num_of_baselines, baseline_len, num_of_samples = [Array{Int64}(undef, length(this_rank_chunk)) for i in (1:4)]

        for i in 1:length(this_rank_chunk)
            detector_number[i] = this_rank_chunk[i].pol_number
            first_time[i] = (this_rank_chunk[i].first_idx-1)*baseline_length_s
            last_time[i] = this_rank_chunk[i].last_idx*baseline_length_s -0.99*(1/fsamp_hz)
            num_of_baselines[i] =  this_rank_chunk[i].num_of_elements
            num_of_samples[i] = num_of_baselines[i]*baseline_length_s*fsamp_hz
        end
        return (detector_number, first_time, last_time, num_of_baselines, num_of_samples)
    end


function polar_kernel(n::Int, nlines::Int, line_start::Int, scale::Float64,
                     u_curr::CuDeviceArray{Float64,1}, v_curr::CuDeviceArray{Float64,1},
                     l_curr::CuDeviceArray{Float64,1}, rho::CuDeviceArray{Float64,1},
                     shift_lines::Int, param::CuDeviceArray{Float64,2},
                     _YffR::CuDeviceArray{Float64,1}, _YffI::CuDeviceArray{Float64,1},
                     _YftR::CuDeviceArray{Float64,1}, _YftI::CuDeviceArray{Float64,1},
                     _YttR::CuDeviceArray{Float64,1}, _YttI::CuDeviceArray{Float64,1},
                     _YtfR::CuDeviceArray{Float64,1}, _YtfI::CuDeviceArray{Float64,1},
                     frBound::CuDeviceArray{Float64,1}, toBound::CuDeviceArray{Float64,1})

    tx = threadIdx().x
    ty = threadIdx().y
    I = blockIdx().x
    id_line = I + shift_lines

    x = @cuDynamicSharedMem(Float64, n)
    xl = @cuDynamicSharedMem(Float64, n, n*sizeof(Float64))
    xu = @cuDynamicSharedMem(Float64, n, (2*n)*sizeof(Float64))

    @inbounds begin
        YffR = _YffR[id_line]; YffI = _YffI[id_line]
        YftR = _YftR[id_line]; YftI = _YftI[id_line]
        YttR = _YttR[id_line]; YttI = _YttI[id_line]
        YtfR = _YtfR[id_line]; YtfI = _YtfI[id_line]

        pij_idx = line_start + 8*(I-1)

        xl[1] = sqrt(frBound[2*(id_line-1)+1])
        xu[1] = sqrt(frBound[2*id_line])
        xl[2] = sqrt(toBound[2*(id_line-1)+1])
        xu[2] = sqrt(toBound[2*id_line])
        xl[3] = -2*pi
        xu[3] = 2*pi
        xl[4] = -2*pi
        xu[4] = 2*pi

        x[1] = min(xu[1], max(xl[1], sqrt(u_curr[pij_idx+4])))
        x[2] = min(xu[2], max(xl[2], sqrt(u_curr[pij_idx+5])))
        x[3] = min(xu[3], max(xl[3], u_curr[pij_idx+6]))
        x[4] = min(xu[4], max(xl[4], u_curr[pij_idx+7]))

        param[1,id_line] = l_curr[pij_idx]
        param[2,id_line] = l_curr[pij_idx+1]
        param[3,id_line] = l_curr[pij_idx+2]
        param[4,id_line] = l_curr[pij_idx+3]
        param[5,id_line] = l_curr[pij_idx+4]
        param[6,id_line] = l_curr[pij_idx+5]
        param[7,id_line] = l_curr[pij_idx+6]
        param[8,id_line] = l_curr[pij_idx+7]
        param[9,id_line] = rho[pij_idx]
        param[10,id_line] = rho[pij_idx+1]
        param[11,id_line] = rho[pij_idx+2]
        param[12,id_line] = rho[pij_idx+3]
        param[13,id_line] = rho[pij_idx+4]
        param[14,id_line] = rho[pij_idx+5]
        param[15,id_line] = rho[pij_idx+6]
        param[16,id_line] = rho[pij_idx+7]
        param[17,id_line] = v_curr[pij_idx]
        param[18,id_line] = v_curr[pij_idx+1]
        param[19,id_line] = v_curr[pij_idx+2]
        param[20,id_line] = v_curr[pij_idx+3]
        param[21,id_line] = v_curr[pij_idx+4]
        param[22,id_line] = v_curr[pij_idx+5]
        param[23,id_line] = v_curr[pij_idx+6]
        param[24,id_line] = v_curr[pij_idx+7]

        CUDA.sync_threads()

        status, minor_iter = tron_kernel(n, shift_lines, 500, 200, 1e-6, scale, true, x, xl, xu,
                                         param, YffR, YffI, YftR, YftI, YttR, YttI, YtfR, YtfI)

        vi_vj_cos = x[1]*x[2]*cos(x[3] - x[4])
        vi_vj_sin = x[1]*x[2]*sin(x[3] - x[4])

        u_curr[pij_idx] = YffR*x[1]^2 + YftR*vi_vj_cos + YftI*vi_vj_sin
        u_curr[pij_idx+1] = -YffI*x[1]^2 - YftI*vi_vj_cos + YftR*vi_vj_sin
        u_curr[pij_idx+2] = YttR*x[2]^2 + YtfR*vi_vj_cos - YtfI*vi_vj_sin
        u_curr[pij_idx+3] = -YttI*x[2]^2 - YtfI*vi_vj_cos - YtfR*vi_vj_sin
        u_curr[pij_idx+4] = x[1]^2
        u_curr[pij_idx+5] = x[2]^2
        u_curr[pij_idx+6] = x[3]
        u_curr[pij_idx+7] = x[4]
    end

    return
end

function polar_kernel_cpu(n::Int, nline::Int, line_start::Int, scale::Float64,
                          u_curr::AbstractVector{Float64}, v_curr::AbstractVector{Float64},
                          l_curr::AbstractVector{Float64}, rho::AbstractVector{Float64},
                          shift::Int,
                          param::Array{Float64},
                          YffR::Array{Float64}, YffI::Array{Float64},
                          YftR::Array{Float64}, YftI::Array{Float64},
                          YttR::Array{Float64}, YttI::Array{Float64},
                          YtfR::Array{Float64}, YtfI::Array{Float64},
                          frBound::Array{Float64}, toBound::Array{Float64})
    avg_minor_it = 0

    x = zeros(n)
    xl = zeros(n)
    xu = zeros(n)

    xl[3] = -2*pi
    xu[3] = 2*pi
    xl[4] = -2*pi
    xu[4] = 2*pi

    @inbounds for I=1:nline
        pij_idx = line_start + 8*(I-1)
        id_line = shift + I

        xl[1] = sqrt(frBound[2*(id_line-1)+1])
        xu[1] = sqrt(frBound[2*id_line])
        xl[2] = sqrt(toBound[2*(id_line-1)+1])
        xu[2] = sqrt(toBound[2*id_line])

        x[1] = min(xu[1], max(xl[1], sqrt(u_curr[pij_idx+4])))
        x[2] = min(xu[2], max(xl[2], sqrt(u_curr[pij_idx+5])))
        x[3] = min(xu[3], max(xl[3], u_curr[pij_idx+6]))
        x[4] = min(xu[4], max(xl[4], u_curr[pij_idx+7]))

        param[1,id_line] = l_curr[pij_idx]
        param[2,id_line] = l_curr[pij_idx+1]
        param[3,id_line] = l_curr[pij_idx+2]
        param[4,id_line] = l_curr[pij_idx+3]
        param[5,id_line] = l_curr[pij_idx+4]
        param[6,id_line] = l_curr[pij_idx+5]
        param[7,id_line] = l_curr[pij_idx+6]
        param[8,id_line] = l_curr[pij_idx+7]
        param[9,id_line] = rho[pij_idx]
        param[10,id_line] = rho[pij_idx+1]
        param[11,id_line] = rho[pij_idx+2]
        param[12,id_line] = rho[pij_idx+3]
        param[13,id_line] = rho[pij_idx+4]
        param[14,id_line] = rho[pij_idx+5]
        param[15,id_line] = rho[pij_idx+6]
        param[16,id_line] = rho[pij_idx+7]
        param[17,id_line] = v_curr[pij_idx]
        param[18,id_line] = v_curr[pij_idx+1]
        param[19,id_line] = v_curr[pij_idx+2]
        param[20,id_line] = v_curr[pij_idx+3]
        param[21,id_line] = v_curr[pij_idx+4]
        param[22,id_line] = v_curr[pij_idx+5]
        param[23,id_line] = v_curr[pij_idx+6]
        param[24,id_line] = v_curr[pij_idx+7]

        function eval_f_cb(x)
            f = eval_f_polar_kernel_cpu(id_line, scale, x, param, YffR, YffI, YftR, YftI, YttR, YttI, YtfR, YtfI)
            return f
        end

        function eval_g_cb(x, g)
            eval_grad_f_polar_kernel_cpu(id_line, scale, x, g, param, YffR, YffI, YftR, YftI, YttR, YttI, YtfR, YtfI)
            return
        end

        function eval_h_cb(x, mode, rows, cols, _scale, lambda, values)
            eval_h_polar_kernel_cpu(id_line, x, mode, scale, rows, cols, lambda, values,
                                    param, YffR, YffI, YftR, YftI, YttR, YttI, YtfR, YtfI)
            return
        end

        nele_hess = 10
        tron = ExaTron.createProblem(4, xl, xu, nele_hess, eval_f_cb, eval_g_cb, eval_h_cb;
                                     :tol => 1e-6, :matrix_type => :Dense, :max_minor => 200,
                                     :frtol => 1e-12)

        tron.x .= x
        status = ExaTron.solveProblem(tron)
        x .= tron.x
        avg_minor_it += tron.minor_iter

        cos_ij = cos(x[3] - x[4])
        sin_ij = sin(x[3] - x[4])
        vi_vj_cos = x[1]*x[2]*cos_ij
        vi_vj_sin = x[1]*x[2]*sin_ij

        u_curr[pij_idx] = YffR[id_line]*x[1]^2 + YftR[id_line]*vi_vj_cos + YftI[id_line]*vi_vj_sin
        u_curr[pij_idx+1] = -YffI[id_line]*x[1]^2 - YftI[id_line]*vi_vj_cos + YftR[id_line]*vi_vj_sin
        u_curr[pij_idx+2] = YttR[id_line]*x[2]^2 + YtfR[id_line]*vi_vj_cos - YtfI[id_line]*vi_vj_sin
        u_curr[pij_idx+3] = -YttI[id_line]*x[2]^2 - YtfI[id_line]*vi_vj_cos - YtfR[id_line]*vi_vj_sin
        u_curr[pij_idx+4] = x[1]^2
        u_curr[pij_idx+5] = x[2]^2
        u_curr[pij_idx+6] = x[3]
        u_curr[pij_idx+7] = x[4]
    end

    return 0, avg_minor_it / nline
end

function polar_kernel_two_level(
    n::Int, nline::Int, line_start::Int, bus_start::Int, scale::Float64,
    u::CuDeviceArray{Float64}, xbar::CuDeviceArray{Float64},
    z::CuDeviceArray{Float64}, l::CuDeviceArray{Float64}, rho::CuDeviceArray{Float64},
    shift_lines::Int, param::CuDeviceArray{Float64},
    _YffR::CuDeviceArray{Float64}, _YffI::CuDeviceArray{Float64},
    _YftR::CuDeviceArray{Float64}, _YftI::CuDeviceArray{Float64},
    _YttR::CuDeviceArray{Float64}, _YttI::CuDeviceArray{Float64},
    _YtfR::CuDeviceArray{Float64}, _YtfI::CuDeviceArray{Float64},
    frBound::CuDeviceArray{Float64}, toBound::CuDeviceArray{Float64},
    brBusIdx::CuDeviceArray{Int}
)
    I = blockIdx().x
    id_line = I + shift_lines

    x = @cuDynamicSharedMem(Float64, n)
    xl = @cuDynamicSharedMem(Float64, n, n*sizeof(Float64))
    xu = @cuDynamicSharedMem(Float64, n, (2*n)*sizeof(Float64))

    @inbounds begin
        YffR = _YffR[id_line]; YffI = _YffI[id_line]
        YftR = _YftR[id_line]; YftI = _YftI[id_line]
        YttR = _YttR[id_line]; YttI = _YttI[id_line]
        YtfR = _YtfR[id_line]; YtfI = _YtfI[id_line]

        pij_idx = line_start + 8*(I-1)
        xbar_pij_idx = line_start + 4*(I-1)

        xl[1] = sqrt(frBound[2*(id_line-1)+1])
        xu[1] = sqrt(frBound[2*id_line])
        xl[2] = sqrt(toBound[2*(id_line-1)+1])
        xu[2] = sqrt(toBound[2*id_line])
        xl[3] = -2*pi
        xu[3] = 2*pi
        xl[4] = -2*pi
        xu[4] = 2*pi

        x[1] = min(xu[1], max(xl[1], sqrt(u[pij_idx+4])))
        x[2] = min(xu[2], max(xl[2], sqrt(u[pij_idx+5])))
        x[3] = min(xu[3], max(xl[3], u[pij_idx+6]))
        x[4] = min(xu[4], max(xl[4], u[pij_idx+7]))

        fr_idx = bus_start + 2*(brBusIdx[2*(id_line-1)+1] - 1)
        to_idx = bus_start + 2*(brBusIdx[2*id_line] - 1)

        param[1,id_line] = l[pij_idx]
        param[2,id_line] = l[pij_idx+1]
        param[3,id_line] = l[pij_idx+2]
        param[4,id_line] = l[pij_idx+3]
        param[5,id_line] = l[pij_idx+4]
        param[6,id_line] = l[pij_idx+5]
        param[7,id_line] = l[pij_idx+6]
        param[8,id_line] = l[pij_idx+7]
        param[9,id_line] = rho[pij_idx]
        param[10,id_line] = rho[pij_idx+1]
        param[11,id_line] = rho[pij_idx+2]
        param[12,id_line] = rho[pij_idx+3]
        param[13,id_line] = rho[pij_idx+4]
        param[14,id_line] = rho[pij_idx+5]
        param[15,id_line] = rho[pij_idx+6]
        param[16,id_line] = rho[pij_idx+7]
        param[17,id_line] = xbar[xbar_pij_idx] - z[pij_idx]
        param[18,id_line] = xbar[xbar_pij_idx+1] - z[pij_idx+1]
        param[19,id_line] = xbar[xbar_pij_idx+2] - z[pij_idx+2]
        param[20,id_line] = xbar[xbar_pij_idx+3] - z[pij_idx+3]
        param[21,id_line] = xbar[fr_idx] - z[pij_idx+4]
        param[22,id_line] = xbar[to_idx] - z[pij_idx+5]
        param[23,id_line] = xbar[fr_idx+1] - z[pij_idx+6]
        param[24,id_line] = xbar[to_idx+1] - z[pij_idx+7]

        CUDA.sync_threads()

        status, minor_iter = tron_kernel(n, shift_lines, 500, 200, 1e-6, scale, true, x, xl, xu,
                                         param, YffR, YffI, YftR, YftI, YttR, YttI, YtfR, YtfI)

        vi_vj_cos = x[1]*x[2]*cos(x[3] - x[4])
        vi_vj_sin = x[1]*x[2]*sin(x[3] - x[4])

        u[pij_idx] = YffR*x[1]^2 + YftR*vi_vj_cos + YftI*vi_vj_sin
        u[pij_idx+1] = -YffI*x[1]^2 - YftI*vi_vj_cos + YftR*vi_vj_sin
        u[pij_idx+2] = YttR*x[2]^2 + YtfR*vi_vj_cos - YtfI*vi_vj_sin
        u[pij_idx+3] = -YttI*x[2]^2 - YtfI*vi_vj_cos - YtfR*vi_vj_sin
        u[pij_idx+4] = x[1]^2
        u[pij_idx+5] = x[2]^2
        u[pij_idx+6] = x[3]
        u[pij_idx+7] = x[4]
    end

    return
end

function polar_kernel_two_level_cpu(
    n::Int, nline::Int, line_start::Int, bus_start::Int, scale::Float64,
    u::AbstractVector{Float64}, xbar::AbstractVector{Float64},
    z::AbstractVector{Float64}, l::AbstractVector{Float64}, rho::AbstractVector{Float64},
    shift::Int,
    param::Array{Float64},
    YffR::Array{Float64}, YffI::Array{Float64},
    YftR::Array{Float64}, YftI::Array{Float64},
    YttR::Array{Float64}, YttI::Array{Float64},
    YtfR::Array{Float64}, YtfI::Array{Float64},
    frBound::Array{Float64}, toBound::Array{Float64},
    brBusIdx::Array{Int}
)
    avg_minor_it = 0

    x = zeros(n)
    xl = zeros(n)
    xu = zeros(n)

    xl[3] = -2*pi
    xu[3] = 2*pi
    xl[4] = -2*pi
    xu[4] = 2*pi

    @inbounds for I=1:nline
        pij_idx = line_start + 8*(I-1)
        xbar_pij_idx = line_start + 4*(I-1)
        id_line = shift + I

        xl[1] = sqrt(frBound[2*(id_line-1)+1])
        xu[1] = sqrt(frBound[2*id_line])
        xl[2] = sqrt(toBound[2*(id_line-1)+1])
        xu[2] = sqrt(toBound[2*id_line])

        x[1] = min(xu[1], max(xl[1], sqrt(u[pij_idx+4])))
        x[2] = min(xu[2], max(xl[2], sqrt(u[pij_idx+5])))
        x[3] = min(xu[3], max(xl[3], u[pij_idx+6]))
        x[4] = min(xu[4], max(xl[4], u[pij_idx+7]))

        fr_idx = bus_start + 2*(brBusIdx[2*(id_line-1)+1] - 1)
        to_idx = bus_start + 2*(brBusIdx[2*id_line] - 1)

        param[1,id_line] = l[pij_idx]
        param[2,id_line] = l[pij_idx+1]
        param[3,id_line] = l[pij_idx+2]
        param[4,id_line] = l[pij_idx+3]
        param[5,id_line] = l[pij_idx+4]
        param[6,id_line] = l[pij_idx+5]
        param[7,id_line] = l[pij_idx+6]
        param[8,id_line] = l[pij_idx+7]
        param[9,id_line] = rho[pij_idx]
        param[10,id_line] = rho[pij_idx+1]
        param[11,id_line] = rho[pij_idx+2]
        param[12,id_line] = rho[pij_idx+3]
        param[13,id_line] = rho[pij_idx+4]
        param[14,id_line] = rho[pij_idx+5]
        param[15,id_line] = rho[pij_idx+6]
        param[16,id_line] = rho[pij_idx+7]
        param[17,id_line] = xbar[xbar_pij_idx] - z[pij_idx]
        param[18,id_line] = xbar[xbar_pij_idx+1] - z[pij_idx+1]
        param[19,id_line] = xbar[xbar_pij_idx+2] - z[pij_idx+2]
        param[20,id_line] = xbar[xbar_pij_idx+3] - z[pij_idx+3]
        param[21,id_line] = xbar[fr_idx] - z[pij_idx+4]
        param[22,id_line] = xbar[to_idx] - z[pij_idx+5]
        param[23,id_line] = xbar[fr_idx+1] - z[pij_idx+6]
        param[24,id_line] = xbar[to_idx+1] - z[pij_idx+7]

        function eval_f_cb(x)
            f = eval_f_polar_kernel_cpu(id_line, scale, x, param, YffR, YffI, YftR, YftI, YttR, YttI, YtfR, YtfI)
            return f
        end

        function eval_g_cb(x, g)
            eval_grad_f_polar_kernel_cpu(id_line, scale, x, g, param, YffR, YffI, YftR, YftI, YttR, YttI, YtfR, YtfI)
            return
        end

        function eval_h_cb(x, mode, rows, cols, _scale, lambda, values)
            eval_h_polar_kernel_cpu(id_line, x, mode, scale, rows, cols, lambda, values,
                                    param, YffR, YffI, YftR, YftI, YttR, YttI, YtfR, YtfI)
            return
        end

        nele_hess = 10
        tron = ExaTron.createProblem(4, xl, xu, nele_hess, eval_f_cb, eval_g_cb, eval_h_cb;
                                     :tol => 1e-6, :matrix_type => :Dense, :max_minor => 200,
                                     :frtol => 1e-12)

        tron.x .= x
        status = ExaTron.solveProblem(tron)
        x .= tron.x
        avg_minor_it += tron.minor_iter

        vi_vj_cos = x[1]*x[2]*cos(x[3] - x[4])
        vi_vj_sin = x[1]*x[2]*sin(x[3] - x[4])

        u[pij_idx] = YffR[id_line]*x[1]^2 + YftR[id_line]*vi_vj_cos + YftI[id_line]*vi_vj_sin
        u[pij_idx+1] = -YffI[id_line]*x[1]^2 - YftI[id_line]*vi_vj_cos + YftR[id_line]*vi_vj_sin
        u[pij_idx+2] = YttR[id_line]*x[2]^2 + YtfR[id_line]*vi_vj_cos - YtfI[id_line]*vi_vj_sin
        u[pij_idx+3] = -YttI[id_line]*x[2]^2 - YtfI[id_line]*vi_vj_cos - YtfR[id_line]*vi_vj_sin
        u[pij_idx+4] = x[1]^2
        u[pij_idx+5] = x[2]^2
        u[pij_idx+6] = x[3]
        u[pij_idx+7] = x[4]
    end

    return 0, avg_minor_it / nline
end


module OBJFileIO

export Shapeobject, loadOBJFile, createCylinderVertices

############### Begin Type declaration for Shape Object ################
type Shapeobject
    #A new type declaration. This is a shape object that is composed of
    #An array of vertices and an array of faces
    vertices::Array{Float64, 2}
    faces::Array{Int64, 2}

    #Constructor that creates an object from an obj file
    function Shapeobject(filename::String)
        #Create instance of the object
        shapeOBJ = new()
        #Populate the object fields using the loadOBJFile method
        shapeOBJ.vertices, shapeOBJ.faces = loadOBJFile(filename)
        #Return object
        return shapeOBJ
    end #end constructor

    #Constructor that creates an object from an obj file
    function Shapeobject(cylDiamter::Float64, cylLength::Float64,
                         numVertices::Int64, filename::String)
        #Create instance of the object
        shapeOBJ = new()
        #Populate the object.faces field using the loadOBJFile method
        x, shapeOBJ.faces = loadOBJFile(filename)
        #Populate the object.vertices field using the createCylinderVertices method
        shapeOBJ.vertices = createCylinderVertices(cylDiamter, cylLength, numVertices)
        #Return object
        return shapeOBJ
    end #end constructor

end # end of type declaration

############### Begin Functions ###############

#FUNCTION
#SUMMARY:
#This function loads an obj file and returns an array of vertices and an array of faces
#
#INPUTS:
#    objFile::String -
#    Path to the obj file containing the object that you want
#
#OUTPUTS:
#    Tuple of arrays
#
#    objectVertices::Array{Float64,2} -
#    array containing the vertices of the object
#
#    objectFaces::Array{Float64,2} -
#    array containing the faces of the object
function loadOBJFile(objFile::String)
    #Initiate variables for the object vertices and faces
    objectVertices = zeros(Float64,1,3)
    objectFaces = zeros(Int64,1,3)

    #Initialise variables to temporarily store the data for each line.
    lineVertex = zeros(Float64,1,3)
    lineFace = zeros(Int64,1,3)

    #Initialise counter for the vertex and face line number
    vertexLineCounter = 1
    faceLineCounter = 1

    #Open OBJ file for reading
    objFileContent = open(objFile)

    #Loop through each line of the file and look for vertex lines and face lines
    #Those lines begin with a 'v' or and 'f' respectively.
    #Foor those lines store the values in the repsective objectvertices or object faces
    #array.
    for line in eachline(objFileContent)
        if line[1] == 'v'
            vertexLine = split(line)
            #Initialise counter for the vertex line number
            for i = 1:length(vertexLine)
                if i != 1
                    if vertexLineCounter == 1
                        objectVertices[vertexLineCounter,i-1] = parsefloat(vertexLine[i])
                    else
                        lineVertex[i-1] = parsefloat(vertexLine[i])
                    end
                end
            end
            if vertexLineCounter != 1
                #Concatenate the vertex from the parsed line to the object vertices array
                objectVertices = vcat(objectVertices,lineVertex)
            end
            #Increment counter
            vertexLineCounter += 1

        elseif line[1] == 'f'
            faceLine = split(line)
            for i = 1:length(faceLine)
                if i != 1
                    if faceLineCounter == 1
                        objectFaces[faceLineCounter,i-1] = parseint(faceLine[i])
                    else
                        lineFace[i-1] = parsefloat(faceLine[i])
                    end
                end
            end
            if faceLineCounter != 1
                #Concatenate the face from the parsed line to the object vertices array
                objectFaces = vcat(objectFaces,lineFace)
            end
            #Increment counter
            faceLineCounter += 1
        end
    end

    #Close the file now we are done with it
    close(objFileContent)

    #return Object vertices and faces
    return objectVertices, objectFaces
end #end loadOBJFile function

#FUNCTION
#SUMMARY:
#Takes in parameters and returns a cylinder object from those parameters
#
#INPUTS:
#    cylDiameter::Float64 -
#    Diameter of cylinder
#
#    cylLength::Float64 -
#    Axial length of the cylinder
#
#    numVertices::Int64 -
#    number of vertices around each end of cylinder i.e. this is half of the total number
#    of vertices in the cylinder object
#
#OUTPUTS:
#    vertices::Array{Float64,2}
#    An 2*numVertices x 3 array containing the coordinates of each vertex of the cylinder
function createCylinderVertices(cylDiamter::Float64, cylLength::Float64, numVertices::Int64)
    #Calculate radius
    cylRadius = cylDiamter/2

    #Create xCoordinates
    cylBase = cylLength/2
    xCoordBase = -cylBase
    xCoordTop = cylBase

    #Calculate angular step around circle
    angleToVertex = -2π/numVertices

    #Preallocate vertices array
    vertices = zeros(Float64, 2 * numVertices, 3)

    #Loop through each vertex
    for vertex = 1:numVertices
        #Calculate points around the circle
        yCoord = cylRadius * cos((vertex - 1) * angleToVertex)
        ZCoord = cylRadius * sin((vertex - 1) * angleToVertex)

        #Add points to the vertices array for the Base
        vertices[2 * vertex - 1, 1] = xCoordBase
        vertices[2 * vertex - 1, 2] = yCoord
        vertices[2 * vertex - 1, 3] = ZCoord

        #Add points to the vertices array for the Top
        vertices[2 * vertex, 1] = xCoordTop
        vertices[2 * vertex, 2] = yCoord
        vertices[2 * vertex, 3] = ZCoord
    end

    #return vertices
    return vertices
end #end createCylinderVertices method

end #end module


"""implementation of Simultaneous Sampling-and-Search Planning (SSSP)"""
module LibSSSP
export SSSP

import Printf: @sprintf, @printf
import Base: @kwdef
import ...MRMP: AbsState, Node, now, elapsed_sec, gen_uniform_sampling, get_mid_status, dist
import DataStructures: PriorityQueue, enqueue!, dequeue!
import ..Solvers: gen_g_func, get_distance_tables, get_distance_table

@kwdef mutable struct SuperNode{State<:AbsState}
    Q::Vector{Node{State}}  # set of search nodes
    next::Int64  # next agent, 0 -> fixed agents
    id::String = get_Q_id(Q, next)
    parent_id::Union{Nothing,String} = nothing  # parent node
    g::Float64 = 0.0  # g-value
    h::Float64 = 0.0  # h-value
    f::Float64 = g + h  # f-value
    depth::Int64 = 1  # depth
end

"""
    SSSP(
        config_init::Vector{State},
        config_goal::Vector{State},
        connect::Function,
        collide::Function,
        check_goal::Function;
        g_func::Function = gen_g_func(greedy = true),
        steering_depth::Int64 = 2,
        num_vertex_expansion::Int64 = 10,
        init_min_dist_thread::Float64 = 0.1,
        decreasing_rate_min_dist_thread::Float64 = 0.99,
        epsilon::Union{Float64,Nothing} = nothing,
        TIME_LIMIT::Union{Nothing,Real} = 30,
        VERBOSE::Int64 = 0,

        use_random_h_func::Bool = false,         # for ablation study
        no_roadmap_at_beginning::Bool = false,   # for ablation study

        no_fast_collision_check::Bool = false,   # use 'slow' collision checker
    )::Tuple{
        Union{Nothing,Vector{Vector{Node{State}}}},  # solution
        Vector{Vector{Node{State}}},  # roadmap
    } where {State<:AbsState}

implementation of SSSP
"""
function SSSP(
    config_init::Vector{State},
    config_goal::Vector{State},
    connect::Function,
    collide::Function,
    check_goal::Function;
    g_func::Function = gen_g_func(greedy = true),
    steering_depth::Int64 = 2,
    num_vertex_expansion::Int64 = 10,
    init_min_dist_thread::Float64 = 0.1,
    decreasing_rate_min_dist_thread::Float64 = 0.99,
    epsilon::Union{Float64,Nothing} = nothing,
    TIME_LIMIT::Union{Nothing,Real} = 30,
    VERBOSE::Int64 = 0,

    # for ablation study
    use_random_h_func::Bool = false,
    no_roadmap_at_beginning::Bool = false,

    # just for fairness of experiments
    no_fast_collision_check::Bool = false,
)::Tuple{
    Union{Nothing,Vector{Vector{Node{State}}}},  # solution
    Vector{Vector{Node{State}}},  # roadmap
} where {State<:AbsState}
    """get initial solution / refinement"""

    t_s = now()
    elapsed() = elapsed_sec(t_s)
    timeover() = TIME_LIMIT != nothing && elapsed() > TIME_LIMIT

    # number of agents
    N = length(config_init)

    # define sampler
    sampler = gen_uniform_sampling(config_init[1])

    # used in steering
    conn(q_from::State, q_to::State, i::Int64) = begin
        (isnothing(epsilon) || dist(q_from, q_to) <= epsilon) && connect(q_from, q_to, i)
    end

    # get initial roadmap by RRT-connect
    roadmaps = (
        no_roadmap_at_beginning ?
        map(
            i -> begin
                v_init = Node{State}(config_init[i], 1, [])
                v_goal = Node{State}(config_goal[i], 2, [])
                conn(v_init.q, v_goal.q, i) && push!(v_init.neighbors, v_goal.id)
                conn(v_goal.q, v_init.q, i) && push!(v_goal.neighbors, v_init.id)
                [v_init, v_goal]
            end,
            1:N,
        ) :
        gen_RRT_connect_roadmaps(
            config_init,
            config_goal,
            connect;
            steering_depth = steering_depth,
            epsilon = epsilon,
            TIME_LIMIT = (isnothing(TIME_LIMIT) ? nothing : TIME_LIMIT - elapsed()),
        )
    )
    if isnothing(roadmaps)
        VERBOSE > 0 &&
            @info @sprintf("\t%6.4f sec: failed to construct initial roadmaps\n", elapsed())
        return (nothing, map(i -> Vector{Node{State}}(), 1:N))
    end

    VERBOSE > 0 && !no_roadmap_at_beginning && @info ("\tdone, setup initial roadmaps")

    # setup distance tables
    distance_tables = get_distance_tables(roadmaps)

    # verbose
    print_progress! =
        (S::SuperNode{State}, loop_cnt::Int64; force::Bool = false) -> begin
            (VERBOSE < 1 || (!force && (loop_cnt % 100 != 0))) && return
            @printf(
                "\r\t\t%6.4f sec, explored node: %08d, f: %.4f, depth: %04d",
                elapsed(),
                loop_cnt,
                S.f,
                S.depth
            )
        end

    # setup heuristic function
    h_func(Q::Vector{Node{State}}) =
        (use_random_h_func ? rand() : sum(map(i -> distance_tables[i][Q[i].id], 1:N)) / N)

    # initial configuration
    Q_init = [roadmaps[i][1] for i = 1:N]

    # initial search node
    S_init = SuperNode(Q = Q_init, next = 1, id = get_Q_id(Q_init, 0), h = h_func(Q_init))

    k = 0
    while !timeover()
        k += 1

        # open list
        OPEN = PriorityQueue{SuperNode{State},Float64}()

        # discovered list to avoid duplication
        VISITED = Dict{String,SuperNode{State}}()

        # threshold of space-filling metric
        min_dist_thread = init_min_dist_thread * (decreasing_rate_min_dist_thread^(k - 1))

        # setup initail node
        enqueue!(OPEN, S_init, S_init.f)
        VISITED[S_init.id] = S_init

        loop_cnt = 0
        while !isempty(OPEN) && !timeover()
            loop_cnt += 1

            # pop
            S = dequeue!(OPEN)

            # check goal
            if check_goal(S.Q)
                print_progress!(S, loop_cnt, force = true)
                VERBOSE > 0 && @info @sprintf("\n\t%6.4f sec: found solution\n", elapsed())
                return (backtrack(S, VISITED), roadmaps)
            end

            # initial search or update for refine agents
            i = S.next
            j = mod1(S.next + 1, N)

            v = S.Q[i]

            # explore new states
            expand!(
                (q_from::State, q_to::State) -> conn(q_from, q_to, i),
                sampler,
                v,
                roadmaps[i],
                min_dist_thread,
                num_vertex_expansion,
                steering_depth,
            ) && (distance_tables[i] = get_distance_table(roadmaps[i]))

            # expand search node
            for p_id in vcat(v.neighbors, v.id)

                # create new configuration
                p = roadmaps[i][p_id]
                Q = copy(S.Q)
                Q[i] = p

                # check duplication and collision
                Q_id = get_Q_id(Q, j)
                haskey(VISITED, Q_id) && continue
                !no_fast_collision_check && collide(S.Q, p.q, i) && continue
                no_fast_collision_check && collide(S.Q, Q) && continue

                # create new search node
                S_new = SuperNode(
                    Q = Q,
                    next = j,
                    id = Q_id,
                    parent_id = S.id,
                    h = h_func(Q),
                    g = S.g + g_func(S.Q, Q),
                    depth = S.depth + 1,
                )

                # insert
                enqueue!(OPEN, S_new, S_new.f)
                VISITED[S_new.id] = S_new
            end
            print_progress!(S, loop_cnt, force = isempty(OPEN))
        end

        VERBOSE > 1 && println()
    end

    VERBOSE > 0 && @info @sprintf("\t%6.4f sec: failed to find solution\n", elapsed())
    return (nothing, roadmaps)
end

"""implementation of vertex expansion"""
function expand!(
    connect::Function,
    sampler::Function,
    v_from::Node{State},
    roadmap::Vector{Node{State}},
    min_dist_thread::Float64,
    num_vertex_expansion::Int64,
    steering_depth::Int64,
)::Bool where {State<:AbsState}

    updated = false
    for _ = 1:num_vertex_expansion
        # steering
        q_new = steering(connect, sampler(), v_from.q, steering_depth)
        # check space-filling metric
        if minimum(v -> dist(v.q, q_new), roadmap) > min_dist_thread
            # add vertex and edges
            u = Node(
                q_new,
                length(roadmap) + 1,
                map(v -> v.id, filter(v -> connect(q_new, v.q), roadmap)),
            )
            push!(roadmap, u)
            # update neighbors
            foreach(
                v -> push!(v.neighbors, u.id),
                filter(v -> connect(v.q, q_new), roadmap),
            )
            updated = true
        end
    end
    return updated
end

function steering(
    connect::Function,
    q_rand::State,
    q_near::State,
    steering_depth::Int64,
)::State where {State<:AbsState}

    q_h = q_rand  # probably unsafe -> eventually safe
    if !connect(q_near, q_h)
        # binary search
        q_l = q_near  # safe
        for _ = 1:steering_depth
            q = get_mid_status(q_l, q_h)
            if connect(q_near, q)
                q_l = q
            else
                q_h = q
            end
        end
        q_h = q_l
    end
    return q_h
end

"""generate initial roadmaps via RRT-Connect"""
function gen_RRT_connect_roadmaps(
    config_init::Vector{State},
    config_goal::Vector{State},
    connect::Function;
    steering_depth::Int64 = 2,
    epsilon::Union{Float64,Nothing} = 0.2,
    TIME_LIMIT::Union{Nothing,Real} = 30,
)::Union{Nothing,Vector{Vector{Node{State}}}} where {State<:AbsState}

    # utilities for timeout
    t_s = now()
    elapsed() = elapsed_sec(t_s)
    timeover() = TIME_LIMIT != nothing && elapsed() > TIME_LIMIT

    N = length(config_init)
    roadmaps = Vector{Vector{Node{State}}}()
    for i = 1:N
        rmp = gen_RRT_connect_roadmap(
            i,
            config_init[i],
            config_goal[i],
            connect;
            steering_depth = steering_depth,
            epsilon = epsilon,
            TIME_LIMIT = (isnothing(TIME_LIMIT) ? nothing : TIME_LIMIT - elapsed()),
        )
        isnothing(rmp) && return nothing
        push!(roadmaps, rmp)
    end
    return roadmaps
end

function gen_RRT_connect_roadmap(
    i::Int64,
    q_init::State,
    q_goal::State,
    connect::Function;
    steering_depth::Int64 = 2,
    epsilon::Union{Float64,Nothing} = 0.2,
    TIME_LIMIT::Union{Nothing,Real} = 30,
)::Union{Nothing,Vector{Node{State}}} where {State<:AbsState}

    # utilities for timeout
    t_s = now()
    elapsed() = elapsed_sec(t_s)
    timeover() = TIME_LIMIT != nothing && elapsed() > TIME_LIMIT

    # define sampler
    sampler = gen_uniform_sampling(q_init)

    # re-define connect function
    conn(q_from::State, q_to::State)::Bool = begin
        # check distance
        !(isnothing(epsilon) || dist(q_from, q_to) <= epsilon) && return false

        !connect(q_from, q_to, i) && return false

        return true
    end

    conn(q::State)::Bool = connect(q, i)

    # special case
    if conn(q_init, q_goal)
        v_init = Node(q_init, 1, [2])
        v_goal = Node(q_goal, 2, Vector{Int64}())
        conn(q_goal, q_init) && push!(v_goal.neighbors, v_init.id)
        return [v_init, v_goal]
    end

    # store all samples
    V1 = [q_init]
    V2 = [q_goal]

    V = V1

    # main loop
    iter = 0
    while !timeover()
        iter += 1
        from_start = (iter % 2 == 1)

        (flg, q_new) = extend!(conn, sampler(), V1, from_start, steering_depth)
        if flg != :trapped &&
           extend!(conn, q_new, V2, !from_start, steering_depth)[1] == :reached
            # fix tree
            if from_start
                V1, V2 = V2, V1
            end
            # create roadmap, insert vertices
            roadmap = [Node{State}(q_init, 1, []), Node{State}(q_goal, 2, [])]
            foreach(q -> push!(roadmap, Node{State}(q, length(roadmap) + 1, [])), V1[2:end])
            foreach(q -> push!(roadmap, Node{State}(q, length(roadmap) + 1, [])), V2[2:end])
            # create roadmap, insert edges
            for (k, v_from) in enumerate(roadmap)
                for (l, v_to) in enumerate(roadmap)
                    if k != l && conn(v_from.q, v_to.q)
                        push!(v_from.neighbors, v_to.id)
                    end
                end
                timeover() && return nothing
            end
            return roadmap
        end

        # swap tree
        V1, V2 = V2, V1
    end

    return nothing
end

"""used in RRT-Connect"""
function extend!(
    connect::Function,
    q_rand::State,  # sampled point
    V::Vector{State},  # vertices
    from_start::Bool,  # true -> tree rooted C_init, false -> tree rooted C_goal
    steering_depth::Int64,
)::Tuple{Symbol,State} where {State<:AbsState}

    # find nearest existing vertex
    q_near =
        (from_start ? argmin(q -> dist(q, q_rand), V) : argmin(q -> dist(q_rand, q), V))
    flg = :reached

    q_l = q_near  # safe sample
    q_h = q_rand  # probably unsafe sample -> eventually safe

    # steering, binary search
    if from_start ? !connect(q_near, q_h) : !(connect(q_h) && connect(q_h, q_near))
        flg = :trapped
        for _ = 1:steering_depth
            q = from_start ? get_mid_status(q_l, q_h) : get_mid_status(q_h, q_l)
            if from_start ? connect(q_near, q) : (connect(q) && connect(q, q_near))
                flg = :advanced
                q_l = q
            else
                q_h = q
            end
        end
        q_h = q_l
    end

    flg != :trapped && push!(V, q_h)
    return (flg, q_h)
end

"""generate id of search nodes"""
function get_Q_id(Q::Vector{Node{State}}, next::Int64)::String where {State<:AbsState}
    return @sprintf("%s_%d", join([v.id for v in Q], "-"), next)
end

"""obtain solution from search nodes by backtracking"""
function backtrack(
    S_fin::SuperNode{State},
    VISITED::Dict{String,SuperNode{State}},
)::Vector{Vector{Node{State}}} where {State<:AbsState}

    S = S_fin
    solution = Vector{Vector{Node{State}}}()
    while S.parent_id != nothing
        pushfirst!(solution, S.Q)
        S = VISITED[S.parent_id]
    end
    pushfirst!(solution, S.Q)
    return solution
end

end