# AffineDecisionFunction #
# ========================== #
MA.mutability(::Type{<:TypedDecisionLike}) = MA.IsMutable()

function MA.mutable_copy(f::AffineDecisionFunction)
    return AffineDecisionFunction(MA.mutable_copy(f.variable_part),
                                  MA.mutable_copy(f.decision_part),
                                  MA.mutable_copy(f.known_part))
end
function MA.mutable_copy(f::LinearPart)
    return LinearPart(
        MA.mutable_copy(f.variable_part),
        MA.mutable_copy(f.decision_part))
end
function MA.mutable_copy(f::QuadraticPart)
    return QuadraticPart(
        MA.mutable_copy(f.variable_part),
        MA.mutable_copy(f.decision_part),
        MA.mutable_copy(f.cross_terms))
end
function MA.mutable_copy(f::QuadraticDecisionFunction)
    return QuadraticDecisionFunction(
        MA.mutable_copy(f.linear_quadratic_terms),
        MA.mutable_copy(f.known_part),
        MA.mutable_copy(f.known_variable_terms),
        MA.mutable_copy(f.known_decision_terms))
end

function MA.isequal_canonical(f::F, g::F) where F<:Union{AffineDecisionFunction, VectorAffineDecisionFunction}
    return MA.isequal_canonical(f.variable_part, g.variable_part) &&
        MA.isequal_canonical(f.decision_part, g.decision_part) &&
        MA.isequal_canonical(f.known_part, g.known_part)
end

function MA.isequal_canonical(f::F, g::F) where F <: LinearPart
    return MA.isequal_canonical(f.variable_part, g.variable_part) &&
        MA.isequal_canonical(f.decision_part, g.decision_part)
end
function MA.isequal_canonical(f::F, g::F) where F <: QuadraticPart
    return MA.isequal_canonical(f.variable_part, g.variable_part) &&
        MA.isequal_canonical(f.decision_part, g.decision_part) &&
        MA.isequal_canonical(f.cross_terms, g.cross_terms)
end
function MA.isequal_canonical(f::F, g::F) where F <: QuadraticDecisionFunction
    return MA.isequal_canonical(f.linear_quadratic_terms, g.linear_quadratic_terms) &&
        MA.isequal_canonical(f.known_part, g.known_part) &&
        MA.isequal_canonical(f.known_variable_terms, g.known_variable_terms) &&
        MA.isequal_canonical(f.known_decision_terms, g.known_decision_terms)
end

function MA.iszero!(f::TypedScalarDecisionLike)
    return iszero!(MOI.constant(f)) && MOIU._is_constant(MOIU.canonicalize!(f))
end

function MA.scaling(f::TypedScalarDecisionLike)
    return MA.scaling(f.variable_part)
end

MA.promote_operation(::Union{typeof(zero), typeof(one)}, F::Type{<:TypedScalarDecisionLike}) = F

const PROMOTE_IMPLEMENTED_OP = Union{typeof(+), typeof(-), typeof(*), typeof(/)}
function MA.promote_operation(op::PROMOTE_IMPLEMENTED_OP,
                              F::Type{<:ScalarDecisionLike{T}},
                              G::Type{<:ScalarDecisionLike{T}}) where T
    MOIU.promote_operation(op, T, F, G)
end
function MA.promote_operation(op::PROMOTE_IMPLEMENTED_OP, F::Type{T},
                              G::Type{<:TypedDecisionLike{T}}) where T
    MOIU.promote_operation(op, T, F, G)
end
function MA.promote_operation(op::PROMOTE_IMPLEMENTED_OP, F::Type{<:TypedDecisionLike{T}},
                              G::Type{T}) where T
    MOIU.promote_operation(op, T, F, G)
end
function MA.promote_operation(op::PROMOTE_IMPLEMENTED_OP, F::Type{<:Number},
                              G::Type{<:Union{SingleDecision, VectorOfDecisions}})
    MOIU.promote_operation(op, F, F, G)
end
function MA.promote_operation(op::PROMOTE_IMPLEMENTED_OP,
                              F::Type{<:Union{SingleDecision, VectorOfDecisions}},
                              G::Type{<:Number})
    MOIU.promote_operation(op, G, F, G)
end

function MA.mutable_operate!(op::Union{typeof(zero), typeof(one)}, f::AffineDecisionFunction)
    MA.mutable_operate!(op, f.variable_part)
    MA.mutable_operate!(zero, f.decision_part)
    MA.mutable_operate!(zero, f.known_part)
    return f
end
function MA.mutable_operate!(op::Union{typeof(zero), typeof(one)}, f::QuadraticDecisionFunction{T,LinearPart{T}}) where T
    lq = f.linear_quadratic_terms
    MA.mutable_operate!(op, lq.variable_part)
    MA.mutable_operate!(zero, lq.decision_part)
    MA.mutable_operate!(zero, f.known_part)
    MA.mutable_operate!(zero, f.known_variable_terms)
    MA.mutable_operate!(zero, f.known_decision_terms)
    return f
end
function MA.mutable_operate!(op::Union{typeof(zero), typeof(one)}, f::QuadraticDecisionFunction{T,QuadraticPart{T}}) where T
    lq = f.linear_quadratic_terms
    MA.mutable_operate!(op, lq.variable_part)
    MA.mutable_operate!(zero, lq.decision_part)
    MA.mutable_operate!(zero, lq.cross_terms)
    MA.mutable_operate!(zero, f.known_part)
    MA.mutable_operate!(zero, f.known_variable_terms)
    MA.mutable_operate!(zero, f.known_decision_terms)
    return f
end

function MA.mutable_operate!(::typeof(-), f::AffineDecisionFunction)
    MA.mutable_operate!(op, f.variable_part)
    MA.mutable_operate!(op, f.decision_part)
    MA.mutable_operate!(op, f.known_part)
    return f
end
function MA.mutable_operate!(::typeof(-), f::QuadraticDecisionFunction{T,LinearPart{T}}) where T
    lq = f.linear_quadratic_terms
    MA.mutable_operate!(op, lq.variable_part)
    MA.mutable_operate!(op, lq.decision_part)
    MA.mutable_operate!(op, f.known_part)
    MA.mutable_operate!(op, f.known_variable_terms)
    MA.mutable_operate!(op, f.known_decision_terms)
    return f
end
function MA.mutable_operate!(::typeof(-), f::QuadraticDecisionFunction{T,QuadraticPart{T}}) where T
    lq = f.linear_quadratic_terms
    MA.mutable_operate!(op, lq.variable_part)
    MA.mutable_operate!(op, lq.decision_part)
    MA.mutable_operate!(op, lq.cross_terms)
    MA.mutable_operate!(op, f.known_part)
    MA.mutable_operate!(op, f.known_variable_terms)
    MA.mutable_operate!(op, f.known_decision_terms)
    return f
end

function MA.mutable_operate!(op::Union{typeof(+), typeof(-)},
                             f::AffineDecisionFunction{T},
                             g::T) where T
    MA.mutable_operate!(op, f.variable_part, g)
    return f
end
function MA.mutable_operate!(op::Union{typeof(+), typeof(-)},
                             f::AffineDecisionFunction{T},
                             g::MOI.SingleVariable) where T
    _operate_terms!(op, f.variable_part, AffineDecisionFunction{T}(g).variable_part)
    return f
end
function MA.mutable_operate!(op::Union{typeof(+), typeof(-)},
                             f::AffineDecisionFunction{T},
                             g::SingleDecision) where T
    _operate_terms!(op, f.decision_part, AffineDecisionFunction{T}(g).decision_part)
    return f
end
function MA.mutable_operate!(op::Union{typeof(+), typeof(-)},
                             f::AffineDecisionFunction{T},
                             g::SingleKnown) where T
    _operate_terms!(op, f.known_part, AffineDecisionFunction{T}(g).known_part)
    return f
end
function MA.mutable_operate!(op::Union{typeof(+), typeof(-)},
                             f::AffineDecisionFunction{T},
                             g::MOI.ScalarAffineFunction{T}) where T
    _operate_terms!(op, f.variable_part, g)
    f.variable_part.constant = op(f.variable_part.constant, g.constant)
    return f
end
function MA.mutable_operate!(op::Union{typeof(+), typeof(-)},
                             f::AffineDecisionFunction{T},
                             g::AffineDecisionFunction{T}) where T
    # Variable part
    _operate_terms!(op, f.variable_part, g.variable_part)
    f.variable_part.constant = op(f.variable_part.constant, g.variable_part.constant)
    # Decision part
    _operate_terms!(op, f.decision_part, g.decision_part)
    f.decision_part.constant = op(f.decision_part.constant, g.decision_part.constant)
    # Known part
    _operate_terms!(op, f.known_part, g.known_part)
    f.known_part.constant = op(f.known_part.constant, g.known_part.constant)
    return f
end

function MA.mutable_operate!(op::Union{typeof(+), typeof(-)},
                             f::QuadraticDecisionFunction{T},
                             g::T) where T
    MA.mutable_operate!(op, f.linear_quadratic_terms.variable_part, g)
    return f
end
function MA.mutable_operate!(op::Union{typeof(+), typeof(-)},
                             f::QuadraticDecisionFunction{T},
                             g::MOI.SingleVariable) where T
    _operate_terms!(op, f.linear_quadratic_terms.variable_part, convert(MOI.ScalarAffineFunction{T}, g))
    return f
end
function MA.mutable_operate!(op::Union{typeof(+), typeof(-)},
                             f::QuadraticDecisionFunction{T},
                             g::SingleDecision) where T
    _operate_terms!(op, f.linear_quadratic_terms.decision_part, convert(AffineDecisionFunction{T}, g))
    return f
end
function MA.mutable_operate!(op::Union{typeof(+), typeof(-)},
                             f::QuadraticDecisionFunction{T},
                             g::SingleKnown) where T
    _operate_terms!(op, f.known_part, convert(AffineDecisionFunction{T}, g))
    return f
end
function MA.mutable_operate!(op::Union{typeof(+), typeof(-)},
                             f::QuadraticDecisionFunction{T},
                             g::MOI.ScalarAffineFunction{T}) where T
    lq = f.linear_quadratic_terms
    _operate_terms!(op, lq.variable_part, g)
    lq.variable_part.constant = op(lq.variable_part.constant, g.constant)
    return f
end
function MA.mutable_operate!(op::Union{typeof(+), typeof(-)},
                             f::QuadraticDecisionFunction{T},
                             g::MOI.ScalarQuadraticFunction{T}) where T
    _operate_terms!(op, f.variable_part, g)
    f.variable_part.constant = op(f.variable_part.constant, g.constant)
    return f
end
function MA.mutable_operate!(op::Union{typeof(+), typeof(-)},
                             f::QuadraticDecisionFunction{T},
                             g::AffineDecisionFunction{T}) where T
    lq = f.linear_quadratic_terms
    # Variable_Part
    _operate_terms!(op, lq.variable_part, g.variable_part)
    lq.variable_part.constant = op(lq.variable_part.constant, g.variable_part.constant)
    # Decision part
    _operate_terms!(op, lq.decision_part, g.decision_part)
    lq.decision_part.constant = op(lq.decision_part.constant, lq.decision_part.constant)
    # Known part
    _operate_terms!(op, f.known_part, g.known_part)
    f.known_part.constant = op(f.known_part.constant, g.known_part.constant)
    return f
end
function MA.mutable_operate!(op::Union{typeof(+), typeof(-)},
                             f::QuadraticDecisionFunction{T,LQ},
                             g::QuadraticDecisionFunction{T,LinearPart{T}}) where {T, LQ <: LinearQuadraticPart{T}}
    f_lq = f.linear_quadratic_terms
    g_lq = g.linear_quadratic_terms
    # Variable part
    _operate_terms!(op, f_lq.variable_part, g_lq.variable_part)
    f_lq.variable_part.constant = op(f_lq.variable_part.constant, g_lq.variable_part.constant)
    # Decision part
    _operate_terms!(op, f_lq.decision_part, g_lq.decision_part)
    # Known part
    _operate_terms!(op, f.known_part, g.known_part)
    f.known_part.constant = op(f.known_part.constant, g.known_part.constant)
    _operate_terms!(op, f.known_variable_terms, g.known_variable_terms)
    _operate_terms!(op, f.known_decision_terms, g.known_decision_terms)
    f.known_decision_terms.constant = op(f.known_decision_terms.constant, g.known_decision_terms.constant)
    return f
end
function MA.mutable_operate!(op::Union{typeof(+), typeof(-)},
                             f::QuadraticDecisionFunction{T,QuadraticPart{T}},
                             g::QuadraticDecisionFunction{T,QuadraticPart{T}}) where T
    f_lq = f.linear_quadratic_terms
    g_lq = g.linear_quadratic_terms
    # Variable part
    _operate_terms!(op, f_lq.variable_part, g_lq.variable_part)
    f_lq.variable_part.constant = op(f_lq.variable_part.constant, g_lq.variable_part.constant)
    # Decision part
    _operate_terms!(op, f_lq.decision_part, g_lq.decision_part)
    f_lq.decision_part.constant = op(f_lq.decision_part.constant, g_lq.decision_part.constant)
    # Cross terms
    _operate_terms!(op, f_lq.cross_terms, g_lq.cross_terms)
    # Known part
    _operate_terms!(op, f.known_part, g.known_part)
    f.known_part.constant = op(f.known_part.constant, g.known_part.constant)
    _operate_terms!(op, f.known_variable_terms, g.known_variable_terms)
    _operate_terms!(op, f.known_decision_terms, g.known_decision_terms)
    f.known_decision_terms.constant = op(f.known_decision_terms.constant, g.known_decision_terms.constant)
    return f
end

function _operate_terms!(::typeof(+),
                         f::MOI.ScalarAffineFunction,
                         g::MOI.ScalarAffineFunction)
    for term in g.terms
        add_term!(f.terms, term)
    end
    return nothing
end

function _operate_terms!(::typeof(-),
                         f::MOI.ScalarAffineFunction,
                         g::MOI.ScalarAffineFunction)
    for term in g.terms
        remove_term!(f.terms, term)
    end
    return nothing
end

function _operate_terms!(::typeof(+),
                         f::MOI.ScalarQuadraticFunction,
                         g::MOI.ScalarAffineFunction)
    for term in g.terms
        add_term!(f.affine_terms, term)
    end
    return nothing
end

function _operate_terms!(::typeof(-),
                         f::MOI.ScalarQuadraticFunction,
                         g::MOI.ScalarAffineFunction)
    for term in g.terms
        remove_term!(f.affine_terms, term)
    end
    return nothing
end

function _operate_terms!(::typeof(+),
                         f::MOI.ScalarQuadraticFunction,
                         g::MOI.ScalarQuadraticFunction)
    for term in g.affine_terms
        add_term!(f.affine_terms, term)
    end
    for term in g.quadratic_terms
        add_term!(f.quadratic_terms, term)
    end
    return nothing
end

function _operate_terms!(::typeof(-),
                         f::MOI.ScalarQuadraticFunction,
                         g::MOI.ScalarQuadraticFunction)
    for term in g.affine_terms
        remove_term!(f.affine_terms, term)
    end
    for term in g.quadratic_terms
        remove_term!(f.quadratic_terms, term)
    end
    return nothing
end


"""
    ComtradeData

`ComtradeData` holds data and configuration information from a set of COMTRADE
files. Fields include:

* `filename::String` -- Base file name (without extension).
* `cfg::ComtradeCfg` -- Configuration information.
* `dat::DataFrame` -- Table of results with each channel as a column.
* `hdr::String` -- Contents of the "hdr" file.
* `inf::String` -- Contents of the "inf" file.
"""

using CSV

mutable struct ComtradeData
    filename::String
    cfg::ComtradeCfg
    dat::DataFrame
    hdr::String
    inf::String
end

function addcol!(d::DataFrame, x)
    d[!,Symbol(string("x", ncol(d) + 1))]=x
end

"""
    read_comtrade(basename)

Read contents of a set of COMTRADE files with base file name `basename` (no
extension).

Returns a `COMTRADE.ComtradeData` with the following fields:

* `filename::String` -- Base file name (without extension).
* `cfg::ComtradeCfg` -- Configuration information.
* `dat::DataFrame` -- Table of results with each channel as a column.
* `hdr::String` -- Contents of the "hdr" file.
* `inf::String` -- Contents of the "inf" file.

See also `COMTRADE.ComtradeCfg` for a description of the `cfg` field.
"""
function read_comtrade(basename)
    cfg = read_cfg("$basename.cfg")
    if cfg.ft == "ASCII"
        d = CSV.read("$basename.dat", delim = ',', header = false)
        #for i in 1:cfg.nA+2   # convert DataArray columns to Arrays
        #    d[i] = d[i]
        #end
        for i in (1 + 2 + cfg.nA):(cfg.nD + 2 + cfg.nA)
            d[!,i] = map(Bool, d[!,i])
        end
    elseif cfg.ft == "BINARY"
        x = reinterpret(Int16, read(open("$basename.dat")))
        n = cfg.nA + div(cfg.nD, 16) + 2 + 2
        x = reshape(x[1:(n * cfg.npts)], (n, cfg.npts))
        global d = DataFrame()
        addcol!(d, reshape(reinterpret(UInt32, x[1:2, :]), (cfg.npts,)))
        #addcol!(d, reinterpret(UInt32, x[1:2, :]))

        addcol!(d, reshape(reinterpret(UInt32, x[3:4, :]), (cfg.npts,)))
        #addcol!(d, reinterpret(UInt32, x[3:4, :]))

        for i = 1:cfg.nA
            addcol!(d, reshape(x[i+4, :], (cfg.npts,)))
        end
        for i = 1:cfg.nD
            addcol!(d, Bool[(x >>> (mod1(i, 16) - 1)) & 0x01 for x in x[cfg.nA + 4 + cld(i, 16), :]])
            # addcol!(d, Bool[(x >>> (16 - mod1(i, 16))) & 0x01 for x in x[cfg.nA + 4 + cld(i, 16), :]])
        end
    else
        error("Unsupported data type $(x.ft)")
    end
    newnames = DataFrames.make_unique(vcat(:n, :time, [Symbol(x) for x in vcat(cfg.A[!,:ch_id], cfg.D[!,:ch_id])]),makeunique=true)
    rename!(d, newnames)
    # analog multipliers
    for i in 1:cfg.nA
        d[!,i+2] = cfg.A[i,:a] * d[!,i+2] .+ cfg.A[i,:b]
    end
    # analog multipliers
    d[!,2] = 1e-6 * cfg.timemult * d[!,2]
    hdr = isfile("$basename.hdr") ? read("$basename.hdr",String) : ""
    inf = isfile("$basename.inf") ? read("$basename.inf",String) : ""
    hdr= ""
    inf= ""
    return ComtradeData(basename, cfg, d, hdr, inf)
end

"""
    S"some name"

Returns a symbol. Equivalent to `symbol("some name")`. This helps when COMTRADE
data columns use nonstandard names.
"""
macro S_str(arg)
    Meta.quot(Symbol(arg))
end


"""
Used to plot macro geographic map as vector. From the free data source: https://www.diva-gis.org/data

kwarg "adminlevel" refers to the available shape files in the DIVA dataset:
    0... country outlines,
    1... regional (e.g. home nations in uk case),
    2... county
"""

function DIVA(;
    territory::String="GBR",
    exclShapeBelowLen::Int=200,
    adminLevel=0)

    # Paths
    env = loadPaths()
    subDir = "$(territory)_adm"
    shapeFile = "$(territory)_adm$(adminLevel).shp"
    path = joinpath(env.paths[:basemap_DIVA],subDir,shapeFile)

    # Load shapefile
    shapeSource = open(path, "r") do io
        read(io, Shapefile.Handle)
    end

    # Main loop
    traces = [adminLevelTraces(
        shapeSource.shapes[iShape],
        exclShapeBelowLen,
        territory)
    for iShape in 1:length(shapeSource.shapes)]

    # Merge trace vectors
    traces = vcat(traces...)

    # Layout scale ratio constrained based on midLat
    allLat = vcat([trace[:y] for trace in traces]...)
    midLat = (maximum(allLat)+minimum(allLat))/2
    latLonRatio = cos(midLat*π/180)
    layout = Layout(
        showlegend=false,
        xaxis=attr(zeroline=false,
            scaleanchor="y", scaleratio=latLonRatio),
        hovermode="closest"
    )

    # Plot (suppress in application with semi-colon)
    plt = plot(traces,layout);

    return plt
end

function adminLevelTraces(shapeObj,exclShapeBelowLen,territory)
    shape = GeoInterface.coordinates(shapeObj)

    # Remove shapes with boundaries shorter than exclShapeBelowLen::Int
    shape = [shp for shp in shape if length(shp[1]) > exclShapeBelowLen]

    # Set boundary name
    shapeNames = "$(territory) basemap"

    # Build traces of all polygons in shape (i.e. separate islands)
    coordsets = [hcat(shp[1]...) for shp in shape]
    traces = [scatter(
            x=coordset[1,:][:],
            y=coordset[2,:][:],
            name=shapeNames,mode="lines",line=attr(width=0.1,color="#444444BB"),
            fill="tonexty",fillcolor="#58B19F10",
            hoverinfo="skip",hovertemplate=nothing)
    for coordset in coordsets]

    return traces
end


using MultiAgentPOMDPs
using POMDPs
using Test

mutable struct X <: JointMDP{Vector{Float64},Vector{Bool}} end
abstract type Z <: JointMDP{Vector{Float64},Vector{Int}} end
mutable struct Y <: Z end

@testset "MultiAgentPOMDPs.jl" begin
    @testset "inference" begin
        @test_throws ErrorException statetype(Int)
        @test_throws ErrorException actiontype(Int)
        @test_throws ErrorException obstype(Int)

        @test statetype(X) == Vector{Float64}
        @test statetype(Y) == Vector{Float64}
        @test actiontype(X) == Vector{Bool}
        @test actiontype(Y) == Vector{Int}
    end
end


#=
This file is auto-generated. Do not edit.
=#
"""
    mutable struct LCLFilter <: Filter
        lf::Float64
        rf::Float64
        cf::Float64
        lg::Float64
        rg::Float64
        ext::Dict{String, Any}
        states::Vector{Symbol}
        n_states::Int
    end

Parameters of a LCL filter outside the converter, the states are in the grid's reference frame

# Arguments
- `lf::Float64`: Series inductance in p.u. of converter filter, validation range: `(0, nothing)`
- `rf::Float64`: Series resistance in p.u. of converter filter, validation range: `(0, nothing)`
- `cf::Float64`: Shunt capacitance in p.u. of converter filter, validation range: `(0, nothing)`
- `lg::Float64`: Series inductance in p.u. of converter filter to the grid, validation range: `(0, nothing)`
- `rg::Float64`: Series resistance in p.u. of converter filter to the grid, validation range: `(0, nothing)`
- `ext::Dict{String, Any}`
- `states::Vector{Symbol}`: The states of the LCLFilter model are:
	ir_cnv: Real current out of the converter,
	ii_cnv: Imaginary current out of the converter,
	vr_filter: Real voltage at the filter's capacitor,
	vi_filter: Imaginary voltage at the filter's capacitor,
	ir_filter: Real current out of the filter,
	ii_filter: Imaginary current out of the filter
- `n_states::Int`: LCLFilter has 6 states
"""
mutable struct LCLFilter <: Filter
    "Series inductance in p.u. of converter filter"
    lf::Float64
    "Series resistance in p.u. of converter filter"
    rf::Float64
    "Shunt capacitance in p.u. of converter filter"
    cf::Float64
    "Series inductance in p.u. of converter filter to the grid"
    lg::Float64
    "Series resistance in p.u. of converter filter to the grid"
    rg::Float64
    ext::Dict{String, Any}
    "The states of the LCLFilter model are:
	ir_cnv: Real current out of the converter,
	ii_cnv: Imaginary current out of the converter,
	vr_filter: Real voltage at the filter's capacitor,
	vi_filter: Imaginary voltage at the filter's capacitor,
	ir_filter: Real current out of the filter,
	ii_filter: Imaginary current out of the filter"
    states::Vector{Symbol}
    "LCLFilter has 6 states"
    n_states::Int
end

function LCLFilter(lf, rf, cf, lg, rg, ext=Dict{String, Any}(), )
    LCLFilter(lf, rf, cf, lg, rg, ext, [:ir_cnv, :ii_cnv, :vr_filter, :vi_filter, :ir_filter, :ii_filter], 6, )
end

function LCLFilter(; lf, rf, cf, lg, rg, ext=Dict{String, Any}(), states=[:ir_cnv, :ii_cnv, :vr_filter, :vi_filter, :ir_filter, :ii_filter], n_states=6, )
    LCLFilter(lf, rf, cf, lg, rg, ext, states, n_states, )
end

# Constructor for demo purposes; non-functional.
function LCLFilter(::Nothing)
    LCLFilter(;
        lf=0,
        rf=0,
        cf=0,
        lg=0,
        rg=0,
        ext=Dict{String, Any}(),
    )
end

"""Get [`LCLFilter`](@ref) `lf`."""
get_lf(value::LCLFilter) = value.lf
"""Get [`LCLFilter`](@ref) `rf`."""
get_rf(value::LCLFilter) = value.rf
"""Get [`LCLFilter`](@ref) `cf`."""
get_cf(value::LCLFilter) = value.cf
"""Get [`LCLFilter`](@ref) `lg`."""
get_lg(value::LCLFilter) = value.lg
"""Get [`LCLFilter`](@ref) `rg`."""
get_rg(value::LCLFilter) = value.rg
"""Get [`LCLFilter`](@ref) `ext`."""
get_ext(value::LCLFilter) = value.ext
"""Get [`LCLFilter`](@ref) `states`."""
get_states(value::LCLFilter) = value.states
"""Get [`LCLFilter`](@ref) `n_states`."""
get_n_states(value::LCLFilter) = value.n_states

"""Set [`LCLFilter`](@ref) `lf`."""
set_lf!(value::LCLFilter, val) = value.lf = val
"""Set [`LCLFilter`](@ref) `rf`."""
set_rf!(value::LCLFilter, val) = value.rf = val
"""Set [`LCLFilter`](@ref) `cf`."""
set_cf!(value::LCLFilter, val) = value.cf = val
"""Set [`LCLFilter`](@ref) `lg`."""
set_lg!(value::LCLFilter, val) = value.lg = val
"""Set [`LCLFilter`](@ref) `rg`."""
set_rg!(value::LCLFilter, val) = value.rg = val
"""Set [`LCLFilter`](@ref) `ext`."""
set_ext!(value::LCLFilter, val) = value.ext = val


module Piwall

using Compose

greet() = print("Hello World Again! And Again !")

export Wall, Screen

struct Screen
    xres::Integer
    # Todo : extend this to use Units and Measurements uncertainty
    xsize::Float32
    xbezel_left::Float32
    xbezel_right::Float32
    yres::Integer
    ysize::Float32
    ybezel_left::Float32
    ybezel_right::Float32
    manufacturer::String
    model::String

    # Ctors
    Screen(x::Integer, y::Integer) = new(x, 0.0, 0.0, 0.0, y, 0.0, 0.0, 0.0, "Manufacturer undefined", "Model undefined")
end

# Todo: extension for non-planar walls
struct Wall
    screens::Array{Screen}
end

function render(s::Screen)
    picture = compose(context(), rectangle(0,0,s.xres, s.yres), fill="tomato")
end

end # module


function check_for_new_closed(client, config, bot_path)
    new_closed = glob(joinpath(bot_path[2:end], "temp/new_closed*.json"))
    if length(new_closed) > 0
        yr, m = split(string(now()), "-")[1:2]
        current_closed_orders_path = joinpath(bot_path, "history/" * yr * "/" * m * ".json")
        !isdir(dirname(current_closed_orders_path)) ? mkpath(dirname(current_closed_orders_path)) : ""

        closed_orders = isfile(current_closed_orders_path) ? JSON.parsefile(current_closed_orders_path) : Dict()
        !haskey(closed_orders, string(today())) ? closed_orders[string(today())] = [] : ""
        #
        ## If testing is active
        test_bot = config["testing"] == "1" ? true : false
        if test_bot
            #
            ## Profit is estimated using the assumption that BNB is used for fee payments
            ### the associated BNB value is subtracted from the raw profit
            for new in new_closed
                tmp = JSON.parsefile(new)
                if tmp["type"] == "normal"
                    paid = tmp["buy_order"]["cummulativeQuoteQty"]
                    got = tmp["order"]["cummulativeQuoteQty"]
                elseif tmp["type"] == "initial"
                    price = typeof(config["init_state"]["price"]) != Float64 ? parse(Float64, config["init_state"]["price"]) : config["init_state"]["price"]
                    got = typeof(tmp["order"]["cummulativeQuoteQty"]) != Float64 ? parse(Float64, tmp["order"]["cummulativeQuoteQty"]) : tmp["order"]["cummulativeQuoteQty"]
                    paid = got * price/parse(Float64, tmp["order"]["price"])
                end
                #
                ## Fees paid using BNB assumed (0.075% per trade)
                fees = (paid + got) * 0.00075
                profit = got - paid - fees
                tmp["profit"] = profit
                # Calculate thes profit and profit percent
                if !haskey(tmp, "current_volume")
                    current_volume = typeof(config["total_value"]) == Float64 ? config["total_value"] : parse(Float64, config["total_value"])
                    if length(config["extra_volume"]) > 0
                        current_volume += sum(config["extra_volume"])
                    end
                    current_volume += config["thes_volume"]
                else
                    current_volume = parse(Float64, config["total_value"])
                end
                tmp["profit_percent"] = tmp["profit"] * 100 / current_volume
                tmp["profit_thes"] = tmp["profit"] * config["thes_factor"]

                push!(closed_orders[string(today())], tmp)

                open(current_closed_orders_path, "w") do f
                    JSON.print(f, closed_orders, 4)
                end
                open(new, "w") do f
                    JSON.print(f, tmp, 4)
                end

                if tmp["type"] == "normal"
                    mv(new, joinpath(bot_path, "closed_orders/" * basename(new)))
                elseif tmp["type"] == "initial"
                    mv(new, joinpath(bot_path, "closed_init_orders/" * basename(new)))
                end
            end
        else
            #
            ## Real trades
            Pinguu.anaylse_closed_orders(client, config, bot_path; limit=2)
        end
        return 0
    else
        return 1
    end
end
export check_for_new_closed
# checkFrNewClosed(config_paths, users) = @task check_for_new_closed(client, config, status, bot_path)


export iauAtic13
"""
Transform star RA,Dec from geocentric CIRS to ICRS astrometric.

This function is part of the International Astronomical Union's
SOFA (Standards of Fundamental Astronomy) software collection.

Status:  support function.

Given:
    ri,di  double  CIRS geocentric RA,Dec (radians)
    date1  double  TDB as a 2-part...
    date2  double  ...Julian Date (Note 1)

Returned:
    rc,dc  double  ICRS astrometric RA,Dec (radians)
    eo     double  equation of the origins (ERA-GST, Note 4)

Notes:

    1. The TDB date date1+date2 is a Julian Date, apportioned in any
    convenient way between the two arguments.  For example,
    JD(TDB)=2450123.7 could be expressed in any of these ways, among
    others:

        date1          date2

        2450123.7           0.0       (JD method)
        2451545.0       -1421.3       (J2000 method)
        2400000.5       50123.2       (MJD method)
        2450123.5           0.2       (date & time method)

    The JD method is the most natural and convenient to use in cases
    where the loss of several decimal digits of resolution is
    acceptable.  The J2000 method is best matched to the way the
    argument is handled internally and will deliver the optimum
    resolution.  The MJD method and the date & time methods are both
    good compromises between resolution and convenience.  For most
    applications of this function the choice will not be at all
    critical.

    TT can be used instead of TDB without any significant impact on
    accuracy.

    2. Iterative techniques are used for the aberration and light
    deflection corrections so that the functions iauAtic13 (or
    iauAticq) and iauAtci13 (or iauAtciq) are accurate inverses;
    even at the edge of the Sun's disk the discrepancy is only about
    1 nanoarcsecond.

    3. The available accuracy is better than 1 milliarcsecond, limited
    mainly by the precession-nutation model that is used, namely
    IAU 2000A/2006.  Very close to solar system bodies, additional
    errors of up to several milliarcseconds can occur because of
    unmodeled light deflection;  however, the Sun's contribution is
    taken into account, to first order.  The accuracy limitations of
    the SOFA function iauEpv00 (used to compute Earth position and
    velocity) can contribute aberration errors of up to
    5 microarcseconds.  Light deflection at the Sun's limb is
    uncertain at the 0.4 mas level.

    4. Should the transformation to (equinox based) J2000.0 mean place
    be required rather than (CIO based) ICRS coordinates, subtract the
    equation of the origins from the returned right ascension:
    RA = RI - EO.  (The iauAnp function can then be applied, as
    required, to keep the result in the conventional 0-2pi range.)

Called:
    iauApci13    astrometry parameters, ICRS-CIRS, 2013
    iauAticq     quick CIRS to ICRS astrometric

This revision:   2013 October 9

SOFA release 2018-01-30

Copyright (C) 2018 IAU SOFA Board.  See notes at end.
"""
# void iauAtic13(double ri, double di, double date1, double date2,
#                double *rc, double *dc, double *eo)

function iauAtic13(ri::Real, di::Real, date1::Real, date2::Real)
   # Allocate return values
   ref_rc = Ref{Float64}(0.0)
   ref_dc = Ref{Float64}(0.0)
   ref_eo  = Ref{Float64}(0.0)

   status = ccall((:iauAtic13, libsofa_c), Cint,
            (Cdouble, Cdouble, Cdouble, Cdouble,
            Ref{Cdouble}, Ref{Cdouble}, Ref{Cdouble}),
            convert(Float64, ri), convert(Float64, di),
            convert(Float64, date1), convert(Float64, date2),
            ref_rc, ref_dc, ref_eo)

    return ref_rc[], ref_dc[], ref_eo[]
end


struct MLDState{DM, DS}
    vehicle_states::Vector{SVector{3,Float64}} # last one is own state
    driver_models::Vector{DM}
    driver_states::Vector{DS}
    terminal::Nullable{Symbol}
end

MLDState(vss, dms, dss) = MLDState(vss, dms, dss, Nullable{Symbol}())

# const MLDPhysicalState = Vector{SVector{3, Float64}}
struct MLDPhysicalState
    vehicle_states::Vector{SVector{3, Float64}}
end

my_idx(s::Union{MLDState, MLDPhysicalState}) = length(s.vehicle_states)

struct MLDPOMDP{DM, DS} <: POMDP{MLDState{DM, DS}, AccelSpeed, MLDPhysicalState}
    reward::MLDRewardModel
    roadway::Highway
    vehicle_model::HighwayVehicle
    dt::Float64
    radius::Float64
    safety::MaxSafeAccel
    dm_dist::IDMGaussianCopula
    _normal_actions::Vector{AccelSpeed}
end

function MLDPOMDP(;reward=MLDRewardModel()
                  roadway::Highway=Highway(4, 4.0, 100.0),
                  vehicle_model::HighwayVehicle=HighwayVehicle(),
                  adjustment_accel::Float64=1.0,
                  dt::Float64=1.0,
                  radius::Float64=50.0,
                  safety=MaxSafeAccel(dt),
                  dm_dist::IDMGaussianCopula=IDMGaussianCopula(0.75)
                 )

    lr_speed = roadway.lanewidth/2.0 # half a lane per second
    accels = (-adjustment_accel, 0.0, adjustment_accel)
    na = vec(collect(AccelSpeed(a, s) for a in accels, s in (-lr_speed, 0.0, lr_speed)))
    DM = NoCrashMLDriver{IDM, Void, MaxSafeAccel}
    C = HighwayDrivingContext{typeof(vehicle_model), SVector{3,Float64}}
    DS = MLDriverState{C, nothing, C}
    return MLDPOMDP{DM, DS}(reward, roadway, vehicle_model, dt, safety, dm_dist, na)
end

isterminal(p::MLPOMDP, s::MLDState) = !isnull(s.terminal)

function generate_sr(p::MLDPOMDP, s::MLDState, a::AccelSpeed, rng::AbstractRNG)
    r = 0.0

    context = HighwayDrivingContext(p.vehicle_model, s.vehicle_states, p.roadway)

    # ego vehicle
    if a.long_accel < -p.reward.brake_thresh
        r -= p.reward.hard_brake_penalty
    end
    my_sp = propagate(p.vehicle_model, s.vehicle_states[end], a, context, p.dt)

    # others
    n_other = length(s.driver_models)
    vss = similar(s.vehicle_states, 0)
    dms = similar(s.driver_models, 0)
    old_is = Int[]
    own_xp = pos(my_sp)[1]
    for i in 1:n_other
        m = s.driver_models[i]
        ds = s.driver_states[i]
        da = sample_action(m, ds, i, AccelSpeed, rng)
        if da.long_accel < -p.reward.brake_thresh
            r -= p.reward.hard_brake_penalty
        end
        vs = s.vehicle_states[i]
        vsp = propagate(p.vehicle_model, vs, da, context, p.dt)
        if abs(pos(vsp)[1]-own_xp) > p.radius
            push!(vss, vsp)
            push!(dms, s.driver_models[i])
            push!(old_is, i)
        end
    end

    # insert ego vehicle
    push!(vss, my_sp)
    holdovers = HighwayDrivingContext(p.vehicle_model, vss, p.roadway)

    all_vss = vss[1:end-1] # all except ego
    newf = push_new_cars_front!(dms, all_vss, holdovers, p.dm_dist, own_xp + p.radius)
    newb = push_new_cars_back!(dms, all_vss, holdovers, p.dm_dist, own_xp - p.radius)

    push!(all_vss, my_sp)

    new_ctx = HighwayDrivingContext(p.vehicle_model, all_vss, p.roadway)::HighwayDrivingContext{typeof(p.vehicle_model), eltype(vss)}

    # see if terminal
    if pos(my_sp)[1] > p.roadway.len
        terminal = Nullable(:reached_end)
    elseif all(occupied_lanes(new_ctx, length(vss)) .== p.reward.target_lane)
        terminal = Nullable(:success)
        r += p.reward.success_reward
    else
        terminal = Nullable{Symbol}()
    end

    # update driver states
    dss = similar(s.driver_states, 0)
    for i in 1:length(vss)-1
        if i <= length(old_is)
            ds = observe(dms[i],
                         s.driver_states[old_is[i]],
                         new_ctx, p.dt)
            push!(dss, ds)
        else
            ds = initial_state(dms[i], new_ctx)
            push!(dss, ds)
        end
    end

    return MLDState(vss, s.driver_models, dss, terminal)::typeof(s), r
end

generate_o(p::MLPOMDP, sp::MLDState, rng::AbstractRNG) = MLDPhysicalState(s.vehicle_states)

function initial_state(p::MLPOMDP, rng::AbstractRNG)
    init = seed_state(p)
    burn_in_steps = 100
    my_model = NORMAL_IDM
    policy = ModelPolicy(p, my_model)
    hr = HistoryRecorder(max_steps=burn_in_steps)
    up = PrimedPreviousObservationUpdater(init)
    hist = simulate(hr, p, policy, up, init, init)
    return state_hist(hist)[end]
end

function seed_state(p::MLPOMDP{DM, DS})
    cars_per_100_m = 3
    xs = linspace(0.0, 2.0*p.radius, cars_per_100_m)
    car_inits = SVector{3, Float64}[]
    for l in 1:p.roadway.n_lanes
        y = (l-1)*p.roadway.lanewidth
        for x in xs
            if l > 1 || abs(x-p.radius) # stay away from ego at (radius,0)
                push!(car_inits, SVector(x, y, 20.0))
            end
        end
    end
    dms = DM[]
    dss = DS[]
    for 1:length(car_inits)
        dm = rand(rng, p.dm_dist)
        push!(dms, dm)
        push!(dss, initial_state(dm))
    end
    push!(car_inits, SVector(p.radius, 0.0, 20.0)) # ego
    return MLDState(vss, dms, dss)
end

# ACTIONS

function actions(p::MLDPOMDP, s::Union{MLDState, MLDPhysicalState})
    na = p._normal_actions
    c = HighwayDrivingContext(p.vehicle_model, s.vehicle_states, p.roadway)
    brake_acc = min(max_safe_acc(p.safety, c, my_idx(s)), -p.reward.brake_thresh/2.0)
    brake = AccelSpeed(brake_acc, 0.0)
    acceptable = [brake]
    for a in na
        # leaving road is always unsafe
        if !is_safe(RoadEdges(p.dt), c, my_idx(s), a)
            continue
        end

        # prevent running into the person in front or to the side
        if is_safe(p.safety, c, my_idx(s), a)
            push!(acceptable, a)
        end
    end
    return acceptable
end

struct ModelPolicy{M} <: Policy
    p::MLDPOMDP
    model::M
end

function action(policy::ModelPolicy, s::Union{MLDState, MLDPhysicalState})
    c = HighwayDrivingContext(policy.p.vehicle_model, s.vehicle_states, policy.p.roadway)
    accel = action(policy.model, c, my_idx(s), LongAccel)
    return AccelSpeed(accel, 0.0)
end

# REWARD
@with_kw struct MLDRewardModel
    brake_thresh::Float64    = 4.0 # always positive
    brake_penalty::Float64   = 1.0 # always positive
    target_lane::Int         =
    success_reward::Float64
end

# ASSUME IDM MODELS for access to sstar
function push_new_cars_front!(models, states, c::Context, dm_dist, front_x::Float64, rng::AbstractRNG)
    # the models MUST correspond to the same indices as the states in c
    for l in 1:c.roadway.n_lanes
        head = lane_head(c, l)
        if head !=0
            head_x = pos(state(c, head))[1]
            delta_x = front_x - head_x
            head_xdot = vel(state(c, head))
            delta_xdot = head_xdot - NORMAL_IDM.v0
        end
        if head == 0 || delta_x >= sstar(NORMAL_IDM, head_xdot, delta_xdot)
            push!(models, rand(rng, p.dm_dist))
            push!(states, SVector(front_x, (l-1)*c.roadway.lanewidth, NORMAL_IDM.v0))
        end
    end
end

function push_new_cars_back!(models, states, c::Context, dm_dist, back_x::Float64)
    for l in 1:c.roadway.n_lanes
        tail = lane_tail(c, l)
        if tail != 0
            tail_x = pos(state(c, tail))[1]
            delta_x = tail_x - back_x
            tail_xdot = vel(state(c, tail))
            delta_xdot = NORMAL_IDM.v0 - tail_xdot
        end
        if tail == 0 || delta_x >= sstar(NORMAL_IDM, NORMAL_IDM.v0, delta_xdot)
            push!(models, rand(rng, p.dm_dist))
            push!(states, SVector(front_x, (l-1)*c.roadway.lanewidth, NORMAL_IDM.v0))
        end
    end

    return models, states
end


module CbcMathProgSolverInterface

using Cbc.CoinMPInterface

importall MathProgBase.SolverInterface

export CbcMathProgModel,
    CbcSolver,
    model,
    loadproblem!,
    writeproblem,
    updatemodel!,
    setsense!,
    getsense,
    numvar,
    numconstr,
    setvartype!,
    optimize!,
    status,
    getobjval,
    getobjbound,
    getsolution,
    getrawsolver

type CbcMathProgModel <: AbstractMathProgModel
    inner::CoinProblem
    sense
end

immutable CbcSolver <: AbstractMathProgSolver
    options
end
CbcSolver(;kwargs...) = CbcSolver(kwargs)

function CbcMathProgModel(;options...)
    c = CoinProblem()
    setOption(c, "LogLevel", 0)
    for (optname, optval) in options
        setOption(c, string(optname), optval)
    end
    return CbcMathProgModel(c,:Min)
end

model(s::CbcSolver) = CbcMathProgModel(;s.options...)

function loadproblem!(m::CbcMathProgModel, A, collb, colub, obj, rowlb, rowub, sense)
    @assert sense == :Min || sense == :Max
    dir = 1
    if sense == :Max
        dir = -1
    end
    m.sense = sense
    LoadMatrix(m.inner, dir, 0.0, obj, collb, colub, rowlb, rowub, A)
end

function writeproblem(m::CbcMathProgModel, filename::String)
    if endswith(filename,".mps")
        WriteFile(m.inner, 3, filename)
    else
        error("Only MPS output supported")
    end
end

updatemodel(m::CbcMathProgModel) = nothing

function setsense(m::CbcMathProgModel,sense)
    if sense != m.sense
        error("CoinMP interface does not permit modifying the problem sense")
    end
end

getsense(m::CbcMathProgModel) = m.sense

numvar(m::CbcMathProgModel) = GetColCount(m.inner)
numconstr(m::CbcMathProgModel) = GetRowCount(m.inner)

function setvartype!(m::CbcMathProgModel,vartype::Vector{Symbol})
    ncol = numvar(m)
    @assert length(vartype) == ncol
    coltype = Array(Uint8,ncol)
    for i in 1:ncol
        if vartype[i] == :Int
            coltype[i] = 'I'
        elseif vartype[i] == :Cont
            coltype[i] = 'C'
        elseif vartype[i] == :Bin
            coltype[i] = 'B'
        else
            error("Unsupported variable type $(vartype[i]) present")
        end
    end
    LoadInteger(m.inner,coltype)
end

optimize!(m::CbcMathProgModel) = OptimizeProblem(m.inner)

function status(m::CbcMathProgModel)
    stat = GetSolutionText(m.inner)
    # CoinMP status reporting is faulty,
    # add logic from CbcModel.cpp.
    objval = GetObjectValue(m.inner)
    if stat == "Optimal solution found"
        if objval < 1e30
            return :Optimal
        else
            return :Infeasible
        end
    elseif stat == "Problem primal infeasible"
        return :Infeasible
    elseif stat == "Problem dual infeasible" # what does this mean for MIP??
        return :Unbounded
    elseif stat == "Stopped on iterations" || stat == "Stopped by user"
        return :UserLimit
    elseif stat == "Stopped due to errors"
        return :Error
    else
        error("Internal library error")
    end
end

getobjval(m::CbcMathProgModel) = GetObjectValue(m.inner)

getobjbound(m::CbcMathProgModel) = GetMipBestBound(m.inner)

getsolution(m::CbcMathProgModel) = GetSolutionValues(m.inner)

getrawsolver(m::CbcMathProgModel) = m.inner

end


function binary_op(op, W, @nospecialize(T))
  ty = LLVM_TYPES[T]
  if isone(W)
    V = T
  else
    ty = "<$W x $ty>"
    V = NTuple{W,VecElement{T}}
  end
  instrs = "%res = $op $ty %0, %1\nret $ty %res"
  call = :($LLVMCALL($instrs, $V, Tuple{$V,$V}, data(v1), data(v2)))
  W > 1 && (call = Expr(:call, :Vec, call))
  Expr(:block, Expr(:meta, :inline), call)
end

# Integer
for (op, f) ∈ [("add", :+), ("sub", :-), ("mul", :*), ("shl", :<<)]
  ff = Symbol('v', op)
  fnsw = Symbol(ff, "_nsw")
  fnuw = Symbol(ff, "_nuw")
  fnw = Symbol(ff, "_nw")
  ff_fast = Symbol(ff, :_fast)
  @eval begin
    # @inline $ff(a,b) = $ff_fast(a,b)
    @inline $ff(a::T, b::T) where {T<:Union{FloatingTypes,IntegerTypesHW,AbstractSIMD}} =
      $ff_fast(a, b)
    # @generated $ff_fast(v1::Vec{W,T}, v2::Vec{W,T}) where {W,T<:IntegerTypesHW} = binary_op($op * (T <: Signed ? " nsw" : " nuw"), W, T)
    @generated $ff_fast(v1::Vec{W,T}, v2::Vec{W,T}) where {W,T<:IntegerTypesHW} =
      binary_op($op, W, T)
    @generated $fnsw(v1::Vec{W,T}, v2::Vec{W,T}) where {W,T<:IntegerTypesHW} =
      binary_op($(op * " nsw"), W, T)
    @generated $fnuw(v1::Vec{W,T}, v2::Vec{W,T}) where {W,T<:IntegerTypesHW} =
      binary_op($(op * " nuw"), W, T)
    @generated $fnw(v1::Vec{W,T}, v2::Vec{W,T}) where {W,T<:IntegerTypesHW} =
      binary_op($(op * " nsw nuw"), W, T)
    # @generated Base.$f(v1::Vec{W,T}, v2::Vec{W,T}) where {W,T<:IntegerTypesHW} = binary_op($op, W, T)
    @inline Base.$f(v1::Vec{W,T}, v2::Vec{W,T}) where {W,T<:IntegerTypesHW} =
      $ff_fast(v1, v2)
    # @generated $ff(v1::Vec{W,T}, v2::Vec{W,T}) where {W,T<:IntegerTypesHW} = binary_op($op, W, T)
    @inline $ff_fast(x, y) = $f(x, y)

    # @generated $ff_fast(v1::T, v2::T) where {T<:IntegerTypesHW} = binary_op($op * (T <: Signed ? " nsw" : " nuw"), 1, T)
    # @generated $ff_fast(v1::T, v2::T) where {T<:IntegerTypesHW} = binary_op($op, 1, T)
    @generated $fnsw(v1::T, v2::T) where {T<:IntegerTypesHW} =
      binary_op($(op * " nsw"), 1, T)
    @generated $fnuw(v1::T, v2::T) where {T<:IntegerTypesHW} =
      binary_op($(op * " nuw"), 1, T)
    @generated $fnw(v1::T, v2::T) where {T<:IntegerTypesHW} =
      binary_op($(op * " nsw nuw"), 1, T)
  end
end
@inline vadd_fast(v1::T, v2::T) where {T<:IntegerTypesHW} = Base.add_int(v1, v2)
@inline vsub_fast(v1::T, v2::T) where {T<:IntegerTypesHW} = Base.sub_int(v1, v2)
@inline vmul_fast(v1::T, v2::T) where {T<:IntegerTypesHW} = Base.mul_int(v1, v2)
@inline vshl_fast(v1::T, v2::T) where {T<:IntegerTypesHW} = Base.shl_int(v1, v2)
for (op, f) ∈ [("div", :÷), ("rem", :%)]
  ff = Symbol('v', op) #_ff = Symbol(:_, ff)
  sbf = Symbol('s', op, :_int)
  ubf = Symbol('u', op, :_int)
  sbf128 = Sys.WORD_SIZE == 32 ? f : sbf
  ubf128 = Sys.WORD_SIZE == 32 ? f : ubf
  @eval begin
    @generated $ff(v1::Vec{W,T}, v2::Vec{W,T}) where {W,T<:Integer} =
      binary_op((T <: Signed ? 's' : 'u') * $op, W, T)
    @inline $ff(a::I, b::I) where {I<:SignedHW} = Base.$sbf(a, b)
    @inline $ff(a::I, b::I) where {I<:UnsignedHW} = Base.$ubf(a, b)
    @inline $ff(a::Int128, b::Int128) = Base.$sbf128(a, b)
    @inline $ff(a::UInt128, b::UInt128) = Base.$ubf128(a, b)
    # @generated $_ff(v1::T, v2::T) where {T<:Integer} = binary_op((T <: Signed ? 's' : 'u') * $op, 1, T)
    # @inline $ff(v1::T, v2::T) where {T<:IntegerTypesHW} = $_ff(v1, v2)
  end
end
# for (op,f) ∈ [("div",:÷),("rem",:%)]
#   @eval begin
#     @generated Base.$f(v1::Vec{W,T}, v2::Vec{W,T}) where {W,T<:Integer} = binary_op((T <: Signed ? 's' : 'u') * $op, W, T)
#     @generated Base.$f(v1::T, v2::T) where {T<:IntegerTypesHW} = binary_op((T <: Signed ? 's' : 'u') * $op, 1, T)
#   end
# end
@inline vcld(x, y) = vadd(vdiv(vsub(x, one(x)), y), one(x))
@inline function vdivrem(x, y)
  d = vdiv(x, y)
  r = vsub(x, vmul(d, y))
  d, r
end
for (op, sub) ∈ [
  ("ashr", :SignedHW),
  ("lshr", :UnsignedHW),
  ("lshr", :IntegerTypesHW),
  ("and", :IntegerTypesHW),
  ("or", :IntegerTypesHW),
  ("xor", :IntegerTypesHW),
]
  ff = sub === :UnsignedHW ? :vashr : Symbol('v', op)
  @eval begin
    @generated $ff(v1::Vec{W,T}, v2::Vec{W,T}) where {W,T<:$sub} = binary_op($op, W, T)
    @generated $ff(v1::T, v2::T) where {T<:$sub} = binary_op($op, 1, T)
  end
end

for (op, f) ∈ [("fadd", :vadd), ("fsub", :vsub), ("fmul", :vmul), ("fdiv", :vfdiv)]#,("frem",:vrem)]
  ff = Symbol(f, :_fast)
  fop_fast = f === :vfdiv ? "fdiv fast" : op * ' ' * fast_flags(true)
  fop_contract = op * ' ' * fast_flags(false)
  @eval begin
    @generated $f(v1::Vec{W,T}, v2::Vec{W,T}) where {W,T<:Union{Float32,Float64}} =
      binary_op($fop_contract, W, T)
    @generated $ff(v1::Vec{W,T}, v2::Vec{W,T}) where {W,T<:Union{Float32,Float64}} =
      binary_op($fop_fast, W, T)
    @inline $f(v1::Vec{W,Float16}, v2::Vec{W,Float16}) where {W} =
      $f(convert(Vec{W,Float32}, v1), convert(Vec{W,Float32}, v2))
    @inline $ff(v1::Vec{W,Float16}, v2::Vec{W,Float16}) where {W} =
      $ff(convert(Vec{W,Float32}, v1), convert(Vec{W,Float32}, v2))
  end
end
@inline vsub(a::T, b::T) where {T<:Union{Float32,Float64}} = Base.sub_float(a, b)
@inline vadd(a::T, b::T) where {T<:Union{Float32,Float64}} = Base.add_float(a, b)
@inline vmul(a::T, b::T) where {T<:Union{Float32,Float64}} = Base.mul_float(a, b)
@inline vsub_fast(a::T, b::T) where {T<:Union{Float32,Float64}} = Base.sub_float_fast(a, b)
@inline vadd_fast(a::T, b::T) where {T<:Union{Float32,Float64}} = Base.add_float_fast(a, b)
@inline vmul_fast(a::T, b::T) where {T<:Union{Float32,Float64}} = Base.mul_float_fast(a, b)

@inline vdiv(v1::AbstractSIMD{W,T}, v2::AbstractSIMD{W,T}) where {W,T<:FloatingTypes} =
  trunc(vfdiv_fast(v1, v2))
@inline vdiv_fast(v1::AbstractSIMD{W,T}, v2::AbstractSIMD{W,T}) where {W,T<:FloatingTypes} =
  trunc(vfdiv_fast(v1, v2))
@inline vdiv_fast(v1::T, v2::T) where {T<:FloatingTypes} =
  trunc(Base.FastMath.div_float_fast(v1, v2))
@inline vdiv_fast(v1::T, v2::T) where {T<:Number} = v1 ÷ v2
@inline vdiv(v1::T, v2::T) where {T<:Number} = v1 ÷ v2
@inline vdiv(v1::T, v2::T) where {T<:FloatingTypes} = vdiv_fast(v1, v2)
@inline vrem(a, b) = vfnmadd(vdiv_fast(a, b), b, a)
@inline vrem_fast(a, b) = vfnmadd(vdiv_fast(a, b), b, a)
# @inline vdiv_fast(v1::AbstractSIMD{W,T}, v2::AbstractSIMD{W,T}) where {W,T<:IntegerTypesHW} = trunc(T, vfloat_fast(v1) / vfloat_fast(v2))
@inline vdiv_fast(
  v1::AbstractSIMD{W,T},
  v2::AbstractSIMD{W,T},
) where {W,T<:IntegerTypesHW} = trunc(T, vfloat(v1) / vfloat(v2))
@inline function vdiv_fast(v1, v2)
  v3, v4 = promote_div(v1, v2)
  vdiv_fast(v3, v4)
end
@inline vdiv_fast(v1::VecUnroll{N,1,T,T}, s::T) where {N,T<:SignedHW} =
  VecUnroll(fmap(Base.sdiv_int, data(v1), s))
@inline vdiv_fast(v1::VecUnroll{N,1,T,T}, s::T) where {N,T<:UnsignedHW} =
  VecUnroll(fmap(Base.udiv_int, data(v1), s))
@inline vdiv_fast(v1::VecUnroll{N,1,T,T}, v2::VecUnroll{N,1,T,T}) where {N,T<:SignedHW} =
  VecUnroll(fmap(Base.sdiv_int, data(v1), data(v2)))
@inline vdiv_fast(v1::VecUnroll{N,1,T,T}, v2::VecUnroll{N,1,T,T}) where {N,T<:UnsignedHW} =
  VecUnroll(fmap(Base.udiv_int, data(v1), data(v2)))

@inline vfdiv(a::AbstractSIMDVector{W}, b::AbstractSIMDVector{W}) where {W} =
  vfdiv(vfloat(a), vfloat(b))
# @inline vfdiv_fast(a::AbstractSIMDVector{W}, b::AbstractSIMDVector{W}) where {W} = vfdiv_fast(vfloat_fast(a), vfloat_fast(b))
@inline vfdiv_fast(a::AbstractSIMDVector{W}, b::AbstractSIMDVector{W}) where {W} =
  vfdiv_fast(vfloat(a), vfloat(b))
@inline vfdiv(a, b) = a / b
@inline vfdiv_fast(a, b) = Base.FastMath.div_fast(a, b)

for (f, op) ∈ ((:vand, :and_int), (:vor, :or_int), (:vxor, :xor_int))
  @eval @inline $f(b1::Bool, b2::Bool) = Base.$op(b1, b2)
end

for f ∈ [:vadd, :vsub, :vmul]
  for s ∈ [Symbol(""), :_fast, :_nsw, :_nuw, :_nw]
    fs = Symbol(f, s)
    @eval begin
      @inline function $fs(
        a::Union{FloatingTypes,IntegerTypesHW,AbstractSIMD},
        b::Union{FloatingTypes,IntegerTypesHW,AbstractSIMD},
      )
        c, d = promote(a, b)
        $fs(c, d)
      end
    end
  end
end
# @inline vsub(a::T, b::T) where {T<:Base.BitInteger} = Base.sub_int(a, b)
for (vf, bf) ∈ [
  (:vadd, :add_int),
  (:vsub, :sub_int),
  (:vmul, :mul_int),
  (:vadd_fast, :add_int),
  (:vsub_fast, :sub_int),
  (:vmul_fast, :mul_int),
  (:vadd_nsw, :add_int),
  (:vsub_nsw, :sub_int),
  (:vmul_nsw, :mul_int),
  (:vadd_nuw, :add_int),
  (:vsub_nuw, :sub_int),
  (:vmul_nuw, :mul_int),
  (:vadd_nw, :add_int),
  (:vsub_nw, :sub_int),
  (:vmul_nw, :mul_int),
]
  @eval begin
    @inline $vf(a::Int128, b::Int128) = Base.$bf(a, b)
    @inline $vf(a::UInt128, b::UInt128) = Base.$bf(a, b)
  end
end
# @inline vrem(a::Float32, b::Float32) = Base.rem_float_fast(a, b)
# @inline vrem(a::Float64, b::Float64) = Base.rem_float_fast(a, b)

@inline function Base.FastMath.add_fast(a::AbstractSIMD, b::AbstractSIMD, c::AbstractSIMD)
  Base.FastMath.add_fast(Base.FastMath.add_fast(a, b), c)
end
@inline function Base.FastMath.add_fast(a::T, b::T, c::T) where {T<:AbstractSIMD}
  Base.FastMath.add_fast(Base.FastMath.add_fast(a, b), c)
end

@inline function Base.FastMath.add_fast(
  a::AbstractSIMD,
  b::AbstractSIMD,
  c::AbstractSIMD,
  d::AbstractSIMD,
)
  x = Base.FastMath.add_fast(a, b)
  y = Base.FastMath.add_fast(c, d)
  Base.FastMath.add_fast(x, y)
end
@inline function Base.FastMath.add_fast(a::T, b::T, c::T, d::T) where {T<:AbstractSIMD}
  x = Base.FastMath.add_fast(a, b)
  y = Base.FastMath.add_fast(c, d)
  Base.FastMath.add_fast(x, y)
end

@inline function Base.FastMath.add_fast(
  a::AbstractSIMD,
  b::AbstractSIMD,
  c::AbstractSIMD,
  d::AbstractSIMD,
  e::AbstractSIMD,
  f::Vararg{Number,K},
) where {K}
  x = Base.FastMath.add_fast(a, b)
  y = Base.FastMath.add_fast(c, d)
  Base.FastMath.add_fast(Base.FastMath.add_fast(x, y), e, f...)
end
@inline function Base.FastMath.add_fast(
  a::T,
  b::T,
  c::T,
  d::T,
  e::T,
  f::Vararg{T,K},
) where {T<:AbstractSIMD,K}
  x = Base.FastMath.add_fast(a, b)
  y = Base.FastMath.add_fast(c, d)
  Base.FastMath.add_fast(Base.FastMath.add_fast(x, y), e, f...)
end


module DarcyCubedSphereTestsMPI
  using PartitionedArrays
  using Test
  using FillArrays
  using Gridap
  using GridapPETSc
  using GridapGeosciences

  include("../DarcyCubedSphereTests.jl")
  include("../ConvergenceAnalysisTools.jl")

  function petsc_gamg_options()
    """
      -ksp_type gmres -ksp_rtol 1.0e-06 -ksp_atol 0.0
      -ksp_monitor -pc_type gamg -pc_gamg_type agg
      -mg_levels_esteig_ksp_type gmres -mg_coarse_sub_pc_type lu
      -mg_coarse_sub_pc_factor_mat_ordering_type nd -pc_gamg_process_eq_limit 50
      -pc_gamg_square_graph 9 pc_gamg_agg_nsmooths 1
    """
  end
  function petsc_mumps_options()
    """
      -ksp_type preonly -ksp_error_if_not_converged true
      -pc_type lu -pc_factor_mat_solver_type mumps
    """
  end
  function main(parts)
    GridapPETSc.with(args=split(petsc_gamg_options())) do
       num_refs=[2,3,4,5]
       hs=[2.0/2^n for n in num_refs]
       model_args_series=zip(Fill(parts,length(num_refs)),num_refs)
       t = PartitionedArrays.PTimer(parts,verbose=true)
       PartitionedArrays.tic!(t)
       hs1,k1errors,s1=convergence_study(solve_darcy,hs,model_args_series,1,4,PETScLinearSolver())
       PartitionedArrays.toc!(t,"DarcyCubedSphereConvergenceStudy")
       display(t)
       println(hs1)
       println(k1errors)
       println(s1)
    end
  end
  prun(main,mpi,4)
end #module


abstract type Clock <: System end
timeunit(::Type{<:Clock}) = u"hr"
@system Clock{timeunit = timeunit(Clock)} begin
    context ~ ::Nothing
    config ~ ::Config(override)
    init => 0 ~ preserve(unit=timeunit, parameter)
    step => 1 ~ preserve(unit=timeunit, parameter)
    time => nothing ~ advance(init=init, step=step, unit=timeunit)
    tick => nothing ~ advance::int
end

abstract type DailyClock <: Clock end
timeunit(::Type{<:DailyClock}) = u"d"
@system DailyClock{timeunit = timeunit(DailyClock)}(Clock) <: Clock

timeunit(c::C) where {C<:Clock} = timeunit(C)

export Clock


using PackageCompiler
using MKL_jll

include("../src/install.jl")
enable_mkl_startup()


mutable struct CollectGemsDirected{T, R} <: AbstractGridWorld
    world::GridWorldBase{Tuple{Agent, Wall, Gem}}
    agent_pos::CartesianIndex{2}
    agent_dir::AbstractDirection
    reward::T
    rng::R
    num_gem_init::Int
    num_gem_current::Int
    gem_reward::T
    gem_pos_init::Vector{CartesianIndex{2}}
    done::Bool
end

@generate_getters(CollectGemsDirected)
@generate_setters(CollectGemsDirected)

mutable struct CollectGemsUndirected{T, R} <: AbstractGridWorld
    world::GridWorldBase{Tuple{Agent, Wall, Gem}}
    agent_pos::CartesianIndex{2}
    reward::T
    rng::R
    num_gem_init::Int
    num_gem_current::Int
    gem_reward::T
    gem_pos_init::Vector{CartesianIndex{2}}
    done::Bool
end

@generate_getters(CollectGemsUndirected)
@generate_setters(CollectGemsUndirected)

#####
# Directed
#####

function CollectGemsDirected(; T = Float32, height = 8, width = 8, num_gem_init = floor(Int, sqrt(height * width)), rng = Random.GLOBAL_RNG)
    objects = (AGENT, WALL, GEM)
    world = GridWorldBase(objects, height, width)

    room = Room(CartesianIndex(1, 1), height, width)
    place_room!(world, room)

    agent_pos = CartesianIndex(2, 2)
    world[AGENT, agent_pos] = true

    agent_dir = RIGHT
    reward = zero(T)
    num_gem_current = num_gem_init
    gem_reward = one(T)
    gem_pos_init = CartesianIndex{2}[]
    done = false

    env = CollectGemsDirected(world, agent_pos, agent_dir, reward, rng, num_gem_init, num_gem_current, gem_reward, gem_pos_init, done)

    RLBase.reset!(env)

    return env
end

RLBase.state_space(env::CollectGemsDirected, ::RLBase.Observation, ::RLBase.DefaultPlayer) = nothing
const COLLECT_GEMS_DIRECTED_LAYERS = SA.SVector(2, 3)
RLBase.state(env::CollectGemsDirected, ::RLBase.Observation, ::RLBase.DefaultPlayer) = get_grid(get_world(env), get_agent_pos(env), get_agent_dir(env), get_half_size(env), COLLECT_GEMS_DIRECTED_LAYERS)

RLBase.state_space(env::CollectGemsDirected, ::RLBase.InternalState, ::RLBase.DefaultPlayer) = nothing
RLBase.state(env::CollectGemsDirected, ::RLBase.InternalState, ::RLBase.DefaultPlayer) = (copy(get_grid(env)), get_agent_dir(env))

RLBase.action_space(env::CollectGemsDirected, ::RLBase.DefaultPlayer) = DIRECTED_NAVIGATION_ACTIONS
RLBase.reward(env::CollectGemsDirected, ::RLBase.DefaultPlayer) = get_reward(env)
RLBase.is_terminated(env::CollectGemsDirected) = get_done(env)

function RLBase.reset!(env::CollectGemsDirected{T}) where {T}
    world = get_world(env)
    rng = get_rng(env)
    gem_pos_init = get_gem_pos_init(env)
    num_gem_init = get_num_gem_init(env)

    world[AGENT, get_agent_pos(env)] = false
    map(pos -> world[GEM, pos] = false, gem_pos_init)
    empty!(gem_pos_init)

    for i in 1:num_gem_init
        pos = rand(rng, pos -> !any(@view world[:, pos]), env)
        world[GEM, pos] = true
        push!(gem_pos_init, pos)
    end
    set_num_gem_current!(env, num_gem_init)

    agent_start_pos = rand(rng, pos -> !any(@view world[:, pos]), env)
    set_agent_pos!(env, agent_start_pos)
    world[AGENT, agent_start_pos] = true

    set_agent_dir!(env, rand(rng, DIRECTIONS))

    set_reward!(env, zero(T))
    set_done!(env, false)

    return nothing
end

function (env::CollectGemsDirected{T})(action::AbstractTurnAction) where {T}
    new_dir = turn(action, get_agent_dir(env))
    set_agent_dir!(env, new_dir)
    world = get_world(env)

    set_done!(env, get_num_gem_current(env) <= 0)
    set_reward!(env, zero(T))

    return nothing
end

function (env::CollectGemsDirected{T})(action::AbstractMoveAction) where {T}
    world = get_world(env)

    agent_pos = get_agent_pos(env)
    dest = move(action, get_agent_dir(env), agent_pos)
    if !world[WALL, dest]
        world[AGENT, agent_pos] = false
        set_agent_pos!(env, dest)
        world[AGENT, dest] = true
    end

    set_reward!(env, zero(T))
    agent_pos = get_agent_pos(env)
    if world[GEM, agent_pos]
        world[GEM, agent_pos] = false
        set_num_gem_current!(env, get_num_gem_current(env) - 1)
        set_reward!(env, get_gem_reward(env))
    end

    set_done!(env, get_num_gem_current(env) <= 0)

    return nothing
end

#####
# Undirected
#####

RLBase.state_space(env::CollectGemsUndirected, ::RLBase.Observation, ::RLBase.DefaultPlayer) = nothing
const COLLECT_GEMS_UNDIRECTED_LAYERS = SA.SVector(2, 3)
RLBase.state(env::CollectGemsUndirected, ::RLBase.Observation, ::RLBase.DefaultPlayer) = get_grid(get_world(env), get_agent_pos(env), get_half_size(env), COLLECT_GEMS_UNDIRECTED_LAYERS)

RLBase.state_space(env::CollectGemsUndirected, ::RLBase.InternalState, ::RLBase.DefaultPlayer) = nothing
RLBase.state(env::CollectGemsUndirected, ::RLBase.InternalState, ::RLBase.DefaultPlayer) = copy(get_grid(env))

RLBase.action_space(env::CollectGemsUndirected, player::RLBase.DefaultPlayer) = UNDIRECTED_NAVIGATION_ACTIONS
RLBase.reward(env::CollectGemsUndirected, ::RLBase.DefaultPlayer) = get_reward(env)
RLBase.is_terminated(env::CollectGemsUndirected) = get_done(env)

function CollectGemsUndirected(; T = Float32, height = 8, width = 8, num_gem_init = floor(Int, sqrt(height * width)), rng = Random.GLOBAL_RNG)
    objects = (AGENT, WALL, GEM)
    world = GridWorldBase(objects, height, width)

    room = Room(CartesianIndex(1, 1), height, width)
    place_room!(world, room)

    agent_pos = CartesianIndex(2, 2)
    world[AGENT, agent_pos] = true

    reward = zero(T)
    num_gem_current = num_gem_init
    gem_reward = one(T)
    gem_pos_init = CartesianIndex{2}[]
    done = false

    env = CollectGemsUndirected(world, agent_pos, reward, rng, num_gem_init, num_gem_current, gem_reward, gem_pos_init, done)

    RLBase.reset!(env)

    return env
end

function RLBase.reset!(env::CollectGemsUndirected{T}) where {T}
    world = get_world(env)
    rng = get_rng(env)
    gem_pos_init = get_gem_pos_init(env)
    num_gem_init = get_num_gem_init(env)

    world[AGENT, get_agent_pos(env)] = false
    map(pos -> world[GEM, pos] = false, gem_pos_init)
    empty!(gem_pos_init)

    for i in 1:num_gem_init
        pos = rand(rng, pos -> !any(@view world[:, pos]), env)
        world[GEM, pos] = true
        push!(gem_pos_init, pos)
    end
    set_num_gem_current!(env, num_gem_init)

    agent_start_pos = rand(rng, pos -> !any(@view world[:, pos]), env)
    set_agent_pos!(env, agent_start_pos)
    world[AGENT, agent_start_pos] = true

    set_reward!(env, zero(T))
    set_done!(env, false)

    return nothing
end

function (env::CollectGemsUndirected{T})(action::AbstractMoveAction) where {T}
    world = get_world(env)

    agent_pos = get_agent_pos(env)
    dest = move(action, agent_pos)
    if !world[WALL, dest]
        world[AGENT, agent_pos] = false
        set_agent_pos!(env, dest)
        world[AGENT, dest] = true
    end

    set_reward!(env, zero(T))
    agent_pos = get_agent_pos(env)
    if world[GEM, agent_pos]
        world[GEM, agent_pos] = false
        set_num_gem_current!(env, get_num_gem_current(env) - 1)
        set_reward!(env, get_gem_reward(env))
    end

    set_done!(env, get_num_gem_current(env) <= 0)

    return nothing
end


# MIT License

# Copyright (c) 2021 Dr. William A. Pisani

# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
# in the Software without restriction, including without limitation the rights
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:

# The above copyright notice and this permission notice shall be included in all
# copies or substantial portions of the Software.

# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
# SOFTWARE.
"""
    impropers(bonds::Array{Array{Int64,N} where N,1}) -> Array{NTuple{4,Int64},1}

Determines the impropers of a molecule or molecular system given the bonds.
"""
function impropers(bonds::Array{Array{Int64,N} where N,1})
    println("Determining impropers...")
    count_atomid1s = countmap(atomid[1] for atomid in bonds)
    count_atomid2s = countmap(atomid[2] for atomid in bonds)
    count_atomids = mergewith(+,count_atomid1s,count_atomid2s)

    # Get atomids with 3 or more bonds, those atoms have impropers
    atomids = [key for (key,value) in count_atomids if value >= 3]

    improper_array = Tuple{Int64,Int64,Int64,Int64}[]

    # Find atom ids connected to improper atoms
    for atomid in atomids
        atomid_connections = []
        for bond in bonds
            if atomid == bond[1]
                push!(atomid_connections,bond[2])
            elseif atomid == bond[2]
                push!(atomid_connections,bond[1])
            end
        end

        # Now that connections are found, get combinations of impropers
        if length(atomid_connections) == 3 # If only three bonds
            imp = (atomid_connections[1],atomid,atomid_connections[2],atomid_connections[3])
            push!(improper_array,imp)

        elseif length(atomid_connections) == 4 # If four bonds
            imp = (atomid_connections[1],atomid,atomid_connections[2],atomid_connections[3]) # 1-2-3
            push!(improper_array,imp)

            imp = (atomid_connections[2],atomid,atomid_connections[3],atomid_connections[4]) # 2-3-4
            push!(improper_array,imp)

            imp = (atomid_connections[1],atomid,atomid_connections[2],atomid_connections[4]) # 1-2-4
            push!(improper_array,imp)

            imp = (atomid_connections[1],atomid,atomid_connections[3],atomid_connections[4]) # 1-3-4
            push!(improper_array,imp)

        end
    end
    println("Impropers determined!")
    return improper_array
end


#== import modules here ==#
using PyCall
psi4 = pyimport("psi4")

using HDF5
#using MPI

#== psi4 setup ==#
psi4.set_memory("500 MB")
psi4.core.set_output_file("sad_guess.dat")

#===============#
#== MPI setup ==#
#===============#
#MPI.Init()
#comm = MPI.COMM_WORLD

#mpi_rank = MPI.Comm_rank(comm)
#mpi_size = MPI.Comm_size(comm)

#==============================#
#== get atom/basis set pairs ==#
#==============================#

pairs = Vector{String}([])
status_bsed = h5open("../records/bsed.h5","r") do bsed
  #== loop over atoms ==#
  for symbol in names(bsed)
    #== loop over basis sets in atom ==#
    for basis in names(bsed["$symbol"])
      #== add atom/basis-set pair to pair list ==#
      pair = "$symbol/$basis"
      push!(pairs, pair)
    end
  end
end

status_sadgss = h5open("sadgss.h5","w") do sadgss
  #if mpi_rank == 0
    flush(sadgss)
  #end

  #symbol = "O"
  #basis = "6-31G"
  #pair = "$symbol/$basis"

  #MPI.Barrier(comm)
  for (pair_idx, pair) in enumerate(pairs)
    #if mpi_rank != (pair_idx-1)%mpi_size continue end

    pair_regex = match(r"(.*)/(.*)", pair)
    symbol = pair_regex.captures[1]
    basis = pair_regex.captures[2]

    println("$symbol/$basis")

    try
      geom = psi4.geometry("""
        $symbol  0.0 0.0 0.0
        symmetry=c1
      """)

      options = Dict(
        "REFERENCE" => "ROHF",
        "BASIS" => basis,
        "SCF_TYPE"  => "PK",
        "GUESS" => "SAD",
	"PUREAM" => false,
        #"SAD_SCF_TYPE" => "DIRECT",
        "E_CONVERGENCE" => 1e-10,
        "D_CONVERGENCE" => 1e-10,
        #"MAXITER" => 1,
        "FAIL_ON_MAXITER" => false
      )
      psi4.set_options(options)

      name = "scf/"*options["BASIS"]
      scf_e, scf_wfn = psi4.energy(name, molecule=geom, return_wfn=true)

      #density_a = scf_wfn.Da().array_interface()[1]
      #density_b = scf_wfn.Db().array_interface()[1]

      density_a = scf_wfn.Da().to_array()
      density_b = scf_wfn.Db().to_array()

      guess = density_a .+ density_b
      write(sadgss, pair, guess[:])
    catch e
      bt = catch_backtrace()
      msg = sprint(showerror, e, bt)
      println(msg)
      continue
    end
  end
end

#MPI.Finalize()


# This file is a part of Julia. License is MIT: https://julialang.org/license

module Versions

export VersionBound, VersionRange, VersionSpec, semver_spec, isjoinable

################
# VersionBound #
################
struct VersionBound
    t::NTuple{3,UInt32}
    n::Int
    function VersionBound(tin::NTuple{n,Integer}) where n
        n <= 3 || throw(ArgumentError("VersionBound: you can only specify major, minor and patch versions"))
        n == 0 && return new((0,           0,      0), n)
        n == 1 && return new((tin[1],      0,      0), n)
        n == 2 && return new((tin[1], tin[2],      0), n)
        n == 3 && return new((tin[1], tin[2], tin[3]), n)
        error("invalid $n")
    end
end
VersionBound(t::Integer...) = VersionBound(t)
VersionBound(v::VersionNumber) = VersionBound(v.major, v.minor, v.patch)

Base.getindex(b::VersionBound, i::Int) = b.t[i]

function ≲(v::VersionNumber, b::VersionBound)
    b.n == 0 && return true
    b.n == 1 && return v.major <= b[1]
    b.n == 2 && return (v.major, v.minor) <= (b[1], b[2])
    return (v.major, v.minor, v.patch) <= (b[1], b[2], b[3])
end

function ≲(b::VersionBound, v::VersionNumber)
    b.n == 0 && return true
    b.n == 1 && return v.major >= b[1]
    b.n == 2 && return (v.major, v.minor) >= (b[1], b[2])
    return (v.major, v.minor, v.patch) >= (b[1], b[2], b[3])
end

function isless_ll(a::VersionBound, b::VersionBound)
    m, n = a.n, b.n
    for i = 1:min(m, n)
        a[i] < b[i] && return true
        a[i] > b[i] && return false
    end
    return m < n
end

stricterlower(a::VersionBound, b::VersionBound) = isless_ll(a, b) ? b : a

# Comparison between two upper bounds
function isless_uu(a::VersionBound, b::VersionBound)
    m, n = a.n, b.n
    for i = 1:min(m, n)
        a[i] < b[i] && return true
        a[i] > b[i] && return false
    end
    return m > n
end

stricterupper(a::VersionBound, b::VersionBound) = isless_uu(a, b) ? a : b

# `isjoinable` compares an upper bound of a range with the lower bound of the next range
# to determine if they can be joined, as in [1.5-2.8, 2.5-3] -> [1.5-3]. Used by `union!`.
# The equal-length-bounds case is special since e.g. `1.5` can be joined with `1.6`,
# `2.3.4` can be joined with `2.3.5` etc.

function isjoinable(up::VersionBound, lo::VersionBound)
    up.n == 0 && lo.n == 0 && return true
    if up.n == lo.n
        n = up.n
        for i = 1:(n - 1)
            up[i] > lo[i] && return true
            up[i] < lo[i] && return false
        end
        up[n] < lo[n] - 1 && return false
        return true
    else
        l = min(up.n, lo.n)
        for i = 1:l
            up[i] > lo[i] && return true
            up[i] < lo[i] && return false
        end
    end
    return true
end

Base.hash(r::VersionBound, h::UInt) = hash(hash(r.t, h), r.n)

# Hot code
function VersionBound(s::AbstractString)
    s = strip(s)
    s == "*" && return VersionBound()
    first(s) == 'v' && (s = SubString(s, 2))
    l = lastindex(s)

    p = findnext('.', s, 1)
    b = p === nothing ? l : (p-1)
    i = parse(Int64, SubString(s, 1, b))
    p === nothing && return VersionBound(i)

    a = p+1
    p = findnext('.', s, a)
    b = p === nothing ? l : (p-1)
    j = parse(Int64, SubString(s, a, b))
    p === nothing && return VersionBound(i, j)

    a = p+1
    p = findnext('.', s, a)
    b = p === nothing ? l : (p-1)
    k = parse(Int64, SubString(s, a, b))
    p === nothing && return VersionBound(i, j, k)

    error("invalid VersionBound string $(repr(s))")
end

################
# VersionRange #
################
struct VersionRange
    lower::VersionBound
    upper::VersionBound
    # NOTE: ranges are allowed to be empty; they are ignored by VersionSpec anyway
    function VersionRange(lo::VersionBound, hi::VersionBound)
        # lo.t == hi.t implies that digits past min(lo.n, hi.n) are zero
        # lo.n < hi.n example: 1.2-1.2.0 => 1.2.0
        # lo.n > hi.n example: 1.2.0-1.2 => 1.2
        lo.t == hi.t && (lo = hi)
        return new(lo, hi)
    end
end
VersionRange(b::VersionBound=VersionBound()) = VersionRange(b, b)
VersionRange(t::Integer...)                  = VersionRange(VersionBound(t...))
VersionRange(v::VersionNumber)               = VersionRange(VersionBound(v))
VersionRange(lo::VersionNumber, hi::VersionNumber) = VersionRange(VersionBound(lo), VersionBound(hi))

# The vast majority of VersionRanges are in practice equal to "1"
const VersionRange_1 = VersionRange(VersionBound("1"), VersionBound("1"))
function VersionRange(s::AbstractString)
    s == "1" && return VersionRange_1
    p = split(s, "-")
    if length(p) != 1 && length(p) != 2
        throw(ArgumentError("invalid version range: $(repr(s))"))
    end
    lower = VersionBound(p[1])
    upper = length(p) == 1 ? lower : VersionBound(p[2])
    return VersionRange(lower, upper)
end

function Base.isempty(r::VersionRange)
    for i = 1:min(r.lower.n, r.upper.n)
        r.lower[i] > r.upper[i] && return true
        r.lower[i] < r.upper[i] && return false
    end
    return false
end

function Base.print(io::IO, r::VersionRange)
    m, n = r.lower.n, r.upper.n
    if (m, n) == (0, 0)
        print(io, '*')
    elseif m == 0
        print(io, "0-")
        join(io, r.upper.t, '.')
    elseif n == 0
        join(io, r.lower.t, '.')
        print(io, "-*")
    else
        join(io, r.lower.t[1:m], '.')
        if r.lower != r.upper
            print(io, '-')
            join(io, r.upper.t[1:n], '.')
        end
    end
end
Base.show(io::IO, r::VersionRange) = print(io, "VersionRange(\"", r, "\")")

Base.in(v::VersionNumber, r::VersionRange) = r.lower ≲ v ≲ r.upper

Base.intersect(a::VersionRange, b::VersionRange) = VersionRange(stricterlower(a.lower, b.lower), stricterupper(a.upper, b.upper))

function Base.union!(ranges::Vector{<:VersionRange})
    l = length(ranges)
    l == 0 && return ranges

    sort!(ranges, lt=(a, b) -> (isless_ll(a.lower, b.lower) || (a.lower == b.lower && isless_uu(a.upper, b.upper))))

    k0 = 1
    ks = findfirst(!isempty, ranges)
    ks === nothing && return empty!(ranges)

    lo, up, k0 = ranges[ks].lower, ranges[ks].upper, 1
    for k = (ks + 1):l
        isempty(ranges[k]) && continue
        lo1, up1 = ranges[k].lower, ranges[k].upper
        if isjoinable(up, lo1)
            isless_uu(up, up1) && (up = up1)
            continue
        end
        vr = VersionRange(lo, up)
        @assert !isempty(vr)
        ranges[k0] = vr
        k0 += 1
        lo, up = lo1, up1
    end
    vr = VersionRange(lo, up)
    if !isempty(vr)
        ranges[k0] = vr
        k0 += 1
    end
    resize!(ranges, k0 - 1)
    return ranges
end

###############
# VersionSpec #
###############
struct VersionSpec
    ranges::Vector{VersionRange}
    VersionSpec(r::Vector{<:VersionRange}) = new(union!(r))
    VersionSpec(vs::VersionSpec) = vs
end

VersionSpec(r::VersionRange) = VersionSpec(VersionRange[r])
VersionSpec(v::VersionNumber) = VersionSpec(VersionRange(v))
const _all_versionsspec = VersionSpec(VersionRange())
VersionSpec() = _all_versionsspec
VersionSpec(s::AbstractString) = VersionSpec(VersionRange(s))
VersionSpec(v::AbstractVector) = VersionSpec(map(VersionRange, v))

# Hot code
function Base.in(v::VersionNumber, s::VersionSpec)
    for r in s.ranges
        v in r && return true
    end
    return false
end

Base.copy(vs::VersionSpec) = VersionSpec(vs)

const empty_versionspec = VersionSpec(VersionRange[])
const _empty_symbol = "∅"

Base.isempty(s::VersionSpec) = all(isempty, s.ranges)
@assert isempty(empty_versionspec)
# Hot code, measure performance before changing
function Base.intersect(A::VersionSpec, B::VersionSpec)
    (isempty(A) || isempty(B)) && return copy(empty_versionspec)
    ranges = Vector{VersionRange}(undef, length(A.ranges) * length(B.ranges))
    i = 1
    @inbounds for a in A.ranges, b in B.ranges
        ranges[i] = intersect(a, b)
        i += 1
    end
    VersionSpec(ranges)
end
Base.intersect(a::VersionNumber, B::VersionSpec) = a in B ? VersionSpec(a) : empty_versionspec
Base.intersect(A::VersionSpec, b::VersionNumber) = intersect(b, A)

function Base.union(A::VersionSpec, B::VersionSpec)
    A == B && return A
    Ar = copy(A.ranges)
    append!(Ar, B.ranges)
    union!(Ar)
    return VersionSpec(Ar)
end

Base.:(==)(A::VersionSpec, B::VersionSpec) = A.ranges == B.ranges
Base.hash(s::VersionSpec, h::UInt) = hash(s.ranges, h + (0x2fd2ca6efa023f44 % UInt))

function Base.print(io::IO, s::VersionSpec)
    isempty(s) && return print(io, _empty_symbol)
    length(s.ranges) == 1 && return print(io, s.ranges[1])
    print(io, '[')
    for i = 1:length(s.ranges)
        1 < i && print(io, ", ")
        print(io, s.ranges[i])
    end
    print(io, ']')
end
Base.show(io::IO, s::VersionSpec) = print(io, "VersionSpec(\"", s, "\")")

###################
# Semver notation #
###################

function semver_spec(s::String; throw = true)
    ranges = VersionRange[]
    for ver in strip.(split(strip(s), ','))
        range = nothing
        found_match = false
        for (ver_reg, f) in ver_regs
            if occursin(ver_reg, ver)
                range = f(match(ver_reg, ver))
                found_match = true
                break
            end
        end
        if !found_match
            if throw
                error("invalid version specifier: \"$s\"")
            else
                return nothing
            end
        end
        push!(ranges, range)
    end
    return VersionSpec(ranges)
end

function semver_interval(m::RegexMatch)
    @assert length(m.captures) == 4
    n_significant = count(x -> x !== nothing, m.captures) - 1
    typ, _major, _minor, _patch = m.captures
    major =                           parse(Int, _major)
    minor = (n_significant < 2) ? 0 : parse(Int, _minor)
    patch = (n_significant < 3) ? 0 : parse(Int, _patch)
    if n_significant == 3 && major == 0 && minor == 0 && patch == 0
        error("invalid version: \"0.0.0\"")
    end
    # Default type is :caret
    vertyp = (typ == "" || typ == "^") ? :caret : :tilde
    v0 = VersionBound((major, minor, patch))
    if vertyp === :caret
        if major != 0
            return VersionRange(v0, VersionBound((v0[1],)))
        elseif minor != 0
            return VersionRange(v0, VersionBound((v0[1], v0[2])))
        else
            if n_significant == 1
                return VersionRange(v0, VersionBound((0,)))
            elseif n_significant == 2
                return VersionRange(v0, VersionBound((0, 0,)))
            else
                return VersionRange(v0, VersionBound((0, 0, v0[3])))
            end
        end
    else
        if n_significant == 3 || n_significant == 2
            return VersionRange(v0, VersionBound((v0[1], v0[2],)))
        else
            return VersionRange(v0, VersionBound((v0[1],)))
        end
    end
end

const _inf = VersionBound("*")
function inequality_interval(m::RegexMatch)
    @assert length(m.captures) == 4
    typ, _major, _minor, _patch = m.captures
    n_significant = count(x -> x !== nothing, m.captures) - 1
    major =                           parse(Int, _major)
    minor = (n_significant < 2) ? 0 : parse(Int, _minor)
    patch = (n_significant < 3) ? 0 : parse(Int, _patch)
    if n_significant == 3 && major == 0 && minor == 0 && patch == 0
        error("invalid version: 0.0.0")
    end
    v = VersionBound(major, minor, patch)
    if occursin(r"^<\s*$", typ)
        nil = VersionBound(0, 0, 0)
        if v[3] == 0
            if v[2] == 0
                v1 = VersionBound(v[1]-1)
            else
                v1 = VersionBound(v[1], v[2]-1)
            end
        else
            v1 = VersionBound(v[1], v[2], v[3]-1)
        end
        return VersionRange(nil, v1)
    elseif occursin(r"^=\s*$", typ)
        return VersionRange(v)
    elseif occursin(r"^>=\s*$", typ) || occursin(r"^≥\s*$", typ)
           return VersionRange(v, _inf)
    else
        error("invalid prefix $typ")
    end
end

function hyphen_interval(m::RegexMatch)
    @assert length(m.captures) == 6
    _lower_major, _lower_minor, _lower_patch, _upper_major, _upper_minor, _upper_patch = m.captures
    if isnothing(_lower_minor)
        lower_bound = VersionBound(parse(Int, _lower_major))
    elseif isnothing(_lower_patch)
        lower_bound = VersionBound(parse(Int, _lower_major),
                                   parse(Int, _lower_minor))
    else
        lower_bound = VersionBound(parse(Int, _lower_major),
                                   parse(Int, _lower_minor),
                                   parse(Int, _lower_patch))
    end
    if isnothing(_upper_minor)
        upper_bound = VersionBound(parse(Int, _upper_major))
    elseif isnothing(_upper_patch)
        upper_bound = VersionBound(parse(Int, _upper_major),
                                   parse(Int, _upper_minor))
    else
        upper_bound = VersionBound(parse(Int, _upper_major),
                                   parse(Int, _upper_minor),
                                   parse(Int, _upper_patch))
    end
    return VersionRange(lower_bound, upper_bound)
end

const version = "v?([0-9]+?)(?:\\.([0-9]+?))?(?:\\.([0-9]+?))?"
const ver_regs =
Pair{Regex,Any}[
                Regex("^([~^]?)?$version\$") => semver_interval, # 0.5 ^0.4 ~0.3.2
                Regex("^((?:≥\\s*)|(?:>=\\s*)|(?:=\\s*)|(?:<\\s*)|(?:=\\s*))v?$version\$")  => inequality_interval,# < 0.2 >= 0.5,2
                Regex("^[\\s]*$version[\\s]*?\\s-\\s[\\s]*?$version[\\s]*\$") => hyphen_interval, # 0.7 - 1.3
]

end


# addprocs(7)
# ask first for user convenience
using Caesar

include("/home/dehann/Documents/blandauthlocal.jl")
cloudGraph, addrdict = standardcloudgraphsetup(nparticles=true, addrdict=addrdict)

using RoME
using CloudGraphs, Neo4j

# using IncrementalInference

# session = addrdict["session"]
session = "TestHexagonalDrive11"
addrdict["num particles"] = "100"
Nparticles = parse(Int, addrdict["num particles"])
println("Attempting to solve session $(session) with $(Nparticles) particles per marginal...")

fg = initfg(sessionname=session, cloudgraph=cloudGraph)

setBackendWorkingSet!(fg.cg.neo4j.connection, session)

# TODO -- incremental graph and subgraphs are works in progress
while true
  println("=================================================")
  fg = initfg(sessionname=session, cloudgraph=cloudGraph)

  setBackendWorkingSet!(fg.cg.neo4j.connection, session)

  println("get local copy of graph")

  # removeGenericMarginals!(conn) # function should not be necessary, but fixes a minor bug following elimination algorithm
  if fullLocalGraphCopy!(fg)
    tree = wipeBuildNewTree!(fg,drawpdf=true)
    # removeGenericMarginals!(conn)

    # while true # repeat while graph unchanged
      # okay now do the solve
      inferOverTree!(fg, tree, N=Nparticles)
      # if true # current hack till test is inserted
      #   break;
      # end
    # end
  else
    sleep(0.2)
  end
end

# fg = initfg(sessionname=session, cloudgraph=cloudGraph)
# fullLocalGraphCopy!(fg, conn)
#
# ls(fg)
#
# tree = wipeBuildNewTree!(fg,drawpdf=true)
#
# inferOverTreeR!(fg, tree, N=100)

  #


using FileIO, NRRD, ImageCore

Alast = nothing
for (A, name) in ((rand(UInt8, 3, 5), "uint8"),
                  (reinterpret(N0f8, rand(UInt8, 3, 5)), "u8"),
                  (rand(Gray{N0f8}, 3, 5), "gray_u8"),
                  (rand(RGB{N0f8}, 3, 5), "rgb_u8"),
                  (rand(Gray{N0f16}, 3, 5), "gray_u16"),
                  (reinterpret(Gray{N4f12}, rand(0x0000:0x0fff, 3, 5)), "gray_u12"))
    fn = joinpath(dirname(@__FILE__), "io", "eltype_$name.nrrd")
    save(fn, A)
    Alast = A
end

# Now manually tweak for the "hex" version
fn = joinpath(dirname(@__FILE__), "io", "eltype_gray_u12_hex.nrrd")
save(fn, Alast, props=Dict("sample units" => "gray 0x0fff"))


# MIT license
# Copyright (c) Microsoft Corporation. All rights reserved.
# See LICENSE in the project root for full license information.

# declaring the Geometry module. in the future we might add or move existing code to it. currently it contains the basic types: Vec3, Vec4 and Transform
module Geometry

include("Transform.jl")

end # module Geometry
export Geometry

using .Geometry

include("Ray.jl")

include("Surface.jl")

# not ideal having this like this, but we need the types for Intersection
include("../Optical/OpticalInterface.jl")

include("CSG/Intersection.jl")
include("CSG/Interval.jl")

include("Primitives/NonCSG/Triangle.jl")

include("BoundingBox.jl")

include("Primitives/Plane.jl")
include("Primitives/Cylinder.jl")
include("Primitives/Sphere.jl")
include("Primitives/SphericalCap.jl")

include("Primitives/NonCSG/Rectangle.jl")
include("Primitives/NonCSG/Hexagon.jl")
include("Primitives/NonCSG/Ellipse.jl")
include("Primitives/NonCSG/Stop.jl")

# include("BoundingVolumeHierarchy.jl")

include("AccelSurface.jl")

include("Primitives/Curves/Knots.jl")
include("Primitives/Curves/Spline.jl")
include("Primitives/Curves/BSpline.jl")
include("Primitives/Curves/Bezier.jl")
include("Primitives/Curves/PowerBasis.jl")

include("CSG/CSG.jl")

include("Primitives/Zernike.jl")
include("Primitives/Qtype.jl")
include("Primitives/Chebyshev.jl")
include("Primitives/GridSag.jl")

include("AnalyticIntersection.jl")

###########################################################################################################

"""
    BoundedCylinder(radius::T, height::T; interface::NullOrFresnel{T} = nullinterface(T)) -> CSGGenerator{T}

Create a cylinder with planar caps on both ends centred at `(0, 0, 0)` with axis `(0, 0, 1)`.
"""
function BoundedCylinder(radius::T, height::T; interface::NullOrFresnel{T} = NullInterface(T)) where {T<:Real}
    barrel = Cylinder(radius, height, interface = interface)
    top = Plane(SVector{3,T}(0.0, 0.0, 1.0), SVector{3,T}(0.0, 0.0, height / 2), vishalfsizeu = radius, vishalfsizev = radius)
    bottom = Plane(SVector{3,T}(0.0, 0.0, -1.0), SVector{3,T}(0.0, 0.0, -height / 2), vishalfsizeu = radius, vishalfsizev = radius)
    return csgintersection(leaf(barrel), csgintersection(top, bottom))
end
export BoundedCylinder

"""
    Cuboid(halfsizex::T, halfsizey::T, halfsizez::T; interface::NullOrFresnel{T} = nullinterface(T)) -> CSGGenerator{T}

Create a cuboid centred at `(0, 0, 0)`.
"""
function Cuboid(halfsizex::T, halfsizey::T, halfsizez::T; interface::NullOrFresnel{T} = NullInterface(T)) where {T<:Real}
    xmin = Plane(SVector{3,T}(-1.0, 0.0, 0.0), SVector{3,T}(-halfsizex, 0.0, 0.0), vishalfsizeu = halfsizez, vishalfsizev = halfsizey, interface = interface)
    xmax = Plane(SVector{3,T}(1.0, 0.0, 0.0), SVector{3,T}(halfsizex, 0.0, 0.0), vishalfsizeu = halfsizez, vishalfsizev = halfsizey, interface = interface)
    ymin = Plane(SVector{3,T}(0.0, -1.0, 0.0), SVector{3,T}(0.0, -halfsizey, 0.0), vishalfsizeu = halfsizez, vishalfsizev = halfsizex, interface = interface)
    ymax = Plane(SVector{3,T}(0.0, 1.0, 0.0), SVector{3,T}(0.0, halfsizey, 0.0), vishalfsizeu = halfsizez, vishalfsizev = halfsizex, interface = interface)
    zmin = Plane(SVector{3,T}(0.0, 0.0, -1.0), SVector{3,T}(0.0, 0.0, -halfsizez), vishalfsizeu = halfsizex, vishalfsizev = halfsizey, interface = interface)
    zmax = Plane(SVector{3,T}(0.0, 0.0, 1.0), SVector{3,T}(0.0, 0.0, halfsizez), vishalfsizeu = halfsizex, vishalfsizev = halfsizey, interface = interface)
    xint = csgintersection(xmin, xmax)
    yint = csgintersection(ymin, ymax)
    zint = csgintersection(zmin, zmax)
    return csgintersection(xint, csgintersection(yint, zint))
end
export Cuboid

"""
    HexagonalPrism(side_length::T, visheight::T = 2.0; interface::NullOrFresnel{T} = nullinterface(T)) -> CSGGenerator{T}

Create an infinitely tall hexagonal prism with axis `(0, 0, 1)`, the longer hexagon diameter is along the x axis.
For visualization `visheight` is used, **note that this does not fully represent the surface**.
"""
function HexagonalPrism(side_length::T, visheight::T = 2.0; interface::NullOrFresnel{T} = NullInterface(T)) where {T<:Real}
    h = side_length * sqrt(3.0) / 2.0
    hcos = h * sqrt(3.0) / 2.0
    hsin = h / 2.0
    side_length *= 1.0001
    s1 = Plane(SVector{3,T}(0.0, 1.0, 0.0), SVector{3,T}(0.0, h, 0.0), vishalfsizeu = visheight / 2.0, vishalfsizev = side_length / 2.0, interface = interface)
    s2 = Plane(SVector{3,T}(0.0, -1.0, 0.0), SVector{3,T}(0.0, -h, 0.0), vishalfsizeu = visheight / 2.0, vishalfsizev = side_length / 2.0, interface = interface)
    s3 = Plane(SVector{3,T}(hcos, hsin, 0.0), SVector{3,T}(hcos, hsin, 0.0), vishalfsizeu = visheight / 2.0, vishalfsizev = side_length / 2.0, interface = interface)
    s4 = Plane(SVector{3,T}(-hcos, -hsin, 0.0), SVector{3,T}(-hcos, -hsin, 0.0), vishalfsizeu = visheight / 2.0, vishalfsizev = side_length / 2.0, interface = interface)
    s5 = Plane(SVector{3,T}(hcos, -hsin, 0.0), SVector{3,T}(hcos, -hsin, 0.0), vishalfsizeu = visheight / 2.0, vishalfsizev = side_length / 2.0, interface = interface)
    s6 = Plane(SVector{3,T}(-hcos, hsin, 0.0), SVector{3,T}(-hcos, hsin, 0.0), vishalfsizeu = visheight / 2.0, vishalfsizev = side_length / 2.0, interface = interface)
    i1 = csgintersection(s1, s2)
    i2 = csgintersection(s3, s4)
    i3 = csgintersection(s5, s6)
    return csgintersection(i1, csgintersection(i2, i3))
end
export HexagonalPrism

"""
    RectangularPrism(halfsizex::T, halfsizey::T, visheight::T=2.0; interface::NullOrFresnel{T} = nullinterface(T)) -> CSGGenerator{T}

Create an infinitely tall rectangular prism with axis `(0, 0, 1)`.
For visualization `visheight` is used, **note that this does not fully represent the surface**.
"""
function RectangularPrism(halfsizex::T, halfsizey::T, visheight::T = 2.0; interface::NullOrFresnel{T} = NullInterface(T)) where {T<:Real}
    s1 = Plane(SVector{3,T}(0.0, 1.0, 0.0), SVector{3,T}(0.0, halfsizey, 0.0), vishalfsizeu = visheight / 2.0, vishalfsizev = halfsizex, interface = interface)
    s2 = Plane(SVector{3,T}(0.0, -1.0, 0.0), SVector{3,T}(0.0, -halfsizey, 0.0), vishalfsizeu = visheight / 2.0, vishalfsizev = halfsizex, interface = interface)
    s3 = Plane(SVector{3,T}(1.0, 0.0, 0.0), SVector{3,T}(halfsizex, 0.0, 0.0), vishalfsizeu = visheight / 2.0, vishalfsizev = halfsizey, interface = interface)
    s4 = Plane(SVector{3,T}(-1.0, 0.0, 0.0), SVector{3,T}(-halfsizex, 0.0, 0.0), vishalfsizeu = visheight / 2.0, vishalfsizev = halfsizey, interface = interface)
    i1 = csgintersection(s1, s2)
    i2 = csgintersection(s3, s4)
    return csgintersection(i1, i2)
end
export RectangularPrism

"""
    TriangularPrism(side_length::T, visheight::T = 2.0; interface::NullOrFresnel{T} = nullinterface(T)) -> CSGGenerator{T}

Create an infinitely tall triangular prism with axis `(0, 0, 1)`.
For visualization `visheight` is used, **note that this does not fully represent the surface**.
"""
function TriangularPrism(side_length::T, visheight::T = 2.0; interface::NullOrFresnel{T} = NullInterface(T)) where {T<:Real}
    side_length = side_length / 2
    p1 = Plane(SVector{3,T}(-1.0, 0.0, 0.0), SVector{3,T}(-side_length / 2, 0.0, 0.0), interface = interface, vishalfsizev = side_length, vishalfsizeu = visheight / 2)
    p2 = Plane(SVector{3,T}(sind(30), cosd(30), 0.0), SVector{3,T}(side_length / 2 * sind(30), side_length / 2 * cosd(30), 0.0), interface = interface, vishalfsizev = side_length, vishalfsizeu = visheight / 2)
    p3 = Plane(SVector{3,T}(sind(30), -cosd(30), 0.0), SVector{3,T}(side_length / 2 * sind(30), -side_length / 2 * cosd(30), 0.0), interface = interface, vishalfsizev = side_length, vishalfsizeu = visheight / 2)
    return csgintersection(leaf(p1), csgintersection(p2, p3))
end
export TriangularPrism

"""
    Spider(narms::Int, armwidth::T, radius::T, origin::SVector{3,T} = SVector{3,T}(0.0, 0.0, 0.0), normal::SVector{3,T} = SVector{3,T}(0.0, 0.0, 1.0)) -> Vector{Rectangle{T}}

Creates a 'spider' obscuration with `narms` rectangular arms evenly spaced around a circle defined by `origin` and `normal`.
Each arm is a rectangle `armwidth`×`radius`.

e.g. for 3 and 4 arms we get:
```
   |         _|_
  / \\         |
```
"""
function Spider(narms::Int, armwidth::T, radius::T, origin::SVector{3,T} = SVector{3,T}(0.0, 0.0, 0.0), normal::SVector{3,T} = SVector{3,T}(0.0, 0.0, 1.0)) where {T<:Real}
    dθ = 2π / narms
    rects = Vector{Rectangle{T}}(undef, 0)
    for i in 0:(narms - 1)
        θ = i * dθ
        r = Rectangle(armwidth / 2, radius / 2, normal, origin + SVector(radius / 2 * cos(θ), radius / 2 * sin(θ), 0.0), rotationvec = SVector(cos(θ), sin(θ), 0.0), interface = opaqueinterface(T))
        push!(rects, r)
    end
    return rects
end
export Spider