text:
module Renderer

export respond, redirect, render

import Revise
import HTTP, Markdown, Logging, FilePaths, SHA
import Genie

const DEFAULT_CHARSET = "charset=utf-8"
const DEFAULT_CONTENT_TYPE = :html
const DEFAULT_LAYOUT_FILE = "app"

const VIEWS_FOLDER = "views"

const BUILD_NAME = "GenieViews"

"""
    const CONTENT_TYPES = Dict{Symbol,String}

Collection of content-types mappings between user friendly short names and
MIME types plus charset.
"""
const CONTENT_TYPES = Dict{Symbol,String}(
  :html       => "text/html; $DEFAULT_CHARSET",
  :text       => "text/plain; $DEFAULT_CHARSET",
  :json       => "application/json; $DEFAULT_CHARSET",
  :javascript => "application/javascript; $DEFAULT_CHARSET",
  :xml        => "text/xml; $DEFAULT_CHARSET",
  :markdown   => "text/markdown; $DEFAULT_CHARSET",
  :favicon    => "image/x-icon",
)

const MIME_TYPES = Dict(
  :html       => MIME"text/html",
  :text       => MIME"text/plain",
  :json       => MIME"application/json",
  :javascript => MIME"application/javascript",
  :xml        => MIME"text/xml",
  :markdown   => MIME"text/markdown"
)

push_content_type(s::Symbol, content_type::String, charset::String = DEFAULT_CHARSET) = (CONTENT_TYPES[s] = "$content_type; $charset")

const ResourcePath = Union{String,Symbol}
const HTTPHeaders = Dict{String,String}

const Path = FilePaths.Path
const FilePath = Union{FilePaths.PosixPath,FilePaths.WindowsPath}
const filepath = FilePaths.Path

macro path_str(s)
  :(FilePaths.@p_str($s))
end

export FilePath, filepath, Path, @path_str
export @vars
export WebRenderable

init_task_local_storage() = (haskey(task_local_storage(), :__vars) || task_local_storage(:__vars, Dict{Symbol,Any}()))
init_task_local_storage()

"""
    mutable struct WebRenderable

Represents an object that can be rendered on the web as a HTTP Response
"""
mutable struct WebRenderable
  body::String
  content_type::Symbol
  status::Int
  headers::HTTPHeaders
end

"""
    WebRenderable(body::String)

Creates a new instance of `WebRenderable` with `body` as the body of the response and
default content type, no headers, and 200 status code.

#Examples
```jldoctest
julia> Genie.Renderer.WebRenderable("hello")
Genie.Renderer.WebRenderable("hello", :html, 200, Dict{String,String}())
```
"""
WebRenderable(body::String) = WebRenderable(body, DEFAULT_CONTENT_TYPE, 200, HTTPHeaders())

"""
    WebRenderable(body::String, content_type::Symbol)

Creates a new instance of `WebRenderable` with `body` as the body of the response and
`content_type` as the content type, no headers, and 200 status code.

#Examples
```jldoctest
julia> Genie.Renderer.WebRenderable("hello", :json)
Genie.Renderer.WebRenderable("hello", :json, 200, Dict{String,String}())
```
"""
WebRenderable(body::String, content_type::Symbol) = WebRenderable(body, content_type, 200, HTTPHeaders())

"""
    WebRenderable(; body::String = "", content_type::Symbol = DEFAULT_CONTENT_TYPE,
                    status::Int = 200, headers::HTTPHeaders = HTTPHeaders())

Creates a new instance of `WebRenderable` using the values passed as keyword arguments.

#Examples
```jldoctest
julia> Genie.Renderer.WebRenderable()
Genie.Renderer.WebRenderable("", :html, 200, Dict{String,String}())

julia> Genie.Renderer.WebRenderable(body = "bye", content_type = :javascript, status = 301, headers = Dict("Location" => "/bye"))
Genie.Renderer.WebRenderable("bye", :javascript, 301, Dict("Location" => "/bye"))
```
"""
WebRenderable(; body::String = "", content_type::Symbol = DEFAULT_CONTENT_TYPE,
                status::Int = 200, headers::HTTPHeaders = HTTPHeaders()) = WebRenderable(body, content_type, status, headers)

"""
    WebRenderable(wr::WebRenderable, status::Int, headers::HTTPHeaders)

Returns `wr` overwriting its `status` and `headers` fields with the passed arguments.

#Examples
```jldoctest
julia> Genie.Renderer.WebRenderable(Genie.Renderer.WebRenderable(body = "good morning", content_type = :javascript), 302, Dict("Location" => "/morning"))
Genie.Renderer.WebRenderable("good morning", :javascript, 302, Dict("Location" => "/morning"))
```
"""
function WebRenderable(wr::WebRenderable, status::Int, headers::HTTPHeaders)
  wr.status = status
  wr.headers = headers

  wr
end

function WebRenderable(wr::WebRenderable, content_type::Symbol, status::Int, headers::HTTPHeaders)
  wr.content_type = content_type
  wr.status = status
  wr.headers = headers

  wr
end

function WebRenderable(f::Function, args...)
  fr::String = try
    f()::String
  catch
    Base.invokelatest(f)::String
  end

  WebRenderable(fr, args...)
end

function render end

### REDIRECT RESPONSES ###

"""
Sets redirect headers and prepares the `Response`.
"""
function redirect(location::String, code::Int = 302, headers::HTTPHeaders = HTTPHeaders()) :: HTTP.Response
  headers["Location"] = location
  WebRenderable("Redirecting you to $location", :text, code, headers) |> respond
end
function redirect(named_route::Symbol, code::Int = 302, headers::HTTPHeaders = HTTPHeaders(); route_args...) :: HTTP.Response
  redirect(Genie.Router.linkto(named_route; route_args...), code, headers)
end

"""
    hasrequested(content_type::Symbol) :: Bool

Checks wheter or not the requested content type matches `content_type`.
"""
function hasrequested(content_type::Symbol) :: Bool
  task_local_storage(:__params)[:response_type] == content_type
end

### RESPONSES ###

"""
Constructs a `Response` corresponding to the content-type sof the request.
"""
function respond(r::WebRenderable) :: HTTP.Response
  haskey(r.headers, "Content-Type") || (r.headers["Content-Type"] = CONTENT_TYPES[r.content_type])

  HTTP.Response(r.status, [h for h in r.headers], body = r.body)
end

function respond(response::HTTP.Response) :: HTTP.Response
  response
end

function respond(body::String, params::Dict{Symbol,T})::HTTP.Response where {T}
  r = params[:RESPONSE]
  r.data = body

  r |> respond
end

function respond(err::T, content_type::Union{Symbol,String} = Genie.Router.responsetype(), code::Int = 500) :: T where {T<:Exception}
  T
end

function respond(body::String, content_type::Union{Symbol,String} = Genie.Router.responsetype(), code::Int = 200) :: HTTP.Response
  HTTP.Response(code,
                (isa(content_type, Symbol) ? ["Content-Type" => CONTENT_TYPES[content_type]] : ["Content-Type" => content_type]),
                body = body)
end

function respond(body, code::Int = 200, headers::HTTPHeaders = HTTPHeaders())
  HTTP.Response(code, [h for h in headers], body = string(body))
end

function respond(f::Function, code::Int = 200, headers::HTTPHeaders = HTTPHeaders())
  respond(f(), code, headers)
end

"""
    registervars(vars...) :: Nothing

Loads the rendering vars into the task's scope
"""
function registervars(vars...) :: Nothing
  init_task_local_storage()
  task_local_storage(:__vars, merge(task_local_storage(:__vars), Dict{Symbol,Any}(vars)))

  nothing
end

function injectvars() :: String
  output = ""
  for kv in task_local_storage(:__vars)
    output *= "$(kv[1]) = Genie.Renderer.@vars($(repr(kv[1]))) \n"
  end

  output
end

function injectvars(context::Module) :: Nothing
  for kv in task_local_storage(:__vars)
    # isdefined(context, Symbol(kv[1])) ||
    Core.eval(context, Meta.parse("$(kv[1]) = Renderer.@vars($(repr(kv[1])))"))
  end

  nothing
end

"""
    view_file_info(path::String, supported_extensions::Vector{String}) :: Tuple{String,String}

Extracts path and extension info about a file
"""
function view_file_info(path::String, supported_extensions::Vector{String}) :: Tuple{String,String}

  _path, _extension = "", ""

  if isfile(path)
    _path_without_extension, _extension = Base.Filesystem.splitext(path)
    _path = _path_without_extension * _extension
  else
     for file_extension in supported_extensions
       if isfile(path * file_extension)
         _path, _extension = path * file_extension, file_extension
         break
       end
     end
   end

  # to check that it is among supported files
  # in(_extension, supported_extensions) || @warn("$_extension is not among $supported_extensions")

  return _path, _extension
end

function vars_signature() :: String
  task_local_storage(:__vars) |> keys |> collect |> sort |> string
end

"""
    function_name(file_path::String)

Generates function name for generated HTML+Julia views.
"""
function function_name(file_path::String) :: String
  "func_$(SHA.sha1( relpath(isempty(file_path) ? " " : file_path) * vars_signature() ) |> bytes2hex)"
end

"""
    m_name(file_path::String)

Generates module name for generated HTML+Julia views.
"""
function m_name(file_path::String) :: String
  string(SHA.sha1( relpath(isempty(file_path) ? " " : file_path) * vars_signature()) |> bytes2hex)
end

"""
    build_is_stale(file_path::String, build_path::String) :: Bool

Checks if the view template has been changed since the last time the template was compiled.
"""
function build_is_stale(file_path::String, build_path::String) :: Bool
  isfile(file_path) || return true

  file_mtime = stat(file_path).mtime
  build_mtime = stat(build_path).mtime
  status = file_mtime > build_mtime

  status
end

"""
    build_module(content::String, path::String, mod_name::String) :: String

Persists compiled Julia view data to file and returns the path
"""
function build_module(content::String, path::String, mod_name::String) :: String
  module_path = joinpath(Genie.config.path_build, BUILD_NAME, mod_name)

  isdir(dirname(module_path)) || mkpath(dirname(module_path))

  open(module_path, "w") do io
    write(io, "# $path \n\n")
    write(io, content)
  end

  module_path
end

"""
    preparebuilds() :: Bool

Sets up the build folder and the build module file for generating the compiled views.
"""
function preparebuilds(subfolder = BUILD_NAME) :: Bool
  build_path = joinpath(Genie.config.path_build, subfolder)
  isdir(build_path) || mkpath(build_path)

  true
end

function purgebuilds(subfolder = BUILD_NAME) :: Bool
  rm(joinpath(Genie.config.path_build, subfolder), force = true, recursive = true)

  true
end

function changebuilds(subfolder = BUILD_NAME) :: Bool
  Genie.config.path_build = Genie.Configuration.buildpath()
  preparebuilds()
end

"""
    @vars

Utility macro for accessing view vars
"""
macro vars()
  :(task_local_storage(:__vars))
end

"""
    @vars(key)

Utility macro for accessing view vars stored under `key`
"""
macro vars(key)
  :(task_local_storage(:__vars)[$key])
end

"""
    @vars(key, value)

Utility macro for setting a new view var, as `key` => `value`
"""
macro vars(key, value)
  quote
    try
      task_local_storage(:__vars)[$key] = $(esc(value))
    catch
      init_task_local_storage()
      task_local_storage(:__vars)[$key] = $(esc(value))
    end
  end
end

function set_negotiated_content(req::HTTP.Request, res::HTTP.Response, params::Dict{Symbol,Any})
  req_type = Genie.Router.request_type(req)
  params[:response_type] = req_type === nothing ? DEFAULT_CONTENT_TYPE : req_type
  params[Genie.PARAMS_MIME_KEY] = get(MIME_TYPES, params[:response_type], MIME_TYPES[DEFAULT_CONTENT_TYPE])
  push!(res.headers, "Content-Type" => get(CONTENT_TYPES, params[:response_type], "text/html"))

  req, res, params
end

"""
    negotiate_content(req::Request, res::Response, params::Params) :: Response

Computes the content-type of the `Response`, based on the information in the `Request`.
"""
function negotiate_content(req::HTTP.Request, res::HTTP.Response, params::Dict{Symbol,Any}) :: Tuple{HTTP.Request,HTTP.Response,Dict{Symbol,Any}}
  headers = Dict(res.headers)

  if haskey(params, :response_type) && in(Symbol(params[:response_type]), collect(keys(CONTENT_TYPES)) )
    params[:response_type] = Symbol(params[:response_type])
    params[Genie.PARAMS_MIME_KEY] = MIME_TYPES[params[:response_type]]
    headers["Content-Type"] = CONTENT_TYPES[params[:response_type]]

    res.headers = [k for k in headers]

    return req,res,params
  end

  negotiation_header = haskey(headers, "Accept") ? "Accept" : ( haskey(headers, "Content-Type") ? "Content-Type" : "" )

  if isempty(negotiation_header)
    req, res, params = set_negotiated_content(req, res, params)
    return req, res, params
  end

  accept_parts = split(headers[negotiation_header], ";")

  if isempty(accept_parts)
    req, res, params = set_negotiated_content(req, res, params)
    return req, res, params
  end

  accept_order_parts = split(accept_parts[1], ",")

  if isempty(accept_order_parts)
    req, res, params = set_negotiated_content(req, res, params)
    return req, res, params
  end

  for mime in accept_order_parts
    if occursin("/", mime)
      content_type = split(mime, "/")[2] |> lowercase |> Symbol
      if haskey(CONTENT_TYPES, content_type)
        params[:response_type] = content_type
        params[Genie.PARAMS_MIME_KEY] = MIME_TYPES[params[:response_type]]
        headers["Content-Type"] = CONTENT_TYPES[params[:response_type]]

        res.headers = [k for k in headers]
        return req,res,params
      end
    end
  end

  req, res, params = set_negotiated_content(req, res, params)

  return req, res, params
end

push!(Genie.Router.content_negotiation_hooks, negotiate_content)

include("renderers/Html.jl")
include("renderers/Json.jl")
include("renderers/Js.jl")

end


text:
struct SimpleOG{TI <: Integer, T <: Real,
                TR <: AbstractArray{T}, TQ <: AbstractArray{T}}
    B :: TI
    M :: TI
    α :: T
    β :: T
    R :: TR
    Q :: TQ
end

function SimpleOG{T <: Real}(;B::Integer=10, M::Integer=5, α::T=0.5, β::T=0.9)

    u(c) = c^α
    n = B + M + 1
    m = M + 1

    R = Matrix{T}(n, m)
    Q = zeros(Float64,n,m,n)

    for a in 0:M
        Q[:, a + 1, (a:(a + B)) + 1] = 1 / (B + 1)
        for s in 0:(B + M)
            R[s + 1, a + 1] = a<=s ? u(s - a) : -Inf
        end
    end

    return SimpleOG(B, M, α, β, R, Q)
end

ext: jl


text:
# idea: use a vector of RDiffs and a stack of Ops
using DataStructures
import Base: convert, promote_rule, +, -, *, /, >, <, log

immutable RDiff <: Real
    val::Float64
    adj::Float64
    RDiff(val, adj) = new(val, adj)
end
typealias Converts Union{Float64, Int, Irrational, Rational}
RDiff(x::Converts) = RDiff(x, 0.)

convert(::Type{RDiff}, x::Converts) = RDiff(x)
promote_rule(::Type{Bool}, ::Type{RDiff}) = RDiff
promote_rule(::Type{Float64}, ::Type{RDiff}) = RDiff
promote_rule{T<:Integer}(::Type{T}, ::Type{RDiff}) = RDiff

value(x::RDiff) = x.val
adjoint(x::RDiff) = x.adj
>(x::RDiff, y::RDiff) = x.val > y.val
<(x::RDiff, y::RDiff) = x.val < y.val

typealias Node Tuple{Function, Vector{Int}, Vector{Int}}

global edges = Vector{RDiff}()
global ops = Stack(Node)

function grad(f)

    function ∇f(x)
        Nx = length(x)
        copy!(edges, x)
        f(y)
        edges[end] = RDiff(value(edges[end]), 1.)
        for op in ops
            backprop(op...)
        end
        map(value, edges[1:Nx])
    end

    return ∇f
end

macro diff_rule(sig, code)
    fn = _fn_of_vals(sig)
    op = sig.args[1]
    argsyms = Symbol[s.args[1] for s in sig.args[2:end]]
    ex = quote
        function $op($(sig.args[2:end]...))
            res = map(RDiff, $fn)
            le = length(edges)
            if ndims(res) == 0
                push!(edges, res)
                push!(ops, ($op, [$(argsyms...)], [le + 1]))
            else
                append!(edges, res[:])
                lr = length(res)
                push!(ops, ($op, [$(argsyms...)], collect(le + (1:lr))))
            end
            res
        end

        function backprop($op, )
            $code
            nothing
        end
    end
    esc(ex)
end

_fn_of_vals(x) = x
function _fn_of_vals(ex::Expr)
    if ex.head == :(::)
        out = :(value($(ex.args[1])))
    else
        out = copy(ex)
        for i in eachindex(out.args)
            out.args[i] = _fn_of_vals(out.args[i])
        end
    end
    out
end

@diff_rule log(x::RDiff) begin
    x.adj += this.adj / x.val
end

@diff_rule +(x::RDiff, y::RDiff) begin
    x.adj += this.adj
    y.adj += this.adj
end

@diff_rule -(x::RDiff, y::RDiff) begin
    x.adj += this.adj
    y.adj += -this.adj
end

@diff_rule *(x::RDiff, y::RDiff) begin
    x.adj += this.adj * y.val
    y.adj += this.adj * x.val
end

@diff_rule /(x::RDiff, y::RDiff) begin
    x.adj += this.adj / y.val
    y.adj += -this.adj * x.val / y.val^2
end

@diff_rule -(x::RDiff) begin
    x.adj += -this.adj
end


text:
#- Completion and Error Codes ----------------------------------------------*/

const VI_SUCCESS = 0
const VI_ERROR  = -2147483647-1  # 0x80000000

const VI_SUCCESS_EVENT_EN                 =  0x3FFF0002 # 3FFF0002,  1073676290
const VI_SUCCESS_EVENT_DIS                =  0x3FFF0003 # 3FFF0003,  1073676291
const VI_SUCCESS_QUEUE_EMPTY              =  0x3FFF0004 # 3FFF0004,  1073676292
const VI_SUCCESS_TERM_CHAR                =  0x3FFF0005 # 3FFF0005,  1073676293
const VI_SUCCESS_MAX_CNT                  =  0x3FFF0006 # 3FFF0006,  1073676294
const VI_SUCCESS_DEV_NPRESENT             =  0x3FFF007D # 3FFF007D,  1073676413
const VI_SUCCESS_TRIG_MAPPED              =  0x3FFF007E # 3FFF007E,  1073676414
const VI_SUCCESS_QUEUE_NEMPTY             =  0x3FFF0080 # 3FFF0080,  1073676416
const VI_SUCCESS_NCHAIN                   =  0x3FFF0098 # 3FFF0098,  1073676440
const VI_SUCCESS_NESTED_SHARED            =  0x3FFF0099 # 3FFF0099,  1073676441
const VI_SUCCESS_NESTED_EXCLUSIVE         =  0x3FFF009A # 3FFF009A,  1073676442
const VI_SUCCESS_SYNC                     =  0x3FFF009B # 3FFF009B,  1073676443

const VI_WARN_QUEUE_OVERFLOW              =  0x3FFF000C # 3FFF000C,  1073676300
const VI_WARN_CONFIG_NLOADED              =  0x3FFF0077 # 3FFF0077,  1073676407
const VI_WARN_NULL_OBJECT                 =  0x3FFF0082 # 3FFF0082,  1073676418
const VI_WARN_NSUP_ATTR_STATE             =  0x3FFF0084 # 3FFF0084,  1073676420
const VI_WARN_UNKNOWN_STATUS              =  0x3FFF0085 # 3FFF0085,  1073676421
const VI_WARN_NSUP_BUF                    =  0x3FFF0088 # 3FFF0088,  1073676424
const VI_WARN_EXT_FUNC_NIMPL              =  0x3FFF00A9 # 3FFF00A9,  1073676457

const VI_ERROR_SYSTEM_ERROR      = VI_ERROR+0x3FFF0000 # BFFF0000, -1073807360
const VI_ERROR_INV_OBJECT        = VI_ERROR+0x3FFF000E # BFFF000E, -1073807346
const VI_ERROR_RSRC_LOCKED       = VI_ERROR+0x3FFF000F # BFFF000F, -1073807345
const VI_ERROR_INV_EXPR          = VI_ERROR+0x3FFF0010 # BFFF0010, -1073807344
const VI_ERROR_RSRC_NFOUND       = VI_ERROR+0x3FFF0011 # BFFF0011, -1073807343
const VI_ERROR_INV_RSRC_NAME     = VI_ERROR+0x3FFF0012 # BFFF0012, -1073807342
const VI_ERROR_INV_ACC_MODE      = VI_ERROR+0x3FFF0013 # BFFF0013, -1073807341
const VI_ERROR_TMO               = VI_ERROR+0x3FFF0015 # BFFF0015, -1073807339
const VI_ERROR_CLOSING_FAILED    = VI_ERROR+0x3FFF0016 # BFFF0016, -1073807338
const VI_ERROR_INV_DEGREE        = VI_ERROR+0x3FFF001B # BFFF001B, -1073807333
const VI_ERROR_INV_JOB_ID        = VI_ERROR+0x3FFF001C # BFFF001C, -1073807332
const VI_ERROR_NSUP_ATTR         = VI_ERROR+0x3FFF001D # BFFF001D, -1073807331
const VI_ERROR_NSUP_ATTR_STATE   = VI_ERROR+0x3FFF001E # BFFF001E, -1073807330
const VI_ERROR_ATTR_READONLY     = VI_ERROR+0x3FFF001F # BFFF001F, -1073807329
const VI_ERROR_INV_LOCK_TYPE     = VI_ERROR+0x3FFF0020 # BFFF0020, -1073807328
const VI_ERROR_INV_ACCESS_KEY    = VI_ERROR+0x3FFF0021 # BFFF0021, -1073807327
const VI_ERROR_INV_EVENT         = VI_ERROR+0x3FFF0026 # BFFF0026, -1073807322
const VI_ERROR_INV_MECH          = VI_ERROR+0x3FFF0027 # BFFF0027, -1073807321
const VI_ERROR_HNDLR_NINSTALLED  = VI_ERROR+0x3FFF0028 # BFFF0028, -1073807320
const VI_ERROR_INV_HNDLR_REF     = VI_ERROR+0x3FFF0029 # BFFF0029, -1073807319
const VI_ERROR_INV_CONTEXT       = VI_ERROR+0x3FFF002A # BFFF002A, -1073807318
const VI_ERROR_QUEUE_OVERFLOW    = VI_ERROR+0x3FFF002D # BFFF002D, -1073807315
const VI_ERROR_NENABLED          = VI_ERROR+0x3FFF002F # BFFF002F, -1073807313
const VI_ERROR_ABORT             = VI_ERROR+0x3FFF0030 # BFFF0030, -1073807312
const VI_ERROR_RAW_WR_PROT_VIOL  = VI_ERROR+0x3FFF0034 # BFFF0034, -1073807308
const VI_ERROR_RAW_RD_PROT_VIOL  = VI_ERROR+0x3FFF0035 # BFFF0035, -1073807307
const VI_ERROR_OUTP_PROT_VIOL    = VI_ERROR+0x3FFF0036 # BFFF0036, -1073807306
const VI_ERROR_INP_PROT_VIOL     = VI_ERROR+0x3FFF0037 # BFFF0037, -1073807305
const VI_ERROR_BERR              = VI_ERROR+0x3FFF0038 # BFFF0038, -1073807304
const VI_ERROR_IN_PROGRESS       = VI_ERROR+0x3FFF0039 # BFFF0039, -1073807303
const VI_ERROR_INV_SETUP         = VI_ERROR+0x3FFF003A # BFFF003A, -1073807302
const VI_ERROR_QUEUE_ERROR       = VI_ERROR+0x3FFF003B # BFFF003B, -1073807301
const VI_ERROR_ALLOC             = VI_ERROR+0x3FFF003C # BFFF003C, -1073807300
const VI_ERROR_INV_MASK          = VI_ERROR+0x3FFF003D # BFFF003D, -1073807299
const VI_ERROR_IO                = VI_ERROR+0x3FFF003E # BFFF003E, -1073807298
const VI_ERROR_INV_FMT           = VI_ERROR+0x3FFF003F # BFFF003F, -1073807297
const VI_ERROR_NSUP_FMT          = VI_ERROR+0x3FFF0041 # BFFF0041, -1073807295
const VI_ERROR_LINE_IN_USE       = VI_ERROR+0x3FFF0042 # BFFF0042, -1073807294
const VI_ERROR_NSUP_MODE         = VI_ERROR+0x3FFF0046 # BFFF0046, -1073807290
const VI_ERROR_SRQ_NOCCURRED     = VI_ERROR+0x3FFF004A # BFFF004A, -1073807286
const VI_ERROR_INV_SPACE         = VI_ERROR+0x3FFF004E # BFFF004E, -1073807282
const VI_ERROR_INV_OFFSET        = VI_ERROR+0x3FFF0051 # BFFF0051, -1073807279
const VI_ERROR_INV_WIDTH         = VI_ERROR+0x3FFF0052 # BFFF0052, -1073807278
const VI_ERROR_NSUP_OFFSET       = VI_ERROR+0x3FFF0054 # BFFF0054, -1073807276
const VI_ERROR_NSUP_VAR_WIDTH    = VI_ERROR+0x3FFF0055 # BFFF0055, -1073807275
const VI_ERROR_WINDOW_NMAPPED    = VI_ERROR+0x3FFF0057 # BFFF0057, -1073807273
const VI_ERROR_RESP_PENDING      = VI_ERROR+0x3FFF0059 # BFFF0059, -1073807271
const VI_ERROR_NLISTENERS        = VI_ERROR+0x3FFF005F # BFFF005F, -1073807265
const VI_ERROR_NCIC              = VI_ERROR+0x3FFF0060 # BFFF0060, -1073807264
const VI_ERROR_NSYS_CNTLR        = VI_ERROR+0x3FFF0061 # BFFF0061, -1073807263
const VI_ERROR_NSUP_OPER         = VI_ERROR+0x3FFF0067 # BFFF0067, -1073807257
const VI_ERROR_INTR_PENDING      = VI_ERROR+0x3FFF0068 # BFFF0068, -1073807256
const VI_ERROR_ASRL_PARITY       = VI_ERROR+0x3FFF006A # BFFF006A, -1073807254
const VI_ERROR_ASRL_FRAMING      = VI_ERROR+0x3FFF006B # BFFF006B, -1073807253
const VI_ERROR_ASRL_OVERRUN      = VI_ERROR+0x3FFF006C # BFFF006C, -1073807252
const VI_ERROR_TRIG_NMAPPED      = VI_ERROR+0x3FFF006E # BFFF006E, -1073807250
const VI_ERROR_NSUP_ALIGN_OFFSET = VI_ERROR+0x3FFF0070 # BFFF0070, -1073807248
const VI_ERROR_USER_BUF          = VI_ERROR+0x3FFF0071 # BFFF0071, -1073807247
const VI_ERROR_RSRC_BUSY         = VI_ERROR+0x3FFF0072 # BFFF0072, -1073807246
const VI_ERROR_NSUP_WIDTH        = VI_ERROR+0x3FFF0076 # BFFF0076, -1073807242
const VI_ERROR_INV_PARAMETER     = VI_ERROR+0x3FFF0078 # BFFF0078, -1073807240
const VI_ERROR_INV_PROT          = VI_ERROR+0x3FFF0079 # BFFF0079, -1073807239
const VI_ERROR_INV_SIZE          = VI_ERROR+0x3FFF007B # BFFF007B, -1073807237
const VI_ERROR_WINDOW_MAPPED     = VI_ERROR+0x3FFF0080 # BFFF0080, -1073807232
const VI_ERROR_NIMPL_OPER        = VI_ERROR+0x3FFF0081 # BFFF0081, -1073807231
const VI_ERROR_INV_LENGTH        = VI_ERROR+0x3FFF0083 # BFFF0083, -1073807229
const VI_ERROR_INV_MODE          = VI_ERROR+0x3FFF0091 # BFFF0091, -1073807215
const VI_ERROR_SESN_NLOCKED      = VI_ERROR+0x3FFF009C # BFFF009C, -1073807204
const VI_ERROR_MEM_NSHARED       = VI_ERROR+0x3FFF009D # BFFF009D, -1073807203
const VI_ERROR_LIBRARY_NFOUND    = VI_ERROR+0x3FFF009E # BFFF009E, -1073807202
const VI_ERROR_NSUP_INTR         = VI_ERROR+0x3FFF009F # BFFF009F, -1073807201
const VI_ERROR_INV_LINE          = VI_ERROR+0x3FFF00A0 # BFFF00A0, -1073807200
const VI_ERROR_FILE_ACCESS       = VI_ERROR+0x3FFF00A1 # BFFF00A1, -1073807199
const VI_ERROR_FILE_IO           = VI_ERROR+0x3FFF00A2 # BFFF00A2, -1073807198
const VI_ERROR_NSUP_LINE         = VI_ERROR+0x3FFF00A3 # BFFF00A3, -1073807197
const VI_ERROR_NSUP_MECH         = VI_ERROR+0x3FFF00A4 # BFFF00A4, -1073807196
const VI_ERROR_INTF_NUM_NCONFIG  = VI_ERROR+0x3FFF00A5 # BFFF00A5, -1073807195
const VI_ERROR_CONN_LOST         = VI_ERROR+0x3FFF00A6 # BFFF00A6, -1073807194
const VI_ERROR_MACHINE_NAVAIL    = VI_ERROR+0x3FFF00A7 # BFFF00A7, -1073807193
const VI_ERROR_NPERMISSION       = VI_ERROR+0x3FFF00A8 # BFFF00A8, -1073807192

# Dictionary mapping codes to more verbose information #
# See Appendix A of the NI-VISA Programmers Reference
#Dictionary maps integer codes to tuples of (Code, Meaning)
codes = Dict{Int,Tuple{String,String}}(
    VI_SUCCESS => ("VI_SUCCESS", "Operation completed successfully."),
    VI_SUCCESS_EVENT_EN => ("VI_SUCCESS_EVENT_EN", "Specified event is already enabled for at least one of the specified mechanisms."),
    VI_SUCCESS_EVENT_DIS => ("VI_SUCCESS_EVENT_DIS", "Specified event is already disabled for at least one of the specified mechanisms."),
    VI_SUCCESS_QUEUE_EMPTY => ("VI_SUCCESS_QUEUE_EMPTY", "Operation completed successfully, but queue was already empty."),
    VI_SUCCESS_TERM_CHAR => ("VI_SUCCESS_TERM_CHAR", "The specified termination character was read."),
    VI_SUCCESS_MAX_CNT => ("VI_SUCCESS_MAX_CNT", "The number of bytes transferred is equal to the requested input count. More data may be available."),
    VI_SUCCESS_DEV_NPRESENT => ("VI_SUCCESS_DEV_NPRESENT", "Session opened successfully, but the device at the specified address is not responding."),
    VI_SUCCESS_TRIG_MAPPED => ("VI_SUCCESS_TRIG_MAPPED", "The path from trigSrc to trigDest is already mapped."),
    VI_SUCCESS_QUEUE_NEMPTY => ("VI_SUCCESS_QUEUE_NEMPTY", "Wait terminated successfully on receipt of an event notification. There is at least one more event object of the requested type(s) available for this session."),
    VI_SUCCESS_NCHAIN => ("VI_SUCCESS_NCHAIN", "Event handled successfully. Do not invoke any other handlers on this session for this event."),
    VI_SUCCESS_NESTED_SHARED => ("VI_SUCCESS_NESTED_SHARED", "Operation completed successfully, and this session has nested shared locks."),
    VI_SUCCESS_NESTED_EXCLUSIVE=> ("VI_SUCCESS_NESTED_EXCLUSIVE", "Operation completed successfully, and this session has nested exclusive locks."),
    VI_SUCCESS_SYNC => ("VI_SUCCESS_SYNC", "Operation completed successfully, but the operation was actually synchronous rather than asynchronous."),
    VI_WARN_QUEUE_OVERFLOW => ("VI_WARN_QUEUE_OVERFLOW", "VISA received more event information of the specified type than the configured queue size could hold."),
    VI_WARN_CONFIG_NLOADED => ("VI_WARN_CONFIG_NLOADED", "The specified configuration either does not exist or could not be loaded. VISA-specified defaults will be used."),
    VI_WARN_NULL_OBJECT => ("VI_WARN_NULL_OBJECT", "The specified object reference is uninitialized."),
    VI_WARN_NSUP_ATTR_STATE => ("VI_WARN_NSUP_ATTR_STATE", "Although the specified state of the attribute is valid, it is not supported by this implementation."),
    VI_WARN_UNKNOWN_STATUS => ("VI_WARN_UNKNOWN_STATUS", "The status code passed to the operation could not be interpreted."),
    VI_WARN_NSUP_BUF => ("VI_WARN_NSUP_BUF", "The specified I/O buffer type is not supported."),
    VI_WARN_EXT_FUNC_NIMPL => ("VI_WARN_EXT_FUNC_NIMPL", "The operation succeeded, but a lower level driver did not implement the extended functionality."),
    VI_ERROR_SYSTEM_ERROR => ("VI_ERROR_SYSTEM_ERROR", "Unknown system error (miscellaneous error)."),
    VI_ERROR_INV_OBJECT => ("VI_ERROR_INV_OBJECT", "The given session or object reference is invalid."),
    VI_ERROR_RSRC_LOCKED => ("VI_ERROR_RSRC_LOCKED", "Specified type of lock cannot be obtained, or specified operation cannot be performed, because the resource is locked."),
    VI_ERROR_INV_EXPR => ("VI_ERROR_INV_EXPR", "Invalid expression specified for search."),
    VI_ERROR_RSRC_NFOUND => ("VI_ERROR_RSRC_NFOUND", "Insufficient location information or the requested device or resource is not present in the system."),
    VI_ERROR_INV_RSRC_NAME => ("VI_ERROR_INV_RSRC_NAME", "Invalid resource reference specified. Parsing error."),
    VI_ERROR_INV_ACC_MODE => ("VI_ERROR_INV_ACC_MODE", "Invalid access mode."),
    VI_ERROR_TMO => ("VI_ERROR_TMO", "Timeout expired before operation completed."),
    VI_ERROR_CLOSING_FAILED => ("VI_ERROR_CLOSING_FAILED", "The VISA driver failed to properly close the session or object reference. This might be due to an error freeing internal or OS resources, a failed network connection, or a lower-level driver or OS error."),
    VI_ERROR_INV_DEGREE => ("VI_ERROR_INV_DEGREE", "Specified degree is invalid."),
    VI_ERROR_INV_JOB_ID => ("VI_ERROR_INV_JOB_ID", "Specified job identifier is invalid."),
    VI_ERROR_NSUP_ATTR => ("VI_ERROR_NSUP_ATTR", "The specified attribute is not defined or supported by the referenced object."),
    VI_ERROR_NSUP_ATTR_STATE => ("VI_ERROR_NSUP_ATTR_STATE", "The specified state of the attribute is not valid, or is not supported as defined by the object."),
    VI_ERROR_ATTR_READONLY => ("VI_ERROR_ATTR_READONLY", "The specified attribute is read-only."),
    VI_ERROR_INV_LOCK_TYPE => ("VI_ERROR_INV_LOCK_TYPE", "The specified type of lock is not supported by this resource."),
    VI_ERROR_INV_ACCESS_KEY => ("VI_ERROR_INV_ACCESS_KEY", "The access key to the resource associated with the specified session is invalid."),
    VI_ERROR_INV_EVENT => ("VI_ERROR_INV_EVENT", "Specified event type is not supported by the resource."),
    VI_ERROR_INV_MECH => ("VI_ERROR_INV_MECH", "Invalid mechanism specified."),
    VI_ERROR_HNDLR_NINSTALLED => ("VI_ERROR_HNDLR_NINSTALLED", "A handler was not installed."),
    VI_ERROR_INV_HNDLR_REF => ("VI_ERROR_INV_HNDLR_REF", "The given handler reference is either invalid or was not installed."),
    VI_ERROR_INV_CONTEXT => ("VI_ERROR_INV_CONTEXT", "Specified event context is invalid."),
    VI_ERROR_QUEUE_OVERFLOW => ("VI_ERROR_QUEUE_OVERFLOW", "The event queue for the specified type has overflowed (usually due to previous events not having been closed)."),
    VI_ERROR_NENABLED => ("VI_ERROR_NENABLED", "You must be enabled for events of the specified type in order to receive them."),
    VI_ERROR_ABORT => ("VI_ERROR_ABORT", "User abort occurred during transfer."),
    VI_ERROR_RAW_WR_PROT_VIOL => ("VI_ERROR_RAW_WR_PROT_VIOL", "Violation of raw write protocol occurred during transfer."),
    VI_ERROR_RAW_RD_PROT_VIOL => ("VI_ERROR_RAW_RD_PROT_VIOL", "Violation of raw read protocol occurred during transfer."),
    VI_ERROR_OUTP_PROT_VIOL => ("VI_ERROR_OUTP_PROT_VIOL", "Device reported an output protocol error during transfer."),
    VI_ERROR_INP_PROT_VIOL => ("VI_ERROR_INP_PROT_VIOL", "Device reported an input protocol error during transfer."),
    VI_ERROR_BERR => ("VI_ERROR_BERR", "Bus error occurred during transfer."),
    VI_ERROR_IN_PROGRESS => ("VI_ERROR_IN_PROGRESS", "Unable to queue the asynchronous operation because there is already an operation in progress."),
    VI_ERROR_INV_SETUP => ("VI_ERROR_INV_SETUP", "Unable to start operation because setup is invalid (usually due to attributes being set to an inconsistent state)."),
    VI_ERROR_QUEUE_ERROR => ("VI_ERROR_QUEUE_ERROR", "Unable to queue the asynchronous operation (usually due to the I/O completion event not being enabled or insufficient space in the session's queue)."),
    VI_ERROR_ALLOC => ("VI_ERROR_ALLOC", "Insufficient system resources to perform necessary memory allocation."),
    VI_ERROR_INV_MASK => ("VI_ERROR_INV_MASK", "Invalid buffer mask specified."),
    VI_ERROR_IO => ("VI_ERROR_IO", "Could not perform operation because of I/O error."),
    VI_ERROR_INV_FMT => ("VI_ERROR_INV_FMT", "A format specifier in the format string is invalid."),
    VI_ERROR_NSUP_FMT => ("VI_ERROR_NSUP_FMT", "A format specifier in the format string is not supported."),
    VI_ERROR_LINE_IN_USE => ("VI_ERROR_LINE_IN_USE", "The specified trigger line is currently in use."),
    VI_ERROR_NSUP_MODE => ("VI_ERROR_NSUP_MODE", "The specified mode is not supported by this VISA implementation."),
    VI_ERROR_SRQ_NOCCURRED => ("VI_ERROR_SRQ_NOCCURRED", "Service request has not been received for the session."),
    VI_ERROR_INV_SPACE => ("VI_ERROR_INV_SPACE", "Invalid address space specified."),
    VI_ERROR_INV_OFFSET => ("VI_ERROR_INV_OFFSET", "Invalid offset specified."),
    VI_ERROR_INV_WIDTH => ("VI_ERROR_INV_WIDTH", "Invalid access width specified."),
    VI_ERROR_NSUP_OFFSET => ("VI_ERROR_NSUP_OFFSET", "Specified offset is not accessible from this hardware."),
    VI_ERROR_NSUP_VAR_WIDTH => ("VI_ERROR_NSUP_VAR_WIDTH", "Cannot support source and destination widths that are different."),
    VI_ERROR_WINDOW_NMAPPED => ("VI_ERROR_WINDOW_NMAPPED", "The specified session is not currently mapped."),
    VI_ERROR_RESP_PENDING => ("VI_ERROR_RESP_PENDING", "A previous response is still pending, causing a multiple query error."),
    VI_ERROR_NLISTENERS => ("VI_ERROR_NLISTENERS", "No listeners condition is detected (both NRFD and NDAC are deasserted)."),
    VI_ERROR_NCIC => ("VI_ERROR_NCIC", "The interface associated with this session is not currently the controller in charge."),
    VI_ERROR_NSYS_CNTLR => ("VI_ERROR_NSYS_CNTLR", "The interface associated with this session is not the system controller."),
    VI_ERROR_NSUP_OPER => ("VI_ERROR_NSUP_OPER", "The given session or object reference does not support this operation."),
    VI_ERROR_INTR_PENDING => ("VI_ERROR_INTR_PENDING", "An interrupt is still pending from a previous call."),
    VI_ERROR_ASRL_PARITY => ("VI_ERROR_ASRL_PARITY", "A parity error occurred during transfer."),
    VI_ERROR_ASRL_FRAMING => ("VI_ERROR_ASRL_FRAMING", "A framing error occurred during transfer."),
    VI_ERROR_ASRL_OVERRUN => ("VI_ERROR_ASRL_OVERRUN", "An overrun error occurred during transfer. A character was not read from the hardware before the next character arrived."),
    VI_ERROR_TRIG_NMAPPED => ("VI_ERROR_TRIG_NMAPPED", "The path from trigSrc to trigDest is not currently mapped."),
    VI_ERROR_NSUP_ALIGN_OFFSET => ("VI_ERROR_NSUP_ALIGN_OFFSET", "The specified offset is not properly aligned for the access width of the operation."),
    VI_ERROR_USER_BUF => ("VI_ERROR_USER_BUF", "A specified user buffer is not valid or cannot be accessed for the required size."),
    VI_ERROR_RSRC_BUSY => ("VI_ERROR_RSRC_BUSY", "The resource is valid, but VISA cannot currently access it."),
    VI_ERROR_NSUP_WIDTH => ("VI_ERROR_NSUP_WIDTH", "Specified width is not supported by this hardware."),
    VI_ERROR_INV_PARAMETER => ("VI_ERROR_INV_PARAMETER", "The value of some parameter (which parameter is not known) is invalid."),
    VI_ERROR_INV_PROT => ("VI_ERROR_INV_PROT", "The protocol specified is invalid."),
    VI_ERROR_INV_SIZE => ("VI_ERROR_INV_SIZE", "Invalid size of window specified."),
    VI_ERROR_WINDOW_MAPPED => ("VI_ERROR_WINDOW_MAPPED", "The specified session currently contains a mapped window."),
    VI_ERROR_NIMPL_OPER => ("VI_ERROR_NIMPL_OPER", "The given operation is not implemented."),
    VI_ERROR_INV_LENGTH => ("VI_ERROR_INV_LENGTH", "Invalid length specified."),
    VI_ERROR_INV_MODE => ("VI_ERROR_INV_MODE", "Invalid mode specified."),
    VI_ERROR_SESN_NLOCKED => ("VI_ERROR_SESN_NLOCKED", "The current session did not have a lock on the resource."),
    VI_ERROR_MEM_NSHARED => ("VI_ERROR_MEM_NSHARED", "The device does not export any memory."),
    VI_ERROR_LIBRARY_NFOUND => ("VI_ERROR_LIBRARY_NFOUND", "A code library required by VISA could not be located or loaded."),
    VI_ERROR_NSUP_INTR => ("VI_ERROR_NSUP_INTR", "The interface cannot generate an interrupt on the requested level or with the requested statusID value."),
    VI_ERROR_INV_LINE => ("VI_ERROR_INV_LINE", "The value specified by the line parameter is invalid."),
    VI_ERROR_FILE_ACCESS => ("VI_ERROR_FILE_ACCESS", "An error occurred while trying to open the specified file. Possible reasons include an invalid path or lack of access rights."),
    VI_ERROR_FILE_IO => ("VI_ERROR_FILE_IO", "An error occurred while performing I/O on the specified file."),
    VI_ERROR_NSUP_LINE => ("VI_ERROR_NSUP_LINE", "One of the specified lines (trigSrc or trigDest) is not supported by this VISA implementation, or the combination of lines is not a valid mapping."),
    VI_ERROR_NSUP_MECH => ("VI_ERROR_NSUP_MECH", "The specified mechanism is not supported for the given event type."),
    VI_ERROR_INTF_NUM_NCONFIG => ("VI_ERROR_INTF_NUM_NCONFIG", "The interface type is valid but the specified interface number is not configured."),
    VI_ERROR_CONN_LOST => ("VI_ERROR_CONN_LOST", "The connection for the given session has been lost."),
    VI_ERROR_MACHINE_NAVAIL => ("VI_ERROR_MACHINE_NAVAIL", "The remote machine does not exist or is not accepting any connections. If the NI-VISA server is installed and running on the remote machine, it may have an incompatible version or may be listening on a different port."),
    VI_ERROR_NPERMISSION => ("VI_ERROR_NPERMISSION", "Access to the resource or remote machine is denied. This is due to lack of sufficient privileges for the current user or machine")
)


text:
using RecyclePermanent
using JLD2
using BenchmarkTools

include("../models/gkModel.jl")
include("bench.jl")

@load "data/gkData.jld" ys θ
ys = ys[1:100]
model = makeGKModel(θ, ys)

N = 80*model.n

trials = 1000

rpio = RPIO{model.Tx}(N, model.n, Threads.nthreads())
testEstimator(() -> simpleEstimate(model, rpio), trials)
testEstimator(() -> biasedEstimate(model, rpio), trials)
testEstimator(() -> rpEstimate(model, rpio), trials)

rpio = RPIO{model.Tx}(N, model.n, 1)
testEstimator(() -> simpleEstimate(model, rpio), trials)
testEstimator(() -> biasedEstimate(model, rpio), trials)
testEstimator(() -> rpEstimate(model, rpio), trials)


text:
"""
```
shapeFunction(::LinearQuadrangle3D, x, y, z)
```
Computes the linear shape functions `N[1:8]` for an `(x, y, z)` point on a 3D linear quadrangle.
"""
function shapeFunction(::LinearQuadrangle3D, x, y, z)
    # N[n](x,y,z) := shape function n.
    omx = 1 - x
    omy = 1 - y
    omz = 1 - z
    opx = 1 + x
    opy = 1 + y
    opz = 1 + z

    N =
        SVector{8, typeof(x)}(
            omx * omy * omz,
            opx * omy * omz,
            opx * opy * omz,
            omx * opy * omz,
            omx * omy * opz,
            opx * omy * opz,
            opx * opy * opz,
            omx * opy * opz,
        ) / 8

    return N
end
"""
```
shapeFunction(::LinearQuadrangle3D, x::AbstractVector, y::AbstractVector, z::AbstractVector)
```
Computes the linear shape functions `N[1:8][p]` for `(x, y, z)` point `p` on a 3D linear quadrangle.
"""
function shapeFunction(
    ::LinearQuadrangle3D,
    x::AbstractVector,
    y::AbstractVector,
    z::AbstractVector,
)
    numPoints = length(x)
    @assert numPoints == length(y) == length(z)
    xType = eltype(x)
    # N[n, p](x_vec,y_vec,z_vec) := shape function n for point p.
    N = Array{SVector{8, xType}}(undef, numPoints)

    @inbounds @simd for i in eachindex(x)
        omx = 1 - x[i]
        omy = 1 - y[i]
        omz = 1 - z[i]
        opx = 1 + x[i]
        opy = 1 + y[i]
        opz = 1 + z[i]

        N[i] =
            SVector{8, xType}(
                omx * omy * omz,
                opx * omy * omz,
                opx * opy * omz,
                omx * opy * omz,
                omx * omy * opz,
                opx * omy * opz,
                opx * opy * opz,
                omx * opy * opz,
            ) / 8
    end

    return N
end

"""
```
shapeFunctionDeriv(shape<:AbstractShapeFunction, x, y, z)
```
Computes the derivatives of the linear shape functions `N[1:3, 1:8]` for an `(x, y, z)` point on a 3D linear quadrangle.

## Returns
```
dNdS[x, n](x,y,z) := x'th derivative of shape function n.
```
"""
function shapeFunctionDeriv(::LinearQuadrangle3D, x, y, z)
    # dNdS[x, n](x,y,z) := x'th derivative of shape function n.
    omx = 1 - x
    omy = 1 - y
    omz = 1 - z
    opx = 1 + x
    opy = 1 + y
    opz = 1 + z

    dNdS =
        SMatrix{3, 8, typeof(x)}(
            -omy * omz,
            -omx * omz,
            -omx * omy,
            omy * omz,
            -opx * omz,
            -opx * omy,
            opy * omz,
            opx * omz,
            -opx * opy,
            -opy * omz,
            omx * omz,
            -omx * opy,
            -omy * opz,
            -omx * opz,
            omx * omy,
            omy * opz,
            -opx * opz,
            opx * omy,
            opy * opz,
            opx * opz,
            opx * opy,
            -opy * opz,
            omx * opz,
            omx * opy,
        ) / 8

    return dNdS
end
"""
```
shapeFunctionDeriv(shape<:AbstractShapeFunction, x::AbstractVector, y::AbstractVector, z::AbstractVector)
```
Computes the derivatives of the linear shape functions `N[1:3, 1:8]` for an `(x, y, z)` point on a 3D linear quadrangle.

## Returns
```
dNdS[x, n, p](x,y,z) := x'th derivative of shape function n for point p.
```
"""
function shapeFunctionDeriv(
    ::LinearQuadrangle3D,
    x::AbstractVector,
    y::AbstractVector,
    z::AbstractVector,
)
    numPoints = length(x)
    @assert numPoints == length(y) == length(z)
    xType = eltype(x)

    # dNdS[x, n, p](x,y,z) := x'th derivative of shape function n for point p.
    dNdS = Array{SMatrix{3, 8, xType}}(undef, numPoints)

    @inbounds @simd for i in eachindex(x)
        omx = 1 - x[i]
        omy = 1 - y[i]
        omz = 1 - z[i]
        opx = 1 + x[i]
        opy = 1 + y[i]
        opz = 1 + z[i]

        dNdS[i] =
            SMatrix{3, 8, xType}(
                -omy * omz,
                -omx * omz,
                -omx * omy,
                omy * omz,
                -opx * omz,
                -opx * omy,
                opy * omz,
                opx * omz,
                -opx * opy,
                -opy * omz,
                omx * omz,
                -omx * opy,
                -omy * opz,
                -omx * opz,
                omx * omy,
                omy * opz,
                -opx * opz,
                opx * omy,
                opy * opz,
                opx * opz,
                opx * opy,
                -opy * opz,
                omx * opz,
                omx * opy,
            ) / 8
    end

    return dNdS
end

max_stars_repo_name: dcelisgarza/DDD

max_forks_repo_name: dcelisgarza/DDD


text:
mutable struct Stoutsmearing <: Abstractsmearing
    staples_for_stout::Array{Array{Wilson_loop_set,1},1}
    tensor_derivative::Array{Tensor_derivative_set,1}
    staples_for_stout_dag::Array{Array{Wilson_loop_set,1},1}
    tensor_derivative_dag::Array{Tensor_derivative_set,1}
    ρs::Array{Array{Float64,1},1}
end

struct STOUT_dataset{Dim}
    closedloop::Vector{Wilsonline{Dim}}
    Cμ::Vector{Vector{Wilsonline{Dim}}}
    dCμdUν::Matrix{Vector{DwDU{Dim}}} #(mu,nu) num loops
    dCμdagdUν::Matrix{Vector{DwDU{Dim}}} #(mu,nu) num loops
    #dCmudUnu::Dict{Tuple{Int64,Int64,Int64},Array{DwDU{Dim},1}}
end
#=
struct STOUT_dataset{Dim}
    closedloop::Vector{Wilsonline{Dim}}
    Cμ::Vector{Vector{Wilsonline{Dim}}}
    dCμdUν::Array{Array{Dict{Tuple{Int8,Int8},Vector{DwDU{Dim}}},1},1}#Matrix{Vector{DwDU{Dim}}} #(mu,nu) num loops
    dCμdUνdag::Array{Array{Dict{Tuple{Int8,Int8},Vector{DwDU{Dim}}},1},1}#Matrix{Vector{DwDU{Dim}}} #(mu,nu) num loops
    #dCmudUnu::Dict{Tuple{Int64,Int64,Int64},Array{DwDU{Dim},1}}
end
=#

struct STOUT_Layer{Dim} <: CovLayer{Dim}
    ρs::Vector{Float64}
    dataset::Vector{STOUT_dataset{Dim}}
end

function get_name(s::STOUT_Layer)
    return "STOUT"
end

function Base.show(s::STOUT_Layer{Dim}) where Dim
    println("num. of terms: ", length(s.ρs))
    for i=1:length(s.ρs)
        if i==1
            string = "st"
        elseif i==2
            string = "nd"
        elseif i==3
            string = "rd"
        else
            string = "th"
        end
        println("-------------------------------")
        println("      $i-$string term: ")
        println("          coefficient: ",s.ρs[i])
        println("      -------------------------")
        show(s.dataset[i].closedloop)
        println("      -------------------------")
    end
end

function set_parameters(s::STOUT_Layer,ρs)
    s.ρs[:] .= ρs
end

function Base.length(layer::STOUT_Layer)
    return length(layer.ρs)
end

function get_Cμ(layer::STOUT_Layer,i)
    return layer.dataset[i].Cμ
end

function get_dCμdUν(layer::STOUT_Layer,i)
    return layer.dataset[i].dCμdUν
end

function get_dCμdagdUν(layer::STOUT_Layer,i)
    return layer.dataset[i].dCμdagdUν
end

function get_ρ(layer::STOUT_Layer,i)
    return layer.ρs[i]
end

function CovNeuralnet_STOUT(loops_smearing,ρs,L;Dim=4)

    numlayers = length(ρs)
    layers = Array{CovLayer,1}(undef,numlayers)
    for i=1:numlayers
        stout_layer = STOUT_Layer(loops_smearing,ρs[i],L,Dim=Dim)
        layers[i] = stout_layer
    end

    return CovNeuralnet{Dim}(numlayers,layers)#,_temp_gaugefields,_temp_TA_gaugefields)

end

#=
closedloops -> sum_{nm} P_nm
Cmu = dP_nm/dUmu
dCmu/dUnu = (d/dUnu) dP_nm/dUmu
=#

function STOUT_dataset(closedloops;Dim=4)
    #Ci = #Dict{Tuple{Int64,Int64},Array{Wilsonline{Dim},1}}[]
    #dCmudUnu = #Dict{Tuple{Int64,Int64,Int64},Array{DwDU{Dim},1}}[]
    num = length(closedloops) #number of loops

    Cμs = Vector{Vector{Wilsonline{Dim}}}(undef,Dim)
    for μ=1:Dim
        Cμs[μ] = Vector{Wilsonline{Dim}}[] #set of staples. In the case of plaq, there are six staples.
    end

    for i=1:num
        glinks = closedloops[i]
        for μ=1:Dim
            Cμ = make_Cμ(glinks,μ)
            for j=1:length(Cμ)
                push!(Cμs[μ],Cμ[j])
            end
        end
    end

    CmudUnu = Matrix{Vector{DwDU{Dim}}}(undef,Dim,Dim)
    CmudagdUnu = Matrix{Vector{DwDU{Dim}}}(undef,Dim,Dim)

    #=
    CmudUnu = Array{Array{Dict{Tuple{Int8,Int8},Vector{DwDU{Dim}}},1},1}(undef,Dim)
    CmudUnudag = Array{Array{Dict{Tuple{Int8,Int8},Vector{DwDU{Dim}}},1},1}(undef,Dim)
    for μ=1:Dim
        Cμ = Cμs[μ]
        numCμ = length(Cμ)
        CmudUnu[μ] = Array{Dict{Int8,Vector{DwDU{Dim}}},1}(undef,numCμ )
        CmudUnudag[μ] = Array{Dict{Int8,Vector{DwDU{Dim}}},1}(undef,numCμ )

        for i=1:numCμ
            Cμi = Cμ[i]
            CmudUnu[μ][i]=  Dict{Int8,Vector{DwDU{Dim}}}()
            CmudUnudag[μ][i]=  Dict{Int8,Vector{DwDU{Dim}}}()
            for ν=1:4
                CmudUnu[μ][i][ν] = derive_U(Cμi,ν)
                CmudUnudag[μ][i][ν] = derive_Udag(Cμi,ν)
            end
        end
    end

    return STOUT_dataset{Dim}(closedloops,Cμs,CmudUnu,CmudUnudag)
    =#

    for ν=1:Dim
        for μ=1:Dim
            CmudUnu[μ,ν] = Vector{DwDU{Dim}}[]
            CmudagdUnu[μ,ν] = Vector{DwDU{Dim}}[]
            Cμ = Cμs[μ]
            numCμ = length(Cμ)
            for j=1:numCμ
                Cμj = Cμ[j]
                dCμjν = derive_U(Cμj,ν)
                numdCμjν = length(dCμjν)
                for k=1:numdCμjν
                    push!(CmudUnu[μ,ν],dCμjν[k])
                end

                dCμjνdag = derive_U(Cμj',ν)
                numdCμjνdag = length(dCμjνdag)
                for k=1:numdCμjνdag
                    push!(CmudagdUnu[μ,ν],dCμjνdag[k])
                end

            end
            #println("dC$(μ)/dU$(ν): ")
            #show(CmudUnu[μ,ν])
        end
    end

    #=
    for μ=1:Dim
        println("μ = $μ")
        show(Cμs[μ])
        for ν=1:Dim
            println("dC$(μ)/dU$(ν): ")
            show(CmudUnu[μ,ν])
        end

        for ν=1:Dim
            println("dCdag$(μ)/dU$(ν): ")
            show(CmudagdUnu[μ,ν])
        end
    end
    =#

    return STOUT_dataset{Dim}(closedloops,Cμs,CmudUnu,CmudagdUnu)
end

function STOUT_Layer(loops_smearing,ρ,L;Dim=4)
    num = length(loops_smearing)
    loopset = make_loopforactions(loops_smearing,L)
    dataset = Array{STOUT_dataset{Dim},1}(undef,num)
    for i=1:num
        closedloops = loopset[i] #one of loopset, like plaq. There are several loops.
        dataset[i] =  STOUT_dataset(closedloops,Dim=Dim)
    end

    return STOUT_Layer{Dim}(ρ,dataset)
end

"""
δ_prev = δ_current*exp(Q) - C^+ Λ
        + sum_{i} sum_{μ',m}  [B^i_{μ,m} U_{μ'}^+ Λ_{μ',m} A_{μ,m} - bar{B}_{μ,m} Λ_{μ',m}U_{μ',m} bar{A}_{μ,m} ]

"""
function layer_pullback!(δ_prev::Array{<: AbstractGaugefields{NC,Dim},1},δ_current,layer::STOUT_Layer{Dim},Uprev,temps,tempf) where {NC,Dim}
    clear_U!(δ_prev)
    #δ_prev[ν](n) = δ_current[ν](n)*exp(Qν[Uprev](n)) + F(δ_current,Uprev)
    #F(δ_current,Uprev) = sum_μ sum_m Fm[μ](δ_current,Uprev)
    #δ_prev[ν](n) = dS/dU[ν](n)

    Cμs = similar(Uprev)
    construct_Cμ!(Cμs,layer,Uprev,temps)

    Qμs = similar(Uprev)

    #F0 = tempf[1]
    #construct_Qμs!(F0,Cμs,Uprev,temps)
    #substitute_U!(Qμs,F0)

    construct_Qμs!(Qμs,Cμs,Uprev,temps)
    Λs = similar(Uprev)
    temp1 = temps[1]
    temp2 = temps[2]
    temp3 = temps[3]
    temp4 = temps[4]

    for μ=1:Dim
        construct_Λmatrix_forSTOUT!(Λs[μ],δ_current[μ],Qμs[μ],Uprev[μ])

        #println(Λs[μ][1,1,1,1,1,1])

        exptU!(temp3,1,Qμs[μ],[temp1,temp2]) # exp(Q)
        #println("Q ",Qμs[μ][:,:,1,1,1,1])
        #println("QdagQ ",Qμs[μ][:,:,1,1,1,1]'*Qμs[μ][:,:,1,1,1,1])
        #println("expQ ",temp3[1,1,1,1,1,1])
        mul!(δ_prev[μ],δ_current[μ],temp3) #δ_prev  =δ_current*exp(Q)
        #println(δ_prev[μ][1,1,1,1,1,1])

        mul!(temp1,Cμs[μ]',Λs[μ])
        #println("temp1 ",temp1[1,1,1,1,1,1])
        add_U!(δ_prev[μ],-1,temp1) #δ_prev += -C'Λ
        #println(δ_prev[μ][1,1,1,1,1,1])

    end

    #error("lambda!")

    numterms = length(layer)

    for i=1:numterms
        dCμdUν = get_dCμdUν(layer,i)
        dCμdagdUν = get_dCμdagdUν(layer,i)
        ρi = get_ρ(layer,i)
        #println("ρi  = ",ρi )

        for ν=1:Dim
            for μ=1:Dim
                Λμ = Λs[μ]
                Uμ = Uprev[μ]

                numdCμdUν = length(dCμdUν[μ,ν])

                for j=1:numdCμdUν
                    dCμdUν_j = dCμdUν[μ,ν][j]
                    position = dCμdUν_j.position
                    m = Tuple(-collect(position))
                    Λμm = shift_U(Λμ,m)
                    Uμm = shift_U(Uμ,m)

                    leftlinks = get_leftlinks(dCμdUν_j)
                    rightlinks = get_rightlinks(dCμdUν_j)

                    A = temp3
                    evaluate_gaugelinks!(A,leftlinks,Uprev,[temp1,temp2])

                    B = temp4
                    evaluate_gaugelinks!(B,rightlinks,Uprev,[temp1,temp2])

                    #B Udag Λ A
                    mul!(temp1,Λμm,A)
                    mul!(temp2,Uμm',temp1)
                    mul!(temp1,B,temp2)
                    add_U!(δ_prev[ν],ρi,temp1)

                end

                numdCμdagdUν = length( dCμdagdUν[μ,ν])
                for j=1:numdCμdagdUν
                    dCμdagdUν_j = dCμdagdUν[μ,ν][j]

                    position = dCμdagdUν_j.position
                    m = Tuple(-collect(position))
                    Uμm = shift_U(Uμ,m)
                    Λμm = shift_U(Λμ,m)

                    leftlinks = get_leftlinks(dCμdagdUν_j)
                    rightlinks = get_rightlinks(dCμdagdUν_j)

                    barA = temp3
                    evaluate_gaugelinks!(barA,leftlinks,Uprev,[temp1,temp2])
                    barB = temp4
                    evaluate_gaugelinks!(barB,rightlinks,Uprev,[temp1,temp2])

                    #-B Λ U A
                    mul!(temp1,Uμm,barA)
                    mul!(temp2,Λμm,temp1)
                    mul!(temp1,barB,temp2)

                    add_U!(δ_prev[ν],-ρi,temp1)
                end
            end
        end
    end

    set_wing_U!(δ_prev)
    #error("stout")

end

function parameter_derivatives(δ_current,layer::STOUT_Layer{Dim},U_current,temps) where {Dim}
    #δ_prev[ν](n) = δ_current[ν](n)*exp(Qν[Uprev](n)) + F(δ_current,Uprev)
    #F(δ_current,Uprev) = sum_μ sum_m Fm[μ](δ_current,Uprev)
    #δ_prev[ν](n) = dS/dU[ν](n)

    Cμs = similar(δ_current)
    construct_Cμ!(Cμs,layer,U_current,temps)

    Qμs = similar(U_current)

    #F0 = tempf[1]
    #construct_Qμs!(F0,Cμs,Uprev,temps)
    #substitute_U!(Qμs,F0)

    construct_Qμs!(Qμs,Cμs,U_current,temps)
    Λs = similar(U_current)
    temp1 = temps[1]
    temp2 = temps[2]
    temp3 = temps[3]
    temp4 = temps[4]

    for μ=1:Dim
        construct_Λmatrix_forSTOUT!(Λs[μ],δ_current[μ],Qμs[μ],U_current[μ])
    end

    #error("lambda!")

    numterms = length(layer)
    dSdρ=zeros(Float64,numterms)

    for i=1:numterms
        C = get_Cμ(layer,i)
        s =0.0
        for μ=1:Dim
            Λμ = Λs[μ]
            Uμ = U_current[μ]
            Cμ = C[μ]

            dCμdρ = temp3
            evaluate_gaugelinks!(dCμdρ,Cμ,U_current,[temp1,temp2])
            #Udag Λ dCμdρ
            mul!(temp1,Λμ,dCμdρ)
            mul!(temp2,Uμ',temp1)
            s += 2*real(tr(temp2))
        end
        dSdρ[i] = s
    end
    return dSdρ

end

"""
M = U δ star dexp(Q)/dQ
"""

function construct_Qμ!(Qμ,μ,Cμs,Uin::Array{<: AbstractGaugefields{NC,Dim},1},temps) where {NC,Dim}
    temp1  = temps[1]
    mul!(temp1,Cμs[μ],Uin[μ]')
    clear_U!(Qμ)
    Traceless_antihermitian!(Qμ,temp1)

end

function construct_Qμs!(Qμs,Cμs,Uin::Array{<: AbstractGaugefields{NC,Dim},1},temps) where {NC,Dim}
    for μ=1:Dim
        construct_Qμ!(Qμs[μ],μ,Cμs,Uin,temps)
    end
end

function construct_Cμ!(Cμs,layer::STOUT_Layer{Dim},Uin::Array{<: AbstractGaugefields{NC,Dim},1},temps) where {NC,Dim}
    #println_verbose_level3(Uin[1],"construct_Cμ!")
    ρs = layer.ρs
    temp1  = temps[1]
    temp2  = temps[2]
    temp3  = temps[3]
    num = length(ρs)

    for μ=1:Dim
        clear_U!(Cμs[μ])
        for i=1:num
            #println("ρi  = ",ρs[i] )
            loops = layer.dataset[i].Cμ[μ]
            evaluate_gaugelinks!(temp3,loops,Uin,[temp1,temp2])
            #println("i = $i")
            #println(temp3[1,1,1,1,1,1])
            add_U!(Cμs[μ],ρs[i],temp3)
        end
        #println("U ", Uin[1][1,1,1,1,1,1])
        #println("C[1,1] = ",Cμs[μ][1,1,1,1,1,1])

    end
    #error("dd")

end

function apply_layer!(Uout::Array{<: AbstractGaugefields{NC,Dim},1},layer::STOUT_Layer{Dim},Uin,temps,tempf) where {NC,Dim}
    Cμs = similar(Uin)
    construct_Cμ!(Cμs,layer,Uin,temps)

    temp1  = temps[1]
    temp2  = temps[2]
    temp3  = temps[3]
    Qμ = tempf[1]

    for μ=1:Dim
        construct_Qμ!(Qμ,μ,Cμs,Uin,temps)
        # mul!(temp1,Cμs[μ],Uin[μ]') #Cμ*U^+
        #clear_U!(F0)
        #Traceless_antihermitian_add!(F0,1,temp1)

        exptU!(temp3,1,Qμ,[temp1,temp2])
        mul!(Uout[μ],temp3,Uin[μ])
    end
    set_wing_U!(Uout)

    #=

    Cμ  = temps[4]
    temp1  = temps[1]
    temp2  = temps[2]
    temp3  = temps[3]

    F0 = tempf[1]
    ρs = layer.ρs

    num = length(ρs)

    for μ=1:Dim
        clear_U!(Cμ)
        for i=1:num
            loops = layer.dataset[i].Cμ[μ]
            evaluate_gaugelinks!(temp3,loops,Uin,[temp1,temp2])
            add_U!(Cμ,ρs[i],temp3)
        end
        mul!(temp1,Cμ,Uin[μ]') #Cμ*U^+
        clear_U!(F0)
        Traceless_antihermitian_add!(F0,1,temp1)

        exptU!(temp3,1,F0,[temp1,temp2])

        mul!(Uout[μ],temp3,Uin[μ])
    end
    set_wing_U!(Uout)
    =#

end

function Stoutsmearing(loops_smearing,ρs)
    numloops = length(loops_smearing)
    staplesforsmear_set, tensor_derivative, staplesforsmear_dag_set, tensor_derivative_dag = make_loops_Stoutsmearing(loops_smearing,rand(numloops))
    return Stoutsmearing(
        staplesforsmear_set,
        tensor_derivative,
        staplesforsmear_dag_set,
        tensor_derivative_dag,
        ρs
    )
end

function make_loops_Stoutsmearing(loops_smearing,ρs)
    num = length(loops_smearing)
    @assert num == length(ρs) "number of ρ elements in stout smearing scheme should be $num. Now $(length(ρs)). "
    staplesforsmear_set = Array{Wilson_loop_set,1}[]
    staplesforsmear_dag_set = Array{Wilson_loop_set,1}[]
    println("staple for stout smearing")

    tensor_derivative = Array{Tensor_derivative_set,1}(undef,num)
    tensor_derivative_dag = Array{Tensor_derivative_set,1}(undef,num)

    for i=1:num
        loop_smearing = loops_smearing[i]

        staplesforsmear = Array{Wilson_loop_set,1}(undef,4)
        staplesforsmear_dag = Array{Wilson_loop_set,1}(undef,4)

        staple = make_staples(loop_smearing)

        for μ=1:4
            staplesforsmear_dag[μ] = staple[μ]##make_plaq_staple(μ)
            staplesforsmear[μ] = staplesforsmear_dag[μ]'
            #println("$μ -direction")
            #display(staplesforsmear[μ])
            #staplesforsmear[μ] = make_plaq_staple(μ)
            #staplesforsmear_dag[μ] = staplesforsmear[μ]'
            #println("dagger: $μ -direction")
            #display(staplesforsmear_dag[μ])
        end
        tensor_derivative[i] = Tensor_derivative_set(staplesforsmear)
        tensor_derivative_dag[i] = Tensor_derivative_set(staplesforsmear_dag)

        push!(staplesforsmear_set,staplesforsmear )
        push!(staplesforsmear_dag_set,staplesforsmear_dag)

    end

    return staplesforsmear_set,
        tensor_derivative,
        staplesforsmear_dag_set,
        tensor_derivative_dag

end


text:
using Test
using WordTokenizers

@testset "Orignal Penn" begin
    @test penn_tokenize("hello there mate") == ["hello", "there", "mate"]
    @test penn_tokenize("you shouldn't do that") == ["you", "should", "n't", "do", "that"]
    @test penn_tokenize("Dr. Rob is here") == ["Dr.", "Rob", "is", "here"]
    @test penn_tokenize("He (was) worried!") == ["He", "(", "was", ")", "worried", "!"]
    @test penn_tokenize("Today: work, sleep, eat") == ["Today", ":", "work", ",", "sleep", ",", "eat"]
    @test penn_tokenize("I cannot tokenize") == ["I", "can", "not", "tokenize"]

    # Original does not sperate the second of  repreated "cannot"
    s4 = "I cannot cannot work under these conditions!"
    @test penn_tokenize(s4) ==
        ["I", "can", "not", "cannot", "work", "under", "these", "conditions", "!"]
end

@testset "Common $(word_tokenize)" for word_tokenize in [improved_penn_tokenize, nltk_word_tokenize]
    @testset "basic tests" begin
        @test word_tokenize("hello there mate") == ["hello", "there", "mate"]
        @test word_tokenize("you shouldn't do that") == ["you", "should", "n't", "do", "that"]
        @test word_tokenize("He (was) worried!") == ["He", "(", "was", ")", "worried", "!"]
        @test word_tokenize("Dr. Rob is here") == ["Dr.", "Rob", "is", "here"]
        @test word_tokenize("Today: work, sleep, eat") == ["Today", ":", "work", ",", "sleep", ",", "eat"]
    end

    @testset "NLTK's tests" begin
        # from https://github.com/nltk/nltk/blob/326f3ceb38eb103bec8158cbdc18d931a548f0ad/nltk/test/tokenize.doctest#L12-L41

        s1 = "On a \$50,000 mortgage of 30 years at 8 percent, the monthly payment would be \$366.88."
        @test word_tokenize(s1) ==
            ["On", "a", "\$", "50,000", "mortgage", "of", "30", "years", "at", "8", "percent", ",", "the", "monthly", "payment", "would", "be", "\$", "366.88", "."]

        s2 = "\"We beat some pretty good teams to get here,\" Slocum said."
        @test word_tokenize(s2) ==
            ["``", "We", "beat", "some", "pretty", "good", "teams", "to", "get", "here", ",", "''", "Slocum", "said", "."]

        s3 = "Well, we couldn't have this predictable, cliche-ridden, \"Touched by an Angel\" (a show creator John Masius worked on) wanna-be if she didn't."
        @test word_tokenize(s3) ==
            ["Well", ",", "we", "could", "n't", "have", "this", "predictable", ",", "cliche-ridden", ",", "``", "Touched", "by", "an", "Angel", "''", "(", "a", "show", "creator", "John", "Masius", "worked", "on", ")", "wanna-be", "if", "she", "did", "n't", "."]

        s4 = "I cannot cannot work under these conditions!"
        @test word_tokenize(s4) ==
            ["I", "can", "not", "can", "not", "work", "under", "these", "conditions", "!"]

        s5 = "The company spent \$30,000,000 last year."
        @test word_tokenize(s5) ==
            ["The", "company", "spent", "\$", "30,000,000", "last", "year", "."]

        s6 = "The company spent 40.75% of its income last year."
        @test word_tokenize(s6) ==
            ["The", "company", "spent", "40.75", "%", "of", "its", "income", "last", "year", "."]

        s7 = "He arrived at 3:00 pm."
        @test word_tokenize(s7) ==
            ["He", "arrived", "at", "3:00", "pm", "."]

        s8 = "I bought these items: books, pencils, and pens."
        @test word_tokenize(s8) ==
            ["I", "bought", "these", "items", ":", "books", ",", "pencils", ",", "and", "pens", "."]

        s9 = "Though there were 150, 100 of them were old."
        word_tokenize(s9) ==
            ["Though", "there", "were", "150", ",", "100", "of", "them", "were", "old", "."]

        s10 = "There were 300,000, but that wasn't enough."
        @test word_tokenize(s10) ==
            ["There", "were", "300,000", ",", "but", "that", "was", "n't", "enough", "."]
    end
end


text:
logit(x::Float64) = log(x/(1-x))
invlogit(x::Float64) = exp(x)/(1+exp(x))


text:
# import Pkg.Operations: print_diff, print_status
# using Pkg.Operations: is_instantiated, stat_rep, diff_array, is_package_downloaded, is_tracking_registry
# using Pkg: Types
# using Pkg: PackageSpec, Types.EnvCache, Types.is_stdlib, Types.Context

# @Base.kwdef struct PkgColorMap
#     add::Color = Color()
#     rm::Color = Color()
#     upgrade::Color = Color()
#     downgrade::Color = Color()
#     same::Color = Color()
#     uuid::Color = Color()
#     brackets::Color = Color()
# end
# const PKG_COLORS = Ref(PkgColorMap())
# function set_pkg_colors(;
#     default = nothing,
#     add = nothing,
#     rm = nothing,
#     upgrade = nothing,
#     downgrade = nothing,
#     same = nothing,
#     uuid = nothing,
#     brackets = nothing
# )
#     c = PKG_COLORS[]
#     PKG_COLORS[] = PkgColorMap(;
#         add = Color(coal(add, default, c.add)),
#         rm = Color(coal(rm, default, c.rm)),
#         upgrade = Color(coal(upgrade, default, c.upgrade)),
#         downgrade = Color(coal(downgrade, default, c.downgrade)),
#         same = Color(coal(same, default, c.same)),
#         uuid = Color(coal(uuid, default, c.uuid)),
#         brackets = Color(coal(brackets, default, c.brackets))
#     )

#     nothing
# end

# # utils.jl

# stdlib_dir() = normpath(joinpath(Sys.BINDIR::String, "..", "share", "julia", "stdlib", "v$(VERSION.major).$(VERSION.minor)"))

# function pathrepr(path::String)
#     # print stdlib paths as @stdlib/Name
#     if startswith(path, stdlib_dir())
#         path = "@stdlib/" * basename(path)
#     end
#     return "`" * Base.contractuser(path) * "`"
# end

# printpkgstyle(ctx::Context, cmd::Symbol, text::String, ignore_indent::Bool=false; color=:green) = printpkgstyle(ctx.io, cmd, text, ignore_indent; color = color)
# function printpkgstyle(io::IO, cmd::Symbol, text::String, ignore_indent::Bool=false; color=:green)
#     indent = textwidth(string(:Precompiling)) # "Precompiling" is the longest operation
#     ignore_indent && (indent = 0)
#     printstyled(io, lpad(string(cmd), indent), color=color, bold=true)
#     println(io, " ", text)
# end

# print_diff(ctx::Context, old, new) = print_diff(ctx.io, old, new)
# function print_diff(io::IO, old, new)
#     if !is_instantiated(old) && is_instantiated(new)
#         printstyled(io, PKG_COLORS[].add, "+ $(stat_rep(new))")
#     elseif !is_instantiated(new)
#         printstyled(io, PKG_COLORS[].rm, "- $(stat_rep(old))")
#     elseif is_tracking_registry(old) && is_tracking_registry(new) &&
#            new.version isa VersionNumber && old.version isa VersionNumber && new.version != old.version
#         if new.version > old.version
#             printstyled(io, PKG_COLORS[].upgrade, "↑ $(stat_rep(old)) ⇒ $(stat_rep(new; name=false))")
#         else
#             printstyled(io, PKG_COLORS[].downgrade, "↓ $(stat_rep(old)) ⇒ $(stat_rep(new; name=false))")
#         end
#     else
#         printstyled(io, PKG_COLORS[].same, "~ $(stat_rep(old)) ⇒ $(stat_rep(new; name=false))")
#     end
# end

# function print_status(env::EnvCache, old_env::Union{Nothing,EnvCache}, header::Symbol,
#                       uuids::Vector, names::Vector; manifest=true, diff=false, ignore_indent=false, io)
#     not_installed_indicator = sprint((io, args) -> printstyled(io, args...; color=:red), "→", context=io)
#     filter = !isempty(uuids) || !isempty(names)
#     # setup
#     xs = diff_array(old_env, env; manifest=manifest)
#     # filter and return early if possible
#     if isempty(xs) && !diff
#         printpkgstyle(io, header, "$(pathrepr(manifest ? env.manifest_file : env.project_file)) (empty " *
#                       (manifest ? "manifest" : "project") * ")", ignore_indent)
#         return nothing
#     end
#     xs = !diff ? xs : eltype(xs)[(id, old, new) for (id, old, new) in xs if old != new]
#     if isempty(xs)
#         printpkgstyle(io, Symbol("No Changes"), "to $(pathrepr(manifest ? env.manifest_file : env.project_file))", ignore_indent)
#         return nothing
#     end
#     xs = !filter ? xs : eltype(xs)[(id, old, new) for (id, old, new) in xs if (id in uuids || something(new, old).name in names)]
#     if isempty(xs)
#         printpkgstyle(io, Symbol("No Matches"),
#                       "in $(diff ? "diff for " : "")$(pathrepr(manifest ? env.manifest_file : env.project_file))", ignore_indent)
#         return nothing
#     end
#     # main print
#     printpkgstyle(io, header, pathrepr(manifest ? env.manifest_file : env.project_file), ignore_indent)
#     # Sort stdlibs and _jlls towards the end in status output
#     xs = sort!(xs, by = (x -> (is_stdlib(x[1]), endswith(something(x[3], x[2]).name, "_jll"), something(x[3], x[2]).name, x[1])))
#     all_packages_downloaded = true
#     for (uuid, old, new) in xs
#         if Types.is_project_uuid(env, uuid)
#             continue
#         end
#         pkg_downloaded = !is_instantiated(new) || is_package_downloaded(env.project_file, new)
#         all_packages_downloaded &= pkg_downloaded
#         print(io, pkg_downloaded ? " " : not_installed_indicator)
#         printstyled(io, PKG_COLORS[].brackets, " [")
#         printstyled(io, PKG_COLORS[].uuid, string(uuid)[1:8])
#         printstyled(io, PKG_COLORS[].brackets, "] ")
#         diff ? print_diff(io, old, new) : print_single(io, new)
#         println(io)
#     end
#     if !all_packages_downloaded
#         printpkgstyle(io, :Info, "packages marked with $not_installed_indicator not downloaded, use `instantiate` to download", ignore_indent)
#     end
#     return nothing
# end

# function print_status(ctx::Context, old_ctx::Union{Nothing,Context}, header::Symbol,
#                       uuids::Vector, names::Vector; manifest=true, diff=false, ignore_indent=false)
#     not_installed_indicator = sprint((io, args) -> printstyled(io, args...; color=:red), "→", context=ctx.io)
#     ctx.io = something(ctx.status_io, ctx.io) # for instrumenting tests
#     filter = !isempty(uuids) || !isempty(names)
#     # setup
#     xs = diff_array(old_ctx, ctx; manifest=manifest)
#     # filter and return early if possible
#     if isempty(xs) && !diff
#         printpkgstyle(ctx, header, "$(pathrepr(manifest ? ctx.env.manifest_file : ctx.env.project_file)) (empty " *
#                       (manifest ? "manifest" : "project") * ")", ignore_indent)
#         return nothing
#     end
#     xs = !diff ? xs : eltype(xs)[(id, old, new) for (id, old, new) in xs if old != new]
#     if isempty(xs)
#         printpkgstyle(ctx, Symbol("No Changes"), "to $(pathrepr(manifest ? ctx.env.manifest_file : ctx.env.project_file))", ignore_indent)
#         return nothing
#     end
#     xs = !filter ? xs : eltype(xs)[(id, old, new) for (id, old, new) in xs if (id in uuids || something(new, old).name in names)]
#     if isempty(xs)
#         printpkgstyle(ctx, Symbol("No Matches"),
#                       "in $(diff ? "diff for " : "")$(pathrepr(manifest ? ctx.env.manifest_file : ctx.env.project_file))", ignore_indent)
#         return nothing
#     end
#     # main print
#     printpkgstyle(ctx, header, pathrepr(manifest ? ctx.env.manifest_file : ctx.env.project_file), ignore_indent)
#     # Sort stdlibs and _jlls towards the end in status output
#     xs = sort!(xs, by = (x -> (is_stdlib(x[1]), endswith(something(x[3], x[2]).name, "_jll"), something(x[3], x[2]).name, x[1])))
#     all_packages_downloaded = true
#     for (uuid, old, new) in xs
#         if Types.is_project_uuid(ctx, uuid)
#             continue
#         end
#         pkg_downloaded = !is_instantiated(new) || is_package_downloaded(ctx, new)
#         all_packages_downloaded &= pkg_downloaded
#         print(ctx.io, pkg_downloaded ? " " : not_installed_indicator)
#         printstyled(io, PKG_COLORS[].brackets, " [")
#         printstyled(io, PKG_COLORS[].uuid, string(uuid)[1:8])
#         printstyled(io, PKG_COLORS[].brackets, "] ")
#         diff ? print_diff(ctx, old, new) : print_single(ctx, new)
#         println(ctx.io)
#     end
#     if !all_packages_downloaded
#         printpkgstyle(ctx, :Info, "packages marked with $not_installed_indicator not downloaded, use `instantiate` to download", ignore_indent)
#     end
#     return nothing
# end


text:
@testset "LM algorithm" begin

    @testset "Rosenbrock" for TYPE in (Rosenbrock, Rosenbrock_Static)
        using NLS_Solver: LevenbergMarquardt

        nls = TYPE()
        θ=Float64[1;0]

        result=LevenbergMarquardt(nls, θ)

        @test converged(result)
        @test solution(result) ≈ Float64[1;1]
        @test iteration_count(result) == 8
    end

    @testset "Rosenbrock with solve()" for TYPE in (Rosenbrock, Rosenbrock_Static)
        nls = TYPE()
        conf = LevenbergMarquardt_Conf()
        θ=Float64[1;0]

        result=solve( nls, θ, conf)

        @test converged(result)
        @test solution(result) ≈ Float64[1;1]
        @test iteration_count(result) == 8
    end

    @testset "Rosenbrock with solve() + distant initial point" for TYPE in (Rosenbrock, Rosenbrock_Static)
        nls = TYPE()
        conf = LevenbergMarquardt_Conf()
        θ=Float64[-10;-10.5]

        result=solve( nls, θ, conf)

        @test converged(result)
        @test solution(result) ≈ Float64[1;1]
        @test iteration_count(result) == 30
    end

    @testset "PowellSingular" begin
        nls = PowellSingular()
        conf = LevenbergMarquardt_Conf(ε_grad_Inf_norm=1e-15,
                                       ε_step_Inf_norm=1e-15)
        θ=Float64[3;-1;0;1]
        result=solve( nls, θ, conf)

        @test converged(result)
        @test isapprox(solution(result),zeros(4),atol=1e-4)
        @test iteration_count(result) == 23
    end
end


text:
# Raytracer.jl
# Raytracing for the generation of photorealistic images in Julia
# Copyright (c) 2021 Samuele Colombo, Paolo Galli

# Unit test file for imagetracer.jl

image = HdrImage(4, 2)
camera = PerspectiveCamera(aspect_ratio=2f0)
tracer = ImageTracer(image, camera)

struct LambdaRenderer <: Renderer
    f::Function
end

function (lr::LambdaRenderer)(ray::Ray)
    lr.f(ray)
end

@testset "uv_sub_mapping" begin
    ray1 = fire_ray(tracer, 1, 1, u_pixel=2.5f0, v_pixel=1.5f0)
    ray2 = fire_ray(tracer, 3, 2, u_pixel=0.5f0, v_pixel=0.5f0)
    @test ray1 ≈ ray2
end

@testset "image_coverage" begin
    fire_all_rays!(tracer, LambdaRenderer(ray -> RGB(1f0, 2f0, 3f0)))
    rangerow, rangecol = axes(tracer.image)
    for row ∈ rangerow, col ∈ rangecol
        @test image[row, col] == RGB(1f0, 2f0, 3f0)
    end
end

@testset "orientation" begin
    # Fire a ray against top-left corner of the screen
    top_left_ray = fire_ray(tracer, 1, 1, u_pixel=0f0, v_pixel=0f0)
    @test Point(0f0, 2f0, 1f0) ≈ top_left_ray(1f0)

    # Fire a ray against bottom-right corner of the screen
    bottom_right_ray = fire_ray(tracer, 4, 2, u_pixel=1f0, v_pixel=1f0)
    @test Point(0f0, -2f0, -1f0) ≈ bottom_right_ray(1f0)
end


text:
include("../required.jl")

# Removing firts row and first column of M matrix to keep only values
M_wrow = M[setdiff(1:end, 1), :]
M_wcol = M_wrow[:, setdiff(1:end, 1)]

# Convert ones and zeros in boolean
M_bool = convert(Array{Bool}, M_wcol .== 1)

# Convert matrix in bipartite network
B = BipartiteNetwork(M_bool)

# Calculate modularity
modules = brim(lp(B)...)
Q(modules...)
Qr(modules...)

max_stars_repo_name: kiristern/reseaux

max_issues_repo_name: kiristern/reseaux

max_forks_repo_name: kiristern/reseaux


text:
#for running test cases from the generative model
using MetaGen
using Gen
using PyPlot
const plt = PyPlot
using Random

Random.seed!(1235)

################################################################################
num_videos = 10
num_frames = 300
params = Video_Params()

#for testing purposes, let's fix V
gt_v = [0.4 0.4; 0.3 0.3; 0.1 0.7; 0.05 0.05; 0.005 0.5]

cm = choicemap()
cm[:init_v_matrix => :lambda_fa => 1 => :fa] = gt_v[1, 1]
cm[:init_v_matrix => :lambda_fa => 2 => :fa] = gt_v[2, 1]
cm[:init_v_matrix => :lambda_fa => 3 => :fa] = gt_v[3, 1]
cm[:init_v_matrix => :lambda_fa => 4 => :fa] = gt_v[4, 1]
cm[:init_v_matrix => :lambda_fa => 5 => :fa] = gt_v[5, 1]
cm[:init_v_matrix => :miss_rate => 1 => :miss] = gt_v[1, 2]
cm[:init_v_matrix => :miss_rate => 2 => :miss] = gt_v[2, 2]
cm[:init_v_matrix => :miss_rate => 3 => :miss] = gt_v[3, 2]
cm[:init_v_matrix => :miss_rate => 4 => :miss] = gt_v[4, 2]
cm[:init_v_matrix => :miss_rate => 5 => :miss] = gt_v[5, 2]
for v = 1:num_videos
	cm[:videos => v => :v_matrix => :lambda_fa => 1 => :fa] = gt_v[1, 1]
	cm[:videos => v => :v_matrix => :lambda_fa => 2 => :fa] = gt_v[2, 1]
	cm[:videos => v => :v_matrix => :lambda_fa => 3 => :fa] = gt_v[3, 1]
	cm[:videos => v => :v_matrix => :lambda_fa => 4 => :fa] = gt_v[4, 1]
	cm[:videos => v => :v_matrix => :lambda_fa => 5 => :fa] = gt_v[5, 1]
	cm[:videos => v => :v_matrix => :miss_rate => 1 => :miss] = gt_v[1, 2]
	cm[:videos => v => :v_matrix => :miss_rate => 2 => :miss] = gt_v[2, 2]
	cm[:videos => v => :v_matrix => :miss_rate => 3 => :miss] = gt_v[3, 2]
	cm[:videos => v => :v_matrix => :miss_rate => 4 => :miss] = gt_v[4, 2]
	cm[:videos => v => :v_matrix => :miss_rate => 5 => :miss] = gt_v[5, 2]
end

# 	for f = 1:num_frames
# 		camera_params = camera_trajectories[v, f]
# 		cm[:videos => v => :frame_chain => f => :camera => :camera_location_x] = camera_params.camera_location.x
# 		cm[:videos => v => :frame_chain => f => :camera => :camera_location_y] = camera_params.camera_location.y
# 		cm[:videos => v => :frame_chain => f => :camera => :camera_location_z] = camera_params.camera_location.z
# 		cm[:videos => v => :frame_chain => f => :camera => :camera_focus_x] = camera_params.camera_focus.x
# 		cm[:videos => v => :frame_chain => f => :camera => :camera_focus_y] = camera_params.camera_focus.y
# 		cm[:videos => v => :frame_chain => f => :camera => :camera_focus_z] = camera_params.camera_focus.z
#
# 		#for rf = 1:num_receptive_fields
# 			#println("objects_observed[v, f][rf] ", objects_observed[v, f][rf])
# 			#println("type ", typeof(objects_observed[v, f][rf]))
# 		cm[:videos => v => :frame_chain => f => :observations_2D] = convert(Array{Any, 1}, objects_observed[v, f])
# 		#end
# 	end
# 	#def
# end

gt_trace,_ = Gen.generate(main, (num_videos, num_frames, params), cm)
#println(gt_trace)
gt_choices = get_choices(gt_trace)
for v = 1:num_videos
	println("scene at v ", gt_trace[:videos => v => :init_scene])
end

params = Video_Params()

receptive_fields = make_receptive_fields()
num_receptive_fields = length(receptive_fields)

#might be a bug in getting the objects_observed right
objects_observed = Matrix{Array{Detection2D}}(undef, num_videos, num_frames)
camera_trajectories = Matrix{Camera_Params}(undef, num_videos, num_frames)
for v = 1:num_videos
	for f = 1:num_frames
		temp = gt_choices[:videos => v => :frame_chain => f => :observations_2D]
		#temp_sorted_into_rfs = map(rf -> filter(p -> within(p, rf), temp), receptive_fields)
		#objects_observed[v, f] = temp_sorted_into_rfs
		objects_observed[v, f] = temp
		if length(temp) > 4
			println("temp too long ", temp)
			println("at video number ", v)
		end
		location_x = gt_choices[:videos => v => :frame_chain => f => :camera => :camera_location_x]
		location_y = gt_choices[:videos => v => :frame_chain => f => :camera => :camera_location_y]
		location_z = gt_choices[:videos => v => :frame_chain => f => :camera => :camera_location_z]
		focus_x = gt_choices[:videos => v => :frame_chain => f => :camera => :camera_focus_x]
		focus_y = gt_choices[:videos => v => :frame_chain => f => :camera => :camera_focus_y]
		focus_z = gt_choices[:videos => v => :frame_chain => f => :camera => :camera_focus_z]
		c = Camera_Params(camera_location = Coordinate(location_x, location_y, location_z), camera_focus = Coordinate(focus_x, focus_y, focus_z))
		camera_trajectories[v, f] = c
	end
end

#visualize_observations(objects_observed, 1, 1, receptive_fields)
#visualize_observations(objects_observed, 1, 2, receptive_fields)

outfile = string("test_case.csv")
file = open(outfile, "w")
#file header
print(file, "gt_V & ")
for v=1:(num_videos-1)
	print(file, "avg V ", v, " & ")
    print(file, "dictionary realities PF for scene ", v, " & ")
	print(file, "mode realities PF for scene ", v, " & ")
end
print(file, "avg V ", num_videos, " &")
print(file, "dictionary realities PF for scene ", num_videos, " & ")
print(file, "mode realities PF for scene  ", num_videos,)

print(file, "\n")

print(file, gt_v, " & ")

num_particles = 10
traces = unfold_particle_filter(false, num_particles, objects_observed, camera_trajectories, params, file)
println("done")
#visualize_observations(objects_observed, 1, 1, receptive_fields)
#visualize_trace(traces, 1, camera_trajectories, 1, 1, params)

close(file)

# # Gets v_hat matrix
# best_trace = Gen.get_choices(traces[1])
# v_hat = Matrix{Real}(undef, length(params.possible_objects), 2)
# for j=1:length(params.possible_objects)
# 	v_hat[j, 1] = best_trace[:videos => num_videos => :v_matrix => :lambda_fa => j => :fa]
# 	v_hat[j, 2] = best_trace[:videos => num_videos => :v_matrix => :miss_rate => j => :miss]
# end
#
# #plot errors of final estimate
# L1_errors = vec(abs.(gt_v - v_hat))
#
# plt.clf()
# plt.title("V matrix L1 errors, $(num_videos) videos, $(num_frames) frames, $(num_particles) particles")
# bins = [0, 0.01, 0.05, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1]
# plt.hist(L1_errors, bins=bins)
# plt.xlim(0, 1)
# plt.savefig("errors_$(num_videos)_$(num_frames)_$(num_particles).pdf")
# plt.clf()
#
# #plot MSE as function of videos
# MSEs = []
#
# for i=1:num_videos
# 	# makes v_hat
# 	v_pred = Matrix{Real}(undef, length(params.possible_objects), 2)
# 	for j=1:length(params.possible_objects)
# 		v_pred[j, 1] = best_trace[:videos => i => :v_matrix => :lambda_fa => j => :fa]
# 		v_pred[j, 2] = best_trace[:videos => i => :v_matrix => :miss_rate => j => :miss]
# 	end
# 	mse = sum((gt_v - v_pred).^2) / (2 * length(params.possible_objects))
# 	push!(MSEs, mse)
# end
#
# plt.plot(MSEs)
# plt.title("MSEs $(num_videos) videos, $(num_frames) frames, $(num_particles) particles")
# plt.savefig("mses_$(num_videos)_$(num_frames)_$(num_particles).pdf")
# plt.clf()

max_stars_repo_name: zhangir-azerbayev/MetaGen

max_issues_repo_name: zhangir-azerbayev/MetaGen

max_forks_repo_name: zhangir-azerbayev/MetaGen


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

if  pwd() ∉ LOAD_PATH
    push!(LOAD_PATH,pwd())
end

using Revise, mwe

mwe.func()

max_stars_repo_name: clintonTE/CCA

max_issues_repo_name: clintonTE/CCA

max_forks_repo_name: clintonTE/CCA


text:
#!/usr/bin/env julia
#
# Called with no arguments will replace the default system image with one that
# includes the JuliaFormatter
#
# Called with a single argument the system image for the formatter will be
# placed in the path specified by the argument (relative to the callers path)

using Pkg
Pkg.add("PackageCompiler")
using PackageCompiler

if isfile(PackageCompiler.backup_default_sysimg_path())
    @error """
    A custom default system image already exists.
    Either restore default with:
        julia -e "using PackageCompiler; PackageCompiler.restore_default_sysimage()"
    or use the script
        $(abspath(joinpath(@__DIR__, "..", ".dev", "clima_formatter_image.jl")))
    which will use a custom path for the system image
    """
    exit(1)
end

# If a current Manifest exist for the formatter we remove it so that we have the
# latest version
rm(joinpath(@__DIR__, "Manifest.toml"); force = true)

Pkg.activate(joinpath(@__DIR__))
PackageCompiler.create_sysimage(
    :JuliaFormatter;
    precompile_execution_file = joinpath(@__DIR__, "precompile.jl"),
    replace_default = true,
)

max_forks_repo_name: jm-c/CLIMA


text:
# TODO: clean up

struct PlanarContactSituation{T}
    active_body_poses::Dict{BodyID, Transform3D{T}}
    support_polygon::DConvexHull{T}
end

PlanarContactSituation{T}() where {T} = PlanarContactSituation(Dict{BodyID, Transform3D{T}}(), DConvexHull{T}())

struct ICPWalkingStateMachine{N, I<:ICPTrajectoryGenerator, S<:SE3PDController}
    contacts::Dict{BodyID, Vector{QPControl.ContactPoint{N}}}
    icp_trajectory_generator::I
    end_effector_controllers::Dict{BodyID, S}
    contact_plan::Piecewise{Constant{PlanarContactSituation{Float64}}, Float64, Vector{Constant{PlanarContactSituation{Float64}}}, Vector{Float64}}
    bodies::Vector{BodyID}
    # TODO: footstep generator
end

# TODO: not super nice:
isdone(trajectory::SE3Trajectory, t) = isdone(trajectory.angular, t) || isdone(trajectory.linear, t)
isdone(trajectory::Constant, t) = false
isdone(trajectory::Interpolated, t) = t > trajectory.xf
isdone(trajectory::BasicFootTrajectory, t) = t > trajectory.tf

function ICPWalkingStateMachine(
            mechanism::Mechanism,
            contacts::Dict{BodyID, <:Vector{<:QPControl.ContactPoint}},
            icp_trajectory_generator::ICPTrajectoryGenerator
        )
    T = Float64
    n = num_segments(icp_trajectory_generator)
    contact_situations = [PlanarContactSituation{Float64}() for i = 1 : n]
    times = zeros(T, n + 1)
    contact_plan = Piecewise(Constant.(contact_situations), times, clamp=true)
    bodies = sort!(collect(keys(contacts)), by = x -> x.value)

    # create end effector controllers
    end_effector_controllers = map(bodies) do bodyid
        body = findbody(mechanism, bodyid)
        bodyframe = default_frame(body)
        baseframe = root_frame(mechanism)
        gains = SE3PDGains(FramePDGains(bodyframe, PDGains(0.0, 20.0)), FramePDGains(bodyframe, PDGains(0.0, 0.0)))
        angulartraj = interpolated_orientation_trajectory(0.0, 0.0, one(Quat{T}), one(Quat{T}))
        lineartraj = convert(BasicFootTrajectory{T}, SVector(0.0, 0.0, 0.0), 0.0)
        trajectory = SE3Trajectory(bodyframe, baseframe, angulartraj, lineartraj)
        weight = Diagonal(vcat(fill(10.0, 3), fill(10.0, 3)))
        BodyID(body) => SE3PDController(BodyID(root_body(mechanism)), BodyID(body), trajectory, weight, gains)
    end |> Dict

    statemachine = ICPWalkingStateMachine(
        contacts, icp_trajectory_generator, end_effector_controllers,
        contact_plan, bodies)
end

function enable_contacts!(statemachine::ICPWalkingStateMachine, bodyid::BodyID)
    for contact in statemachine.contacts[bodyid]
        contact.maxnormalforce[] = 1e6 # TODO
    end
end

function disable_contacts!(statemachine::ICPWalkingStateMachine, bodyid::BodyID)
    for contact in statemachine.contacts[bodyid]
        disable!(contact)
    end
end

# TODO: temporary
function init_footstep_plan!(statemachine::ICPWalkingStateMachine, state::MechanismState, foot_polygons)
    T = Float64
    body_poses = Dict{BodyID, Transform3D{T}}()
    for bodyid in keys(foot_polygons)
        body_pose = transform_to_root(state, bodyid)
        body_poses[bodyid] = body_pose
    end
    Δt_first_transfer = 2.0
    Δt_transfer = 0.55
    Δt_swing = 0.75
    step_length = 0.4

    ts = breaks(statemachine.contact_plan)
    ts[1] = 0
    contact_situations = subfunctions(statemachine.contact_plan)
    n = length(contact_situations)
    support_bodies = Set(keys(foot_polygons))
    next_swing_id_iterator = Iterators.cycle(statemachine.bodies)
    next_swing_id, next_swing_id_state = iterate(next_swing_id_iterator)
    first_step = true

    for i = 1 : n
        contact_situation = contact_situations[i].value
        foot_polygons_world = valtype(foot_polygons)[]
        for bodyid in support_bodies
            foot_polygon = foot_polygons[bodyid]
            body = findbody(state.mechanism, bodyid)
            sole_to_world = body_poses[bodyid] * frame_definition(body, foot_polygon.frame)
            push!(foot_polygons_world, transform(foot_polygon, sole_to_world))
        end
        support_points = collect(Iterators.flatten(vertices(unwrap(foot_polygon_world)) for foot_polygon_world in foot_polygons_world))
        jarvis_march!(contact_situation.support_polygon, support_points)
        empty!(contact_situation.active_body_poses)
        for body in support_bodies
            push!(contact_situation.active_body_poses, body => body_poses[body])
        end
        if length(support_bodies) == length(foot_polygons)
            Δt = first_step ? Δt_first_transfer : Δt_transfer
            delete!(support_bodies, next_swing_id)
        else
            Δt = Δt_swing
            foot_displacement = if first_step
                first_step = false
                SVector(step_length, 0.0, 0.0)
            else
                SVector(2 * step_length, 0.0, 0.0)
            end
            foot_pose = body_poses[next_swing_id]
            body_poses[next_swing_id] = Transform3D(foot_pose.from, foot_pose.to, rotation(foot_pose), translation(foot_pose) + foot_displacement)
            next_swing_id, next_swing_id_state = iterate(next_swing_id_iterator, next_swing_id_state)
            for bodyid in keys(foot_polygons)
                push!(support_bodies, bodyid)
            end
        end
        ts[i + 1] = ts[i] + Δt
    end
    ξ0 = horizontal_projection(icp(state).v)
    p0 = ξ0
    ξf = centroid(last(contact_situations).value.support_polygon)
    generator = statemachine.icp_trajectory_generator
    empty!(generator)
    initialize!(generator, p0, ξ0, ξf)
    for i in eachindex(contact_situations)
        Δt = ts[i + 1] - ts[i]
        push_segment!(generator, Δt, contact_situations[i].value.support_polygon)
    end
    solve!(statemachine.icp_trajectory_generator)

    for end_effector_controller in values(statemachine.end_effector_controllers)
        init_support!(end_effector_controller; t0=ts[1], tf=ts[2])
    end
end

function (statemachine::ICPWalkingStateMachine)(t, state::MechanismState)
    active_body_poses = statemachine.contact_plan(t).active_body_poses

    # Enable/disable contacts
    for bodyid in statemachine.bodies
        if bodyid in keys(active_body_poses)
            enable_contacts!(statemachine, bodyid)
        else
            disable_contacts!(statemachine, bodyid)
        end
    end

    # Gain scheduling / trajectory initialization
    contact_plan = statemachine.contact_plan
    for (bodyid, end_effector_controller) in statemachine.end_effector_controllers
        if isdone(end_effector_controller.trajectory[], t)
            t′ = clamp(t, first(breaks(contact_plan)), last(breaks(contact_plan)))
            n = length(subfunctions(contact_plan))
            index = min(searchsortedlast(breaks(contact_plan), t′), n)
            tf = breaks(contact_plan)[min(index + 1, n)]
            if bodyid in keys(active_body_poses)
                init_support!(end_effector_controller; t0=t′, tf=tf)
            else
                previous_pose = subfunctions(contact_plan)[index - 1].value.active_body_poses[bodyid]
                next_pose = subfunctions(contact_plan)[index + 1].value.active_body_poses[bodyid]
                init_swing!(end_effector_controller, previous_pose, next_pose; t0=t′, tf=tf)
            end
        end
    end
end


text:
############################################

function _fit_curve(
    pts::AbstractMatrix{Float64}, # 2×n
    desired_distance_between_samples::Real;
    max_iterations::Int=50,
    epsilon::Float64=1e-4,
    n_intervals_in_arclen::Int=100,
    )

    @assert(size(pts, 1) == 2)

    spline_coeffs = fit_cubic_spline(pts)

    L = calc_curve_length(spline_coeffs[1], spline_coeffs[2], n_intervals_per_segment=n_intervals_in_arclen)
    n = round(Int, L/desired_distance_between_samples, RoundNearestTiesUp)+1

    s_arr = collect(linspace(0.0,L,n))
    t_arr = calc_curve_param_given_arclen(spline_coeffs[1], spline_coeffs[2], s_arr,
        curve_length=L, max_iterations=max_iterations, epsilon=epsilon, n_intervals_in_arclen=n_intervals_in_arclen)

    x_arr = sample_spline(spline_coeffs[1], t_arr)
    y_arr = sample_spline(spline_coeffs[2], t_arr)
    θ_arr = sample_spline_theta(spline_coeffs[1], spline_coeffs[2], t_arr)

    κ_arr = sample_spline_curvature(spline_coeffs[1], spline_coeffs[2], t_arr)
    κd_arr = sample_spline_derivative_of_curvature(spline_coeffs[1], spline_coeffs[2], t_arr)

    @assert(!any(s->isnan(s), s_arr))
    @assert(!any(s->isnan(s), x_arr))
    @assert(!any(s->isnan(s), y_arr))
    @assert(!any(s->isnan(s), θ_arr))

    curve = Array{CurvePt}(undef, n)
    for i in 1 : n
        pos = VecSE2(x_arr[i], y_arr[i], θ_arr[i])
        curve[i] = CurvePt(pos, s_arr[i], κ_arr[i], κd_arr[i])
    end
    curve
end

"""
    read_dxf(io::IO, ::Type{Roadway})
Return a Roadway generated from a DXF file

    Layers with names such as seg001 will contain LWPOLYLINEs.
    Each LWPOLYLINE corresponds to a lane centerline, which together
    are all neighbored.
"""
function read_dxf(io::IO, ::Type{Roadway};
    dist_threshold_lane_connect::Float64 = 0.25, # [m]
    desired_distance_between_curve_samples::Float64 = 1.0 # [m]
    )

    lines = readlines(io)

    i = findfirst(isequal("ENTITIES\n"), lines)
    i != nothing || error("ENTITIES section not found")

    ###################################################
    # Pull pts for each lane
    lane_pts_dict = Dict{LaneTag, Vector{VecE2}}()

    i = findnext(lines, "LWPOLYLINE\n", i)
    while i != 0
        i = findnext(lines, "  8\n", i)
        if i != 0 # segment identifier found in LWPOLYLINE

            if ismatch(r"(?<=seg)(\d*)", lines[i+1])
                segid = parse(Int, match(r"(?<=seg)(\d*)", lines[i+1]).match)

                    i = findnext(lines, "AcDbPolyline\n", i)
                i != 0 || error("AcDbPolyline not found in LWPOLYLINE!")
                i = findnext(lines, " 90\n", i)
                i != 0 || error("Number of vertices not found in AcDbPolyline!")

                N = parse(Int, lines[i+1])
                N > 0 || error("Empty line segment!")

                pts = Array{VecE2}(undef, N)

                i = findnext(lines, " 10\n", i)
                i != 0 || error("First point not found in AcDbPolyline!")

                for j in 1 : N
                    x = parse(Float64, lines[i+1])
                    y = parse(Float64, lines[i+3])
                    i += 4
                    pts[j] = VecE2(x,y)
                end

                laneid = 1
                for tag in keys(lane_pts_dict)
                    if tag.segment == segid
                        laneid += 1
                    end
                end
                lane_pts_dict[LaneTag(segid, laneid)] = pts
            end

            i = findnext(lines, "LWPOLYLINE\n", i)
        end
    end

    ###################################################
    # Shift pts to connect to previous / next pts
    lane_next_dict = Dict{LaneTag, Tuple{VecE2, LaneTag}}()
    lane_prev_dict = Dict{LaneTag, Tuple{VecE2, LaneTag}}()

    for (tag, pts) in lane_pts_dict
        # see if can connect to next
        best_tag = NULL_LANETAG
        best_ind = -1
        best_sq_dist = dist_threshold_lane_connect
        for (tag2, pts2) in lane_pts_dict
            if tag2.segment != tag.segment
                for (ind,pt) in enumerate(pts2)
                    sq_dist = normsquared(VecE2(pt - pts[end]))
                    if sq_dist < best_sq_dist
                        best_sq_dist = sq_dist
                        best_ind = ind
                        best_tag = tag2
                    end
                end
            end
        end
        if best_tag != NULL_LANETAG
            # remove our last pt and set next to pt to their pt
            pop!(pts)
            lane_next_dict[tag] = (lane_pts_dict[best_tag][best_ind], best_tag)
            if best_ind == 1 # set connect prev as well
                lane_prev_dict[best_tag] = (pts[end], tag)
            end
        end
    end
    for (tag, pts) in lane_pts_dict
        # see if can connect to prev
        if !haskey(lane_prev_dict, tag)
            best_tag = NULL_LANETAG
            best_ind = -1
            best_sq_dist = dist_threshold_lane_connect
            for (tag2, pts2) in lane_pts_dict
                if tag2.segment != tag.segment
                    for (ind,pt) in enumerate(pts2)
                        sq_dist = normsquared(VecE2(pt - pts[1]))
                        if sq_dist < best_sq_dist
                            best_sq_dist = sq_dist
                            best_ind = ind
                            best_tag = tag2
                        end
                    end
                end
            end
            if best_tag != NULL_LANETAG
                # connect 'em
                shift!(pts)
                lane_prev_dict[tag] = (lane_pts_dict[best_tag][best_ind], best_tag)
            end
        end
    end

    ###################################################
    # Build the roadway
    retval = Roadway()
    for (tag, pts) in lane_pts_dict
        if !has_segment(retval, tag.segment)
            push!(retval.segments, RoadSegment(tag.segment))
        end
    end

    lane_new_dict = Dict{LaneTag, LaneTag}() # old -> new tag
    for seg in retval.segments

        # pull lanetags for this seg
        lanetags = LaneTag[]
        for tag in keys(lane_pts_dict)
            if tag.segment == seg.id
                push!(lanetags, tag)
            end
        end

        # sort the lanes such that the rightmost lane is lane 1
        # do this by taking the first lane,
        # then project each lane's midpoint to the perpendicular at the midpoint

        @assert(!isempty(lanetags))
        proj_positions = Array{Float64}(undef, length(lanetags))

        first_lane_pts = lane_pts_dict[lanetags[1]]
        n = length(first_lane_pts)
        lo = first_lane_pts[div(n,2)]
        hi = first_lane_pts[div(n,2)+1]
        midpt_orig = (lo + hi)/2
        dir = polar(1.0, atan(hi - lo) + π/2) # direction perpendicular (left) of lane

        for (i,tag) in enumerate(lanetags)
            pts = lane_pts_dict[tag]
            n = length(pts)
            midpt = (pts[div(n,2)] + pts[div(n,2)+1])/2
            proj_positions[i] = proj(midpt - midpt_orig, dir, Float64)
        end

        for (i,j) in enumerate(sortperm(proj_positions))

            tag = lanetags[j]
            boundary_left = i == length(proj_positions) ? LaneBoundary(:solid, :white) : LaneBoundary(:broken, :white)
            boundary_right = i == 1 ? LaneBoundary(:solid, :white) : LaneBoundary(:broken, :white)

            pts = lane_pts_dict[tag]
            pt_matrix = Array{Float64}(undef, 2, length(pts))
            for (k,P) in enumerate(pts)
                pt_matrix[1,k] = P.x
                pt_matrix[2,k] = P.y
            end

            println("fitting curve ", length(pts), "  "); tic()
            curve = _fit_curve(pt_matrix, desired_distance_between_curve_samples)
            toc()

            tag_new = LaneTag(seg.id, length(seg.lanes)+1)
            lane = Lane(tag_new, curve,
                        boundary_left = boundary_left,
                        boundary_right = boundary_right)
            push!(seg.lanes, lane)
            lane_new_dict[tag] = tag_new
        end
    end

    ###################################################
    # Connect the lanes
    for (tag_old, tup) in lane_next_dict
        next_pt, next_tag_old = tup
        lane = retval[lane_new_dict[tag_old]]
        next_tag_new = lane_new_dict[next_tag_old]
        dest = retval[next_tag_new]
        roadproj = proj(VecSE2(next_pt, 0.0), dest, retval)

        # println("connecting $(lane.tag) to $(dest.tag)")

        cindS = curveindex_end(lane.curve)
        cindD = roadproj.curveproj.ind

        if cindD == CURVEINDEX_START # a standard connection
            connect!(lane, dest)
            # remove any similar connection from lane_prev_dict
            if haskey(lane_prev_dict, next_tag_old) && lane_prev_dict[next_tag_old][2] == tag_old
                delete!(lane_prev_dict, next_tag_old)
            end
        else
            # otherwise connect as before
            pushfirst!(lane.exits,  LaneConnection(true,  cindS, RoadIndex(cindD, dest.tag)))
            push!(dest.entrances, LaneConnection(false, cindD, RoadIndex(cindS, lane.tag)))
        end
    end
    for (tag_old, tup) in lane_prev_dict
        prev_pt, prev_tag_old = tup
        lane = retval[lane_new_dict[tag_old]]
        prev_tag_new = lane_new_dict[prev_tag_old]
        prev = retval[prev_tag_new]
        roadproj = proj(VecSE2(prev_pt, 0.0), prev, retval)

        # println("connecting $(lane.tag) from $(prev.tag)")

        cindS = roadproj.curveproj.ind
        cindD = CURVEINDEX_START

        if cindS == curveindex_end(prev) # a standard connection
            @assert(!has_prev(prev))
            connect!(prev, lane)
        else
            # a standard connection
            push!(prev.exits,  LaneConnection(true,  cindS, RoadIndex(cindD, lane.tag)))
            pushfirst!(lane.entrances, LaneConnection(false, cindD, RoadIndex(cindS, prev.tag)))
        end
    end

    retval
end