text:
# Syntax Parser
# =============
module Parsing
export parse_traitsfunction
import WhereTraits
using WhereTraits.Utils
using SimpleMatch
using ExprParsers
include("NormalizeType.jl")
using .NormalizeType

parse_traitsfunction(env, expr::Expr, expr_original; kwargs...) = parse_traitsfunction(env, EP.Function()(expr), expr_original; kwargs...)
function parse_traitsfunction(env, func_parsed::EP.Function_Parsed, expr_original; on_traits_dropped = msg -> throw(ArgumentError(msg)))
  # Parse main syntax
  # =================
  args_names = [parse_expr(EP.Arg(), arg).name for arg in func_parsed.args]
  args_names_matcher = EP.AnyOf(args_names...)
  # TODO we could also allow dispatch on keyword arguments
  # kwargs_names = [EP.Arg()(kwarg).name for kwarg in func_parsed.kwargs]
  # kwargs_names_matcher = EP.AnyOf(kwargs_names...)

  whereexpr_parser = EP.AnyOf(
    # we allow dispatch on arguments
    EP.Named{:arg}(args_names_matcher), # interpreted as bool
    EP.Named{:arg}(EP.TypeAnnotation(name=args_names_matcher)),
    EP.Named{:arg}(EP.TypeRange(name=args_names_matcher)),

    # TODO we could also allow dispatch on keyword arguments
    # How? we store all kwargs per signature and then define traitfunctions like `func(kwargs[:b])`
    # EP.Named{:kwarg}(kwargs_names_matcher), # interpreted as bool
    # EP.Named{:kwarg}(EP.TypeAnnotation(name=kwargs_names_matcher)),
    # EP.Named{:kwarg}(EP.TypeRange(name=kwargs_names_matcher)),

    # all other symbols refer to standard TypeVariables
    # TypeAnnotation on TypeVar make only sense in the special case that the typevariable can take different types of binary type
    # both Symbol and TypeRange are normal where syntax, TypeAnnotation is not
    EP.Named{:normal}(EP.anysymbol),
    EP.Named{:typevar}(EP.TypeAnnotation(name=EP.anysymbol)),
    EP.Named{:normal}(EP.TypeRange(name=EP.anysymbol)),

    # we further allow to dispatch on functions from either args, kwargs, or standard typevars
    # which should be all the rest
    EP.Named{:func}(EP.Call()),  # interpreted as bool
    EP.Named{:func}(EP.TypeAnnotation()),
    EP.Named{:func}(EP.TypeRange()),
  )
  parsed_wheres = [parse_expr(whereexpr_parser, w) for w in func_parsed.wheres]
  normal_wheres, extra_wheres = filtersplit(parsed_wheres) do x
    x isa EP.Named{:normal}
  end

  # prepare extra wheres
  # ====================

  traits = map(extra_wheres) do expr
    @match(expr) do f
      f(x::EP.Named{:arg, Symbol}) = :(Val{$(to_expr(x.value))}())  # plain arguments are interpreted as bool
      # plain calls are assumed to refer to boolean expressions
      function f(x::EP.Named{:func, EP.Call_Parsed})
        if x.value.name == :!
          # if the last call is negation "!" we take the thing which is negated
          # as a trait function and dispatch on False
          :(Val{$(to_expr(x.value.args[1]))}())
        else
          :(Val{$(to_expr(x.value))}())
        end
      end
      f(x::EP.Named{<:Any, EP.TypeAnnotation_Parsed}) = to_expr(x.value.name)
      f(x::EP.Named{<:Any, EP.TypeRange_Parsed}) = to_expr(x.value.name)
    end
  end

  traits_matching_types = map(enumerate(extra_wheres)) do (i, expr)
    @match(expr) do f
      f(x::EP.Named{:arg, Symbol}) = :(Val{true})  # plain arguments are interpreted as bool
      f(x::EP.Named{:func, EP.Call_Parsed}) = (x.value.name == :!) ? :(Val{false}) : :(Val{true})  # plain calls are assumed to refer to boolean expressions
      f(x::EP.Named{<:Any, EP.TypeAnnotation_Parsed}) = to_expr(x.value.type)
      function f(x::EP.Named{<:Any, EP.TypeRange_Parsed})
        tr = x.value
        @assert !(tr.lb === Union{} && tr.ub == Any) "should have at least an upperbound or a lowerbound"
        if tr.lb === Union{}  # only upperbound
          :(Type{<:$(to_expr(tr.ub))})
        elseif tr.ub === Any  # only LowerBound
          :(Type{>:$(to_expr(tr.lb))})
        else  # both
          :(Type{T} where {$(to_expr(tr.lb)) <: T <: $(to_expr(tr.up))})
        end
      end
    end
  end

  # normalize first function dispatch level
  # =======================================

  outerfunc_parsed = EP.Function_Parsed(
    name = func_parsed.name,
    curlies = func_parsed.curlies,
    args = func_parsed.args,
    kwargs = [],
    # we need to use to_expr here, as normalize_func expects a
    # standard Function_Parsed, where wheres is a list of Expr
    wheres = to_expr(normal_wheres),
    body = :nothing
  )

  outerfunc_fixed, innerfunc_fixed = normalize_func(env, outerfunc_parsed)

  @assert(
    isempty(intersect(keys(innerfunc_fixed.args_mapping), keys(innerfunc_fixed.typevars_mapping))),
    "args and typevariables must not overlap!
    Found $(intersect(keys(innerfunc_fixed.args_mapping), keys(innerfunc_fixed.typevars_mapping))) in both.")

  typevars_old = map(normal_wheres) do parsed
    @match(parsed) do f
      f(x::EP.Named{:normal, Symbol}) = x.value
      f(x::EP.Named{:normal, EP.TypeRange_Parsed}) = x.value.name
    end
  end
  kept_typevars_old = values(innerfunc_fixed.typevars_mapping)
  dropped_typevars_old = [v for v in typevars_old if v ∉ kept_typevars_old]

  traits_filtered = filter(t -> !depends_on(t, dropped_typevars_old), traits)
  if length(traits) != length(traits_filtered)
    traits_dropped = setdiff(traits, traits_filtered)
    on_traits_dropped("Given traits depend on droppable typeparameters ($dropped_typevars_old). WhereTraits: $traits_dropped")
  end

  old_to_new = Dict(v => k for (k, v) in merge(innerfunc_fixed.args_mapping, innerfunc_fixed.typevars_mapping))
  # TODO we currently do not normalize the traits function names
  # TODO e.g. using both `Base.IteratorSize(a)` and `IteratorSize(a)` result in two different traits currently
  traits_normalized = _change_symbols(traits_filtered, old_to_new)
  # we map the traitsdefinition to a signature including a name for easier debugging and the correct type
  traits_mapping = Dict(k => Expr(:(::), Symbol("'", k, "'"), v)
                        for (k, v) in zip(traits_normalized, traits_matching_types))
  # we may encounter no traits at all, namely in the case where a default clause is defined
  innerargs_traits = isempty(traits_normalized) ? [] : sortexpr(unique(traits_normalized))
  outerfunc′ = WhereTraits.InternalState.DefOuterFunc(
    outerfunc_fixed,
    WhereTraits.InternalState.DefOuterFuncNonFixedPart(innerargs_traits),
  )
  innerfunc′ = WhereTraits.InternalState.DefInnerFunc(
    WhereTraits.InternalState.DefInnerFuncFixedPart(
      args_mapping = innerfunc_fixed.args_mapping,
      typevars_mapping = innerfunc_fixed.typevars_mapping,
      traits_mapping = traits_mapping,
    ),
    WhereTraits.InternalState.DefInnerFuncNonFixedPart(
      kwargs = func_parsed.kwargs,
      body = func_parsed.body,
      expr_original = expr_original,
    ),
  )
  outerfunc′, innerfunc′
end

_change_symbols(vec::Vector, symbol_mapping) = [_change_symbols(x, symbol_mapping) for x in vec]
_change_symbols(sym::Symbol, symbol_mapping) = get(symbol_mapping, sym, sym)
_change_symbols(any, symbol_mapping) = any
_change_symbols(qn::QuoteNode, symbol_mapping) = QuoteNode(_change_symbols(qn.value, symbol_mapping))
_change_symbols(expr::Expr, symbol_mapping) = Expr(expr.head, _change_symbols(expr.args, symbol_mapping)...)

# As part of the parsing, we need to normalization the outer function
# -------------------------------------------------------------------

struct _BetweenCurliesAndArgs end

normalize_func(env::MacroEnv, func_expr::Expr) = normalize_func(env, EP.Function()(func_expr))
function normalize_func(env::MacroEnv, func_parsed::EP.Function_Parsed)
  args_parsed = [parse_expr(EP.Arg(), a) for a in func_parsed.args]
  @assert all(arg -> arg.default isa EP.NoDefault, args_parsed) "Can only normalize functions without positional default arguments"

  # normalize types
  # ---------------
  # we add _BetweenCurliesAndArgs to reuse `type` as identifying signature without mixing curly types and arg types
  typeexpr = Expr(:curly, Tuple, func_parsed.curlies...,
    _BetweenCurliesAndArgs, (to_expr(a.type) for a in args_parsed)...)
  typeexpr_full = :($typeexpr where {$(to_expr(func_parsed.wheres)...)})
  type = Base.eval(env.mod, typeexpr_full)
  typebase, typevars_old = split_typevar(type)  # typebase == Tuple{P1, P2, P3, ...}
  typeexprs_new, typevars_new, typevar_new_to_old = normalize_typevars(typebase.parameters, typevars_old)

  curlies_typeexprs_new = typeexprs_new[1:length(func_parsed.curlies)]
  args_typeexprs_new = typeexprs_new[length(func_parsed.curlies)+2:end]  # + 2 because of _BetweenCurliesAndArgs

  # normalize args
  # --------------
  arg_new_to_old = Dict{Symbol, Symbol}()
  countfrom_args = Base.Iterators.Stateful(Base.Iterators.countfrom(1))
  for (a, type) in zip(args_parsed, args_typeexprs_new)
    a.type = type
    arg_position = next!(countfrom_args)
    new = normalized_arg_by_position(arg_position)
    if !isnothing(a.name)  # might be an arg without name
      arg_new_to_old[new] = a.name
    end
    a.name = new
  end

  # return
  # ------

  # we return the whole signature to easily decide whether the function overwrites another
  # TODO we use call by position
  # TODO because call by keyword leads to UnreachableReached, see https://github.com/JuliaLang/julia/issues/35698
  outerfunc_fixed = WhereTraits.InternalState.DefOuterFuncFixedPart{type}(
    # everything under fixed should be identifiable via signature
    type,
    func_parsed.name,
    curlies_typeexprs_new,
    args_parsed,
    typevars_new,
    # body information
    sort([a.name for a in args_parsed]),
    sort([tv.name for tv in typevars_new]),
  )
  # outerfunc_fixed = WhereTraits.InternalState.DefOuterFuncFixedPart{type}(
  #   # everything under fixed should be identifiable via signature
  #   signature = type,
  #   name = func_parsed.name,
  #   curlies = curlies_typeexprs_new,
  #   args = args_parsed,
  #   wheres = typevars_new,
  #   # body information
  #   innerargs_args = sort([a.name for a in args_parsed]),
  #   innerargs_typevars = sort([tv.name for tv in typevars_new]),
  # )
  # this innerfunc_fixed is only an intermediate state, hence we fallback to using plain namedtuples
  innerfunc_fixed = (
    args_mapping = arg_new_to_old,
    typevars_mapping = typevar_new_to_old,
  )
  outerfunc_fixed, innerfunc_fixed
end

normalized_arg_by_position(position::Int) = Symbol("a", position)

end # module


text:
# Function to generate a plot from the district results
function plot_generation(districts, hexshape)
    plot_area = plot(axis=false, ticks=false)
    if length(unique(districts.location)) > 1
        districts.shape = hexshape.geometry
        normalized_values_area = equalize(districts.location)
        colors_area = Array([cgrad(:Pastel1_9, length(unique(districts.location)), categorical = true)[value] for value in normalized_values_area])
        for x = 1:nrow(districts)
            if districts[x, :location] == districts[x, :index]
                plot!(plot_area, districts[x, :shape], color=RGB(0/255,0/255,0/255))
            elseif districts[x, :location] > 0
                plot!(plot_area, districts[x, :shape], color=colors_area[x])
            else
                plot!(plot_area, districts[x, :shape], color=nothing)
            end
        end
    end
    return plot_area
end

ext: jl


text:
export fit_statespace_dp

# Piecewise constant segmentation ==============================================
@inline function argmin_const(y,w,t1,t2)
    # y'w/sum(w) weighted mean of y
    s = 0.
    dp = 0.
    for t = t1:t2
        dp += y[t]*w[t]
        s += w[t]
    end
    dp/s
end
@inline function cost_const(y,w,t1,t2,argminfun)
    # w'(y-dpamin)^2 weigted variance of y
    dp = 0.
    dpm = argminfun(y,w,t1,t2)
    for t = t1:t2
        dp += w[t]*(y[t]-dpm)^2
    end
    dp
end

# Piecewise linear segmentation ============================================
@inline function argmin_lin(input,w,t1,t2)
    y   = input[1]
    A   = input[2]
    yminus = A[t1:t2,1] .- y[t1]
    yreg = y[t1:t2] .- yminus
    ones(t2-t1+1,1)\yreg # TODO: does not take care of w yet
end

# inuti costfun måste hela y användas, dvs simulation i stället för prediction. Annars hamnar alla knutpunkter på kurvan.

@inline function cost_lin(input,w,t1,t2,argminfun)
    if t2 == t1
        return Inf
    end
    y   = input[1]
    A   = input[2]
    n   = size(y,1) ÷ length(w)
    k   = argminfun(input,w,t1,t2)
    yminus = A[t1:t2,1] .- y[t1]

    e   = y[t1:t2] .- yminus .- k
    return e⋅e
end

# Piecewise statespace segmentation ============================================
@inline function argmin_ss(input,w,t1,t2)
    y   = input[1]
    A   = input[2]
    n   = size(y,1) ÷ length(w)
    ii  = (t1-1)*n+1
    ii2 = t2*n-1
    A[ii:ii2,:]\y[ii:ii2] # TODO: does not take care of w yet
end
@inline function cost_ss(input,w,t1,t2,argminfun)
    y   = input[1]
    A   = input[2]
    n   = size(y,1) ÷ length(w)
    ii  = (t1-1)*n+1
    ii2 = t2*n-1
    k   = argminfun(input,w,t1,t2)
    e   = y[ii:ii2]-A[ii:ii2,:]*k
    return e⋅e
end

using Base.Threads
function seg_bellman(y,M,w, costfun=cost_const, argminfun=argmin_const; doplot=false)
    doplot = @static isinteractive() && doplot
    n = length(w)
    @assert (M < n) "M must be smaller than the length of the data sequence"
    B = zeros(Int,M-1,n) # back-pointer matrix

    # initialize Bellman iteration
    fi = zeros(n)
    for jj = M:n
        fi[jj] = costfun(y,w,jj,n,argminfun)
    end
    doplot && plot(fi, lab="fi init")
    memorymat = fill(Inf,n,n)

    # iterate Bellman iteration
    fnext = Vector{Float64}(undef,n)
    for jj = M-1:-1:1
        for kk = jj:n-(M-jj)
            opt   = Inf
            optll = 0
            for ll = kk+1:n-(M-jj-1)
                if memorymat[kk,ll-1] == Inf
                    memorymat[kk,ll-1] = costfun(y,w,kk,ll-1,argminfun)
                end
                cost = memorymat[kk,ll-1] + fi[ll]
                if cost < opt
                    opt   = cost
                    optll = ll
                end
            end
            fnext[kk] = opt
            B[jj,kk]  = optll-1
        end
        fi .= fnext
        fnext = Vector{Float64}(undef,n)
        doplot && plot!(fi, lab="fi at $jj")
    end

    # last Bellman iterate
    V = [(memorymat[1,jj] == Inf ? costfun(y,w,1,jj,argminfun) : memorymat[1,jj])+fi[jj+1] for jj = 1:n-M]
    if isempty(V) || length(V) <= M
        error("Failed to perform segmentation, try lowering M")
    end
    doplot && plot!(V, lab="V")

    # backward pass
    t = Vector{Int}(undef,M);
    a = Vector{typeof(argminfun(y,w,1,2))}(undef,M+1);
    _,t[1] = findmin(V[1:end-M])
    a[1] = argminfun(y,w,1,t[1])

    for jj = 2:M
        t[jj] = B[jj-1,t[jj-1]]
        a[jj] = argminfun(y,w,t[jj-1]+1,t[jj])
    end
    a[M+1] = argminfun(y,w,t[M]+1,n)

    V,t,a
end

"""
model = fit_statespace_dp(x,u, M; extend=true)

Fit model using dynamic programming. `M` is the number of changepoints.
´x` and `u` should have time in the second dimension.

See Bagge Carlson et al. 2018 section IID
Usage example in function `benchmark_ss`
"""
function fit_statespace_dp(x,u, M; extend=true, kwargs...)
    n,T = size(x)
    m = size(u,1)
    d = iddata(x[:,2:end],u, x[:,1:end-1])
    input = matrices(SimpleLTVModel(d), d)
    @show size.(input)
    # input = matrices(x,u)
    V,bps,a = seg_bellman(input,M, ones(T-1), cost_ss, argmin_ss, doplot=false)
    At,Bt = segments2full(a,bps,n,m,T)

    SimpleLTVModel(At, Bt,extend)
end

# Tests
# Dynamic programming ==========================================================
function benchmark_const(N, M=1, doplot=false)
    doplot = @static isinteractive() && doplot
    n = 3N
    y = [0.1randn(N); 10 .+ 0.1randn(N); 20 .+ 0.1randn(N).+range(1, stop=10, length=N)]
    V,t,a = @time seg_bellman(y,M, ones(length(y)))
    if doplot
        tplot = [1;t;n];
        aplot = [a;a[end]];
        plot(y)
        plot!(tplot, aplot, l=:step);gui()
    end
    # yhat, x, a, b
end

function benchmark_lin(T_, M, doplot=false)
    doplot = @static isinteractive() && doplot

    # M        = 1
    # T_       = 400
    x = sin.(range(0, stop=2π, length=T_))
    u = ones(1,T_)
    d = iddata(x',u, x')
    input = matrices(SimpleLTVModel(d), d)
    @time V,t,a = seg_bellman(input,M, ones(T_-1), cost_lin, argmin_lin, doplot=false)
    # if doplot
    #     k = reduce(hcat, a)'
    #     @show size(a), size(k)
    #     At = reshape(k[:,1:n^2]',n,n,M+1)
    #     At = permutedims(At, [2,1,3])
    #     tplot = [1;t;T_]
    #     plot()
    #     for i = 1:M+1
    #         plot!(tplot[i:i+1],flatten(At)[i,:]'.*ones(2), c=[:blue :green :red :magenta], xlabel="Time index", ylabel="Model coefficients")
    #     end
    #     plot!([1,T_÷2-1], [0.95 0.1; 0 0.95][:]'.*ones(2), ylims=(-0.1,1), l=(:dash,:black, 1))
    #     plot!([T_÷2,T_], [0.5 0.05; 0 0.5][:]'.*ones(2), l=(:dash,:black, 1), grid=false)
    #     gui()
    # end
    V,t,a
end

function benchmark_ss(T_, M, doplot=false)
    doplot = @static isinteractive() && doplot

    # M        = 1
    # T_       = 400
    x,xm,u,n,m = testdata(T_)
    dn = iddata(xm,u,xm)
    input = matrices(SimpleLTVModel(dn), dn)

    @time V,t,a = seg_bellman(input,M, ones(T_-1), cost_ss, argmin_ss, doplot=false)
    if doplot
        k = reduce(hcat, a)'
        At = reshape(k[:,1:n^2]',n,n,M+1)
        Bt = reshape(k[:,n^2+1:end]',m,n,M+1)
        At = permutedims(At, [2,1,3])
        Bt = permutedims(Bt, [2,1,3])
        tplot = [1;t;T_]
        plot()
        for i = 1:M+1
            plot!(tplot[i:i+1],flatten(At)[i,:]'.*ones(2), c=[:blue :green :red :magenta], xlabel="Time index", ylabel="Model coefficients")
        end
        plot!([1,T_÷2-1], [0.95 0.1; 0 0.95][:]'.*ones(2), ylims=(-0.1,1), l=(:dash,:black, 1))
        plot!([T_÷2,T_], [0.5 0.05; 0 0.5][:]'.*ones(2), l=(:dash,:black, 1), grid=false)
        gui()
    end
    V,t,a
end
# T_vec = round(Int,logspace(2, 4,10))
# times = map(T_vec) do T
#     println(T)
#     benchmark_ss(T, 1)
# end
# plot(T_vec, times)
#
# M_vec = [2,4,8,16,32]
# times = map(M_vec) do M
#     println(M)
#     @elapsed benchmark_ss(110, M)
# end
# plot(M_vec, times)


text:
function num_of_param(x; seen=IdSet())
    n = 0
    fields, _ = functor(x)
    for f in fields
        n += num_of_param(f, seen=seen)
    end
    return n
end
num_of_param(x::Number; seen=IdSet()) = (push!(seen, x); 1)
function num_of_param(x::AbstractArray{<:Number}; seen=IdSet())
    x in seen && return 0
    n = length(x)
    push!(seen, x)
    return n
end


text: struct nonfixedIncome :> Income end

max_stars_repo_name: adamwillisXanax/accountingJulia

max_issues_repo_name: adamwillisXanax/accountingJulia

max_forks_repo_name: adamwillisXanax/accountingJulia


text:
module Chunk

using SimpleHttpIO

export IOChunked,
       done,
       size,
       data,
       block,
       iterator,
       ChunkIterator

const N = SimpleHttpIO.N
const N_TYPE = SimpleHttpIO.N_TYPE

type IOChunked <: AbstractIOSocket
    io::IO
    env::Associative
end

IOChunked(io::IO; kwargs...) = IOChunked(io, Dict{Any, Any}(kwargs))

write(io::IOChunked, data) = (isempty(data) ? 0 : writeln(io, hex(sizeof(data))) + writeln(io, data))
done(io::IOChunked) = writeln(io, hex(0)) + writeln(io)

type ChunkException <: Exception
    info
end

function size(io::IOChunked, loop=true)
    # read first char
    byte = readbyte(io)

    if byte == '\r'
        byte = readbyte(io)
        byte == '\n' || throw(ChunkException([:keys => ("parse_error", "unexpected_byte"),
                              :msg => "Expected \\n byte after \\r.", :byte => byte]))
        N
    else
        d = UTF8String(append!(BYTE_TYPE[byte], readlinebare(io)))
        # parse hex data
        s = parseint(d, 16)

        if loop
            while s == 0
                # read next block
                s = size(io, false)
            end
        end

        s
    end
end

function data(io::IOChunked, size::Integer)
    d = readbytes(io, size)
    # read CRLF
    readbytes(io, 2)
    # return data
    d
end

function block(io::IOChunked)
    s = size(io)
    if s != N
        data(io, s)
    end
    N
end

type ChunkIterator
    io::IOChunked
end

const iterator = ChunkIterator

Base.start(r::ChunkIterator) = size(r.io)
Base.done(r::ChunkIterator, state) = state == N
Base.next(r::ChunkIterator, state) = begin
    d = data(r.io, state)
    new_state = size(r.io)
    d, new_state
end

end


text:
# Visualize with Director
addprocs(2)

using TransformUtils
using KernelDensityEstimate
# using IncrementalInference
using RoME
using Caesar
using GeometryTypes
using DrakeVisualizer
using ColorTypes: RGBA
# using MeshIO, FileIO
using CoordinateTransformations
using Rotations
using Distributions

# # start visualizer and models
# vc = startdefaultvisualization(draworigin=false)
# sleep(2.0)
# rovt(vc)
# am = Translation(0,0,0) ∘ LinearMap(Rotations.AngleAxis(pi/2,0,0,1.0))
# rovt(vc, am )
#
# # and some default object in the world
# boxdata = GeometryData(HyperRectangle(Vec(0.0,4.0,-0.7), Vec(5.0,5.0,1.4)))
# boxdata.color = RGBA(0.5,0.1,0.0,0.5)
# model = Visualizer(boxdata,100)

# start visualizer with some scene
vc = startdefaultvisualization(draworigin=false)
sleep(3.0)
# defaultscene01!(vc)
sc01 = loadmodel(:scene01)
sc01(vc)

# load necessary data
rovt = loadmodel(:rov)
rovt(vc)

am = Translation(0,0,0.0) ∘ LinearMap(Rotations.AngleAxis(pi/2,0,0,1.0))
rovt(vc, am )

l1p = [0.0; 4.0; 0.7]
featuredata = GeometryData(HyperSphere(Point(l1p...), 0.10) )
featuredata.color = RGBA(0.5, 1.0, 0.0, 0.7)
model = Visualizer(featuredata,101)

# l1p = [0.0; 4.0; 0.7]
# featuredata = GeometryData(HyperSphere(Point(l1p...), 0.1) )
# featuredata.color = RGBA(0.0, 0.0, 1.0, 0.6)
# model = Visualizer(featuredata,102)

# @async begin
#   for t in linspace(0,1,50)
#     x = -t*0.7
#     phi = pi/4.0*t
#     am = Translation(x,0,0) ∘ LinearMap(Rotations.AngleAxis(pi/2,0,0,1.0)) ∘ LinearMap( Rotations.AngleAxis(phi,1.0,0,0))
#     rovt(vc, am )
#     sleep(0.05)
#   end
# end

# true position
gt = Dict{Symbol, Vector{Float64}}()
gt[:l1] = [4.0;0.0;-0.7]

# okay build the graph
fg = initfg()
N = 200

initCov = 0.01*eye(6)
[initCov[i,i] = 0.001 for i in 4:6];
odoCov = deepcopy(initCov)

println("Adding PriorPose3 and :x1 to graph...")

initp = SE3(zeros(3),TransformUtils.AngleAxis(pi/2,[0;0;1.0]))
initPosePrior = PriorPose3( MVNormal(veeEuler(initp), initCov) )
X= getSample(initPosePrior,N)[1]
v0 = addVariable!(fg, :x1,  X,  N=N)
f1  = addFactor!(fg,[v0], initPosePrior)

rangecov, bearingcov=3e-3, 3e-3
println("Adding :l1 LinearRangeBearingElevation to graph...")
addLinearArrayConstraint(fg, (4.0, 0.0), :x1, :l1, rangecov=rangecov,bearingcov=bearingcov)
visualizeDensityMesh!(vc, fg, :l1, meshid=2)

# @async begin
for t in linspace(0,1,50)
  x = -t*0.7
  phi = pi/4.0*t
  am = Translation(x,0,0) ∘ LinearMap(Rotations.AngleAxis(pi/2,0,0,1.0)) ∘ LinearMap( Rotations.AngleAxis(phi,1.0,0,0))
  rovt(vc, am )
  sleep(0.02)
end
# end

println("Adding :x2 pose and to graph...")
tf = [0.0;0.7;0.0;pi/4;0.0;0.0]
odo = Pose3Pose3(MvNormal(tf, odoCov))
addOdoFG!(fg, odo , N=N)

visualizeallposes!(vc, fg, drawlandms=false)
addLinearArrayConstraint(fg, (4.0, 0.0), :x2, :l2, rangecov=rangecov,bearingcov=bearingcov)

visualizeDensityMesh!(vc, fg, :l2, meshid=2)
sleep(5.0)

# and a third seen from both poses
addLinearArrayConstraint(fg, (4.0, 0.0), :x1, :l3, rangecov=rangecov,bearingcov=bearingcov)
addLinearArrayConstraint(fg, (4.0, 0.0), :x2, :l3, rangecov=rangecov,bearingcov=bearingcov)

# visualizeDensityMesh!(vc, fg, :l2, meshid=3)

solveTree!(fg)
visualize(fg, vc, drawlandms=false, densitymeshes=[:l3;:l2], N=N)
visualizeDensityMesh!(vc, fg, :x2, meshid=3)

#


text:
module Pendulum3D_3

println("... Pendulum3D_3 - Simple pendulum modeled with multibody and equation-based components")

using Modia

# Modia equation-based models
include("$(Modia.modelsPath)/AllModels.jl")

Pendulum = Model3D(
    world = Object3D(feature=Scene(animationFile="Pendulum3.json")),
    obj1  = Object3D(feature=Solid(shape=Box(lengthX=1.0, lengthY=0.2, lengthZ=0.2),
                solidMaterial="Steel", visualMaterial=VisualMaterial(color="Blue"))),
    obj2  = Object3D(parent=:obj1, feature=Visual(shape=Cylinder(diameter=0.1, length=0.21),
                visualMaterial=VisualMaterial(color="Red")), translation=[-0.5, 0.0, 0.0]),
    rev   = RevoluteWithFlange(obj1=:world, obj2=:obj2),

    damper  = Damper | Map(d=100.0),
    fixed   = Fixed,
    connect = :[(damper.flange_b, rev.flange),
                (damper.flange_a, fixed.flange)]
)

pendulum = @instantiateModel(Pendulum, unitless=true)
simulate!(pendulum, stopTime=3.0)

@usingModiaPlot
plot(pendulum, "rev.phi")
end


text:
using Resonance
omni, etoh, tps, complete_brain, metabolites, species = startup()

##

using CairoMakie
using AlgebraOfGraphics
using Microbiome.Distances
using Microbiome.MultivariateStats

@chain DataFrame(subject=get(species, :subject), timepoint = get(species, :timepoint), age = get(species, :ageMonths)) begin
    groupby(:subject)
    combine(:age => (a-> all(ismissing, a) ? 0 : maximum(skipmissing(a))) => :maxage)
    subset(:maxage => ByRow(a-> a > 24))
end

met_pcoa = pcoa(metabolites)
spec_pcoa = pcoa(species)

kidsidx = get(species, :Mother_Child) .== "C"
kidsspecies = species[:, kidsidx]
kidsspecies = kidsspecies[vec(prevalence(kidsspecies) .> 0), :]
ginisimpson!(kidsspecies)
shannon!(kidsspecies)

kids_pcoa = pcoa(kidsspecies)

using Microbiome.Distances
using MultivariateStats

brain_dm = pairwise(Euclidean(), Matrix(tps[complete_brain, Resonance.brainmeta]), dims=1)
brain_pcoa = fit(MDS, brain_dm, distances=true)

##
function getcolors(vals, clip=(0.1, 30); highclip=Makie.to_color(:white), lowclip=Makie.to_color(:black), colormap=:plasma)
    kidscolor = Makie.to_colormap(colormap)

    map(vals) do val
        if ismissing(val)
            return Makie.to_color(:gray)
        elseif val < clip[1]
            return lowclip
        elseif val > clip[2]
            return highclip
        else
            Makie.interpolated_getindex(kidscolor, val, clip)
        end
    end
end

##

fig = Figure(resolution=(800, 800))
ax1 = Axis(fig[1,1], xlabel="MDS1 ($(round(varexplained(met_pcoa)[1] * 100, digits=2))%)",
                     ylabel="MDS2 ($(round(varexplained(met_pcoa)[2] * 100, digits=2))%)")

scatter!(ax1, Resonance.loadings(met_pcoa)[:,1], Resonance.loadings(met_pcoa)[:,2],
        color=[ismissing(x) ? :grey : x == "M" ? :dodgerblue : :orange for x in get(metabolites, :Mother_Child)])

leg = Legend(fig[1,2], [MarkerElement(color=:orange, marker=:circle),
                        MarkerElement(color=:dodgerblue, marker=:circle)],
                        ["Kids", "Moms"])

ax2 = Axis(fig[2,1],
            xlabel="MDS1 ($(round(varexplained(met_pcoa)[1] * 100, digits=2))%)",
            ylabel="MDS2 ($(round(varexplained(met_pcoa)[2] * 100, digits=2))%)"
)

scatter!(ax2, Resonance.loadings(met_pcoa)[:,1], Resonance.loadings(met_pcoa)[:,2],
         color = getcolors(get(metabolites, :ageMonths), (0,25)))
cleg = Colorbar(fig[2, 2], limits=(0.1, 25), colormap=:plasma, highclip=:white, lowclip=:black, label="Age in Months")
save("figures/metabolites_pcoa.png", fig)

fig

##

fig = Figure(resolution=(800, 800))
ax1 = Axis(fig[1,1], xlabel="MDS1 ($(round(varexplained(spec_pcoa)[1] * 100, digits=2))%)",
                     ylabel="MDS2 ($(round(varexplained(spec_pcoa)[2] * 100, digits=2))%)")
scatter!(ax1, Resonance.loadings(spec_pcoa)[:,1], Resonance.loadings(spec_pcoa)[:,2],
        color=[ismissing(x) ? :grey : x == "M" ? :dodgerblue : :orange for x in get(species, :Mother_Child)],
        strokewidth=0.5
)

leg = Legend(fig[1,2], [MarkerElement(color=:orange, marker=:circle, strokewidth=0.5),
                        MarkerElement(color=:dodgerblue, marker=:circle, strokewidth=0.5)],
                        ["Kids", "Moms"])

ax2 = Axis(fig[2,1],
            xlabel="MDS1 ($(round(varexplained(spec_pcoa)[1] * 100, digits=2))%)",
            ylabel="MDS2 ($(round(varexplained(spec_pcoa)[2] * 100, digits=2))%)"
)

scatter!(ax2, Resonance.loadings(spec_pcoa)[:,1], Resonance.loadings(spec_pcoa)[:,2],
         color = getcolors(get(species, :ageMonths) ./ 12, (0,10)),
         strokewidth=0.5
)
cleg = Colorbar(fig[2, 2], limits=(0.1, 10), colormap=:plasma, highclip=:white, lowclip=:black, label="Age in Years")
save("figures/species_pcoa.png", fig)

fig

##

fig = Figure(resolution=(600, 600))
ax1 = Axis(fig[1,1], xlabel="Age (years)",
                     ylabel="MDS1 ($(round(varexplained(kids_pcoa)[1] * 100, digits=2))%)"
)

sc = scatter!(ax1, get(kidsspecies, :ageMonths) ./12, Resonance.loadings(kids_pcoa)[:,1],
        color = get(kidsspecies, :shannon),
        strokewidth=0.5
)

lb = Label(fig[1,2], "α diveristy (Shannon)", rotation=π/2, tellheight=false, padding=(-10,-10,0,0))
cleg = Colorbar(fig[1, 3], sc)

save("figures/species_pcoa_kids_age.png", fig)

fig
##

fig = Figure(resolution=(800, 800))
ax1 = Axis(fig[1,1],
            xlabel="PCA1 ($(round(varexplained(brain_pcoa)[1] * 100, digits=2))%)",
            ylabel="PCA2 ($(round(varexplained(brain_pcoa)[2] * 100, digits=2))%)"
)

scatter!(ax1, Resonance.loadings(brain_pcoa)[:,1], Resonance.loadings(brain_pcoa)[:,2],
         color = getcolors(tps[complete_brain, :ageMonths] ./ 12, (0,10)),
         strokewidth=0.5
)
cleg = Colorbar(fig[1, 2], limits=(0.1, 10), colormap=:plasma, highclip=:white, lowclip=:black, label="Age in Years")

ax2 = Axis(fig[2,1],
        xlabel="PCA1 ($(round(varexplained(brain_pcoa)[1] * 100, digits=2))%)",
        ylabel="Age in Years"
)

scatter!(ax2, Resonance.loadings(brain_pcoa)[:,1], tps[complete_brain, :ageMonths] ./ 12,
        color = getcolors(tps[complete_brain, :cogScore], (65,135), colormap=:viridis),
        strokewidth=0.5
)

cleg = Colorbar(fig[2, 2], limits=(65, 135), colormap=:viridis, highclip=:white, lowclip=:black, label="Cognitive function score")

save("figures/brain_pcoa.png", fig)

fig

##

srt_metab, srt_spec = stp_overlap(collect(zip(get(metabolites, :subject), get(metabolites, :timepoint))),
                                  collect(zip(get(species, :subject), get(species, :timepoint)))
)

##

fig = Figure(resolution=(800, 800))
ax1 = Axis(fig[1,1],
            xlabel="Species MDS1 ($(round(varexplained(spec_pcoa)[1] * 100, digits=2))%)",
            ylabel="Metabolites MDS1 ($(round(varexplained(met_pcoa)[1] * 100, digits=2))%)"
)

scatter!(ax1, Resonance.loadings(spec_pcoa)[srt_spec,1], Resonance.loadings(met_pcoa)[srt_metab,1],
        color=[ismissing(x) ? :grey : x == "M" ? :dodgerblue : :orange for x in get(metabolites, :Mother_Child)[srt_metab]],
        strokewidth=0.5
)

leg = Legend(fig[1,2], [MarkerElement(color=:orange, marker=:circle),
                        MarkerElement(color=:dodgerblue, marker=:circle)],
                        ["Kids", "Moms"])

ax2 = Axis(fig[2,1],
            xlabel="Species MDS1 ($(round(varexplained(spec_pcoa)[1] * 100, digits=2))%)",
            ylabel="Metabolites MDS1 ($(round(varexplained(met_pcoa)[1] * 100, digits=2))%)"
)

scatter!(ax2, Resonance.loadings(spec_pcoa)[srt_spec,1], Resonance.loadings(met_pcoa)[srt_metab,1],
         color = getcolors(get(metabolites, :ageMonths)[srt_metab], (0,25)),
         strokewidth=0.5
)
cleg = Colorbar(fig[2, 2], limits=(0.1, 25), colormap=:plasma, highclip=:white, lowclip=:black, label="Age in Months")

fig

##

srt_brain, srt_spec = stp_overlap(collect(zip(tps[complete_brain, :subject], tps[complete_brain, :timepoint])),
                                  collect(zip(get(species, :subject), get(species, :timepoint)))
)

##

fig = Figure(resolution=(800, 800))
ax1 = Axis(fig[1,1],
            xlabel="Species MDS1 ($(round(varexplained(spec_pcoa)[1] * 100, digits=2))%)",
            ylabel="Brain PCA1 ($(round(varexplained(brain_pcoa)[1] * 100, digits=2))%)"
)

scatter!(ax1, Resonance.loadings(spec_pcoa)[srt_spec,1], Resonance.loadings(brain_pcoa)[srt_brain,1],
        color=[ismissing(x) ? :grey : x == "M" ? :dodgerblue : :orange for x in get(species, :Mother_Child)[srt_spec]],
        strokewidth=0.5
)

leg = Legend(fig[1,2], [MarkerElement(color=:orange, marker=:circle),
                        MarkerElement(color=:dodgerblue, marker=:circle)],
                        ["Kids", "Moms"])

ax2 = Axis(fig[2,1],
            xlabel="Species MDS1 ($(round(varexplained(spec_pcoa)[1] * 100, digits=2))%)",
            ylabel="Brain MDS1 ($(round(varexplained(brain_pcoa)[1] * 100, digits=2))%)"
)

scatter!(ax2, Resonance.loadings(spec_pcoa)[srt_spec,1], Resonance.loadings(brain_pcoa)[srt_brain,1],
         color = getcolors(get(species, :ageMonths)[srt_spec], (0,120)),
         strokewidth=0.5
)
cleg = Colorbar(fig[2, 2], limits=(0.1, 120), colormap=:plasma, highclip=:white, lowclip=:black, label="Age in Months")

fig

##

gaba = findfirst(f-> commonname(f) === "gamma-Aminobutyric acid", features(metabolites))
glutamate = findfirst(f-> commonname(f) === "Glutamic acid", features(metabolites))

momsidx = string.(get(metabolites, :Mother_Child)) .=== "M"
kidsidx = string.(get(metabolites, :Mother_Child)) .=== "C"

##

fig = Figure(resolution=(800,800))
ax1 = Axis(fig[1:4,1:4], xlabel="GABA (log abundance)", ylabel="Glutamate (log abundance)")
ax2 = Axis(fig[0,1:4], height=200, xlabels=false, xticklabelsvisible=false)
ax3 = Axis(fig[2:5, 5], width=200, ylabels=false, yticklabelsvisible=false)

scmom = scatter!(ax1, log.(vec(abundances(metabolites[gaba, momsidx]))), log.(vec(abundances(metabolites[glutamate,momsidx]))))
hist!(ax2, log.(vec(metabolites[gaba, momsidx] |> abundances)))
hist!(ax3, log.(vec(metabolites[glutamate, momsidx] |> abundances)), direction=:x)

sckid = scatter!(ax1, log.(vec(metabolites[gaba,kidsidx] |> abundances)), log.(vec(metabolites[glutamate, kidsidx] |> abundances)))
hist!(ax2, log.(vec(metabolites[gaba, kidsidx] |> abundances)))
hist!(ax3, log.(vec(metabolites[glutamate, kidsidx] |> abundances)), direction=:x)

leg = Legend(fig[1,5], [scmom, sckid], ["Moms", "Kids"], tellwidth = false, tellheight = false)

save("figures/gaba-glutamate.png", fig)
fig

##

fig = Figure(resolution=(1200,1200))
ax1 = Axis(fig[1:2,1:2], xlabel="GABA (log)", ylabel="Glutamate (log)")
ax2 = Axis(fig[0,1:2], height=200)
ax3 = Axis(fig[2:3, 3], width=200)

scmom = scatter!(ax1, log.(vec(abundances(metabolites[gaba,momsidx]))),
                      log.(vec(abundances(metabolites[glutamate, momsidx]))), color=:gray)
histmom = hist!(ax2, log.(vec(abundances(metabolites[gaba,momsidx]))), color=:gray)
hist!(ax3, log.(vec(abundances(metabolites[glutamate, momsidx]))), direction=:x, color=:gray)

sckid = scatter!(ax1, log.(vec(abundances(metabolites[gaba,kidsidx]))),
                      log.(vec(abundances(metabolites[glutamate, kidsidx]))),
                      color = getcolors(get(metabolites[:, kidsidx], :ageMonths), (0,25)),
                      strokewidth=1)
histkid = hist!(ax2, log.(vec(abundances(metabolites[gaba,kidsidx]))))
hist!(ax3, log.(vec(abundances(metabolites[glutamate,kidsidx]))), direction=:x)

cleg = Colorbar(fig[2:3, 4], limits=(0.1, 25), colormap=:plasma, highclip=:white, lowclip=:black, label="Age in Months")
leg = Legend(fig[1,3], [histmom, histkid], ["Moms", "Kids"], tellwidth = false, tellheight = false)

save("figures/gaba-glutamate_age.png", fig)
fig

##

allmeta = leftjoin(tps, select(omni, [:subject, :timepoint, :sample]), on=[:subject, :timepoint])
unirefs = Resonance.read_gfs_arrow()
set!(unirefs, allmeta)
##

neuroactive = Resonance.getneuroactive(map(f-> replace(f, "UniRef90_"=>""), featurenames(unirefs)))

overlap = intersect(Set(zip(get(unirefs, :subject), get(unirefs, :timepoint))), Set(zip(get(metabolites, :subject), get(metabolites, :timepoint))))

metaboverlap = metabolites[:, map(s-> (s.subject, s.timepoint) in overlap, samples(metabolites))]

##

using GLM
using MixedModels
using AlgebraOfGraphics

genemetab = DataFrame(
    gabasynth = log.(1 .+ map(sum, eachcol(abundances(unirefs[neuroactive["GABA synthesis"], [esmap[s] for s in samplenames(metaboverlap)]])))),
    gabadegr  = log.(1 .+ map(sum, eachcol(abundances(unirefs[neuroactive["GABA degradation"], [esmap[s] for s in samplenames(metaboverlap)]])))),
    gabagut   = log.(vec(abundances(metaboverlap[gaba, :]))),
    glutsynth = log.(1 .+ map(sum, eachcol(abundances(unirefs[neuroactive["Glutamate synthesis"], [esmap[s] for s in samplenames(metaboverlap)]])))),
    glutdegr  = log.(1 .+ map(sum, eachcol(abundances(unirefs[neuroactive["Glutamate degradation"], [esmap[s] for s in samplenames(metaboverlap)]])))),
    glutgut   = log.(vec(abundances(metaboverlap[glutamate, :]))),
    mc        = get(metaboverlap, :Mother_Child))

gabasynthlm = lm(@formula(gabagut ~ gabasynth + mc), genemetab)
gabadegrlm = lm(@formula(gabagut ~ gabadegr + mc), genemetab)
glutsynthlm = lm(@formula(glutgut ~ glutsynth + mc), genemetab)
glutdegrlm = lm(@formula(glutgut ~ glutdegr + mc), genemetab)

##

pred = DataFrame(gabasynth = repeat(range(extrema(genemetab.gabasynth)..., length=50), outer=2),
                 gabadegr  = repeat(range(extrema(genemetab.gabadegr)..., length=50), outer=2),
                 gabagut   = zeros(100),
                 glutsynth = repeat(range(extrema(genemetab.glutsynth)..., length=50), outer=2),
                 glutdegr  = repeat(range(extrema(genemetab.glutdegr)..., length=50), outer=2),
                 glutgut   = zeros(100),
                 mc        = repeat(["M", "C"], inner=50))

pred.gabasynth_pred = predict(gabasynthlm, pred)
pred.gabadegr_pred  = predict(gabadegrlm, pred)
pred.glutsynth_pred = predict(glutsynthlm, pred)
pred.glutdegr_pred  = predict(glutdegrlm, pred)

predgrp = groupby(pred, :mc)

##

fig = Figure(resolution=(800,800))
ax1 = Axis(fig[1,1], xlabel="GABA Synthesis log(RPKM)", ylabel="GABA (log abundance)")
ax2 = Axis(fig[2,1], xlabel="GABA Degradation log(RPKM)", ylabel="GABA (log abundance)")

scatter!(ax1, log.(1+genemetab.gabasynth), log.(1+genemetab.gabagut),
              color=[ismissing(x) ? :gray : x == "M" ? :dodgerblue : :orange for x in genemetab.mc])

for grp in groupby(pred, :mc)
    c = first(grp.mc) == "M" ? :dodgerblue : :orange
    lines!(ax1, grp.gabasynth, grp.gabasynth_pred, color=c)
end

scatter!(ax2, log.(1+genemetab.gabadegr), log.(1+genemetab.gabagut),
              color=[ismissing(x) ? :gray : x == "M" ? :dodgerblue : :orange for x in genemetab.mc])

for grp in groupby(pred, :mc)
    c = first(grp.mc) == "M" ? :dodgerblue : :orange
    lines!(ax2, grp.gabadegr, grp.gabadegr_pred, color=c)
end

##

m1 = MarkerElement(color=:dodgerblue, marker=:circle)
l1 = LineElement(color=:dodgerblue)
m2 = MarkerElement(color=:orange, marker=:circle)
l2 = LineElement(color=:orange)
Legend(fig[1:2, 2], [[m1, l1], [m2, l2]], ["moms", "kids"])

save("figures/gaba_genes_metabolites.png", fig)
fig

##

##

fig = Figure(resolution=(850, 1100))
ax1 = Axis(fig[1,1], xlabel="Glutamate Synthesis log(RPKM)", ylabel="Glutamate (log abundance)")
ax2 = Axis(fig[2,1], xlabel="Glutamate Degradation log(RPKM)", ylabel="Glutamate (log abundance)")

scatter!(ax1, log.(1+genemetab.glutsynth), log.(1+genemetab.glutgut),
              color=[ismissing(x) ? :gray : x == "M" ? :dodgerblue : :orange for x in genemetab.mc])

for grp in groupby(pred, :mc)
    c = first(grp.mc) == "M" ? :dodgerblue : :orange
    lines!(ax1, grp.glutsynth, grp.glutsynth_pred, color=c)
end

scatter!(ax2, log.(1+genemetab.glutdegr), log.(1+genemetab.glutgut),
              color=[ismissing(x) ? :gray : x == "M" ? :dodgerblue : :orange for x in genemetab.mc])

for grp in groupby(pred, :mc)
    c = first(grp.mc) == "M" ? :dodgerblue : :orange
    lines!(ax2, grp.glutdegr, grp.glutdegr_pred, color=c)
end

##

m1 = MarkerElement(color=:dodgerblue, marker=:circle)
l1 = LineElement(color=:dodgerblue)
m2 = MarkerElement(color=:orange, marker=:circle)
l2 = LineElement(color=:orange)
Legend(fig[4, 1], [[m1, l1], [m2, l2]], ["moms", "kids"], orientation=:horizontal, tellheight=true, tellwidth=false)

save("figures/glutamate_genes_metabolites.png", fig)
fig

##

scatter(log.(1 .+ genemetab.glutdegr), log.(1 .+ genemetab.glutsynth), genemetab.glutgut,
    axis=(; xlabel="degradataion", ylabel="synthesis", zlabel="concentration"))

##

using PERMANOVA

max_stars_repo_name: Klepac-Ceraj-Lab/ResonanceAnalysis

max_issues_repo_name: Klepac-Ceraj-Lab/ResonanceAnalysis

max_forks_repo_name: Klepac-Ceraj-Lab/ResonanceAnalysis


text:
@testset "Collision: Sphere-sphere" begin
    function get_two_body(;
        radius_body1=0.5,
        radius_body2=0.5,
        mass_body1=1.0,
        mass_body2=1.0,
        friction_type=:nonlinear,
        friction_coefficient=0.5,
        joint_world_body1=:Fixed,
        gravity=-9.81,
        timestep=0.1)

        origin = Dojo.Origin{Float64}()
        pbody = Dojo.Sphere(radius_body1, mass_body1)
        cbody = Dojo.Sphere(radius_body2, mass_body2)
        joint = Dojo.JointConstraint(eval(joint_world_body1)(origin, pbody))
        bodies = [pbody, cbody]
        joints = [joint]

        # contact model
        collision = Dojo.SphereSphereCollision{Float64,2,3,6}(
                szeros(3),
                szeros(3),
                pbody.shape.r,
                cbody.shape.r)

        if friction_type == :nonlinear
            friction_parameterization = SA{Float64}[
                    1.0  0.0
                    0.0  1.0
            ]
            body_body = Dojo.NonlinearContact{Float64,8}(friction_coefficient, friction_parameterization, collision)
        elseif friction_type == :linear
            friction_parameterization = SA{Float64}[
                0.0  1.0
                0.0 -1.0
                1.0  0.0
               -1.0  0.0
            ]
            body_body = Dojo.LinearContact{Float64,12}(friction_coefficient, friction_parameterization, collision)
        elseif friction_type == :impact
            friction_parameterization = szeros(Float64, 0, 2)
            collision = Dojo.SphereSphereCollision{Float64,0,3,0}(
                        szeros(3),
                        szeros(3),
                        pbody.shape.r,
                        cbody.shape.r)
            body_body = Dojo.ImpactContact{Float64,2}(friction_parameterization, collision)
        end

        contacts = [Dojo.ContactConstraint((body_body, pbody.id, cbody.id), name=:body_body)]

        # z-drop (w/ gravity)
        mechanism = Dojo.Mechanism(origin, bodies, joints, contacts;
                    gravity,
                    timestep)
        return mechanism
    end

    function test_jacobians(mechanism)

        # unpack
        collision = mechanism.contacts[1].model.collision
        xp = mechanism.bodies[1].state.x2
        qp = mechanism.bodies[1].state.q2
        xc = mechanism.bodies[2].state.x2
        qc = mechanism.bodies[2].state.q2

        for jacobian in [:parent, :child]
            # distance
            dis = Dojo.distance(collision, xp, qp, xc, qc)

            X = Dojo.∂contact_normal_transpose∂x(jacobian, collision, xp, qp, xc, qc)
            if jacobian == :parent
                FD = ForwardDiff.jacobian(x -> Dojo.contact_normal(collision, x, qp, xc, qc)', xp)
            elseif jacobian == :child
                FD = ForwardDiff.jacobian(x -> Dojo.contact_normal(collision, xp, qp, x, qc)', xc)
            end

            @test norm((dis >= 0.0 ? 1.0 : -1.0) * X - FD, Inf) < 1.0e-6

            Q = Dojo.∂contact_normal_transpose∂q(jacobian, collision, xp, qp, xc, qc)
            if jacobian == :parent
                FD = ForwardDiff.jacobian(q -> Dojo.contact_normal(collision, xp, Quaternion(q..., false), xc, qc)', Dojo.vector(qp))
            elseif jacobian == :child
                FD = ForwardDiff.jacobian(q -> Dojo.contact_normal(collision, xp, qp, xc, Quaternion(q..., false))', Dojo.vector(qc))
            end

            @test norm((dis >= 0.0 ? 1.0 : -1.0) * Q - FD, Inf) < 1.0e-6

            X = Dojo.∂contact_tangent_one_transpose∂x(jacobian, collision, xp, qp, xc, qc)
            if jacobian == :parent
                FD = ForwardDiff.jacobian(x -> Dojo.contact_tangent(collision, x, qp, xc, qc)[1, :]', xp)
            elseif jacobian == :child
                FD = ForwardDiff.jacobian(x -> Dojo.contact_tangent(collision, xp, qp, x, qc)[1, :]', xc)
            end

            @test norm(FD - X, Inf) < 1.0e-6

            X = Dojo.∂contact_tangent_two_transpose∂x(jacobian, collision, xp, qp, xc, qc)
            if jacobian == :parent
                FD = ForwardDiff.jacobian(x -> Dojo.contact_tangent(collision, x, qp, xc, qc)[2, :]', xp)
            elseif jacobian == :child
                FD = ForwardDiff.jacobian(x -> Dojo.contact_tangent(collision, xp, qp, x, qc)[2, :]', xc)
            end

            @test norm(FD - X, Inf) < 1.0e-6

            Q = Dojo.∂contact_tangent_one_transpose∂q(jacobian, collision, xp, qp, xc, qc)
            if jacobian == :parent
                FD = ForwardDiff.jacobian(q -> Dojo.contact_tangent(collision, xp, Quaternion(q..., false), xc, qc)[1, :]', Dojo.vector(qp))
            elseif jacobian == :child
                FD = ForwardDiff.jacobian(q -> Dojo.contact_tangent(collision, xp, qp, xc, Quaternion(q..., false))[1, :]', Dojo.vector(qc))
            end

            @test norm(FD - Q, Inf) < 1.0e-6

            Q = Dojo.∂contact_tangent_two_transpose∂q(jacobian, collision, xp, qp, xc, qc)
            if jacobian == :parent
                FD = ForwardDiff.jacobian(q -> Dojo.contact_tangent(collision, xp, Quaternion(q..., false), xc, qc)[2, :]', Dojo.vector(qp))
            elseif jacobian == :child
                FD = ForwardDiff.jacobian(q -> Dojo.contact_tangent(collision, xp, qp, xc, Quaternion(q..., false))[2, :]', Dojo.vector(qc))
            end

            @test norm(FD - Q, Inf) < 1.0e-6

            # gradients
            gradient = jacobian

            D = Dojo.∂distance∂x(gradient, collision, xp, qp, xc, qc)
            if gradient == :parent
                FD = FiniteDiff.finite_difference_jacobian(x -> Dojo.distance(collision, x, qp, xc, qc), xp)
            elseif gradient == :child
                FD = FiniteDiff.finite_difference_jacobian(x -> Dojo.distance(collision, xp, qp, x, qc), xc)
            end

            @test norm(D - FD, Inf) < 1.0e-5

            Q = Dojo.∂distance∂q(gradient, collision, xp, qp, xc, qc)
            if gradient == :parent
                FD = FiniteDiff.finite_difference_jacobian(q -> Dojo.distance(collision, xp, Quaternion(q..., false), xc, qc), Dojo.vector(qp))
            elseif gradient == :child
                FD = FiniteDiff.finite_difference_jacobian(q -> Dojo.distance(collision, xp, qp, xc, Quaternion(q..., false)), Dojo.vector(qc))
            end

            @test norm(Q - FD, Inf) < 1.0e-5

            for relative in [:parent, :child]
                X = Dojo.∂contact_point∂x(relative, jacobian, collision, xp, qp, xc, qc)

                if jacobian == :parent
                    FD =  ForwardDiff.jacobian(x -> Dojo.contact_point(relative, collision, x, qp, xc, qc), xp)
                elseif jacobian == :child
                    FD = ForwardDiff.jacobian(x -> Dojo.contact_point(relative, collision, xp, qp, x, qc), xc)
                end

                @test norm(X - FD, Inf) < 1.0e-6

                Q = Dojo.∂contact_point∂q(relative, jacobian, collision, xp, qp, xc, qc)

                if jacobian == :parent
                    FD = ForwardDiff.jacobian(q -> Dojo.contact_point(relative, collision, xp, Quaternion(q..., false), xc, qc), Dojo.vector(qp))
                elseif jacobian == :child
                    FD = ForwardDiff.jacobian(q -> Dojo.contact_point(relative, collision, xp, qp, xc, Quaternion(q..., false)), Dojo.vector(qc))
                end

                @test norm(Q - FD, Inf) < 1.0e-6
            end
        end
    end

    for friction_type in [:nonlinear, :linear, :impact]
        ## z drop
        mech = get_two_body(;
            friction_type=friction_type,
            gravity=-9.81)

        # initialize
        mech.bodies[1].state.x2 = [0.0, 0.0, 0.0]
        mech.bodies[2].state.x2 = [0.0, 0.0, 2.0]
        mech.bodies[1].state.v15 = [0.0, 0.0, 0.0]
        mech.bodies[2].state.v15 = [0.0, 0.0, 0.0]

        # initial Jacobians
        test_jacobians(mech)

        # initial distance
        d = Dojo.distance(mech.contacts[1].model.collision,
            mech.bodies[1].state.x2, mech.bodies[1].state.q2,
            mech.bodies[2].state.x2, mech.bodies[2].state.q2,)
        @test abs(d - 1.0) < 1.0e-6

        # body 1 contact point
        cp = Dojo.contact_point(:parent, mech.contacts[1].model.collision,
            mech.bodies[1].state.x2, mech.bodies[1].state.q2,
            mech.bodies[2].state.x2, mech.bodies[2].state.q2,)
        @test norm(cp - [0.0; 0.0; 0.5], Inf) < 1.0e-6

        # body 2 contact point
        cc = Dojo.contact_point(:child, mech.contacts[1].model.collision,
            mech.bodies[1].state.x2, mech.bodies[1].state.q2,
            mech.bodies[2].state.x2, mech.bodies[2].state.q2,)
        @test norm(cc - [0.0; 0.0; 1.5], Inf) < 1.0e-6

        # contact normal
        cn = Dojo.contact_normal(mech.contacts[1].model.collision,
            mech.bodies[1].state.x2, mech.bodies[1].state.q2,
            mech.bodies[2].state.x2, mech.bodies[2].state.q2,)
        @test norm(cn - [0.0 0.0 -1.0], Inf) < 1.0e-6

        # contact tangent
        ct = Dojo.contact_tangent(mech.contacts[1].model.collision,
            mech.bodies[1].state.x2, mech.bodies[1].state.q2,
            mech.bodies[2].state.x2, mech.bodies[2].state.q2,)
        @test norm(ct - [0.0 1.0 0.0; -1.0 0.0 0.0], Inf) < 1.0e-6

        # graph
        am = Dojo.adjacency_matrix(mech.joints, mech.bodies, mech.contacts)
        @test norm(am - [0 1 0 0; 1 0 0 1; 0 0 0 1; 0 1 1 0], Inf) < 1.0e-6

        # simulate
        storage = Dojo.simulate!(mech, 2.0,
            verbose=false,
            record=true)

        # Jacobians
        test_jacobians(mech)

        # test no interpentration
        @test norm(storage.x[2][end] - [0.0; 0.0; 1.0], Inf) < 1.0e-4

        ## z velocity (no gravity)
        mech = get_two_body(;
            friction_type=friction_type,
            gravity=0.0)

        # initialize
        mech.bodies[1].state.x2 = [0.0, 0.0, 0.0]
        mech.bodies[2].state.x2 = [0.0, 0.0, 2.0]
        mech.bodies[1].state.v15 = [0.0, 0.0, 0.0]
        mech.bodies[2].state.v15 = [0.0, 0.0, -5.0]

        # initial Jacobians
        test_jacobians(mech)

        # simulate
        storage = simulate!(mech, 2.0,
            verbose=false,
            record=true)

        # Jacobians
        test_jacobians(mech)

        # test no interpentration
        @test storage.x[2][end][end] > 1.0

        # z velocity (no gravity)
        mech = get_two_body(;
            friction_type=friction_type,
            gravity=0.0)

        # initialize
        mech.bodies[1].state.x2 = [0.0, 0.0, 0.0]
        mech.bodies[2].state.x2 = [0.0, 0.0, 2.0]
        mech.bodies[1].state.v15 = [0.0, 0.0, 0.0]
        mech.bodies[2].state.v15 = [0.0, 0.0, -5.0]

        # initial Jacobians
        test_jacobians(mech)

        # simulate
        storage = Dojo.simulate!(mech, 2.0,
            verbose=false,
            record=true)

        # Jacobians
        test_jacobians(mech)

        # test no interpentration
        @test storage.x[2][end][end] > 1.0

        ## z velocity (no gravity, floating parent)
        mech = get_two_body(;
            friction_type=friction_type,
            joint_world_body1=:Floating,
            gravity=0.0)

        # initialize
        mech.bodies[1].state.x2 = [0.0, 0.0, 0.0]
        mech.bodies[2].state.x2 = [0.0, 0.0, 2.0]
        mech.bodies[1].state.v15 = [0.0, 0.0, 0.0]
        mech.bodies[2].state.v15 = [0.0, 0.0, -5.0]

        # initial Jacobians
        test_jacobians(mech)

        # simulate
        storage = Dojo.simulate!(mech, 2.0,
            verbose=false,
            record=true)

        # Jacobians
        test_jacobians(mech)

        # test no interpentration
        @test storage.x[2][end][end] - storage.x[1][end][end] > 1.0
        @test storage.x[2][end][end] < 0.0

        ## x velocity (fixed parent)
        mech = get_two_body(;
            friction_type=friction_type,
            gravity=0.0)

        # initialize
        mech.bodies[1].state.x2 = [0.0, 0.0, 0.0]
        mech.bodies[2].state.x2 = [2.0, 0.0, 0.0]
        mech.bodies[1].state.v15 = [0.0, 0.0, 0.0]
        mech.bodies[2].state.v15 = [-5.0, 0.0, 0.0]

        # initial Jacobians
        test_jacobians(mech)

        # initial distance
        d = Dojo.distance(mech.contacts[1].model.collision,
            mech.bodies[1].state.x2, mech.bodies[1].state.q2,
            mech.bodies[2].state.x2, mech.bodies[2].state.q2,)
        @test abs(d - 1.0) < 1.0e-6

        # body 1 contact point
        cp = Dojo.contact_point(:parent, mech.contacts[1].model.collision,
            mech.bodies[1].state.x2, mech.bodies[1].state.q2,
            mech.bodies[2].state.x2, mech.bodies[2].state.q2,)
        @test norm(cp - [0.5; 0.0; 0.0], Inf) < 1.0e-6

        # body 2 contact point
        cc = Dojo.contact_point(:child, mech.contacts[1].model.collision,
            mech.bodies[1].state.x2, mech.bodies[1].state.q2,
            mech.bodies[2].state.x2, mech.bodies[2].state.q2,)
        @test norm(cc - [1.5; 0.0; 0.0], Inf) < 1.0e-6

        # contact normal
        cn = Dojo.contact_normal(mech.contacts[1].model.collision,
            mech.bodies[1].state.x2, mech.bodies[1].state.q2,
            mech.bodies[2].state.x2, mech.bodies[2].state.q2,)
        @test norm(cn - [-1.0 0.0 0.0], Inf) < 1.0e-6

        # contact tangent
        ct = Dojo.contact_tangent(mech.contacts[1].model.collision,
            mech.bodies[1].state.x2, mech.bodies[1].state.q2,
            mech.bodies[2].state.x2, mech.bodies[2].state.q2,)
        @test norm(ct - [0.0 0.0 1.0; 0.0 -1.0 0.0], Inf) < 1.0e-6

        # graph
        am = Dojo.adjacency_matrix(mech.joints, mech.bodies, mech.contacts)
        @test norm(am - [0 1 0 0; 1 0 0 1; 0 0 0 1; 0 1 1 0], Inf) < 1.0e-6

        # simulate
        storage = Dojo.simulate!(mech, 2.0,
            verbose=false,
            record=true)

        # Jacobians
        test_jacobians(mech)

        # test no interpentration
        @test storage.x[2][end][1] > 1.0

        ## x velocity (floating parent)
        mech = get_two_body(;
            friction_type=friction_type,
            joint_world_body1=:Floating,
            gravity=0.0)

        # initialize
        mech.bodies[1].state.x2 = [0.0, 0.0, 0.0]
        mech.bodies[2].state.x2 = [2.0, 0.0, 0.0]
        mech.bodies[1].state.v15 = [0.0, 0.0, 0.0]
        mech.bodies[2].state.v15 = [-5.0, 0.0, 0.0]

        # initial Jacobians
        test_jacobians(mech)

        # simulate
        storage = Dojo.simulate!(mech, 2.0,
            verbose=false,
            record=true)

        # Jacobians
        test_jacobians(mech)

        # test no interpentration
        @test storage.x[2][end][1] - storage.x[1][end][1] > 1.0
        @test storage.x[1][end][1] < 0.0

        ## y velocity (fixed parent)
        mech = get_two_body(;
            friction_type=friction_type,
            gravity=0.0)

        # initialize
        mech.bodies[1].state.x2 = [0.0, 0.0, 0.0]
        mech.bodies[2].state.x2 = [0.0, 2.0, 0.0]
        mech.bodies[1].state.v15 = [0.0, 0.0, 0.0]
        mech.bodies[2].state.v15 = [0.0, -5.0, 0.0]

        # initial Jacobians
        test_jacobians(mech)

        # initial distance
        d = Dojo.distance(mech.contacts[1].model.collision,
            mech.bodies[1].state.x2, mech.bodies[1].state.q2,
            mech.bodies[2].state.x2, mech.bodies[2].state.q2,)
        @test abs(d - 1.0) < 1.0e-6

        # body 1 contact point
        cp = Dojo.contact_point(:parent, mech.contacts[1].model.collision,
            mech.bodies[1].state.x2, mech.bodies[1].state.q2,
            mech.bodies[2].state.x2, mech.bodies[2].state.q2,)
        @test norm(cp - [0.0; 0.5; 0.0], Inf) < 1.0e-6

        # body 2 contact point
        cc = Dojo.contact_point(:child, mech.contacts[1].model.collision,
            mech.bodies[1].state.x2, mech.bodies[1].state.q2,
            mech.bodies[2].state.x2, mech.bodies[2].state.q2,)
        @test norm(cc - [0.0; 1.5; 0.0], Inf) < 1.0e-6

        # contact normal
        cn = Dojo.contact_normal(mech.contacts[1].model.collision,
            mech.bodies[1].state.x2, mech.bodies[1].state.q2,
            mech.bodies[2].state.x2, mech.bodies[2].state.q2,)
        @test norm(cn - [0.0 -1.0 0.0], Inf) < 1.0e-6

        # contact tangent
        ct = Dojo.contact_tangent(mech.contacts[1].model.collision,
            mech.bodies[1].state.x2, mech.bodies[1].state.q2,
            mech.bodies[2].state.x2, mech.bodies[2].state.q2,)
        @test norm(ct - [0.0 0.0 -1.0; -1.0 0.0 0.0], Inf) < 1.0e-6

        # graph
        am = Dojo.adjacency_matrix(mech.joints, mech.bodies, mech.contacts)
        @test norm(am - [0 1 0 0; 1 0 0 1; 0 0 0 1; 0 1 1 0], Inf) < 1.0e-6

        # simulate
        storage = Dojo.simulate!(mech, 2.0,
            verbose=false,
            record=true)

        # Jacobians
        test_jacobians(mech)

        # test no interpentration
        @test storage.x[2][end][2] > 1.0

        ## y velocity (floating parent)
        mech = get_two_body(;
            friction_type=friction_type,
            joint_world_body1=:Floating,
            gravity=0.0)

        # initialize
        mech.bodies[1].state.x2 = [0.0, 0.0, 0.0]
        mech.bodies[2].state.x2 = [0.0, 2.0, 0.0]
        mech.bodies[1].state.v15 = [0.0, 0.0, 0.0]
        mech.bodies[2].state.v15 = [0.0, -5.0, 0.0]

        # initial Jacobians
        test_jacobians(mech)

        # simulate
        storage = Dojo.simulate!(mech, 2.0,
            verbose=false,
            record=true)

        # Jacobians
        test_jacobians(mech)

        # test no interpentration
        @test storage.x[2][end][2] - storage.x[1][end][2] > 1.0
        @test storage.x[1][end][2] < 0.0

        ## xyz velocity (fixed parent)
        mech = get_two_body(;
            friction_type=friction_type,
            gravity=0.0)

        # initialize
        mech.bodies[1].state.x2 = [0.0, 0.0, 0.0]
        mech.bodies[2].state.x2 = [2.0, 2.0, 2.0]
        mech.bodies[1].state.v15 = [0.0, 0.0, 0.0]
        mech.bodies[2].state.v15 = [-2.0, -2.0, -2.0]

        # initial Jacobians
        test_jacobians(mech)

        # initial distance
        d = Dojo.distance(mech.contacts[1].model.collision,
            mech.bodies[1].state.x2, mech.bodies[1].state.q2,
            mech.bodies[2].state.x2, mech.bodies[2].state.q2,)
        @test abs(d - 2.4641016) < 1.0e-4

        # body 1 contact point
        cp = Dojo.contact_point(:parent, mech.contacts[1].model.collision,
            mech.bodies[1].state.x2, mech.bodies[1].state.q2,
            mech.bodies[2].state.x2, mech.bodies[2].state.q2,)
        @test norm(cp - [0.288675; 0.288675; 0.288675], Inf) < 1.0e-4

        # body 2 contact point
        cc = Dojo.contact_point(:child, mech.contacts[1].model.collision,
            mech.bodies[1].state.x2, mech.bodies[1].state.q2,
            mech.bodies[2].state.x2, mech.bodies[2].state.q2,)
        @test norm(cc - [1.711324; 1.711324; 1.711324], Inf) < 1.0e-4

        # contact normal
        cn = Dojo.contact_normal(mech.contacts[1].model.collision,
            mech.bodies[1].state.x2, mech.bodies[1].state.q2,
            mech.bodies[2].state.x2, mech.bodies[2].state.q2,)
        @test norm(cn - [-0.57735 -0.57735 -0.57735], Inf) < 1.0e-4

        # contact tangent
        ct = Dojo.contact_tangent(mech.contacts[1].model.collision,
            mech.bodies[1].state.x2, mech.bodies[1].state.q2,
            mech.bodies[2].state.x2, mech.bodies[2].state.q2,)
        @test norm(ct - [0.0 0.57735 -0.57735; -0.66666 0.33333 0.33333], Inf) < 1.0e-4

        # graph
        am = Dojo.adjacency_matrix(mech.joints, mech.bodies, mech.contacts)
        @test norm(am - [0 1 0 0; 1 0 0 1; 0 0 0 1; 0 1 1 0], Inf) < 1.0e-6

        # simulate
        storage = Dojo.simulate!(mech, 2.0,
            verbose=false,
            record=true)

        # Jacobians
        test_jacobians(mech)

        # test no interpentration
        @test storage.x[2][end][1] > 0.5
        @test storage.x[2][end][2] > 0.5
        @test storage.x[2][end][3] > 0.5

        # test reverse velocity direction
        @test norm(Dojo.normalize(storage.v[2][end]) + Dojo.normalize([-2.0, -2.0, -2.0]), Inf) < 1.0e-5

        ## xyz velocity (floating parent)
        mech = get_two_body(;
            friction_type=friction_type,
            joint_world_body1=:Floating,
            gravity=0.0)

        # initialize
        mech.bodies[1].state.x2 = [0.0, 0.0, 0.0]
        mech.bodies[2].state.x2 = [2.0, 2.0, 2.0]
        mech.bodies[1].state.v15 = [0.0, 0.0, 0.0]
        mech.bodies[2].state.v15 = [-2.0, -2.0, -2.0]

        # initial Jacobians
        test_jacobians(mech)

        # simulate
        storage = Dojo.simulate!(mech, 2.0,
            verbose=false,
            record=true)

        # Jacobians
        test_jacobians(mech)

        @test norm(storage.x[2][end] - storage.x[1][end]) > 1.0
        @test norm(Dojo.normalize(storage.v[2][end]) - Dojo.normalize([-2.0, -2.0, -2.0]), Inf) < 1.0e-5
        @test norm(Dojo.normalize(storage.v[1][end]) - Dojo.normalize([-2.0, -2.0, -2.0]), Inf) < 1.0e-5
    end
end

# Sphere-Capsule
@testset "Collision: Sphere-capsule" begin
    function simulate_sphere_capsule(x1, x2)
        origin = Dojo.Origin{Float64}()
        pbody = Dojo.Capsule(0.5, 2.0, 1.0, orientation_offset=Dojo.Quaternion(sqrt(2.0) / 2.0, sqrt(2.0) / 2.0, 0.0, 0.0))
        cbody = Dojo.Sphere(0.5, 1.0)
        joint = Dojo.JointConstraint(Dojo.Fixed(origin, pbody))

        bodies = [pbody, cbody]
        joints = [joint]

        collision = Dojo.SphereCapsuleCollision{Float64,2,3,6}(
            szeros(3),
            SA[0.0; 1.0; 0.0],
            SA[0.0; -1.0; 0.0],
            0.5,
            0.5,
        )

        friction_parameterization = SA{Float64}[
            1.0  0.0
            0.0  1.0
        ]
        body_body_contact = Dojo.NonlinearContact{Float64,8}(0.5, friction_parameterization, collision)

        contacts = [Dojo.ContactConstraint((body_body_contact, cbody.id, pbody.id), name=:body_body)]

        mech = Dojo.Mechanism(origin, bodies, joints, contacts,
                    gravity=1.0 * -9.81,
                    timestep=0.05)

        mech.bodies[1].state.x2 = x1
        mech.bodies[2].state.x2 = x2

        storage = Dojo.simulate!(mech, 1.0,
            verbose=false,
            record=true)

        return mech, storage
    end

    x2 = [[0.0, 0.0, 2.0], [0.2, 0.5, 2.0]]
    mech, storage = simulate_sphere_capsule([0.0, 0.0, 0.0], x2[1])

    d = Dojo.distance(mech.contacts[1].model.collision,
        storage.x[2][end], storage.q[2][end],
        storage.x[1][end], storage.q[1][end],)

    @test d > 0.0

    cp = Dojo.contact_point(:parent, mech.contacts[1].model.collision,
        mech.bodies[2].state.x2, mech.bodies[2].state.q2,
        mech.bodies[1].state.x2, mech.bodies[1].state.q2,)

    @test norm(cp - [0.0; 0.0; 0.5], Inf) < 1.0e-2

    cc = Dojo.contact_point(:child, mech.contacts[1].model.collision,
        mech.bodies[2].state.x2, mech.bodies[2].state.q2,
        mech.bodies[1].state.x2, mech.bodies[1].state.q2,
        )

    @test norm(cc - [0.0; 0.0; 0.5], Inf) < 1.0e-2

    cn = Dojo.contact_normal(mech.contacts[1].model.collision,
        mech.bodies[2].state.x2, mech.bodies[2].state.q2,
        mech.bodies[1].state.x2, mech.bodies[1].state.q2,
        )

    @test norm(cn - [0.0 0.0 1.0], Inf) < 1.0e-3

    ct = Dojo.contact_tangent(mech.contacts[1].model.collision,
        mech.bodies[2].state.x2, mech.bodies[2].state.q2,
        mech.bodies[1].state.x2, mech.bodies[1].state.q2,
        )

    @test norm(ct - [0.0 -1.0 0.0; -1.0 0.0 0.0], Inf) < 1.0e-2

    mech, storage = simulate_sphere_capsule([0.0, 0.0, 0.0], x2[2])

    d = Dojo.distance(mech.contacts[1].model.collision,
        storage.x[2][end], storage.q[2][end],
        storage.x[1][end], storage.q[1][end],)

    @test d > 0.0
end

# Sphere-Box
@testset "Collision: Sphere-box" begin
    function simulate_sphere_box(x1, x2, v2)
        origin = Dojo.Origin{Float64}()
        pbody = Dojo.Box(1.0, 1.0, 1.0, 1.0)
        cbody = Dojo.Sphere(0.5, 1.0)
        joint = Dojo.JointConstraint(Dojo.Fixed(origin, pbody))

        bodies = [pbody, cbody]
        joints = [joint]

        collision = Dojo.SphereBoxCollision{Float64,2,3,6}(
            szeros(3),
            1.0,
            1.0,
            1.0,
            0.5,
        )

        friction_parameterization = SA{Float64}[
            1.0  0.0
            0.0  1.0
        ]
        body_body_contact = Dojo.NonlinearContact{Float64,8}(0.5, friction_parameterization, collision)

        contacts = [Dojo.ContactConstraint((body_body_contact, cbody.id, pbody.id), name=:body_body)]

        mech = Dojo.Mechanism(origin, bodies, joints, contacts,
                    gravity=1.0 * -9.81,
                    timestep=0.05)

        mech.bodies[1].state.x2 = x1
        mech.bodies[2].state.x2 = x2
        mech.bodies[2].state.v15 = v2

        storage = Dojo.simulate!(mech, 1.0,
                verbose=false,
                record=true)

        return mech, storage
    end

    x1 = [0.0, 0.0, 0.0]
    x2 = [
        [0.0, 0.0, 2.0],
        [0.0, 1.0, 2.0],
        [1.0, 1.0, 2.0],
        [-1.0, 1.0, 2.0],
        [-0.5; 0.5; 2.0],
        [0.0, -1.0, 2.0],
        [0.0, -1.0, 2.0],
        [-1.0, -1.0, -2.0],
        [0.0, 0.0, -2.0],
    ]
    v2 = [
        [0.0, 0.0, 0.0],
        [0.0; -1.0; -2.0],
        [-1.0; -1.0; -2.0],
        [1.0; -1.0; -2.0],
        [0.0; 0.0; 0.0],
        [0.0; 1.0; -2.0],
        [0.0; 1.0; -2.0],
        [5.0; 5.0; 10.0],
        [0.0; 0.0; 10.0],
    ]

    N = length(x2)

    for i = 1:N
        mech, storage = simulate_sphere_box(x1, x2[i], v2[i])

        d = Dojo.distance(mech.contacts[1].model.collision,
                storage.x[2][end], storage.q[2][end],
                storage.x[1][end], storage.q[1][end],)

        @test d > 0.0

        if i == 1
            cp = Dojo.contact_point(:parent, mech.contacts[1].model.collision,
                mech.bodies[2].state.x2, mech.bodies[2].state.q2,
                mech.bodies[1].state.x2, mech.bodies[1].state.q2,)

            @test norm(cp - [0.0; 0.0; 0.5], Inf) < 1.0e-2

            cc = Dojo.contact_point(:child, mech.contacts[1].model.collision,
                mech.bodies[2].state.x2, mech.bodies[2].state.q2,
                mech.bodies[1].state.x2, mech.bodies[1].state.q2,
                )

            @test norm(cc - [0.0; 0.0; 0.5], Inf) < 1.0e-2

            cn = Dojo.contact_normal(mech.contacts[1].model.collision,
                mech.bodies[2].state.x2, mech.bodies[2].state.q2,
                mech.bodies[1].state.x2, mech.bodies[1].state.q2,
                )

            @test norm(cn - [0.0 0.0 1.0], Inf) < 1.0e-3

            ct = Dojo.contact_tangent(mech.contacts[1].model.collision,
                mech.bodies[2].state.x2, mech.bodies[2].state.q2,
                mech.bodies[1].state.x2, mech.bodies[1].state.q2,
                )

            @test norm(ct - [0.0 -1.0 0.0; -1.0 0.0 0.0], Inf) < 1.0e-2
        end
    end
end

@testset "Collision: String" begin
    origin = Origin{Float64}()
    pbody = Sphere(0.5, 1.0)
    cbody = Sphere(0.5, 1.0)
    joint = JointConstraint(Fixed(origin, pbody))

    bodies = [pbody, cbody]
    joints = [joint]

    collision = StringCollision{Float64,2,3,6}(
            szeros(3),
            szeros(3),
            2.0)
    friction_parameterization = SA{Float64}[
        1.0  0.0
        0.0  1.0
    ]
    body_body_contact = NonlinearContact{Float64,8}(0.5, friction_parameterization, collision)

    contacts = [ContactConstraint((body_body_contact, pbody.id, cbody.id), name=:body_body)]

    mech = Mechanism(origin, bodies, joints, contacts;
                gravity=1.0 * -9.81,
                timestep=0.1)

    mech.bodies[1].state.x2 = [0.0, 0.0, 0.0]
    mech.bodies[2].state.x2 = [0.0, 0.0, 1.0]

    storage = simulate!(mech, 2.0,
        verbose=false,
        record=true)

    # vis = Visualizer()
    # render(vis)
    # visualize(mech, storage,
    #     vis=vis)

    @test norm(mech.bodies[2].state.x2 - mech.bodies[1].state.x2) < 2.0
end


text:
"""
    run_leapfrog(ps, params_sim, filename)
"""
function run_leapfrog(ps, params_sim, filename)
    open(filename, "w") do out

        @unpack ϵ, Δt, t_end, Δt_out = params_sim
        calc_force!(ps, ϵ)

        t = 0.
        E₀ = getTotalEnergy(ps)
        while t < t_end
            t%Δt_out == 0 && save_snapshot_txt(out, t, ps)

            leapfrog!(ps, ϵ, Δt)
            t += Δt
        end
        E₁ = getTotalEnergy(ps)
        ΔE = E₁ - E₀
        @show E₀
        @show E₁
        @show ΔE / E₀
    end
end

function calc_force!(ps, ϵ)
    for p in ps
        p.a .= 0
    end

    for i in eachindex(ps)
        for j in eachindex(ps)
            i ≥ j && continue

            r = SVector{3}(ps[j].r) - SVector{3}(ps[i].r)
            r² = norm(r)^2 + ϵ^2
            r⁻³ = r²^(-3/2)

            ps[i].a .+= G * ps[j].m * r * r⁻³
            ps[j].a .-= G * ps[i].m * r * r⁻³
        end
    end
end

function leapfrog!(ps, ϵ, Δt)
    for p in ps
        @. p.v += p.a * 0.5Δt  # Velocity at half-step later
        @. p.r += p.v * Δt     # Position at one-step later
    end

    calc_force!(ps, ϵ)  # Acceleration at one-step later

    for p in ps
        @. p.v += p.a * 0.5Δt  # Velocity at one-step later
    end
end

max_stars_repo_name: MasanoriKanamaru/Astroshaper

max_issues_repo_name: MasanoriKanamaru/Astroshaper

max_forks_repo_name: MasanoriKanamaru/Astroshaper


text:
using PowerModels

PowerModels.silence()

build_acp(data_file::String) = instantiate_model(
    PowerModels.parse_file(data_file),
    ACPPowerModel,
    PowerModels.build_opf
)

function run_sqp_opf(data_file::String, max_iter::Int = 100)
    pm = build_acp(data_file)
    qp_solver = optimizer_with_attributes(
        Ipopt.Optimizer,
        "print_level" => 0,
        "warm_start_init_point" => "yes",
    )
    result = optimize_model!(pm, optimizer = optimizer_with_attributes(
        SQP.Optimizer,
        "algorithm" => "SQP-TR",
        "external_optimizer" => qp_solver,
        "max_iter" => max_iter,
    ))
    return result
end


text:
context("eltype") do
    @fact eltype_or(HyperCube, nothing) --> nothing
    @fact eltype_or(HyperCube{2}, nothing) --> nothing
    @fact eltype_or(HyperCube{2, Float32}, nothing) --> Float32
    @fact eltype_or(SimpleRectangle, Int) --> Int
    @fact eltype_or(SimpleRectangle{Float32}, Int) --> Float32

    @fact eltype(SimpleRectangle(0,0,1,1)) --> Int
    @fact eltype(SimpleRectangle) --> Any
    @fact eltype(HyperCube{2}) --> Any
    @fact eltype(HyperCube{2, Float32}) --> Float32
    @fact eltype(SimpleRectangle{Float32}) --> Float32
end
context("ndims") do
    @fact ndims_or(HyperCube, nothing) --> nothing
    @fact ndims_or(HyperCube{2}, nothing) --> 2
    @fact ndims_or(HyperCube{2, Float32}, nothing) --> 2
    @fact ndims_or(SimpleRectangle, 0) --> 2

    @fact ndims(SimpleRectangle(0,0,1,1)) --> 2
    @fact ndims(SimpleRectangle) --> 2
    @fact ndims(HyperCube{2}) --> 2
    @fact ndims(HyperCube{2, Float32}) --> 2
    @fact_throws ndims(HyperCube)
end


text:
using Documenter, Logging

# Outputs are Julia version specific
if VERSION >= v"1.6"
    # Disable Documeter's Info logging
    oldlvl = Logging.min_enabled_level(current_logger())
    disable_logging(Logging.Info)
    try
        DocMeta.setdocmeta!(AssociatedLegendrePolynomials, :DocTestSetup, :(using AssociatedLegendrePolynomials); recursive=true)
        doctest(AssociatedLegendrePolynomials, testset="Doc Tests")
    finally
        disable_logging(oldlvl - 1)
    end
end


text:
############################################################################################
# Check if data changes, PF settings change
markov_data_small = randn(_rng, Float16, 50)
markov_data_large = randn(_rng, Float16, 10000)

markov_pf = ParticleFilter(_rng, deepcopy(markov_objective))
_vals, _diag = propose(_rng, markov_pf, markov_objective)

@testset "PF - Markov Allocations - Data smaller than initiation" begin
    Ndata = length(markov_data_small)
    propose!(_rng, markov_pf, markov_objective.model, markov_data_small)
    @test length(markov_objective.model.val.latent) == Ndata
    @test size(markov_pf.particles.val,2) == Ndata
    @test size(markov_pf.particles.ancestor,2) == Ndata
    @test length(markov_pf.particles.buffer.resampled) == Ndata
    @test markov_pf.tune.chains.Ndata == Ndata
end

#=
markov_pf = ParticleFilter(_rng, deepcopy(markov_objective))
_vals, _diag = propose(_rng, markov_pf, markov_objective)

@testset "PF - SemiMarkov Allocations - Data larger than initiation" begin
    Ndata = length(markov_data_large)
    propose!(_rng, markov_pf, markov_objective.model, markov_data_large)
    @test length(markov_objective.model.val.latent) == Ndata
    @test size(markov_pf.particles.val,2) == Ndata
    @test size(markov_pf.particles.ancestor,2) == Ndata
    @test length(markov_pf.particles.buffer.resampled) == Ndata
    @test markov_pf.tune.particles.Ndata == Ndata
end
=#


text:
using .NetCDF

"""
    NetCDFDataset

Dataset backend to read NetCDF files using NetCDF.jl

The following keyword arguments are allowed when using :netcdf
as a data sink:

- `compress = -1` set the compression level for the NetCDF file
"""
struct NetCDFDataset
  filename::String
  mode::UInt16
end
NetCDFDataset(filename) = NetCDFDataset(filename,NC_NOWRITE)

import .NetCDF: AbstractDiskArray, readblock!, writeblock!, haschunks, eachchunk

struct NetCDFVariable{T,N} <: AbstractDiskArray{T,N}
  filename::String
  varname::String
  size::NTuple{N,Int}
end
#Define method forwarding for DiskArray methods
for m in [:haschunks, :eachchunk]
  eval(:(function $(m)(v::NetCDFVariable,args...;kwargs...)
    NetCDF.open(a->$(m)(a,args...;kwargs...), v.filename, v.varname)
end
))
end
writeblock!(v::NetCDFVariable, aout, r::AbstractUnitRange...) = NetCDF.open(a->writeblock!(a,aout,r...), v.filename, v.varname, mode=NC_WRITE)
readblock!(v::NetCDFVariable, aout, r::AbstractUnitRange...) = NetCDF.open(a->readblock!(a,aout,r...), v.filename, v.varname)

Base.size(v::NetCDFVariable) = v.size

get_var_dims(ds::NetCDFDataset,name) = NetCDF.open(v->map(i->i.name,v[name].dim),ds.filename)
get_varnames(ds::NetCDFDataset) = NetCDF.open(v->collect(keys(v.vars)),ds.filename)
get_var_attrs(ds::NetCDFDataset, name) = NetCDF.open(v->v[name].atts,ds.filename)
get_global_attrs(ds::NetCDFDataset) = NetCDF.open(nc->nc.gatts, ds.filename)
function Base.getindex(ds::NetCDFDataset, i)
  s,et = NetCDF.open(j->(size(j),eltype(j)),ds.filename,i)
  NetCDFVariable{et,length(s)}(ds.filename, i, s)
end
Base.haskey(ds::NetCDFDataset,k) = NetCDF.open(nc->haskey(nc.vars,k),ds.filename)

function add_var(p::NetCDFDataset, T::Type, varname, s, dimnames, attr;
  chunksize=s, compress = -1)
  dimsdescr = Iterators.flatten(zip(dimnames,s))
  nccreate(p.filename, varname, dimsdescr..., atts = attr, t=T, chunksize=chunksize, compress=compress)
  NetCDFVariable{T,length(s)}(p.filename,varname,(s...,))
end

function create_empty(::Type{NetCDFDataset}, path, gatts=Dict())
  NetCDF.create(_->nothing, path, NcVar[], gatts = gatts)
  NetCDFDataset(path)
end

allow_parallel_write(::Type{<:NetCDFDataset}) = false
allow_parallel_write(::NetCDFDataset) = false

allow_missings(::Type{<:NetCDFDataset}) = false
allow_missings(::NetCDFDataset) = false

backendlist[:netcdf] = NetCDFDataset
push!(backendregex,r".nc$"=>NetCDFDataset)


text:
"""
`assemble_system!` - assemble the linear system for solving
"""
function assemble_system!(problem::FiniteDifferenceBVPProblem{TF, TBCL, TBCR}) where {TF<:AbstractArray, TBCL<:DirichletBC, TBCR<:DirichletBC}

    # build first row
    problem.A[1,1] = 2/problem.Δx^2;
    problem.A[1,2] = -1/problem.Δx^2;
    # build rows 2-n-1
    for i in 2:problem.n-1
        problem.A[i,i-1] = -1/problem.Δx^2;
        problem.A[i,i] = 2/problem.Δx^2;
        problem.A[i,i+1] = -1/problem.Δx^2;
    end
    # build row n
    problem.A[problem.n,problem.n-1] =  -1/problem.Δx^2;
    problem.A[problem.n,problem.n] =  2/problem.Δx^2;

    # build rhs
    @. problem.rhs = problem.f;
    problem.rhs[1] += problem.left_bc.u_bc/problem.Δx^2
    problem.rhs[problem.n] += problem.right_bc.u_bc/problem.Δx^2

    problem
end

function assemble_system!(problem::FiniteDifferenceBVPProblem{TF, TBCL, TBCR}) where {TF<:Function, TBCL<:DirichletBC, TBCR<:DirichletBC}

    # build first row
    problem.A[1,1] = 2/problem.Δx^2;
    problem.A[1,2] = -1/problem.Δx^2;
    # build rows 2-n-1
    for i in 2:problem.n-1
        problem.A[i,i-1] = -1/problem.Δx^2;
        problem.A[i,i] = 2/problem.Δx^2;
        problem.A[i,i+1] = -1/problem.Δx^2;
    end
    # build row n
    problem.A[problem.n,problem.n-1] =  -1/problem.Δx^2;
    problem.A[problem.n,problem.n] =  2/problem.Δx^2;

    # build rhs
    @. problem.rhs = problem.f(problem.x);
    problem.rhs[1] += problem.left_bc.u_bc/problem.Δx^2
    problem.rhs[problem.n] += problem.right_bc.u_bc/problem.Δx^2

    problem
end

function assemble_system!(problem::FiniteDifferenceBVPProblem{TF, TBCL, TBCR}) where {TF<:AbstractArray, TBCL<:NeumannBC, TBCR<:NeumannBC}

    # build first row
    problem.A[1,1] = 1/problem.Δx^2;
    problem.A[1,2] = -1/problem.Δx^2;
    # build rows 2-n+1
    for i in 2:problem.n+1
        problem.A[i,i-1] = -1/problem.Δx^2;
        problem.A[i,i] = 2/problem.Δx^2;
        problem.A[i,i+1] = -1/problem.Δx^2;
    end
    # build row n+2
    problem.A[end,end-1] =  -1/problem.Δx^2;
    problem.A[end,end] =  1/problem.Δx^2;

    # build rhs
    @. problem.rhs[2:end-1] = problem.f;
    problem.rhs[1] = -problem.left_bc.v_bc/problem.Δx
    problem.rhs[end] = problem.right_bc.v_bc/problem.Δx

    problem
end

function assemble_system!(problem::FiniteDifferenceBVPProblem{TF, TBCL, TBCR}) where {TF<:Function, TBCL<:NeumannBC, TBCR<:NeumannBC}

    # build first row
    problem.A[1,1] = 1/problem.Δx^2;
    problem.A[1,2] = -1/problem.Δx^2;
    # build rows 2-n+1
    for i in 2:problem.n+1
        problem.A[i,i-1] = -1/problem.Δx^2;
        problem.A[i,i] = 2/problem.Δx^2;
        problem.A[i,i+1] = -1/problem.Δx^2;
    end
    # build row n+2
    problem.A[end,end-1] =  -1/problem.Δx^2;
    problem.A[end,end] =  1/problem.Δx^2;

    # build rhs
    @. problem.rhs[2:end-1] = problem.f(problem.x[2:end-1]);
    # problem.rhs[1] = 0.5 * problem.f(problem.x[1])-problem.left_bc.v_bc/problem.Δx
    # problem.rhs[end] = 0.5 * problem.f(problem.x[end]) + problem.right_bc.v_bc/problem.Δx
    problem.rhs[1] = -problem.left_bc.v_bc/problem.Δx
    problem.rhs[end] =problem.right_bc.v_bc/problem.Δx

    problem
end

function assemble_system!(problem::FiniteDifferenceBVPProblem{TF, TBCL, TBCR}) where {TF<:AbstractArray, TBCL<:PeriodicBC, TBCR<:PeriodicBC}

    # build first row
    problem.A[1,1] = 2/problem.Δx^2;
    problem.A[1,2] = -1/problem.Δx^2;
    problem.A[1,end] = -1/problem.Δx^2;
    # build rows 2-n
    for i in 2:problem.n
        problem.A[i,i-1] = -1/problem.Δx^2;
        problem.A[i,i] = 2/problem.Δx^2;
        problem.A[i,i+1] = -1/problem.Δx^2;
    end
    # build row n+1
    problem.A[end,1] =  -1/problem.Δx^2;
    problem.A[end,end-1] =  -1/problem.Δx^2;
    problem.A[end,end] =  2/problem.Δx^2;

    # build rhs
    @. problem.rhs = problem.f;

    problem
end

function assemble_system!(problem::FiniteDifferenceBVPProblem{TF, TBCL, TBCR}) where {TF<:Function, TBCL<:PeriodicBC, TBCR<:PeriodicBC}

    # build first row
    problem.A[1,1] = 2/problem.Δx^2;
    problem.A[1,2] = -1/problem.Δx^2;
    problem.A[1,end] = -1/problem.Δx^2;
    # build rows 2-n
    for i in 2:problem.n
        problem.A[i,i-1] = -1/problem.Δx^2;
        problem.A[i,i] = 2/problem.Δx^2;
        problem.A[i,i+1] = -1/problem.Δx^2;
    end
    # build row n+1
    problem.A[end,1] =  -1/problem.Δx^2;
    problem.A[end,end-1] =  -1/problem.Δx^2;
    problem.A[end,end] =  2/problem.Δx^2;

    # build rhs
    @. problem.rhs = problem.f(problem.x);

    problem
end


text:
#======================================#
#             Moves to use             #
#======================================#

# a critical will have a % chance of happening and will increase the damage by a certain %
# both the crit percentages will be calculated somehow from the crit stat
function attack(_attackStat ::Int64, _critStat ::Int64)
  # the chance that a crit will hit
  if rand((1:100)) < _critStat
    crit = true
    attackCritDmg = _attackStat + (_attackStat * (_critStat / 10))
    return attackCritDmg, crit
  else
    return _attackStat_
  end
end

# add chance to dodge the next attack by computer
# percentage to be added comes from the speed stat somehow
# if the dodge fails and the attack hits you, the chance for a dodge resets to the original speed stat
# if you actually dodge the attack you can keep adding to the dodge percentages by using dodge when its your turn.
function dodge(_currentDodgePercentage, _speedStat)
  if rand((1:100)) < _currentDodgePercentage
    dodged = true
    _currentDodgePercentage = _currentDodgePercentage + _speedStat
    return _currentDodgePercentage, dodged
  else
    dodged = false
    _currentDodgePercentage = _speedStat
    return _currentDodgePercentage, dodged_
  end
end # returns true if attack was dodged and returns the new dodge chance. if attack wasnt dodged, returns false and sets dodge chance to default speed stat.

# works like dodge, if you use a defence as your turn it will take some damage away from the attack that hits you. You can stack them but if the attack lands all defence goes away.
function defend(_incommingDamage, _defenceStat, _currentDefence)
  if _incommingDamage == 0
    _currentDefence += _defenceStat
    return _currentDefence
  else
    _incommingDamage -= _defenceStat
    return _incommingDamage
  end
end

max_stars_repo_name: NarutoXY/austroporean-cards

max_issues_repo_name: NarutoXY/austroporean-cards

max_forks_repo_name: NarutoXY/austroporean-cards


text:
type Lattice{A}
    arclists::Vector{Vector{A}}
end

function beamsearch(lat::Lattice, beamsize::Int)

end


text:
#########################################################################
#                                                                       #
# This file provides functions for interfacing with Matlab .m files     #
#                                                                       #
#########################################################################

# this could benefit from a much more robust parser

export parse_matlab_file, parse_matlab_string

function parse_matlab_file(file_string::String; kwargs...)
    data_string = read(open(file_string), String)
    return parse_matlab_string(data_string; kwargs...)
end

function parse_matlab_string(data_string::String; extended = false)
    data_lines = split(data_string, '\n')

    matlab_dict = Dict{String, Any}()
    struct_name = nothing
    function_name = nothing
    column_names = Dict{String, Any}()

    last_index = length(data_lines)
    index = 1
    while index <= last_index
        line = strip(data_lines[index])
        line = "$(line)"

        if length(line) <= 0 || strip(line)[1] == '%'
            index = index + 1
            continue
        end

        if occursin("function", line)
            func, value = _extract_matlab_assignment(line)
            struct_name = strip(replace(func, "function" => ""))
            function_name = value
        elseif occursin("=", line)
            if struct_name != nothing && !occursin("$(struct_name).", line)
                @warn "assignments are expected to be made to \"$(struct_name)\" but given: $(line)"
            end

            if occursin("[", line)
                matrix_dict = _parse_matlab_matrix(data_lines, index)
                matlab_dict[matrix_dict["name"]] = matrix_dict["data"]
                if haskey(matrix_dict, "column_names")
                    column_names[matrix_dict["name"]] = matrix_dict["column_names"]
                end
                index = index + matrix_dict["line_count"] - 1
            elseif occursin("{", line)
                cell_dict = _parse_matlab_cells(data_lines, index)
                matlab_dict[cell_dict["name"]] = cell_dict["data"]
                if haskey(cell_dict, "column_names")
                    column_names[cell_dict["name"]] = cell_dict["column_names"]
                end
                index = index + cell_dict["line_count"] - 1
            else
                name, value = _extract_matlab_assignment(line)
                value = _type_value(value)
                matlab_dict[name] = value
            end
        else
            @error "Matlab parser skipping line number $(index) consisting of:\n  $(line)"
        end

        index += 1
    end

    if extended
        return matlab_dict, function_name, column_names
    else
        return matlab_dict
    end
end

"breaks up matlab strings of the form 'name = value;'"
function _extract_matlab_assignment(string::AbstractString)
    statement = split(string, ';')[1]
    statement_parts = split(statement, '=')
    @assert(length(statement_parts) == 2)
    name = strip(statement_parts[1])
    value = strip(statement_parts[2])
    return name, value
end

"Attempts to determine the type of a string extracted from a matlab file"
function _type_value(value_string::AbstractString)
    value_string = strip(value_string)

    if occursin("'", value_string) # value is a string
        value = strip(value_string, '\'')
    else
        # if value is a float
        if occursin(".", value_string) || occursin("e", value_string)
            value = check_type(Float64, value_string)
        else # otherwise assume it is an int
            value = check_type(Int, value_string)
        end
    end

    return value
end

"Attempts to determine the type of an array of strings extracted from a matlab file"
function _type_array(string_array::Vector{T}) where {T <: AbstractString}
    value_string = [strip(value_string) for value_string in string_array]

    return if any(occursin("'", value_string) for value_string in string_array)
        [strip(value_string, '\'') for value_string in string_array]
    elseif any(
        occursin(".", value_string) || occursin("e", value_string)
        for value_string in string_array
    )
        [check_type(Float64, value_string) for value_string in string_array]
    else # otherwise assume it is an int
        [check_type(Int, value_string) for value_string in string_array]
    end
end

""
_parse_matlab_cells(lines, index) = _parse_matlab_data(lines, index, '{', '}')

""
_parse_matlab_matrix(lines, index) = _parse_matlab_data(lines, index, '[', ']')

""
function _parse_matlab_data(lines, index, start_char, end_char)
    last_index = length(lines)
    line_count = 0
    columns = -1

    @assert(occursin("=", lines[index + line_count]))
    matrix_assignment = split(lines[index + line_count], '%')[1]
    matrix_assignment = strip(matrix_assignment)

    @assert(occursin(".", matrix_assignment))
    matrix_assignment_parts = split(matrix_assignment, '=')
    matrix_name = strip(matrix_assignment_parts[1])

    matrix_assignment_rhs = ""
    if length(matrix_assignment_parts) > 1
        matrix_assignment_rhs = strip(matrix_assignment_parts[2])
    end

    line_count = line_count + 1
    matrix_body_lines = [matrix_assignment_rhs]
    found_close_bracket = occursin(string(end_char), matrix_assignment_rhs)

    while index + line_count < last_index && !found_close_bracket
        line = strip(lines[index + line_count])

        if length(line) == 0 || line[1] == '%'
            line_count += 1
            continue
        end

        line = strip(split(line, '%')[1])

        if occursin(string(end_char), line)
            found_close_bracket = true
        end

        push!(matrix_body_lines, line)

        line_count = line_count + 1
    end

    #print(matrix_body_lines)
    matrix_body_lines =
        [_add_line_delimiter(line, start_char, end_char) for line in matrix_body_lines]
    #print(matrix_body_lines)

    matrix_body = join(matrix_body_lines, ' ')
    matrix_body =
        strip(replace(strip(strip(matrix_body), start_char), "$(end_char);" => ""))
    matrix_body_rows = split(matrix_body, ';')
    matrix_body_rows = matrix_body_rows[1:(length(matrix_body_rows) - 1)]

    matrix = []
    for row in matrix_body_rows
        row_items = split_line(strip(row))
        #println(row_items)
        push!(matrix, row_items)
        if columns < 0
            columns = length(row_items)
        elseif columns != length(row_items)
            @error "matrix parsing error, inconsistent number of items in each row\n$(row)"
        end
    end

    rows = length(matrix)
    typed_columns = [_type_array([matrix[r][c] for r in 1:rows]) for c in 1:columns]
    for r in 1:rows
        matrix[r] = [typed_columns[c][r] for c in 1:columns]
    end

    matrix_dict = Dict("name" => matrix_name, "data" => matrix, "line_count" => line_count)

    if index > 1 && occursin("%column_names%", lines[index - 1])
        column_names_string = lines[index - 1]
        column_names_string = replace(column_names_string, "%column_names%" => "")
        column_names = split(column_names_string)
        if length(matrix[1]) != length(column_names)
            @error "column name parsing error, data rows $(length(matrix[1])), column names $(length(column_names)) \n$(column_names)"
        end
        if any([column_name == "index" for column_name in column_names])
            @error "column name parsing error, \"index\" is a reserved column name \n$(column_names)"
        end
        matrix_dict["column_names"] = column_names
    end

    return matrix_dict
end

const single_quote_expr = r"\'((\\.|[^\'])*?)\'"

""
function split_line(mp_line::AbstractString)
    if occursin(single_quote_expr, mp_line)
        # splits a string on white space while escaping text quoted with "'"
        # note that quotes will be stripped later, when data typing occurs

        #println(mp_line)
        tokens = []
        while length(mp_line) > 0 && occursin(single_quote_expr, mp_line)
            #println(mp_line)
            m = match(single_quote_expr, mp_line)

            if m.offset > 1
                push!(tokens, mp_line[1:(m.offset - 1)])
            end
            push!(tokens, replace(m.match, "\\'" => "'")) # replace escaped quotes

            mp_line = mp_line[(m.offset + length(m.match)):end]
        end
        if length(mp_line) > 0
            push!(tokens, mp_line)
        end
        #println(tokens)

        items = []
        for token in tokens
            if occursin("'", token)
                push!(items, strip(token))
            else
                for parts in split(token)
                    push!(items, strip(parts))
                end
            end
        end
        #println(items)

        #return [strip(mp_line, '\'')]
        return items
    else
        return split(mp_line)
    end
end

""
function _add_line_delimiter(mp_line::AbstractString, start_char, end_char)
    if strip(mp_line) == string(start_char)
        return mp_line
    end

    if !occursin(";", mp_line) && !occursin(string(end_char), mp_line)
        mp_line = "$(mp_line);"
    end

    if occursin(string(end_char), mp_line)
        prefix = strip(split(mp_line, end_char)[1])
        if length(prefix) > 0 && !occursin(";", prefix)
            mp_line = replace(mp_line, end_char => ";$(end_char)")
        end
    end

    return mp_line
end

"Checks if the given value is of a given type, if not tries to make it that type"
function check_type(typ, value)
    if isa(value, typ)
        return value
    elseif isa(value, String) || isa(value, SubString)
        try
            value = parse(typ, value)
            return value
        catch e
            @error "parsing error, the matlab string \"$(value)\" can not be parsed to $(typ) data"
            rethrow(e)
        end
    else
        try
            value = typ(value)
            return value
        catch e
            @error "parsing error, the matlab value $(value) of type $(typeof(value)) can not be parsed to $(typ) data"
            rethrow(e)
        end
    end
end


text:
using SymbolicWedderburn
using PermutationGroups
using Cyclotomics

using DynamicPolynomials
using SumOfSquares
using SCS

@polyvar x[1:4]

let m = SOSModel(SCS.Optimizer)
    @variable m t
    @objective m Max t
    @variable m sos SOSPoly([1; x])
    @constraint m sum(x) + sum(x.^2) - t == sos
    optimize!(m)
end

# @constraint m  sum(x.^2) - t in SOSCone() symmetry_group = C2
using DynamicPolynomials
using MultivariatePolynomials
const MP = MultivariatePolynomials
using MultivariateBases

function SymbolicWedderburn.OnPoints(basis::Union{MonomialVector, AbstractVector{<:Monomial}})
    basis_exps = Vector{Vector{Int}}(undef, length(basis))
    basis_dict = Dict{Vector{Int}, Int}()
    sizehint!(basis_dict, length(basis))

    for (i, b) in enumerate(basis)
        e = MP.exponents(b) # so that we allocate exponents only once
        basis_exps[i] = e
        basis_dict[e] = i
    end

    return SymbolicWedderburn.OnPoints(basis_exps, basis_dict)
end

include(joinpath(@__DIR__, "..", "test", "smallgroups.jl"));
G = SmallPermGroups[4][1]
# G = PermGroup([perm"(1,2)", perm"(1,2,3,4)"])
chars_vars = SymbolicWedderburn.characters_dixon(G)

mvec = monomials(x, 0:1)

chars_mvec = let chars = chars_vars, basis = mvec

    @assert all(χ.inv_of == first(chars).inv_of for χ in chars)

    induced_action = SymbolicWedderburn.OnPoints(basis)
    ccG_large = induced_action.(conjugacy_classes(first(chars)))

    # double check:
    let ccls = ccG_large, large_gens = induced_action.(gens(G))
        G_large = PermGroup(large_gens)
        ccG_large = conjugacy_classes(G_large)
        @assert all(Set.(collect.(ccG_large)) .== Set.(collect.(ccls)))
    end

    [SymbolicWedderburn.Character(values(χ), χ.inv_of, ccG_large) for χ in chars]
end

vr_chars = SymbolicWedderburn.real_vchars(chars_mvec)

U = filter!(x->all(!iszero, x), [SymbolicWedderburn.matrix_projection(χ) for χ in vr_chars])

R = map(U) do c_u
    u = last(c_u)
    if all(isreal, u)
        image_coeffs, pivots = SymbolicWedderburn.row_echelon_form(float.(u))
        dim = length(pivots)
        image_coeffs[1:dim, :]
    else
        throw("Not Implemented")
    end
end

R = filter!(!iszero ∘ first ∘ size, R)

msym = let msym = SOSModel(SCS.Optimizer)
    @variable   msym t
    @objective  msym Max t

    @variable   msym sos1 SOSPoly(FixedPolynomialBasis(R[1]*mvec))
    @variable   msym sos2 SOSPoly(FixedPolynomialBasis(R[2]*mvec))
    @variable   msym sos3 SOSPoly(FixedPolynomialBasis(R[3]*mvec))
    # @variable   msym sos4
    @constraint msym sum(x) + sum(x.^2) - t == sos1 + sos2 + sos3
    optimize!(msym)
    msym
end

termination_status(msym)

value.(msym[:sos1].Q)

value.(msym[:sos2].Q)

value.(msym[:sos3].Q)


text:
module NIfTI_AFNI

using NIfTI,LightXML

export isafni,AFNIExtension,NIfTI1Extension

"""isafni(e::NIfTI.NIfTI1Extension)

Is this an AFNI Extension?"""
isafni(e::NIfTI.NIfTI1Extension) = e.ecode==4

"""parse_quotedstrings(a::AbstractString)

A simple function to parse apart the strings in AFNI NIfTI Extensions"""
function parse_quotedstrings(a::AbstractString)
    strings = SubString[]
    stringstart = 0
    stringend = 0

    i = 1
    while i<=length(a)
        a[i] == '\\' && (i += 2; continue)

        if a[i]=='"'
            if stringstart==0
                stringstart = i+1
                stringend = 0
            elseif stringend==0
                push!(strings,SubString(a,stringstart,i-1))
                stringstart = 0
            end
        end
        i += 1
    end
    strings
end

mutable struct AFNIExtension
    ecode::Int32
    edata::Vector{UInt8}

    raw_xml::String
    header::Dict{String,Any}
end

function AFNIExtension(e::NIfTI.NIfTI1Extension)
    isafni(e) || error("Trying to convert an unknown NIfTIExtension to AFNIExtension")

    edata = copy(e.edata)
    raw_xml = String(edata)

    xdoc = parse_string(raw_xml)
    xroot = root(xdoc)

    header_dict = Dict{String,Any}()

    for atr in xroot["AFNI_atr"]
        t = attribute(atr,"ni_type")
        n = attribute(atr,"atr_name")

        if t=="String"
            header_dict[n] = parse_quotedstrings(content(atr))
        elseif t=="int"
            header_dict[n] = parse.(Int,split(content(atr)))
        elseif t=="float"
            header_dict[n] = parse.(Float64,split(content(atr)))
        end

        isa(header_dict[n],AbstractArray) && length(header_dict[n])==1 && (header_dict[n] = header_dict[n][1])
        if n == "BRICK_LABS"
            isa(header_dict[n],AbstractArray) && (header_dict[n] = join(header_dict[n]))
            header_dict[n] = split(header_dict[n],"~")
        end
        n == "BRICK_STATSYM" && (header_dict[n] = split(header_dict[n],";"))
    end

    free(xdoc)
    AFNIExtension(e.ecode,e.edata,raw_xml,header_dict)
end

NIfTI1Extension(a::AFNIExtension) = NIfTI.NIfTI1Extension(a.ecode,a.edata)

end # module