# Formulas for representing and working with linear-model-type expressions
# Original by Harlan D. Harris.  Later modifications by John Myles White
# and Douglas M. Bates.

## Formulas are written as expressions and parsed by the Julia parser.
## For example :(y ~ a + b + log(c))
## In Julia the & operator is used for an interaction.  What would be written
## in R as y ~ a + b + a:b is written :(y ~ a + b + a&b) in Julia.
## The equivalent R expression, y ~ a*b, is the same in Julia

## The lhs of a one-sided formula is 'nothing'
## The rhs of a formula can be 1

type Formula
    lhs::@compat(Union{Symbol, Expr, Void})
    rhs::@compat(Union{Symbol, Expr, Integer})
end

macro ~(lhs, rhs)
    ex = Expr(:call,
              :Formula,
              Base.Meta.quot(lhs),
              Base.Meta.quot(rhs))
    return ex
end

#
# TODO: implement contrast types in Formula/Terms
#
type Terms
    terms::Vector
    eterms::Vector        # evaluation terms
    factors::Matrix{Bool} # maps terms to evaluation terms
    ## An eterms x terms matrix which is true for terms that need to be "promoted"
    ## to full rank in constructing a model matrx
    is_non_redundant::Matrix{Bool}
# order can probably be dropped.  It is vec(sum(factors, 1))
    order::Vector{Int}    # orders of rhs terms
    response::Bool        # indicator of a response, which is eterms[1] if present
    intercept::Bool       # is there an intercept column in the model matrix?
end

type ModelFrame
    df::AbstractDataFrame
    terms::Terms
    msng::BitArray
    ## mapping from df keys to contrasts matrices
    contrasts::Dict{Symbol, ContrastsMatrix}
end

typealias AbstractFloatMatrix{T<:AbstractFloat} AbstractMatrix{T}

type ModelMatrix{T <: AbstractFloatMatrix}
    m::T
    assign::Vector{Int}
end

Base.size(mm::ModelMatrix) = size(mm.m)
Base.size(mm::ModelMatrix, dim...) = size(mm.m, dim...)

function Base.show(io::IO, f::Formula)
    print(io,
          string("Formula: ",
                 f.lhs == nothing ? "" : f.lhs, " ~ ", f.rhs))
end

## Return, as a vector of symbols, the names of all the variables in
## an expression or a formula
function allvars(ex::Expr)
    if ex.head != :call error("Non-call expression encountered") end
    cc=Symbol[]
    for i in ex.args[2:end] cc=append!(cc,allvars(i)) end
    cc
end
allvars(f::Formula) = unique(vcat(allvars(f.rhs), allvars(f.lhs)))
allvars(sym::Symbol) = [sym]
allvars(v::Any) = Array(Symbol, 0)

# special operators in formulas
const specials = Set([:+, :-, :*, :/, :&, :|, :^])

function dospecials(ex::Expr)
    if ex.head != :call error("Non-call expression encountered") end
    a1 = ex.args[1]
    if !(a1 in specials) return ex end
    excp = copy(ex)
    excp.args = vcat(a1,map(dospecials, ex.args[2:end]))
    if a1 != :* return excp end
    aa = excp.args
    a2 = aa[2]
    a3 = aa[3]
    if length(aa) > 3
        excp.args = vcat(a1, aa[3:end])
        a3 = dospecials(excp)
    end
    ## this order of expansion gives the R-style ordering of interaction
    ## terms (after sorting in increasing interaction order) for higher-
    ## order interaction terms (e.g. x1 * x2 * x3 should expand to x1 +
    ## x2 + x3 + x1&x2 + x1&x3 + x2&x3 + x1&x2&x3)
    :($a2 + $a2 & $a3 + $a3)
end
dospecials(a::Any) = a

## Distribution of & over +
const distributive = @compat Dict(:& => :+)

distribute(ex::Expr) = distribute!(copy(ex))
distribute(a::Any) = a
## apply distributive property in-place
function distribute!(ex::Expr)
    if ex.head != :call error("Non-call expression encountered") end
    [distribute!(a) for a in ex.args[2:end]]
    ## check that top-level can be distributed
    a1 = ex.args[1]
    if a1 in keys(distributive)

        ## which op is being DISTRIBUTED (e.g. &, *)?
        distributed_op = a1
        ## which op is doing the distributing (e.g. +)?
        distributing_op = distributive[a1]

        ## detect distributing sub-expression (first arg is, e.g. :+)
        is_distributing_subex(e) =
            typeof(e)==Expr && e.head == :call && e.args[1] == distributing_op

        ## find first distributing subex
        first_distributing_subex = findfirst(is_distributing_subex, ex.args)
        if first_distributing_subex != 0
            ## remove distributing subexpression from args
            subex = splice!(ex.args, first_distributing_subex)

            newargs = Any[distributing_op]
            ## generate one new sub-expression, which calls the distributed operation
            ## (e.g. &) on each of the distributing sub-expression's arguments, plus
            ## the non-distributed arguments of the original expression.
            for a in subex.args[2:end]
                new_subex = copy(ex)
                push!(new_subex.args, a)
                ## need to recurse here, in case there are any other
                ## distributing operations in the sub expression
                distribute!(new_subex)
                push!(newargs, new_subex)
            end
            ex.args = newargs
        end
    end
    ex
end
distribute!(a::Any) = a

const associative = Set([:+,:*,:&])       # associative special operators

## If the expression is a call to the function s return its arguments
## Otherwise return the expression
function ex_or_args(ex::Expr,s::Symbol)
    if ex.head != :call error("Non-call expression encountered") end
    if ex.args[1] == s
        ## recurse in case there are more :calls of s below
        return vcat(map(x -> ex_or_args(x, s), ex.args[2:end])...)
    else
        ## not a :call to s, return condensed version of ex
        return condense(ex)
    end
end
ex_or_args(a,s::Symbol) = a

## Condense calls like :(+(a,+(b,c))) to :(+(a,b,c))
function condense(ex::Expr)
    if ex.head != :call error("Non-call expression encountered") end
    a1 = ex.args[1]
    if !(a1 in associative) return ex end
    excp = copy(ex)
    excp.args = vcat(a1, map(x->ex_or_args(x,a1), ex.args[2:end])...)
    excp
end
condense(a::Any) = a

## always return an ARRAY of terms
getterms(ex::Expr) = (ex.head == :call && ex.args[1] == :+) ? ex.args[2:end] : Expr[ex]
getterms(a::Any) = Any[a]

ord(ex::Expr) = (ex.head == :call && ex.args[1] == :&) ? length(ex.args)-1 : 1
ord(a::Any) = 1

const nonevaluation = Set([:&,:|])        # operators constructed from other evaluations
## evaluation terms - the (filtered) arguments for :& and :|, otherwise the term itself
function evt(ex::Expr)
    if ex.head != :call error("Non-call expression encountered") end
    if !(ex.args[1] in nonevaluation) return ex end
    filter(x->!isa(x,Number), vcat(map(getterms, ex.args[2:end])...))
end
evt(a) = Any[a]

function Terms(f::Formula)
    rhs = condense(distribute(dospecials(f.rhs)))
    tt = unique(getterms(rhs))
    tt = tt[!(tt .== 1)]             # drop any explicit 1's
    noint = (tt .== 0) | (tt .== -1) # should also handle :(-(expr,1))
    tt = tt[!noint]
    oo = Int[ord(t) for t in tt]     # orders of interaction terms
    if !issorted(oo)                 # sort terms by increasing order
        pp = sortperm(oo)
        tt = tt[pp]
        oo = oo[pp]
    end
    etrms = map(evt, tt)
    haslhs = f.lhs != nothing
    if haslhs
        unshift!(etrms, Any[f.lhs])
        unshift!(oo, 1)
    end
    ev = unique(vcat(etrms...))
    sets = [Set(x) for x in etrms]
    facs = Bool[t in s for t in ev, s in sets]
    non_redundants = fill(false, size(facs)) # initialize to false
    Terms(tt, ev, facs, non_redundants, oo, haslhs, !any(noint))
end

## Default NA handler.  Others can be added as keyword arguments
function na_omit(df::DataFrame)
    cc = complete_cases(df)
    df[cc,:], cc
end

## Trim the pool field of da to only those levels that occur in the refs
function dropunusedlevels!(da::PooledDataArray)
    rr = da.refs
    uu = unique(rr)
    length(uu) == length(da.pool) && return da
    T = eltype(rr)
    su = sort!(uu)
    dict = Dict(zip(su, one(T):convert(T, length(uu))))
    da.refs = map(x -> dict[x], rr)
    da.pool = da.pool[uu]
    da
end
dropunusedlevels!(x) = x

is_categorical(::PooledDataArray) = true
is_categorical(::Any) = false

## Check for non-redundancy of columns.  For instance, if x is a factor with two
## levels, it should be expanded into two columns in y~0+x but only one column
## in y~1+x.  The default is the rank-reduced form (contrasts for n levels only
## produce n-1 columns).  In general, an evaluation term x within a term
## x&y&... needs to be "promoted" to full rank if y&... hasn't already been
## included (either explicitly in the Terms or implicitly by promoting another
## term like z in z&y&...).
##
## This modifies the Terms, setting `trms.is_non_redundant = true` for all non-
## redundant evaluation terms.
function check_non_redundancy!(trms::Terms, df::AbstractDataFrame)

    (n_eterms, n_terms) = size(trms.factors)

    encountered_columns = Vector{eltype(trms.factors)}[]

    if trms.intercept
        push!(encountered_columns, zeros(eltype(trms.factors), n_eterms))
    end

    for i_term in 1:n_terms
        for i_eterm in 1:n_eterms
            ## only need to check eterms that are included and can be promoted
            ## (e.g., categorical variables that expand to multiple mm columns)
            if Bool(trms.factors[i_eterm, i_term]) && is_categorical(df[trms.eterms[i_eterm]])
                dropped = trms.factors[:,i_term]
                dropped[i_eterm] = 0

                if findfirst(encountered_columns, dropped) == 0
                    trms.is_non_redundant[i_eterm, i_term] = true
                    push!(encountered_columns, dropped)
                end

            end
        end
        ## once we've checked all the eterms in this term, add it to the list
        ## of encountered terms/columns
        push!(encountered_columns, Compat.view(trms.factors, :, i_term))
    end

    return trms.is_non_redundant
end

const DEFAULT_CONTRASTS = DummyCoding

## Set up contrasts:
## Combine actual DF columns and contrast types if necessary to compute the
## actual contrasts matrices, levels, and term names (using DummyCoding
## as the default)
function evalcontrasts(df::AbstractDataFrame, contrasts::Dict = Dict())
    evaledContrasts = Dict()
    for (term, col) in eachcol(df)
        is_categorical(col) || continue
        evaledContrasts[term] = ContrastsMatrix(haskey(contrasts, term) ?
                                                contrasts[term] :
                                                DEFAULT_CONTRASTS(),
                                                col)
    end
    return evaledContrasts
end

function ModelFrame(trms::Terms, d::AbstractDataFrame;
                    contrasts::Dict = Dict())
    df, msng = na_omit(DataFrame(map(x -> d[x], trms.eterms)))
    names!(df, convert(Vector{Symbol}, map(string, trms.eterms)))
    for c in eachcol(df) dropunusedlevels!(c[2]) end

    evaledContrasts = evalcontrasts(df, contrasts)

    ## Check for non-redundant terms, modifying terms in place
    check_non_redundancy!(trms, df)

    ModelFrame(df, trms, msng, evaledContrasts)
end

ModelFrame(df::AbstractDataFrame, term::Terms, msng::BitArray) = ModelFrame(df, term, msng, evalcontrasts(df))
ModelFrame(f::Formula, d::AbstractDataFrame; kwargs...) = ModelFrame(Terms(f), d; kwargs...)
ModelFrame(ex::Expr, d::AbstractDataFrame; kwargs...) = ModelFrame(Formula(ex), d; kwargs...)

## modify contrasts in place
function setcontrasts!(mf::ModelFrame, new_contrasts::Dict)
    new_contrasts = Dict([ Pair(col, ContrastsMatrix(contr, mf.df[col]))
                      for (col, contr) in filter((k,v)->haskey(mf.df, k), new_contrasts) ])

    mf.contrasts = merge(mf.contrasts, new_contrasts)
    return mf
end
setcontrasts!(mf::ModelFrame; kwargs...) = setcontrasts!(mf, Dict(kwargs))

"""
    StatsBase.model_response(mf::ModelFrame)
Extract the response column, if present.  `DataVector` or
`PooledDataVector` columns are converted to `Array`s
"""
function StatsBase.model_response(mf::ModelFrame)
    if mf.terms.response
        convert(Array, mf.df[mf.terms.eterms[1]])
    else
        error("Model formula one-sided")
    end
end

## construct model matrix columns from model frame + name (checks for contrasts)
function modelmat_cols{T<:AbstractFloatMatrix}(::Type{T}, name::Symbol, mf::ModelFrame; non_redundant::Bool = false)
    if haskey(mf.contrasts, name)
        modelmat_cols(T, mf.df[name],
                      non_redundant ?
                      ContrastsMatrix{FullDummyCoding}(mf.contrasts[name]) :
                      mf.contrasts[name])
    else
        modelmat_cols(T, mf.df[name])
    end
end

modelmat_cols{T<:AbstractFloatMatrix}(::Type{T}, v::DataVector) = convert(T, reshape(v.data, length(v), 1))
modelmat_cols{T<:AbstractFloatMatrix}(::Type{T}, v::Vector) = convert(T, reshape(v, length(v), 1))

"""
    modelmat_cols{T<:AbstractFloatMatrix}(::Type{T}, v::PooledDataVector, contrast::ContrastsMatrix)

Construct `ModelMatrix` columns of type `T` based on specified contrasts, ensuring that
levels align properly.
"""
function modelmat_cols{T<:AbstractFloatMatrix}(::Type{T}, v::PooledDataVector, contrast::ContrastsMatrix)
    ## make sure the levels of the contrast matrix and the categorical data
    ## are the same by constructing a re-indexing vector. Indexing into
    ## reindex with v.refs will give the corresponding row number of the
    ## contrast matrix
    reindex = [findfirst(contrast.levels, l) for l in levels(v)]
    contrastmatrix = convert(T, contrast.matrix)
    return indexrows(contrastmatrix, reindex[v.refs])
end

indexrows(m::SparseMatrixCSC, ind::Vector{Int}) = m'[:, ind]'
indexrows(m::AbstractMatrix, ind::Vector{Int}) = m[ind, :]

"""
    expandcols{T<:AbstractFloatMatrix}(trm::Vector{T})
Create pairwise products of columns from a vector of matrices
"""
function expandcols{T<:AbstractFloatMatrix}(trm::Vector{T})
    if length(trm) == 1
        trm[1]
    else
        a = trm[1]
        b = expandcols(trm[2 : end])
        reduce(hcat, [broadcast(*, a, Compat.view(b, :, j)) for j in 1 : size(b, 2)])
    end
end

"""
    droprandomeffects(trms::Terms)
Expressions of the form `(a|b)` are "random-effects" terms and are not
incorporated in the ModelMatrix.  This function checks for such terms and,
if any are present, drops them from the `Terms` object.
"""
function droprandomeffects(trms::Terms)
    retrms = Bool[Meta.isexpr(t, :call) && t.args[1] == :| for t in trms.terms]
    if !any(retrms)  # return trms unchanged
        trms
    elseif all(retrms) && !trms.response   # return an empty Terms object
        Terms(Any[],Any[],Array(Bool, (0,0)),Array(Bool, (0,0)), Int[], false, trms.intercept)
    else
        # the rows of `trms.factors` correspond to `eterms`, the columns to `terms`
        # After dropping random-effects terms we drop any eterms whose rows are all false
        ckeep = !retrms                 # columns to retain
        facs = trms.factors[:, ckeep]
        rkeep = vec(sum(facs, 2) .> 0)
        Terms(trms.terms[ckeep], trms.eterms[rkeep], facs[rkeep, :],
              trms.is_non_redundant[rkeep, ckeep],
              trms.order[ckeep], trms.response, trms.intercept)
    end
end

"""
    dropresponse!(trms::Terms)
Drop the response term, `trms.eterms[1]` and the first row and column
of `trms.factors` if `trms.response` is true.
"""
function dropresponse!(trms::Terms)
    if trms.response
        ckeep = 2:size(trms.factors, 2)
        rkeep = vec(any(trms.factors[:, ckeep], 2))
        Terms(trms.terms, trms.eterms[rkeep], trms.factors[rkeep, ckeep],
              trms.is_non_redundant[rkeep, ckeep], trms.order[ckeep], false, trms.intercept)
    else
        trms
    end
end

"""
    ModelMatrix{T<:AbstractFloatMatrix}(mf::ModelFrame)
Create a `ModelMatrix` of type `T` (default `Matrix{Float64}`) from the
`terms` and `df` members of `mf`.

This is basically a map-reduce where terms are mapped to columns by `cols`
and reduced by `hcat`.  During the collection of the columns the `assign`
vector is created.  `assign` maps columns of the model matrix to terms in
the model frame.  It can also be considered as mapping coefficients to
terms and, hence, to names.

If there is an intercept in the model, that column occurs first and its
`assign` value is zero.

Mixed-effects models include "random-effects" terms which are ignored when
creating the model matrix.
"""
@compat function (::Type{ModelMatrix{T}}){T<:AbstractFloatMatrix}(mf::ModelFrame)
    dfrm = mf.df
    terms = droprandomeffects(dropresponse!(mf.terms))

    blocks = T[]
    assign = Int[]
    if terms.intercept
        push!(blocks, ones(size(dfrm, 1), 1))  # columns of 1's is first block
        push!(assign, 0)                       # this block corresponds to term zero
    end

    factors = terms.factors

    ## Map eval. term name + redundancy bool to cached model matrix columns
    eterm_cols = @compat Dict{Tuple{Symbol,Bool}, T}()
    ## Accumulator for each term's vector of eval. term columns.

    ## TODO: this method makes multiple copies of the data in the ModelFrame:
    ## first in term_cols (1-2x per evaluation term, depending on redundancy),
    ## second in constructing the matrix itself.

    ## turn each term into a vector of mm columns for its eval. terms, using
    ## "promoted" full-rank versions of categorical columns for non-redundant
    ## eval. terms:
    for (i_term, term) in enumerate(terms.terms)
        term_cols = T[]
        ## Pull out the eval terms, and the non-redundancy flags for this term
        ff = Compat.view(factors, :, i_term)
        eterms = Compat.view(terms.eterms, ff)
        non_redundants = Compat.view(terms.is_non_redundant, ff, i_term)
        ## Get cols for each eval term (either previously generated, or generating
        ## and storing as necessary)
        for (et, nr) in zip(eterms, non_redundants)
            if ! haskey(eterm_cols, (et, nr))
                eterm_cols[(et, nr)] = modelmat_cols(T, et, mf, non_redundant=nr)
            end
            push!(term_cols, eterm_cols[(et, nr)])
        end
        push!(blocks, expandcols(term_cols))
        append!(assign, fill(i_term, size(blocks[end], 2)))
    end

    if isempty(blocks)
        error("Could not construct model matrix. Resulting matrix has 0 columns.")
    end

    I = size(dfrm, 1)
    J = mapreduce(x -> size(x, 2), +, blocks)
    X = similar(blocks[1], I, J)
    i = 1
    for block in blocks
        len = size(block, 2)
        X[:, i:(i + len - 1)] = block
        i += len
    end
    ModelMatrix{T}(X, assign)
end
ModelMatrix(mf::ModelFrame) = ModelMatrix{Matrix{Float64}}(mf)

"""
    termnames(term::Symbol, col)
Returns a vector of strings with the names of the coefficients
associated with a term.  If the column corresponding to the term
is not a `PooledDataArray` a one-element vector is returned.
"""
termnames(term::Symbol, col) = [string(term)]
function termnames(term::Symbol, mf::ModelFrame; non_redundant::Bool = false)
    if haskey(mf.contrasts, term)
        termnames(term, mf.df[term],
                  non_redundant ?
                  ContrastsMatrix{FullDummyCoding}(mf.contrasts[term]) :
                  mf.contrasts[term])
    else
        termnames(term, mf.df[term])
    end
end

termnames(term::Symbol, col::Any, contrast::ContrastsMatrix) =
    ["$term: $name" for name in contrast.termnames]

function expandtermnames(term::Vector)
    if length(term) == 1
        return term[1]
    else
        return foldr((a,b) -> vec([string(lev1, " & ", lev2) for
                                   lev1 in a,
                                   lev2 in b]),
                     term)
    end
end

"""
    coefnames(mf::ModelFrame)
Returns a vector of coefficient names constructed from the Terms
member and the types of the evaluation columns.
"""
function coefnames(mf::ModelFrame)
    terms = droprandomeffects(dropresponse!(mf.terms))

    ## strategy mirrors ModelMatrx constructor:
    eterm_names = @compat Dict{Tuple{Symbol,Bool}, Vector{Compat.UTF8String}}()
    term_names = Vector{Compat.UTF8String}[]

    if terms.intercept
        push!(term_names, Compat.UTF8String["(Intercept)"])
    end

    factors = terms.factors

    for (i_term, term) in enumerate(terms.terms)

        ## names for columns for eval terms
        names = Vector{Compat.UTF8String}[]

        ff = Compat.view(factors, :, i_term)
        eterms = Compat.view(terms.eterms, ff)
        non_redundants = Compat.view(terms.is_non_redundant, ff, i_term)

        for (et, nr) in zip(eterms, non_redundants)
            if !haskey(eterm_names, (et, nr))
                eterm_names[(et, nr)] = termnames(et, mf, non_redundant=nr)
            end
            push!(names, eterm_names[(et, nr)])
        end
        push!(term_names, expandtermnames(names))
    end

    reduce(vcat, Vector{Compat.UTF8String}(), term_names)
end

function Formula(t::Terms)
    lhs = t.response ? t.eterms[1] : nothing
    rhs = Expr(:call,:+)
    if t.intercept
        push!(rhs.args,1)
    end
    append!(rhs.args,t.terms)
    Formula(lhs,rhs)
end


import Cairo #, Fontconfig
using GraphPlottingBFS
using Compose
using Colors
using Gadfly
using DataFrames
using GraphUtils

using PGFPlotsX
latexengine!(PGFPlotsX.PDFLATEX)
using PGFPlots
using Printf
using Parameters
using Measures
using LightGraphs
using MetaGraphs

ROBOT_COLOR = RGB(0.1,0.5,0.9)

interpolate(a,b,t) = (1 - t)*a + t*b

function interp_position(path::Vector{Int},vtxs,t)
    t1 = Int(floor(t))+1
    idx1 = path[max(1,min(t1,length(path)))]
    idx2 = path[max(1,min(t1+1,length(path)))]
    x=interpolate(vtxs[idx1][1],vtxs[idx2][1],t-(t1-1))
    y=interpolate(vtxs[idx1][2],vtxs[idx2][2],t-(t1-1))
    return x,y
end
function interp_position(paths::Vector{Vector{Int}},vtxs,t)
    coords = map(p->interp_position(p,vtxs,t),paths)
end
function interp_path(path::Vector{Int},vtxs,t)
    x,y = interp_position(path,vtxs,t)
    t1 = Int(floor(t))+1
    idx1 = max(1,min(t1,length(path)))
    idx2 = max(1,min(t1+1,length(path)))
    xpath = [x, map(v->v[1],vtxs[path[idx2:end]])...]
    ypath = [y, map(v->v[2],vtxs[path[idx2:end]])...]
    return xpath,ypath
end

function gen_object_colors(n)
  cs = distinguishable_colors(n,
      [colorant"#FE4365", colorant"#eca25c"], # seed colors
      lchoices=Float64[58, 45, 72.5, 90],     # lightness choices
      transform=c -> deuteranopic(c, 0.1),    # color transform
      cchoices=Float64[20,40],                # chroma choices
      hchoices=[75,51,35,120,180,210,270,310] # hue choices
  )

  convert(Vector{Color}, cs)
end

function record_video(outfile_name,render_function,
        save_function = (t,path,s)-> render_function(t) |> PNG(path,s[1],s[2])
    ;
        t0 = 0,
        tf = 10,
        dt = 0.25,
        t_history=t0:dt:tf,
        fps = 10,
        ext = "png",
        s = (4inch, 4inch),
        res = "1080x1080",
    )
    tmpdir = mktempdir()
    for (i,t) in enumerate(t_history)
        filename = joinpath(tmpdir,string(i,".",ext))
        # render_function(t) |> PNG(filename, s[1], s[2])
        save_function(t,filename,s)# |> PNG(filename, s[1], s[2])
    end
    outfile = outfile_name
    run(pipeline(`ffmpeg -y -r $fps -f image2 -i $tmpdir/%d.$ext -crf 20 -s $res $outfile`, stdout=devnull, stderr=devnull))
    run(pipeline(`rm -rf $tmpdir`, stdout=devnull, stderr=devnull))
end

function get_grid_world_layer(vtxs,color,cw)
    if length(vtxs) > 0
        return layer(
            xmin=map(vec->vec[1], vtxs) .- cw,
            ymin=map(vec->vec[2], vtxs) .- cw,
            xmax=map(vec->vec[1], vtxs) .+ cw,
            ymax=map(vec->vec[2], vtxs) .+ cw,
            Geom.rect, Theme(default_color=color) )
    else
        return layer(x=[],y=[])
    end
end

@with_kw struct SolutionSummary
    robot_paths::Vector{Vector{Int}} = Vector{Vector{Int}}()
    object_paths::Dict{Int,Vector{Int}} = Dict{Int,Vector{Int}}()
    object_intervals::Dict{Int,Tuple{Int,Int}} = Dict{Int,Tuple{Int,Int}}()
end
function SolutionSummary(env::SearchEnv)
    t0 = 0
    tf = maximum(map(length,get_paths(get_route_plan(env))))
    history = map(t->get_env_state(env,t),t0:tf)
    robot_ids = sort(collect(keys(get_robot_ICs(get_schedule(env)))))
    object_ids = sort(collect(keys(get_object_ICs(get_schedule(env)))))
    robot_paths = map(
        id->map(s->get_id(get_location_id(s.robot_positions[id])),history),
        robot_ids)
    object_paths = Dict(
        get_id(id)=>map(s->get_id(get_location_id(get(object_positions(s),id,OBJECT_AT(id,-1)))),
        history) for id in object_ids)
    active_objects = Dict(
        get_id(id)=>map(s->get(objects_active(s),id,false),history) for id in object_ids)
    object_intervals = Dict(
        k=>(findfirst(v)-1,findlast(v)-1) for (k,v) in active_objects)
    SolutionSummary(robot_paths,object_paths,object_intervals)
end
function get_robot_positions(env,env_summary,t)
    vec = Vector{Point3{Float64}}()
    for (i,p) in enumerate(env_summary.robot_paths)
        if length(p) > 0
            x,y = interp_position(p,get_vtxs(env),t)
            push!(vec,Point(x,y,0.0))
        end
    end
    return vec
end
function get_object_positions(env,env_summary,t)
    object_paths = env_summary.object_paths
    object_intervals = env_summary.object_intervals
    vec = Vector{Point3{Float64}}()
    for (i,k) in enumerate(sort(collect(keys(object_paths)))) # dictionary
        p = object_paths[k]
        if length(p) > 0
            x,y = interp_position(p,get_vtxs(env),t)
            push!(vec,Point(x,y,0.0))
        end
    end
    return vec
end

abstract type RenderModel end
struct RenderStack{T}
    models::T
end
@with_kw mutable struct RobotPositions <: RenderModel
    label_robots    ::Bool              = true
    colors_vec      ::Vector{Color}     = Color[]
    rsize           ::Measures.Length   = 5pt
    line_width      ::Measures.Length   = 2pt
    show_paths      ::Bool              = false
end
function RobotPositions(n::Int)
    color_scale = Scale.color_discrete_hue()
    RobotPositions(colors_vec = color_scale.f(n))
end
@with_kw mutable struct ObjectPositions <: RenderModel
    label_objects           ::Bool          = true
    show_inactive_objects   ::Bool          = true
    # show_completed_objects   ::Bool          = true
    osize                   ::Measures.Length = 5pt
    inactive_object_colors  ::Vector{Color} = Color[]
    completed_object_colors ::Vector{Color} = Color[]
    active_object_colors    ::Vector{Color} = Color[]
    object_sizes            ::Vector{Tuple{Int,Int}} = Tuple{Int,Int}[]
end
function ObjectPositions(n::Int)
    ObjectPositions(
        active_object_colors = map(i->colorant"Black",1:n),
        inactive_object_colors = map(i->colorant"Gray",1:n),
        completed_object_colors = map(i->colorant"Gray",1:n),
        object_sizes            = map(i->(1,1),1:n)
    )
end
@with_kw mutable struct Floor <: RenderModel
    cell_width      ::Float64   = 0.9
    floor_color     ::Color     = colorant"LightGray"
    pickup_color    ::Color     = colorant"LightBlue"
    dropoff_color   ::Color     = colorant"Pink"
end
function render_layer(model::RobotPositions,env::GridFactoryEnvironment,summary,t,theme=Theme())
    robot_paths = summary.robot_paths
    vtxs = get_vtxs(env)
    robot_layers = []
    path_layers = []
    for (i,p) in enumerate(robot_paths)
        if length(p) > 0
            x,y = interp_position(p,vtxs,t)
            label = model.label_robots ? string("R",i) : ""
            push!(robot_layers,
                layer(
                    x=[x],
                    y=[y],
                    label=[label],
                    Geom.label(position=:centered,hide_overlaps=false),
                    Geom.point,
                    size=[model.rsize],
                    Theme(
                        theme,
                        default_color=model.colors_vec[i],
                        # highlight_width=0pt,
                        # point_label_font=point_label_font,
                        # point_label_color=point_label_color,
                        # point_label_font_size=point_label_font_size
                        )
                    )
            )
            if model.show_paths
                xpath,ypath = interp_path(p,vtxs,t)
                push!(path_layers,
                    layer(x=xpath,y=ypath,Geom.path,
                        Theme(theme,line_width=model.line_width,default_color=model.colors_vec[i])
                    )
                )
            end
        end
    end
    return [robot_layers..., path_layers...]
end
function render_layer(model::ObjectPositions,env::GridFactoryEnvironment,summary,t,theme=Theme())
    object_paths = summary.object_paths
    object_intervals = summary.object_intervals
    object_layers = []
    for (i,k) in enumerate(sort(collect(keys(object_paths)))) # dictionary
        p = object_paths[k]
        interval = object_intervals[k]
        if interval[1] > t
            object_color = model.inactive_object_colors[i]
        elseif interval[2] < t
            object_color = model.completed_object_colors[i]
        else
            object_color = model.active_object_colors[i]
        end
        if length(p) > 0 && interval[2] > t
            if (interval[1] <= t) #|| model.show_inactive_objects
                x,y = interp_position(p,get_vtxs(env),t)
                label = model.label_objects ? string("O",k) : ""
                if model.object_sizes[i] == (1,1)
                    push!(object_layers,layer(
                            x=[x],
                            y=[y],
                            label=[label],
                            Geom.label(position=:centered,hide_overlaps=false),
                            Geom.point,
                            size=[model.osize],
                            Theme(theme,default_color=object_color,)
                            ))
                else
                    x = [x]
                    y = [y]
                    s = model.object_sizes[i]
                    if s == (2,1)
                        push!(x,x[1] + env.cell_width)
                        push!(y,y[1])
                    elseif s == (1,2)
                        push!(x,x[1])
                        push!(y,y[1] + env.cell_width)
                    elseif s == (2,2)
                        append!(x,[
                            x[1] + env.cell_width,
                            x[1] + env.cell_width,
                            x[1],
                            x[1],
                            ])
                        append!(y,[
                            y[1],
                            y[1] + env.cell_width,
                            y[1] + env.cell_width,
                            y[1],
                            ])
                    end
                    push!(object_layers,layer(x=x,y=y,
                        label=[label,map(j->"",prod(s)-1)],
                        Geom.label(position=:centered,hide_overlaps=false),
                            Geom.path,Geom.point,size=[model.osize],
                            Theme(theme,
                            default_color=object_color,
                            line_width=model.osize)
                            ))
                end
            end
        end
    end
    return object_layers
end
function render_layer(model::Floor,env::GridFactoryEnvironment,summary,t,theme=Theme())
    map(args->get_grid_world_layer(args...,model.cell_width/2.0),
        [
            (get_pickup_vtxs(env),  model.pickup_color),
            (get_dropoff_vtxs(env), model.dropoff_color),
            (get_vtxs(env),         model.floor_color),
        ]
    )
end

function render_env(rstack::RenderStack,env,summary,t;
        cell_width=1.0,
        plot_padding=[1mm],
        background_color="Gray",
        highlight_width=0pt,
        point_label_font="arial",
        point_label_color="White",
        point_label_font_size=10pt,
        theme = Theme(
            plot_padding=plot_padding,
            background_color=background_color,
            highlight_width=highlight_width,
            point_label_font=point_label_font,
            point_label_color=point_label_color,
            point_label_font_size=point_label_font_size,
        )
    )
    layers = vcat(map(m->render_layer(m,env,summary,t,theme),rstack.models)...)
    xpts = map(v->v[1],get_vtxs(env))
    ypts = map(v->v[2],get_vtxs(env))
    Gadfly.plot(
        layers...,
        Coord.cartesian(fixed=true,
            xmin=minimum(xpts) - cell_width/2,
            ymin=minimum(ypts) - cell_width/2,
            xmax=maximum(xpts) + cell_width/2,
            ymax=maximum(ypts) + cell_width/2),
        Guide.xticks(;ticks=nothing),
        Guide.yticks(;ticks=nothing),
        Guide.xlabel(nothing),
        Guide.ylabel(nothing),
        theme
    )
end

"""
    `visualize_env`

    Displays the current agent and object locations as well as the current
    planned path segment of each agent.
"""
function visualize_env(search_env::S,vtxs,pickup_vtxs,dropoff_vtxs,t=0;
        robot_vtxs=[],
        object_vtxs=[],
        robot_paths=[],
        object_paths=[],
        search_patterns=[],
        goals=[],
        vpad = 0.05,
        n=80,
        rsize=5pt,
        osize=3pt,
        cell_width=1.0,
        floor_color = "LightGray",
        pickup_color= "LightBlue",
        dropoff_color="Pink",
        line_width=2pt) where {S<:SearchEnv}

    cache = get_cache(search_env)
    sched = get_schedule(search_env)

    color_scale = Scale.color_discrete_hue()
    colors_vec = color_scale.f(n)

    cw = cell_width/2 - vpad
    xpts = map(vtx->cell_width*vtx[1], vtxs)
    ypts = map(vtx->cell_width*vtx[2], vtxs)

    t1 = Int(floor(t))+1
    path_layers = []
    for v in 1:get_num_vtxs(sched)
        vtx_id = get_vtx_id(sched, v)
        if typeof(vtx_id) <: ActionID
            node = get_node_from_id(sched,vtx_id)
            if (get_t0(sched,v) <= t) && (get_tF(sched,v) >= t)
                spec = get_path_spec(sched, v)
                agent_id = get_id(get_robot_id(node))
                agent_path = robot_paths[agent_id]
                p = agent_path[max(1,min(t1+1,get_tF(sched,v)+1)):get_tF(sched,v)+1]
                if length(p) > 0
                    idx1 = agent_path[max(1,min(t1,length(agent_path)))]
                    idx2 = agent_path[max(1,min(t1+1,length(agent_path)))]
                    x0=interpolate(vtxs[idx1][1],vtxs[idx2][1],t-(t1-1))
                    y0=interpolate(vtxs[idx1][2],vtxs[idx2][2],t-(t1-1))
                    push!(path_layers,
                        layer(
                            x=[x0, map(v->vtxs[v][1],p)...],
                            y=[y0, map(v->vtxs[v][2],p)...],Geom.path,
                            Theme(line_width=line_width,default_color=colors_vec[agent_id]))
                    )
                end
            end
            # end
        end
    end
    robot_layers = []
    for (i,p) in enumerate(robot_paths)
        if length(p) > 0
            idx1 = p[max(1,min(t1,length(p)))]
            idx2 = p[max(1,min(t1+1,length(p)))]
            push!(robot_layers,
                layer(
                    x=[interpolate(vtxs[idx1][1],vtxs[idx2][1],t-(t1-1))],
                    y=[interpolate(vtxs[idx1][2],vtxs[idx2][2],t-(t1-1))],
                    Geom.point,size=[rsize],Theme(default_color=colors_vec[i]))
            )
        end
    end
    search_pattern_layers = []
    for (i,p) in enumerate(search_patterns)
        df = DataFrame(x=map(s->vtxs[s[1]][1],p),y=map(s->vtxs[s[1]][2],p),t=map(s->s[2],p))
        push!(path_layers, layer(df,x="x",y="y",color="t",Geom.point,size=[3pt]))
    end
    goal_layers = []
    for (i,s) in enumerate(goals)
        push!(goal_layers,
            layer(x=[vtxs[s[1]][1]],y=[vtxs[s[1]][2]],Geom.point,size=[vsize],shape=[Shape.diamond],Theme(
                    default_color=colors_vec[i]))
        )
    end
    object_layers = []
    for (i,p) in enumerate(object_paths)
        if length(p) > 0
            interpolate(p[min(t1,length(p))],p[min(t1+1,length(p))],t1-(t+1))
            idx1 = p[max(1,min(t1,length(p)))]
            idx2 = p[max(1,min(t1+1,length(p)))]
            push!(object_layers,
                layer(
                    x=[interpolate(vtxs[idx1][1],vtxs[idx2][1],t-(t1-1))],
                    y=[interpolate(vtxs[idx1][2],vtxs[idx2][2],t-(t1-1))],
                    Geom.point,size=[osize],Theme(default_color="Black"))
            )
        end
    end
    # object_layer = layer( x=map(v->vtxs[v][1], object_vtxs), y=map(v->vtxs[v][2], object_vtxs),
    # size=[3pt], Geom.point, Theme(default_color="Black") )

    plot(
        object_layers...,
        # object_layer,
        robot_layers...,
        path_layers...,
        # search_pattern_layers...,
        get_grid_world_layer(pickup_vtxs,pickup_color,cw),
        get_grid_world_layer(dropoff_vtxs,dropoff_color,cw),
        get_grid_world_layer(vtxs,floor_color,cw),
        Coord.cartesian(fixed=true,
            xmin=minimum(xpts) - cell_width/2,
            ymin=minimum(ypts) - cell_width/2,
            xmax=maximum(xpts) + cell_width/2,
            ymax=maximum(ypts) + cell_width/2),
        Guide.xticks(;ticks=nothing),
        Guide.yticks(;ticks=nothing),
        Guide.xlabel(nothing),
        Guide.ylabel(nothing),
        Theme(
            plot_padding=[1mm],
            # default_color="LightGray",
            background_color="Gray"
        )
    )
end
function visualize_env(search_env::S,env::E,t=0;kwargs...) where {S<:SearchEnv,E<:GridFactoryEnvironment}
    visualize_env(search_env,get_vtxs(env),get_pickup_vtxs(env),get_dropoff_vtxs(env),t;kwargs...)
end
function visualize_env(vtxs,pickup_vtxs,dropoff_vtxs,t=0;
        robot_vtxs=[],
        object_vtxs=[],
        robot_paths=[],
        object_paths=[],
        object_intervals=map(p->[0,length(p)], object_paths),
        search_idx=1,
        show_search_paths=false,
        search_patterns=[],
        goals=[],
        vpad = 0.05,
        n=80,
        rsize=4pt,
        osize=3pt,
        search_size=1.5pt,
        goal_size=2.0pt,
        cell_width=1.0,
        line_width=2pt,
        color_scale = Scale.color_discrete_hue(),
        colors_vec = color_scale.f(n),
        floor_color = "LightGray",
        pickup_color= "LightBlue",
        dropoff_color="Pink",
        active_object_color="Black",
        project_idxs=map(i->1,object_paths),
        proj_colors_vec = gen_object_colors(maximum([1,project_idxs...])),
        active_object_colors=map(idx->proj_colors_vec[idx],project_idxs),
        label_objects=false,
        label_robots=false,
        point_label_font="arial",
        point_label_color="White",
        point_label_font_size=10pt,
        inactive_object_color="Gray",
        inactive_object_colors=map(idx->inactive_object_color,project_idxs),
        completed_object_color="Gray",
        completed_object_colors=map(idx->completed_object_color,project_idxs),
        show_inactive_objects=true,
        show_paths=true
    )

    cw = cell_width/2 - vpad
    vtxs = map(vtx->(cell_width*vtx[1],cell_width*vtx[2]),vtxs)
    xpts = map(vtx->vtx[1], vtxs)
    ypts = map(vtx->vtx[2], vtxs)

    theme = Theme(
        plot_padding=[1mm],
        background_color="Gray",
        highlight_width=0pt,
        point_label_font=point_label_font,
        point_label_color=point_label_color,
        point_label_font_size=point_label_font_size
    )

    t1 = Int(floor(t))+1
    path_layers = []
    if show_paths
        for (i,p) in enumerate(robot_paths)
            if length(p) > 0
                push!(path_layers,
                    layer(x=map(v->vtxs[v][1],p),y=map(v->vtxs[v][2],p),Geom.path,
                        Theme(theme,line_width=line_width,default_color=colors_vec[i]))
                )
            end
        end
    end
    robot_layers = []
    for (i,p) in enumerate(robot_paths)
        if length(p) > 0
            # interpolate(p[min(t1,length(p))],p[min(t1+1,length(p))],t1-(t+1))
            idx1 = p[max(1,min(t1,length(p)))]
            idx2 = p[max(1,min(t1+1,length(p)))]
            label = label_robots ? string("R",i) : ""
            push!(robot_layers,
                layer(
                    x=[interpolate(vtxs[idx1][1],vtxs[idx2][1],t-(t1-1))],
                    y=[interpolate(vtxs[idx1][2],vtxs[idx2][2],t-(t1-1))],
                    label=[label],
                    Geom.label(position=:centered,hide_overlaps=false),
                    Geom.point,
                    size=[rsize],
                    Theme(
                        theme,
                        default_color=colors_vec[i],
                        )
                    )
            )
        end
    end
    search_pattern_layers = []
    if show_search_paths
        for (i,p) in enumerate(search_patterns)
            if length(p) > 0
                idx = max(1,min(search_idx,length(p)))
                t_search = p[idx][2] # time step
                df = DataFrame(x=map(s->vtxs[s[1]][1],p[1:idx]),y=map(s->vtxs[s[1]][2],p[1:idx]),t=map(s->s[2],p[1:idx]))
                push!(search_pattern_layers,
                    layer(x=[df.x[end]],y=[df.y[end]],Geom.point,size=[search_size],
                        Theme(default_color="Black",highlight_width=0pt))
                    )
                push!(search_pattern_layers,
                    layer(df,x="x",y="y",Geom.point,size=[search_size],
                        Theme(default_color="Gray",highlight_width=0pt))
                    )
            end
        end
    end
    goal_layers = []
    for (i,s) in enumerate(goals)
        push!(goal_layers,
            layer(x=[vtxs[s[1]][1]],y=[vtxs[s[1]][2]],Geom.point,size=[goal_size],
                Theme(default_color="red",highlight_width=0pt)
                # Theme(default_color=colors_vec[i])
            )
        )
    end
    object_layers = []
    for (i,(p,interval)) in enumerate(zip(object_paths,object_intervals))
        if interval[1] > t
            object_color = inactive_object_colors[i]
        elseif interval[2] < t
            object_color = completed_object_colors[i]
        else
            object_color = active_object_colors[i]
        end
        if length(p) > 0 && interval[2] > t
            if (interval[1] <= t) || show_inactive_objects
                interpolate(p[min(t1,length(p))],p[min(t1+1,length(p))],t1-(t+1))
                idx1 = p[max(1,min(t1,length(p)))]
                idx2 = p[max(1,min(t1+1,length(p)))]
                label = label_objects ? string("O",i) : ""
                push!(object_layers,
                    layer(
                        x=[interpolate(vtxs[idx1][1],vtxs[idx2][1],t-(t1-1))],
                        y=[interpolate(vtxs[idx1][2],vtxs[idx2][2],t-(t1-1))],
                        label=[label],
                        Geom.label(position=:centered,hide_overlaps=false),
                        Geom.point,
                        size=[osize],
                        Theme(
                            theme,
                            default_color=object_color,
                            )
                        )
                )
            end
        end
    end
    if length(object_vtxs) > 0
        push!(object_layers,layer( x=map(v->vtxs[v][1], object_vtxs), y=map(v->vtxs[v][2], object_vtxs),
            size=[3pt], Geom.point, Theme(default_color="Black")))
    end
    for (i,idx) in enumerate(robot_vtxs)
        push!(robot_layers,
            layer(
                x=[vtxs[idx][1]],
                y=[vtxs[idx][2]],
                Geom.point,size=[rsize],Theme(default_color=colors_vec[i]))
        )
    end

    Gadfly.plot(
        search_pattern_layers...,
        goal_layers...,
        object_layers...,
        robot_layers...,
        path_layers...,
        get_grid_world_layer(pickup_vtxs,pickup_color,cw),
        get_grid_world_layer(dropoff_vtxs,dropoff_color,cw),
        get_grid_world_layer(vtxs,floor_color,cw),
        Coord.cartesian(fixed=true,
            xmin=minimum(xpts) - cell_width/2,
            ymin=minimum(ypts) - cell_width/2,
            xmax=maximum(xpts) + cell_width/2,
            ymax=maximum(ypts) + cell_width/2),
        Guide.xticks(;ticks=nothing),
        Guide.yticks(;ticks=nothing),
        Guide.xlabel(nothing),
        Guide.ylabel(nothing),
        theme
    )
end
function visualize_env(env::GridFactoryEnvironment,t=0;kwargs...)
    visualize_env(get_vtxs(env),get_pickup_vtxs(env),get_dropoff_vtxs(env),t;kwargs...)
end

render_env(t) = visualize_env(factory_env,t;robot_vtxs=r0,object_vtxs=s0,paths=paths,search_patterns=[],goals=[])
render_paths(t,factory_env,robot_paths,object_paths=[];kwargs...) = visualize_env(factory_env,t;robot_paths=robot_paths,object_paths=object_paths,kwargs...)
render_both(t,paths1,paths2) = hstack(render_paths(t,paths1),render_paths(t,paths2))
render_search(t_start,factory_env;kwargs...) = visualize_env(factory_env,t_start;show_search_paths=true,kwargs...)

# Plotting results
function preprocess_results!(df)
    if nrow(df) > 0
        if :depth_bias in names(df)
            begin df[!,:depth_bias_string] = string.(df.depth_bias)
                df
            end
        end
        if :N in names(df)
            begin df[!,:N_string] = string.(df.N)
                df
            end
        end
        sort!(df, (:M,:N))
    end
    df
end
function preprocess_results!(df_dict::Dict)
    for (k,df) in df_dict
        preprocess_results!(df)
    end
    df_dict
end

function robots_vs_task_vs_time_box_plot(df;
        title="Solution time by Number of Robots (N) and Number of Tasks (M)",
        yticks=[-1,0,1,2],
        ymin=-1.5,
        ymax=2.3,
        y_bounds=[0.01,100.0],
        big_font=14pt,
        small_font=12pt,
        y=:time,
        x=:N_string,
        xgroup=:M,
        color=:N_string,
        ylabel="computation time (s)",
        scale_y = Scale.y_log10(minvalue=y_bounds[1],maxvalue=y_bounds[2])
    )
    latex_fonts = Theme(major_label_font="CMU Serif", major_label_font_size=big_font,
                    minor_label_font="CMU Serif", minor_label_font_size=small_font,
                    key_title_font="CMU Serif", key_title_font_size=small_font,
                    key_label_font="CMU Serif", key_label_font_size=small_font)

    plot(df, xgroup=xgroup, x=x, y=y, color=color,
        Geom.subplot_grid(
            Geom.boxplot(;suppress_outliers=false),
            Coord.cartesian(; ymin=ymin, ymax=ymax),
            Guide.yticks(;ticks=yticks),
            Guide.xticks(;label=false),
            ),
        Guide.title(title),
        Guide.colorkey(title="number of robots", labels=["10","20","30","40"], pos=[0.1w,-0.32h]),
        Scale.group_discrete(labels=M->string(M," Tasks"),levels=[10,20,30,40,60]),
        Guide.xlabel("number of tasks"),
        Guide.ylabel(ylabel),
        scale_y,
        latex_fonts
    )
end

# function get_box_plot_group_plot_normal(df;
#         obj=:time,
#         outer_key=:M,
#         inner_key=:N,
#         outer_range=10:10:60,
#         inner_range=10:10:40,
#         xmin=0,
#         xmax=length(inner_range)+1,
#         ymin=0.007,
#         ymax=120,
#         xtick=[10,20,30,40],
#         xticklabels=[10,20,30,40],
#         tickpos="left",
#         ytick=[0.1,1,10,100],
#         ylabel_shift="0pt",
#         title="",
#         title_shift=[3.3,2.55],
#         inner_sym="n",
#         outer_sym="m",
#         xlabels=map(m->string(outer_sym," = ",m), outer_range),
#         ylabels=map(n->string(inner_sym," = ",n), inner_range),
#         ylabel="time (s)",
#         draw_labels=true,
#         ymode="log",
#         width="3.25cm",
#         height="6cm",
#         legend_draw="black",
#         legend_fill="white",
#         legend_x_shift="2pt",
#         vertical_sep = "0pt",
#         horizontal_sep = "0pt",
#         mark="*",
#     )
#     group_style = [
#         "group name"=>"myPlots",
#         "group size"=>"$(length(outer_range)) by 1",
#         "xlabels at"=>"edge bottom",
#         "xticklabels at"=>"edge bottom",
#         "vertical sep"=>"0pt",
#         "horizontal sep"=>"2pt"
#     ]
#     gstyle=prod(map(p->string(p.first,"=",p.second,","),group_style))
#     gp = PGFPlots.GroupPlot(groupStyle=gstyle,
#             # "boxplot"
#             # "boxplot/draw direction = y",
#             # ymode=ymode,
#             # "\footnotesize",
#             # width=width,
#             # height=height,
#             # xmin=xmin,
#             # xmax=xmax,
#             # ymin=ymin,
#             # ymax=ymax,
#             # xtick=xtick,
#             # xticklabels=xtick,
#             # tickpos=tickpos,
#             # ytick=ytick,
#             # yticklabels=[],
#             # "ylabel shift=$(ylabel_shift)",
#             # "ytick align=outside",
#             # "xtick align=outside",
#     )
#     # pushPGFPlotsOptions("boxplot")
#
#     # for (i,m) in enumerate(outer_range)
#     #     if i == 1 && draw_labels
#     #         push!(gp,
#     #             xlabel="\$$(xlabels[i])\$",
#     #             ylabel=ylabel,
#     #             yticklabels=ytick,
#     #             "legend style=[draw=$legend_draw,
#     #                 fill=$legend_fill,
#     #                 xshift=$legend_x_shift,
#     #                 yshift=0pt],
#     #                 legend pos=north west
#     #             ],"
#     #             map(j->LegendEntry({},@sprintf("\$%s\$",ylabels[j]),false),1:length(inner_range))...,
#     #             """
#     #             \\addlegendimage{no markers,blue}
#     #             \\addlegendimage{no markers,red}
#     #             \\addlegendimage{no markers,brown}
#     #             \\addlegendimage{no markers,black}
#     #             """,
#     #             map(n->PGFPlotsX.PlotInc({boxplot,mark=mark},Table(
#     #                         {"y index"=0},
#     #                         [:data=>df[(df[:,outer_key] .== m) .& (df[:,inner_key] .== n),obj]])),inner_range)...)
#     #     else
#     #         push!(gp, {
#     #                 xlabel=@sprintf("\$%s\$",xlabels[i]),
#     #                 ymajorticks="false",
#     #                 yminorticks="false"
#     #             },
#     #             map(n->PGFPlotsX.PlotInc({boxplot,mark=mark},Table(
#     #                         {"y index"=0},
#     #                         [:data=>df[(df[:,outer_key] .== m) .& (df[:,inner_key] .== n),obj]])),inner_range)...)
#     #     end
#     # end;
#     gp
# end
function get_box_plot_group_plot(df;
        obj=:time,
        outer_key=:M,
        inner_key=:N,
        outer_range=sort(unique(df[!,outer_key])),
        inner_range=sort(unique(df[!,inner_key])),
        xmin=0,
        xmax=length(inner_range)+1,
        ymin=0.007,
        ymax=maximum(df[!,obj]),
        xtick=[10,20,30,40],
        xticklabels=[10,20,30,40],
        tickpos="left",
        ytick=[0.1,1,10,100],
        ylabel_shift="0pt",
        title="",
        title_shift=[3.3,2.55],
        inner_sym="n",
        outer_sym="m",
        xlabels=map(m->string(outer_sym," = ",m), outer_range),
        ylabels=map(n->string(inner_sym," = ",n), inner_range),
        ylabel="time (s)",
        draw_labels=true,
        ymode="log",
        width="3.25cm",
        height="6cm",
        legend_draw="black",
        legend_fill="white",
        legend_x_shift="2pt",
        mark="*",
    )
    @pgf gp = PGFPlotsX.GroupPlot({group_style = {
                "group name"="myPlots",
                "group size"=string(length(outer_range)," by 1"),
                "xlabels at"="edge bottom",
                "xticklabels at"="edge bottom",
                "vertical sep"="0pt",
                "horizontal sep"="2pt"
            },
            boxplot,
            "boxplot/draw direction"="y",
            ymode=ymode,
            footnotesize,
            width=width,
            height=height,
            xmin=xmin,
            xmax=xmax,
            ymin=ymin,
            ymax=ymax,
            xtick=xtick,
            xticklabels=xtick,
            tickpos=tickpos,
            ytick=ytick,
            yticklabels=[],
            # "set layers"="standard",
            "ylabel shift"=ylabel_shift,
            "ytick align"="outside",
            "xtick align"="outside",
            # "legend entries"={map(j->@sprintf("\$%s\$",ylabels[j]),1:length(inner_range))...},
            # "legend cell align"="left",
            # "legend to name"="grouplegend",
            # "legend style"={
            #     draw=legend_draw,
            #     fill=legend_fill,
            #     xshift=legend_x_shift,
            #     "inner sep"="1pt",
            #     yshift="0pt",
            #     font="\\scriptsize"
            # },
            # "legend pos"="north west"
        }

        );

    @pgf for (i,m) in enumerate(outer_range)
        if i == 1 && draw_labels
            # push!(gp,
            #     """
            #     \\coordinate (leg) at (rel axis cs:0,1);
            #     """
            # )
            push!(gp,
                {xlabel=@sprintf("\$%s\$",xlabels[i]),
                ylabel=ylabel,
                yticklabels=ytick,
                "legend style"={
                    draw=legend_draw,
                    fill=legend_fill,
                    xshift=legend_x_shift,
                    yshift="0pt"},
                "legend pos"="north west"
                },
                map(j->PGFPlotsX.LegendEntry({},@sprintf("\$%s\$",ylabels[j]),false),1:length(inner_range))...,
                """
                \\addlegendimage{no markers,blue}
                \\addlegendimage{no markers,red}
                \\addlegendimage{no markers,brown}
                \\addlegendimage{no markers,black}
                """,
                map(n->PGFPlotsX.PlotInc({boxplot,mark=mark},PGFPlotsX.Table(
                            {"y index"=0},
                            [:data=>df[(df[:,outer_key] .== m) .& (df[:,inner_key] .== n),obj]])),inner_range)...)
        else
            push!(gp, {
                    xlabel=@sprintf("\$%s\$",xlabels[i]),
                    ymajorticks="false",
                    yminorticks="false"
                },
                map(n->PGFPlotsX.PlotInc({boxplot,mark=mark},PGFPlotsX.Table(
                            {"y index"=0},
                            [:data=>df[(df[:,outer_key] .== m) .& (df[:,inner_key] .== n),obj]])),inner_range)...)
        end
    end;
    gp
end
function get_titled_group_box_plot(df;
        title="",
        title_shift=[3.3,2.55],
        scale=0.7,
        kwargs...)
    gp = get_box_plot_group_plot(df;kwargs...)
    if title != ""
        tikzpic = @pgf TikzPicture({scale=scale},
            """
            \\centering
            \\pgfplotsset{set layers=standard,cell picture=true}
            """,
            gp,
            @sprintf("""
            \\node (title) at (\$(myPlots c1r1.center)!0.5!(myPlots c2r1.center)+(%2.2fcm,%2.2fcm)\$) {\\textbf{%s}};
            """,title_shift...,title),
            # """
            # \\node[anchor= north west] (leg) at (myPlots c1r1.north west){\\pgfplotslegendfromname{grouplegend}};
            # """
        )
    else
        tikzpic = @pgf TikzPicture({scale=scale},
            """
            \\centering
            \\pgfplotsset{set layers=standard,cell picture=true}
            """,
            gp,
            # """
            # \\node[anchor= north west] (leg) at (myPlots c1r1.north west){\\pgfplotslegendfromname{grouplegend}};
            # """
        )
    end
    return tikzpic
end

# print_tex(gp)

function preprocess_collab_results!(df_dict)
    num_tasks = [12,18,24]
    num_robots = [24]
    depth_biases=[0.1]
    task_size_distributions = [1,2,3,4]
    num_trials=16
    task_ratios = Int[]
    for (M,N,task_sizes,trial) in Base.Iterators.product(
            num_tasks,num_robots,task_size_distributions,1:num_trials
            )
        push!(task_ratios, task_sizes)
    end

    for (k,df) in df_dict
        if nrow(df) > 0
            begin df[!,:depth_bias_string] = string.(df.depth_bias)
                df
            end
            begin df[!,:N_string] = string.(df.N)
                df
            end
            begin df[!,:M_string] = string.(df.M)
                df
            end
            begin df[!,:TaskRatio] = task_ratios[df.problem_id]
                df
            end
        end
    end
    df_dict
end
function plot_collab_runtimes(df;
        title="Solution time by Number of Tasks (M) and ratio of tasks (task ratio )",
        yticks=[-1,0,1,2],
        ymin=-1.5,
        ymax=2.3,
        y_bounds=[0.01,100.0],
        big_font=14pt,
        small_font=12pt,
        xgroup=:TaskRatio,
        x=:M_string,
        y=:time,
        suppress_outliers=true,
        scale_y = Scale.y_log10(minvalue=y_bounds[1],maxvalue=y_bounds[2])
    )
    latex_fonts = Theme(major_label_font="CMU Serif", major_label_font_size=big_font,
                    minor_label_font="CMU Serif", minor_label_font_size=small_font,
                    key_title_font="CMU Serif", key_title_font_size=small_font,
                    key_label_font="CMU Serif", key_label_font_size=small_font)

    plot(df, xgroup=xgroup, x=x, y=y, color=x,
        Geom.subplot_grid(
            Geom.boxplot(;suppress_outliers=suppress_outliers),
            Coord.cartesian(; ymin=ymin, ymax=ymax),
            Guide.yticks(;ticks=yticks),
            Guide.xticks(;label=false),
            ),
        Guide.title(title),
        Guide.colorkey(title="num tasks", labels=["12","18","24"], pos=[0.1w,-0.32h]),
        # Scale.group_discrete(labels=M->string(M," Tasks"),levels=[1,2,3,4]),
        Guide.xlabel("task ratio"),
        Guide.ylabel("computation time (s)"),
        scale_y,
        latex_fonts
    )
end
function plot_collab_counts(df;
        title="# failures vs. # Tasks (M) x Task Ratio",
        yticks=[0,4,8,12,16],
        ymin=0,
        ymax=16,
        y_bounds=[0.01,100.0],
        big_font=14pt,
        small_font=12pt,
        task_ratios = [
            ( 1=>1.0, 2=>0.0, 4=>0.0 ),
            ( 1=>1.0, 2=>1.0, 4=>0.0 ),
            ( 1=>1.0, 2=>1.0, 4=>1.0 ),
            ( 1=>0.0, 2=>1.0, 4=>1.0 ),
        ],
        key=df.optimal
    )

    opt_df = DataFrame( M = Int[], task_ratio = Int[], num_no = Int[], num_yes = Int[] )

    for (i,ratio) in enumerate(task_ratios)
        for m in Set(collect(df.M))
        push!(
            opt_df,
            Dict(
                :M =>m,
                :task_ratio => i,
                :num_no => nrow(df[(df.M .== m) .* (df.TaskRatio .== i) .* (key .== false),:]),
                :num_yes => nrow(df[(df.M .== m) .* (df.TaskRatio .== i) .* (key .== true),:]),
            )
        )
        end
    end
    begin opt_df[!,:M_string] = string.(opt_df.M)
        opt_df
    end

    latex_fonts = Theme(major_label_font="CMU Serif", major_label_font_size=big_font,
                    minor_label_font="CMU Serif", minor_label_font_size=small_font,
                    key_title_font="CMU Serif", key_title_font_size=small_font,
                    key_label_font="CMU Serif", key_label_font_size=small_font)

    plot(opt_df, xgroup=:task_ratio, x=:M_string, y=:num_no, color=:M_string,
        Geom.subplot_grid(
            Geom.bar,
            Coord.cartesian(; ymin=ymin, ymax=ymax),
            Guide.yticks(;ticks=yticks),
            Guide.xticks(;label=false),
            ),
        Guide.title(title),
        Guide.colorkey(title="num tasks", labels=["12","18","24"], pos=[0.1w,-0.32h]),
        Scale.group_discrete(labels=M->string(M," Tasks"),levels=[1,2,3,4]),
        Guide.xlabel("task ratio"),
        Guide.ylabel("computation time (s)"),
        latex_fonts
    )
end

function plot_histories_pgf(df_dict::Dict,
    key_list=sort(string.(collect(keys(df_dict)))),args...;
    legend_entries = key_list,
    kwargs...)
    plot_histories_pgf([df_dict[k] for k in key_list],args...;
        legend_entries = legend_entries,
        kwargs...
    )
end
function plot_histories_pgf(df_list::Vector,ax=PGFPlots.Axis();
        y_key=:makespans,
        x_key=:arrival_times,
        m_key=:M,
        m_vals=sort(intersect([unique(df[!,m_key]) for df in df_list]...)),
        n_key=:arrival_interval,
        n_vals=sort(intersect([unique(df[!,n_key]) for df in df_list]...)),
        include_keys=[],
        exclude_keys=[],
        opt_key=:backup_flags,
        use_y_lims=false,
        y_min=minimum(map(df->minimum(map(minimum,df[!,y_key])),df_list)),
        y_max=maximum(map(df->maximum(map(maximum,df[!,y_key])),df_list)),
        xlabel=string("\$",string(m_key)," = ",m_vals...,"\$"),
        ylabel=string("\$",string(n_key)," = ",n_vals...,"\$"),
        colors=["red","blue","black","brown"],
        legend_entries = ["" for c in colors],
        legend_flag = true,
        ytick_show=false,
        ymode="log",
        legend_pos = "outer north east",
        legend_font = "\\footnotesize",
        lines=true,
        opt_marks=map(d->"x",df_list),
        non_opt_marks=opt_marks,
    )

    ax.ymode    =   ymode
    if length(xlabel) > 0
        ax.xlabel = xlabel
    end
    if ytick_show
        ax.ylabel = ylabel
    else
        ax.ylabel = nothing
        if ax.style === nothing
            ax.style="yticklabels=none"
        elseif findfirst(ax.style,"yticklabels") === nothing
            ax.style=string(ax.style,", ","yticklabels=none")
        end
    end
    if use_y_lims
        ax.ymin = y_min
        ax.ymax = y_max
    end
    if length(legend_pos) > 0
        ax.legendPos = legend_pos
    end
    ax.legendStyle = "font=$(legend_font)"
    counters = zeros(length(df_list))
    for (i,m_val) in enumerate(m_vals)
        for (j,n_val) in enumerate(n_vals)
            for (df_idx,(df,color,l_entry,opt_mark,non_opt_mark)) in enumerate(
                    zip(df_list,colors,legend_entries,opt_marks,non_opt_marks))
                idxs = .&(
                    (df[!,m_key] .== m_val),
                    (df[!,n_key] .== n_val),
                    )
                if !any(idxs)
                    continue
                end
                if !isempty(include_keys)
                    idxs = .&(idxs, [(df[!,k] .== v) for (k,v) in include_keys]...)
                end
                if !isempty(exclude_keys)
                    idxs = .&(idxs, [(df[!,k] .!= v) for (k,v) in exclude_keys]...)
                end
                df_cut = df[idxs,:]
                style=string(color,",solid, mark options={solid,fill=",color,"}")
                for k in 1:nrow(df_cut)
                    y_arr = df_cut[!,y_key][k]
                    x_arr = x_key == :none ? collect(1:length(y_arr)) : df_cut[x_key][k]
                    opt_vals = df_cut[!,opt_key][k] # tracks whether the fall back was employed

                    idx = minimum([length(x_arr),length(y_arr),length(opt_vals)])
                    # idx = minimum([length(x_arr),length(y_arr)])
                    # Overall trend
                    if lines
                        push!(ax,
                            Plots.Linear(x_arr[1:idx], y_arr[1:idx], style=style, mark="none")
                        )
                    end
                    # Timeouts
                    if 0 < idx < length(y_arr)
                        # End points
                        push!(ax,
                            Plots.Linear(
                                [x_arr[idx]],
                                [y_arr[idx]],
                                mark="o",
                                style=style,
                                onlyMarks=true, )
                        )
                        idx -= 1 # decrement so that the final point is not covered twice
                    end

                    # tmidxs=findall(.~opt_vals[1:idx])
                    # tmidxs=1:idx
                    for (tmidxs, mark) in [
                        (findall(.!opt_vals[1:idx]),opt_mark),
                        (findall(opt_vals[1:idx]),non_opt_mark),
                        ]
                        if length(tmidxs) > 0
                            p = Plots.Linear(
                                        x_arr[tmidxs],
                                        y_arr[tmidxs],
                                        style=style,
                                        onlyMarks=true,
                                        mark=mark,
                                        )
                            if legend_flag && counters[df_idx] == 0 && length(l_entry) > 0
                                p.legendentry = l_entry
                                counters[df_idx] += 1
                                insert!(ax.plots,1,p)
                            else
                                push!(ax,p)
                            end
                        end
                    end
                end
            end
        end
    end
    return ax
end

function plot_history_layers(df,keylist,ax=PGFPlots.Axis();
        legend_entries = map(string,keylist),
        colors = ["red","blue","black","brown"],
        opt_marks=["diamond","x","triangle","+"],
        kwargs...
    )
    plt = ax
    for (i,k) in enumerate(keylist)
        plt = plot_histories_pgf([df],plt;
            y_key = k,
            legend_entries=[legend_entries[i]],
            colors = [colors[i]],
            opt_marks = [opt_marks[i]],
            kwargs...,
            )
    end
    return plt
end

function group_history_plot(df_dict::Dict,
    key_list=sort(string.(collect(keys(df_dict))));
    kwargs...)
    group_history_plot([df_dict[k] for k in key_list];
        kwargs...
    )
end
function group_history_plot(df_list::Vector;
    m_key=:M,
    m_vals=sort(intersect([unique(df[m_key]) for df in df_list]...)),
    n_key=:arrival_interval,
    n_vals=sort(intersect([unique(df[n_key]) for df in df_list]...)),
    y_key=:makespans,
    y_min=minimum(map(df->minimum(map(minimum,df[!,y_key])),df_list)),
    y_max=maximum(map(df->maximum(map(maximum,df[!,y_key])),df_list)),
    legend_location = (1,1),
    kwargs...
    )
    g = PGFPlots.GroupPlot(length(m_vals),length(n_vals))
    for (i,m_val) in enumerate(m_vals)
        ytick_show = (i == 1)
        for (j,n_val) in enumerate(n_vals)
            legend_flag = (legend_location[1] == i) && (legend_location[2] == j)
            plt = plot_histories_pgf(df_list;
                y_key=y_key,
                m_key=m_key,
                m_vals=[m_val,],
                n_key=n_key,
                n_vals=[n_val,],
                ytick_show = ytick_show,
                legend_flag = legend_flag,
                y_min=y_min,
                y_max=y_max,
                kwargs...)
            push!(g,plt)
        end
    end
    return g
end

"""
    ResultsTable

A simple Table data structure for compiling tabular results.
"""
struct ResultsTable
    xkeys::Vector{Dict{Any,Any}}
    ykeys::Vector{Dict{Any,Any}}
    data::Matrix{Any}
end
get_data(tab::ResultsTable) = tab.data
get_data(tab) = tab
get_xkeys(tab::ResultsTable) = tab.xkeys
get_ykeys(tab::ResultsTable) = tab.ykeys
Base.size(tab::ResultsTable) = size(get_data(tab))
Base.size(tab::ResultsTable,dim) = size(get_data(tab),dim)
Base.transpose(tab::ResultsTable) = ResultsTable(get_ykeys(tab),get_xkeys(tab),transpose(get_data(tab)))

"""
    build_table

Takes in a dataframe, builds a table of `obj` values along axes `xkey` and
`ykey`, where the `obj` values are aggregated via the function `f`.
"""
function build_table(df;
    obj=:tasks_per_second,
    xkey=:M,
    ykey = :arrival_interval,
    xvals = sort(unique(df[!,xkey])),
    yvals = sort(unique(df[!,ykey])),
    aggregator = vals -> sum(vals)/length(vals),
    )

    xkeys = map(x->Dict(xkey=>x), xvals)
    ykeys = map(y->Dict(ykey=>y), yvals)
    tab = zeros(length(xvals),length(yvals))
    tab = ResultsTable(xkeys, ykeys, tab)
    for (i,x) in enumerate(xvals)
        for (j,y) in enumerate(yvals)
            idxs = .&((df[!,xkey] .== x),(df[!,ykey] .== y))
            # if any(idxs)
                vals = df[idxs,obj]
                tab.data[i,j] = aggregator(vals)
            # else
            #     tab.data[i,j] = nothing
            # end
        end
    end
    # xkeys = map(x->Dict(xkey=>x), xvals)
    # ykeys = map(y->Dict(ykey=>y), yvals)
    # return ResultsTable(xkeys, ykeys, tab)
    return tab
end
function table_product(tables,mode=:horizontal)
    @assert length(unique(size.(tables))) == 1 "All tables must have same dimensions"
    if mode == :horizontal
        data = collect(zip(map(get_data, tables)...))
        xkeys = map(tup->merge(tup...), collect(zip(map(get_xkeys, tables)...)))
        ykeys = map(tup->merge(tup...), collect(zip(map(get_ykeys, tables)...)))
        return ResultsTable(xkeys,ykeys,data)
    else
        return transpose(table_product(map(transpose,tables),:horizontal))
    end
end
function table_reduce(table,op)
    return ResultsTable(get_xkeys(table),get_ykeys(table),map(op,get_data(table)))
end
""" utility for printing real-valued elements of a table """
function print_real(io,v;
        precision=3,
        )
    if isa(v,Int)
        print(io,v)
    elseif isnan(v)
        print(io,"")
    else
        print(io,round(v;digits=precision))
    end
end
""" """
function print_multi_value_real(io,vals;
        delim=" & ",
        surround=["",""],
        stopchar="",
        comp_func=vals->-1,
        kwargs...)
    best_idx = comp_func(vals)
    best_idxs = findall(vals .== vals[best_idx])
    for (i,v) in enumerate(vals)
        print(io,surround[1])
        i in best_idxs ? print(io,"\\textbf{") : nothing
        print_real(io,v;kwargs...)
        i in best_idxs ? print(io,"}") : nothing
        print(io,surround[2])
        if i < length(vals)
            print(io,delim)
        else
            print(io,stopchar)
        end
    end
end
function print_latex_header(io,tab;span=length(get_data(tab)[1,1]),delim=" & ",newline=" \\\\\n",start_char="& ")
    print(io,"\\begin{tabular}{",map(i->"l ",1:size(tab,2)*span+1)...,"}","\n")
    print(io,start_char)
    for (j,ykeys) in enumerate(get_ykeys(tab))
        print(io,"\\multicolumn{$span}{c}{",
            ["\$$(k)=$(v)\$" for (k,v) in ykeys]...,"}")
        if j < size(tab,2)
            print(io,delim)
        else
            print(io,newline)
        end
    end
end
function print_latex_row_start(io,tab,i;span=length(get_data(tab)[1,1]),delim=" & ")
    xkeys = get_xkeys(tab)[i]
    for (k,v) in xkeys
        print(io,"\$$(k)=$(v)\$")
        if length(xkeys > 1)
            print(io,",")
        end
    end
    # print(io,["\$$(k)=$(v)\$" for (k,v) in xkeys]...)
    print(io,delim)
end
function write_tex_table(io,tab;
        delim = " & ",
        newline = " \\\\\n",
        print_func = (io,v) -> print_real(io,v),
        header_printer = print_latex_header,
        row_start_printer = print_latex_row_start,
        kwargs...,
    )
    header_printer(io,tab)
    for (i,xkeys) in enumerate(get_xkeys(tab))
        row_start_printer(io,tab,i)
        for (j,ykeys) in enumerate(get_ykeys(tab))
            print_func(io,get_data(tab)[i,j];kwargs...)
            if j < size(tab,2)
                print(io,delim)
            else
                print(io,newline)
            end
        end
    end
    print(io,"\\end{tabular}")
end
function write_tex_table(fname::String,args...;kwargs...)
    open(fname,"w") do io
        write_tex_table(io,args...;kwargs...)
    end
end

# For rendering the factory floor as a custom ImageAppearance in Webots
function caution_tape(d,n;yellow_color=RGB(0.8,0.7,0.0),black_color=RGB(0.0,0.0,0.0))
    compose(context(),map(i->(context(),
                (context(),polygon([
                            (0,(i-1)/n),
                            (d,(2*i-1)/(2*n)),
                            (d,i/n),
                            (0,(2*i-1)/(2*n))]),fill(yellow_color)),
            ), 1:n)...,
            (context(),polygon([(0,0),(0,1),(d,1),(d,0)]),fill(black_color))
    )
end
function taped_square(d=0.1;n=10,yellow_color=RGB(0.8,0.7,0.0),black_color=RGB(0.0,0.0,0.0))
    compose(
        context(),
        map(i->(context(rotation=Rotation(i*π/2,0.5,0.5)),
                caution_tape(d,n;yellow_color=yellow_color,black_color=black_color)), 0:3)...
    )
end
function render_factory_floor(env::GridFactoryEnvironment;
        d=0.1,n=10,yellow_color=RGB(0.8,0.7,0.0),black_color=RGB(0.0,0.0,0.0),
        floor_color=RGB(0.6,0.6,0.6)
    )
    x_dim = get_x_dim(env)
    y_dim = get_y_dim(env)
    compose(
        context(units=UnitBox()),
        map(vtx->(context((vtx[1]-1)/x_dim,(vtx[2]-1)/y_dim,1/x_dim,1/y_dim),
                taped_square(d;n=n,yellow_color=yellow_color,black_color=black_color)), get_pickup_vtxs(env))...,
        map(vtx->(context((vtx[1]-1)/x_dim,(vtx[2]-1)/y_dim,1/x_dim,1/y_dim),
                taped_square(d;n=n,yellow_color=yellow_color,black_color=black_color)), get_dropoff_vtxs(env))...,
        (context(), rectangle(),fill(floor_color))
    )
end

function get_node_shape(search_env::SearchEnv,graph,v,x,y,r)
    sched = get_schedule(search_env)
    cache = get_cache(search_env)
    node_id = get_vtx_id(sched,get_prop(graph,v,:vtx_id))
    if typeof(node_id) <: ActionID
        return Compose.circle(x,y,r)
    elseif typeof(node_id) <: RobotID
        return Compose.ngon(x,y,r,4)
    elseif typeof(node_id) <: ObjectID
        return Compose.ngon(x,y,r,3)
    elseif typeof(node_id) <: OperationID
        return Compose.circle(x,y,r)
    end
end
function get_node_color(search_env::SearchEnv,graph,v,x,y,r)
    sched = get_schedule(search_env)
    cache = get_cache(search_env)
    node_id = get_vtx_id(sched,get_prop(graph,v,:vtx_id))
    if typeof(node_id) <: ActionID
        return "cyan"
    elseif typeof(node_id) <: CleanUpBotID
        return "purple"
    elseif typeof(node_id) <: BotID
        return "lime"
    elseif typeof(node_id) <: ObjectID
        return "orange"
    elseif typeof(node_id) <: OperationID
        return "red"
    end
end
function get_node_text(search_env::SearchEnv,graph,v,x,y,r)
    sched = get_schedule(search_env)
    cache = get_cache(search_env)
    v_ = get_prop(graph,v,:vtx_id)
    string(get_t0(sched,v_)," - ",get_tF(sched,v_),"\n",get_local_slack(sched,v_)," - ",get_slack(sched,v_))
end

function show_times(sched::OperatingSchedule,v,scalar_slack=true)
    if scalar_slack
        slack = get_slack(sched,v)[1]
        slack_string = slack == Inf ? string(slack) : string(Int(slack))
    else
        slack_string = string(get_slack(sched,v))
    end
    return string(get_t0(sched,v),", ",get_tF(sched,v),", ",slack_string)
end
# function show_times(cache::PlanningCache,v)
#     slack = minimum(get_slack(sched,v))
#     slack_string = slack == Inf ? string(slack) : string(Int(slack))
#     return string(get_t0(sched,v),",",get_tF(sched,v),",",slack_string)
# end

function plot_project_schedule(sched::OperatingSchedule;
        mode=:root_aligned,
        verbose=true,
        shape_function = (G,v,x,y,r)->Compose.circle(x,y,r),
        color_function = (G,v,x,y,r)->get_prop(G,v,:color),
        text_function = (G,v,x,y,r)->string(
            title_string(
                get_node_from_id(sched,
                    get_vtx_id(sched, v)),
                verbose),
            "\n",show_times(sched,v)
            )
        )
    rg = get_display_metagraph(sched;
        # f=(v,p)->string(v,",",get_id(get_default_robot_id(get_node(sched,v)))
        )
    plot_graph_bfs(rg;
        mode=mode,
        shape_function=shape_function,
        color_function=color_function,
        text_function=text_function
    )
    # `inkscape -z project_schedule1.svg -e project_schedule1.png`
    # OR: `for f in *.svg; do inkscape -z $f -e $f.png; done`
end
plot_project_schedule(search_env::SearchEnv;kwargs...) = plot_project_schedule(get_schedule(search_env);kwargs...)

function print_project_schedule(filename::String,args...;kwargs...)
    plot_project_schedule(args...;kwargs...) |> Compose.SVG(string(filename,".svg"))
end

function snapshot_color_func(env,t)
    sched = get_schedule(env)
    active = Set(filter(v->get_t0(sched,v) <= t < get_tF(sched,v) + maximum(get_local_slack(sched,v)),vertices(sched)))
    for v in collect(active)
        setdiff!(active,outneighbors(get_graph(sched),v))
    end
    # active = Set(filter(v->get_t0(sched,v) <= t <= get_tF(sched,v)))
    # active, fixed = get_active_and_fixed_vtxs(sched,cache,t)
    return (G,v,x,y,r)-> v in active ? get_prop(G,v,:color) : "gray"
end
function snapshot_text_func(env,t;verbose = true)
    sched = get_schedule(env)
    active, fixed = get_active_and_fixed_vtxs(sched,t)
    (G,v,x,y,r)->string(
    title_string(get_node_from_id(sched,get_vtx_id(sched, v)),verbose && (v in active)),
    "\n",show_times(sched,v)
    )
end
function plot_schedule_snapshot(env,t;
    mode=:leaf_aligned,
    verbose=true,
    color_function=snapshot_color_func(env,t),
    text_function=snapshot_text_func(env,t;verbose=verbose),
    kwargs...
    )
    plot_project_schedule(env;
        mode=:leaf_aligned,
        color_function=color_function,
        text_function=text_function,
        )
end

function convert_svg_file_to_png(path,outpath=string(splitext(path)[1],".png");
    width = 512,
    )
    run(`inkscape -z -w $width $(path) -e $(outpath)`)
end
function concat_image_files(path1,path2,outpath)
    run(`convert $(path1) $(path2) -append $(outpath)`)
end

"""
    get_graphic_dims

Return `width`, `height` to plot a graphic of size `dx × dy` at scale `scale`,
such that the aspect_ratio falls within appropriate bounds.
"""
function get_graphic_dims(dx,dy;
        scale = 1,
        desired_aspect = 0.7,
        aspect_bounds = [0.5,0.8],
    )
    aspect_ratio = min(max(desired_aspect, aspect_bounds[1]),aspect_bounds[2])
    gw = dx*scale
    gh = dy*scale
    if gh > gw*aspect_ratio
        gw = gh/aspect_ratio
    else
        gh = gw*aspect_ratio
    end
    gw,gh
end

"""
    render_env_and_schedule

Saves a ".png" file that shows the env state at time `t` as well as the schedule
state (i.e., the active nodes) at time `t`.
"""
function render_env_and_schedule(outpath,rstack,env::SearchEnv,t;
        pix_width = 512,
        env_scale = 1cm,
        min_env_aspect = 0.3,
        max_aspect = 0.5,
        base_dir = tempdir(),
        kwargs...
    )
    env_summary = SolutionSummary(env)
    sched_plot = plot_schedule_snapshot(env,t;kwargs...)
    dx,dy = Compose.default_graphic_width, Compose.default_graphic_height
    sched_plot |> SVG(joinpath(base_dir,"plot2.svg"),dx,dy)

    gw, gh = get_graphic_dims(get_graph(env).x_dim,get_graph(env).y_dim;
        scale=1cm,
        desired_aspect=max_aspect - dy/dx,
        aspect_bounds=[min_env_aspect,max_aspect]
    )
    Compose.set_default_graphic_size(gw,gh)
    env_plot = render_env(rstack,get_graph(env),env_summary,t)
    env_plot |> SVG(joinpath(base_dir,"plot1.svg"),gw,gh)

    convert_svg_file_to_png(joinpath(base_dir,"plot1.svg");width=pix_width)
    convert_svg_file_to_png(joinpath(base_dir,"plot2.svg");width=pix_width)
    concat_image_files(joinpath(base_dir,"plot1.png"),joinpath(base_dir,"plot2.png"),outpath)
end


using Base.Intrinsics

import Base: convert, ==, !=, .==, zero, show, length,
             getindex, setindex!, rand, string

# Residues
# ========

"""
Most of the **MIToS** design is created around the `Residue` bitstype. It represents the 20 natural amino acids and a GAP value to represent insertion, deletion but also missing data: ambiguous residues and non natural amino acids.
Each residue is encoded as an integer number, this allows fast indexing operation using Residues of probability or frequency matrices.

**Residue creation and conversion**

Creation and `convert`ion of `Residue`s should be treated carefully.
`Residue` is encoded as an 8 bits type similar to `Int8`, to get faster indexing using `Int(x::Residue)`.
In this way, `Int`, `Int8` and other signed integers returns the integer value encoded by the residue.
Conversions to and from `Char`s and `Uint8` are different, to use the `Char`acter representation in IO operations.

```julia

julia> alanine = Residue('A')
A

julia> Int(alanine)
1

julia> Char(alanine)
'A'

julia> UInt8(alanine) # 0x41 == 65 == 'A'
0x41

julia> for residue in res"ARNDCQEGHILKMFPSTWYV-"
           println(residue, " ", Int(residue))
       end
A 1
R 2
N 3
D 4
C 5
Q 6
E 7
G 8
H 9
I 10
L 11
K 12
M 13
F 14
P 15
S 16
T 17
W 18
Y 19
V 20
- 21

```
"""
bitstype 8 Residue

# Conversion from/to integers
# ---------------------------
#
# This conversions are used for indexing, comparison and operations

convert(::Type{Residue}, x::Int8) = box(Residue,unbox(Int8,x))
convert(::Type{Int8}, x::Residue) = box(Int8,unbox(Residue,x))

for typ in (Int16, Int32, Int64)
    @eval convert(::Type{$typ}, x::Residue) = convert($typ,Int8(x))
    @eval convert(::Type{Residue},  x::$typ)= convert(Residue,Int8(x))
end

Residue(x) = convert(Residue, x)

# Gaps
# ----

"""
`GAP` is the character/number representation on **MIToS** for gaps (also for non standard residues).
Lowercase characters and dots are also encoded as `GAP` in conversion from `String`s and `Char`s.
This `Residue` constant is encoded as `Residue(21)`.
"""
const GAP = Residue(21)

# Conversion from/to Char/Uint8
# -----------------------------
#
# Conversions for input and output.
# Inserts (lowercase and dots) and invalid characters will be converted into gaps ('-').

const _to_char  = Char['A','R','N','D','C','Q','E','G','H','I','L',
                       'K','M','F','P','S','T','W','Y','V','-']

convert(::Type{Char}, x::Residue) = _to_char[ Int( x ) ]

const _to_uint8 = UInt8[ UInt8(ch) for ch in  _to_char ]

convert(::Type{UInt8}, x::Residue) = _to_uint8[ Int( x ) ]

const _to_residue = fill(GAP,256)

for i in 1:length( _to_char )
    _to_residue[ Int(_to_char[i]) ] = Residue(i)
end

convert(::Type{Residue}, x::UInt8) = _to_residue[ Int(x) ];
convert(::Type{Residue}, x::Char)  = _to_residue[ Int(x) ];

# Conversion from/to ASCIIString
# ------------------------------
#
# They are useful for IO and creation of Vector{Residue}

convert(::Type{Residue}, str::ASCIIString) = Residue[ Residue(char) for char in str.data ]
convert(::Type{Residue}, str::Vector{UInt8}) = convert(Vector{Residue}, str)
convert(::Type{ASCIIString}, seq::AbstractVector{Residue}) = ASCIIString(
    UInt8[ UInt8(res) for res in seq ] )

string(seq::AbstractVector{Residue}) = ascii(seq) # "AR..." instead of the standar "[A,R,..."

# For convert a MSA stored as Vector{ASCIIString} to Matrix{Residue}
function convert(::Type{Matrix{Residue}}, sequences::Array{ASCIIString,1})
    nseq = length(sequences)
    nres = length(sequences[1])
    aln = Array(Residue,nseq,nres)
    @inbounds for i in 1:nseq
        seq = sequences[i].data
        if length(seq) == nres
            aln[i,:] = seq
        else
            throw(ErrorException(string(
            "There is an aligned sequence with different number of columns [ ",
            length(seq), " != ", nres," ]: ", ascii(seq) )))
        end
    end
    aln
end

"""
The MIToS macro `@res_str` takes a string and returns a `Vector` of `Residues` (sequence).

```julia

julia> res"MIToS"
5-element Array{MIToS.MSA.Residue,1}:
 M
 I
 T
 -
 S

```
"""
macro res_str(str)
    Residue(str)
end

# Show
# ----

show(io::IO, x::Residue) = write(io, convert(Char, x))

# Comparisons
# -----------

for fun in [:(==), :(!=), :(.==)] # , :(<), :(<=)
    @eval $(fun)(x::Residue,y::Residue) = $(fun)(Int8(x), Int8(y))
end

length(x::Residue) = length(UInt8(x))

# Random
# ------

"""
It chooses from the 20 natural residues (it doesn't generate gaps).

```julia

julia> rand(Residue)
T

julia> rand(Residue, 4, 4)
4x4 Array{MIToS.MSA.Residue,2}:
 L  E  F  L
 R  Y  L  K
 K  V  S  G
 Q  V  M  T

```
"""
rand(r, ::Type{Residue}) = Residue( rand(r, Int8(1):Int8(20)) )

# Three letters (for PDB)
# =======================

const _res2three = ASCIIString[ "ALA", "ARG", "ASN", "ASP", "CYS", "GLN", "GLU", "GLY", "HIS", "ILE", "LEU", "LYS", "MET", "PHE", "PRO", "SER", "THR", "TRP", "TYR", "VAL" ]

# Thanks to Elin- for this list
const _three2res = Dict{ASCIIString, Residue}(
			                 "GLY"=>'G',
                             "ALA"=>'A',
                             "LEU"=>'L',
                             "MET"=>'M',
                             "PHE"=>'F',
                             "TRP"=>'W',
                             "LYS"=>'K',
                             "GLN"=>'Q',
                             "GLU"=>'E',
                             "SER"=>'S',
                             "PRO"=>'P',
                             "VAL"=>'V',
                             "ILE"=>'I',
                             "CYS"=>'C',
                             "TYR"=>'Y',
                             "HIS"=>'H',
                             "ARG"=>'R',
                             "ASN"=>'N',
                             "ASP"=>'D',
                             "THR"=>'T',
                             "MSE"=>'M',
                             "CSS"=>'C',
                             "2AS"=>'D',
                             "3AH"=>'H',
                             "5HP"=>'E',
                             "ACL"=>'R',
                             "AIB"=>'A',
                             "ALM"=>'A',
                             "ALO"=>'T',
                             "ALY"=>'K',
                             "ARM"=>'R',
                             "ASA"=>'D',
                             "ASB"=>'D',
                             "ASK"=>'D',
                             "ASL"=>'D',
                             "ASQ"=>'D',
                             "AYA"=>'A',
                             "BCS"=>'C',
                             "BHD"=>'D',
                             "BMT"=>'T',
                             "BNN"=>'A',
                             "BUC"=>'C',
                             "BUG"=>'L',
                             "C5C"=>'C',
                             "C6C"=>'C',
                             "CCS"=>'C',
                             "CEA"=>'C',
                             "CHG"=>'A',
                             "CLE"=>'L',
                             "CME"=>'C',
                             "CSD"=>'A',
                             "CSO"=>'C',
                             "CSP"=>'C',
                             "CSS"=>'C',
                             "CSW"=>'C',
                             "CXM"=>'M',
                             "CY1"=>'C',
                             "CY3"=>'C',
                             "CYG"=>'C',
                             "CYM"=>'C',
                             "CYQ"=>'C',
                             "DAH"=>'F',
                             "DAL"=>'A',
                             "DAR"=>'R',
                             "DAS"=>'D',
                             "DCY"=>'C',
                             "DGL"=>'E',
                             "DGN"=>'Q',
                             "DHA"=>'A',
                             "DHI"=>'H',
                             "DIL"=>'I',
                             "DIV"=>'V',
                             "DLE"=>'L',
                             "DLY"=>'K',
                             "DNP"=>'A',
                             "DPN"=>'F',
                             "DPR"=>'P',
                             "DSN"=>'S',
                             "DSP"=>'D',
                             "DTH"=>'T',
                             "DTR"=>'W',
                             "DTY"=>'Y',
                             "DVA"=>'V',
                             "EFC"=>'C',
                             "FLA"=>'A',
                             "FME"=>'M',
                             "GGL"=>'E',
                             "GLZ"=>'G',
                             "GMA"=>'E',
                             "GSC"=>'G',
                             "HAC"=>'A',
                             "HAR"=>'R',
                             "HIC"=>'H',
                             "HIP"=>'H',
                             "HMR"=>'R',
                             "HPQ"=>'F',
                             "HTR"=>'W',
                             "HYP"=>'P',
                             "IIL"=>'I',
                             "IYR"=>'Y',
                             "KCX"=>'K',
                             "LLP"=>'K',
                             "LLY"=>'K',
                             "LTR"=>'W',
                             "LYM"=>'K',
                             "LYZ"=>'K',
                             "MAA"=>'A',
                             "MEN"=>'N',
                             "MHS"=>'H',
                             "MIS"=>'S',
                             "MLE"=>'L',
                             "MPQ"=>'G',
                             "MSA"=>'G',
                             "MSE"=>'M',
                             "MVA"=>'V',
                             "NEM"=>'H',
                             "NEP"=>'H',
                             "NLE"=>'L',
                             "NLN"=>'L',
                             "NLP"=>'L',
                             "NMC"=>'G',
                             "OAS"=>'S',
                             "OCS"=>'C',
                             "OMT"=>'M',
                             "PAQ"=>'Y',
                             "PCA"=>'E',
                             "PEC"=>'C',
                             "PHI"=>'F',
                             "PHL"=>'F',
                             "PR3"=>'C',
                             "PRR"=>'A',
                             "PTR"=>'Y',
                             "SAC"=>'S',
                             "SAR"=>'G',
                             "SCH"=>'C',
                             "SCS"=>'C',
                             "SCY"=>'C',
                             "SEL"=>'S',
                             "SEP"=>'S',
                             "SET"=>'S',
                             "SHC"=>'C',
                             "SHR"=>'K',
                             "SOC"=>'C',
                             "STY"=>'Y',
                             "SVA"=>'S',
                             "TIH"=>'A',
                             "TPL"=>'W',
                             "TPO"=>'T',
                             "TPQ"=>'A',
                             "TRG"=>'K',
                             "TRO"=>'W',
                             "TYB"=>'Y',
                             "TYQ"=>'Y',
                             "TYS"=>'Y',
                             "TYY"=>'Y',
                             "AGM"=>'R',
                             "GL3"=>'G',
                             "SMC"=>'C',
                             "ASX"=>'B',
                             "CGU"=>'E',
                             "CSX"=>'C',
                             "GLX"=>'Z',
                             "UNK"=>'X'
                            )

"""
This function returns the three letter name of the `Residue`.

```julia
julia> residue2three(Residue('G'))
"GLY"

```
"""
residue2three(res::Residue) = _res2three[Int(res)]

"""
It takes a three letter residue name and returns the `Residue`.

```julia
julia> three2residue("ALA")
A

```
"""
function three2residue(res::ASCIIString)
    if length(res) == 3
        get(_three2res, uppercase(res), GAP)
    else
        throw(ErrorException("The residue name should have 3 letters."))
    end
end


using SbmlInterface
using ModelingToolkit
using OrdinaryDiffEq
using Test

@testset "SbmlInterface.jl" begin
    @testset "sbml2modelingtoolkit.jl" begin include("sbml2modelingtoolkit.jl") end
end


using WAV
using ArgParse

push!(LOAD_PATH, pwd())

using Plotting

# Main entry
function main(wav_file)

	# Read the WAV file
	s, fs = wavread(wav_file)

	plot_wave(s, fs)
	plot_frec(s, fs)
	plot_spectrogram(s, fs)
end

if PROGRAM_FILE == "SignalAnalysis.jl"

	# Set backend renderer
	set_plot()

	# Parse the argument
	s = ArgParseSettings("Audio dialing analisys")

	@add_arg_table s begin
		"--audio"
		help = "The audio file (WAV)"
		required = true
	end

	parsed_args = parse_args(ARGS, s)

	main(parsed_args["audio"])
end


using Octavian, StaticArrays, LinearAlgebra, BenchmarkTools, ProgressMeter

# BLAS.set_num_threads(1)

# For laptops that thermally throttle, you can set the `JULIA_SLEEP_BENCH` environment variable for #seconds to sleep before each `@belapsed`
const SLEEPTIME = parse(Float64, get(ENV, "JULIA_SLEEP_BENCH", "0"))
maybe_sleep() = iszero(SLEEPTIME) || sleep(SLEEPTIME)
# BenchmarkTools.DEFAULT_PARAMETERS.samples = 1_000_000
# BenchmarkTools.DEFAULT_PARAMETERS.seconds = 10

matrix_sizes(x::Integer) = (x,x,x)
matrix_sizes(x::NTuple{3}) = x

const matmulmethodnames = [:SMatrix, :MMatrix, :OctavianStatic, :OctavianDynamic];
function fill_bench_results!(br, lp, (M,K,N), t, i, j)
    name = matmulmethodnames[j]
    br[i,j,1] = t
    gflops = 2e-9M*K*N / t
    br[i,j,2] = gflops
    lp[j+1] = (name, gflops)
    nothing
end

function runbenches(sr, ::Type{T} = Float64) where {T}
    bench_results = Array{Float64}(undef, length(sr), 4, 2)
    p = Progress(length(sr))
    last_perfs = Vector{Tuple{Symbol,Union{Float64,NTuple{3,Int}}}}(undef, size(bench_results,2) + 1)
    for (i,s) ∈ enumerate(sr)
        M, K, N = matrix_sizes(s)
        last_perfs[1] = (:Size, (M,K,N))
        Astatic = @SMatrix rand(T, M, K);
        Bstatic = @SMatrix rand(T, K, N);
        maybe_sleep()
        t = @belapsed $(Ref(Astatic))[] * $(Ref(Bstatic))[]
        fill_bench_results!(bench_results, last_perfs, (M,K,N), t, i, 1)
        Amutable = MArray(Astatic);
        Bmutable = MArray(Bstatic);
        Cmutable = MMatrix{M,N,T}(undef);
        maybe_sleep()
        t = @belapsed mul!($Cmutable, $Amutable, $Bmutable)
        fill_bench_results!(bench_results, last_perfs, (M,K,N), t, i, 2)
        Cmutable0 = copy(Cmutable); Cmutable .= NaN;
        maybe_sleep()
        t = @belapsed matmul!($Cmutable, $Amutable, $Bmutable)
        fill_bench_results!(bench_results, last_perfs, (M,K,N), t, i, 3)
        A = Array(Amutable); B = Array(Bmutable); C = Array(Cmutable);
        maybe_sleep()
        t = @belapsed matmul!($C, $A, $B)
        fill_bench_results!(bench_results, last_perfs, (M,K,N), t, i, 4)
        @assert Array(Cmutable) ≈ Array(Cmutable0) ≈ C
        ProgressMeter.next!(p; showvalues = last_perfs)
    end
    bench_results
end

sizerange = 2:48
br = runbenches(sizerange);
using DataFrames, VegaLite

df = DataFrame(@view(br[:,:,2]));
rename!(df, matmulmethodnames);
df.Size = sizerange

function pick_suffix(desc = "")
    suffix = if Bool(Octavian.VectorizationBase.has_feature(Val(:x86_64_avx512f)))
        "AVX512"
    elseif Bool(Octavian.VectorizationBase.has_feature(Val(:x86_64_avx2)))
        "AVX2"
    elseif Bool(Octavian.VectorizationBase.has_feature(Val(:x86_64_avx)))
        "AVX"
    else
        "REGSIZE$(Octavian.VectorizationBase.register_size())"
    end
    if desc != ""
        suffix *= '_' * desc
    end
    "$(Sys.CPU_NAME)_$suffix"
end

dfs = stack(df, matmulmethodnames, variable_name = :MatMulType, value_name = :GFLOPS);
p = dfs |> @vlplot(:line, x = :Size, y = :GFLOPS, width = 900, height = 600, color = {:MatMulType});
save(joinpath(pkgdir(Octavian), "docs/src/assets/sizedarraybenchmarks_$(pick_suffix()).svg"), p)