text:
export MIMOFilt

"""
`MIMOFilt([domainType=Float64::Type,] dim_in::Tuple, B::Vector{AbstractVector}, [A::Vector{AbstractVector},])`

`MIMOFilt(x::AbstractMatrix, b::Vector{AbstractVector}, [a::Vector{AbstractVector},])`

Creates a `LinearOperator` which, when multiplied with a matrix `X`, returns a matrix `Y`. Here a Multiple Input Multiple Output system is evaluated: the columns of `X` and `Y` represent the input signals and output signals respectively.

```math
\\mathbf{y}_i = \\sum_{j = 1}^{M} \\mathbf{h}_{i,j} * \\mathbf{x}_j
```
where ``\\mathbf{y}_i`` and ``\\mathbf{x}_j`` are the ``i``-th and ``j``-th columns of the output `Y` and input `X` matrices respectively.

The filters ``\\mathbf{h}_{i,j}`` can be represented either by providing coefficients `B` and `A` (IIR) or `B` alone (FIR). These coefficients must be given in a `Vector` of `Vector`s.

For example for a `3` by `2` MIMO system (i.e. `size(X,2) == 3` inputs and `size(Y,2) == 2` outputs) `B` must be:

`B = [b11, b12, b13, b21, b22, b23]`

where `bij` are vector containing the filter coeffients of `h_{i,j}`.

```julia
julia> m,n = 10,3; #time samples, number of inputs

julia> B  = [[1.;0.;1.],[1.;0.;1.],[1.;0.;1.],[1.;0.;1.],[1.;0.;1.],[1.;0.;1.], ];
      #B = [   b11   ,     b12   ,    b13   ,   b21    ,   b22,       b23    , ]

julia> A  = [[1.;1.;1.],[2.;2.;2.],[      3.],[      4.],[      5.],[      6.], ];
      #A = [   a11   ,     a12   ,    a13   ,   a21    ,   a22,       a23    , ]

julia> op = MIMOFilt(Float64, (m,n), B, A)
※  ℝ^(10, 3) -> ℝ^(10, 2)

julia> X = randn(m,n); #input signals

julia> Y = op*X;       #output signals

julia> Y[:,1] ≈ filt(B[1],A[1],X[:,1])+filt(B[2],A[2],X[:,2])+filt(B[3],A[3],X[:,3])
true

julia> Y[:,2] ≈ filt(B[4],A[4],X[:,1])+filt(B[5],A[5],X[:,2])+filt(B[6],A[6],X[:,3])
true

```

"""
struct MIMOFilt{T, A<:AbstractVector{T}} <: LinearOperator
	dim_out::Tuple{Int,Int}
	dim_in::Tuple{Int,Int}
	B::Vector{A}
	A::Vector{A}
	SI::Vector{A}
end

# Constructors

#default constructor
function MIMOFilt(domainType::Type, dim_in::NTuple{N,Int}, b::Vector, a::Vector) where {N}

	N != 2 && error("length(dim_in) must be equal to 2")
	eltype(b) != eltype(a) && error("eltype(b) must be equal to eltype(a)")
	typeof(b[1][1]) != domainType && error("filter coefficient of b must be $domainType")
	typeof(a[1][1]) != domainType && error("filter coefficient of a must be $domainType")

	length(b) != length(a) && error("filter vectors b must be as many as a")

	mod(length(b),dim_in[2]) !=0 && error("wrong number of filters")
	dim_out = (dim_in[1], div(length(b),dim_in[2]) )

    B,A,SI = similar(b),similar(b),similar(b)

	for i = 1:length(b)
		a[i][1] == 0  && error("filter vector a[$i][1] must be nonzero")

		B[i]  = b[i]
		A[i]  = a[i]

		as = length(A[i])
		bs = length(B[i])
		sz = max(as, bs)
		silen = sz - 1

		# Filter coefficient normalization
		if A[i][1] != 1
			norml = A[i][1]
			A[i] ./= norml
			B[i] ./= norml
		end

		# Pad the coefficients with zeros if needed
		bs<sz   && (B[i] = copyto!(zeros(domainType, sz), B[i]))
		1<as<sz && (A[i] = copyto!(zeros(domainType, sz), A[i]))

		SI[i] = zeros(domainType, max(length(a[i]), length(b[i]))-1)

	end
	MIMOFilt{domainType, typeof(B[1])}(dim_out, dim_in, B, A, SI)
end

MIMOFilt(dim_in::Tuple,  b::Vector{D1}, a::Vector{D1}) where {D1<:AbstractVector} =
MIMOFilt(eltype(b[1]), dim_in, b, a)

MIMOFilt(dim_in::Tuple,  b::Vector{D1}) where {D1<:AbstractVector} =
MIMOFilt(eltype(b[1]), dim_in, b, [[1.0] for i in eachindex(b)])

MIMOFilt(x::AbstractMatrix,  b::Vector{D1}, a::Vector{D1}) where {D1<:AbstractVector} =
MIMOFilt(eltype(x), size(x), b, a)

MIMOFilt(x::AbstractMatrix,  b::Vector{D1}) where {D1<:AbstractVector} =
MIMOFilt(eltype(x), size(x), b, [[1.0] for i in eachindex(b)])

# Mappings

function mul!(y::AbstractArray{T},L::MIMOFilt{T,A},x::AbstractArray{T}) where {T,A}
	cnt = 0
	cx  = 0
	y .= 0. #TODO avoid this?
	for cy = 1:L.dim_out[2]
		cnt += 1
		cx  += 1
		length(L.A[cnt]) != 1 ? add_iir!(y,L.B[cnt],L.A[cnt],x,L.SI[cnt],cy,cx) :
		add_fir!(y,L.B[cnt],x,L.SI[cnt],cy,cx)

		for c2 = 2:L.dim_in[2]
			cnt += 1
			cx  += 1
			length(L.A[cnt]) != 1 ? add_iir!(y,L.B[cnt],L.A[cnt],x,L.SI[cnt],cy,cx) :
			add_fir!(y,L.B[cnt],x,L.SI[cnt],cy,cx)
		end
		cx = 0
	end
end

function mul!(y::AbstractArray{T},M::AdjointOperator{MIMOFilt{T,A}},x::AbstractArray{T}) where {T,A}
    L = M.A
	cnt = 0
	cx  = 0
	y .= 0. #TODO avoid this?
	for cy = 1:L.dim_out[2]
		cnt += 1
		cx  += 1
		length(L.A[cnt]) != 1 ? add_iir_rev!(y,L.B[cnt],L.A[cnt],x,L.SI[cnt],cx,cy) :
		add_fir_rev!(y,L.B[cnt],x,L.SI[cnt],cx,cy)

		for c2 = 2:L.dim_in[2]
			cnt += 1
			cx  += 1
			length(L.A[cnt]) != 1 ? add_iir_rev!(y,L.B[cnt],L.A[cnt],x,L.SI[cnt],cx,cy) :
			add_fir_rev!(y,L.B[cnt],x,L.SI[cnt],cx,cy)
		end
		cx = 0
	end
end

# Properties

domainType(L::MIMOFilt{T, M}) where {T, M} = T
codomainType(L::MIMOFilt{T, M}) where {T, M} = T

size(L::MIMOFilt) = L.dim_out, L.dim_in

#TODO find out a way to verify this,
# probably for IIR it means zeros inside unit circle
is_full_row_rank(L::MIMOFilt)    = true
is_full_column_rank(L::MIMOFilt) = true

fun_name(L::MIMOFilt)  = "※"

# Utilities

function add_iir!(y, b, a, x, si, coly, colx)
    silen = length(si)
    @inbounds for i=1:size(x, 1)
        xi = x[i,colx]
        val = si[1] + b[1]*xi
        for j=1:(silen-1)
            si[j] = si[j+1] + b[j+1]*xi - a[j+1]*val
        end
        si[silen] = b[silen+1]*xi - a[silen+1]*val
        y[i,coly] += val
    end
    si .= 0. #reset state
end

function add_iir_rev!(y, b, a, x, si, coly, colx)
    silen = length(si)
    @inbounds for i=size(x, 1):-1:1
        xi = x[i,colx]
        val = si[1] + b[1]*xi
        for j=1:(silen-1)
            si[j] = si[j+1] + b[j+1]*xi - a[j+1]*val
        end
        si[silen] = b[silen+1]*xi - a[silen+1]*val
        y[i,coly] += val
    end
    si .= 0.
end

function add_fir!(y, b, x, si, coly, colx)
    silen = length(si)
    @inbounds for i=1:size(x, 1)
        xi = x[i,colx]
        val = si[1] + b[1]*xi
        for j=1:(silen-1)
            si[j] = si[j+1] + b[j+1]*xi
        end
        si[silen] = b[silen+1]*xi
        y[i,coly] += val
    end
    si .= 0.
end

function add_fir_rev!(y, b, x, si, coly, colx)
    silen = length(si)
    @inbounds for i=size(x, 1):-1:1
        xi = x[i,colx]
        val = si[1] + b[1]*xi
        for j=1:(silen-1)
            si[j] = si[j+1] + b[j+1]*xi
        end
        si[silen] = b[silen+1]*xi
        y[i,coly] += val
    end
    si .= 0.
end


text:
using BinaryProvider # requires BinaryProvider 0.3.0 or later

# Parse some basic command-line arguments
const verbose = "--verbose" in ARGS
const prefix = Prefix(get([a for a in ARGS if a != "--verbose"], 1, joinpath(@__DIR__, "usr")))
products = [
    LibraryProduct(prefix, ["libgdbm"], Symbol("libgdbm")),
]

# Download binaries from hosted location
bin_prefix = "https://github.com/BenLauwens/GDBMBuilder/releases/download/v1.18"

# Listing of files generated by BinaryBuilder:
download_info = Dict(
    Linux(:aarch64, libc=:glibc) => ("$bin_prefix/GDBM.v1.18.0.aarch64-linux-gnu.tar.gz", "6296ac1c91630603cb0ee89657d73947adf359aea25e80ce127eddf274d118b9"),
    Linux(:aarch64, libc=:musl) => ("$bin_prefix/GDBM.v1.18.0.aarch64-linux-musl.tar.gz", "4db3847215e6a4f6f9b66e4dbd505e85543053d489242d915a3f9fc03a80cb00"),
    Linux(:armv7l, libc=:glibc, call_abi=:eabihf) => ("$bin_prefix/GDBM.v1.18.0.arm-linux-gnueabihf.tar.gz", "10821eb089c01acfe4570ab5d9b0f551bbc832726582dfcdfbac6ed0bcd00197"),
    Linux(:armv7l, libc=:musl, call_abi=:eabihf) => ("$bin_prefix/GDBM.v1.18.0.arm-linux-musleabihf.tar.gz", "acf378d31047b69d784562c3b53ab35bef74aef34c7000bd22846fa5713ed7ed"),
    Linux(:i686, libc=:glibc) => ("$bin_prefix/GDBM.v1.18.0.i686-linux-gnu.tar.gz", "0ea1aead5eb714c345b5202387aeb4e0695c3646ebc62914643d5aecd42fec71"),
    Linux(:i686, libc=:musl) => ("$bin_prefix/GDBM.v1.18.0.i686-linux-musl.tar.gz", "5ed7b4a5b0896f2b8b1b1591651069d36c95a76fc0f5c42fa806f02e0451514d"),
    Windows(:i686) => ("$bin_prefix/GDBM.v1.18.0.i686-w64-mingw32.tar.gz", "7a05c97dd7822724167680020e89f051c7c1683c881e4794d98882d6ea789a5f"),
    Linux(:powerpc64le, libc=:glibc) => ("$bin_prefix/GDBM.v1.18.0.powerpc64le-linux-gnu.tar.gz", "d3cf566636403dc15b05011af8d372d0af066df08d294846ff2c5cd3bfcdd5db"),
    MacOS(:x86_64) => ("$bin_prefix/GDBM.v1.18.0.x86_64-apple-darwin14.tar.gz", "0433454410db18e4336f4a3a8abd2c9cdfc8548c2b95f642333c8444c6000c9a"),
    Linux(:x86_64, libc=:glibc) => ("$bin_prefix/GDBM.v1.18.0.x86_64-linux-gnu.tar.gz", "ed6ca3bd594dd4e66e1e60de057fc17497b0596d3ea2182c1e95e9aacd16449f"),
    Linux(:x86_64, libc=:musl) => ("$bin_prefix/GDBM.v1.18.0.x86_64-linux-musl.tar.gz", "09432d35b83f472f7cee6166cd21d46c32003411d8277fefebc6547f7ca531de"),
    FreeBSD(:x86_64) => ("$bin_prefix/GDBM.v1.18.0.x86_64-unknown-freebsd11.1.tar.gz", "90f5e25d6604e25145d440552f2ef77219038595efc599f6992459f3b9ecfa6f"),
    Windows(:x86_64) => ("$bin_prefix/GDBM.v1.18.0.x86_64-w64-mingw32.tar.gz", "a72f5f7c0cfa997bed9b5ce2de216751e145790af7cbeb41fea2e4fda5440d10"),
)

# Install unsatisfied or updated dependencies:
unsatisfied = any(!satisfied(p; verbose=verbose) for p in products)
if haskey(download_info, platform_key())
    url, tarball_hash = download_info[platform_key()]
    if unsatisfied || !isinstalled(url, tarball_hash; prefix=prefix)
        # Download and install binaries
        install(url, tarball_hash; prefix=prefix, force=true, verbose=verbose)
    end
elseif unsatisfied
    # If we don't have a BinaryProvider-compatible .tar.gz to download, complain.
    # Alternatively, you could attempt to install from a separate provider,
    # build from source or something even more ambitious here.
    error("Your platform $(triplet(platform_key())) is not supported by this package!")
end

# Write out a deps.jl file that will contain mappings for our products
write_deps_file(joinpath(@__DIR__, "deps.jl"), products, verbose=verbose)

ext: jl


text:
module Reporter

using Base64
using CSV
using DataFrames
using GitHub
using JSON
using Pkg

using ..Config
using ..Utils

export log_report

const report_repo = Config.get_config("github.report_repo")
const report_branch = Config.get_config("github.report_branch", "master")
const target_repo = Config.get_config("target.repo")

if get(ENV, "TRAVIS", "false") == "true"
    const bot_token = get(ENV, "BenchmarkBot_GITHUB_TOKEN", "none")
else
    const bot_token = Config.get_config("github.token")
end

function _log_report()
    logs = Vector{String}()
    project_dir = if Base.JLOptions().project == C_NULL
        pwd()
    else
        unsafe_string(Base.JLOptions().project)
    end
    log_report = (log::String...) -> begin
        isempty(log) && return logs
        commit_id = snip7(githeadsha(project_dir))
        log = map(log) do x "[$commit_id] " * x end
        push!(logs, log...)
    end
    log_save = (file, save_path) -> begin
        log_text = join(logs, "\n")
        cd(() -> write(file, log_text), save_path)
    end
    return (log_report, log_save)
end
log_report, _log_save = _log_report()

function send(benchmark, bm_info, report_file)
    bot_auth = GitHub.authenticate(bot_token)
    name = benchmark

    # 1. commit the report
    content = open(report_file) do io read(io, String) end
    content = base64encode(content)
    params = Dict("branch" => name,
                  "message" => "[skip ci] $(name) Report",
                  "content" => content)

    commit = GitHub.create_file(report_repo, "jobs/$(name).report.md";
                                params=params, auth=bot_auth)
    commit_id = commit["commit"].sha
    report_url = "https://github.com/$report_repo/blob/$name/" *
        "jobs/$(name).report.md"

    # 2. find the tracking PR, comment on it
    all_prs = GitHub.pull_requests(report_repo; auth=bot_auth)
    tracking_pr = nothing
    for item in  all_prs[1]
        if item.title == name
            tracking_pr = item
            params = Dict(
                "body" => Utils.bm_pr_report_content(name, commit_id, report_url)
            )
            create_comment(report_repo, item.number, :pr; params=params, auth=bot_auth)
            break
        end
    end

    # 3. find the trigger issue, comment it
    issue_url = bm_info["trigger"]["issue_url"]
    user = bm_info["trigger"]["user"]
    reg = r".*://github.com/([^/]+/[^/]+)/issues/(\d+)"
    m = match(reg, issue_url)
    params = Dict(
        "body" => Utils.bm_reply1_content(name, user, report_repo, commit_id, report_url)
    )
    create_comment(m[1], parse(Int, m[2]), :issue; params=params, auth=bot_auth)
end

function send_from_travis(benchmark, triggerer_cid, report_file)
    bot_auth = GitHub.authenticate(bot_token)
    name = benchmark

    # 1. commit the report
    content = open(report_file) do io read(io, String) end
    content = base64encode(content)
    params = Dict("branch" => report_branch,
                  "message" => "[skip ci] $(name) Report",
                  "content" => content)

    commit = GitHub.create_file(report_repo, "jobs/$(name).report.md";
                                params=params, auth=bot_auth)
    commit_id = commit["commit"].sha
    report_url = "https://github.com/$report_repo/blob/$report_branch/" *
        "jobs/$(name).report.md"

    # 2. find the trigger commit, comment on it
    params = Dict(
        "body" => Utils.bm_commit_report_content(name, commit_id, report_url)
    )
    create_comment(target_repo, triggerer_cid, :commit; params=params, auth=bot_auth)
end

function send_error(benchmark, bm_info, exc)
    bot_auth = GitHub.authenticate(bot_token)
    name = benchmark

    # 1. find the tracking PR, comment on it
    all_prs = GitHub.pull_requests(report_repo; auth=bot_auth)
    tracking_pr = nothing
    for item in  all_prs[1]
        if item.title == name
            tracking_pr = item
            params = Dict("body" => Utils.bm_pr_error_content(exc))
            create_comment(report_repo, item.number, :pr; params=params, auth=bot_auth)
            break
        end
    end

    # 2. find the trigger issue, comment it
    issue_url = bm_info["trigger"]["issue_url"]
    user = bm_info["trigger"]["user"]
    reg = r".*://github.com/([^/]+/[^/]+)/issues/(\d+)"
    m = match(reg, issue_url)
    params = Dict("body" => Utils.bm_reply2_content(name, user, report_repo, exc))
    create_comment(m[1], parse(Int, m[2]), :issue; params=params, auth=bot_auth)
end

function save_result(log_data::Dict, source::String, path::String)
    if !haskey(log_data, "name")
        log_data["name"] = basename(source)
    end
    if Config.get_config("reporter.use_dataframe", false)
        return save_result(DataFrame(log_data), source, path)
    end
    result_file = Utils.result_filename(log_data) * ".json"
    cd(() -> write(result_file, JSON.json(log_data, 2)), path)
end

function save_result(log_data::DataFrame, source::String, path::String)
    if !in(:name, names(log_data))
        row_count = size(log_data)[1]
        name_col= DataFrame(name=repeat([basename(source)], row_count))
        log_data = hcat(name_col, log_data)
    end
    result_file = Utils.result_filename(log_data) * ".csv"
    cd(() -> CSV.write(result_file, log_data), path)
end

function save_log(log_data::Union{Dict, DataFrame}, path::String)
    if Config.get_config("reporter.use_dataframe", false) && isa(log_data, Dict)
        log_data = DataFrame(log_data)
    end
    log_file = Utils.result_filename(log_data) * ".log"
    _log_save(log_file, path)
end

function write_report!(bm_name, filename, branches::Vector, shas::Vector, base_branch="")
    if Config.get_config("reporter.use_dataframe", false)
        report_md = generate_report_dataframe(bm_name, branches, shas, base_branch)
    else
        report_md = generate_report(bm_name, branches, shas, base_branch)
    end
    result_path = joinpath(result_dir(bm_name), filename)
    write(result_path, report_md)
end

end


text:
import PhysicsPrimitives2D
import PhysicsPrimitives2D: PP2D
import StaticArrays
const SA = StaticArrays
import LinearAlgebra
const LA = LinearAlgebra
import Test

function test_collision_list_no_dir(collision_list_no_dir)
    for (a, b, pos_ba, value) in collision_list_no_dir
        Test.@test PP2D.is_colliding(a, b, pos_ba) == value
    end
end

function test_collision_list(collision_list)
    for (a, b, pos_ba, dir_ba, value) in collision_list
        Test.@test PP2D.is_colliding(a, b, pos_ba, dir_ba) == value
    end
end

function test_manifold_list_no_dir(manifold_list_no_dir)
    for (i, (a, b, pos_ba, value)) in enumerate(manifold_list_no_dir)
        manifold_ba = PP2D.Manifold(a, b, pos_ba)
        Test.@test PP2D.get_penetration(manifold_ba) ≈ PP2D.get_penetration(value)
        Test.@test PP2D.get_normal(manifold_ba) ≈ PP2D.get_normal(value)
        Test.@test PP2D.get_contact(manifold_ba) ≈ PP2D.get_contact(value)
    end
end

function test_manifold_list(manifold_list)
    for (i, (a, b, pos_ba, dir_ba, value)) in enumerate(manifold_list)
        manifold_ba = PP2D.Manifold(a, b, pos_ba, dir_ba)
        Test.@test PP2D.get_penetration(manifold_ba) ≈ PP2D.get_penetration(value)
        Test.@test PP2D.get_normal(manifold_ba) ≈ PP2D.get_normal(value)
        Test.@test PP2D.get_contact(manifold_ba) ≈ PP2D.get_contact(value)
    end
end

Test.@testset "PhysicsPrimitives2D.jl" begin
    T = Float32
    VecType = SA.SVector{2, T}

    origin = zero(VecType)
    d = convert(T, 0.01)

    std_dir = SA.SVector(one(T), zero(T))
    i_cap = SA.SVector(one(T), zero(T))
    j_cap = SA.SVector(zero(T), one(T))

    theta = convert(T, π / 6)
    rotated_dir = SA.SVector(cos(theta), sin(theta))
    theta_45 = convert(T, π / 4)
    unit_45 = (i_cap + j_cap) / convert(T, sqrt(2))

    point = PP2D.StdPoint{T}()

    half_length_l1 = one(T)
    l1 = PP2D.StdLine(half_length_l1)
    p1_l1 = PP2D.get_tail(l1)
    p2_l1 = PP2D.get_head(l1)

    half_length_l2 = convert(T, 2)
    l2 = PP2D.StdLine(half_length_l2)
    p1_l2 = PP2D.get_tail(l2)
    p2_l2 = PP2D.get_head(l2)

    r_c1 = one(T)
    c1 = PP2D.StdCircle(r_c1)

    r_c2 = convert(T, 2)
    c2 = PP2D.StdCircle(r_c2)

    half_width_r1 = one(T)
    half_height_r1 = convert(T, 0.5)
    r1 = PP2D.StdRect(half_width_r1, half_height_r1)
    top_right_r1 = PP2D.get_top_right(r1)
    theta_r1 = atan(half_height_r1, half_width_r1)

    half_width_r2 = convert(T, 2)
    half_height_r2 = one(T)
    r2 = PP2D.StdRect(half_width_r2, half_height_r2)
    top_right_r2 = PP2D.get_top_right(r2)
    theta_r2 = atan(half_height_r2, half_width_r2)

    Test.@testset "Area" begin
        Test.@testset "StdRect" begin
            Test.@test PP2D.get_area(r2) == convert(T, 8)
        end

        Test.@testset "StdCircle" begin
            Test.@test PP2D.get_area(c1) ≈ convert(T, π)
        end
    end

    Test.@testset "Collision detection" begin
        Test.@testset "StdLine vs. StdLine" begin
            collision_list_no_dir = [
            # std_dir
            (l1, l2, origin, true),

            (l1, l2, (half_length_l1 + half_length_l2 + d) * -i_cap, false),
            (l1, l2, (half_length_l1 + half_length_l2 - d) * -i_cap, true),
            (l1, l2, (half_length_l1 + half_length_l2 - d) * i_cap, true),
            (l1, l2, (half_length_l1 + half_length_l2 + d) * i_cap, false),

            (l1, l2, d * -j_cap, false),
            (l1, l2, d * j_cap, false),
            ]

            test_collision_list_no_dir(collision_list_no_dir)

            collision_list = [
            # std_dir
            (l1, l2, origin, std_dir, true),

            (l1, l2, (half_length_l1 + half_length_l2 + d) * -i_cap, std_dir, false),
            (l1, l2, (half_length_l1 + half_length_l2 - d) * -i_cap, std_dir, true),
            (l1, l2, (half_length_l1 + half_length_l2 - d) * i_cap, std_dir, true),
            (l1, l2, (half_length_l1 + half_length_l2 + d) * i_cap, std_dir, false),

            (l1, l2, d * -j_cap, std_dir, false),
            (l1, l2, d * j_cap, std_dir, false),

            # rotated_dir
            (l1, l2, origin, rotated_dir, true),

            (l1, l2, (half_length_l1 + half_length_l2 + d) * -i_cap, rotated_dir, false),
            (l1, l2, (half_length_l1 + half_length_l2 - d) * -i_cap, rotated_dir, false),
            (l1, l2, (half_length_l1 + d) * -i_cap, rotated_dir, false),
            (l1, l2, (half_length_l1 - d) * -i_cap, rotated_dir, true),
            (l1, l2, (half_length_l1 - d) * i_cap, rotated_dir, true),
            (l1, l2, (half_length_l1 + d) * i_cap, rotated_dir, false),
            (l1, l2, (half_length_l1 + half_length_l2 - d) * i_cap, rotated_dir, false),
            (l1, l2, (half_length_l1 + half_length_l2 + d) * i_cap, rotated_dir, false),

            (l2, l1, (half_length_l1 * sin(theta) + d) * -j_cap, rotated_dir, false),
            (l2, l1, (half_length_l1 * sin(theta) - d) * -j_cap, rotated_dir, true),
            (l2, l1, d * -j_cap, rotated_dir, true),
            (l2, l1, d * j_cap, rotated_dir, true),
            (l2, l1, (half_length_l1 * sin(theta) - d) * j_cap, rotated_dir, true),
            (l2, l1, (half_length_l1 * sin(theta) + d) * j_cap, rotated_dir, false),
            ]

            test_collision_list(collision_list)

        end

        Test.@testset "StdCircle vs. StdPoint" begin
            collision_list = [
            # std_dir
            (c1, point, origin, std_dir, true),

            (c1, point, (r_c1 + d) * -i_cap, std_dir, false),
            (c1, point, (r_c1 - d) * -i_cap, std_dir, true),
            (c1, point, (r_c1 - d) * i_cap, std_dir, true),
            (c1, point, (r_c1 + d) * i_cap, std_dir, false),

            (c1, point, (r_c1 + d) * -j_cap, std_dir, false),
            (c1, point, (r_c1 - d) * -j_cap, std_dir, true),
            (c1, point, (r_c1 - d) * j_cap, std_dir, true),
            (c1, point, (r_c1 + d) * j_cap, std_dir, false),

            (c1, point, (r_c1 + d) * unit_45, std_dir, false),
            (c1, point, (r_c1 - d) * unit_45, std_dir, true),

            # reverse check with std_dir
            (point, c1, origin, std_dir, true),

            (point, c1, (r_c1 + d) * -i_cap, std_dir, false),
            (point, c1, (r_c1 - d) * -i_cap, std_dir, true),
            (point, c1, (r_c1 - d) * i_cap, std_dir, true),
            (point, c1, (r_c1 + d) * i_cap, std_dir, false),

            (point, c1, (r_c1 + d) * -j_cap, std_dir, false),
            (point, c1, (r_c1 - d) * -j_cap, std_dir, true),
            (point, c1, (r_c1 - d) * j_cap, std_dir, true),
            (point, c1, (r_c1 + d) * j_cap, std_dir, false),

            (point, c1, (r_c1 + d) * unit_45, std_dir, false),
            (point, c1, (r_c1 - d) * unit_45, std_dir, true),
            ]

            test_collision_list(collision_list)

            collision_list_no_dir = [
            # std_dir
            (c1, point, origin, true),

            (c1, point, (r_c1 + d) * -i_cap, false),
            (c1, point, (r_c1 - d) * -i_cap, true),
            (c1, point, (r_c1 - d) * i_cap, true),
            (c1, point, (r_c1 + d) * i_cap, false),

            (c1, point, (r_c1 + d) * -j_cap, false),
            (c1, point, (r_c1 - d) * -j_cap, true),
            (c1, point, (r_c1 - d) * j_cap, true),
            (c1, point, (r_c1 + d) * j_cap, false),

            (c1, point, (r_c1 + d) * unit_45, false),
            (c1, point, (r_c1 - d) * unit_45, true),

            # reverse check with std_dir
            (point, c1, origin, true),

            (point, c1, (r_c1 + d) * -i_cap, false),
            (point, c1, (r_c1 - d) * -i_cap, true),
            (point, c1, (r_c1 - d) * i_cap, true),
            (point, c1, (r_c1 + d) * i_cap, false),

            (point, c1, (r_c1 + d) * -j_cap, false),
            (point, c1, (r_c1 - d) * -j_cap, true),
            (point, c1, (r_c1 - d) * j_cap, true),
            (point, c1, (r_c1 + d) * j_cap, false),

            (point, c1, (r_c1 + d) * unit_45, false),
            (point, c1, (r_c1 - d) * unit_45, true),
            ]

            test_collision_list_no_dir(collision_list_no_dir)
        end

        Test.@testset "StdCircle vs. StdLine" begin
            collision_list = [
            # std_dir
            (l1, c2, origin, std_dir, true),
            (l2, c1, origin, std_dir, true),

            (l2, c1, (half_length_l2 + r_c1 + d) * -i_cap, std_dir, false),
            (l2, c1, (half_length_l2 + r_c1 - d) * -i_cap, std_dir, true),
            (l2, c1, (half_length_l2 + r_c1 - d) * i_cap, std_dir, true),
            (l2, c1, (half_length_l2 + r_c1 + d) * i_cap, std_dir, false),

            (l2, c1, (r_c1 + d) * -j_cap, std_dir, false),
            (l2, c1, (r_c1 - d) * -j_cap, std_dir, true),
            (l2, c1, (r_c1 - d) * j_cap, std_dir, true),
            (l2, c1, (r_c1 + d) * j_cap, std_dir, false),

            # reverse check with std_dir
            (c2, l1, origin, std_dir, true),
            (c1, l2, origin, std_dir, true),

            (c1, l2, (half_length_l2 + r_c1 + d) * -i_cap, std_dir, false),
            (c1, l2, (half_length_l2 + r_c1 - d) * -i_cap, std_dir, true),
            (c1, l2, (half_length_l2 + r_c1 - d) * i_cap, std_dir, true),
            (c1, l2, (half_length_l2 + r_c1 + d) * i_cap, std_dir, false),

            (c1, l2, (r_c1 + d) * -j_cap, std_dir, false),
            (c1, l2, (r_c1 - d) * -j_cap, std_dir, true),
            (c1, l2, (r_c1 - d) * j_cap, std_dir, true),
            (c1, l2, (r_c1 + d) * j_cap, std_dir, false),

            # reverse check with rotated_dir
            (c2, l1, origin, rotated_dir, true),
            (c1, l2, origin, rotated_dir, true),

            (c1, l1, (sqrt(r_c1 ^ 2 - (half_length_l1 * sin(theta)) ^ 2) + half_length_l1 * cos(theta) + d) * -i_cap, rotated_dir, false),
            (c1, l1, (sqrt(r_c1 ^ 2 - (half_length_l1 * sin(theta)) ^ 2) + half_length_l1 * cos(theta) - d) * -i_cap, rotated_dir, true),
            (c1, l1, (sqrt(r_c1 ^ 2 - (half_length_l1 * sin(theta)) ^ 2) + half_length_l1 * cos(theta) - d) * i_cap, rotated_dir, true),
            (c1, l1, (sqrt(r_c1 ^ 2 - (half_length_l1 * sin(theta)) ^ 2) + half_length_l1 * cos(theta) + d) * i_cap, rotated_dir, false),

            (c1, l2, (r_c1 / cos(theta) + d) * -j_cap, rotated_dir, false),
            (c1, l2, (r_c1 / cos(theta) - d) * -j_cap, rotated_dir, true),
            (c1, l2, (r_c1 / cos(theta) - d) * j_cap, rotated_dir, true),
            (c1, l2, (r_c1 / cos(theta) + d) * j_cap, rotated_dir, false),
            ]

            test_collision_list(collision_list)

            collision_list_no_dir = [
            # std_dir
            (l1, c2, origin, true),
            (l2, c1, origin, true),

            (l2, c1, (half_length_l2 + r_c1 + d) * -i_cap, false),
            (l2, c1, (half_length_l2 + r_c1 - d) * -i_cap, true),
            (l2, c1, (half_length_l2 + r_c1 - d) * i_cap, true),
            (l2, c1, (half_length_l2 + r_c1 + d) * i_cap, false),

            (l2, c1, (r_c1 + d) * -j_cap, false),
            (l2, c1, (r_c1 - d) * -j_cap, true),
            (l2, c1, (r_c1 - d) * j_cap, true),
            (l2, c1, (r_c1 + d) * j_cap, false),

            # reverse check with std_dir
            (c2, l1, origin, true),
            (c1, l2, origin, true),

            (c1, l2, (half_length_l2 + r_c1 + d) * -i_cap, false),
            (c1, l2, (half_length_l2 + r_c1 - d) * -i_cap, true),
            (c1, l2, (half_length_l2 + r_c1 - d) * i_cap, true),
            (c1, l2, (half_length_l2 + r_c1 + d) * i_cap, false),

            (c1, l2, (r_c1 + d) * -j_cap, false),
            (c1, l2, (r_c1 - d) * -j_cap, true),
            (c1, l2, (r_c1 - d) * j_cap, true),
            (c1, l2, (r_c1 + d) * j_cap, false),
            ]

            test_collision_list_no_dir(collision_list_no_dir)
        end

        Test.@testset "StdCircle vs. StdCircle" begin
            collision_list = [
            # std_dir
            (c1, c2, origin, std_dir, true),

            (c1, c2, (r_c1 + r_c2 + d) * -i_cap, std_dir, false),
            (c1, c2, (r_c1 + r_c2 - d) * -i_cap, std_dir, true),
            (c1, c2, (r_c1 + r_c2 - d) * i_cap, std_dir, true),
            (c1, c2, (r_c1 + r_c2 + d) * i_cap, std_dir, false),

            (c1, c2, (r_c1 + r_c2 + d) * -j_cap, std_dir, false),
            (c1, c2, (r_c1 + r_c2 - d) * -j_cap, std_dir, true),
            (c1, c2, (r_c1 + r_c2 - d) * j_cap, std_dir, true),
            (c1, c2, (r_c1 + r_c2 + d) * j_cap, std_dir, false),

            (c1, c2, (r_c1 + r_c2 + d) * unit_45, std_dir, false),
            (c1, c2, (r_c1 + r_c2 - d) * unit_45, std_dir, true),
            ]

            test_collision_list(collision_list)

            collision_list_no_dir = [
            # std_dir
            (c1, c2, origin, true),

            (c1, c2, (r_c1 + r_c2 + d) * -i_cap, false),
            (c1, c2, (r_c1 + r_c2 - d) * -i_cap, true),
            (c1, c2, (r_c1 + r_c2 - d) * i_cap, true),
            (c1, c2, (r_c1 + r_c2 + d) * i_cap, false),

            (c1, c2, (r_c1 + r_c2 + d) * -j_cap, false),
            (c1, c2, (r_c1 + r_c2 - d) * -j_cap, true),
            (c1, c2, (r_c1 + r_c2 - d) * j_cap, true),
            (c1, c2, (r_c1 + r_c2 + d) * j_cap, false),

            (c1, c2, (r_c1 + r_c2 + d) * unit_45, false),
            (c1, c2, (r_c1 + r_c2 - d) * unit_45, true),
            ]

            test_collision_list_no_dir(collision_list_no_dir)
        end

        Test.@testset "StdRect vs. StdPoint" begin
            collision_list = [
            # std_dir
            (r1, point, origin, std_dir, true),

            (r1, point, (half_width_r1 + d) * -i_cap, std_dir, false),
            (r1, point, (half_width_r1 - d) * -i_cap, std_dir, true),
            (r1, point, (half_width_r1 - d) * i_cap, std_dir, true),
            (r1, point, (half_width_r1 + d) * i_cap, std_dir, false),

            (r1, point, (half_height_r1 + d) * -j_cap, std_dir, false),
            (r1, point, (half_height_r1 - d) * -j_cap, std_dir, true),
            (r1, point, (half_height_r1 - d) * j_cap, std_dir, true),
            (r1, point, (half_height_r1 + d) * j_cap, std_dir, false),

            (r1, point, top_right_r1 .+ d, std_dir, false),
            (r1, point, top_right_r1 .- d, std_dir, true),

            # reverse check with std_dir
            (point, r1, origin, std_dir, true),

            (point, r1, (half_width_r1 + d) * -i_cap, std_dir, false),
            (point, r1, (half_width_r1 - d) * -i_cap, std_dir, true),
            (point, r1, (half_width_r1 - d) * i_cap, std_dir, true),
            (point, r1, (half_width_r1 + d) * i_cap, std_dir, false),

            (point, r1, (half_height_r1 + d) * -j_cap, std_dir, false),
            (point, r1, (half_height_r1 - d) * -j_cap, std_dir, true),
            (point, r1, (half_height_r1 - d) * j_cap, std_dir, true),
            (point, r1, (half_height_r1 + d) * j_cap, std_dir, false),

            (point, r1, top_right_r1 .+ d, std_dir, false),
            (point, r1, top_right_r1 .- d, std_dir, true),

            # reverse check with rotated_dir
            (point, r1, origin, rotated_dir, true),

            (point, r1, (half_height_r1 / sin(theta) + d) * -i_cap, rotated_dir, false),
            (point, r1, (half_height_r1 / sin(theta) - d) * -i_cap, rotated_dir, true),
            (point, r1, (half_height_r1 / sin(theta) - d) * i_cap, rotated_dir, true),
            (point, r1, (half_height_r1 / sin(theta) + d) * i_cap, rotated_dir, false),

            (point, r1, (half_height_r1 / cos(theta) + d) * -j_cap, rotated_dir, false),
            (point, r1, (half_height_r1 / cos(theta) - d) * -j_cap, rotated_dir, true),
            (point, r1, (half_height_r1 / cos(theta) - d) * j_cap, rotated_dir, true),
            (point, r1, (half_height_r1 / cos(theta) + d) * j_cap, rotated_dir, false),
            ]

            test_collision_list(collision_list)

            collision_list_no_dir = [
            # std_dir
            (r1, point, origin, true),

            (r1, point, (half_width_r1 + d) * -i_cap, false),
            (r1, point, (half_width_r1 - d) * -i_cap, true),
            (r1, point, (half_width_r1 - d) * i_cap, true),
            (r1, point, (half_width_r1 + d) * i_cap, false),

            (r1, point, (half_height_r1 + d) * -j_cap, false),
            (r1, point, (half_height_r1 - d) * -j_cap, true),
            (r1, point, (half_height_r1 - d) * j_cap, true),
            (r1, point, (half_height_r1 + d) * j_cap, false),

            (r1, point, top_right_r1 .+ d, false),
            (r1, point, top_right_r1 .- d, true),

            # reverse check with std_dir
            (point, r1, origin, true),

            (point, r1, (half_width_r1 + d) * -i_cap, false),
            (point, r1, (half_width_r1 - d) * -i_cap, true),
            (point, r1, (half_width_r1 - d) * i_cap, true),
            (point, r1, (half_width_r1 + d) * i_cap, false),

            (point, r1, (half_height_r1 + d) * -j_cap, false),
            (point, r1, (half_height_r1 - d) * -j_cap, true),
            (point, r1, (half_height_r1 - d) * j_cap, true),
            (point, r1, (half_height_r1 + d) * j_cap, false),

            (point, r1, top_right_r1 .+ d, false),
            (point, r1, top_right_r1 .- d, true),
            ]

            test_collision_list_no_dir(collision_list_no_dir)
        end

        Test.@testset "StdRect vs. StdLine" begin
            collision_list = [
            # std_dir
            (r1, l1, origin, std_dir, true),
            (r1, l2, origin, std_dir, true),

            (r1, l1, (half_width_r1 + half_length_l1 + d) * -i_cap, std_dir, false),
            (r1, l1, (half_width_r1 + half_length_l1 - d) * -i_cap, std_dir, true),
            (r1, l1, (half_width_r1 + half_length_l1 - d) * i_cap, std_dir, true),
            (r1, l1, (half_width_r1 + half_length_l1 + d) * i_cap, std_dir, false),

            (r1, l1, (half_height_r1 + d) * -j_cap, std_dir, false),
            (r1, l1, (half_height_r1 - d) * -j_cap, std_dir, true),
            (r1, l1, (half_height_r1 - d) * j_cap, std_dir, true),
            (r1, l1, (half_height_r1 + d) * j_cap, std_dir, false),

            # rotated_dir
            (r1, l1, origin, rotated_dir, true),
            (r1, l2, origin, rotated_dir, true),

            (r2, l1, (half_width_r2 + half_length_l1 * cos(theta) + d) * -i_cap, rotated_dir, false),
            (r2, l1, (half_width_r2 + half_length_l1 * cos(theta) - d) * -i_cap, rotated_dir, true),
            (r2, l1, (half_width_r2 + half_length_l1 * cos(theta) - d) * i_cap, rotated_dir, true),
            (r2, l1, (half_width_r2 + half_length_l1 * cos(theta) + d) * i_cap, rotated_dir, false),

            (r2, l1, (half_height_r2 + half_length_l1 * sin(theta) + d) * -j_cap, rotated_dir, false),
            (r2, l1, (half_height_r2 + half_length_l1 * sin(theta) - d) * -j_cap, rotated_dir, true),
            (r2, l1, (half_height_r2 + half_length_l1 * sin(theta) - d) * j_cap, rotated_dir, true),
            (r2, l1, (half_height_r2 + half_length_l1 * sin(theta) + d) * j_cap, rotated_dir, false),

            # reverse check with std_dir
            (l1, r1, origin, std_dir, true),
            (l2, r1, origin, std_dir, true),

            (l1, r1, (half_width_r1 + half_length_l1 + d) * -i_cap, std_dir, false),
            (l1, r1, (half_width_r1 + half_length_l1 - d) * -i_cap, std_dir, true),
            (l1, r1, (half_width_r1 + half_length_l1 - d) * i_cap, std_dir, true),
            (l1, r1, (half_width_r1 + half_length_l1 + d) * i_cap, std_dir, false),

            (l1, r1, (half_height_r1 + d) * -j_cap, std_dir, false),
            (l1, r1, (half_height_r1 - d) * -j_cap, std_dir, true),
            (l1, r1, (half_height_r1 - d) * j_cap, std_dir, true),
            (l1, r1, (half_height_r1 + d) * j_cap, std_dir, false),

            # reverse check with rotated_dir
            (l1, r1, origin, rotated_dir, true),
            (l2, r1, origin, rotated_dir, true),

            (l2, r2, (half_height_r2 / sin(theta) + half_length_l2 + d) * -i_cap, rotated_dir, false),
            (l2, r2, (half_height_r2 / sin(theta) + half_length_l2 - d) * -i_cap, rotated_dir, true),
            (l2, r2, (half_height_r2 / sin(theta) + half_length_l2 - d) * i_cap, rotated_dir, true),
            (l2, r2, (half_height_r2 / sin(theta) + half_length_l2 + d) * i_cap, rotated_dir, false),

            (l2, r2, (half_width_r2 * sin(theta) + half_height_r2 * cos(theta) + d) * -j_cap, rotated_dir, false),
            (l2, r2, (half_width_r2 * sin(theta) + half_height_r2 * cos(theta) - d) * -j_cap, rotated_dir, true),
            (l2, r2, (half_width_r2 * sin(theta) + half_height_r2 * cos(theta) - d) * j_cap, rotated_dir, true),
            (l2, r2, (half_width_r2 * sin(theta) + half_height_r2 * cos(theta) + d) * j_cap, rotated_dir, false),
            ]

            test_collision_list(collision_list)

            collision_list_no_dir = [
            # std_dir
            (r1, l1, origin, true),
            (r1, l2, origin, true),

            (r1, l1, (half_width_r1 + half_length_l1 + d) * -i_cap, false),
            (r1, l1, (half_width_r1 + half_length_l1 - d) * -i_cap, true),
            (r1, l1, (half_width_r1 + half_length_l1 - d) * i_cap, true),
            (r1, l1, (half_width_r1 + half_length_l1 + d) * i_cap, false),

            (r1, l1, (half_height_r1 + d) * -j_cap, false),
            (r1, l1, (half_height_r1 - d) * -j_cap, true),
            (r1, l1, (half_height_r1 - d) * j_cap, true),
            (r1, l1, (half_height_r1 + d) * j_cap, false),

            # reverse check with std_dir
            (l1, r1, origin, true),
            (l2, r1, origin, true),

            (l1, r1, (half_width_r1 + half_length_l1 + d) * -i_cap, false),
            (l1, r1, (half_width_r1 + half_length_l1 - d) * -i_cap, true),
            (l1, r1, (half_width_r1 + half_length_l1 - d) * i_cap, true),
            (l1, r1, (half_width_r1 + half_length_l1 + d) * i_cap, false),

            (l1, r1, (half_height_r1 + d) * -j_cap, false),
            (l1, r1, (half_height_r1 - d) * -j_cap, true),
            (l1, r1, (half_height_r1 - d) * j_cap, true),
            (l1, r1, (half_height_r1 + d) * j_cap, false),
            ]

            test_collision_list_no_dir(collision_list_no_dir)
        end

        Test.@testset "StdRect vs. StdCircle" begin
            collision_list = [
            # std_dir
            (r1, c1, origin, std_dir, true),

            (r1, c1, (half_width_r1 + r_c1 + d) * -i_cap, std_dir, false),
            (r1, c1, (half_width_r1 + r_c1 - d) * -i_cap, std_dir, true),
            (r1, c1, (half_width_r1 + r_c1 - d) * i_cap, std_dir, true),
            (r1, c1, (half_width_r1 + r_c1 + d) * i_cap, std_dir, false),

            (r1, c1, (half_height_r1 + r_c1 + d) * -j_cap, std_dir, false),
            (r1, c1, (half_height_r1 + r_c1 - d) * -j_cap, std_dir, true),
            (r1, c1, (half_height_r1 + r_c1 - d) * j_cap, std_dir, true),
            (r1, c1, (half_height_r1 + r_c1 + d) * -j_cap, std_dir, false),

            (r1, c1, top_right_r1 + (r_c1 + d) * unit_45, std_dir, false),
            (r1, c1, top_right_r1 + (r_c1 - d) * unit_45, std_dir, true),

            # reverse check with std_dir
            (c1, r1, origin, std_dir, true),

            (c1, r1, (half_width_r1 + r_c1 + d) * -i_cap, std_dir, false),
            (c1, r1, (half_width_r1 + r_c1 - d) * -i_cap, std_dir, true),
            (c1, r1, (half_width_r1 + r_c1 - d) * i_cap, std_dir, true),
            (c1, r1, (half_width_r1 + r_c1 + d) * i_cap, std_dir, false),

            (c1, r1, (half_height_r1 + r_c1 + d) * -j_cap, std_dir, false),
            (c1, r1, (half_height_r1 + r_c1 - d) * -j_cap, std_dir, true),
            (c1, r1, (half_height_r1 + r_c1 - d) * j_cap, std_dir, true),
            (c1, r1, (half_height_r1 + r_c1 + d) * -j_cap, std_dir, false),

            (c1, r1, top_right_r1 + (r_c1 + d) * unit_45, std_dir, false),
            (c1, r1, top_right_r1 + (r_c1 - d) * unit_45, std_dir, true),

            # reverse check with rotated_dir
            (c2, r2, origin, rotated_dir, true),

            (c2, r2, (sqrt(r_c2 ^ 2 - (half_width_r2 * sin(theta) - half_height_r2 * cos(theta)) ^ 2) + half_width_r2 * cos(theta) + half_height_r2 * sin(theta) + d) * -i_cap, rotated_dir, false),
            (c2, r2, (sqrt(r_c2 ^ 2 - (half_width_r2 * sin(theta) - half_height_r2 * cos(theta)) ^ 2) + half_width_r2 * cos(theta) + half_height_r2 * sin(theta) - d) * -i_cap, rotated_dir, true),
            (c2, r2, (sqrt(r_c2 ^ 2 - (half_width_r2 * sin(theta) - half_height_r2 * cos(theta)) ^ 2) + half_width_r2 * cos(theta) + half_height_r2 * sin(theta) - d) * i_cap, rotated_dir, true),
            (c2, r2, (sqrt(r_c2 ^ 2 - (half_width_r2 * sin(theta) - half_height_r2 * cos(theta)) ^ 2) + half_width_r2 * cos(theta) + half_height_r2 * sin(theta) + d) * i_cap, rotated_dir, false),

            (c2, r2, ((r_c2 + half_height_r2) / cos(theta) + d) * -j_cap, rotated_dir, false),
            (c2, r2, ((r_c2 + half_height_r2) / cos(theta) - d) * -j_cap, rotated_dir, true),
            (c2, r2, ((r_c2 + half_height_r2) / cos(theta) - d) * j_cap, rotated_dir, true),
            (c2, r2, ((r_c2 + half_height_r2) / cos(theta) + d) * j_cap, rotated_dir, false),
            ]

            test_collision_list(collision_list)

            collision_list_no_dir = [
            # std_dir
            (r1, c1, origin, true),

            (r1, c1, (half_width_r1 + r_c1 + d) * -i_cap, false),
            (r1, c1, (half_width_r1 + r_c1 - d) * -i_cap, true),
            (r1, c1, (half_width_r1 + r_c1 - d) * i_cap, true),
            (r1, c1, (half_width_r1 + r_c1 + d) * i_cap, false),

            (r1, c1, (half_height_r1 + r_c1 + d) * -j_cap, false),
            (r1, c1, (half_height_r1 + r_c1 - d) * -j_cap, true),
            (r1, c1, (half_height_r1 + r_c1 - d) * j_cap, true),
            (r1, c1, (half_height_r1 + r_c1 + d) * -j_cap, false),

            (r1, c1, top_right_r1 + (r_c1 + d) * unit_45, false),
            (r1, c1, top_right_r1 + (r_c1 - d) * unit_45, true),

            # reverse check with std_dir
            (c1, r1, origin, true),

            (c1, r1, (half_width_r1 + r_c1 + d) * -i_cap, false),
            (c1, r1, (half_width_r1 + r_c1 - d) * -i_cap, true),
            (c1, r1, (half_width_r1 + r_c1 - d) * i_cap, true),
            (c1, r1, (half_width_r1 + r_c1 + d) * i_cap, false),

            (c1, r1, (half_height_r1 + r_c1 + d) * -j_cap, false),
            (c1, r1, (half_height_r1 + r_c1 - d) * -j_cap, true),
            (c1, r1, (half_height_r1 + r_c1 - d) * j_cap, true),
            (c1, r1, (half_height_r1 + r_c1 + d) * -j_cap, false),

            (c1, r1, top_right_r1 + (r_c1 + d) * unit_45, false),
            (c1, r1, top_right_r1 + (r_c1 - d) * unit_45, true),
            ]

            test_collision_list_no_dir(collision_list_no_dir)
        end

        Test.@testset "Rect2D vs. Rect2D" begin
            collision_list = [
            # std_dir
            (r2, r1, origin, std_dir, true),

            (r2, r1, (half_width_r1 + half_width_r2 + d) * -i_cap, std_dir, false),
            (r2, r1, (half_width_r1 + half_width_r2 - d) * -i_cap, std_dir, true),
            (r2, r1, (half_width_r1 + half_width_r2 - d) * i_cap, std_dir, true),
            (r2, r1, (half_width_r1 + half_width_r2 + d) * i_cap, std_dir, false),

            (r2, r1, (half_height_r1 + half_height_r2 + d) * -j_cap, std_dir, false),
            (r2, r1, (half_height_r1 + half_height_r2 - d) * -j_cap, std_dir, true),
            (r2, r1, (half_height_r1 + half_height_r2 - d) * j_cap, std_dir, true),
            (r2, r1, (half_height_r1 + half_height_r2 + d) * j_cap, std_dir, false),

            (r2, r1, top_right_r1 + top_right_r2 .+ d, std_dir, false),
            (r2, r1, top_right_r1 + top_right_r2 .- d, std_dir, true),

            # rotated_dir
            (r2, r1, origin, rotated_dir, true),

            (r2, r1, (half_width_r2 + half_width_r1 * cos(theta) + half_height_r1 * sin(theta) + d) * -i_cap, rotated_dir, false),
            (r2, r1, (half_width_r2 + half_width_r1 * cos(theta) + half_height_r1 * sin(theta) - d) * -i_cap, rotated_dir, true),
            (r2, r1, (half_width_r2 + half_width_r1 * cos(theta) + half_height_r1 * sin(theta) - d) * i_cap, rotated_dir, true),
            (r2, r1, (half_width_r2 + half_width_r1 * cos(theta) + half_height_r1 * sin(theta) + d) * i_cap, rotated_dir, false),

            (r2, r1, (half_height_r2 + half_width_r1 * sin(theta) + half_height_r1 * cos(theta) + d) * -j_cap, rotated_dir, false),
            (r2, r1, (half_height_r2 + half_width_r1 * sin(theta) + half_height_r1 * cos(theta) - d) * -j_cap, rotated_dir, true),
            (r2, r1, (half_height_r2 + half_width_r1 * sin(theta) + half_height_r1 * cos(theta) - d) * j_cap, rotated_dir, true),
            (r2, r1, (half_height_r2 + half_width_r1 * sin(theta) + half_height_r1 * cos(theta) + d) * j_cap, rotated_dir, false),
            ]

            test_collision_list(collision_list)

            collision_list_no_dir = [
            # std_dir
            (r2, r1, origin, true),

            (r2, r1, (half_width_r1 + half_width_r2 + d) * -i_cap, false),
            (r2, r1, (half_width_r1 + half_width_r2 - d) * -i_cap, true),
            (r2, r1, (half_width_r1 + half_width_r2 - d) * i_cap, true),
            (r2, r1, (half_width_r1 + half_width_r2 + d) * i_cap, false),

            (r2, r1, (half_height_r1 + half_height_r2 + d) * -j_cap, false),
            (r2, r1, (half_height_r1 + half_height_r2 - d) * -j_cap, true),
            (r2, r1, (half_height_r1 + half_height_r2 - d) * j_cap, true),
            (r2, r1, (half_height_r1 + half_height_r2 + d) * j_cap, false),

            (r2, r1, top_right_r1 + top_right_r2 .+ d, false),
            (r2, r1, top_right_r1 + top_right_r2 .- d, true),
            ]

            test_collision_list_no_dir(collision_list_no_dir)
        end
    end

    Test.@testset "Manifold generation" begin
        Test.@testset "StdCircle vs. StdCircle" begin
            manifold_list = [
            # std_dir
            (c1, c2, (r_c1 + r_c2 - d) * -i_cap, std_dir, PP2D.Manifold(d, -i_cap, (r_c1 - d / 2) * -i_cap)),
            (c1, c2, r_c2 * -i_cap, std_dir, PP2D.Manifold(r_c1, -i_cap, (r_c1 / 2) * -i_cap)),
            (c1, c2, r_c2 * i_cap, std_dir, PP2D.Manifold(r_c1, i_cap, (r_c1 / 2) * i_cap)),
            (c1, c2, (r_c1 + r_c2 - d) * i_cap, std_dir, PP2D.Manifold(d, i_cap, (r_c1 - d / 2) * i_cap)),

            (c1, c2, (r_c1 + r_c2 - d) * -j_cap, std_dir, PP2D.Manifold(d, -j_cap, (r_c1 - d / 2) * -j_cap)),
            (c1, c2, r_c2 * -j_cap, std_dir, PP2D.Manifold(r_c1, -j_cap, (r_c1 / 2) * -j_cap)),
            (c1, c2, r_c2 * j_cap, std_dir, PP2D.Manifold(r_c1, j_cap, (r_c1 / 2) * j_cap)),
            (c1, c2, (r_c1 + r_c2 - d) * j_cap, std_dir, PP2D.Manifold(d, j_cap, (r_c1 - d / 2) * j_cap)),

            (c1, c2, (r_c1 + r_c2 - d) * unit_45, std_dir, PP2D.Manifold(d, unit_45, (r_c1 - d / 2) * unit_45)),
            (c1, c2, r_c2 * unit_45, std_dir, PP2D.Manifold(r_c1, unit_45, (r_c1 / 2) * unit_45)),
            ]

            test_manifold_list(manifold_list)
        end

        Test.@testset "StdRect vs. StdCircle" begin
            manifold_list_no_dir = [
            # std_dir
            (r1, c1, (half_width_r1 + r_c1 - d) * -i_cap, PP2D.Manifold(d, -i_cap, (half_width_r1 - d / 2) * -i_cap)),
            (r1, c1, (half_width_r1 + d) * -i_cap, PP2D.Manifold(r_c1 - d, -i_cap, (half_width_r1 - (r_c1 - d) / 2) * -i_cap)),
            (r1, c1, (half_width_r1 - d) * -i_cap, PP2D.Manifold(r_c1 + d, -i_cap, (half_width_r1 - (r_c1 + d) / 2) * -i_cap)),
            (r1, c1, (half_width_r1 - d) * i_cap, PP2D.Manifold(r_c1 + d, i_cap, (half_width_r1 - (r_c1 + d) / 2) * i_cap)),
            (r1, c1, (half_width_r1 + d) * i_cap, PP2D.Manifold(r_c1 - d, i_cap, (half_width_r1 - (r_c1 - d) / 2) * i_cap)),
            (r1, c1, (half_width_r1 + r_c1 - d) * i_cap, PP2D.Manifold(d, i_cap, (half_width_r1 - d / 2) * i_cap)),

            (r1, c1, (half_height_r1 + r_c1 - d) * -j_cap, PP2D.Manifold(d, -j_cap, (half_height_r1 - d / 2) * -j_cap)),
            (r1, c1, (half_height_r1 + d) * -j_cap, PP2D.Manifold(r_c1 - d, -j_cap, (half_height_r1 - (r_c1 - d) / 2) * -j_cap)),
            (r1, c1, (half_height_r1 - d) * -j_cap, PP2D.Manifold(r_c1 + d, -j_cap, (half_height_r1 - (r_c1 + d) / 2) * -j_cap)),
            (r1, c1, (half_height_r1 - d) * j_cap, PP2D.Manifold(r_c1 + d, j_cap, (half_height_r1 - (r_c1 + d) / 2) * j_cap)),
            (r1, c1, (half_height_r1 + d) * j_cap, PP2D.Manifold(r_c1 - d, j_cap, (half_height_r1 - (r_c1 - d) / 2) * j_cap)),
            (r1, c1, (half_height_r1 + r_c1 - d) * j_cap, PP2D.Manifold(d, j_cap, (half_height_r1 - d / 2) * j_cap)),

            (r1, c1, top_right_r1 + (r_c1 - d) * unit_45, PP2D.Manifold(d, unit_45, top_right_r1 + (d / 2) * -unit_45)),

            # reverse check with std_dir
            (c1, r1, (half_width_r1 + r_c1 - d) * -i_cap, PP2D.Manifold(d, -i_cap, (r_c1 - d / 2) * -i_cap)),
            (c1, r1, (half_width_r1 + d) * -i_cap, PP2D.Manifold(r_c1 - d, -i_cap, (r_c1 - (r_c1 - d) / 2) * -i_cap)),
            (c1, r1, (half_width_r1 - d) * -i_cap, PP2D.Manifold(r_c1 + d, -i_cap, (r_c1 - (r_c1 + d) / 2) * -i_cap)),
            (c1, r1, (half_width_r1 - d) * i_cap, PP2D.Manifold(r_c1 + d, i_cap, (r_c1 - (r_c1 + d) / 2) * i_cap)),
            (c1, r1, (half_width_r1 + d) * i_cap, PP2D.Manifold(r_c1 - d, i_cap, (r_c1 - (r_c1 - d) / 2) * i_cap)),
            (c1, r1, (half_width_r1 + r_c1 - d) * i_cap, PP2D.Manifold(d, i_cap, (r_c1 - d / 2) * i_cap)),

            (c1, r1, (half_height_r1 + r_c1 - d) * -j_cap, PP2D.Manifold(d, -j_cap, (r_c1 - d / 2) * -j_cap)),
            (c1, r1, (half_height_r1 + d) * -j_cap, PP2D.Manifold(r_c1 - d, -j_cap, (r_c1 - (r_c1 - d) / 2) * -j_cap)),
            (c1, r1, (half_height_r1 - d) * -j_cap, PP2D.Manifold(r_c1 + d, -j_cap, (r_c1 - (r_c1 + d) / 2) * -j_cap)),
            (c1, r1, (half_height_r1 - d) * j_cap, PP2D.Manifold(r_c1 + d, j_cap, (r_c1 - (r_c1 + d) / 2) * j_cap)),
            (c1, r1, (half_height_r1 + d) * j_cap, PP2D.Manifold(r_c1 - d, j_cap, (r_c1 - (r_c1 - d) / 2) * j_cap)),
            (c1, r1, (half_height_r1 + r_c1 - d) * j_cap, PP2D.Manifold(d, j_cap, (r_c1 - d / 2) * j_cap)),

            (c1, r1, top_right_r1 + (r_c1 - d) * unit_45, PP2D.Manifold(d, unit_45, (r_c1 - d / 2) * unit_45)),
            ]

            test_manifold_list_no_dir(manifold_list_no_dir)

            manifold_list = [
            # std_dir
            (r1, c1, (half_width_r1 + r_c1 - d) * -i_cap, std_dir, PP2D.Manifold(d, -i_cap, (half_width_r1 - d / 2) * -i_cap)),
            (r1, c1, (half_width_r1 + d) * -i_cap, std_dir, PP2D.Manifold(r_c1 - d, -i_cap, (half_width_r1 - (r_c1 - d) / 2) * -i_cap)),
            (r1, c1, (half_width_r1 - d) * -i_cap, std_dir, PP2D.Manifold(r_c1 + d, -i_cap, (half_width_r1 - (r_c1 + d) / 2) * -i_cap)),
            (r1, c1, (half_width_r1 - d) * i_cap, std_dir, PP2D.Manifold(r_c1 + d, i_cap, (half_width_r1 - (r_c1 + d) / 2) * i_cap)),
            (r1, c1, (half_width_r1 + d) * i_cap, std_dir, PP2D.Manifold(r_c1 - d, i_cap, (half_width_r1 - (r_c1 - d) / 2) * i_cap)),
            (r1, c1, (half_width_r1 + r_c1 - d) * i_cap, std_dir, PP2D.Manifold(d, i_cap, (half_width_r1 - d / 2) * i_cap)),

            (r1, c1, (half_height_r1 + r_c1 - d) * -j_cap, std_dir, PP2D.Manifold(d, -j_cap, (half_height_r1 - d / 2) * -j_cap)),
            (r1, c1, (half_height_r1 + d) * -j_cap, std_dir, PP2D.Manifold(r_c1 - d, -j_cap, (half_height_r1 - (r_c1 - d) / 2) * -j_cap)),
            (r1, c1, (half_height_r1 - d) * -j_cap, std_dir, PP2D.Manifold(r_c1 + d, -j_cap, (half_height_r1 - (r_c1 + d) / 2) * -j_cap)),
            (r1, c1, (half_height_r1 - d) * j_cap, std_dir, PP2D.Manifold(r_c1 + d, j_cap, (half_height_r1 - (r_c1 + d) / 2) * j_cap)),
            (r1, c1, (half_height_r1 + d) * j_cap, std_dir, PP2D.Manifold(r_c1 - d, j_cap, (half_height_r1 - (r_c1 - d) / 2) * j_cap)),
            (r1, c1, (half_height_r1 + r_c1 - d) * j_cap, std_dir, PP2D.Manifold(d, j_cap, (half_height_r1 - d / 2) * j_cap)),

            (r1, c1, top_right_r1 + (r_c1 - d) * unit_45, std_dir, PP2D.Manifold(d, unit_45, top_right_r1 + (d / 2) * -unit_45)),

            # reverse check with std_dir
            (c1, r1, (half_width_r1 + r_c1 - d) * -i_cap, std_dir, PP2D.Manifold(d, -i_cap, (r_c1 - d / 2) * -i_cap)),
            (c1, r1, (half_width_r1 + d) * -i_cap, std_dir, PP2D.Manifold(r_c1 - d, -i_cap, (r_c1 - (r_c1 - d) / 2) * -i_cap)),
            (c1, r1, (half_width_r1 - d) * -i_cap, std_dir, PP2D.Manifold(r_c1 + d, -i_cap, (r_c1 - (r_c1 + d) / 2) * -i_cap)),
            (c1, r1, (half_width_r1 - d) * i_cap, std_dir, PP2D.Manifold(r_c1 + d, i_cap, (r_c1 - (r_c1 + d) / 2) * i_cap)),
            (c1, r1, (half_width_r1 + d) * i_cap, std_dir, PP2D.Manifold(r_c1 - d, i_cap, (r_c1 - (r_c1 - d) / 2) * i_cap)),
            (c1, r1, (half_width_r1 + r_c1 - d) * i_cap, std_dir, PP2D.Manifold(d, i_cap, (r_c1 - d / 2) * i_cap)),

            (c1, r1, (half_height_r1 + r_c1 - d) * -j_cap, std_dir, PP2D.Manifold(d, -j_cap, (r_c1 - d / 2) * -j_cap)),
            (c1, r1, (half_height_r1 + d) * -j_cap, std_dir, PP2D.Manifold(r_c1 - d, -j_cap, (r_c1 - (r_c1 - d) / 2) * -j_cap)),
            (c1, r1, (half_height_r1 - d) * -j_cap, std_dir, PP2D.Manifold(r_c1 + d, -j_cap, (r_c1 - (r_c1 + d) / 2) * -j_cap)),
            (c1, r1, (half_height_r1 - d) * j_cap, std_dir, PP2D.Manifold(r_c1 + d, j_cap, (r_c1 - (r_c1 + d) / 2) * j_cap)),
            (c1, r1, (half_height_r1 + d) * j_cap, std_dir, PP2D.Manifold(r_c1 - d, j_cap, (r_c1 - (r_c1 - d) / 2) * j_cap)),
            (c1, r1, (half_height_r1 + r_c1 - d) * j_cap, std_dir, PP2D.Manifold(d, j_cap, (r_c1 - d / 2) * j_cap)),

            (c1, r1, top_right_r1 + (r_c1 - d) * unit_45, std_dir, PP2D.Manifold(d, unit_45, (r_c1 - d / 2) * unit_45)),

            # reverse check with rotated_dir
            (c1, r1, (r_c1 - d) * unit_45 + PP2D.rotate(top_right_r1, rotated_dir), rotated_dir, PP2D.Manifold(d, unit_45, (r_c1 - d / 2) * unit_45)),
            ]

            test_manifold_list(manifold_list)
        end

        Test.@testset "Rect2D vs. Rect2D" begin
            manifold_list_no_dir = [
            # std_dir
            (r2, r1, (half_height_r2 + half_height_r1 - d) * -j_cap, PP2D.Manifold(d, -j_cap, (half_height_r2 - d/2) * -j_cap)),
            (r2, r1, (half_height_r2 + d) * -j_cap, PP2D.Manifold(half_height_r1 - d, -j_cap, (half_height_r2 - (half_height_r1 - d)/2) * -j_cap)),
            (r2, r1, (half_height_r2 - d) * -j_cap, PP2D.Manifold(half_height_r1 + d, -j_cap, (half_height_r2 - (half_height_r1 + d)/2) * -j_cap)),
            (r2, r1, d * -j_cap, PP2D.Manifold(half_height_r2 + half_height_r1 - d, -j_cap, d * -j_cap)),
            (r2, r1, d * j_cap, PP2D.Manifold(half_height_r2 + half_height_r1 - d, j_cap, d * j_cap)),
            (r2, r1, (half_height_r2 - d) * j_cap, PP2D.Manifold(half_height_r1 + d, j_cap, (half_height_r2 - (half_height_r1 + d)/2) * j_cap)),
            (r2, r1, (half_height_r2 + d) * j_cap, PP2D.Manifold(half_height_r1 - d, j_cap, (half_height_r2 - (half_height_r1 - d)/2) * j_cap)),
            (r2, r1, (half_height_r2 + half_height_r1 - d) * j_cap, PP2D.Manifold(d, j_cap, (half_height_r2 - d/2) * j_cap)),

            (r2, r1, (half_width_r2 + half_width_r1 - d) * -i_cap, PP2D.Manifold(d, -i_cap, (half_width_r2 - d/2) * -i_cap)),
            (r2, r1, (half_width_r2 + d) * -i_cap, PP2D.Manifold(half_width_r1 - d, -i_cap, (half_width_r2 - (half_width_r1 - d)/2) * -i_cap)),
            (r2, r1, (half_width_r2 - d) * -i_cap, PP2D.Manifold(half_width_r1 + d, -i_cap, (half_width_r2 - (half_width_r1 + d)/2) * -i_cap)),
            (r2, r1, (half_width_r2 - d) * i_cap, PP2D.Manifold(half_width_r1 + d, i_cap, (half_width_r2 - (half_width_r1 + d)/2) * i_cap)),
            (r2, r1, (half_width_r2 + d) * i_cap, PP2D.Manifold(half_width_r1 - d, i_cap, (half_width_r2 - (half_width_r1 - d)/2) * i_cap)),
            (r2, r1, (half_width_r2 + half_width_r1 - d) * i_cap, PP2D.Manifold(d, i_cap, (half_width_r2 - d/2) * i_cap)),

            (r2, r1, top_right_r2 .- d, PP2D.Manifold(half_height_r1 + d, j_cap, top_right_r2 + (half_width_r1 + d)/2 * -i_cap + (half_height_r1 + d)/2 * -j_cap)),
            (r2, r1, top_right_r2, PP2D.Manifold(half_height_r1, j_cap, top_right_r2 + (half_width_r1/2) * -i_cap + (half_height_r1/2) * -j_cap)),
            (r2, r1, top_right_r2 .+ d, PP2D.Manifold(half_height_r1 - d, j_cap, top_right_r2 + (half_width_r1 - d)/2 * -i_cap + (half_height_r1 - d)/2 * -j_cap)),
            ]

            test_manifold_list_no_dir(manifold_list_no_dir)

            manifold_list = [
            # std_dir
            (r2, r1, (half_height_r2 + half_height_r1 - d) * -j_cap, std_dir, PP2D.Manifold(d, -j_cap, (half_height_r2 - d/2) * -j_cap)),
            (r2, r1, (half_height_r2 + d) * -j_cap, std_dir, PP2D.Manifold(half_height_r1 - d, -j_cap, (half_height_r2 - (half_height_r1 - d)/2) * -j_cap)),
            (r2, r1, (half_height_r2 - d) * -j_cap, std_dir, PP2D.Manifold(half_height_r1 + d, -j_cap, (half_height_r2 - (half_height_r1 + d)/2) * -j_cap)),
            (r2, r1, d * -j_cap, std_dir, PP2D.Manifold(half_height_r2 + half_height_r1 - d, -j_cap, d * -j_cap)),
            (r2, r1, d * j_cap, std_dir, PP2D.Manifold(half_height_r2 + half_height_r1 - d, j_cap, d * j_cap)),
            (r2, r1, (half_height_r2 - d) * j_cap, std_dir, PP2D.Manifold(half_height_r1 + d, j_cap, (half_height_r2 - (half_height_r1 + d)/2) * j_cap)),
            (r2, r1, (half_height_r2 + d) * j_cap, std_dir, PP2D.Manifold(half_height_r1 - d, j_cap, (half_height_r2 - (half_height_r1 - d)/2) * j_cap)),
            (r2, r1, (half_height_r2 + half_height_r1 - d) * j_cap, std_dir, PP2D.Manifold(d, j_cap, (half_height_r2 - d/2) * j_cap)),

            (r2, r1, (half_width_r2 + half_width_r1 - d) * -i_cap, std_dir, PP2D.Manifold(d, -i_cap, (half_width_r2 - d/2) * -i_cap)),
            (r2, r1, (half_width_r2 + d) * -i_cap, std_dir, PP2D.Manifold(half_width_r1 - d, -i_cap, (half_width_r2 - (half_width_r1 - d)/2) * -i_cap)),
            (r2, r1, (half_width_r2 - d) * -i_cap, std_dir, PP2D.Manifold(half_width_r1 + d, -i_cap, (half_width_r2 - (half_width_r1 + d)/2) * -i_cap)),
            (r2, r1, (half_width_r2 - d) * i_cap, std_dir, PP2D.Manifold(half_width_r1 + d, i_cap, (half_width_r2 - (half_width_r1 + d)/2) * i_cap)),
            (r2, r1, (half_width_r2 + d) * i_cap, std_dir, PP2D.Manifold(half_width_r1 - d, i_cap, (half_width_r2 - (half_width_r1 - d)/2) * i_cap)),
            (r2, r1, (half_width_r2 + half_width_r1 - d) * i_cap, std_dir, PP2D.Manifold(d, i_cap, (half_width_r2 - d/2) * i_cap)),

            (r2, r1, top_right_r2 .- d, std_dir, PP2D.Manifold(half_height_r1 + d, j_cap, top_right_r2 + (half_width_r1 + d)/2 * -i_cap + (half_height_r1 + d)/2 * -j_cap)),
            (r2, r1, top_right_r2, std_dir, PP2D.Manifold(half_height_r1, j_cap, top_right_r2 + (half_width_r1/2) * -i_cap + (half_height_r1/2) * -j_cap)),
            (r2, r1, top_right_r2 .+ d, std_dir, PP2D.Manifold(half_height_r1 - d, j_cap, top_right_r2 + (half_width_r1 - d)/2 * -i_cap + (half_height_r1 - d)/2 * -j_cap)),

            # rotated_dir
            (r2, r1, (half_height_r2 - d) * -j_cap - PP2D.rotate(top_right_r1, rotated_dir), rotated_dir, PP2D.Manifold(d, -j_cap, ((zero(T) - d / tan(theta) + d * tan(theta)) / 3) * i_cap + ((-half_height_r2 + d - half_height_r2 - half_height_r2) / 3) * j_cap)),
            (r2, r1, (half_height_r2 - d) * j_cap + PP2D.rotate(top_right_r1, rotated_dir), rotated_dir, PP2D.Manifold(d, j_cap, ((zero(T) + d / tan(theta) - d * tan(theta)) / 3) * i_cap + ((half_height_r2 - d + half_height_r2 + half_height_r2) / 3) * j_cap)),

            (r2, r1, (half_width_r2 + LA.norm(top_right_r1) * cos(-theta_r1 + theta) - d) * -i_cap, rotated_dir, PP2D.Manifold(d, -i_cap, ((-half_width_r2 + d - half_width_r2 - half_width_r2) / 3) * i_cap + (LA.norm(top_right_r1) * sin(-theta_r1 + theta) + (d / tan(theta) - d * tan(theta)) / 3) * j_cap)),
            (r2, r1, (half_width_r2 + LA.norm(top_right_r1) * cos(-theta_r1 + theta) - d) * i_cap, rotated_dir, PP2D.Manifold(d, i_cap, ((half_width_r2 - d + half_width_r2 + half_width_r2) / 3) * i_cap + (LA.norm(top_right_r1) * sin(convert(T, pi - theta_r1) + theta) + (d * tan(theta) - d / tan(theta)) / 3) * j_cap)),
            ]

            test_manifold_list(manifold_list)
        end
    end
end


text:
module ORInterface

abstract type Problem end
abstract type Constraint end
abstract type Solution end
abstract type Status end

export 	Problem,
        Constraint,
        Solution,
        Status

end


text:
#towel map:
println("Testing discrete system evolution...")
using StaticArrays, Base.Test, DynamicalSystems

@testset "Logistic Map" begin

  d1 = Systems.logistic(0.1)
  d2 = DiscreteDS1D(0.1, d1.eom)
  d3 = DiscreteDS1D(big(0.1), d1.eom, d1.deriv)

  @testset "Evolution & Timeseries" begin
    dd1 = evolve!(d1)
    dd2 = evolve!(d2)
    dd3 = evolve!(d3)
    @test dd1.state == dd2.state
    @test dd1.state ≈ dd3.state
    @test typeof(dd3.state) == BigFloat
    ts1 = timeseries(dd1, 100, mutate = false)
    ts3 = timeseries(dd3, 100, mutate = false)
    @test ts1[10] ≈ ts3[10]
    @test eltype(ts3) == BigFloat
  end
  @testset "Derivatives" begin
    f1 = d1.deriv(d1.state)
    f2 = d2.deriv(d2.state)
    f3 = d3.deriv(d3.state)
    @test isapprox(f1, f2;rtol = 1e-12)
    @test isapprox(f1, f3;rtol = 1e-12)
    @test typeof(f3) == BigFloat
  end
end

@testset "Folded-Towel Map" begin

  s1 = Systems.towel(0.1ones(3))
  s2 = DiscreteDS(0.1ones(3), s1.eom)
  s4 = DiscreteDS(round.(big.(0.1ones(3)),3), s1.eom, s1.jacob)

  @testset "Evolution & Timeseries" begin
    st1 = evolve(s1)
    st2 = evolve(s2)
    st4 = evolve(s4)

    @test isapprox.(st1, st2; rtol = 1e-12) == trues(s1.state)
    @test isapprox.(st1, st4; rtol = 1e-12) == trues(s1.state)

    evolve!(s1); evolve!(s2); evolve!(s4);
    @test s1.state ≈ s2.state
    @test s2.state ≈ s4.state

    ts = timeseries(s1, 100; mutate = false)
    @test size(ts) == (100, 3)
    ts4 = timeseries(s4, 100; mutate = false)
    @test size(ts4) == (100, 3)
    @test eltype(ts4) == BigFloat
    @test isapprox.(ts[10, :],ts4[10, :]) == trues(3)
  end
  @testset "Jacobians" begin

    J1 = jacobian(s1)
    @test typeof(J1) <: SMatrix
    J2 = jacobian(s2)
    J4 = jacobian(s4)
    @test typeof(J4) <: SMatrix

    @test isapprox.(J1, J2; rtol = 1e-6) == trues(J1)
    @test isapprox.(J1, J4; rtol = 1e-6) == trues(J1)
    @test eltype(J4) == BigFloat
  end
end


text:
struct GridWorldMDP <: MDP{GWPos, Symbol}
    g::SimpleGridWorld
    costs::Dict{GWPos, Float64}
    cost_penalty::Float64
end

GridWorldMDP(;size::Tuple{Int, Int}           = (10,10),
              rewards::Dict{GWPos, Float64}   = Dict(GWPos(4,3)=>-10.0, GWPos(4,6)=>-5.0, GWPos(9,3)=>10.0, GWPos(8,8)=>3.0),
              costs::Dict{GWPos, Float64}     = Dict{GWPos, Float64}(),
              cost_penalty::Float64           = 0.0,
              terminate_from::Set{GWPos}      = Set(keys(rewards)),
              tprob::Float64                  = 0.7,
              discount::Float64               = 0.95) = GridWorldMDP(SimpleGridWorld(size = size, rewards = rewards, terminate_from = terminate_from, tprob = tprob, discount = discount), costs, cost_penalty)

function cost(mdp::GridWorldMDP, s)
    get(mdp.costs, s, 0)
end

function POMDPs.transition(mdp::GridWorldMDP, s, a)
    transition(mdp.g, s, a)
end

function POMDPs.reward(mdp::GridWorldMDP, s, a)
    reward(mdp.g, s, a)
end
function POMDPs.reward(mdp::GridWorldMDP, s)
    reward(mdp.g, s)
end

function POMDPs.gen(mdp::GridWorldMDP, s, a, rng = Random.GLOBAL_RNG, info=Dict())
    sp = rand(rng, transition(mdp.g, s, a))
    c = cost(mdp,s)
    r = POMDPs.reward(mdp.g, s, a) - mdp.cost_penalty*c
    info["cost"] = c
    (;sp, r)
end

function POMDPs.initialstate(mdp::GridWorldMDP)
    function istate(rng::AbstractRNG)
        while true
            x, y = rand(rng, 1:mdp.g.size[1]), rand(rng, 1:mdp.g.size[2])
            !(GWPos(x,y) in mdp.g.terminate_from) && return GWPos(x,y)
        end
    end
    return ImplicitDistribution(istate)
end

POMDPs.convert_s(::Type{AbstractArray}, s::GWPos, mdp::GridWorldMDP) = Float32.([s...])

POMDPs.actions(mdp::GridWorldMDP) = POMDPs.actions(mdp.g)
POMDPs.actionindex(mdp::GridWorldMDP, a) = POMDPs.actionindex(mdp.g, a)

POMDPs.isterminal(mdp::GridWorldMDP, s) = isterminal(mdp.g, s)
POMDPs.discount(mdp::GridWorldMDP) = discount(mdp.g)

POMDPs.states(mdp::GridWorldMDP) = states(mdp.g)
POMDPs.stateindex(mdp::GridWorldMDP, s) = stateindex(mdp.g, s)

function render(mdp::GridWorldMDP, s=GWPos(7,5), a=nothing; color = s->10.0*POMDPs.reward(mdp.g, s), policy= nothing, kwargs...)
    img = POMDPModelTools.render(mdp.g, (s = s,), color = color, policy = isnothing(policy) ? nothing : FunctionPolicy((s) ->  action(policy, convert_s(AbstractArray, s, mdp))); kwargs...)
    tmpfilename = "/tmp/out.png"
    img |> PNG(tmpfilename, 1cm .* mdp.g.size...)
    load(tmpfilename)
end


text:
## Converison

#ensure that COnversion is called
coefficients{DD}(cfs::Vector,A::Fourier{DD},B::Laurent{DD})=(Conversion(A,B)*cfs).coefficients
coefficients{DD}(cfs::Vector,A::Laurent{DD},B::Fourier{DD})=(Conversion(A,B)*cfs).coefficients

hasconversion{DD}(::Fourier{DD},::Laurent{DD})=true
hasconversion{DD}(::Laurent{DD},::Fourier{DD})=true

Conversion{DD}(a::Laurent{DD},b::Fourier{DD})=ConcreteConversion(a,b)
Conversion{DD}(a::Fourier{DD},b::Laurent{DD})=ConcreteConversion(a,b)

function getindex{DD,T}(C::ConcreteConversion{Laurent{DD},Fourier{DD},T},k::Integer,j::Integer)
    if k==j==1
        one(T)
    elseif iseven(k) && k==j
        -one(T)*im
    elseif iseven(k) && k+1==j
        one(T)*im
    elseif isodd(k) && (k==j || k-1==j )
        one(T)
    else
        zero(T)
    end
end

function getindex{DD,T}(C::ConcreteConversion{Fourier{DD},Laurent{DD},T},k::Integer,j::Integer)
    if k==j==1
        one(T)
    elseif iseven(k) && k==j
        one(T)/2*im
    elseif iseven(k) && k+1==j
        one(T)/2
    elseif isodd(k) && k==j
        one(T)/2
    elseif isodd(k) && j==k-1
        -one(T)*im/2
    else
        zero(T)
    end
end

bandinds{DD}(::ConcreteConversion{Laurent{DD},Fourier{DD}})=-1,1
bandinds{DD}(::ConcreteConversion{Fourier{DD},Laurent{DD}})=-1,1

for RULE in (:conversion_rule,:maxspace_rule,:union_rule)
    @eval function $RULE{DD}(A::Laurent{DD},B::Fourier{DD})
        @assert domainscompatible(A,B)
        B
    end
end

conversion_type{DD<:Circle}(A::Fourier{DD},B::Fourier{DD})=domain(A).orientation?A:B

hasconversion{DD}(A::Fourier{DD},B::Fourier{DD})=domain(A) == reverse(domain(B))
function Conversion{DD}(A::Fourier{DD},B::Fourier{DD})
    if A==B
        ConversionWrapper(eye(A))
    else
        @assert domain(A) == reverse(domain(B))
        ConcreteConversion(A,B)
    end
end
bandinds{DD}(::ConcreteConversion{Fourier{DD},Fourier{DD}})=0,0

getindex{DD,T}(C::ConcreteConversion{Fourier{DD},Fourier{DD},T},k::Integer,j::Integer) =
    k==j?(iseven(k)?(-one(T)):one(T)):zero(T)

### Cos/Sine

function Derivative(S::Union{CosSpace,SinSpace},order)
    @assert isa(domain(S),PeriodicInterval)
    ConcreteDerivative(S,order)
end

bandinds{CS<:CosSpace}(D::ConcreteDerivative{CS})=iseven(D.order)?(0,0):(0,1)
bandinds{S<:SinSpace}(D::ConcreteDerivative{S})=iseven(D.order)?(0,0):(-1,0)
rangespace{S<:CosSpace}(D::ConcreteDerivative{S})=iseven(D.order)?D.space:SinSpace(domain(D))
rangespace{S<:SinSpace}(D::ConcreteDerivative{S})=iseven(D.order)?D.space:CosSpace(domain(D))

function getindex{CS<:CosSpace,OT,T}(D::ConcreteDerivative{CS,OT,T},k::Integer,j::Integer)
    d=domain(D)
    m=D.order
    C=T(2/(d.b-d.a)*π)

    if k==j && mod(m,4)==0
        (C*(k-1))^m
    elseif k==j && mod(m,4)==2
        -(C*(k-1))^m
    elseif j==k+1 && mod(m,4)==1
        -(C*k)^m
    elseif j==k+1 && mod(m,4)==3
        (C*k)^m
    else
        zero(T)
    end
end

function getindex{CS<:SinSpace,OT,T}(D::ConcreteDerivative{CS,OT,T},k::Integer,j::Integer)
    d=domain(D)
    m=D.order
    C=T(2/(d.b-d.a)*π)

    if k==j && mod(m,4)==0
        (C*k)^m
    elseif k==j && mod(m,4)==2
        -(C*k)^m
    elseif j==k-1 && mod(m,4)==1
        (C*j)^m
    elseif j==k-1 && mod(m,4)==3
        -(C*j)^m
    else
        zero(T)
    end
end

Integral(::CosSpace,m::Integer) =
    error("Integral not defined for CosSpace.  Use Integral(CosSpace()|(2:∞)) if first coefficient vanishes.")

Integral{DD<:PeriodicInterval}(sp::SinSpace{DD},m::Integer) = ConcreteIntegral(sp,m)

bandinds{CS<:SinSpace}(D::ConcreteIntegral{CS})=iseven(D.order)?(0,0):(-1,0)
rangespace{S<:CosSpace}(D::ConcreteIntegral{S})=iseven(D.order)?D.space:SinSpace(domain(D))
rangespace{S<:SinSpace}(D::ConcreteIntegral{S})=iseven(D.order)?D.space:CosSpace(domain(D))

function getindex{CS<:SinSpace,OT,T}(D::ConcreteIntegral{CS,OT,T},k::Integer,j::Integer)
    d=domain(D)
    @assert isa(d,PeriodicInterval)
    m=D.order
    C=T(2/(d.b-d.a)*π)

    if k==j && mod(m,4)==0
        (C*k)^(-m)
    elseif k==j && mod(m,4)==2
        -(C*k)^(-m)
    elseif j==k-1 && mod(m,4)==1
        -(C*j)^(-m)
    elseif j==k-1 && mod(m,4)==3
        (C*j)^(-m)
    else
        zero(T)
    end
end

function Integral{T,CS<:CosSpace,DD<:PeriodicInterval}(S::SubSpace{CS,UnitCount{Int64},T,DD,1},k::Integer)
    @assert first(S.indexes)==2
    ConcreteIntegral(S,k)
end

bandinds{T,CS<:CosSpace,DD<:PeriodicInterval}(D::ConcreteIntegral{SubSpace{CS,UnitCount{Int64},T,DD,1}}) =
    (0,0)
rangespace{T,CS<:CosSpace,DD<:PeriodicInterval}(D::ConcreteIntegral{SubSpace{CS,UnitCount{Int64},T,DD,1}}) =
    iseven(D.order)?D.space:SinSpace(domain(D))

function getindex{TT,CS<:CosSpace,DD<:PeriodicInterval}(D::ConcreteIntegral{SubSpace{CS,UnitCount{Int64},TT,DD,1}},
                                                            k::Integer,j::Integer)
    d=domain(D)
    m=D.order
    T=eltype(D)
    C=T(2/(d.b-d.a)*π)

    if k==j
        if mod(m,4)==0
            (C*k)^(-m)
        elseif mod(m,4)==2
            -(C*k)^(-m)
        elseif mod(m,4)==1
        (C*k)^(-m)
        else   # mod(m,4)==3
            -(C*k)^(-m)
        end
    else
        zero(T)
    end
end

# CosSpace Multiplicaiton is the same as Chebyshev

Multiplication{CS<:CosSpace}(f::Fun{CS},sp::CS) = ConcreteMultiplication(f,sp)
Multiplication{SS<:SinSpace}(f::Fun{SS},sp::SS) = ConcreteMultiplication(f,sp)
Multiplication{CS<:CosSpace}(f::Fun{CS},sp::SinSpace) = ConcreteMultiplication(f,sp)
function Multiplication{SS<:SinSpace}(f::Fun{SS},sp::CosSpace)
    @assert domain(f) == domain(sp)
    a=f.coefficients/2
    A=ToeplitzOperator(a[2:end],[a[1];0.;-a]) + HankelOperator(a)
    MultiplicationWrapper(f,SpaceOperator(A,sp,SinSpace(domain(sp))))
end

bandinds{CS<:CosSpace}(M::ConcreteMultiplication{CS,CS}) =
    (1-ncoefficients(M.f),ncoefficients(M.f)-1)
rangespace{CS<:CosSpace}(M::ConcreteMultiplication{CS,CS}) = domainspace(M)
getindex{CS<:CosSpace}(M::ConcreteMultiplication{CS,CS},k::Integer,j::Integer) =
    chebmult_getindex(M.f.coefficients,k,j)

function getindex{SS<:SinSpace}(M::ConcreteMultiplication{SS,SS},k::Integer,j::Integer)
    a=M.f.coefficients
    ret=0.5*toeplitz_getindex([0.;-a],a,k,j)
    if k ≥ 2
        ret+=0.5*hankel_getindex(a,k,j)
    end
    ret
end

bandinds{SS<:SinSpace}(M::ConcreteMultiplication{SS,SS})=-ncoefficients(M.f)-1,ncoefficients(M.f)-1
rangespace{SS<:SinSpace}(M::ConcreteMultiplication{SS,SS})=CosSpace(domain(M))

function getindex{SS<:SinSpace,Cs<:CosSpace}(M::ConcreteMultiplication{Cs,SS},k::Integer,j::Integer)
    a=M.f.coefficients
    ret=0.5*toeplitz_getindex(a,k,j)
    if length(a)>=3
        ret-=0.5*hankel_getindex(@compat(view(a,3:length(a))),k,j)
    end
    ret
end

bandinds{SS<:SinSpace,Cs<:CosSpace}(M::ConcreteMultiplication{Cs,SS}) =
    (1-ncoefficients(M.f),ncoefficients(M.f)-1)
rangespace{SS<:SinSpace,Cs<:CosSpace}(M::ConcreteMultiplication{Cs,SS}) =
    SinSpace(domain(M))

function Multiplication{T,D}(a::Fun{Fourier{D},T},sp::Fourier{D})
    d=domain(a)
    c,s=vec(a)
    O=Operator{T}[Multiplication(c,CosSpace(d)) Multiplication(s,SinSpace(d));
                        Multiplication(s,CosSpace(d)) Multiplication(c,SinSpace(d))]
    MultiplicationWrapper(a,InterlaceOperator(O,space(a),sp))
end

## Definite integral

DefiniteIntegral{D<:PeriodicInterval}(sp::Fourier{D}) =
    ConcreteDefiniteIntegral{typeof(sp),Float64}(sp)
DefiniteIntegral{D<:Circle}(sp::Fourier{D}) =
    ConcreteDefiniteIntegral{typeof(sp),Complex{Float64}}(sp)

function getindex{T,D}(Σ::ConcreteDefiniteIntegral{Fourier{D},T},kr::Range)
    d = domain(Σ)
    if isa(d,PeriodicInterval)
        T[k == 1?  d.b-d.a : zero(T) for k=kr]
    else
        @assert isa(d,Circle)
        T[k == 2?  -d.radius*π : (k==3?d.radius*π*im :zero(T)) for k=kr]
    end
end

bandinds{D}(Σ::ConcreteDefiniteIntegral{Fourier{D}}) =
    0,isa(domain(Σ),PeriodicInterval)?0:2

DefiniteLineIntegral{D}(sp::Fourier{D}) =
    ConcreteDefiniteLineIntegral{typeof(sp),Float64}(sp)

getindex{T,D<:PeriodicInterval}(Σ::ConcreteDefiniteLineIntegral{Fourier{D},T},k::Integer) =
    k==1? domain(Σ).b-domain(Σ).a : zero(T)

getindex{T,D<:Circle}(Σ::ConcreteDefiniteLineIntegral{Fourier{D},T},k::Integer) =
    k==1? domain(Σ).radius*π : zero(T)

bandinds{D}(Σ::ConcreteDefiniteLineIntegral{Fourier{D}}) = 0,0

transformtimes{CS<:CosSpace,D}(f::Fun{CS},g::Fun{Fourier{D}}) =
    transformtimes(Fun(interlace(f.coefficients,zeros(eltype(f),ncoefficients(f)-1)),Fourier(domain(f))),g)
transformtimes{SS<:SinSpace,D}(f::Fun{SS},g::Fun{Fourier{D}}) =
    transformtimes(Fun(interlace(zeros(eltype(f),ncoefficients(f)+1),f.coefficients),Fourier(domain(f))),g)
transformtimes{CS<:CosSpace,SS<:SinSpace}(f::Fun{CS},g::Fun{SS}) =
    transformtimes(Fun(interlace(f.coefficients,zeros(eltype(f),ncoefficients(f)-1)),Fourier(domain(f))),
                    Fun(interlace(zeros(eltype(g),ncoefficients(g)+1),g.coefficients),Fourier(domain(g))))
transformtimes{CS<:CosSpace,D}(f::Fun{Fourier{D}},g::Fun{CS}) = transformtimes(g,f)
transformtimes{SS<:SinSpace,D}(f::Fun{Fourier{D}},g::Fun{SS}) = transformtimes(g,f)
transformtimes{SS<:SinSpace,CS<:CosSpace}(f::Fun{SS},g::Fun{CS}) = transformtimes(g,f)


text:
using MPI
using CLIMA.Topologies
using CLIMA.Grids
using CLIMA.AtmosDycore.VanillaAtmosDiscretizations
using CLIMA.MPIStateArrays
using CLIMA.ODESolvers
using CLIMA.LowStorageRungeKuttaMethod
using CLIMA.GenericCallbacks
using CLIMA.AtmosDycore
using CLIMA.MoistThermodynamics
using LinearAlgebra
using Logging, Printf, Dates
using Test
using CLIMA.Vtk

using CLIMA.ParametersType
using CLIMA.PlanetParameters: R_d, cp_d, grav, cv_d, T_triple, MSLP

# FIXME: Will these keywords args be OK?

function tracer_thermal_bubble(x...; ntrace=0, nmoist=0, dim=3)

  DFloat          = eltype(x)
  p0::DFloat      = MSLP
  R_gas::DFloat   = R_d
  c_p::DFloat     = cp_d
  c_v::DFloat     = cv_d
  gravity::DFloat = grav
  T_0::DFloat     = T_triple

  r = sqrt((x[1] - 500)^2 + (x[dim] - 350)^2)
  rc::DFloat = 250
  θ_ref::DFloat = 320
  θ_c::DFloat = 2
  Δθ::DFloat = 0
  Δq_tot::DFloat = 0
  q_tot = 0.0196
  q_tr = 0.0100
  if r <= rc
    Δθ = θ_c * (1 + cospi(r / rc)) / 2
    Δq_tot = q_tot/5 * (1 + cospi(r / rc)) / 2
  end
  θ = θ_ref + Δθ
  π_k = 1 - gravity / (c_p * θ) * x[dim]

  c = c_v / R_gas
  ρ = p0 / (R_gas * θ) * (π_k)^c
  u = zero(DFloat)
  v = zero(DFloat)
  w = zero(DFloat)
  U = ρ * u
  V = ρ * v
  W = ρ * w
  P = p0 * (R_gas * (ρ * θ) / p0)^(c_p / c_v)
  T = P / (ρ * R_gas)
  q_tot += Δq_tot
  e_kin = (u^2 + v^2 + w^2) / 2
  e_pot = gravity * x[dim]
  e_int = MoistThermodynamics.internal_energy(T, PhasePartition(q_tot))
  E_tot = ρ * MoistThermodynamics.total_energy(e_kin, e_pot, T)
  (ρ=ρ, U=U, V=V, W=W, E=E_tot,
   Qmoist = (q_tot * ρ,),  #Qmoist => Moist variable (may have corresponding sources)
   Qtrace = ntuple(j->(-j*ρ),ntrace))   #Qtrace => Arbitrary tracers

end

function main(mpicomm, DFloat, ArrayType, brickrange, nmoist, ntrace, N,
              timeend, bricktopo; gravity=true, dt=nothing,
              exact_timeend=true)
  dim = length(brickrange)
  topl = bricktopo(# MPI communicator to connect elements/partition
                   mpicomm,
                   # tuple of point element edges in each dimension
                   # (dim is inferred from this)
                   brickrange,
                   periodicity=(true, ntuple(j->false, dim-1)...))

  grid = DiscontinuousSpectralElementGrid(topl,
                                          # Compute floating point type
                                          FloatType = DFloat,
                                          # This is the array type to store
                                          # data: CuArray = GPU, Array = CPU
                                          DeviceArray = ArrayType,
                                          # polynomial order for LGL grid
                                          polynomialorder = N,
                                          # how to skew the mesh degrees of
                                          # freedom (for instance spherical
                                          # or topography maps)
                                          # warp = warpgridfun
                                         )

  # spacedisc = data needed for evaluating the right-hand side function
  spacedisc = VanillaAtmosDiscretization(grid,
                                         # Use gravity?
                                         gravity = gravity,
                                         # How many tracer variables
                                         ntrace=ntrace,
                                         # How many moisture variables
                                         nmoist=nmoist)

  # Initial condition from driver
  initialcondition(x...) = tracer_thermal_bubble(x...;
					       ntrace=ntrace,
					       nmoist=nmoist,
					       dim=dim)

  # This is a actual state/function that lives on the grid
  Q = MPIStateArray(spacedisc, initialcondition)

  # Determine the time step
  (dt == nothing) && (dt = VanillaAtmosDiscretizations.estimatedt(spacedisc, Q))
  if exact_timeend
    nsteps = ceil(Int64, timeend / dt)
    dt = timeend / nsteps
  end

  # Initialize the Method (extra needed buffers created here)
  # Could also add an init here for instance if the ODE solver has some
  # state and reading from a restart file

  # TODO: Should we use get property to get the rhs function?
  lsrk = LowStorageRungeKutta(getrhsfunction(spacedisc), Q; dt = dt, t0 = 0)

  # Get the initial energy
  eng0 = norm(Q)
  @info @sprintf """Starting
  norm(Q₀) = %.16e""" eng0

  # Set up the information callback
  starttime = Ref(now())
  cbinfo = GenericCallbacks.EveryXWallTimeSeconds(60, mpicomm) do (s=false)
    if s
      starttime[] = now()
    else
      energy = norm(Q)
      @info @sprintf """Update
  simtime = %.16e
  runtime = %s
  norm(Q) = %.16e""" ODESolvers.gettime(lsrk) Dates.format(convert(Dates.DateTime, Dates.now()-starttime[]), Dates.dateformat"HH:MM:SS") energy
    end
  end

  #= Paraview calculators:
  P = (0.4) * (E  - (U^2 + V^2 + W^2) / (2*ρ) - 9.81 * ρ * coordsZ)
  theta = (100000/287.0024093890231) * (P / 100000)^(1/1.4) / ρ
  =#
  step = [0]
  mkpath("vtk")
  cbvtk = GenericCallbacks.EveryXSimulationSteps(100) do (init=false)
    outprefix = @sprintf("vtk/IS_%dD_rank_%d_of_%d_step%04d", dim,
                         MPI.Comm_rank(mpicomm)+1, MPI.Comm_size(mpicomm),
                         step[1])
    @debug "doing VTK output" outprefix
    step[1] == 0 &&
      writevtk(outprefix, Q, spacedisc)
    step[1] += 1
    nothing
  end

  solve!(Q, lsrk; timeend=timeend, callbacks=(cbinfo, cbvtk))

  # Print some end of the simulation information
  engf = norm(Q)
  @info @sprintf """Finished
  norm(Q)            = %.16e
  norm(Q) / norm(Q₀) = %.16e
  norm(Q) - norm(Q₀) = %.16e""" engf engf/eng0 engf-eng0

  h_Q = ArrayType == Array ? Q.Q : Array(Q.Q)

  for (j, n) = enumerate(spacedisc.tracerange)
    @test -j * (@view h_Q[:, spacedisc.ρid, :]) ≈ (@view h_Q[:, n, :])
  end

  engf
end

let
  MPI.Initialized() || MPI.Init()

  Sys.iswindows() || (isinteractive() && MPI.finalize_atexit())
  mpicomm = MPI.COMM_WORLD
  ll = uppercase(get(ENV, "JULIA_LOG_LEVEL", "INFO"))
  loglevel = ll == "DEBUG" ? Logging.Debug :
  ll == "WARN"  ? Logging.Warn  :
  ll == "ERROR" ? Logging.Error : Logging.Info
  logger_stream = MPI.Comm_rank(mpicomm) == 0 ? stderr : devnull
  global_logger(ConsoleLogger(logger_stream, loglevel))

  nmoist = 1
  ntrace = 2
  Ne = (10, 10, 1)
  N = 3
  dt = 1e-3
  timeend = 2dt
  for DFloat in (Float64, )
    for ArrayType in (Array,)
      for bricktopo in (BrickTopology, StackedBrickTopology)
        for dim in 2:3
          brickrange = ntuple(j->range(DFloat(0); length=Ne[j]+1, stop=1000),
                              dim)
          main(mpicomm, DFloat, ArrayType, brickrange, nmoist, ntrace, N,
               timeend, bricktopo; dt=dt)
          @test true
        end
      end
    end
  end
end

isinteractive() || MPI.Finalize()

max_stars_repo_name: jkozdon/CLIMA

max_issues_repo_name: jkozdon/CLIMA

max_forks_repo_name: jkozdon/CLIMA


text:
using LevelDB
using Base.Test

db = open_db("level.db", true)
batch = create_write_batch()
val = "value10"
batch_put(batch, "key1", val, length(val))
write_batch(db, batch)

readback_value = bytestring(db_get(db, "key1"))

@test  readback_value == val

println("String read back OK")

db_delete(db, "key1")
@test isempty(db_get(db, "key1")) == true
println("Successfully deleted")

# Now write a Float64 array

float_array = Float64[1.0, 2.0, 3.3, 4.4]

key = "FloatKey"

# each Float64 is 8 bytes
db_put(db, key, pointer(float_array), length(float_array) * 8)

readback_value = reinterpret(Float64,db_get(db, key))
@test float_array == readback_value

# assert the two arrays are in different memory block
# by modifying the original and the read-back copy should be different from it

float_array[1] = 100.0
@test float_array != readback_value
println("Floating point array read back OK")

db_put(db, "key2", "v2", 2)
db_put(db, "key3", "v3", 2)

d = {
  "key1" => "v1",
  "key2" => "v2",
  "key3" => "v3",
}

for (k, v) in d
  db_put(db, k, v, length(v))
end

for (k, v) in db_range(db, "key1", "key3")
  @test bytestring(v) == d[k]
end
println("Pass iterator")

close_db(db)
println("All Tests Passed.")


text:
const fp = (1e9, 64.0, 3.46e-8, 2.0e-3, 0.7, 0.02, 1.0)

"""
    DAMMfdata(n)

Generates a DataFrame of n fake data Tₛ, θ and Rₛ

# Examples
```julia-repl
julia> DAMMfdata(5)
5×3 DataFrame
 Row │ Tₛ       θ        Rₛ
     │ Float64  Float64  Float64
─────┼───────────────────────────
   1 │    10.8      0.3  2.04327
   2 │    31.5      0.1  7.8925
   3 │    38.7      0.7  1.6
   4 │    35.7      0.3  7.38025
   5 │    21.9      0.2  3.0012
"""
function DAMMfdata(n)
  Tₛ = rand(0:400, n)./10
  θ = rand(1:7, n)./10
  noise = rand(-20:20, n)./10
  Rₛ = DAMM(hcat(Tₛ, θ), fp) .+ noise
  Rₛ[Rₛ .< 0] .= 0
  df = DataFrame((Tₛ = Tₛ, θ = θ, Rₛ = Rₛ))
  return df
end


text:
#*****************************************************************************#
# General initialization functions for all integrators                        #
#*****************************************************************************#

"Print error for integrators not implemented, yet."
function Integrator(equation::Equation, tableau::AbstractTableau, Δt)
    error("No integrator found for equation ", equation, " and tableau ", tableau)
end

#*****************************************************************************#
# Initialization functions for deterministic integrators                      #
#*****************************************************************************#

"Create integrator for explicit Runge-Kutta tableau."
function Integrator(equation::ODE, tableau::TableauERK, Δt)
    IntegratorERK(equation, tableau, Δt)
end

"Create integrator for diagonally implicit Runge-Kutta tableau."
function Integrator(equation::ODE, tableau::TableauDIRK, Δt)
    IntegratorDIRK(equation, tableau, Δt)
end

"Create integrator for fully implicit Runge-Kutta tableau."
function Integrator(equation::ODE, tableau::TableauFIRK, Δt)
    IntegratorFIRK(equation, tableau, Δt)
end

"Create integrator for explicit partitioned Runge-Kutta tableau."
function Integrator(equation::PODE, tableau::TableauEPRK, Δt)
    IntegratorEPRK(equation, tableau, Δt)
end

"Create integrator for implicit partitioned Runge-Kutta tableau."
function Integrator(equation::PODE, tableau::TableauIPRK, Δt)
    IntegratorIPRK(equation, tableau, Δt)
end

"Create integrator for variational partitioned Runge-Kutta tableau."
function Integrator(equation::IODE, tableau::TableauVPRK, Δt)
    IntegratorVPRK(equation, tableau, Δt)
end

"Create integrator for formal Lagrangian Runge-Kutta tableau."
function Integrator(equation::VODE, tableau::TableauFIRK, Δt)
    IntegratorFLRK(equation, tableau, Δt)
end

"Create integrator for Projected Gauss-Legendre Runge-Kutta tableau."
function Integrator(equation::IODE, tableau::CoefficientsPGLRK, Δt)
    IntegratorPGLRK(equation, tableau, Δt)
end

"Create integrator for variational partitioned additive Runge-Kutta tableau."
function Integrator(equation::IDAE, tableau::TableauVPARK, Δt)
    IntegratorVPARK(equation, tableau, Δt)
end

"Create integrator for variational special partitioned additive Runge-Kutta tableau."
function Integrator(equation::IDAE, tableau::TableauVSPARK, Δt)
    IntegratorVSPARK(equation, tableau, Δt)
end

"Create integrator for variational special partitioned additive Runge-Kutta tableau with projection on primary constraint."
function Integrator(equation::IDAE, tableau::TableauVSPARKprimary, Δt)
    IntegratorVSPARKprimary(equation, tableau, Δt)
end

"Create integrator for variational special partitioned additive Runge-Kutta tableau with projection on secondary constraint."
function Integrator(equation::VDAE, tableau::TableauVSPARKsecondary, Δt)
    IntegratorVSPARKsecondary(equation, tableau, Δt)
end

"Create integrator for Hamiltonian partitioned additive Runge-Kutta tableau."
function Integrator(equation::PDAE, tableau::TableauHPARK, Δt)
    IntegratorHPARK(equation, tableau, Δt)
end

"Create integrator for Hamiltonian special partitioned additive Runge-Kutta tableau."
function Integrator(equation::PDAE, tableau::TableauHSPARK, Δt)
    IntegratorHSPARK(equation, tableau, Δt)
end

"Create integrator for Hamiltonian special partitioned additive Runge-Kutta tableau with projection on primary constraint."
function Integrator(equation::PDAE, tableau::TableauHSPARKprimary, Δt)
    IntegratorHSPARKprimary(equation, tableau, Δt)
end

"Create integrator for splitting tableau."
function Integrator(equation::SODE, tableau::AbstractTableauSplitting, Δt)
    IntegratorSplitting(equation, tableau, Δt)
end

#*****************************************************************************#
# Initialization functions for stochastic integrators                         #
#*****************************************************************************#

"Create integrator for stochastic explicit Runge-Kutta tableau."
function Integrator(equation::SDE, tableau::TableauSERK, Δt)
    IntegratorSERK(equation, tableau, Δt)
end

"Create integrator for weak explicit Runge-Kutta tableau."
function Integrator(equation::SDE, tableau::TableauWERK, Δt)
    IntegratorWERK(equation, tableau, Δt)
end

"Create integrator for stochastic fully implicit Runge-Kutta tableau."
function Integrator(equation::SDE, tableau::TableauSIRK, Δt; K::Int=0)
    IntegratorSIRK(equation, tableau, Δt, K=K)
end

"Create integrator for stochastic fully implicit partitioned Runge-Kutta tableau."
function Integrator(equation::PSDE, tableau::TableauSIPRK, Δt; K::Int=0)
    IntegratorSIPRK(equation, tableau, Δt, K=K)
end

"Create integrator for stochastic fully implicit split partitioned Runge-Kutta tableau."
function Integrator(equation::SPSDE, tableau::TableauSISPRK, Δt; K::Int=0)
    IntegratorSISPRK(equation, tableau, Δt, K=K)
end

"Create integrator for weak fully implicit Runge-Kutta tableau."
function Integrator(equation::SDE, tableau::TableauWIRK, Δt)
    IntegratorWIRK(equation, tableau, Δt)
end

#*****************************************************************************#
# General integration functions for all integrators                           #
#*****************************************************************************#

"Apply integrator for ntime time steps and return solution."
function integrate(integrator::Integrator, ntime::Int, nsave::Int=DEFAULT_NSAVE)
    solution = Solution(equation(integrator), timestep(integrator), ntime, nsave)
    integrate!(integrator, solution)
    return solution
end

"Integrate given equation with given tableau for ntime time steps and return solution."
function integrate(equation::Equation, tableau::AbstractTableau, Δt, ntime, nsave=DEFAULT_NSAVE)
    return integrate(Integrator(equation, tableau, Δt), ntime, nsave)
end

"Integrate ODE specified by vector field and initial condition with given tableau for ntime time steps and return solution."
function integrate(f::Function, x₀::Vector, tableau::AbstractTableau, Δt, ntime, nsave=DEFAULT_NSAVE; t₀=0)
    return integrate(ODE(f, t₀, x₀), tableau, Δt, ntime, nsave)
end

"Integrate PODE specified by two vector fields and initial conditions with given tableau for ntime time steps and return solution."
function integrate(v::Function, f::Function, q₀::Vector, p₀::Vector, tableau::AbstractTableau, Δt, ntime, nsave=DEFAULT_NSAVE; t₀=0)
    return integrate(PODE(v, f, t₀, q₀, p₀), tableau, Δt, ntime, nsave)
end

#*****************************************************************************#
# Integration functions for deterministic integrators                         #
#*****************************************************************************#

"Integrate equation for all initial conditions."
function integrate!(int::DeterministicIntegrator, sol::Solution)
    integrate!(int, sol, 1, sol.ni)
end

"Integrate ODE for initial conditions m with m₁ ≤ m ≤ m₂."
function integrate!(int::DeterministicIntegrator, sol::Solution, m1, m2)
    # integrate initial conditions m with m₁ ≤ m ≤ m₂ for all time steps
    integrate!(int, sol, m1, m2, 1, sol.ntime)
end

"Integrate ODE for initial conditions m with m₁ ≤ m ≤ m₂ for time steps n with n₁ ≤ n ≤ n₂."
function integrate!(int::DeterministicIntegrator{DT,TT}, sol::Solution{DT,TT,N}, m1::Int, m2::Int, n1::Int, n2::Int) where {DT,TT,N}
    @assert m1 ≥ 1
    @assert m2 ≥ m1
    @assert m2 ≤ sol.ni

    @assert n1 ≥ 1
    @assert n2 ≥ n1
    @assert n2 ≤ sol.ntime

    asol = AtomicSolution(equation(int))

    # loop over initial conditions showing progress bar
    for m in m1:m2
        # get cache from solution
        get_initial_conditions!(sol, asol, m, n1)
        initialize!(int, asol)

        # loop over time steps
        for n in n1:n2
            # try
                # integrate one initial condition for one time step
                integrate_step!(int, asol)

                # take care of periodic solutions
                cut_periodic_solution!(asol, periodicity(equation(int)))

                # copy solution from cache to solution
                set_solution!(sol, asol, n, m)
            # catch ex
            #     tstr = " in time step " * string(n)
            #
            #     if m1 ≠ m2
            #         tstr *= " for initial condition " * string(m)
            #     end
            #
            #     tstr *= "."
            #
            #     if isa(ex, DomainError)
            #         @warn("Domain error" * tstr)
            #     elseif isa(ex, ErrorException)
            #         @warn("Simulation exited early" * tstr)
            #         @warn(ex.msg)
            #     else
            #         @warn(string(typeof(ex)) * tstr)
            #         throw(ex)
            #     end
            # end
        end
    end
end

#*****************************************************************************#
# Integration functions for stochastic integrators                            #
#*****************************************************************************#

"Integrate SDE for all sample paths and initial conditions."
function integrate!(int::StochasticIntegrator, sol::StochasticSolution)
    integrate!(int, sol, 1, sol.ns, 1, sol.ni)
end

"Integrate SDE for the sample paths k with k₁ ≤ k ≤ k₂ and initial conditions m with m₁ ≤ m ≤ m₂."
function integrate!(int::StochasticIntegrator, sol::StochasticSolution, k1::Int, k2::Int, m1::Int, m2::Int)
    # initialize integrator for initial conditions m with m₁ ≤ m ≤ m₂ and time step 0
    initialize!(int, sol, k1, k2, m1, m2)

    # integrate initial conditions m with m₁ ≤ m ≤ m₂ for all time steps
    integrate!(int, sol, k1, k2, m1, m2, 1, sol.ntime)
end

"Integrate SDE for the sample paths k with k₁ ≤ k ≤ k₂, the initial conditions m with m₁ ≤ m ≤ m₂, and the time steps n with n₁ ≤ n ≤ n₂."
function integrate!(int::StochasticIntegrator{DT,TT}, sol::StochasticSolution{DT,TT,N}, k1::Int, k2::Int, m1::Int, m2::Int, n1::Int, n2::Int) where {DT,TT,N}
    @assert k1 ≥ 1
    @assert k2 ≥ k1
    @assert k2 ≤ sol.ns

    @assert m1 ≥ 1
    @assert m2 ≥ m1
    @assert m2 ≤ sol.ni

    @assert n1 ≥ 1
    @assert n2 ≥ n1
    @assert n2 ≤ sol.ntime

    # loop over initial conditions
    for m in m1:m2
        # loop over sample paths
        for k in k1:k2
            # loop over time steps
            for n in n1:n2
                # try
                    # integrate one initial condition for one time step
                    integrate_step!(int, sol, k, m, n)
                # catch ex
                #     tstr = " in time step " * string(n)
                #
                #     if m1 ≠ m2
                #         tstr *= " for initial condition " * string(m)
                #     end
                #
                #     tstr *= "."
                #
                #     if isa(ex, DomainError)
                #         @warn("Domain error" * tstr)
                #     elseif isa(ex, ErrorException)
                #         @warn("Simulation exited early" * tstr)
                #         @warn(ex.msg)
                #         throw(ex)
                #     else
                #         @warn(string(typeof(ex)) * tstr)
                #         throw(ex)
                #     end
                # end
            end
        end
    end
end

"Initialize stochastic integrator for the sample paths k with k₁ ≤ k ≤ k₂, initial conditions m with m₁ ≤ m ≤ m₂ and time step 0."
function initialize!(int::StochasticIntegrator, sol::StochasticSolution, k1::Int, k2::Int, m1::Int, m2::Int)
    for m in m1:m2
        for k in k1:k2
            # initialize integrator for the sample path k, initial condition m and time step 0
            initialize!(int, sol, k, m)
        end
    end
end

# TODO Add solver status information to all integrators (if requested).


text:
function init(error_callback=ecall, versionMAJ, versionMIN, fullscreen, height, width, glviewx, glviewy)
    if(!(ccall((GLFWInit, glfw3), Bool, ())))
        println("ERROR 1: \n GLFW INIT FAILED");
        exit(1);
    end

    ccall((:glfwSetErrorCallback, glfw3), (error_callback));
    ccall((:glfwWindowHint, glfw3), (GLFW_CONTEXT_VERSION_MAJOR, versionMAJ));
    ccall((:glfwWindowHint, glfw3), (GLFW_CONTEXT_VERSION_MINOR, versionMIN));
    if(fullscreen)
end

Enumber = 0;

function ecall(error, description)
    println(Enumber + " | Error Number " + error + ": " + description);
end


text:
"""
Abstract type for a subsystem that contains multiple instances of StaticInjection

Subtypes must implement:
- get_subcomponents(subsystem::StaticInjectionSubsystem)

The subcomponents in subtypes must be attached to the System as masked components.
"""
abstract type StaticInjectionSubsystem <: StaticInjection end


text:
# SimLynx/src/SimLynx.jl
# Licensed under the MIT License. See LICENSE.md file in the project root for
# full license information.

__precompile__()

"A Hybrid Simulation Engine and Language in Julia"
module SimLynx

import Base: wait

using Plots; gr()
using Printf
using MacroTools

# Application Program Interface (API)
#
# These are the exported elements of SimLynx needed to run all of the examples
# for v0.5.0. We may need to export additional elements in the future -
# particularly for things like statistics and histories.

# notice.jl
export Notice

# event.jl
export Event
export @event

# activity.jl (future use)

# process.jl
export Process
export process_store
export process_store!
export @process

# simulation.jl
export current_trace
export current_trace!
export current_time
export current_notice
export current_process
export current_process_store
export current_process_store!
export @simulation

# control.jl
export @schedule
export wait
export work
export start_simulation
export stop_simulation
export suspend
export interrupt
export resume

# rendezvous.jl
export @accept
export @send

# variable.jl
export Variable
export plot_history
export print_stats

# queue.jl
export Queue
export FifoQueue
export LifoQueue
export add!
export remove!
export enqueue!
export dequeue!

# resource.jl
export Resource
export request
export release
export @with_resource

# SimLynx.jl (this file)
export greet
export seed

"Print the SimLynx greeting."
function greet()
    println(raw"""
                           _____ _           _                        _ _
          `\.      ,/'    /  ___(_)         | |                      (_) |
           |\\____//|     \ `--. _ _ __ ___ | |    _   _ _ __ __  __  _| |
           )/_ `' _\(      `--. \ | '_ ` _ \| |   | | | | '_ \\ \/ / | | |
          ,'/-`__'-\`\    /\__/ / | | | | | | |___| |_| | | | |>  < _| | |
          /. (_><_) ,\    \____/|_|_| |_| |_\_____/\__, |_| |_/_/\_(_) |_|
          '`)/`--'\(`'                              __/ |           _/ |
            '      '                               |___/           |__/

        A Hybrid Simulation Engine and Language in Julia
        SimLynx.jl Version 0.5.0 2020-12-15
        University of Colorado in Denver
        Dr. Doug Williams, Anthony Dupont, Trystan Kaes
        """)
end

"""
    @thunk ex

Return a thunk (i.e., a function with no arguments) that executes the expression
when called. This is (currently) only used by the @event macro but may be useful
elsewhere in the future. So, we have it here at the top-level.
"""
macro thunk(ex)
    :(() -> $(esc(ex)))
end

# Abstract data type declarations

abstract type ActiveElement end
abstract type AbstractMessage end

include("notice.jl")

# Include the active elements

include("event.jl")
include("activity.jl")
include("process.jl")

# Include the simulation framework

include("simulation.jl")
include("control.jl")
include("rendezvous.jl")
include("variable.jl")
include("queue.jl")
include("resource.jl")

"Helper function to initialize random seeds."
function seed(int::Integer...)::Array{UInt32, 1}
    return [UInt32(s) for s in [int...]]
end

end # module