text:
using FreeTypeAbstraction: hadvance, leftinkbound, inkwidth, get_extent, ascender, descender

function zerorect(x::Rect{N, T}) where {N, T}
    Rect(Vec{N, T}(0), widths(x))
end
function padrect(rect, pad)
    Rect(minimum(rect) .- pad, widths(rect) .+ 2pad)
end

one_attribute_per_char(attribute, string) = (attribute for char in string)

function one_attribute_per_char(font::NativeFont, string)
    return (find_font_for_char(char, font) for char in string)
end

function attribute_per_char(string, attribute)
    n_words = 0
    if attribute isa AbstractVector
        if length(attribute) == length(string)
            return attribute
        else
            n_words = length(split(string, r"\s+"))
            if length(attribute) == n_words
                i = 1
                return map(collect(string)) do char
                    f = attribute[i]
                    char == "\n" && (i += 1)
                    return f
                end
            end
        end
    else
        return one_attribute_per_char(attribute, string)
    end
    error("A vector of attributes with $(length(attribute)) elements was given but this fits neither the length of '$string' ($(length(string))) nor the number of words ($(n_words))")
end

function layout_text(
        string::AbstractString, startpos::VecTypes{N, T}, textsize::Union{AbstractVector, Number},
        font, align, rotation, model, justification, lineheight
    ) where {N, T}

    offset_vec = to_align(align)
    ft_font = to_font(font)
    rscale = to_textsize(textsize)
    rot = to_rotation(rotation)

    atlas = get_texture_atlas()
    mpos = model * Vec4f0(to_ndim(Vec3f0, startpos, 0f0)..., 1f0)
    pos = to_ndim(Point3f0, mpos, 0)

    fontperchar = attribute_per_char(string, ft_font)
    textsizeperchar = attribute_per_char(string, rscale)

    glyphpos = glyph_positions(string, fontperchar, textsizeperchar, offset_vec[1],
        offset_vec[2], lineheight, justification)

    positions = Point3f0[]
    for (i, group) in enumerate(glyphpos)
        for gp in group
            p = to_ndim(Point3f0, gp, 0) #./ Point3f0(4, 4, 1)
            # rotate around the alignment point (this is now at [0, 0, 0])
            p_rotated = rot * p
            push!(positions, pos .+ p_rotated) # TODO why division by 4 necessary?
        end
        # between groups, push a random point for newline, it doesn't matter
        # what it is
        if i < length(glyphpos)
            push!(positions, Point3f0(0, 0, 0))
        end
    end

    return positions
end

function glyph_positions(str::AbstractString, font_per_char, fontscale_px, halign, valign, lineheight_factor, justification)

    char_font_scale = collect(zip([c for c in str], font_per_char, fontscale_px))

    linebreak_indices = (i for (i, c) in enumerate(str) if c == '\n')

    groupstarts = [1; linebreak_indices .+ 1]
    groupstops = [linebreak_indices .- 1; length(str)]

    cfs_groups = map(groupstarts, groupstops) do start, stop
        char_font_scale[start:stop]
    end

    extents = map(cfs_groups) do group
        # TODO: scale as SVector not Number
        [get_extent(font, char) .* SVector(scale, scale) for (char, font, scale) in group]
    end

    # add or subtract kernings?
    xs = map(extents) do extgroup
        cumsum([isempty(extgroup) ? 0.0 : -leftinkbound(extgroup[1]); hadvance.(extgroup[1:end-1])])
    end

    # each linewidth is the last origin plus inkwidth
    linewidths = last.(xs) .+ [isempty(extgroup) ? 0.0 : inkwidth(extgroup[end]) for extgroup in extents]
    maxwidth = maximum(linewidths)

    width_differences = maxwidth .- linewidths

    xs_justified = map(xs, width_differences) do xsgroup, wd
        xsgroup .+ wd * justification
    end

    # make lineheight a multiple of the largest lineheight in each line
    lineheights = map(cfs_groups) do group
        maximum(group) do (char, font, scale)
            font.height / font.units_per_EM * lineheight_factor * scale
        end
    end

    # how to define line height relative to font size?
    ys = cumsum([0; -lineheights[2:end]])

    # x alignment
    xs_aligned = [xsgroup .- halign * maxwidth for xsgroup in xs_justified]

    # y alignment
    # first_max_ascent = maximum(hbearing_ori_to_top, extents[1])
    # last_max_descent = maximum(x -> inkheight(x) - hbearing_ori_to_top(x), extents[end])

    first_line_ascender = maximum(cfs_groups[1]) do (char, font, scale)
        ascender(font) * scale
    end

    last_line_descender = minimum(cfs_groups[end]) do (char, font, scale)
        descender(font) * scale
    end

    overall_height = first_line_ascender - ys[end] - last_line_descender

    ys_aligned = ys .- first_line_ascender .+ (1 - valign) .* overall_height

    # we are still operating in freetype units, let's convert to the chosen scale by dividing with 64
    return [Vec2.(xsgroup, y) for (xsgroup, y) in zip(xs_aligned, ys_aligned)]
end

function text_bb(str, font, size)
    positions = layout_text(
        str, Point2f0(0), size,
        font, Vec2f0(0), Quaternionf0(0,0,0,1), Mat4f0(I), 0.5, 1.0
    )

    scale = widths.(first.(FreeTypeAbstraction.metrics_bb.(collect(str), font, size)))
    return union(FRect3D(positions),  FRect3D(positions .+ to_ndim.(Point3f0, scale, 0)))
end

"""
    move_from_touch(
        parent::GeometryPrimitive{N, T}, child::GeometryPrimitive{N},
        pad::Vec{N}
    ) where {N, T}

calculates how much `child` rectangle needs to move to not touch the `parent`
"""
function move_from_touch(
        parent::GeometryPrimitive{N, T}, child::GeometryPrimitive{N},
        pad::Vec{N}
    ) where {N, T}
    pmini, cmini = minimum(parent), minimum(child) .- pad
    pmaxi, cmaxi = maximum(parent), maximum(child) .+ pad

    move = ntuple(Val(N)) do i
        posdir = ifelse(cmini[i] < pmini[i], (pmini[i] - cmini[i]), zero(T)) #always positive
        negdir = ifelse(cmaxi[i] > pmaxi[i], (pmaxi[i] - cmaxi[i]), zero(T)) #always minus
        ifelse(posdir > abs(negdir), posdir, negdir) # move in the bigger direction
    end
    Vec{N, T}(move)
end

"""
    dont_touch(
        parent::GeometryPrimitive{N}, child::GeometryPrimitive{N},
        pad::Vec{N}
    ) where N

Moves `child` so that it doesn't touch parent. Leaves a gap to parent defined by `pad`.
"""
function dont_touch(
        parent::GeometryPrimitive{N}, child::GeometryPrimitive{N},
        pad::Vec{N}
    ) where N
    child + move_from_touch(parent, child, pad)
end

"""
    fit_factor_stretch(rect, lims::NTuple{N}) where N

Calculates the stretch factor to fill `rect` in all dimension.
Returns a stretch `N` dimensional fit factor.
"""
function fit_factor_stretch(rect, lims::NTuple{N, Any}) where N
    w = widths(rect)
    stretches = ntuple(Val(N)) do i
        from, to = lims[i]
        w[i] / abs(to - from)
    end
    stretches
end

"""
    fit_factor(rect, lims::NTuple{N}) where N

Calculates the scaling one needs to apply to lims to fit `rect` without changing aspect ratio.
Returns float scaling and the full strech as given by [`fit_factor_stretch`](@ref)
"""
function fit_factor(rect, lims::NTuple{N, Any}) where N
    stretches = fit_factor_stretch(rect, lims)
    minimum(stretches), stretches
end

"""
    fit_ratio(rect, lims)

Calculates the ratio one needs to stretch `lims` in order to get the same aspect ratio
"""
function fit_ratio(rect, lims)
    s, stretches = fit_factor(rect, lims)
    stretches ./ s
end

function alignment2num(x::Symbol)
    (x == :center) && return 0.5f0
    (x in (:left, :bottom)) && return 0.0f0
    (x in (:right, :top)) && return 1.0f0
    return 0.0f0 # 0 default, or better to error?
end

grid(x::Transformable...; kw_args...) = grid([x...]; kw_args...)

function grid(plots::Vector{<: Transformable}; kw_args...)
    N = length(plots)
    grid = close2square(N)
    grid(reshape(plots, grid))
end

function grid(plots::Matrix{<: Transformable}; kw_args...)
    N = length(plots)
    grid = size(plots)
    w, h = (0.0, 0.0)
    pscene = Scene()
    cam2d!(pscene)
    for idx in 1:N
        i, j = Tuple(CartesianIndices(grid)[idx])
        child = plots[idx]
        push!(pscene.children, child)
        translate!(child, w, h, 0.0)
        swidth = widths(boundingbox(child))
        if i == grid[1]
            h += (swidth[2] * 1.1)
            w = 0.0
        else
            w += (swidth[1] * 1.1)
        end
    end
    center!(pscene)
    pscene
end

estimated_space(x, N, w) = 1/N

ispixelcam(x::Union{PixelCamera, Camera2D}) = true
ispixelcam(x) = false

"""
    vbox(scenes...; parent = Scene(clear = false), kwargs...)

Box the scenes together on the vertical axis.  For example, two Scenes `vbox`ed
will be placed side-by-side.

```
--------------------  --------------------
--                --  --                --
--    Scene 1     --  --    Scene 2     --
--                --  --                --
--------------------  --------------------
```
"""
vbox(plots::Transformable...; kw_args...) = vbox([plots...]; kw_args...)
"""
    hbox(scenes...; parent = Scene(clear = false), kwargs...)

Attach the given Scenes together on the horizontal axis.  For example, two Scenes `hbox`ed
will be placed one on top of the other.

```
--------------------
--                --
--    Scene 1     --
--                --
--------------------
--------------------
--                --
--    Scene 2     --
--                --
--------------------
```
"""
hbox(plots::Transformable...; kw_args...) = hbox([plots...]; kw_args...)

function hbox(plots::Vector{T}; parent = Scene(clear = false), kw_args...) where T <: Scene
    layout(plots, 2; parent = parent, kw_args...)
end
function vbox(plots::Vector{T}; parent = Scene(clear = false), kw_args...) where T <: Scene
    layout(plots, 1; parent = parent, kw_args...)
end

function to_sizes(x::AbstractVector{<: Number}, widths, dim)
    x .* widths[dim]
end

function layout(
        plots::Vector{T}, dim;
        parent = Scene(clear = false), sizes = nothing, kw_args...
    ) where T <: Scene

    N = length(plots)
    w = 0.0
    area = pixelarea(parent)
    sizes_node = if sizes == nothing
        lift(a-> layout_sizes(plots, widths(a), dim), area)
    else
        lift(a-> to_sizes(sizes, widths(a), dim), area)
    end
    summed_size = lift(sizes_node) do s
        last_s = 0.0
        map(s) do x
            r = last_s; last_s += x; return r
        end
    end
    for idx in 1:N
        p = plots[idx]
        on(area) do a
            h = sizes_node[][idx]
            last = summed_size[][idx]
            mask = unit(Vec2f0, dim)
            # TODO this is terrible!
            new_w = Vec2f0(ntuple(2) do i
                i == dim ? h : widths(a)[i]
            end)
            resize!(p, IRect(minimum(a) .+ (mask .* last), new_w))
        end
        push!(parent.children, p)
        p.parent = parent
        nodes = map(fieldnames(Events)) do field
            if field != :window_area
                connect!(getfield(parent.events, field), getfield(p.events, field))
            end
        end
    end
    area[] = area[]
    parent
end

otherdim(dim) = dim == 1 ? 2 : 1

function scaled_width(bb, other_size, dim)
    wh = widths(bb)
    scaling = other_size / wh[otherdim(dim)]
    wh[dim] * scaling
end

function pixel_boundingbox(scene::Scene)
    if cameracontrols(scene) isa PixelCamera
        return true
    elseif cameracontrols(scene) isa EmptyCamera
        # scene doesn't supply the camera itself, check children
        return all(pixel_boundingbox, scene.children)
    else
        false
    end
end

function layout_sizes(scenes, size, dim)
    odim = otherdim(dim)
    this_size = size[dim]
    other_size = size[odim]
    N = length(scenes)
    pix_size = 0.0 # combined height of pixel bbs
    sizes = fill(-1.0, N)
    scenepix = findall(pixel_boundingbox, scenes)
    perfect_size = this_size / N # equal size for all!
    for i in scenepix
        scene = scenes[i]
        ds = widths(boundingbox(scene))[dim] .+ 10 # pad a bit
        sizes[i] = ds
        pix_size += ds
    end
    perfect_size = (this_size - pix_size) / (N - length(scenepix))
    for i in 1:N
        if sizes[i] == -1
            sizes[i] = perfect_size
        end
    end
    npixies = length(scenepix)
    # # We should only use 1/N per window, so if the accumulated size is bigger
    # # than that, we need to rescale the sizes
    total_size = sum(sizes)
    if this_size <= total_size # we need to rescale
        # no pixelsizes or all pixelsizes, we can just resize everything
        if npixies == 0 || npixies == N
            for i in 1:N
                sizes[i] = (sizes[i] / total_size) * this_size
            end
        else # we have a mix of pix + non pix sizes - only scale non pix
            total_px_size = sum(sizes[scenepix])
            # TODO too big total_px_size
            remaining = total_size - total_px_size
            space = this_size - total_px_size
            for i in 1:N
                (i in scenepix) && continue
                sizes[i] = (sizes[i] / remaining) * space
            end
        end
    end
    # final_pix_size = pix_size
    #
    # if max_pix_size < pix_size
    #     for i in scenes2d
    #         sizes[i] = (sizes[i] / pix_size) * max_pix_size
    #     end
    #     final_pix_size = max_pix_size
    # end
    # nonpixel_size = (this_size - final_pix_size) / (N - npixies)
    # for i in 1:N
    #     if !(i in scenes2d)
    #         sizes[i] = nonpixel_size
    #     end
    # end
    sizes
end

function vbox!(plots::Vector{T}; kw_args...) where T <: AbstractPlot
    N = length(plots)
    w = 0.0
    for idx in 1:N
        p = plots[idx]
        translate!(p, w, 0.0, 0.0)
        swidth = widths(boundingbox(p))
        w += (swidth[1] * 1.1)
    end
end
function hbox!(plots::Vector{T}; kw_args...) where T <: AbstractPlot
    N = length(plots)
    h = 0.0
    for idx in 1:N
        p = plots[idx]
        translate!(p, 0.0, h, 0.0)
        swidth = widths(boundingbox(p))
        h += (swidth[2] * 1.2)
    end
end


text:
# -----------------------------------------------------------------------------------------
# This file introduces `scitype`, `Scitype` methods and associated fallbacks methods.
# It also defines some conveneince methods.
# -----------------------------------------------------------------------------------------

# -----------------------------------------------------------------------------------------
# scitype function (generic) with fallbacks.
"""
    scitype(X, C::Convention)

The scientific type (interpretation) of object `X` (distinct from its
machine type) as specified by the convention 'C'.
"""
function scitype end

scitype(X, C::Convention) = fallback_scitype(X, C)
fallback_scitype(X, C) = Unknown
fallback_scitype(::Missing, C) = Missing
fallback_scitype(::Nothing, C) = Nothing

# -----------------------------------------------------------------------------------------
# `Scitype` function (generic) with fallbacks.
# (When implemented can considerably speed-up computation of `scitype` of abstractarrays.)

"""
    Scitype(T::Type, C::Convention)

Method for implementers of a convention `C` to enable speed-up of
scitype evaluations for arrays.

In general, one cannot infer the scitype of an object of type
`AbstractArray{T, N}` from the machine type `T` alone.

Nevertheless, for some *restricted* machine types `U`, the statement
`type(X) == AbstractArray{T, N}` for some `T<:U` already allows one
deduce that `scitype(X, C) = AbstractArray{S, N}`, where `S` is determined
by `U`, and convention `C` alone. This is the case in the `DefaultConvention` which is
used by *ScientificTypes.jl* , where for example, if `U = Integer`, then `S = Count`.

Such shortcuts are specified as follows:

```
Scitype(::Type{<:U}, ::C) = S
```

which incurs a considerable speed-up in the computation of `scitype(X, C)`.
There is also a speed-up for the case that `T <: Union{U, Missing}`.

For example, in the `DefaultConvention`, one has

```
Scitype(::Type{<:Integer}, ::DefaultConvention) = Count
```
"""
function Scitype end

Scitype(T, C::Convention) = Fallback_Scitype(T, C)
Fallback_Scitype(::Type, C) = Unknown

# To distinguish between `Any` type and `Union{T, Missing}` for some more
# specialised `T`, we define the `Any` case explicitly.
Fallback_Scitype(::Type{Any}, C) = Unknown

# For the case `Union{T, Missing}` we return `Union{S, Missing}` with `S`
# the scientific type corresponding to `T`.
function Fallback_Scitype(::Type{Union{T, Missing}}, C) where T
    return Union{Scitype(Missing, C), Scitype(T, C)}
end

# For the case `Missing` and `Nothing`,
# we return `Missing` and `Nothing` respectively.
Fallback_Scitype(::Type{Missing}, C) = Missing
Fallback_Scitype(::Type{Nothing}, C) = Nothing

# ---------------------------------------------------------------------------------------
# Fallbacks for `scitype` of tuples and abstractarrays.

function fallback_scitype(@nospecialize(t::Tuple), C)
    if length(t) <= TUPLE_SPECIALIZATION_THRESHOLD
        return _tuple_scitype(t, C)
    else
        return Tuple{(scitype(el, C) for el in t)...}
    end
end

@inline function _tuple_scitype(t::T, C) where {T}
    if @generated
        stypes = Tuple(
            :(scitype(getindex(t, $i), C)) for i in 1:fieldcount(T)
        )
        return :(Tuple{$(stypes...)})
    else
        stypes_ = Tuple(
            scitype(getindex(t, i), C) for i in 1:fieldcount(T)
        )
        return Tuple{(stypes_)...}
    end
end

function fallback_scitype(A::Arr{T}, C) where T
    return arr_scitype(A, Scitype(T, C), C)
end

"""
    arr_scitype(A, S, C)

Return the scitype associated with an  array `A` of type `{T, N}` assuming an
explicit `Scitype` correspondence exist mapping `T` to `S`.
"""
@inline function arr_scitype(
    @nospecialize(A::Arr{T, N}), S::Type, C;
    tight::Bool = false
) where {T, N}
    if S === Unknown
        return Arr{scitype_union(A, C), N}
    elseif S === Union{Scitype(Missing, C), Unknown}
        return Arr{Union{Scitype(Missing, C), scitype_union(A, C)}, N}
    else
        return Arr{S, N}
    end
end

# -----------------------------------------------------------------------------------------
# internal methods.

"""
    scitype_union(A, C::convention)

Return the type union, over all elements `x` generated by the iterable `A`,
of `scitype(x, C)` for a given convention `C`. See also [`scitype`](@ref).
"""
function scitype_union(A, C::Convention)
    iterate(A) === nothing && return Unknown
    reduce((a, b)->Union{a, b}, (scitype(el, C) for el in A))
end

ext: jl


text:
@testset "randomForest.jl" begin

    data_surv = CSO.dataset("cso_raw")
    data_trudeau = CSO.dataset("Montreal-Trudeau_15min_precipitations")
    data = innerjoin(data_surv, data_trudeau, on=:Date)
    dropmissing!(data, :Duration)
    train, test = train_test_year(data, 2020)

    varnames = [:d15min, :d1h, :d6h]

    model = RandomForestClassifier(n_trees=20, max_depth = 3)

    #The RMSD is well derived
    @test typeof(RForest(model, "Duration", varnames, train, test)) == Float64

    #Predictions are well derived
    @test typeof(RForest(model, "Duration", varnames, train, test, true, false)) == Vector{Float64}

end

max_stars_repo_name: AlexLsn/CSO

max_issues_repo_name: AlexLsn/CSO

max_forks_repo_name: AlexLsn/CSO


text:
@doc raw"""
    Sphere3

An object `s` of type [`Sphere3`](@ref) is a sphere in the three-dimensional
Euclidean space ``Ε^3``.

The sphere is oriented, i.e. its boundary has clockwise or counterclockwise
orientation. The boundary splits ``Ε^3`` into a positive and a negative
side, where the positive side is to the left of the boundary. The boundary also
splits ``Ε^3`` into a bounded and an unbounded side. Note that the sphere
can be degenerated, i.e. the squared radius may be zero.
"""
Sphere3

@doc raw"""
    Sphere3(center::Point3, r²::Real, ori::Orientation = COUNTERCLOCKWISE)

Introduces a variable `s` of type [`Sphere3`](@ref).

It is initialized to the circle with center `center`, squared radius
`squared_radius`, and orientation `ori`.

!!! info "Precondition"

    `ori` ``≠`` `COPLANAR`, and further, `squared_radius` ``≥ 0``.
"""
Sphere3(center::Point3, r²::Real, ori::Orientation = COUNTERCLOCKWISE)

@cxxdereference Sphere3(c::Point3, r²::Real, ori::Union{Orientation,Ref{Orientation}} = COUNTERCLOCKWISE) =
    Sphere3(c, convert(FT, r²), ori)

"""
    Sphere3(p::Point3, q::Point3, r::Point3, s::Point3)

Introduces a variable `s` of type [`Sphere3`](@ref).

It is initialized to the unique sphere which passes through the points `p`, `q`,
`r` and `s`. The orientation of the sphere is the orientation of the point
quadruple `p`, `q`, `r`, `s`.

!!! info "Precondition"

    `p`, `q`, `r` and `s` are not coplanar.
"""
Sphere3(p::Point3, q::Point3, r::Point3, s::Point3)

@doc raw"""
    Sphere3(p::Point3, q::Point3, r::Point3, ori::Orientation = COUNTERCLOCKWISE)

Introduces a variable `s` of type [`Sphere3`](@ref).

It is initialized to the smallest sphere which passes through the points `p`,
`q`, and `r`. The orientation of the sphere is `ori`.

!!! info "Precondition"

    `ori` ``≠`` `COPLANAR`.
"""
Sphere3(p::Point3, q::Point3, r::Point3, ori::Orientation = COUNTERCLOCKWISE)

@doc raw"""
    Sphere3(p::Point3, q::Point3, ori::Orientation = COUNTERCLOCKWISE)

Introduces a variable `s` of type [`Sphere3`](@ref).

It is initialized to the smallest sphere which passes through the points `p` and
`q`. The orientation of the sphere is `ori`.

!!! info "Precondition"

    `ori` ``≠`` `COPLANAR`.
"""
Sphere3(p::Point3, q::Point3, ori::Orientation = COUNTERCLOCKWISE)

@doc raw"""
    Sphere3(center::Point3, ori::Orientation = COUNTERCLOCKWISE)

Introduces a variable `s` of type [`Sphere3`](@ref).

It is initialized to the sphere with center `center`, squared radius zero and
orientation `ori`.

!!! info "Precondition"

    `ori` ``≠`` `COPLANAR`.

!!! info "Postcondition"

    `is_degenerate(s) == true`.
"""
Sphere3(center::Point3, ori::Orientation = COUNTERCLOCKWISE)

"""
    Sphere3(c::Circle3)

Introduces a variable `s` of type [`Sphere3`](@ref).

It is initialized to the diametral sphere of the circle.
"""
Sphere3(c::Circle3)

"""
    center(s::Sphere3)

Returns the center of `s`.
"""
center(s::Sphere3)

"""
    squared_radius(s::Sphere3)

Returns the squared radius of `s`.
"""
squared_radius(s::Sphere3)

"""
    orientation(s::Sphere3)

Returns the orientation of `s`.
"""
orientation(s::Sphere3)

"""
    ==(s₁::Sphere3, s₂::Sphere3)

Returns `true`, iff `s₁` and `s₂` are equal, i.e. if they have the same center,
same squared radius and same orientation.
"""
==(s₁::Sphere3, s₂::Sphere3)

"""
    is_degenerate(s::Sphere3)

Returns `true`, iff `s` is degenerate, i.e. if `s` has squared radius zero.
"""
is_degenerate(s::Sphere3)

"""
    oriented_side(s::Sphere3, p::Point3)

Returns either the constant [`ON_ORIENTED_BOUNDARY`](@ref),
[`ON_POSITIVE_SIDE`](@ref), or [`ON_NEGATIVE_SIDE`](@ref), iff `p` lies on the
boundary, properly on the positive side, or properly on the negative side of
`s`, resp.
"""
oriented_side(s::Sphere3, p::Point3)

"""
    bounded_side(s::Sphere3, p::Point3)

Returns [`ON_BOUNDED_SIDE`](@ref), [`ON_BOUNDARY`](@ref), or
[`ON_UNBOUNDED_SIDE`](@ref) iff p lies properly inside, on the boundary, or
properly outside of `s`, resp.
"""
bounded_side(s::Sphere3, p::Point3)

"""
    has_on_positive_side(s::Sphere3, p::Point3)

Returns `true`, iff `p` lies on the positive side of `s`.
"""
has_on_positive_side(s::Sphere3, p::Point3)

"""
    has_on_negative_side(s::Sphere3, p::Point3)

Returns `true`, iff `p` lies on the negative side of `s`.
"""
has_on_negative_side(s::Sphere3, p::Point3)

"""
    has_on_boundary(s::Sphere3, p::Point3)

Returns `true`, iff `p` lies on the boundary of `s`.
"""
has_on_boundary(s::Sphere3, p::Point3)

"""
    has_on_bounded_side(s::Sphere3, p::Point3)

Returns `true`, iff `p` lies properly inside of `s`.
"""
has_on_bounded_side(s::Sphere3, p::Point3)

"""
    has_on_unbounded_side(s::Sphere3, p::Point3)

Returns `true`, iff `p` lies properly outside of `s`.
"""
has_on_unbounded_side(s::Sphere3, p::Point3)

"""
    opposite(s::Sphere3)

Returns the sphere with the same center and squared radius as `s`, but with
opposite orientation.
"""
opposite(s::Sphere3)

"""
    orthogonal_transform(s::Sphere3, at::AffTransformation3)

Returns the circle obtained by applying `at` on `s`.

!!! info "Precondition"

    `at` is an orthogonal transformation.
"""
orthogonal_transform(s::Sphere3, at::AffTransformation3)

"""
    bbox(s::Sphere3)

Returns a bounding box containing `s`.
"""
bbox(s::Sphere3)


text:
using LinearAlgebra
using SparseArrays

function ham_to_superop(H::AbstractSparseMatrix; convention)
    # See https://arxiv.org/abs/1312.0111, Appendix B.2
    ⊗(A, B) = kron(A, B)
    𝟙 = SparseMatrixCSC{ComplexF64,Int64}(sparse(I, size(H)[1], size(H)[2]))
    H_T = sparse(transpose(H))
    L = sparse(𝟙 ⊗ H - H_T ⊗ 𝟙)
    if convention == :TDSE
        return L
    elseif convention == :LvN
        return 1im * L
    else
        throw(ArgumentError("convention must be :TDSE or :LvN"))
    end
end

function ham_to_superop(H::AbstractMatrix; convention)
    return ham_to_superop(sparse(H); convention=convention)
end

function lindblad_to_superop(A::AbstractSparseMatrix; convention)
    # See https://arxiv.org/abs/1312.0111, Appendix B.2
    ⊗(A, B) = kron(A, B)
    A⁺ = sparse(A')
    A⁺ᵀ = sparse(transpose(A⁺))
    A⁺_A = sparse(A⁺ * A)
    A⁺_A_ᵀ = sparse(transpose(A⁺_A))
    𝟙 = SparseMatrixCSC{ComplexF64,Int64}(sparse(I, size(A)[1], size(A)[2]))
    D = sparse(A⁺ᵀ ⊗ A - (𝟙 ⊗ A⁺_A) / 2 - (A⁺_A_ᵀ ⊗ 𝟙) / 2)
    if convention == :TDSE
        return 1im * D
    elseif convention == :LvN
        return D
    else
        throw(ArgumentError("convention must be :TDSE or :LvN"))
    end
end

function lindblad_to_superop(A::AbstractMatrix; convention)
    return lindblad_to_superop(sparse(A); convention=convention)
end

function dissipator(c_ops; convention)
    N = size(c_ops[1])[1]
    @assert N == size(c_ops[1])[2]
    D = spzeros(ComplexF64, N^2, N^2)
    for A in c_ops
        D += lindblad_to_superop(A; convention=convention)
    end
    return D
end

nhilbert(H::AbstractMatrix) = size(H)[1]
nhilbert(H::Tuple{HT,ET}) where {HT<:AbstractMatrix,ET} = size(H[1])[1]

@doc raw"""Construct a Liouvillian super-operator.

```julia
ℒ = liouvillian(Ĥ, c_ops=(); convention=:LvN)
```

calculates the sparse Liouvillian super-operator `ℒ` from the Hamiltonian `Ĥ`
and a list `c_ops` of Lindblad operators.

With `convention=:LvN`, applying the resulting `ℒ` to a vectorized density
matrix `ρ⃗` calculates ``\frac{d}{dt} \vec{\rho}(t) = ℒ \vec{\rho}(t)``
equivalent to the Liouville-von-Neumann equation for the density matrix ``ρ̂``,

```math
\frac{d}{dt} ρ̂(t)
= -i [Ĥ, ρ̂(t)] + \sum_k\left(
    Â_k ρ̂ Â_k^\dagger
    - \frac{1}{2} A_k^\dagger Â_k ρ̂
    - \frac{1}{2} ρ̂ Â_k^\dagger Â_k
  \right)\,,
```

where the Lindblad operators ``Â_k`` are the elements of `c_ops`.

The Hamiltonian ``Ĥ`` may be time-dependent, using a nested-tuple format by
default, e.g., `(Ĥ₀, (H₁, ϵ₁), (H₂, ϵ₂))`, where `ϵ₁` and `ϵ₂` are functions of
time. In this case, the resulting `ℒ` will also be in nested tuple
format, `ℒ = (ℒ₀, (ℒ₁, ϵ₁), (ℒ₂, ϵ₂))`, where the initial element contains the
superoperator `ℒ₀` for the static component of the Liouvillian, i.e., the
commutator with the drift Hamiltonian `Ĥ₀`, plus the dissipator (sum over
``k``), as a sparse matrix. Time-dependent Lindblad operators are not
supported. The remaining elements are tuples `(ℒ₁, ϵ₁)` and `(ℒ₂, ϵ₂)`
corresponding to the commutators with the two control Hamiltonians, where `ℒ₁`
and `ℒ₂` again are sparse matrices.

If ``Ĥ`` is not time-dependent, the resulting `ℒ` will be a single-element
tuple containing the Liouvillian as a sparse matrix, `ℒ = (ℒ₀, )`.

With `convention=:TDSE`, the Liouvillian will be constructed for the equation
of motion ``-i \hbar \frac{d}{dt} \vec{\rho}(t) = ℒ \vec{\rho}(t)`` to match
exactly the form of the time-dependent Schrödinger equation. While this
notation is not standard in the literature of open quantum systems, it has the
benefit that the resulting `ℒ` can be used in a numerical propagator for a
(non-Hermitian) Schrödinger equation without any change. Thus, for numerical
applications, `convention=:TDSE` is generally preferred. The returned `ℒ`
between the two conventions differs only by a factor of ``i``, since we
generally assume ``\hbar=1``.

The `convention` keyword argument is mandatory, to force a conscious choice.

See [Goerz et. al. "Optimal control theory for a unitary operation under
dissipative evolution", arXiv 1312.0111v2, Appendix
B.2](https://arxiv.org/abs/1312.0111v2) for the explicit construction of the
Liouvillian superoperator as a sparse matrix.
"""
function liouvillian(H::Tuple, c_ops=(); convention)
    L = []
    if length(c_ops) > 0
        L0 = dissipator(c_ops; convention=convention)
    else
        if length(H) > 0
            N = nhilbert(H[1])
            L0 = spzeros(ComplexF64, N^2, N^2)
        else
            throw(ArgumentError("At least one of H and c_ops must be non-empty"))
        end
    end
    for part in H
        if isa(part, Tuple)
            push!(L, (ham_to_superop(part[1], convention=convention), part[2]))
        else
            L0 += ham_to_superop(part, convention=convention)
        end
    end
    return Tuple((L0, L...))
end

function liouvillian(H::AbstractMatrix, c_ops=(); convention)
    return liouvillian((H,), c_ops; convention=convention)
end

function liouvillian(H::Nothing, c_ops=(); convention)
    return liouvillian((), c_ops; convention=convention)
end


text:
# TODO overload print/show/display function to not display model and data

struct jlModel
   m::Ptr{mjModel}

   qpos0::Array{mjtNum}
   qpos_spring::Array{mjtNum}
   body_parentid::Array{Cint}
   body_rootid::Array{Cint}
   body_weldid::Array{Cint}
   body_mocapid::Array{Cint}
   body_jntnum::Array{Cint}
   body_jntadr::Array{Cint}
   body_dofnum::Array{Cint}
   body_dofadr::Array{Cint}
   body_geomnum::Array{Cint}
   body_geomadr::Array{Cint}
   body_simple::Array{mjtByte}
   body_sameframe::Array{mjtByte}
   body_pos::Array{mjtNum}
   body_quat::Array{mjtNum}
   body_ipos::Array{mjtNum}
   body_iquat::Array{mjtNum}
   body_mass::Array{mjtNum}
   body_subtreemass::Array{mjtNum}
   body_inertia::Array{mjtNum}
   body_invweight0::Array{mjtNum}
   body_user::Array{mjtNum}
   jnt_type::Array{Cint}
   jnt_qposadr::Array{Cint}
   jnt_dofadr::Array{Cint}
   jnt_bodyid::Array{Cint}
   jnt_group::Array{Cint}
   jnt_limited::Array{mjtByte}
   jnt_solref::Array{mjtNum}
   jnt_solimp::Array{mjtNum}
   jnt_pos::Array{mjtNum}
   jnt_axis::Array{mjtNum}
   jnt_stiffness::Array{mjtNum}
   jnt_range::Array{mjtNum}
   jnt_margin::Array{mjtNum}
   jnt_user::Array{mjtNum}

   dof_bodyid::Array{Cint}
   dof_jntid::Array{Cint}
   dof_parentid::Array{Cint}
   dof_Madr::Array{Cint}
   dof_simplenum::Array{Cint}
   dof_solref::Array{mjtNum}
   dof_solimp::Array{mjtNum}
   dof_frictionloss::Array{mjtNum}
   dof_armature::Array{mjtNum}
   dof_damping::Array{mjtNum}
   dof_invweight0::Array{mjtNum}
   dof_M0::Array{mjtNum}

   geom_type::Array{Cint}
   geom_contype::Array{Cint}
   geom_conaffinity::Array{Cint}
   geom_condim::Array{Cint}
   geom_bodyid::Array{Cint}
   geom_dataid::Array{Cint}
   geom_matid::Array{Cint}
   geom_group::Array{Cint}
   geom_priority::Array{Cint}
   geom_sameframe::Array{mjtByte}
   geom_solmix::Array{mjtNum}
   geom_solref::Array{mjtNum}
   geom_solimp::Array{mjtNum}
   geom_size::Array{mjtNum}
   geom_rbound::Array{mjtNum}
   geom_pos::Array{mjtNum}
   geom_quat::Array{mjtNum}
   geom_friction::Array{mjtNum}
   geom_margin::Array{mjtNum}
   geom_gap::Array{mjtNum}
   geom_user::Array{mjtNum}
   geom_rgba::Array{Cfloat}

   site_type::Array{Cint}
   site_bodyid::Array{Cint}
   site_matid::Array{Cint}
   site_group::Array{Cint}
   site_sameframe::Array{mjtByte}
   site_size::Array{mjtNum}
   site_pos::Array{mjtNum}
   site_quat::Array{mjtNum}
   site_user::Array{mjtNum}
   site_rgba::Array{Cfloat}

   cam_mode::Array{Cint}
   cam_bodyid::Array{Cint}
   cam_targetbodyid::Array{Cint}
   cam_pos::Array{mjtNum}
   cam_quat::Array{mjtNum}
   cam_poscom0::Array{mjtNum}
   cam_pos0::Array{mjtNum}
   cam_mat0::Array{mjtNum}
   cam_fovy::Array{mjtNum}
   cam_ipd::Array{mjtNum}
   cam_user::Array{mjtNum}

   light_mode::Array{Cint}
   light_bodyid::Array{Cint}
   light_targetbodyid::Array{Cint}
   light_directional::Array{mjtByte}
   light_castshadow::Array{mjtByte}
   light_active::Array{mjtByte}
   light_pos::Array{mjtNum}
   light_dir::Array{mjtNum}
   light_poscom0::Array{mjtNum}
   light_pos0::Array{mjtNum}
   light_dir0::Array{mjtNum}
   light_attenuation::Array{Cfloat}
   light_cutoff::Array{Cfloat}
   light_exponent::Array{Cfloat}
   light_ambient::Array{Cfloat}
   light_diffuse::Array{Cfloat}
   light_specular::Array{Cfloat}

   mesh_vertadr::Array{Cint}
   mesh_vertnum::Array{Cint}
   mesh_texcoordadr::Array{Cint}
   mesh_faceadr::Array{Cint}
   mesh_facenum::Array{Cint}
   mesh_graphadr::Array{Cint}
   mesh_vert::Array{Cfloat}
   mesh_normal::Array{Cfloat}
   mesh_texcoord::Array{Cfloat}
   mesh_face::Array{Cint}
   mesh_graph::Array{Cint}

   skin_matid::Array{Cint}
   skin_rgba::Array{Float32}
   skin_inflate::Array{Float32}
   skin_vertadr::Array{Cint}
   skin_vertnum::Array{Cint}
   skin_texcoordadr::Array{Cint}
   skin_faceadr::Array{Cint}
   skin_facenum::Array{Cint}
   skin_boneadr::Array{Cint}
   skin_bonenum::Array{Cint}
   skin_vert::Array{Float32}
   skin_texcoord::Array{Float32}
   skin_face::Array{Cint}
   skin_bonevertadr::Array{Cint}
   skin_bonevertnum::Array{Cint}
   skin_bonebindpos::Array{Float32}
   skin_bonebindquat::Array{Float32}
   skin_bonebodyid::Array{Cint}
   skin_bonevertid::Array{Cint}
   skin_bonevertweight::Array{Float32}

   hfield_size::Array{mjtNum}
   hfield_nrow::Array{Cint}
   hfield_ncol::Array{Cint}
   hfield_adr::Array{Cint}
   hfield_data::Array{Cfloat}

   tex_type::Array{Cint}
   tex_height::Array{Cint}
   tex_width::Array{Cint}
   tex_adr::Array{Cint}
   tex_rgb::Array{mjtByte}

   mat_texid::Array{Cint}
   mat_texuniform::Array{mjtByte}
   mat_texrepeat::Array{Cfloat}
   mat_emission::Array{Cfloat}
   mat_specular::Array{Cfloat}
   mat_shininess::Array{Cfloat}
   mat_reflectance::Array{Cfloat}
   mat_rgba::Array{Cfloat}

   pair_dim::Array{Cint}
   pair_geom1::Array{Cint}
   pair_geom2::Array{Cint}
   pair_signature::Array{Cint}
   pair_solref::Array{mjtNum}
   pair_solimp::Array{mjtNum}
   pair_margin::Array{mjtNum}
   pair_gap::Array{mjtNum}
   pair_friction::Array{mjtNum}

   exclude_signature::Array{Cint}

   eq_type::Array{Cint}
   eq_obj1id::Array{Cint}
   eq_obj2id::Array{Cint}
   eq_active::Array{mjtByte}
   eq_solref::Array{mjtNum}
   eq_solimp::Array{mjtNum}
   eq_data::Array{mjtNum}

   tendon_adr::Array{Cint}
   tendon_num::Array{Cint}
   tendon_matid::Array{Cint}
   tendon_group::Array{Cint}
   tendon_limited::Array{mjtByte}
   tendon_width::Array{mjtNum}
   tendon_solref_lim::Array{mjtNum}
   tendon_solimp_lim::Array{mjtNum}
   tendon_solref_fri::Array{mjtNum}
   tendon_solimp_fri::Array{mjtNum}
   tendon_range::Array{mjtNum}
   tendon_margin::Array{mjtNum}
   tendon_stiffness::Array{mjtNum}
   tendon_damping::Array{mjtNum}
   tendon_frictionloss::Array{mjtNum}
   tendon_lengthspring::Array{mjtNum}
   tendon_length0::Array{mjtNum}
   tendon_invweight0::Array{mjtNum}
   tendon_user::Array{mjtNum}
   tendon_rgba::Array{Cfloat}

   wrap_type::Array{Cint}
   wrap_objid::Array{Cint}
   wrap_prm::Array{mjtNum}

   actuator_trntype::Array{Cint}
   actuator_dyntype::Array{Cint}
   actuator_gaintype::Array{Cint}
   actuator_biastype::Array{Cint}
   actuator_trnid::Array{Cint}
   actuator_group::Array{Cint}
   actuator_ctrllimited::Array{mjtByte}
   actuator_forcelimited::Array{mjtByte}
   actuator_dynprm::Array{mjtNum}
   actuator_gainprm::Array{mjtNum}
   actuator_biasprm::Array{mjtNum}
   actuator_ctrlrange::Array{mjtNum}
   actuator_forcerange::Array{mjtNum}
   actuator_gear::Array{mjtNum}
   actuator_cranklength::Array{mjtNum}
   actuator_acc0::Array{mjtNum}
   actuator_length0::Array{mjtNum}
   actuator_lengthrange::Array{mjtNum}
   actuator_user::Array{mjtNum}

   sensor_type::Array{Cint}
   sensor_datatype::Array{Cint}
   sensor_needstage::Array{Cint}
   sensor_objtype::Array{Cint}
   sensor_objid::Array{Cint}
   sensor_dim::Array{Cint}
   sensor_adr::Array{Cint}
   sensor_cutoff::Array{mjtNum}
   sensor_noise::Array{mjtNum}
   sensor_user::Array{mjtNum}

   numeric_adr::Array{Cint}
   numeric_size::Array{Cint}
   numeric_data::Array{mjtNum}

   text_adr::Array{Cint}
   text_size::Array{Cint}
   text_data::Array{UInt8}

   tuple_adr::Array{Cint}
   tuple_size::Array{Cint}
   tuple_objtype::Array{Cint}
   tuple_objid::Array{Cint}
   tuple_objprm::Array{mjtNum}

   key_time::Array{mjtNum}
   key_qpos::Array{mjtNum}
   key_qvel::Array{mjtNum}
   key_act::Array{mjtNum}

   name_bodyadr::Array{Cint}
   name_jntadr::Array{Cint}
   name_geomadr::Array{Cint}
   name_siteadr::Array{Cint}
   name_camadr::Array{Cint}
   name_lightadr::Array{Cint}
   name_meshadr::Array{Cint}
   name_skinadr::Array{Cint}
   name_hfieldadr::Array{Cint}
   name_texadr::Array{Cint}
   name_matadr::Array{Cint}
   name_pairadr::Array{Cint}
   name_excludeadr::Array{Cint}
   name_eqadr::Array{Cint}
   name_tendonadr::Array{Cint}
   name_actuatoradr::Array{Cint}
   name_sensoradr::Array{Cint}
   name_numericadr::Array{Cint}
   name_textadr::Array{Cint}
   name_tupleadr::Array{Cint}
   name_keyadr::Array{Cint}
   names::Array{UInt8}
end

struct jlData
   d::Ptr{mjData} # point to c struct

   stack::Array{mjtNum}

   qpos::Array{mjtNum}
   qvel::Array{mjtNum}
   act::Array{mjtNum}
   qacc_warmstart::Array{mjtNum}
   ctrl::Array{mjtNum}
   qfrc_applied::Array{mjtNum}
   xfrc_applied::Array{mjtNum}
   qacc::Array{mjtNum}
   act_dot::Array{mjtNum}
   mocap_pos::Array{mjtNum}
   mocap_quat::Array{mjtNum}
   userdata::Array{mjtNum}
   sensordata::Array{mjtNum}

   xpos::Array{mjtNum}
   xquat::Array{mjtNum}
   xmat::Array{mjtNum}
   xipos::Array{mjtNum}
   ximat::Array{mjtNum}
   xanchor::Array{mjtNum}
   xaxis::Array{mjtNum}
   geom_xpos::Array{mjtNum}
   geom_xmat::Array{mjtNum}
   site_xpos::Array{mjtNum}
   site_xmat::Array{mjtNum}
   cam_xpos::Array{mjtNum}
   cam_xmat::Array{mjtNum}
   light_xpos::Array{mjtNum}
   light_xdir::Array{mjtNum}

   subtree_com::Array{mjtNum}
   cdof::Array{mjtNum}
   cinert::Array{mjtNum}
   ten_wrapadr::Array{Cint}
   ten_wrapnum::Array{Cint}
   ten_J_rownnz::Array{Cint}
   ten_J_rowadr::Array{Cint}
   ten_J_colind::Array{Cint}
   ten_length::Array{mjtNum}
   ten_J::Array{mjtNum}
   wrap_obj::Array{Cint}
   wrap_xpos::Array{mjtNum}
   actuator_length::Array{mjtNum}
   actuator_moment::Array{mjtNum}

   crb::Array{mjtNum}
   qM::Array{mjtNum}
   qLD::Array{mjtNum}
   qLDiagInv::Array{mjtNum}
   qLDiagSqrtInv::Array{mjtNum}

   contact::Array{mjContact}

   efc_type::Array{Cint}
   efc_id::Array{Cint}
   efc_J_rownnz::Array{Cint}
   efc_J_rowadr::Array{Cint}
   efc_J_rowsuper::Array{Cint}
   efc_J_colind::Array{Cint}
   efc_JT_rownnz::Array{Cint}
   efc_JT_rowadr::Array{Cint}
   efc_JT_rowsuper::Array{Cint}
   efc_JT_colind::Array{Cint}
   efc_J::Array{mjtNum}
   efc_JT::Array{mjtNum}
   efc_pos::Array{mjtNum}
   efc_margin::Array{mjtNum}
   efc_frictionloss::Array{mjtNum}
   efc_diagApprox::Array{mjtNum}
   efc_KBIP::Array{mjtNum}
   efc_D::Array{mjtNum}
   efc_R::Array{mjtNum}

   efc_AR_rownnz::Array{Cint}
   efc_AR_rowadr::Array{Cint}
   efc_AR_colind::Array{Cint}
   efc_AR::Array{mjtNum}

   ten_velocity::Array{mjtNum}
   actuator_velocity::Array{mjtNum}
   cvel::Array{mjtNum}
   cdof_dot::Array{mjtNum}
   qfrc_bias::Array{mjtNum}
   qfrc_passive::Array{mjtNum}
   efc_vel::Array{mjtNum}
   efc_aref::Array{mjtNum}
   subtree_linvel::Array{mjtNum}
   subtree_angmom::Array{mjtNum}
   actuator_force::Array{mjtNum}
   qfrc_actuator::Array{mjtNum}
   qfrc_unc::Array{mjtNum}
   qacc_unc::Array{mjtNum}

   efc_b::Array{mjtNum}
   efc_force::Array{mjtNum}
   efc_state::Array{Cint}
   qfrc_constraint::Array{mjtNum}

   qfrc_inverse::Array{mjtNum}

   cacc::Array{mjtNum}
   cfrc_int::Array{mjtNum}
   cfrc_ext::Array{mjtNum}
end

# added stack field manually
# manually did second terms in the parantheses
function getdatasize(m::mjModel, d::mjData)
   return Dict(
               :stack=>(d.nstack,1),
               :qpos=>(m.nq,1),
               :qvel=>(m.nv,1),
               :act=>(m.na,1),
               :qacc_warmstart=>(m.nv,1),
               :ctrl=>(m.nu,1),
               :qfrc_applied=>(m.nv,1),
               :xfrc_applied=>(m.nbody,6),
               :qacc=>(m.nv,1),
               :act_dot=>(m.na,1),
               :mocap_pos=>(m.nmocap,3),
               :mocap_quat=>(m.nmocap,4),
               :userdata=>(m.nuserdata,1),
               :sensordata=>(m.nsensordata,1),
               :xpos=>(m.nbody,3),
               :xquat=>(m.nbody,4),
               :xmat=>(m.nbody,9),
               :xipos=>(m.nbody,3),
               :ximat=>(m.nbody,9),
               :xanchor=>(m.njnt,3),
               :xaxis=>(m.njnt,3),
               :geom_xpos=>(m.ngeom,3),
               :geom_xmat=>(m.ngeom,9),
               :site_xpos=>(m.nsite,3),
               :site_xmat=>(m.nsite,9),
               :cam_xpos=>(m.ncam,3),
               :cam_xmat=>(m.ncam,9),
               :light_xpos=>(m.nlight,3),
               :light_xdir=>(m.nlight,3),
               :subtree_com=>(m.nbody,3),
               :cdof=>(m.nv,6),
               :cinert=>(m.nbody,10),
               :ten_wrapadr=>(m.ntendon,1),
               :ten_wrapnum=>(m.ntendon,1),
               :ten_J_rownnz=>(m.ntendon,1),
               :ten_J_rowadr=>(m.ntendon,1),
               :ten_J_colind=>(m.ntendon,m.nv),
               :ten_length=>(m.ntendon,1),
               :ten_J=>(m.ntendon,m.nv),
               :wrap_obj=>(m.nwrap*2,1),
               :wrap_xpos=>(m.nwrap*2,3),
               :actuator_length=>(m.nu,1),
               :actuator_moment=>(m.nu,m.nv),
               :crb=>(m.nbody,10),
               :qM=>(m.nM,1),
               :qLD=>(m.nM,1),
               :qLDiagInv=>(m.nv,1),
               :qLDiagSqrtInv=>(m.nv,1),
               :contact=>(m.nconmax,1),
               :efc_type=>(m.njmax,1),
               :efc_id=>(m.njmax,1),
               :efc_J_rownnz=>(m.njmax,1),
               :efc_J_rowadr=>(m.njmax,1),
               :efc_J_rowsuper=>(m.njmax,1),
               :efc_J_colind=>(m.njmax,m.nv),
               :efc_JT_rownnz=>(m.nv,1),
               :efc_JT_rowadr=>(m.nv,1),
               :efc_JT_rowsuper=>(m.nv,1),
               :efc_JT_colind=>(m.nv,m.njmax),
               :efc_J=>(m.njmax,m.nv),
               :efc_JT=>(m.nv,m.njmax),
               :efc_pos=>(m.njmax,1),
               :efc_margin=>(m.njmax,1),
               :efc_frictionloss=>(m.njmax,1),
               :efc_diagApprox=>(m.njmax,1),
               :efc_KBIP=>(m.njmax,4),
               :efc_D=>(m.njmax,1),
               :efc_R=>(m.njmax,1),
               :efc_AR_rownnz=>(m.njmax,1),
               :efc_AR_rowadr=>(m.njmax,1),
               :efc_AR_colind=>(m.njmax,m.njmax),
               :efc_AR=>(m.njmax,m.njmax),
               :ten_velocity=>(m.ntendon,1),
               :actuator_velocity=>(m.nu,1),
               :cvel=>(m.nbody,6),
               :cdof_dot=>(m.nv,6),
               :qfrc_bias=>(m.nv,1),
               :qfrc_passive=>(m.nv,1),
               :efc_vel=>(m.njmax,1),
               :efc_aref=>(m.njmax,1),
               :subtree_linvel=>(m.nbody,3),
               :subtree_angmom=>(m.nbody,3),
               :actuator_force=>(m.nu,1),
               :qfrc_actuator=>(m.nv,1),
               :qfrc_unc=>(m.nv,1),
               :qacc_unc=>(m.nv,1),
               :efc_b=>(m.njmax,1),
               :efc_force=>(m.njmax,1),
               :efc_state=>(m.njmax,1),
               :qfrc_constraint=>(m.nv,1),
               :qfrc_inverse=>(m.nv,1),
               :cacc=>(m.nbody,6),
               :cfrc_int=>(m.nbody,6),
               :cfrc_ext=>(m.nbody,6)
              )
end

function getmodelsize(m::mjModel)
   return Dict(
               :qpos0=>(m.nq,1),
               :qpos_spring=>(m.nq,1),

               :body_parentid=>(m.nbody,1),
               :body_rootid=>(m.nbody,1),
               :body_weldid=>(m.nbody,1),
               :body_mocapid=>(m.nbody,1),
               :body_jntnum=>(m.nbody,1),
               :body_jntadr=>(m.nbody,1),
               :body_dofnum=>(m.nbody,1),
               :body_dofadr=>(m.nbody,1),
               :body_geomnum=>(m.nbody,1),
               :body_geomadr=>(m.nbody,1),
               :body_simple=>(m.nbody,1),
               :body_sameframe=>(m.nbody,1),
               :body_pos=>(m.nbody,3),
               :body_quat=>(m.nbody,4),
               :body_ipos=>(m.nbody,3),
               :body_iquat=>(m.nbody,4),
               :body_mass=>(m.nbody,1),
               :body_subtreemass=>(m.nbody,1),
               :body_inertia=>(m.nbody,3),
               :body_invweight0=>(m.nbody,2),
               :body_user=>(m.nbody,m.nuser_body),

               :jnt_type=>(m.njnt,1),
               :jnt_qposadr=>(m.njnt,1),
               :jnt_dofadr=>(m.njnt,1),
               :jnt_bodyid=>(m.njnt,1),
               :jnt_group=>(m.njnt,1),
               :jnt_limited=>(m.njnt,1),
               :jnt_solref=>(m.njnt,NREF),
               :jnt_solimp=>(m.njnt,NIMP),
               :jnt_pos=>(m.njnt,3),
               :jnt_axis=>(m.njnt,3),
               :jnt_stiffness=>(m.njnt,1),
               :jnt_range=>(m.njnt,2),
               :jnt_margin=>(m.njnt,1),
               :jnt_user=>(m.njnt,m.nuser_jnt),

               :dof_bodyid=>(m.nv,1),
               :dof_jntid=>(m.nv,1),
               :dof_parentid=>(m.nv,1),
               :dof_Madr=>(m.nv,1),
               :dof_simplenum=>(m.nv,1),
               :dof_solref=>(m.nv,NREF),
               :dof_solimp=>(m.nv,NIMP),
               :dof_frictionloss=>(m.nv,1),
               :dof_armature=>(m.nv,1),
               :dof_damping=>(m.nv,1),
               :dof_invweight0=>(m.nv,1),
               :dof_M0=>(m.nv,1),
               :geom_type=>(m.ngeom,1),
               :geom_contype=>(m.ngeom,1),
               :geom_conaffinity=>(m.ngeom,1),
               :geom_condim=>(m.ngeom,1),
               :geom_bodyid=>(m.ngeom,1),
               :geom_dataid=>(m.ngeom,1),
               :geom_matid=>(m.ngeom,1),
               :geom_group=>(m.ngeom,1),
               :geom_priority=>(m.ngeom,1),
               :geom_sameframe=>(m.ngeom,1),
               :geom_solmix=>(m.ngeom,1),
               :geom_solref=>(m.ngeom,NREF),
               :geom_solimp=>(m.ngeom,NIMP),
               :geom_size=>(m.ngeom,3),
               :geom_rbound=>(m.ngeom,1),
               :geom_pos=>(m.ngeom,3),
               :geom_quat=>(m.ngeom,4),
               :geom_friction=>(m.ngeom,3),
               :geom_margin=>(m.ngeom,1),
               :geom_gap=>(m.ngeom,1),
               :geom_user=>(m.ngeom,m.nuser_geom),
               :geom_rgba=>(m.ngeom,4),

               :site_type=>(m.nsite,1),
               :site_bodyid=>(m.nsite,1),
               :site_matid=>(m.nsite,1),
               :site_group=>(m.nsite,1),
               :site_sameframe=>(m.nsite,1),
               :site_size=>(m.nsite,3),
               :site_pos=>(m.nsite,3),
               :site_quat=>(m.nsite,4),
               :site_user=>(m.nsite,m.nuser_site),
               :site_rgba=>(m.nsite,4),

               :cam_mode=>(m.ncam,1),
               :cam_bodyid=>(m.ncam,1),
               :cam_targetbodyid=>(m.ncam,1),
               :cam_pos=>(m.ncam,3),
               :cam_quat=>(m.ncam,4),
               :cam_poscom0=>(m.ncam,3),
               :cam_pos0=>(m.ncam,3),
               :cam_mat0=>(m.ncam,9),
               :cam_fovy=>(m.ncam,1),
               :cam_ipd=>(m.ncam,1),
               :cam_user=>(m.ncam,m.nuser_cam),

               :light_mode=>(m.nlight,1),
               :light_bodyid=>(m.nlight,1),
               :light_targetbodyid=>(m.nlight,1),
               :light_directional=>(m.nlight,1),
               :light_castshadow=>(m.nlight,1),
               :light_active=>(m.nlight,1),
               :light_pos=>(m.nlight,3),
               :light_dir=>(m.nlight,3),
               :light_poscom0=>(m.nlight,3),
               :light_pos0=>(m.nlight,3),
               :light_dir0=>(m.nlight,3),
               :light_attenuation=>(m.nlight,3),
               :light_cutoff=>(m.nlight,1),
               :light_exponent=>(m.nlight,1),
               :light_ambient=>(m.nlight,3),
               :light_diffuse=>(m.nlight,3),
               :light_specular=>(m.nlight,3),

               :mesh_vertadr=>(m.nmesh,1),
               :mesh_vertnum=>(m.nmesh,1),
               :mesh_texcoordadr=>(m.nmesh,1),
               :mesh_faceadr=>(m.nmesh,1),
               :mesh_facenum=>(m.nmesh,1),
               :mesh_graphadr=>(m.nmesh,1),
               :mesh_vert=>(m.nmeshvert,3),
               :mesh_normal=>(m.nmeshvert,3),
               :mesh_texcoord=>(m.nmeshtexvert,2),
               :mesh_face=>(m.nmeshface,3),
               :mesh_graph=>(m.nmeshgraph,1),

               :skin_matid=>(m.nskin,1),
               :skin_rgba=>(m.nskin,4),
               :skin_inflate=>(m.nskin,1),
               :skin_vertadr=>(m.nskin,1),
               :skin_vertnum=>(m.nskin,1),
               :skin_texcoordadr=>(m.nskin,1),
               :skin_faceadr=>(m.nskin,1),
               :skin_facenum=>(m.nskin,1),
               :skin_boneadr=>(m.nskin,1),
               :skin_bonenum=>(m.nskin,1),
               :skin_vert=>(m.nskinvert,3),
               :skin_texcoord=>(m.nskintexvert,2),
               :skin_face=>(m.nskinface,3),
               :skin_bonevertadr=>(m.nskinbone,1),
               :skin_bonevertnum=>(m.nskinbone,1),
               :skin_bonebindpos=>(m.nskinbone,3),
               :skin_bonebindquat=>(m.nskinbone,4),
               :skin_bonebodyid=>(m.nskinbone,1),
               :skin_bonevertid=>(m.nskinbonevert,1),
               :skin_bonevertweight=>(m.nskinbonevert,1),

               :hfield_size=>(m.nhfield,4),
               :hfield_nrow=>(m.nhfield,1),
               :hfield_ncol=>(m.nhfield,1),
               :hfield_adr=>(m.nhfield,1),
               :hfield_data=>(m.nhfielddata,1),

               :tex_type=>(m.ntex,1),
               :tex_height=>(m.ntex,1),
               :tex_width=>(m.ntex,1),
               :tex_adr=>(m.ntex,1),
               :tex_rgb=>(m.ntexdata,1),

               :mat_texid=>(m.nmat,1),
               :mat_texuniform=>(m.nmat,1),
               :mat_texrepeat=>(m.nmat,2),
               :mat_emission=>(m.nmat,1),
               :mat_specular=>(m.nmat,1),
               :mat_shininess=>(m.nmat,1),
               :mat_reflectance=>(m.nmat,1),
               :mat_rgba=>(m.nmat,4),

               :pair_dim=>(m.npair,1),
               :pair_geom1=>(m.npair,1),
               :pair_geom2=>(m.npair,1),
               :pair_signature=>(m.npair,1),
               :pair_solref=>(m.npair,NREF),
               :pair_solimp=>(m.npair,NIMP),
               :pair_margin=>(m.npair,1),
               :pair_gap=>(m.npair,1),
               :pair_friction=>(m.npair,5),

               :exclude_signature=>(m.nexclude,1),

               :eq_type=>(m.neq,1),
               :eq_obj1id=>(m.neq,1),
               :eq_obj2id=>(m.neq,1),
               :eq_active=>(m.neq,1),
               :eq_solref=>(m.neq,NREF),
               :eq_solimp=>(m.neq,NIMP),
               :eq_data=>(m.neq,NEQDATA),

               :tendon_adr=>(m.ntendon,1),
               :tendon_num=>(m.ntendon,1),
               :tendon_matid=>(m.ntendon,1),
               :tendon_group=>(m.ntendon,1),
               :tendon_limited=>(m.ntendon,1),
               :tendon_width=>(m.ntendon,1),
               :tendon_solref_lim=>(m.ntendon,NREF),
               :tendon_solimp_lim=>(m.ntendon,NIMP),
               :tendon_solref_fri=>(m.ntendon,NREF),
               :tendon_solimp_fri=>(m.ntendon,NIMP),
               :tendon_range=>(m.ntendon,2),
               :tendon_margin=>(m.ntendon,1),
               :tendon_stiffness=>(m.ntendon,1),
               :tendon_damping=>(m.ntendon,1),
               :tendon_frictionloss=>(m.ntendon,1),
               :tendon_lengthspring=>(m.ntendon,1),
               :tendon_length0=>(m.ntendon,1),
               :tendon_invweight0=>(m.ntendon,1),
               :tendon_user=>(m.ntendon,m.nuser_tendon),
               :tendon_rgba=>(m.ntendon,4),

               :wrap_type=>(m.nwrap,1),
               :wrap_objid=>(m.nwrap,1),
               :wrap_prm=>(m.nwrap,1),

               :actuator_trntype=>(m.nu,1),
               :actuator_dyntype=>(m.nu,1),
               :actuator_gaintype=>(m.nu,1),
               :actuator_biastype=>(m.nu,1),
               :actuator_trnid=>(m.nu,2),
               :actuator_group=>(m.nu,1),
               :actuator_ctrllimited=>(m.nu,1),
               :actuator_forcelimited=>(m.nu,1),
               :actuator_dynprm=>(m.nu,NDYN),
               :actuator_gainprm=>(m.nu,NGAIN),
               :actuator_biasprm=>(m.nu,NBIAS),
               :actuator_ctrlrange=>(m.nu,2),
               :actuator_forcerange=>(m.nu,2),
               :actuator_gear=>(m.nu,6),
               :actuator_cranklength=>(m.nu,1),
               :actuator_acc0=>(m.nu,1),
               :actuator_length0=>(m.nu,1),
               :actuator_lengthrange=>(m.nu,2),
               :actuator_user=>(m.nu,m.nuser_actuator),

               :sensor_type=>(m.nsensor,1),
               :sensor_datatype=>(m.nsensor,1),
               :sensor_needstage=>(m.nsensor,1),
               :sensor_objtype=>(m.nsensor,1),
               :sensor_objid=>(m.nsensor,1),
               :sensor_dim=>(m.nsensor,1),
               :sensor_adr=>(m.nsensor,1),
               :sensor_cutoff=>(m.nsensor,1),
               :sensor_noise=>(m.nsensor,1),
               :sensor_user=>(m.nsensor,m.nuser_sensor),

               :numeric_adr=>(m.nnumeric,1),
               :numeric_size=>(m.nnumeric,1),
               :numeric_data=>(m.nnumericdata,1),

               :text_adr=>(m.ntext,1),
               :text_size=>(m.ntext,1),
               :text_data=>(m.ntextdata,1),

               :tuple_adr=>(m.ntuple,1),
               :tuple_size=>(m.ntuple,1),
               :tuple_objtype=>(m.ntupledata,1),
               :tuple_objid=>(m.ntupledata,1),
               :tuple_objprm=>(m.ntupledata,1),

               :key_time=>(m.nkey,1),
               :key_qpos=>(m.nkey,m.nq),
               :key_qvel=>(m.nkey,m.nv),
               :key_act=>(m.nkey,m.na),

               :name_bodyadr=>(m.nbody,1),
               :name_jntadr=>(m.njnt,1),
               :name_geomadr=>(m.ngeom,1),
               :name_siteadr=>(m.nsite,1),
               :name_camadr=>(m.ncam,1),
               :name_lightadr=>(m.nlight,1),
               :name_meshadr=>(m.nmesh,1),
               :name_skinadr=>(m.nskin,1),
               :name_hfieldadr=>(m.nhfield,1),
               :name_texadr=>(m.ntex,1),
               :name_matadr=>(m.nmat,1),
               :name_pairadr=>(m.npair,1),
               :name_excludeadr=>(m.nexclude,1),
               :name_eqadr=>(m.neq,1),
               :name_tendonadr=>(m.ntendon,1),
               :name_actuatoradr=>(m.nu,1),
               :name_sensoradr=>(m.nsensor,1),
               :name_numericadr=>(m.nnumeric,1),
               :name_textadr=>(m.ntext,1),
               :name_tupleadr=>(m.ntuple,1),
               :name_keyadr=>(m.nkey,1),
               :names=>(m.nnames,1)
              )
end


text:
using CLIMA.PlanetParameters: Omega
export Gravity, RayleighSponge, Subsidence, GeostrophicForcing, Coriolis

# kept for compatibility
# can be removed if no functions are using this
function atmos_source!(f::Function, atmos::AtmosModel, source::Vars, state::Vars, diffusive::Vars, aux::Vars, t::Real)
  f(atmos, source, state, diffusive, aux, t)
end
function atmos_source!(::Nothing, atmos::AtmosModel, source::Vars, state::Vars, diffusive::Vars, aux::Vars, t::Real)
end
# sources are applied additively
function atmos_source!(stuple::Tuple, atmos::AtmosModel, source::Vars, state::Vars, diffusive::Vars, aux::Vars, t::Real)
  map(s -> atmos_source!(s, atmos, source, state, diffusive, aux, t), stuple)
end

abstract type Source
end

struct Gravity <: Source
end
function atmos_source!(::Gravity, atmos::AtmosModel, source::Vars, state::Vars, diffusive::Vars, aux::Vars, t::Real)
  if atmos.ref_state isa HydrostaticState
    source.ρu -= (state.ρ - aux.ref_state.ρ) * aux.orientation.∇Φ
  else
    source.ρu -= state.ρ * aux.orientation.∇Φ
  end
end

struct Coriolis <: Source
end
function atmos_source!(::Coriolis, atmos::AtmosModel, source::Vars, state::Vars, diffusive::Vars, aux::Vars, t::Real)
  # note: this assumes a SphericalOrientation
  source.ρu -= SVector(0, 0, 2*Omega) × state.ρu
end

struct Subsidence{FT} <: Source
  D::FT
end

function atmos_source!(subsidence::Subsidence, atmos::AtmosModel, source::Vars, state::Vars, diffusive::Vars, aux::Vars, t::Real)
  ρ = state.ρ
  z = altitude(atmos.orientation, aux)
  w_sub = subsidence_velocity(subsidence, z)
  k̂ = vertical_unit_vector(atmos.orientation, aux)

  source.ρe -= ρ*w_sub * dot(k̂, diffusive.∇h_tot)
  source.moisture.ρq_tot -= ρ*w_sub * dot(k̂, diffusive.moisture.∇q_tot)
end

subsidence_velocity(subsidence::Subsidence{FT}, z::FT) where {FT} = -subsidence.D*z

struct GeostrophicForcing{FT} <: Source
  f_coriolis::FT
  u_geostrophic::FT
  v_geostrophic::FT
end
function atmos_source!(s::GeostrophicForcing, atmos::AtmosModel, source::Vars, state::Vars, diffusive::Vars, aux::Vars, t::Real)
  u_geo = SVector(s.u_geostrophic, s.v_geostrophic, 0)
  ẑ = vertical_unit_vector(atmos.orientation, aux)
  fkvector = s.f_coriolis * ẑ
  source.ρu -= fkvector × (state.ρu .- state.ρ*u_geo)
end

"""
    RayleighSponge{FT} <: Source

Rayleigh Damping (Linear Relaxation) for top wall momentum components
Assumes laterally periodic boundary conditions for LES flows. Momentum components
are relaxed to reference values (zero velocities) at the top boundary.
"""
struct RayleighSponge{FT} <: Source
  "Maximum domain altitude (m)"
  z_max::FT
  "Altitude at with sponge starts (m)"
  z_sponge::FT
  "Sponge Strength 0 ⩽ α_max ⩽ 1"
  α_max::FT
  "Relaxation velocity components"
  u_relaxation::SVector{3,FT}
  "Sponge exponent"
  γ::FT
end
function atmos_source!(s::RayleighSponge, atmos::AtmosModel, source::Vars, state::Vars, diffusive::Vars, aux::Vars, t::Real)
  z = altitude(atmos.orientation, aux)
  if z >= s.z_sponge
    r = (z - s.z_sponge)/(s.z_max-s.z_sponge)
    β_sponge = s.α_max * sinpi(r/2)^s.γ
    source.ρu -= β_sponge * (state.ρu .- state.ρ*s.u_relaxation)
  end
end

max_stars_repo_name: favba/CLIMA

max_issues_repo_name: favba/CLIMA

max_forks_repo_name: favba/CLIMA


text:
cd(@__DIR__)
using SignalDecomposition
using DelimitedFiles, Test, Random, Statistics
using DynamicalSystemsBase

ds = Systems.lorenz()
tr = trajectory(ds, 500.0; dt = 0.05, Ttr = 100)
lorenzx = tr[:, 1]/std(tr[:, 1])

tr = trajectory(ds, 20; dt = 0.002, Ttr = 100)
lorenzx_slow = tr[:, 1]/std(tr[:, 1])

ds = Systems.roessler()
tr = trajectory(ds, 500.0, dt = 0.05, Ttr = 10)
roesslerz = tr[:, 3]/std(tr[:, 3])

L = length(lorenzx)

noise = randn(Random.MersenneTwister(12441), L)

# trivial periodic component:
te = range(0, 24π, length = length(lorenzx))
periodicf(t) = @. 4 + 7.2cos(t) + 5.6cos(2t + 3π/5)
periodic = periodicf(te)
twopiidx = findfirst(x -> x  ≥ 2π, te)
tperiods = [twopiidx, twopiidx/2]

# t = cumsum(rand(1000)/2)

# %% Run tests
include("periodic_test.jl")
include("lpv_test.jl")
include("product_test.jl")
include("nonlinear_test.jl")
include("timeanomaly_test.jl")


text:
module Products_indexing_test

function count( maxNumber, count )
    if !isnothing( maxNumber ) && maxNumber > 1 && maxNumber < count
        count = maxNumber
    end
    return [ count÷100, count%100 ]
end

function indexing( count )
    if count[1] > 0
        for i in range( 0, count[1]-1, step=1 )
            println( "start=$(i*100) & rows=100" )
            println( "...\\$(i+1).xml")
        end
    end
    if count[2] > 0
        println( "start=$(count[1]*100) & rows=$(count[2])" )
        println( "...\\$(count[1]+1).xml" )
    end
end

function indexing2( start::Integer, val::Integer, maxNumber::Union{Nothing, Integer}=nothing )
    count = val
    println( "COUNT : ", count )

 #Check if `start` has a sensible value
    if start > val
        start = val - 1
    end
    println( "START : ",start)

 # Check if `maxNumber` has a sensible value
    if !isnothing( maxNumber ) && maxNumber > 0 && maxNumber < val
        count = maxNumber
    end

    if count > start
        count -= start
    end

    println( "COUNT(U1) : ", count )
    count = [ count ÷ 100, count % 100 ]
    println( "MAXNUMBER : ", maxNumber )
    println( "COUNT(U2) : ", count )
    println()

    if count[1] > 0
        println("count[1] > 0")
        for i in range( 0, count[1] - 1, step=1 )
            println( "start=$( start + ( i * 100 ) ) & rows=100" )
        end
    end
    if count[2] > 0
        println("count[2] > 0")
        println( "start=$(count[1] * 100 + start) & rows=$(count[2])" )
    end
    println()
    println("THE END")
end

end # module

max_stars_repo_name: DavideFavaro/Tirocinio

max_issues_repo_name: DavideFavaro/Tirocinio

max_forks_repo_name: DavideFavaro/Tirocinio


text:
import RData
using RNGPool
using CoupledConditionalSMC

# data generated with A = 0.95 (not 0.9)
dataJLS = RData.load("demo/ar1data.RData")
ys = vec(dataJLS["observations"])

include("../demo/lgModel.jl")

# they start at time 0 with observations from time 1,2,...
# so we have an initial distribution of N(0,1+0.9^2)
theta = LGTheta(0.9, 1.0, 1.0, 1.0, 0.0, 1.81)
model = makeLGModel(theta, ys)
lM = LinearGaussian.makelM(theta)
ko = kalman(theta, ys)

import Statistics: mean, std

setRNGs(12345)

m = 1000

using DataFrames
jlsInd_df = DataFrame(n=Int[], N=Int[], Type=String[], mean=Union{Float64, Missing}[],
  std=Union{Float64, Missing}[])

for n in [50, 100, 200, 400, 800, 1600, 3200]
  for k in 6:10
    N = 2^k
    println(n, " ", N)
    vs = CoupledConditionalSMC.couplingTimes(model, N, n, m, true, false, 2000)
    push!(jlsInd_df, [n, N, "AT", mean(vs), std(vs)])
    vs = CoupledConditionalSMC.couplingTimes(model, lM, N, n, m, :AS, true, false, 2000)
    push!(jlsInd_df, [n, N, "AS", mean(vs), std(vs)])
    vs = CoupledConditionalSMC.couplingTimes(model, lM, N, n, m, :BS, true, false, 2000)
    push!(jlsInd_df, [n, N, "BS", mean(vs), std(vs)])
  end
end

using JLD2
@save "paper/jlsInd.jld2" jlsInd_df

println(jlsInd_df)


text:
module Simulate_TriplePendulum

using  Modia3D
import Modia3D.ModiaMath

# Solids
groundMaterial = Modia3D.Material(color="DarkGreen", transparency=0.5)
# massProperties = Modia3D.MassProperties()
vmat1 = Modia3D.Material(color="LightBlue" , transparency=0.5)
vmat2 = Modia3D.Material(color="Red")

@assembly Bar(;Lx = 0.1, Ly=Lx/5, Lz=Ly, m=1.0) begin
   frame0 = Modia3D.Object3D(Modia3D.Solid(Modia3D.SolidBeam(Lx,Ly,Lz), Modia3D.MassProperties(m=m), vmat1))
   frame1 = Modia3D.Object3D(frame0; r=[-Lx/2, 0.0, 0.0])
   frame2 = Modia3D.Object3D(frame0; r=[ Lx/2, 0.0, 0.0])
   cyl    = Modia3D.Cylinder(Ly/2,1.2*Ly; material=vmat2)
   cyl1   = Modia3D.Object3D(frame1, cyl; visualizeFrame=false)
   cyl2   = Modia3D.Object3D(frame2, cyl; visualizeFrame=false)
end

@assembly TriplePendulum(;Lx = 1.0, m=1.0) begin
   world = Modia3D.Object3D(Modia3D.CoordinateSystem(0.5*Lx))
   bar1  = Bar(Lx=Lx, m=m)
   bar2  = Bar(Lx=Lx, m=m)
   bar3  = Bar(Lx=Lx, m=m)
   rev1  = Modia3D.Revolute(world, bar1.frame1; axis = 3)
   rev2  = Modia3D.Revolute(bar1.frame2, bar2.frame1; axis = 3)
   rev3  = Modia3D.Revolute(bar2.frame2, bar3.frame1; axis = 3)
end

# Modia3D.visualizeAssembly!( DoublePendulum(), Modia3D.SceneOptions(visualizeFrames=true, defaultFrameLength=0.3) )

triplePendulum = TriplePendulum(sceneOptions=Modia3D.SceneOptions(visualizeFrames=true, defaultFrameLength=0.3))
model = Modia3D.SimulationModel( triplePendulum )
result = ModiaMath.simulate!(model; stopTime=5.0, tolerance=1e-5,interval=0.001, log=false)

ModiaMath.plot(result, [("rev1.phi", "rev2.phi", "rev3.phi"),
                        ("rev1.w"  , "rev2.w", "rev3.w"),
                        ("rev1.a"  , "rev2.a", "rev3.a")])

println("... success of Simulate_TriplePendulum.jl!")
end


text:
function parse_opt_file(filename)
    lines = readlines(filename)
    idx1 = findlast(line -> occursin("Band", line) & occursin("Eigenvalue", line), lines)
    idx2 = findfirst(line -> occursin("Gap", line), lines[idx1+1:end])
    return map(line -> parse(Float64, getindex(split(line), 2)), lines[idx1+1:idx1+idx2-2])
end


text:
a = factorial(big(100))
s = split(string(a), "")
m = map(x -> parse(Int, x), s)
println(sum(m))

max_stars_repo_name: sujimodern/project-euler-jl

max_issues_repo_name: sujimodern/project-euler-jl

max_forks_repo_name: sujimodern/project-euler-jl


text:
@testset "Convective derivative of the complex potential collection of vortices" begin
    atol = 100*eps()
    nblob = 20
    pos = randn(ComplexF64,nblob)
    str = randn(length(pos))
    σ = 1e-4

    blobs = Vortex.Blob.(pos,str, σ)
    blobsvel = randn(ComplexF64, nblob)

    dFdt = complex(0.0)
    for i=1:nblob
        dFdt += circulation(blobs[i])/(2*π*im)*(-blobsvel[i])*1/(0.0 - blobs[i].z)
    end
    @test isapprox(dFdt, convective_complexpotential(complex(0.0), blobs, blobsvel), atol = atol)
end

@testset "Convective derivative of the complex potential collection of sources/sinks" begin
    atol = 100*eps()
    nblob = 20
    pos = randn(ComplexF64,nblob)
    str = randn(length(pos))
    σ = 1e-4

    blobs = Source.Blob.(pos,str, σ)
    blobsvel = randn(ComplexF64, nblob)

    dFdt = complex(0.0)
    for i=1:nblob
        dFdt += flux(blobs[i])/(2*π)*(-blobsvel[i])*1/(0.0 - blobs[i].z)
    end
    @test isapprox(dFdt, convective_complexpotential(complex(0.0), blobs, blobsvel), atol = atol)
end


text:
using SolidModeling;
using Test;

# Test union of two cubes
function cubeUnion()
    c1 = cube(0.0, 0.0, 0.0, 1.0, 1.0, 1.0);
    c2 = cube(0.5, 0.5, 0.5, 2.0, 2.0, 2.0);
    c = bunion(c1, c2);

    return volume(c)
end

function cubeUnionReversed()
    c1 = cube(0.0, 0.0, 0.0, 1.0, 1.0, 1.0);
    c2 = cube(0.5, 0.5, 0.5, 2.0, 2.0, 2.0);
    c = bunion(c2, c1);

    return volume(c)
end

# Test subtraction of two cubes
function cubeSubtract()
    c1 = cube([0.5, 0.5, 0.5], 1.0, 1.0, 1.0)
    c2 = cube(0.5, 0.5, 0.5, 2.0, 2.0, 2.0)
    r = bsubtract(c1, c2)

    v = volume(r)

    return v
end

# Test intersection of two cubes
function cubeIntersect()
    c1 = cube(0.0, 0.0, 0.0, 1.0, 1.0, 1.0)
    c2 = cube(0.5, 0.5, 0.5, 2.0, 2.0, 2.0)

    v = volume(bintersect(c1, c2))

    return v
end

@testset begin
    @test isapprox(cubeUnion(), 4.25)                   # Test against known volume
    @test isapprox(cubeUnion(), cubeUnionReversed())    # Test if ordering doesnt matter
    @test isapprox(cubeSubtract(), (1-0.125))
    @test isapprox(cubeIntersect(), 0.125)
end


text:
export Airplane
export init_traj_straightline, init_traj_feasible_sos, init_traj_line_sos

mutable struct Airplane <: DynamicsModel
  # state: (x,y,z,ψ,v,γ,ϕ,α)
  # pos- xyz, heading- ψ, speed- v, flight path angle- γ, roll- ϕ, angle of attack- α,
  # control: (u_acc, u_ϕ_d, u_α_d)
  x_dim
  u_dim

  x_min
  x_max

  goal_min
  goal_max

  u_min
  u_max

  clearance

  f::Vector
  A::Vector
  B
end
function Airplane(robot::Plane)
  x_dim,u_dim = 8,3

  x_min = [0.;0;0]
  x_max = [120; 120; 28]
  goal_min = [x_max[1:2]; 15] - [10; 10; 10]
  goal_max = x_max

  u_max = [robot.acc_lim; robot.ϕ_d_lim; robot.α_d_lim]
  u_min = -u_max
  clearance = 25.
  return Airplane(x_dim, u_dim, x_min, x_max, goal_min, goal_max, u_min, u_max, clearance, [], [], [])
end

function SCPParam(model::Airplane, fixed_final_time::Bool)
  convergence_threshold = 0.1

  SCPParam(fixed_final_time, convergence_threshold)
end

function SCPParam_GuSTO(model::Airplane)
  # Straight line in workspace
  Δ0 = 10000.
  ω0 = 1.
  ω_max = 100.
  ε = 1.
  ρ0 = 0.1
  ρ1 = 0.4
  β_succ = 2.
  β_fail = 0.5
  γ_fail = 5.

  # Dynamically feasible
  Δ0 = 20000.
  ω0 = 1.
  ω_max = 100.
  ε = 1.
  ρ0 = 0.01
  ρ1 = 0.1
  β_succ = 2.
  β_fail = 0.5
  γ_fail = 8.

  # Dynamically feasible, collision free
  Δ0 = 5000.
  ω0 = 1.
  ω_max = 1000.
  ε = 1.
  ρ0 = 0.01
  ρ1 = 0.1
  β_succ = 2.
  β_fail = 0.5
  γ_fail = 8.

  SCPParam_GuSTO(Δ0, ω0, ω_max, ε, ρ0, ρ1, β_succ, β_fail, γ_fail)
end

function SCPParam_Mao(model::Airplane)
  # Straight line in workspace
  ρ = [-5.; -2; 0.5]
  Δ_u0 = 10000.
  λ = 1.
  α = 2.

  # Dynamically feasible
  ρ = [-5.; -2; 0.5]
  Δ_u0 = 10000.
  λ = 5.
  α = 2.

  # Dynamically feasible, collision free
  ρ = [-5.; -2; 0.5]
  Δ_u0 = 100.
  λ = 1.
  α = 2.

  SCPParam_Mao(ρ, Δ_u0, λ, α)
end

function SCPParam_TrajOpt(model::Airplane)
  # Straight line in workspace
  μ0 = 1.
  s0 = 10000.
  c = 0.25
  τ_plus = 2.
  τ_minus = 0.5
  k = 8.
  ftol = 0.01
  xtol = 0.1
  ctol = 0.01
  max_penalty_iteration = 5
  max_convex_iteration = 5
  max_trust_iteration = 5

  # Dynamically feasible
  μ0 = 1.
  s0 = 10000.
  c = 0.25
  τ_plus = 2.
  τ_minus = 0.5
  k = 5.
  ftol = 0.01
  xtol = 0.1
  ctol = 0.01
  max_penalty_iteration = 5
  max_convex_iteration = 5
  max_trust_iteration = 5

  # Dynamically feasible, collision free
  μ0 = 1.
  s0 = 10000.
  c = 0.25
  τ_plus = 2.
  τ_minus = 0.5
  k = 5.
  ftol = 0.01
  xtol = 0.1
  ctol = 0.01
  max_penalty_iteration = 5
  max_convex_iteration = 5
  max_trust_iteration = 5

  SCPParam_TrajOpt(μ0, s0, c, τ_plus, τ_minus, k, ftol, xtol, ctol,max_penalty_iteration,max_convex_iteration,max_trust_iteration)
end

###############
# Gurobi stuff
###############
function cost_true(traj, traj_prev::Trajectory, SCPP::SCPProblem{Plane{T}, Airplane}) where T
  U,N = traj.U, SCPP.N
  dtp = traj_prev.dt
  Jm = 0
  for k in 1:N-1
    Jm += dtp*norm(U[:,k])^2
  end
  return Jm
end

function cost_true_convexified(traj, traj_prev::Trajectory, SCPP::SCPProblem{Plane{T}, Airplane, E}) where {T,E}
	cost_true(traj, traj_prev, SCPP)
end

#############################
# Trajectory Initializations
#############################
function init_traj_straightline(TOP::TrajectoryOptimizationProblem{Plane{T}, Airplane, E}) where {T,E}
	model, x_init, x_goal = TOP.PD.model, TOP.PD.x_init, TOP.PD.x_goal
	x_dim, u_dim, N, tf_guess = model.x_dim, model.u_dim, TOP.N, TOP.tf_guess

	X = hcat(range(x_init, stop=x_goal, length=N)...)
	U = zeros(u_dim,N)

  for k in 1:3
    X[k,:] = collect(range(X[k,1], stop=X[k,end], length=N))
  end
  for k in 4:8
    X[k,:] = X[k,1]*ones(N)
  end
	Trajectory(X, U, tf_guess)
end

function init_traj_feasible_sos(TOP::TrajectoryOptimizationProblem{Plane{T}, Airplane, E}) where {T,E}
	model, x_init, x_goal = TOP.PD.model, TOP.PD.x_init, TOP.PD.x_goal
	x_dim, u_dim, N, tf_guess = model.x_dim, model.u_dim, TOP.N, TOP.tf_guess

  vars = matread(joinpath(Pkg.dir("GuSTO"), "src", "dynamics","init_feasible_sos.mat"))
  Xfull, Ufull, Tsfull = vars["X"], vars["U"], vars["t_ref"]
  tf_guess = Tsfull[end]

  Nidx = length(Tsfull)
  if Nidx < N
    warn("Initial seed in file has fewer nodes than reqeusted!")
    quit()
  end

  # downsample
  idx = [round(Int,i) for i in collect(range(1, stop=Nidx, length=N))]
  return Trajectory(Xfull[:,idx],Ufull[:,idx[1:end-1]],tf_guess)
end

function init_traj_line_sos(TOP::TrajectoryOptimizationProblem{Plane{T}, Airplane, E}) where {T,E}
	model, x_init, x_goal = TOP.PD.model, TOP.PD.x_init, TOP.PD.x_goal
	x_dim, u_dim, N, tf_guess = model.x_dim, model.u_dim, TOP.N, TOP.tf_guess

  vars = matread(joinpath(Pkg.dir("GuSTO"), "src", "dynamics","init_straight_line_sos.mat"))
  Xfull, Ufull, Tsfull = vars["X"], vars["U"], vars["t_ref"]
  tf_guess = Tsfull[end]

  Nidx = length(Tsfull)
  if Nidx < N
    warn("Initial seed in file has fewer nodes than reqeusted!")
    quit()
  end

  # downsample
  idx = [round(Int,i) for i in collect(range(1, stop=Nidx, length=N))]
  Xfull = Xfull[:,idx]
  Ufull = Ufull[:,idx[1:end-1]]

  return Trajectory(Xfull, Ufull ,tf_guess)
end

#######################
# Constraint-adding
#######################
function initialize_model_params!(SCPP::SCPProblem{Plane{T}, Airplane, E}, traj_prev::Trajectory) where {T,E}
  N, robot, model = SCPP.N, SCPP.PD.robot, SCPP.PD.model
  x_dim, u_dim = model.x_dim, model.u_dim
  Xp, Up = traj_prev.X, traj_prev.U

  model.f, model.A, model.B = [], [], B_dyn(Xp[:,1],robot,model)
  for k = 1:N-1
    push!(model.f, f_dyn(Xp[:,k],Up[:,k],robot,model))
    push!(model.A, A_dyn(Xp[:,k],robot,model))
  end
end

function update_model_params!(SCPP::SCPProblem{Plane{T}, Airplane, E}, traj_prev::Trajectory) where {T,E}
  N, robot, model = SCPP.N, SCPP.PD.robot, SCPP.PD.model
  Xp, Up, f, A = traj_prev.X, traj_prev.U, model.f, model.A

  for k = 1:N-1
    update_f!(f[k], Xp[:,k], Up[:,k], robot, model)
    update_A!(A[k], Xp[:,k], robot, model)
  end
end

macro constraint_abbrev_airplane(traj, traj_prev, SCPP)
	quote
		X, U, Tf = $(esc(traj)).X, $(esc(traj)).U, $(esc(traj)).Tf
    Xp, Up, Tfp, dtp = $(esc(traj_prev)).X, $(esc(traj_prev)).U, $(esc(traj_prev)).Tf, $(esc(traj_prev)).dt
    robot, model, WS, x_init, x_goal = $(esc(SCPP)).PD.robot, $(esc(SCPP)).PD.model, $(esc(SCPP)).WS, $(esc(SCPP)).PD.x_init, $(esc(SCPP)).PD.x_goal
    x_dim, u_dim, N, dh = model.x_dim, model.u_dim, $(esc(SCPP)).N, $(esc(SCPP)).dh
    X,U,Tf,Xp,Up,Tfp,dtp,robot,model,WS,x_init,x_goal,x_dim,u_dim,N,dh
	end
end

## Dynamics constraints
function dynamics_constraints(traj, traj_prev::Trajectory, SCPP::SCPProblem{Plane{T}, Airplane, E}, k::Int, i::Int) where {T,E}
	# Where i is the state index, and k is the timestep index
	X,U,Tf,Xp,Up,Tfp,dtp,robot,model,WS,x_init,x_goal,x_dim,u_dim,N,dh = @constraint_abbrev_airplane(traj, traj_prev, SCPP)
  fp, Ap, Bp = get_f(k, model), get_A(k, model), get_B(k, model)
  if k == N-1
    return Tf*fp + Tfp*(Ap*(X[:,k]-Xp[:,k]) + Bp*(U[:,k]-Up[:,k])) - (X[:,k+1]-X[:,k])/dh
  else
    return 0.5*(Tf*(fp + get_f(k+1, model)) + Tfp*(Ap*(X[:,k]-Xp[:,k]) + Bp*(U[:,k]-Up[:,k])) +
      Tfp*(Ap*(X[:,k+1]-Xp[:,k+1]) + Bp*(U[:,k+1]-Up[:,k+1]))) - (X[:,k+1]-X[:,k])/dh
  end
end

# Get current dynamics structures for a time step
get_f(k::Int, model::Airplane) = model.f[k]
get_A(k::Int, model::Airplane) = model.A[k]
get_B(k::Int, model::Airplane) = model.B

# Generate full dynamics structures for a time step
function f_dyn(x::Vector, u::Vector, robot::Robot, model::Airplane)
  x_dim = model.x_dim
  f = zeros(x_dim)
  update_f!(f, x, u, robot, model)
  return f
end

function update_f!(f, x::Vector, u::Vector, robot::Robot, model::Airplane)
  # pos- xyz, heading- ψ, speed- v, flight path angle- γ, roll- ϕ, angle of attack- α,
  rx, ry, rz, ψ, v, γ, ϕ, α = x
  u_a, u_ϕd, u_αd = u
  g, ρ, Area, mass = robot.g, robot.ρ, robot.Area, robot.mass
  Cd0, Kd, α_0 = robot.Cd0, robot.Kd, robot.α_0
  Fl = π*ρ*Area*v^2*α
  Fd = ρ*Area*v^2*(Cd0 + 4*π^2*Kd*α^2)

  f[1] = v*cos(ψ)*cos(γ)
  f[2] = v*sin(ψ)*cos(γ)
  f[3] = v*sin(γ)
  f[4] = -Fl*sin(ϕ)/(mass*v*cos(γ))
  f[5] = u_a - Fd/mass - g*sin(γ)
  f[6] = Fl*cos(ϕ)/(mass*v) - g*cos(γ)/v
  f[7] = u_ϕd
  f[8] = u_αd
end

function A_dyn(x::Vector, robot::Robot, model::Airplane)
  x_dim = model.x_dim
  A = zeros(x_dim, x_dim)
  update_A!(A, x, robot, model)
  return A
end

function update_A!(A, x::Vector, robot::Robot, model::Airplane)
  rx, ry, rz, ψ, v, γ, ϕ, α = x
  g, ρ, Area, mass = robot.g, robot.ρ, robot.Area, robot.mass
  Cd0, Kd = robot.Cd0, robot.Kd

  # See scripts/symbolic_math.m for derivation of these equations
  A[1,4:6] = [-v*cos(γ)*sin(ψ) cos(γ)*cos(ψ) -v*cos(ψ)*sin(γ)]
  A[2,4:6] = [ v*cos(γ)*cos(ψ) cos(γ)*sin(ψ) -v*sin(γ)*sin(ψ)]
  A[3,5:6] = [                       sin(γ)           v*cos(γ)]
  A[4,5] = -(π*Area*α*ρ*sin(ϕ))/(mass*cos(γ))
  A[4,6] = -(π*Area*α*ρ*v*sin(γ)*sin(ϕ))/(mass*cos(γ)^2)
  A[4,7] = -(π*Area*α*ρ*v*cos(ϕ))/(mass*cos(γ))
  A[4,8] = -(π*Area*ρ*v*sin(ϕ))/(mass*cos(γ))
  A[5,5] = -(2*Area*ρ*v*((39.4784*Kd*α^2)/ + Cd0))/mass
  A[5,6] = -g*cos(γ)
  A[5,8] = -(78.9568*Area*Kd*α*ρ*v^2)/mass
  A[6,5] = (g*cos(γ))/v^2 + (π*Area*α*ρ*cos(ϕ))/mass
  A[6,6] = (g*sin(γ))/v
  A[6,7] = -(π*Area*α*ρ*v*sin(ϕ))/mass
  A[6,8] = (π*Area*ρ*v*cos(ϕ))/mass
end

function B_dyn(x::Vector, robot::Robot, model::Airplane)
  x_dim, u_dim = model.x_dim, model.u_dim
  B = zeros(x_dim, u_dim)
  B[5,1], B[7,2], B[8,3] = 1., 1., 1.
  return B
end

## Convex state inequality constraints
# X: (x,y,z,ψ,v,γ,ϕ, α)
function csi_goal_region_min_constraints(traj, traj_prev::Trajectory, SCPP::SCPProblem{Plane{T}, Airplane, E}, k::Int, i::Int) where {T,E}
	# Where i is the state index, and k is the timestep index
	X,U,Tf,Xp,Up,Tfp,dtp,robot,model,WS,x_init,x_goal,x_dim,u_dim,N,dh = @constraint_abbrev_airplane(traj, traj_prev, SCPP)
  return model.goal_min[i] - X[i,N]
end

function csi_goal_region_max_constraints(traj, traj_prev::Trajectory, SCPP::SCPProblem{Plane{T}, Airplane, E}, k::Int, i::Int) where {T,E}
	X,U,Tf,Xp,Up,Tfp,dtp,robot,model,WS,x_init,x_goal,x_dim,u_dim,N,dh = @constraint_abbrev_airplane(traj, traj_prev, SCPP)
  return X[i,N] - model.goal_max[i]
end

function csi_v_max_constraints(traj, traj_prev::Trajectory, SCPP::SCPProblem{Plane{T}, Airplane, E}, k::Int, i::Int) where {T,E}
	X,U,Tf,Xp,Up,Tfp,dtp,robot,model,WS,x_init,x_goal,x_dim,u_dim,N,dh = @constraint_abbrev_airplane(traj, traj_prev, SCPP)
  return X[5,k]-robot.v_max
end

function csi_v_min_constraints(traj, traj_prev::Trajectory, SCPP::SCPProblem{Plane{T}, Airplane, E}, k::Int, i::Int) where {T,E}
	X,U,Tf,Xp,Up,Tfp,dtp,robot,model,WS,x_init,x_goal,x_dim,u_dim,N,dh = @constraint_abbrev_airplane(traj, traj_prev, SCPP)
  return -X[5,k]+robot.v_min
end

function csi_γ_max_constraints(traj, traj_prev::Trajectory, SCPP::SCPProblem{Plane{T}, Airplane, E}, k::Int, i::Int) where {T,E}
	X,U,Tf,Xp,Up,Tfp,dtp,robot,model,WS,x_init,x_goal,x_dim,u_dim,N,dh = @constraint_abbrev_airplane(traj, traj_prev, SCPP)
  return X[6,k]-robot.γ_max
end

function csi_γ_min_constraints(traj, traj_prev::Trajectory, SCPP::SCPProblem{Plane{T}, Airplane, E}, k::Int, i::Int) where {T,E}
	X,U,Tf,Xp,Up,Tfp,dtp,robot,model,WS,x_init,x_goal,x_dim,u_dim,N,dh = @constraint_abbrev_airplane(traj, traj_prev, SCPP)
  return -X[6,k]-robot.γ_max
end

function csi_ϕ_max_constraints(traj, traj_prev::Trajectory, SCPP::SCPProblem{Plane{T}, Airplane, E}, k::Int, i::Int) where {T,E}
	X,U,Tf,Xp,Up,Tfp,dtp,robot,model,WS,x_init,x_goal,x_dim,u_dim,N,dh = @constraint_abbrev_airplane(traj, traj_prev, SCPP)
  return X[7,k]-robot.γ_max
end

function csi_ϕ_min_constraints(traj, traj_prev::Trajectory, SCPP::SCPProblem{Plane{T}, Airplane, E}, k::Int, i::Int) where {T,E}
	X,U,Tf,Xp,Up,Tfp,dtp,robot,model,WS,x_init,x_goal,x_dim,u_dim,N,dh = @constraint_abbrev_airplane(traj, traj_prev, SCPP)
  return -X[7,k]-robot.γ_max
end

## Nonconvex state inequality constraints
function ncsi_obstacle_avoidance_constraints(traj, traj_prev::Trajectory, SCPP::SCPProblem{Plane{T}, Airplane, E}, k::Int, i::Int) where {T,E}
  X,U,Tf,Xp,Up,Tfp,dtp,robot,model,WS,x_init,x_goal,x_dim,u_dim,N,dh = @constraint_abbrev_airplane(traj, traj_prev, SCPP)

  rb_idx, env_idx = 1, i
  env_ = WS.btenvironment_keepout

  clearance = model.clearance

  r = X[1:3,k]
  dist,xbody,xobs = BulletCollision.distance(env_,rb_idx,r,env_idx)

  # See Eq. 12b in "Convex optimization for proximity maneuvering of a spacecraft with a robotic manipulator"
  return clearance - dist
end

## Nonconvex state inequality constraints (convexified)
function ncsi_obstacle_avoidance_constraints_convexified(traj, traj_prev::Trajectory, SCPP::SCPProblem{Plane{T}, Airplane, E}, k::Int, i::Int) where {T,E}
  X,U,Tf,Xp,Up,Tfp,dtp,robot,model,WS,x_init,x_goal,x_dim,u_dim,N,dh = @constraint_abbrev_airplane(traj, traj_prev, SCPP)

  rb_idx, env_idx = 1,i
  env_ = WS.btenvironment_keepout

  clearance = model.clearance

  r0 = get_workspace_location(traj_prev, SCPP, k, rb_idx)
  dist, xbody, xobs = BulletCollision.distance(env_, rb_idx, r0, env_idx)

  if dist < SCPP.param.obstacle_toggle_distance
    r = get_workspace_location(traj, SCPP, k, rb_idx)

    # See Eq. 12b in "Convex optimization for proximity maneuvering of a spacecraft with a robotic manipulator"
    nhat = dist > 0 ?
      (xbody-xobs)./norm(xbody-xobs) :
      (xobs-xbody)./norm(xobs-xbody)

    return clearance - (dist + nhat'*(r-r0))
  else
    return 0
  end
end

## State trust region inequality constraints
function stri_state_trust_region(traj, traj_prev::Trajectory, SCPP::SCPProblem{Plane{T}, Airplane, E}, k::Int, i::Int) where {T,E}
  X,U,Tf,Xp,Up,Tfp,dtp,robot,model,WS,x_init,x_goal,x_dim,u_dim,N,dh = @constraint_abbrev_airplane(traj, traj_prev, SCPP)

  return norm(X[:,k]-Xp[:,k])^2
end

## Trust region inequality constraints
function ctri_control_trust_region(traj, traj_prev::Trajectory, SCPP::SCPProblem{Plane{T}, Airplane, E}, k::Int, i::Int) where {T,E}
  X,U,Tf,Xp,Up,Tfp,dtp,robot,model,WS,x_init,x_goal,x_dim,u_dim,N,dh = @constraint_abbrev_airplane(traj, traj_prev, SCPP)
  return norm(U[:,k]-Up[:,k])
end

function get_workspace_location(traj, SCPP::SCPProblem{Plane{T}, Airplane, E}, k::Int, i::Int) where {T,E}
  return traj.X[1:3,k]
end

function SCPConstraints(SCPP::SCPProblem{Plane{T}, Airplane, E}) where {T,E}
	model = SCPP.PD.model
	x_dim, u_dim, N = model.x_dim, model.u_dim, SCPP.N
  WS = SCPP.WS

	SCPC = SCPConstraints()

	## Dynamics constraints
  for k = 1:N-1
    push!(SCPC.dynamics, (dynamics_constraints, k, 0))
  end

	## Convex state equality constraints
	# Init and goal
	for i = 1:x_dim
    push!(SCPC.convex_state_eq, (cse_init_constraints, 0, i))
  end

    # toggle hard equality or box constraints
  for i = 1:x_dim
    push!(SCPC.convex_state_eq, (cse_goal_constraints, 0, i))
  end

	## Convex state inequality constraints
  for i = 1:3
    # push!(SCPC.convex_state_ineq, (csi_goal_region_min_constraints, 0, i))
    # push!(SCPC.convex_state_ineq, (csi_goal_region_max_constraints, 0, i))
  end

	# State bounds
	for k = 1:N
		push!(SCPC.convex_state_ineq, (csi_v_max_constraints, k, 0))
		push!(SCPC.convex_state_ineq, (csi_v_min_constraints, k, 0))
		push!(SCPC.convex_state_ineq, (csi_γ_max_constraints, k, 0))
		push!(SCPC.convex_state_ineq, (csi_γ_min_constraints, k, 0))
		push!(SCPC.convex_state_ineq, (csi_ϕ_max_constraints, k, 0))
		push!(SCPC.convex_state_ineq, (csi_ϕ_min_constraints, k, 0))
	end

	## Nonconvex state equality constraints
  nothing

	## Nonconvex state inequality constraints
  env_ = WS.btenvironment_keepout
  for k = 1:N, i = 1:length(env_.convex_env_components)
    push!(SCPC.nonconvex_state_ineq, (ncsi_obstacle_avoidance_constraints, k, i))
  end

  ## Nonconvex state equality constraints (convexified)
  nothing

	## Nonconvex state inequality constraints (convexified)
  env_ = WS.btenvironment_keepout
  for k = 1:N, i = 1:length(env_.convex_robot_components)
    push!(SCPC.nonconvex_state_ineq, (ncsi_obstacle_avoidance_constraints_convexified, k, i))
  end

  ## Convex control equality constraints
  nothing

	## Convex control inequality constraints
	for k = 1:N-1, i = 1:u_dim
    push!(SCPC.convex_control_ineq, (cci_max_bound_constraints, k, i))
    push!(SCPC.convex_control_ineq, (cci_min_bound_constraints, k, i))
	end

	## State trust region ineqality constraints
  for k = 1:N
    push!(SCPC.state_trust_region_ineq, (stri_state_trust_region, k, 0))
  end

  ## Constrol trust region inequality constraints
  for k = 1:N-1
    push!(SCPC.control_trust_region_ineq, (ctri_control_trust_region, k, 0))
  end

	return SCPC
end

function trust_region_ratio_gusto(traj, traj_prev::Trajectory, SCPP::SCPProblem{Plane{T}, Airplane, E}) where {T,E}
  # Where i is the state index, and k is the timestep index
  X,U,Tf,Xp,Up,Tfp,dtp,robot,model,WS,x_init,x_goal,x_dim,u_dim,N,dh = @constraint_abbrev_airplane(traj, traj_prev, SCPP)
  fp, Ap = model.f, model.A
  num,den = 0, 0
  env_ = WS.btenvironment_keepout

  for k in 1:N-1
    linearized = fp[k] + Ap[k]*(X[:,k]-Xp[:,k])
    num += norm(f_dyn(X[:,k],U[:,k],robot,model) - linearized)
    den += norm(linearized)
  end

  clearance = model.clearance

  for k in 1:N
    r0,r = Xp[1:3,k], X[1:3,k]
    for (rb_idx,body_point) in enumerate(env_.convex_robot_components)
      for (env_idx,convex_env_component) in enumerate(env_.convex_env_components)
        dist,xbody,xobs = BulletCollision.distance(env_,rb_idx,r0,env_idx)
        nhat = dist > 0 ?
          (xbody-xobs)./norm(xbody-xobs) :
          (xobs-xbody)./norm(xobs-xbody)
        linearized = clearance - (dist + nhat'*(r-r0))

        dist,xbody,xobs = BulletCollision.distance(env_,rb_idx,r,env_idx)

        num += abs((clearance-dist) - linearized)
        den += abs(linearized)
      end
    end
  end
  return num/den
end

function trust_region_ratio_trajopt(traj, traj_prev::Trajectory, SCPP::SCPProblem{Plane{T}, Airplane, E}) where {T,E}
  # Where i is the state index, and k is the timestep index
  X,U,Tf,Xp,Up,Tfp,dtp,robot,model,WS,x_init,x_goal,x_dim,u_dim,N,dh = @constraint_abbrev_airplane(traj, traj_prev, SCPP)
  fp = model.f
  num, den = 0, 0
  env_ = WS.btenvironment_keepout
  dt = traj.dt

  for k in 1:N-1
    ϕ_old = norm(fp[k] - (Xp[:,k]-Xp[:,k])/dtp, 1)
    ϕ_new = norm(f_dyn(X[:,k],U[:,k],robot,model) - (X[:,k]-X[:,k])/dt, 1)
    ϕ_hat_new = norm(dynamics_constraints(traj,traj_prev,SCPP,k,0), 1)
    num += (ϕ_old-ϕ_new)
    den += (ϕ_old-ϕ_hat_new)
  end

  clearance = model.clearance

  for k in 1:N
    r0,r = Xp[1:3,k], X[1:3,k]
    for (rb_idx,body_point) in enumerate(env_.convex_robot_components)
      for (env_idx,convex_env_component) in enumerate(env_.convex_env_components)
        dist,xbody,xobs = BulletCollision.distance(env_,rb_idx,r0,env_idx)
        ϕ_old = clearance-dist

        dist,xbody,xobs = BulletCollision.distance(env_,rb_idx,r,env_idx)
        nhat = dist > 0 ?
          (xbody-xobs)./norm(xbody-xobs) :
          (xobs-xbody)./norm(xobs-xbody)
        ϕ_new = clearance-dist
        ϕ_hat_new = clearance - (dist + nhat'*(r-r0))

        num += (ϕ_old-ϕ_new)
        den += (ϕ_old-ϕ_hat_new)
      end
    end
  end

  return num/den
end

function trust_region_ratio_mao(traj, traj_prev::Trajectory, SCPP::SCPProblem{Plane{T}, Airplane, E}) where {T,E}
  # Where i is the state index, and k is the timestep index
  X,U,Tf,Xp,Up,Tfp,dtp,robot,model,WS,x_init,x_goal,x_dim,u_dim,N,dh = @constraint_abbrev_airplane(traj, traj_prev, SCPP)
  num,den = 0, 0

  cost_true_prev = cost_true(traj_prev, traj_prev, SCPP)
  cost_true_new = cost_true(traj, traj, SCPP)
  for k in 1:N-1
    cost_true_prev += norm(dynamics_constraints(traj_prev, traj_prev, SCPP, k, 0), Inf)
    cost_true_new += norm(dynamics_constraints(traj, traj, SCPP, k, 0), Inf)
  end
  cost_linearized_new = cost_true(traj, traj, SCPP)
  return (cost_true_prev-cost_true_new)/(cost_true_prev-cost_linearized_new)
end

##################
# Shooting Method
##################
function get_dual_cvx(prob::Convex.Problem, SCPP::SCPProblem{Plane{T}, Airplane, E}, solver) where {T,E}
  if solver == "Mosek"
    return -MathProgBase.getdual(prob.model)[1:SCPP.PD.model.x_dim]
  else
    return []
  end
end

function get_dual_jump(SCPS::SCPSolution, SCPP::SCPProblem{Plane{T}, Airplane, E}) where {T,E}
  @show -MathProgBase.getconstrduals(SCPS.solver_model.internalModel)[1:SCPP.PD.model.x_dim]
end

##################
# Dynamics
##################
function simEOM(traj, SCPP::SCPProblem{Plane{T}, Airplane, E}, dt_prop=0.01) where {T,E}
  X,U,Tf = traj.X, traj.U, traj.Tf
  N, dt = SCPP.N, traj.dt
  rb = SCPP.PD.robot

  x_dim = SCPP.PD.model.x_dim
  u_dim = SCPP.PD.model.u_dim

  Ufull = Matrix(u_dim,0)
  for k in 1:N-1
    Ufull = [Ufull repmat(U[:,k], 1, Int(round(dt/dt_prop)))]
  end

  x0 = X[:,1]
  Xs = repmat(x0,1,N)

  for k = 1:N-1
    U = Ufull[:,k]
    k1 = fAir(x0,U,rb)
    x2 = x0 + 0.5*dt_prop*k1
    k2 = fAir(x2,U,rb)
    x3 = x0 + 0.5*dt_prop*k2
    k3 = fAir(x3,U,rb)
    x4 = x0 + dt_prop*k3
    k4 = fAir(x4,U,rb)
    x0 = x0 + 1/6*dt_prop*(k1 + 2*k2 + 2*k3 + k4)
    Xs[:,k+1] = x0
  end
  return Xs
end


text:
using TimerOutputs
using GEMPIC
using ProgressMeter

const to = TimerOutput()

function run( steps )

    β = 0.0001
    k = 1.25
    α = 0.0
    σ = [0.2,  0.005773502691896]
    μ = [0.0, 0.0]

    nx   = 32
    xmin = 0.0
    xmax = 2π / k
    n_particles    = 100000
    sampling_case  = :sobol
    symmetric      = true
    splitting_case = :symplectic
    spline_degree  = 3

    mesh   = Mesh( xmin, xmax, nx)

    beta_sin_k(x) = beta * sin(2π * x / (xmax - xmin))

    df = CosSumGaussian{1,2}([[k]], [α], [σ], [μ] )

     # Initialize the particles   (mass and charge set to 1.0 with one weight)
    particle_group = ParticleGroup{1,2}( n_particles)

    sampler = ParticleSampler{1,2}( sampling_case, symmetric, n_particles)

    sample!(  particle_group, sampler, df, mesh)

    kernel_smoother1 = ParticleMeshCoupling( mesh, n_particles, spline_degree-1, :galerkin)
    kernel_smoother0 = ParticleMeshCoupling( mesh, n_particles, spline_degree, :galerkin)
    rho = zeros(Float64, nx);
    efield_poisson = zeros(Float64, nx)
    # Initialize the field solver
    maxwell_solver = Maxwell1DFEM(mesh, spline_degree)
    # efield by Poisson
    solve_poisson!( efield_poisson, particle_group, kernel_smoother0, maxwell_solver, rho )

    # Initialize the arrays for the spline coefficients of the fields
    efield_dofs = [copy(efield_poisson), zeros(Float64, nx)]
    bfield_dofs = zeros(Float64, nx)

    propagator = HamiltonianSplitting( maxwell_solver,
                                       kernel_smoother0,
                                       kernel_smoother1,
                                       particle_group,
                                       efield_dofs,
                                       bfield_dofs)

    efield_dofs_n = propagator.e_dofs

    beta_cos_k(x) = β * cos(2π * x / (xmax - xmin))
    l2projection!( bfield_dofs, maxwell_solver, beta_cos_k, spline_degree-1)

    thdiag = TimeHistoryDiagnostics( particle_group, maxwell_solver,
                            kernel_smoother0, kernel_smoother1 );

    Δt = 0.01

    @showprogress 1 for j = 1:steps # loop over time

        # Strang splitting
        @timeit to "splitting"  begin

            @timeit to "HB"  GEMPIC.operatorHB(   propagator, 0.5Δt)
            @timeit to "HE"  GEMPIC.operatorHE(   propagator, 0.5Δt)
            @timeit to "Hp2" GEMPIC.operatorHp2(  propagator, 0.5Δt)
            @timeit to "Hp1" GEMPIC.operatorHp1(  propagator, 1.0Δt)
            @timeit to "Hp2" GEMPIC.operatorHp2(  propagator, 0.5Δt)
            @timeit to "HE"  GEMPIC.operatorHE(   propagator, 0.5Δt)
            @timeit to "HB"  GEMPIC.operatorHB(   propagator, 0.5Δt)

        end

        # Diagnostics
        @timeit to "poisson" solve_poisson!( efield_poisson, particle_group,
                        kernel_smoother0, maxwell_solver, rho)

        @timeit to "diagnostic" write_step!(thdiag, j * Δt, spline_degree,
                        efield_dofs,  bfield_dofs,
                        efield_dofs_n, efield_poisson)

    end

end

run(1000)

println(to)


text:
using Documenter, EEG

makedocs()

deploydocs(
    repo = "github.com/codles/EEG.jl.git",
    julia = "0.5",
)


text:
# 1. India level timeseries for Confirmed, Recovered and Deceased cases
function get_India_timeseries()
    df = read_CSV_url(case_time_series_csv_url)
    return df
end