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

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

#"pseudo-types" of aspheres
"""
AphericSurfaces polynomial evaluation is optimized for no terms `CONIC`, odd terms `ODD`, even terms `EVEN` or any `ODDEVEN`
"""
@enum AsphericSurfaceType CONIC ODD EVEN ODDEVEN

"""
    AsphericSurface{T,N,Q,M} <: ParametricSurface{T,N}

Surface incorporating an aspheric polynomial - radius, conic and aspherics are defined relative to absolute semi-diameter,.
`T` is the datatype, `N` is the dimensionality, `Q` is the number of aspheric terms, and `M` is the type of aspheric polynomial.

```julia
AsphericSurface(semidiameter; radius, conic, aspherics=nothing, normradius = semidiameter)
```

The surface is centered at the origin and treated as being the cap of an infinite cylinder, thus creating a true half-space.
Outside of `0 <= ρ <= 1` the height of the surface is not necessarily well defined, so NaN may be returned.

`aspherics`  aspherics should be vectors containing tuples of the form (i, v) where i is the polynomial power of the aspheric term

The sag is defined by the equation

```math
z(r,\\phi) = \\frac{cr^2}{1 + \\sqrt{1 - (1+k)c^2r^2}} + \\sum_{i}^{Q}\\alpha_ir^{i}
```

where ``\\rho = \\frac{r}{\\texttt{normradius}}``, ``c = \\frac{1}{\\texttt{radius}}``, and ``k = \\texttt{conic}`` .

The function checks if the aspheric terms are missing, even, odd or both and uses an efficient polynomial evaluation strategy.

"""
struct AsphericSurface{T,N,Q,M} <: ParametricSurface{T,N}
    semidiameter::T
    curvature::T
    conic::T
    aspherics::SVector{Q,T}
    normradius::T
    boundingcylinder::Cylinder{T,N}

    function AsphericSurface(M::AsphericSurfaceType, semidiameter::T, curvature::T, conic::T, aspherics::SVector{Q,T}, normradius::T, boundingcylinder) where {T<:Real,Q}
        new{T,3,Q,M}(semidiameter, curvature, conic, aspherics, normradius, boundingcylinder)
    end

    function AsphericSurface(semidiameter::T; radius::T = typemax(T), conic::T = zero(T), aspherics::Union{Nothing,Vector{Tuple{Int,T}}} = nothing, normradius::T = semidiameter) where {T<:Real}
        @assert semidiameter > 0
        @assert !isnan(semidiameter) && !isnan(radius) && !isnan(conic)
        @assert one(T) - (1 / radius)^2 * (conic + one(T)) * semidiameter^2 > 0 "Invalid surface (conic/radius combination: $radius, $conic)"

        acs = []
        if aspherics===nothing
            M = CONIC
         else
            asphericTerms = [i for (i, ) in aspherics]
            minAsphericTerm = minimum(asphericTerms)
            maxAsphericTerm = maximum(asphericTerms)
            @assert minAsphericTerm > 0 "Aspheric Terms must be Order 1 or higher ($minAsphericTerm)"
            acs = zeros(T, maxAsphericTerm )
            for (i, k) in aspherics
                acs[i] = k
            end
            odd = any([isodd(i) && a != zero(T) for (i, a) in enumerate(acs)])
            even = any([iseven(i) && a != zero(T) for (i, a) in enumerate(acs)])
            if odd && even
                M = ODDEVEN
            elseif even
                M = EVEN
                acs = [acs[2i] for i in 1:(maxAsphericTerm ÷ 2)]
            elseif odd
                M = ODD
                acs = [acs[2i-1] for i in 1:((maxAsphericTerm+1) ÷ 2)]
            else #there are no nonzero aspherics terms in the list
                M = CONIC
                acs = []
            end
        end
        Q = length(acs)
        surf = new{T,3, Q, M}(semidiameter, 1/radius, conic, acs, normradius, Cylinder(semidiameter, interface = opaqueinterface(T)))
        return surf
    end

end

"""
    EvenAsphericSurface(semidiameter, curvature::T, conic::T, aspherics::Vector{T}; normradius::T=semidiameter)

Surface incorporating an aspheric polynomial - radius, conic and aspherics are defined relative to absolute semi-diameter.

`aspherics` should be an array of the even coefficients of the aspheric polynomial starting with A2

"""
function EvenAsphericSurface(semidiameter::T, curvature::T, conic::T, aspherics::Vector{T}; normradius::T=semidiameter) where T<:Real
    Q=length(aspherics)
    AsphericSurface(EVEN, semidiameter, curvature, conic, SVector{Q,T}(aspherics), normradius, Cylinder(semidiameter, interface = opaqueinterface(T)))
end

"""
    OddAsphericSurface(semidiameter, curvature::T, conic::T, aspherics::Vector{T}; normradius::T=semidiameter)

Surface incorporating an aspheric polynomial - radius, conic and aspherics are defined relative to absolute semi-diameter.

`aspherics`  should be an array of the odd coefficients of the aspheric polynomial starting with A1

"""
function OddAsphericSurface(semidiameter::T, curvature::T, conic::T, aspherics::Vector{T}; normradius::T=semidiameter) where T<:Real
    Q=length(aspherics)
    AsphericSurface(ODD, semidiameter, curvature, conic, SVector{Q,T}(aspherics), normradius, Cylinder(semidiameter, interface = opaqueinterface(T)))
end

"""
    OddEvenAsphericSurface(semidiameter, curvature::T, conic::T, aspherics::Vector{T}; normradius::T=semidiameter)

Surface incorporating an aspheric polynomial - radius, conic and aspherics are defined relative to absolute semi-diameter.

`aspherics` should be an array of the both odd and even coefficients of the aspheric polynomial starting with A1

"""
function OddEvenAsphericSurface(semidiameter::T, curvature::T, conic::T, aspherics::Vector{T}; normradius::T=semidiameter) where T<:Real
    Q=length(aspherics)
    AsphericSurface(ODDEVEN, semidiameter, curvature, conic, SVector{Q,T}(aspherics), normradius, Cylinder(semidiameter, interface = opaqueinterface(T)))
end

#don't have a function for CONIC as it would better to directly solve for the surface intersection instead of the rootfinding of a ParametricSurface

"""
    asphericType(surf::AsphericSurface)

Query the polynomial type of `asp.  Returns CONIC, ODD, EVEN, or ODDEVEN. CONIC corresponds to no aspheric terms, ODD
means it only has odd aspheric terms, EVEN means only even aspheric terms and ODDEVEN means both even and odd terms.

This function is to enable proper interpretation of `surf.aspherics` by any optimization routines that directly query the aspheric coefficients.
"""
asphericType(z::AsphericSurface{T,3,Q,M}) where {T<:Real,Q,M} = M

export AsphericSurface, EvenAsphericSurface, OddAsphericSurface, OddEvenAsphericSurface, asphericType, EVEN, CONIC, ODD, ODDEVEN

uvrange(::AsphericSurface{T,N}) where {T<:Real,N} = ((zero(T), one(T)), (-T(π), T(π))) # ρ and ϕ

semidiameter(z::AsphericSurface{T}) where {T<:Real} = z.semidiameter
halfsizeu(z::AsphericSurface{T}) where {T<:Real} = semidiameter(z)
halfsizev(z::AsphericSurface{T}) where {T<:Real} = semidiameter(z)

boundingobj(z::AsphericSurface{T}) where {T<:Real} = z.boundingcylinder

prod_step(z::AsphericSurface{T,N,Q,ODDEVEN}, r, r2) where {T<:Real,N,Q} = r, r
prod_step(z::AsphericSurface{T,N,Q,ODD}, r, r2) where {T<:Real,N,Q} = r, r2
prod_step(z::AsphericSurface{T,N,Q,EVEN}, r, r2) where {T<:Real,N,Q} = r2, r2

function point(z::AsphericSurface{T,3,Q,M}, ρ::T, ϕ::T)::SVector{3,T} where {T<:Real,Q,M}
    rad = z.semidiameter
    r = ρ * rad
    r2 = r^2
    t = one(T) - z.curvature^2 * (z.conic + one(T)) * r^2
    if t < zero(T)
        return SVector{3,T}(NaN, NaN, NaN)
    end
    h = z.curvature * r2 / (one(T) + sqrt(t))
   # sum aspheric
    if M != CONIC
        prod, step = prod_step(z, r, r2)  #multiple dispatch on M
        asp,rest = Iterators.peel(z.aspherics)
        h += asp * prod
        for asp in rest
            prod *= step
            h += asp * prod
        end
    end
    return SVector{3,T}(r * cos(ϕ), r * sin(ϕ), h)
end

partial_prod_step(z::AsphericSurface{T,3,Q,EVEN}, r::T, r2::T) where {T<:Real,Q} = r, r2, 2:2:2Q
partial_prod_step(z::AsphericSurface{T,3,Q,ODD}, r::T, r2::T) where {T<:Real,Q} = one(T), r2, 1:2:(2Q-1)
partial_prod_step(z::AsphericSurface{T,3,Q,ODDEVEN}, r::T, r2::T) where {T<:Real,Q} = one(T), r, 1:1:Q

function partials(z::AsphericSurface{T,3,Q,M}, ρ::T, ϕ::T)::Tuple{SVector{3,T},SVector{3,T}} where {T<:Real,Q,M}
    rad = z.semidiameter
    r = ρ * rad
    r2 = r*r
    t = one(T) - z.curvature^2 * (z.conic + one(T)) * r^2
    if t < zero(T)
        return SVector{3,T}(NaN, NaN, NaN), SVector{3,T}(NaN, NaN, NaN)
    end
    dhdρ = rad * z.curvature * r * sqrt(t) / t
    # sum aspherics partial
    if M != CONIC
        prod, step, mIter = partial_prod_step(z, r, r2)

        ((m, asp), rest) = Iterators.peel(zip(mIter,z.aspherics))
        dhdρ += rad * asp * 2 * prod #first term m=1*2 and prod = r
        for (m,asp) in rest
            prod *= step
            dhdρ += rad * m * asp * prod
        end
    end
    dhdϕ = zero(T)
    cosϕ = cos(ϕ)
    sinϕ = sin(ϕ)
    return SVector{3,T}(rad * cosϕ, rad * sinϕ, dhdρ), SVector{3,T}(r * -sinϕ, r * cosϕ, dhdϕ)
end

function normal(z::AsphericSurface{T,3}, ρ::T, ϕ::T)::SVector{3,T} where {T<:Real}
    du, dv = partials(z, ρ, ϕ)
    if ρ == zero(T) && norm(dv) == zero(T)
        # in cases where there is no δϕ at ρ = 0 (i.e. anything which is rotationally symetric)
        # then we get some big problems, hardcoding this case solves the problems
        return SVector{3,T}(0, 0, 1)
    end
    return normalize(cross(du, dv))
end

function uv(z::AsphericSurface{T,3}, p::SVector{3,T}) where {T<:Real}
    # avoid divide by zero for ForwardDiff
    ϕ = NaNsafeatan(p[2], p[1])
    if p[1] == zero(T) && p[2] == zero(T)
        ρ = zero(T)
    else
        ρ = sqrt(p[1]^2 + p[2]^2) / semidiameter(z)
    end
    return SVector{2,T}(ρ, ϕ)
end

function onsurface(surf::AsphericSurface{T,3}, p::SVector{3,T}) where {T<:Real}
    ρ, ϕ = uv(surf, p)
    if ρ > one(T)
        return false
    else
        surfpoint = point(surf, ρ, ϕ)
        return samepoint(p[3], surfpoint[3])
    end
end

function inside(surf::AsphericSurface{T,3}, p::SVector{3,T}) where {T<:Real}
    ρ, ϕ = uv(surf, p)
    if ρ > one(T)
        return false
    else
        surfpoint = point(surf, ρ, ϕ)
        return p[3] < surfpoint[3]
    end
end

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

# Assumes the ray has been transformed into the canonical coordinate frame which has the vertical axis passing through (0,0,0) and aligned with the z axis.
function surfaceintersection(surf::AcceleratedParametricSurface{T,3,AsphericSurface{T,3}}, r::AbstractRay{T,3}) where {T<:Real}
    cylint = surfaceintersection(surf.surface.boundingcylinder, r)
    if cylint isa EmptyInterval{T}
        return EmptyInterval(T)
    else
        if doesintersect(surf.triangles_bbox, r) || inside(surf.triangles_bbox, origin(r))
            surfint = triangulatedintersection(surf, r)
            if !(surfint isa EmptyInterval{T})
                return intervalintersection(cylint, surfint)
            end
        end
        # hasn't hit the surface
        if lower(cylint) isa RayOrigin{T} && upper(cylint) isa Infinity{T}
            if inside(surf.surface, origin(r))
                return Interval(RayOrigin(T), Infinity(T))
            else
                return EmptyInterval(T)
            end
            # otherwise check that the intersection is underneath the surface
        else
            p = point(closestintersection(cylint, false))
            ρ, ϕ = uv(surf, p)
            surfpoint = point(surf.surface, ρ, ϕ)
            if p[3] < surfpoint[3]
                return cylint # TODO!! UV (and interface) issues?
            else
                return EmptyInterval(T)
            end
        end
    end
end

function AcceleratedParametricSurface(surf::T, numsamples::Int = 17; interface::NullOrFresnel{S} = NullInterface(S)) where {S<:Real,N,T<:AsphericSurface{S,N}}
    # Zernike uses ρ, ϕ uv space so need to modify extension of triangulation
    a = AcceleratedParametricSurface(surf, triangulate(surf, numsamples, true, false, true, false), interface = interface)
    emptytrianglepool!(S)
    return a
end

function asphericSag(surf::AsphericSurface{T,3,Q, EVEN}) where {T<:Real,Q}
    amin = sum(k < zero(T) ? k * surf.semidiameter^(2m) : zero(T) for (m, k) in enumerate(surf.aspherics))
    amax = sum(k > zero(T) ? k * surf.semidiameter^(2m) : zero(T) for (m, k) in enumerate(surf.aspherics))
    return amin, amax
end

function asphericSag(surf::AsphericSurface{T,3,Q, ODD}) where {T<:Real,Q}
    amin = sum(k < zero(T) ? k * surf.semidiameter^(2m-1) : zero(T) for (m, k) in enumerate(surf.aspherics))
    amax = sum(k > zero(T) ? k * surf.semidiameter^(2m-1) : zero(T) for (m, k) in enumerate(surf.aspherics))
    return amin, amax
end

function asphericSag(surf::AsphericSurface{T,3,Q, ODDEVEN}) where {T<:Real,Q}
    amin = sum(k < zero(T) ? k * surf.semidiameter^(m) : zero(T) for (m, k) in enumerate(surf.aspherics))
    amax = sum(k > zero(T) ? k * surf.semidiameter^(m) : zero(T) for (m, k) in enumerate(surf.aspherics))
    return amin, amax
end

function asphericSag(surf::AsphericSurface{T,3,Q, CONIC}) where {T<:Real,Q}
    amin = zero(T)
    amax = zero(T)
    return amin, amax
end

function BoundingBox(surf::AsphericSurface{T,3}) where {T<:Real}
    xmin = -semidiameter(surf)
    xmax = semidiameter(surf)
    ymin = -semidiameter(surf)
    ymax = semidiameter(surf)
    # aspherics can be more comlpicated, so just take sum of all negative and all positives
    amin, amax = asphericSag(surf)
    zmin = amin
    zmax = amax
    # curvature only goes one way
    q = one(T) - (one(T) + surf.conic) * surf.curvature^2 * surf.semidiameter^2
    if q < zero(T)
        throw(ErrorException("The surface is invalid, no bounding box can be constructed"))
    end
    hmax = surf.curvature * surf.semidiameter^2 / (one(T) + sqrt(q))
    if hmax > zero(T)
        zmax += hmax
    else
        zmin += hmax
    end
    return BoundingBox(xmin, xmax, ymin, ymax, zmin, zmax)
end


text:
@testset "FFJORD Model" begin
    @testset "1-var" begin
        @test @timev !isnothing(one_var_test((n_vars, dist) -> FFJORDModel()))
    end
    @testset "2-var" begin
        @test @timev !isnothing(multi_var_test((n_vars, dist) -> FFJORDModel()))
    end
    @testset "3-var" begin
        @test @timev !isnothing(multi_var_test((n_vars, dist) -> FFJORDModel(), n_vars=3))
    end
end

ext: jl


text:
using Javis

function ground(args...)
    background("white")
    sethue("white")
end

video = Video(400, 450)

U = 50
SHIFT = Point(0, -1.7U)

function tangram(obj)
    if obj == :triangle1
        sethue("#C9E891")
        [O, Point(2U, 0), Point(0, 2U)]
    elseif obj == :triangle2
        sethue("#69C0D2")
        [O, Point(-2U, 0), Point(0, 2U)]
    elseif obj == :triangle3
        sethue("#F8CB2D")
        [O, Point(-U,U), Point(U,U)]
    elseif obj == :triangle4
        sethue("#F195C8")
        [O, Point(-U, 0), Point(0,-U)]
    elseif obj == :triangle5
        sethue("#F9F224")
        [O, Point(U, 0), Point(0, -U)]
    elseif obj == :box
        sethue("#A796C2")
        [O, Point(0, -U), Point(U,-U), Point(U, 0)]
    elseif obj == :parallelogram
        sethue("#EF3E62")
        [O, Point(U,-U), Point(0, -U), Point(-U, 0)]
    end
end

javis(video, [
    BackgroundAction(1:400, ground),
    Action(1:400, (args...)->poly(tangram(:triangle1), :fill, close=true); subactions=[
        SubAction(1:1, Translation(O, SHIFT)),
        SubAction(50:60, Translation(O, Point(0, 2U))),
        SubAction(60:70, Rotation(0., -π/2)),
        SubAction(70:80, Translation(O, Point(-2U, 0))),
        SubAction(80:90, Translation(O, Point(U, -U))),
    ]),
    Action(1:400, (args...)->poly(tangram(:triangle2), :fill, close=true); subactions=[
        SubAction(1:1, Translation(O, SHIFT)),
    ]),
    Action(1:400, (args...)->poly(tangram(:triangle3), :fill, close=true); subactions=[
        SubAction(1:1, Translation(O, SHIFT)),
        SubAction(1:1, Translation(O, Point(0, -2U))),
        SubAction(100:110, Translation(O, Point(-3U, 0))),
        SubAction(110:120, Rotation(0., -π/2)),
        SubAction(120:130, Translation(O, Point(-5.5U, 0))),
        SubAction(130:140, Translation(O, Point(0, U))),
    ]),
    Action(1:400, (args...)->poly(tangram(:triangle5), :fill, close=true); subactions=[
        SubAction(1:1, Translation(O, SHIFT)),
        SubAction(1:1, Translation(O, Point(U, 0))),
        SubAction(150:160, Translation(O, Point(0, 4.5*U))),
        SubAction(160:170, Rotation(0., -3π/4)),
        SubAction(170:180, Translation(O, Point(sqrt((2 - √2/2)^2 / 2) * U, - sqrt((2 - √2/2)^2 / 2) * U))),
    ]),
    Action(1:400, (args...)->poly(tangram(:parallelogram), :fill, close=true); subactions=[
        SubAction(1:1, Translation(O, SHIFT)),
        SubAction(1:1, Translation(O, Point(-U, 0))),
        SubAction(200:210, Translation(O, Point(U, -U))),
        SubAction(210:220, Translation(O, Point(U, 0))),
        SubAction(220:230, Rotation(0., π/2)),
        SubAction(230:240, Translation(O, Point(2U, 0))),
        SubAction(240:250, Translation(O, Point(0, U))),
    ]),
    Action(1:400, (args...)->poly(tangram(:triangle4), :fill, close=true);subactions=[
        SubAction(1:1, Translation(O, SHIFT)),
        SubAction(250:260, Translation(O, Point(0, -U))),
        SubAction(260:270, Translation(O, Point(2U, 0))),
        SubAction(270:280, Rotation(0., -π/2)),
        SubAction(280:290, Translation(O, Point(-4U, 0))),
        SubAction(290:300, Translation(O, Point(0.5*U, -0.5*U))),
    ]),
    Action(1:400, (args...)->poly(tangram(:box), :fill, close=true);subactions=[
        SubAction(1:1, Translation(O, SHIFT)),
        SubAction(300:310, Translation(O, Point(-U, 0))),
        SubAction(310:320, Rotation(0., -π/6)),
    ]),
]; pathname="logo.gif")


text:
#!/usr/bin/env julia

# requires the Gtk library:
#  https://github.com/JuliaGraphics/Gtk.jl

# run with:
#  julia HelloworldGtk.jl

using Gtk

function on_button_clicked(w)
    exit()
end

win = GtkWindow("Hello World", 300, 300)

button = GtkButton("Hello World!")
push!(win, button)

signal_connect(on_button_clicked, button, "clicked")

showall(win)

signal_connect(win, :destroy) do widget
    Gtk.gtk_quit()
end

Gtk.gtk_main()

max_stars_repo_name: xgqt/gtkdemos

max_issues_repo_name: xgqt/gtkdemos

max_forks_repo_name: xgqt/gtkdemos


text:
"""
Abstract type for Decision Model and Emulation Model. OperationModel structs are parameterized with DecisionProblem or Emulation Problem structs
"""
abstract type OperationModel end

#TODO: Document the required interfaces for custom types
"""
Abstract type for Decision Problems

# Example

import PowerSimulations
const PSI = PowerSimulations
struct MyCustomProblem <: PSI.DecisionProblem
"""
abstract type DecisionProblem end

"""
Abstract type for Emulation Problems

# Example

import PowerSimulations
const PSI = PowerSimulations
struct MyCustomEmulator <: PSI.EmulationProblem
"""
abstract type EmulationProblem end


text:
# Simple test of function run_geno_pheno_evolution()
# include("../src/CGP.jl") # Assumes that CGP.jl is loaded
# The following lines allow this file to included from CGP.jl, CGP.jl/test, or CGP.jl/src.
cwd = pwd()
if cwd[end-5:end] == "CGP.jl"
  csvfile = "$cwd/test/data/test_run_geno_pheno_evolve.csv"
else
  csvfile = "../test/data/test_run_geno_pheno_evolve.csv"
end
allgoals=false
iterations = allgoals ? 2 : 8
numinputs = 4
numoutputs = 1
nodearity = 2
numinteriors = 10
ngoals = 1
levelsback=numinteriors-5
maxsteps = 100000
df = run_geno_pheno_evolution( iterations, numinputs, numoutputs, numinteriors, ngoals, maxsteps, levelsback, csvfile, allgoals=allgoals )


text:
# Implements standard manipulation operation with Decision Diagrams

export
    ¬,
    ∧,
    ∨,
    apply,
    reduce,
    restrict,
    marginalize

# OPERATIONS
"Returns the complement of the function."
@inline (¬)(α::Node) = Node(index(α), ¬low(α), ¬high(α))
@inline (¬)(α::Terminal{Bool}) = value(α) ? Terminal(false) : Terminal(true)
@inline (¬)(α::Terminal{T}) where T = Terminal(one(T) - value(α))
"Returns the conjunction of the Boolean functions."
@inline (∧)(α::DecisionDiagram{Bool}, β::DecisionDiagram{Bool}) = apply(&, α, β)
"Returns the disjunction of the Boolean functions."
@inline (∨)(α::DecisionDiagram{Bool}, β::DecisionDiagram{Bool}) = apply(|, α, β)
@inline Base.:+(α::DecisionDiagram{T}, β::DecisionDiagram{T}) where T = apply(+, α, β)
"Returns the subtraction of the respective functions."
@inline Base.:-(α::DecisionDiagram{T}, β::DecisionDiagram{T}) where T = apply(-, α, β)
"Returns the product of the respective functions."
@inline Base.:*(α::DecisionDiagram{T}, β::DecisionDiagram{T}) where T = apply(*, α, β)
@inline Base.:*(c::Number,α::Terminal) = Terminal(c * value(α))
@inline Base.:*(c::Number,α::DecisionDiagram) = Node(index(α),c * low(α),c * high(α))
"Returns a Diagram canonical representation of α OP β, where OP is some binary operator."
@inline apply(OP::Function,α::DecisionDiagram{T}, β::DecisionDiagram{T}) where T = apply(OP, α, β, Dict{Tuple{Int, Int}, DecisionDiagram{T}}(), Dict{T,DecisionDiagram{T}}(),Dict{Tuple{Int,Int,Int},DecisionDiagram{T}}())
"""
    apply(OP,α,β,opcache,vcache)

Recursively computes α OP β and cache results.

# Parameters
- `OP`: operation to apply to leaves
- `α`,`β`: decision diagrams
- `opcache`: cache of applied operations
- `vcache`: cache of terminal values
"""
function apply(OP::Function, α::DecisionDiagram{T}, β::DecisionDiagram{T}, opcache::Dict{Tuple{Int, Int}, DecisionDiagram{T}}, vcache::Dict{T,DecisionDiagram{T}}, ncache::Dict{Tuple{Int,Int,Int},DecisionDiagram{T}}) where T
    key = (id(α), id(β))
    if haskey(opcache, key) return opcache[key] end
    #TODO: check identity and anihilator cases
    local γ::DecisionDiagram{T}
    if isa(α,Terminal) && isa(β,Terminal)
        constant = OP(value(α),value(β))
        γ = get!(vcache, constant, Terminal(constant))
        # if haskey(vcache, constant) # avoid duplicate terminals
        #     γ = vcache[constant]
        # else
        #     γ = Terminal(constant)
        #     vcache[constant] = γ
        # end
    else # decompose on least variable (terminals are associated with upper variables)
        if index(α) < index(β) # decompose on variable index(α)
            l = apply(OP, low(α), β, opcache, vcache, ncache)
            h = apply(OP, high(α), β, opcache, vcache, ncache)
        elseif index(α) > index(β) # decompose on variable index(β)
            l = apply(OP, α, low(β), opcache, vcache, ncache)
            h = apply(OP, α, high(β), opcache, vcache, ncache)
        else # both DDs are rooted at the same variable
            l = apply(OP, low(α), low(β), opcache, vcache, ncache)
            h = apply(OP, high(α), high(β), opcache, vcache, ncache)
        end
        if id(l) == id(h) # redundant node
            γ = l
            #γ = index(α) <= index(β) ? l : h
        elseif haskey(ncache,(min(index(α),index(β)),id(l),id(h))) # duplicate or already visited node
            γ = ncache[min(index(α),index(β)),id(l),id(h)]
        else
            γ = ncache[min(index(α),index(β)),id(l),id(h)] = Node(min(index(α),index(β)), l, h)
        end
    end
    opcache[key] = γ
end
"""
    apply(f::Function, α::DecisionDiagram)

Returns the application of function f to α.
"""
function apply(f::Function,α::DecisionDiagram{T}) where T
    apply(f,α,Dict{Tuple{Int,Int,Int},DecisionDiagram{T}}(),Dict{T,DecisionDiagram{T}}())
end
function apply(f::Function,α::Terminal{T}, cache::Dict{Tuple{Int,Int,Int},DecisionDiagram{T}}, vcache::Dict{T,DecisionDiagram{T}}) where T
    v = f(value(α))
    get!(vcache,v,Terminal(v))
end
function apply(f::Function,α::DecisionDiagram{T}, cache::Dict{Tuple{Int,Int,Int},DecisionDiagram{T}}, vcache::Dict{T,DecisionDiagram{T}}) where T
    left, right = apply(f,low(α),cache,vcache), apply(f,high(α),cache,vcache)
    if id(left) == id(right) # redundant node
        return left
    end
    # if there is a node with the same variable and children, return it
    # otherwise, create it
    get!(cache, (index(α),id(left),id(right)), Node{T}(id(α),index(α),left,right))
end

"""
    reduce(α::DecisionDiagram)

Returns the canonical form of the decision diagram α.
A diagram is canonical iff:
- no two terminals are assigned to the same value
- no two nodes have are labeled by the same variable and have the same two reduced children low and high with identical ids

"""
function reduce(α::DecisionDiagram{T}) where T
    reduce(α,Dict{Tuple{Int,Int,Int},DecisionDiagram{T}}(),Dict{T,DecisionDiagram{T}}())
end
"""
reduce(α::DecisionDiagram{T}, cache::Dict{Tuple{Int,Int,Int},DecisionDiagram{T}}, vcache::Dict{T,DecisionDiagram{T}})

Returns the canonical form of decision diagram α using node and value caches.
A diagram is canonical iff:
    - no two terminals are assigned to the same value
    - no two nodes have are labeled by the same variable and have the same two reduced children low and high with identical ids

# Arguments
- `α`: decision diagram to reduce.
- `cache`: cache of inner nodes (identified by ids of variable, low and high children).
- `vcache::Dict{T,DecisionDiagram{T}}`: cache of terminal nodes identified by the associated value.
"""
reduce(α::Terminal{T}, cache::Dict{Tuple{Int,Int,Int},DecisionDiagram{T}}, vcache::Dict{T,DecisionDiagram{T}}) where T = get!(vcache,value(α),α)
function reduce(α::DecisionDiagram{T}, cache::Dict{Tuple{Int,Int,Int},DecisionDiagram{T}}, vcache::Dict{T,DecisionDiagram{T}}) where T
    left, right = reduce(low(α),cache,vcache), reduce(high(α),cache,vcache)
    if id(left) == id(right) # redundant node
        return left
    end
    # if there is a node with the same variable and children, return it
    # otherwise, create it
    get!(cache, (index(α),id(left),id(right)), Node{T}(id(α),index(α),left,right))
    # elseif haskey(cache,(index(α),id(left),id(right))) # duplicate or already visited node
    #     return cache[index(α),id(left),id(right)]
    # end
    # cache[index(α),id(left),id(right)] = Node{T}(id(α),index(α),left,right)
end

"""
    restrict(α::DecisionDiagram, var::Int, value::Bool=true)

Returns the canonical form of the function α constrained at variable var=value.

# Example
```julia
x, y = variable(1), variable(2)
f = (one(x)-x)*(one(y)-1) + x*y
g = Terminal(4)*(one(x)-x) + Terminal(2)*x
h = f + g
r1 = restrict(h,1,false)
r2 = h | (2 => true) # shorthand for restrict(h,2,true)
r3 = h | (y => true) # same as above
```
"""
function restrict(α::DecisionDiagram, var::Int, value::Bool=true)
    if isa(α,Terminal) return α end
    if index(α) != var
        l = restrict(low(α),var,value)
        h = restrict(high(α),var,value)
        return Node(index(α), l, h)
    end
    if value return restrict(high(α),var,value) end
    restrict(low(α),var,value)
end
@inline Base.:|(α::DecisionDiagram,a::Pair{Int,Bool}) = restrict(α,a[1],a[2])

"Marginalize a variable var from function α."
@inline marginalize(α::Terminal, var::Int) = α + α
#marginalize(α::Node{T}, var::Int) = reduce(marginalize_step(α, var))
marginalize(α::DecisionDiagram, var::Int) = restrict(α,var,true) + restrict(α,var,false)
# function marginalize(α::DecisionDiagram, var::Int)
#   # produces non-canonical form (perhaps more efficient?)
#   if index(α) == var return low(α) + high(α) end
#   Node(index(α), marginalize(low(α), var), marginalize(high(α), var))
# end


text:
include("initialize.jl")
using Plots

##
# Make the ODEProblem
PD_prob = let
	## Inputs
    # Model constants
    g = 9.80665

    # Control Signal
    saturation_lims = (-50, 50)
    control, u = PD_controller(
        kp = 1000f0, # Proportional gain
        kd = 300f0, # Derivative gain
    )
    # control, u = dense_neural_controller(50, Flux.relu, only)

	# Simulation parameters
	tf = 5.0 # Stop time (s)

	# Model parameters
	m = 100 # Rod mass (kg)
	L = 0.5 # Rod length (m)
	c = 0.1 # Damping coefficient

	# Initial conditions
	θ₀ = deg2rad(-20) # Initial angle
	ω₀ = deg2rad(45) # Initial angular velocity

    p = (; g, m, L, c, I=(1/3)*m*L^2, control)
	ic = ComponentArray{Float32}(θ=θ₀, ω=ω₀)

    u_sat = Saturated(u, saturation_lims...)
	ODEProblem(pendulum_with_control(u_sat), ic, Float32.((0.0, tf)), p)
end

neural_prob = let prob=deepcopy(PD_prob)
    params, u = dense_neural_controller(50, Flux.relu, only)
    f = prob.f.f
    @set! f.u.f = u
    p = ComponentArray(; NamedTuple(prob.p)..., control=params)
    ODEProblem(f, prob.u0, prob.tspan, p; prob.kwargs...)
end

PD_run_func = make_run_func(PD_prob; saveat=0.1)
neural_run_func = make_run_func(neural_prob; saveat=0.1)

## Solve the plain ODEProblem
PD_sol = solve(PD_prob)
neural_sol = solve(neural_prob)

## Plot stuff
plot(plot(PD_sol, title="PD"), plot(neural_sol, title="Neural"), layout=(2,1), size=(700,500))

## Buiild optimization problems
evaluate_sol = sol -> sum(abs2, Array(sol))
PD_obj = make_objective(PD_prob, evaluate_sol; sensealg=InterpolatingAdjoint())
cb = make_callback(; plot_every=100, doplot=true)

## Solve optimization problem'
x0 = ComponentArray{Float64}(kp=1000, kd=100)
res1 = DiffEqFlux.sciml_train(PD_obj, x0, ADAM(0.05); maxiters=500, cb)
# cb(res1.minimizer, loss_n_ode(res1.minimizer)...; doplot=true)

res2 = DiffEqFlux.sciml_train(PD_obj, res1.minimizer, Newton(); cb)
# cb(res2.minimizer, loss_n_ode(res2.minimizer)...; doplot=true)


text:
function SRAUKFTUIntermediate(T::Type, num_x::Number)
    xi_temp = Vector{T}(undef, num_x)
    qr_zeros = zeros(T, 4 * num_x)
    qr_A = Matrix{T}(undef, 4 * num_x, num_x)
    qr_space_length = calc_gels_working_size(qr_A, qr_zeros)
    SRUKFTUIntermediate(
        Augmented(Matrix{T}(undef, num_x, num_x), Matrix{T}(undef, num_x, num_x)),
        Augmented(xi_temp, xi_temp),
        xi_temp,
        TransformedSigmaPoints(Vector{T}(undef, num_x), Matrix{T}(undef, num_x, 4 * num_x), MeanSetWeightingParameters(0.0)), # Weighting parameters will be reset
        TransformedSigmaPoints(Vector{T}(undef, num_x), Matrix{T}(undef, num_x, 4 * num_x), MeanSetWeightingParameters(0.0)),
        qr_zeros,
        Vector{T}(undef, qr_space_length),
        qr_A,
        Vector{T}(undef, num_x),
        Matrix{T}(undef, num_x, num_x)
    )
end

SRAUKFTUIntermediate(num_x::Number) = SRAUKFTUIntermediate(Float64, num_x)

function SRAUKFMUIntermediate(T::Type, num_x::Number, num_y::Number)
    qr_zeros = zeros(T, 2 * num_x + 2 * num_y)
    qr_A = Matrix{T}(undef, 2 * num_x + 2 * num_y, num_y)
    qr_space_length = calc_gels_working_size(qr_A, qr_zeros)
    SRUKFMUIntermediate(
        Augmented(Matrix{T}(undef, num_x, num_x), Matrix{T}(undef, num_y, num_y)),
        Augmented(Vector{T}(undef, num_x), Vector{T}(undef, num_y)),
        Vector{T}(undef, num_y),
        Vector{T}(undef, num_y),
        TransformedSigmaPoints(Vector{T}(undef, num_y), Matrix{T}(undef, num_y, 2 * num_x + 2 * num_y), MeanSetWeightingParameters(0.0)), # Weighting parameters will be reset
        TransformedSigmaPoints(Vector{T}(undef, num_y), Matrix{T}(undef, num_y, 2 * num_x + 2 * num_y), MeanSetWeightingParameters(0.0)),
        Vector{T}(undef, num_y),
        qr_zeros,
        Vector{T}(undef, qr_space_length),
        qr_A,
        Matrix{T}(undef, num_y, num_y),
        Matrix{T}(undef, num_x, num_y),
        Matrix{T}(undef, num_x, num_y),
        Vector{T}(undef, num_x),
        Matrix{T}(undef, num_x, num_x)
    )
end

SRAUKFMUIntermediate(num_x::Number, num_y::Number) = SRAUKFMUIntermediate(Float64, num_x, num_y)

function time_update!(tu::SRUKFTUIntermediate, x, P::Union{<:AbstractMatrix, <:Cholesky}, f!, Q::Augment; weight_params::AbstractWeightingParameters = WanMerweWeightingParameters())
    time_update!(tu, x, Augmented(P, Q), f!, Q, weight_params = weight_params)
end

function measurement_update!(mu::SRUKFMUIntermediate, x, P::Union{<:AbstractMatrix, <:Cholesky}, y, h!, R::Augment; weight_params::AbstractWeightingParameters = WanMerweWeightingParameters())
    measurement_update!(mu, x, Augmented(P, R), y, h!, R, weight_params = weight_params)
end


text:
# ------------------------------------------------------------------
# Licensed under the ISC License. See LICENSE in the project root.
# ------------------------------------------------------------------

"""
    SimulationProblem(sdata, sdomain, targetvars, nreals)
    SimulationProblem(sdomain, targetvars, nreals)

A spatial simulation problem on a given spatial domain `sdomain`
in which the variables to be simulated are listed in `targetvars`.

For conditional simulation, the data of the problem is stored in
spatial data `sdata`.

For unconditional simulation, a list of pairs `targetvars` must be
provided mapping variable names to their types.

In both cases, a number `nreals` of realizations is requested.

## Examples

Create a conditional simulation problem for porosity and permeability
with 100 realizations:

```julia
julia> SimulationProblem(sdata, sdomain, (:porosity,:permeability), 100)
```

Create an unconditional simulation problem for porosity and facies type
with 100 realizations:

```julia
julia> SimulationProblem(sdomain, (:porosity => Float64, :facies => Int), 100)
```
"""
struct SimulationProblem{S,D,M} <: AbstractProblem
  # input fields
  sdata::S
  sdomain::D
  mapper::M
  targetvars::Dict{Symbol,DataType}
  nreals::Int

  # state fields
  maps::Dict{Symbol,Dict{Int,Int}}

  function SimulationProblem{S,D,M}(sdata, sdomain, targetvars, nreals, mapper) where {S,D,M}
    pnames = Tuple(keys(targetvars))

    @assert !isempty(pnames) "target variables must be specified"
    @assert nreals > 0 "number of realizations must be positive"

    if isnothing(sdata)
      maps   = Dict(var => Dict() for var in pnames)
    else
      vnames = name.(variables(sdata))
      dmaps  = map(sdata, sdomain, vnames, mapper)
      omaps  = Dict(var => Dict() for var in pnames if var ∉ vnames)
      maps   = merge(dmaps, omaps)
    end

    new(sdata, sdomain, mapper, targetvars, nreals, maps)
  end
end

const VarType      = Pair{Symbol,DataType}
const VarOrVarType = Union{Symbol,VarType}

function SimulationProblem(sdata::S, sdomain::D, vars::NTuple{N,VarOrVarType}, nreals::Int;
                           mapper::M=NearestMapper()) where {S,D,M,N}
  datavars = Dict(name(var) => mactype(var) for var in variables(sdata))

  # pairs with variable names and types
  varstypes = map(vars) do vt
    if vt isa Symbol # for variables without type, find the type in spatial data
      var = vt
      if var ∈ keys(datavars)
        var => datavars[var]
      else
        @error "please specify the type of target variable $var"
      end
    else # for variables with type, make sure the type is valid
      var, T = vt
      if var ∈ keys(datavars)
        U = datavars[var]
        @assert U <: T "type $T for variable $var cannot hold values of type $U in spatial data"
      end
      vt
    end
  end

  targetvars = Dict(varstypes)

  @assert coordtype(sdata) == coordtype(sdomain) "data and domain must have the same coordinate type"

  SimulationProblem{S,D,M}(sdata, sdomain, targetvars, nreals, mapper)
end

SimulationProblem(sdata::S, sdomain::D, var::VarOrVarType, nreals::Int;
                  mapper::M=NearestMapper()) where {S,D,M} =
  SimulationProblem(sdata, sdomain, (var,), nreals; mapper=mapper)

SimulationProblem(sdomain::D, vars::NTuple{N,VarType}, nreals::Int;
                  mapper::M=NearestMapper()) where {D,M,N} =
  SimulationProblem{Nothing,D,M}(nothing, sdomain, Dict(vars), nreals, mapper)

SimulationProblem(sdomain::D, var::VarType, nreals::Int;
                  mapper::M=NearestMapper()) where {D,M} =
  SimulationProblem(sdomain, (var,), nreals; mapper=mapper)

"""
    data(problem)

Return the spatial data of the simulation `problem`.
"""
data(problem::SimulationProblem) = problem.sdata

"""
    domain(problem)

Return the spatial domain of the simulation `problem`.
"""
domain(problem::SimulationProblem) = problem.sdomain

"""
    mapper(problem)

Return the mapper of the simulation `problem`.
"""
mapper(problem::SimulationProblem) = problem.mapper

"""
    variables(problem)

Return the target variables of the simulation `problem` and their types.
"""
variables(problem::SimulationProblem) = problem.targetvars

"""
    datamap(problem, targetvar)

Return the mapping from domain locations to data locations for the
`targetvar` of the `problem`.
"""
datamap(problem::SimulationProblem, var::Symbol) = problem.maps[var]

"""
    datamap(problem)

Return the mappings from domain locations to data locations for all
the variables of the `problem`.
"""
datamap(problem::SimulationProblem) = problem.maps

"""
    hasdata(problem)

Return `true` if simulation `problem` has data.
"""
hasdata(problem::SimulationProblem) = !isnothing(problem.sdata)

"""
    nreals(problem)

Return the number of realizations of the simulation `problem`.
"""
nreals(problem::SimulationProblem) = problem.nreals

# ------------
# IO methods
# ------------
function Base.show(io::IO, problem::SimulationProblem)
  N = ncoords(problem.sdomain)
  kind = hasdata(problem) ? "conditional" : "unconditional"
  print(io, "$(N)D SimulationProblem ($kind)")
end

function Base.show(io::IO, ::MIME"text/plain", problem::SimulationProblem)
  vars = ["$var ($T)" for (var,T) in problem.targetvars]
  println(io, problem)
  if problem.sdata ≠ nothing
    println(io, "  data:      ", problem.sdata)
  end
  println(io, "  domain:    ", problem.sdomain)
  println(io, "  variables: ", join(vars, ", ", " and "))
  print(  io, "  N° reals:  ", problem.nreals)
end


text:
@testset "positions.jl" begin

    print("\n\n ####### positions.jl #######\n\n")

    location = SaveLocation(;prefix=PREFIX)
    pdd = PositionDataDescriptor(:box_pbc, 1, 8, 100, [0.1,0.2], location)
    file = datapath(pdd)
    @test file == datapath(PositionDataDescriptor(:box_pbc, 1, 8, 100, [0.1,0.2]); prefix=PREFIX)
    @test file == datapath(PositionDataDescriptor(:box_pbc, 1, 8, 100, [0.1,0.2]), location)
    @test file == datapath(PositionDataDescriptor(:box_pbc, 1, 8), location)
    # clean if exists for some reason
    if isfile(file)
        rm(file)
    end
    pd = load_or_create(pdd)
    @test isfile(file)
    rm(file)
    save(pd)
    @test isfile(file)
    pd2 = load(pdd)
    pd3 = load_positions(:box_pbc, 1, 8; prefix=PREFIX)
    @test pd3 !== nothing # just check whether load worked
    for f in fieldnames(typeof(pd))
        @test getproperty(pd2, f) == getproperty(pd, f)
    end
end


text:
AnyIntType = Union{Int8, Int32, Int64, Int128, Int}

@enum BrainFlowLogLib begin

    BOARD_CONTROLLER = 0
    DATA_HANDLER = 1
    ML_MODULE = 2

end

@enum LogLevels begin

    LEVEL_TRACE = 0
    LEVEL_DEBUG = 1
    LEVEL_INFO = 2
    LEVEL_WARN = 3
    LEVEL_ERROR = 4
    LEVEL_CRITICAL = 5
    LEVEL_OFF = 6

end

function enable_brainflow_logger(log_lib::AnyIntType)
    set_log_level(Integer(LEVEL_INFO), Integer(log_lib))
end

function enable_dev_brainflow_logger(log_lib::AnyIntType)
    set_log_level(Integer(LEVEL_TRACE), Integer(log_lib))
end

function disable_brainflow_logger(log_lib::AnyIntType)
    set_log_level(Integer(LEVEL_OFF), Integer(log_lib))
end

# available only for BoardController, no need to provide log_lib
function log_message(log_level::AnyIntType, message::String)
    ec = Integer(STATUS_OK)
    # due to this bug https://github.com/JuliaLang/julia/issues/29602 libname should be hardcoded
    if Sys.iswindows()
        ec = ccall((:log_message, "BoardController.dll"), Cint, (Cint, Ptr{UInt8}), Int32(log_level), message)
    elseif Sys.isapple()
        ec = ccall((:log_message, "libBoardController.dylib"), Cint, (Cint, Ptr{UInt8}), Int32(log_level), message)
    else
        ec = ccall((:log_message, "libBoardController.so"), Cint, (Cint, Ptr{UInt8}), Int32(log_level), message)
    end
    if ec != Integer(STATUS_OK)
        throw(BrainFlowError(string("Error in log_message ", ec), ec))
    end
end

function set_log_file(log_file::String, log_lib::AnyIntType)
    ec = STATUS_OK
    # due to this bug https://github.com/JuliaLang/julia/issues/29602 libname should be hardcoded
    if Integer(log_lib) == Integer(BOARD_CONTROLLER)
        if Sys.iswindows()
            ec = ccall((:set_log_file, "BoardController.dll"), Cint, (Ptr{UInt8},), log_file)
        elseif Sys.isapple()
            ec = ccall((:set_log_file, "libBoardController.dylib"), Cint, (Ptr{UInt8},), log_file)
        else
            ec = ccall((:set_log_file, "libBoardController.so"), Cint, (Ptr{UInt8},), log_file)
        end
    elseif Integer(log_lib) == Integer(DATA_HANDLER)
        if Sys.iswindows()
            ec = ccall((:set_log_file, "DataHandler.dll"), Cint, (Ptr{UInt8},), log_file)
        elseif Sys.isapple()
            ec = ccall((:set_log_file, "libDataHandler.dylib"), Cint, (Ptr{UInt8},), log_file)
        else
            ec = ccall((:set_log_file, "libDataHandler.so"), Cint, (Ptr{UInt8},), log_file)
        end
    elseif Integer(log_lib) == Integer(ML_MODULE)
        if Sys.iswindows()
            ec = ccall((:set_log_file, "MLModule.dll"), Cint, (Ptr{UInt8},), log_file)
        elseif Sys.isapple()
            ec = ccall((:set_log_file, "libMLModule.dylib"), Cint, (Ptr{UInt8},), log_file)
        else
            ec = ccall((:set_log_file, "libMLModule.so"), Cint, (Ptr{UInt8},), log_file)
        end
    else
        ec = Integer(INVALID_ARGUMENTS_ERROR)
    end
    if ec != Integer(STATUS_OK)
        throw(BrainFlowError(string("Error in set_log_file ", ec), ec))
    end
end

function set_log_level(log_level::AnyIntType, log_lib::AnyIntType)
    ec = Integer(STATUS_OK)
    # due to this bug https://github.com/JuliaLang/julia/issues/29602 libname should be hardcoded
    if Integer(log_lib) == Integer(BOARD_CONTROLLER)
        if Sys.iswindows()
            ec = ccall((:set_log_level, "BoardController.dll"), Cint, (Cint,), Int32(log_level))
        elseif Sys.isapple()
            ec = ccall((:set_log_level, "libBoardController.dylib"), Cint, (Cint,), Int32(log_level))
        else
            ec = ccall((:set_log_level, "libBoardController.so"), Cint, (Cint,), Int32(log_level))
        end
    elseif Integer(log_lib) == Integer(DATA_HANDLER)
        if Sys.iswindows()
            ec = ccall((:set_log_level, "DataHandler.dll"), Cint, (Cint,), Int32(log_level))
        elseif Sys.isapple()
            ec = ccall((:set_log_level, "libDataHandler.dylib"), Cint, (Cint,), Int32(log_level))
        else
            ec = ccall((:set_log_level, "libDataHandler.so"), Cint, (Cint,), Int32(log_level))
        end
    elseif Integer(log_lib) == Integer(ML_MODULE)
        if Sys.iswindows()
            ec = ccall((:set_log_level, "MLModule.dll"), Cint, (Cint,), Int32(log_level))
        elseif Sys.isapple()
            ec = ccall((:set_log_level, "libMLModule.dylib"), Cint, (Cint,), Int32(log_level))
        else
            ec = ccall((:set_log_level, "libMLModule.so"), Cint, (Cint,), Int32(log_level))
        end
    else
        ec = Integer(INVALID_ARGUMENTS_ERROR)
    end
    if ec != Integer(STATUS_OK)
        throw(BrainFlowError(string("Error in set_log_level ", ec), ec))
    end
end

max_stars_repo_name: Cognixion-inc/brainflow

max_issues_repo_name: Cognixion-inc/brainflow

max_forks_repo_name: Cognixion-inc/brainflow


text:
type ClaytonPC <: ParamPC
    θ::Float64

    function ClaytonPC(θ::Float64)
        if θ <= 0
            error("Clayton parameter must be larger than 0")
        end
        new(θ)
    end

end

function params(cop::ClaytonPC)
    return (cop.θ,)
end

#####################
## copula function ##
#####################

## pdf function
##-------------

## function pdf(cop::ClaytonPC, u1::FloatVec, u2::FloatVec)
##     θ = params(cop)[1]

##     val = (θ+1).*(u1.*u2).^(-θ-1) .*
##     				(u1.^-θ + u2.^-θ - 1).^(-(2θ+1)/θ)
##     return val
## end


text:
using Pkg
Pkg.activate(pwd())

# # Strings

str = "Hello, world."
str[1] # returns the first character

#+

typeof(str[1])
'x'

#+

Int('x')
Char(120)

#+

str[1:5] # returns the first five characters
str[[1,2,5,6]]

#+

str[1] # returns the first character as Char
str[1:1] # returns the first character as String

#+

str1 = "This is how a string is created: \"string\"."
str2 = "\$\$\$ dollars everywhere \$\$\$"

println(str1)
println(str2)

#+

mstr = """
This is how a string is created: "string".
"""
print(mstr)

#+

str = """
  Hello,
  world.
"""
print(str)

# ### Exercise:
# Create a string with the following text
#
# > Quotation is the repetition or copy of someone else's statement or thoughts.
# > Quotation marks are punctuation marks used in text to indicate a quotation.
# > Both of these words are sometimes abbreviated as "quote(s)".
#
# and print it into the REPL. The printed string should look the same as the text above,
# i.e., each sentence should be on a separate line. Use an indent of length 4 for each
# sentence.
#
# ---
# ### Solution:

str = """
    Quotation is the repetition or copy of someone else's statement or thoughts.
    Quotation marks are punctuation marks used in text to indicate a quotation.
    Both of these words are sometimes abbreviated as "quote(s)".
""";

println(str)

str = "    Quotation is the repetition or copy of someone else's statement or thoughts.\n    Quotation marks are punctuation marks used in text to indicate a quotation.\n    Both of these words are sometimes abbreviated as \"quote(s)\".";

println(str)

# ---
#
# ## String concatenation and interpolation

string("Hello,", " world")

#+

a = 1.123
string("The variable a is of type ", typeof(a), " and its value is ", a)

#+

"Hello," * " world"
"Hello"^3
repeat("Hello", 3)

#+

a = 1.123

string("The variable a is of type ", typeof(a), " and its value is ", a)
"The variable a is of type $(typeof(a)), and its value is $(a)"

#+

"$typeof(a)"

"$(typeof(a))"

#+

myfunc = typeof

"$myfunc(a)"

"$(myfunc(a))"

#+

v = [1,2,3]
"vector: $v"

t = (1,2,3)
"tuple: $(t)"

# ### Exercise:
# Print the following message for a given vector
#
# > "<vec> is a vector of length <len> with elements of type <type>"
#
# where `<vec>` is the string representation of the given vector, `<len>` is the actual
# length of the given vector, and `<type>` is the type of its elements. Use the following
# two vectors.
#
# ```julia

a = [1,2,3]
b = [:a, :b, :c, :d]

# **Hint:** use the `length` and` eltype` functions.
#
# ---
# ### Solution:

a = [1,2,3];
str = string(a, " is a vector of length ",  length(a), " with elements of type ", eltype(a));
println(str)
b = [:a, :b, :c, :d];
str = "$(b) is a vector of length $(length(b)) with elements of type $(eltype(b))";
println(str)

# ---
#
# ## Useful functions

join(["apples", "bananas", "pineapples"], ", ", " and ")

#+

str = "JuliaLang is a pretty cool language!"

split(str)
split(str, " a ")

#+

contains("JuliaLang is pretty cool!", "Julia")
occursin("Julia", "JuliaLang is pretty cool!")
endswith("figure.png", "png")

#+

str = "JuliaLang is a pretty cool language!"

findall(isequal('a'), str)
findfirst(isequal('a'), str)
findlast(isequal('a'), str)

#+

replace("Sherlock Holmes", "e" => "ee")
replace("Sherlock Holmes", "e" => uppercase)

# ### Exercise:
# Use the `split` function to split the following string
#
# > "Julia!"
#
# into a vector of single-character strings.
# **Hint:** we can say that an empty string `""` separates the characters in the string.
#
# ---
# ### Solution:

split("Julia!", "")


text:
"""
    to_symbol(input) -> String

Cleans up `input` string and converts it into a symbol, needed so that channel
names work with AxisArrays.
"""
function to_symbol(input::String)
    fixed = replace(input, r"[^\w\ \-\_]"=>"")
    fixed = replace(fixed, r"[\ \-\_]+"=>"_")
    Symbol(replace(fixed, r"^[\d]"=>s"_\g<0>"))
end


text:
using Bijectors

using ChainRulesTestUtils
using Combinatorics
using DistributionsAD
using FiniteDifferences
using ForwardDiff
using Functors
using ReverseDiff
using Tracker
using Zygote

using Random, LinearAlgebra, Test

using Bijectors: Log, Exp, Shift, Scale, Logit, SimplexBijector, PDBijector, Permute,
    PlanarLayer, RadialLayer, Stacked, TruncatedBijector, ADBijector

using DistributionsAD: TuringUniform, TuringMvNormal, TuringMvLogNormal,
    TuringPoissonBinomial

import NNlib

const GROUP = get(ENV, "GROUP", "All")

# Always include this since it can be useful for other tests.
include("ad/utils.jl")
include("bijectors/utils.jl")

if GROUP == "All" || GROUP == "Interface"
    include("interface.jl")
    include("transform.jl")
    include("norm_flows.jl")
    include("bijectors/permute.jl")
    include("bijectors/rational_quadratic_spline.jl")
    include("bijectors/named_bijector.jl")
    include("bijectors/leaky_relu.jl")
    include("bijectors/coupling.jl")
end

if GROUP == "All" || GROUP == "AD"
    include("ad/chainrules.jl")
    include("ad/flows.jl")
    include("ad/distributions.jl")
end


text:
# Support files containing all functions for Track Hypothesis pruning

# M-best pruning
function MBest(Tracks::Array{Track,1},AssMat::AssociationMatrix,Cost::HypothesisCost,TuningParams::TuningParams)

    # M
    M = TuningParams.M

    # Columns
    assmat  = copy(AssMat.TOMHT)
    costmat = copy(Cost.CostMatrix)
    tracks  = copy(Tracks)
    ncol    = size(assmat,2)

    # Output Vars (should I just being doing this by refernce to save time?)
    newassmat = Array{Int64}(undef, 0, ncol)
    newcostmat = Array{Float64}(undef, 0, ncol)
    newtracks  = Array{Track,1}()

    while !isempty(assmat) && size(newassmat,1) < M
        # Find minimum cost hypothesis and the measurment ID for that hypoth
        idx = argmin(costmat[:,end])
        IDs = copy(assmat[idx,:])

        # Write hypothesis to output vars
        newassmat  = vcat(newassmat,assmat[idx,:]')
        newcostmat = vcat(newcostmat,costmat[idx,:]')
        newtracks  = vcat(newtracks,tracks[idx])

        # Remove all incompatible tracks from input vars
        for ii in 1:ncol
            ID  = IDs[ii]
            if ID > 0 # Remove all hypotheses that assign that detection
                rmidx   = findall(x -> x==ID , assmat[:,ii])
                assmat  = assmat[setdiff(1:end, rmidx), :]
                costmat = costmat[setdiff(1:end, rmidx), :]
                tracks  = tracks[setdiff(1:end, rmidx), :][:]
            end
        end
    end

    # Enforce there are at least M compatible tracks
    M = min(M,length(newtracks))

    return newtracks[1:M], AssociationMatrix(newassmat[1:M,:]), HypothesisCost(newcostmat[1:M,:])

end # End MBest

export MBest


text:
module Logs

import DataFrames: DataFrame
import OrderedCollections: OrderedDict
import PooledArrays: PooledArray
import ..ARLToolkit: dts

struct LogFormat
  delimiter::Char
  columns::AbstractVector{Pair{Symbol,Any}}
end

const UnetLogs = LogFormat('|', [
  :time => s -> dts(parse(Int64, s)/1000),
  :level => Symbol,
  :source => string,
  :text => string
])

function read(filename, fmt, filters=nothing; limit=nothing)
  df = DataFrame()
  for (lno, line) ∈ enumerate(eachline(filename))
    cols = split(line, fmt.delimiter)
    length(cols) == length(fmt.columns) || continue
    entry = OrderedDict(first(key) => last(key)(value) for (key, value) ∈ zip(fmt.columns, cols))
    if filters !== nothing
      filters isa AbstractVector || (filters = [filters])
      for f1 ∈ filters
        rv = f1(entry)
        if rv isa AbstractDict
          entry = rv
        elseif rv === nothing || rv === false
          entry = nothing
          break
        elseif rv !== true
          throw(ErrorException("Bad return value from filter: $rv"))
        end
      end
      entry === nothing && continue
    end
    entry[:filename] = filename
    entry[:lno] = lno
    push!(df, NamedTuple(entry); cols=:union)
    limit !== nothing && size(df, 1) ≥ limit && break
  end
  df.filename = PooledArray(df.filename)
  df
end

function read(filenames::AbstractVector, fmt, filters=nothing)
  df = vcat([read(filename, fmt, filters) for filename ∈ filenames]...)
  df.filename = PooledArray(df.filename)
  sort!(df, :time)
end

function Message(clazz)
  data -> begin
    m = match(r"^(.+): ?\w+ ?\[(.*)\]:? ?(.*)$", data[:text])
    m === nothing && return
    m[1] == clazz || return
    delete!(data, :text)
    data[:clazz] = clazz
    for kv ∈ split(m[2], ' ')
      if contains(kv, ':')
        k, v = split(kv, ':')
        try
          v = parse(Int, v)
        catch
          try
            v = parse(Float64, v)
          catch
            # its ok
          end
        end
        data[Symbol(k)] = v
      else
        break
      end
    end
    m[3] === nothing || m[3] == "" || (data[:extra] = m[3])
    data
  end
end

end # module


text:
# # Constants

const c_0 = 2.99792458e8
const N_SEAWATER = 1.3800851282


text:
function cuMPO(O::MPO)
  P = copy(O)
  for site in 1:length(O)
    P.data[site] = cuITensor(O.data[site])
  end
  return P
end
cuMPO() = MPO()

cuMPO(A::Vector{ITensor}) = cuMPO(MPO(A))
cuMPO(sites) = cuMPO(MPO(sites))

cu(M::MPO) = cuMPO(M)

function randomCuMPO(sites, m::Int=1)
  M = cuMPO(sites)
  for i in eachindex(sites)
    randn!(M[i])
    normalize!(M[i])
  end
  m > 1 && throw(ArgumentError("randomMPO: currently only m==1 supported"))
  return M
end

function cpu(M::T) where {T<:Union{MPS,MPO}}
  if typeof(tensor(ITensors.data(M)[1])) <: CuDenseTensor
    return T(cpu.(ITensors.data(M)), M.llim, M.rlim)
  else
    return M
  end
end


text:
module TrafficWeavingPlanner

const robot = "/robot"
const human = "/human"

using PyCall
using RobotOS
using StaticArrays
using Rotations
using Interpolations

using TrafficWeavingPlanner
using DifferentialDynamicsModels
using LinearDynamicsModels

@pyimport rospy
@pyimport tf2_ros
@rosimport geometry_msgs.msg: Transform, TransformStamped, PoseStamped, TwistStamped, AccelStamped
@rosimport std_msgs.msg: Float32MultiArray, Float64MultiArray,
                         Int32MultiArray, Int64MultiArray,
                         MultiArrayLayout, MultiArrayDimension,
                         Bool
@rosimport safe_traffic_weaving.msg: Float32MultiArrayStamped, PredictionInput, PredictionOutput
rostypegen()
import geometry_msgs.msg: Transform, TransformStamped, PoseStamped, TwistStamped, AccelStamped
import std_msgs.msg: Float32MultiArray, Float64MultiArray,
                     Int32MultiArray, Int64MultiArray,
                     MultiArrayLayout, MultiArrayDimension,
                     BoolMsg
import safe_traffic_weaving.msg: Float32MultiArrayStamped, PredictionInput, PredictionOutput

# const tfBuffer = tf2_ros.Buffer()
# const listener = tf2_ros.TransformListener(tfBuffer)
# lane_100_py_tf = tfBuffer[:lookup_transform]("world", "lane_100", rospy.Time(), rospy.Duration(1.0))
# lane_0_py_tf = tfBuffer[:lookup_transform]("world", "lane_0", rospy.Time(), rospy.Duration(1.0))

const Timeseries2D = Interpolations.GriddedInterpolation{SVector{2,Float64},1,SVector{2,Float32},Interpolations.Gridded{Interpolations.Linear},Tuple{Array{Float64,1}},0}
Timeseries2D() = interpolate((Float64[],), SVector{2,Float32}[], Gridded(Linear()))
truncate!(v::Vector) = resize!(v, 0)
truncate!(ts::Timeseries2D) = (foreach(truncate!, ts.knots); truncate!(ts.coefs))
Base.push!(ts::Timeseries2D, t, x) = (push!(ts.knots[1], t); push!(ts.coefs, x))
Base.push!(ts::Timeseries2D, tx::Tuple) = push!(ts, tx[1], tx[2])

# struct Timeseries2D
#     t::Vector{Float64}
#     xy::Vector{SVector{2,Float32}}
# end
# Timeseries2D() = Timeseries2D(Float64[], SVector{2,Float32}[])
# truncate!(v::Vector) = resize!(v, 0)
# truncate!(ts::Timeseries2D) = (truncate!(ts.t); truncate!(ts.xy))

struct DoubleIntegratorTimeseries2D
    pos::Timeseries2D
    vel::Timeseries2D
    acc::Timeseries2D
end
DoubleIntegratorTimeseries2D() = DoubleIntegratorTimeseries2D(Timeseries2D(), Timeseries2D(), Timeseries2D())
truncate!(dits::DoubleIntegratorTimeseries2D) = (truncate!(dits.pos); truncate!(dits.vel); truncate!(dits.acc))
Base.getindex(dits::DoubleIntegratorTimeseries2D, t) = [dits.pos[t]; dits.vel[t]; dits.acc[t]]
function last_common_time(dits::DoubleIntegratorTimeseries2D)
    if length(dits.pos.knots[1]) > 0 && length(dits.vel.knots[1]) > 0 && length(dits.acc.knots[1]) > 0
        min(dits.pos.knots[1][end], dits.vel.knots[1][end], dits.acc.knots[1][end])
    else
        -1.0
    end
end
function first_common_time(dits::DoubleIntegratorTimeseries2D)
    if length(dits.pos.knots[1]) > 0 && length(dits.vel.knots[1]) > 0 && length(dits.acc.knots[1]) > 0
        max(dits.pos.knots[1][1], dits.vel.knots[1][1], dits.acc.knots[1][1])
    else
        -1.0
    end
end

mutable struct PlannerState
    initialized::Bool
    terminate::Bool
    start_time::Float64
    computed_up_to_t::Int
    human::Vector{SVector{6,Float32}}     # car2
    robot::Vector{SVector{6,Float32}}     # car1
    human_buffer::DoubleIntegratorTimeseries2D
    robot_buffer::DoubleIntegratorTimeseries2D
    # robot_actions::Vector{Tuple{Int,Int}}
    robot_des::Vector{SVector{6,Float32}} # identical to robot up to current time, but includes future actions
    robot_l0::Int
    robot_lf::Int
    # robot_a0::Vector{Tuple{Int,Int}}    # current queue of actions to be implemented
    # robot_a0_t0::Int                    # timestep of current action
end

mutable struct HumanCarStates
    t::Vector{Float32}
    X::Vector{SVector{6,Float32}}
end

const planner_state = PlannerState(false, false, 0.0, 0,
                                   SVector{6,Float32}[],
                                   SVector{6,Float32}[],
                                   DoubleIntegratorTimeseries2D(),
                                   DoubleIntegratorTimeseries2D(),
                                   # Tuple{Int,Int}[],
                                   SVector{6,Float32}[], 0, 0)

get_xy(msg::PoseStamped) = SVector{2,Float32}(msg.pose.position.x, msg.pose.position.y)
get_xy(msg::TwistStamped) = SVector{2,Float32}(msg.twist.linear.x, msg.twist.linear.y)
get_xy(msg::AccelStamped) = SVector{2,Float32}(msg.accel.linear.x, msg.accel.linear.y)
function append_callback(msg, ts)
    xy = get_xy(msg)
    !isnan(xy[1]) && !isnan(xy[2]) && push!(ts, Float64(msg.header.stamp), xy)
end

# human_pos_callback(msg, ts) = (push!(ts.t, Float64(msg.header.stamp)), push!(ts.xy, SVector{2,Float32}(msg.pose.position.x,
#                                                                                msg.pose.position.y))
# human_vel_callback(msg, ts) = (push!(ts.t, Float64(msg.header.stamp)), push!(ts.xy, SVector{2,Float32}(msg.twist.linear.x,
#                                                                                msg.twist.linear.y))
# human_acc_callback(msg, ts) = (push!(ts.t, Float64(msg.header.stamp)), push!(ts.xy, SVector{2,Float32}(msg.accel.linear.x,
#                                                                                msg.accel.linear.y))
# robot_pos_callback(msg, ts) = (push!(ts.t, Float64(msg.header.stamp)), push!(ts.xy, SVector{2,Float32}(msg.pose.position.x,
#                                                                                msg.pose.position.y))
# robot_vel_callback(msg, ts) = (push!(ts.t, Float64(msg.header.stamp)), push!(ts.xy, SVector{2,Float32}(msg.twist.linear.x,
#                                                                                msg.twist.linear.y))
# robot_acc_callback(msg, ts) = (push!(ts.t, Float64(msg.header.stamp)), push!(ts.xy, SVector{2,Float32}(msg.accel.linear.x,
#                                                                                msg.accel.linear.y))

function terminate_callback(msg::BoolMsg, planner_state::PlannerState)
    planner_state.terminate = msg.data
end

function reset_scenario_callback(msg::BoolMsg, planner_state::PlannerState)
    if msg.data
        planner_state.initialized = true
        planner_state.start_time = Float64(RobotOS.now())
        planner_state.computed_up_to_t = 0
        if RobotOS.get_param("robot_start_lane", "R") == "L"
            planner_state.robot_l0, planner_state.robot_lf = 1, 2
        else
            planner_state.robot_l0, planner_state.robot_lf = 2, 1
        end
        # if planner_state.human_buffer.pos.coefs[end][2] > -4    # HACK
        # planner_state.robot_l0, planner_state.robot_lf = 2, 1
        # else
        #     planner_state.robot_l0, planner_state.robot_lf = 2, 1
        # end
        # planner_state.robot_a0 = Tuple{Int,Int}[]
        # planner_state.robot_actions = [(1,planner_state.robot_l0) for i in 1:(start_wait + computation_interval)]
        rossleep(0.5)
        truncate!(planner_state.robot_des)
        truncate!(planner_state.human)
        truncate!(planner_state.robot)
        truncate!(planner_state.human_buffer)
        truncate!(planner_state.robot_buffer)
    end
end

init_node("traffic_weaving_planner", anonymous=true)
const terminate_sub = Subscriber{BoolMsg}("/terminate", terminate_callback, (planner_state,), queue_size=10)
const reset_sub = Subscriber{BoolMsg}("/reset", reset_scenario_callback, (planner_state,), queue_size=1)
# const reset_sim_sub = Subscriber{BoolMsg}("/reset_sim", reset_scenario_callback, (planner_state,), queue_size=10)

const human_pos_sub = Subscriber{PoseStamped}(human*"/pose", append_callback, (planner_state.human_buffer.pos,), queue_size=10)
const human_vel_sub = Subscriber{TwistStamped}(human*"/vel", append_callback, (planner_state.human_buffer.vel,), queue_size=10)
const human_acc_sub = Subscriber{AccelStamped}(human*"/acc", append_callback, (planner_state.human_buffer.acc,), queue_size=10)
const robot_pos_sub = Subscriber{PoseStamped}(robot*"/pose", append_callback, (planner_state.robot_buffer.pos,), queue_size=10)
const robot_vel_sub = Subscriber{TwistStamped}(robot*"/vel", append_callback, (planner_state.robot_buffer.vel,), queue_size=10)
const robot_acc_sub = Subscriber{AccelStamped}(robot*"/acc", append_callback, (planner_state.robot_buffer.acc,), queue_size=10)

const robot_traj_pub = Publisher{Float32MultiArrayStamped}("/robot/traj_plan", queue_size=10)
convert_to_3d_array(V::Vector{SVector{6,Float32}}) = reinterpret(Float32, copy(V), (6, length(V), 1))
convert_to_reversed_3d_array(V::Vector{SVector{6,Float32}}) = reshape(reinterpret(Float32, copy(V), (6, length(V)))', (1, length(V), 6))
function publish_robot_plan(robot_des::Vector{SVector{6,Float32}}, time_offset)
    msg = Float32MultiArrayStamped()
    msg.data = convert_to_3d_array(robot_des)
    msg.header.stamp = (time_offset isa Number ? RobotOS.Time(time_offset) : time_offset)
    publish(robot_traj_pub, msg)
end

include("utils/planner.jl")

function run(;start_wait=8, computation_interval=3)
    rate = RobotOS.Rate(100)

    time_scale_factor = RobotOS.get_param("time_scale_factor")
    dt = Float32(.1/time_scale_factor)

    reset_scenario_callback(BoolMsg(true), planner_state)
    planner_state.terminate = false
    while !planner_state.terminate
        @assert length(planner_state.human) == length(planner_state.robot)
        Tnew = min(last_common_time(planner_state.human_buffer), last_common_time(planner_state.robot_buffer))
        if isempty(planner_state.human)
            # t_now = Float64(RobotOS.now())
            # push!(planner_state.robot_buffer.pos, t_now, [-138.2, -6.0940447])
            # push!(planner_state.robot_buffer.pos, t_now, [28., 0.])
            # push!(planner_state.robot_buffer.pos, t_now, [0., 0.])
            Tfirst = max(first_common_time(planner_state.human_buffer), first_common_time(planner_state.robot_buffer), planner_state.start_time)
            println("still empty, $Tfirst, $Tnew")
            # println(planner_state.human_buffer.pos.knots)
            # println(planner_state.human_buffer.pos.coefs)
            # println(planner_state.robot_buffer)
            try
                if Tnew == -1 || Tfirst == -1 || Tfirst >= Tnew - 0.02 || planner_state.robot_buffer[Tnew-0.01][1] < -138
                    rossleep(0.3)
                    continue
                end
            catch
                println("brute-force catching our way past race conditions (1)")
                continue
            end
            A, B = Tfirst, Tnew
            Tstart = (A + B)/2
            for i in 1:10
                planner_state.robot_buffer[Tstart][1] < -138 ? (A = Tstart) : (B = Tstart)
                Tstart = (A + B)/2
            end
            println("ready to start")
            planner_state.start_time = Tstart
            initial_actions = [(1,planner_state.robot_l0) for i in 1:(start_wait + computation_interval)]
            planner_state.robot_des = propagate(xdd_choices, y_targets, planner_state.robot_buffer[planner_state.start_time], tibvp, initial_actions)
            publish_robot_plan(planner_state.robot_des, planner_state.start_time)
        end
        T = planner_state.start_time + (length(planner_state.human) - 1)*dt
        Tnew = min(last_common_time(planner_state.human_buffer), last_common_time(planner_state.robot_buffer))    # in case of race with /reset channel?

        race_condition_hit = false
        while T + dt < Tnew
            println("pushing")
            try
                push!(planner_state.human, planner_state.human_buffer[T + dt])
                push!(planner_state.robot, planner_state.robot_buffer[T + dt])
            catch
                println("brute-force catching our way past race conditions (2)")
                race_condition_hit = true
                break
            end
            if length(planner_state.robot) <= length(planner_state.robot_des)
                planner_state.robot_des[length(planner_state.robot)] = planner_state.robot[end]  # maybe unnecessary
            end
            T = T + dt
        end
        race_condition_hit && continue

        println("caught up")
        # t = floor(Int, (Float64(RobotOS.now()) - planner_state.start_time)/dt) + 1
        t = length(planner_state.human)
        t = t - (t % computation_interval)    # enforcing strict computation intervals
        if t <= 5
            rossleep(0.1)
            continue
        end

        println("sufficient history")

        # a0 = planner_state.robot_actions[t:t+computation_interval-1]
        if t+computation_interval > length(planner_state.robot_des) || t <= planner_state.computed_up_to_t
            rossleep(0.01)
            continue
        end
        a0_traj = planner_state.robot_des[t+1:t+computation_interval]
        a0 = ([a0_traj[i][5] for i in 1:computation_interval], [a0_traj[i][6] for i in 1:computation_interval])    # Ẍ and Ÿ; TODO: plumb targets instead of using previous Ÿ
        pfX, pfY = all_future_xy(xdd_choices, y_targets, planner_state.robot[t], tibvp, a0, 4, des_length=15)
        robot_in = convert_to_reversed_3d_array(planner_state.robot)
        human_in = convert_to_reversed_3d_array(planner_state.human)
        extras_in = zeros(Float32, 1, length(planner_state.robot), 0)
        predict!(robot_in, human_in, extras_in, [t], pfX[:,2:end,:], pfY[:,2:end,:], 64)
        for i in 1:15
            _t = t + i
            if _t <= length(planner_state.robot_des)
                planner_state.robot_des[_t] = SVector(pred_container.b[i,:]...)
            else
                push!(planner_state.robot_des, SVector(pred_container.b[i,:]...))
            end
        end
        publish_robot_plan(planner_state.robot_des, planner_state.start_time)
        planner_state.computed_up_to_t = t

        # TrafficWeavingPlanner.pred_container.y[4:6,1:2:end], TrafficWeavingPlanner.pred_container.y[4:6,2:2:end]
        # car1[1,t+1:t+15,:] = TrafficWeavingPlanner.pred_container.y
        # t = t + 3
        # car1_curr_state = car1[1,t,:]
        # a0 = (TrafficWeavingPlanner.pred_container.y[4:6,1:2:end], TrafficWeavingPlanner.pred_container.y[4:6,2:2:end])

        rossleep(rate)
    end
end

@spawn spin()

# mutable struct SimulatorState
#     reset::Bool
#     xh::SE2vState{Float64}
#     uh::AccelerationCurvatureControl{Float64}
# end

# function joystick_callback(msg::Joy, sim_state::SimulatorState)
#     if msg.buttons[1] == 1 && msg.buttons[2] == 1
#         sim_state.reset = true
#     end
#     sim_state.uh = AccelerationCurvatureControl(MAX_LONGITUDINAL_ACC*msg.axes[2],
#                                                 MAX_CURVATURE*msg.axes[1])
# end

# init_node("xbox_car", anonymous=true)

# const sim_state = SimulatorState(false, zeros(SE2vState{Float64}), zeros(AccelerationCurvatureControl{Float64}))
# const joystick_sub = Subscriber{Joy}("joy", joystick_callback, (sim_state,), queue_size=10)
# const human_vel_pub = Publisher{TwistStamped}("human/vel", queue_size=10)
# const human_accel_pub = Publisher{AccelStamped}("human/accel", queue_size=10)
# const tf_broadcaster = tf2_ros.TransformBroadcaster()

# function fill_2D_transform!(t::TransformStamped, x, y, θ, stamp = RobotOS.now())
#     t.header.stamp = stamp
#     t.transform.translation.x = x
#     t.transform.translation.y = y
#     t.transform.rotation.x,
#     t.transform.rotation.y,
#     t.transform.rotation.z,
#     t.transform.rotation.w = tf.transformations[:quaternion_from_euler](0.0, 0.0, θ)
#     t
# end

# function test()
#     rate = Rate(SIM_RATE)

#     world_header = Header()
#     world_header.frame_id = "world"
#     human_car_wrt_world = TransformStamped(world_header, "human_car", Transform())
#     vel_msg, accel_msg = TwistStamped(), AccelStamped()
#     vel_msg.header.frame_id = accel_msg.header.frame_id = "human_car"

#     sim_state.xh = zeros(SE2vState{Float64})
#     while !sim_state.reset
#         timestamp = RobotOS.now()
#         sim_state.xh = propagate(SimpleCarDynamics{1,0}(), sim_state.xh, StepControl(1/SIM_RATE, sim_state.uh))
#         fill_2D_transform!(human_car_wrt_world, sim_state.xh.x, sim_state.xh.y, sim_state.xh.θ, timestamp)

#         vel_msg.header.stamp = accel_msg.header.stamp = timestamp
#         vel_msg.twist.linear.x = sim_state.xh.v
#         accel_msg.accel.linear.x = sim_state.uh.a
#         accel_msg.accel.linear.y = sim_state.uh.κ*sim_state.xh.v^2

#         # p, o = pose_msg.pose.position, pose_msg.pose.orientation
#         # p.x, p.y = sim_state.xh.x, sim_state.xh.y
#         # o.x, o.y, o.z, o.w = tf.transformations[:quaternion_from_euler](0.0, 0.0, sim_state.xh.θ)
#         # v = vel_msg.twist.linear
#         # v.x, v.y = sim_state.xh.v*cos(sim_state.xh.θ), sim_state.xh.v*sin(sim_state.xh.θ)
#         # a = accel_msg.accel.linear
#         # a.x = sim_state.uh.a*cos(sim_state.xh.θ) - sim_state.uh.κ*sim_state.xh.v^2*sin(sim_state.xh.θ)
#         # a.y = sim_state.uh.a*sin(sim_state.xh.θ) + sim_state.uh.κ*sim_state.xh.v^2*cos(sim_state.xh.θ)

#         tf_broadcaster[:sendTransform](human_car_wrt_world)
#         # tf_broadcaster[:sendTransform]((sim_state.xh.x, sim_state.xh.y, 0.0),
#         #                                tf.transformations[:quaternion_from_euler](0.0, 0.0, sim_state.xh.θ),
#         #                                timestamp, "human_car", "world")
#         # publish(human_pose_pub, pose_msg)
#         publish(human_vel_pub, vel_msg)
#         publish(human_accel_pub, accel_msg)
#         rossleep(rate)
#     end
# end

# @spawn spin()

end # module