text:
orig_input = readlines("day5.txt")
input = split.(orig_input)

x1 = zeros(Int64,size(input,1))
x2 = zeros(Int64,size(input,1))
y1 = zeros(Int64,size(input,1))
y2 = zeros(Int64,size(input,1))

for (ind,line) in enumerate(orig_input)
    whole = split(line)
    x1[ind], y1[ind] = split(whole[1], ",").|> x -> parse(Int64,x)
    x2[ind], y2[ind] = split(whole[3], ",").|> x -> parse(Int64,x)
end

minx = minimum(vcat(x1,x2))
maxx = maximum(vcat(x1,x2))

miny = minimum(vcat(y1,y2))
maxy = maximum(vcat(y1,y2))

x1 .+= (1-minx)
x2 .+= (1-minx)
y1 .+= (1-miny)
y2 .+= (1-miny)

matdim = maximum([size(collect(minx:maxx),1),size(collect(miny:maxy),1)])

bigmat = zeros(Int64,matdim,matdim)

iters = size(x1,1)

for i in 1:iters
    if (x1[i] == x2[i])
        sorted = sort([y1[i],y2[i]])
        bigmat[sorted[1]:sorted[2],x1[i]] .+= 1
    elseif (y1[i] == y2[i])
        sorted = sort([x1[i],x2[i]])
        bigmat[y1[i],sorted[1]:sorted[2]] .+= 1
    end
end

sum(map(x->x.>=2,bigmat))


text:
function init_2(t,
                fft_u0, fft_v0,
                A1, A2,
                matr, conjmatr,
                sigma, llambda,
                ktau, epsilon, ntau)

    champu, champv = ftau(t, fft_u0, fft_v0, A1, A2,
                          matr, conjmatr, sigma, llambda)

    champu_fft = fft(champu, 1)
    champv_fft = fft(champv, 1)

    champu_fft[1, :] .= 0.0
    champv_fft[1, :] .= 0.0

    champu = ifft(champu_fft ./ (1im * ktau), 1)
    champv = ifft(champv_fft ./ (1im * ktau), 1)

    fft_ubar = fft_u0 .- epsilon * transpose(champu[1, :])
    fft_vbar = fft_v0 .- epsilon * transpose(champv[1, :])

    println("fft_ubar:",size(fft_ubar))

    C1u, C1v = C1(t, fft_ubar, fft_vbar, A1, A2, matr,
                  conjmatr, sigma, llambda, ktau, epsilon, ntau)

    fft_ug = fft_u0 .- transpose(C1u[1, :])
    fft_vg = fft_v0 .- transpose(C1v[1, :])

    fft_ubar, fft_vbar, fft_ug, fft_vg

end

ext: jl


text:
# test file for high-level APIs
# those tests are transported from Andrew Delong and Anton Osokin's Matlab wrapper. All credit goes to them.

sc = [0 1 2 3 3 3 3 3 3;
      1 0 1 2 3 3 3 3 3;
      2 1 0 1 2 3 3 3 3;
      3 2 1 0 1 2 3 3 3;
      3 3 2 1 0 1 2 3 3;
      3 3 3 2 1 0 1 2 3;
      3 3 3 3 2 1 0 1 2;
      3 3 3 3 3 2 1 0 1;
      3 3 3 3 3 3 2 1 0]

# gco_create
@test_throws Cxx.CxxException gco_create(1, 1)
@test_throws Cxx.CxxException gco_create(0, 2)

# gco_setlabeling & gco_getlabeling
h = gco_create(4, 3)
@test gco_energy(h)[1] == 0
l = [2, 1, 3, 2]
gco_setlabeling(h, l)
@test_throws AssertionError gco_setlabeling(h, [l,1;])
@test gco_getlabeling(h) == l
@test gco_getlabeling(h, 3) == l[3]
@test gco_getlabeling(h, [2,3]) == l[2:3]
@test_throws AssertionError gco_getlabeling(h, [0,1])
@test_throws AssertionError gco_getlabeling(h, [1,5])

gco_setlabeling(h, 4-l)
@test gco_getlabeling(h) == 4-l

# no cost
h = gco_create(4,3)
@test gco_energy(h)[1] == 0
@test gco_expansion(h) == 0
@test gco_energy(h)[1] == 0
gco_setlabeling(h, 4-gco_getlabeling(h))
@test gco_energy(h)[1] == 0
@test gco_expansion(h) == 0

# datacost
h = gco_create(4, 9)
dc = [1 2 5 8 4 2 3 7 9;
      3 1 2 5 4 5 5 5 5;
      5 5 5 5 4 5 2 1 3;
      9 7 3 2 4 8 5 2 1]'

gco_setdatacost(h, dc)
gco_setlabeling(h, [3, 3, 3, 3])
gco_getlabeling(h)
@test gco_energy(h)[1] == sum(dc[3,:])
gco_setlabeling(h, [1, 2, 3, 4])
@test gco_energy(h)[1] == dc[1,1] + dc[2,2] + dc[3,3] + dc[4,4]
gco_setlabeling(h, [5, 5, 5, 5])
gco_αexpansion(h, 8)
@test gco_getlabeling(h) == [5, 5, 8, 8]
gco_αexpansion(h, 3)
@test gco_getlabeling(h) == [5, 3, 8, 8]
gco_expansion(h)
@test gco_getlabeling(h) == [1, 2, 8, 9]
@test gco_energy(h)[1] == dc[1,1] + dc[2,2] + dc[8,3] + dc[9,4]

# data + smooth
h = gco_create(4,9)
dc = [1 2 5 8 4 2 3 7 9;
      3 1 2 5 4 5 5 5 5;
      5 5 5 5 4 5 2 1 3;
      9 7 3 2 4 8 5 2 1]'
gco_setdatacost(h, dc)
gco_setsmoothcost(h, sc)
gco_setneighbors(h, [0 2 0 0;
                     0 0 1 0;
                     0 0 0 2;
                     0 0 0 0])
gco_setlabeling(h, [3, 3, 3, 3])
@test gco_energy(h)[1] == sum(dc[3,:])
gco_setlabeling(h, [1, 2, 4, 5])
@test gco_energy(h)[1] == dc[1,1] + dc[2,2] + dc[4,3] + dc[5,4] + 6
gco_setlabeling(h, [5, 5, 5, 5])
gco_expansion(h)
@test gco_getlabeling(h) == [2, 2, 8, 8]

# data + label
h = gco_create(4,9);
dc = [1 2 5 8 4 2 3 7 9;
      3 1 3 5 4 3 5 5 5;
      5 5 5 5 4 5 1 1 3;
      9 7 3 2 4 8 5 2 1]'
lc = [9, 9, 1, 1, 1, 2, 1, 9, 9]
gco_setdatacost(h, dc)
gco_setlabelcost(h, lc)
gco_setlabeling(h, [3, 3, 3, 3])
@test gco_energy(h)[1] == sum(dc[3,:]) + lc[3]
gco_setlabeling(h, [1, 2, 4, 5])
@test gco_energy(h)[1] == dc[1,1] + dc[2,2] + dc[4,3] + dc[5,4] + sum(lc[[1 2 4 5]])
gco_setlabeling(h, [5, 5, 5, 5])
gco_expansion(h)
gco_getlabeling(h) == [7, 3, 7, 3]

# test setLabelSubsetCost
h = gco_create(4,9)
gco_setdatacost(h, dc)
gco_setlabelcost(h, lc)
gco_setlabelcost(h, 3, [1, 3, 4, 5, 6])
gco_setlabelcost(h, 4, [4, 8, 9])
gco_setlabeling(h, [3, 3, 3, 3])
@test gco_energy(h)[1] == sum(dc[3,:]) + lc[3] + 3
gco_setlabeling(h, [5, 5, 8, 9])
@test gco_energy(h)[1] == dc[5,1] + dc[5,2] + dc[8,3] + dc[9,4] + sum(lc[[5 8 9]]) + 3 + 4
gco_setlabeling(h, [5, 5, 5, 5])
gco_expansion(h)
gco_getlabeling(h) == [7, 7, 7, 7]
gco_setlabelcost(h, 3, [2])
gco_expansion(h)
gco_getlabeling(h) == [2, 2, 7, 7]

# data + smooth + label
h = gco_create(4, 9)
dc = [1 2 5 8 4 2 3 7 9;
      3 1 3 5 4 2 5 5 5;
      5 5 5 5 5 5 1 1 3;
      9 7 3 2 4 8 5 2 1]'
lc = [1, 9, 1, 1, 1, 1, 1, 9, 9]
gco_setsmoothcost(h, sc)
gco_setneighbors(h,[0 2 0 0;
                    0 0 1 0;
                    0 0 0 2;
                    0 0 0 0])
gco_setdatacost(h, dc)
gco_setlabelcost(h, lc)
gco_setlabeling(h, [3, 3, 3, 3])
@test gco_energy(h)[1] == sum(dc[3,:]) + lc[3]
gco_setlabeling(h, [1, 2, 4, 5])
@test gco_energy(h)[1] == dc[1,1] + dc[2,2] + dc[4,3] + dc[5,4] + sum(lc[[1 2 4 5]]) + 6
gco_setlabeling(h, [5, 5, 5, 5])
gco_expansion(h)
@test gco_getlabeling(h) == [6, 6, 7, 7]

# re-test with setLabelSubsetCost
h = gco_create(4,9)
gco_setsmoothcost(h, sc)
gco_setneighbors(h,[0 2 0 0;
                    0 0 1 0;
                    0 0 0 2;
                    0 0 0 0])
gco_setdatacost(h, dc)
gco_setlabelcost(h, lc)
gco_setlabelcost(h, 3, [1, 3, 4, 5, 6])
gco_setlabelcost(h, 4, [5, 8, 9])
gco_setlabeling(h, [3, 3, 3, 3])
@test gco_energy(h)[1] == sum(dc[3,:]) + lc[3] + 3
gco_setlabeling(h, [5, 5, 8, 9])
@test gco_energy(h)[1] == dc[5,1] + dc[5,2] + dc[8,3] + dc[9,4] + sum(lc[[5 8 9]]) + 3 + 4 + 5
gco_setlabeling(h, [1, 2, 4, 5])
@test gco_energy(h)[1] == dc[1,1] + dc[2,2] + dc[4,3] + dc[5,4] + sum(lc[[1 2 4 5]]) + 3 + 4 + 6
gco_setlabeling(h, [5, 5, 5, 5])
gco_expansion(h)
@test gco_getlabeling(h) == [7, 7, 7, 7]
gco_setlabelcost(h, 0, [7])
gco_setlabeling(h, [3, 3, 3, 3])
gco_expansion(h)
@test gco_getlabeling(h) == [7, 7, 7, 7]
@test gco_energy(h)[3] == 0
gco_setlabelcost(h, 4, [7])
gco_setlabelcost(h, 1, [1, 3, 4, 5, 6])
gco_expansion(h)
@test gco_getlabeling(h) == [6, 6, 4, 4]
@test gco_energy(h)[4] == 3
gco_setsmoothcost(h, sc*3)
gco_setlabeling(h, [5, 5, 7, 9])
@test gco_energy(h)[1] == dc[5,1] + dc[5,2] + dc[7,3] + dc[9,4] + sum(lc[[5 9]]) + 1 + 4 + 4 + 6*3

#
h = gco_create(4, 9)
dc = [1 1 1 1 1 1 1 1 1;
      1 1 1 1 1 1 1 1 1;
      1 1 1 1 1 1 1 1 1;
      1 1 1 1 1 1 1 1 1]'
gco_setdatacost(h, dc)
sc_nonmetric = sc.*sc
gco_setsmoothcost(h, sc_nonmetric)
gco_setneighbors(h,[0 2 0 0;
                    0 0 1 0;
                    0 0 0 2;
                    0 0 0 0])
gco_setlabeling(h, [5, 5, 7, 4])
@test_throws Cxx.CxxException gco_αexpansion(h, 5)

# test swap
h = gco_create(4, 9)
dc = [1 2 5 8 4 2 3 7 9;
      3 1 1 5 4 5 5 5 5;
      5 5 5 5 4 5 1 1 3;
      9 7 3 2 4 8 5 2 1]'
gco_setdatacost(h, dc)
gco_setlabeling(h, [5, 5, 5, 5])
gco_swap(h)
@test gco_getlabeling(h) == [1, 2, 7, 9]

h = gco_create(4, 9)
gco_setdatacost(h, dc)
gco_setsmoothcost(h, sc)
gco_setneighbors(h,[0 2 0 0;
                    0 0 1 0;
                    0 0 0 2;
                    0 0 0 0])
gco_setlabeling(h, [5, 5, 5, 5])
gco_swap(h)
@test gco_getlabeling(h) == [2, 2, 8, 8]


text:
function DSSTPartialDerivativesEquations(arg0::JString, arg1::DSSTPropagator, arg2::PropagationType)
    return DSSTPartialDerivativesEquations((JString, DSSTPropagator, PropagationType), arg0, arg1, arg2)
end

function compute_derivatives(obj::DSSTPartialDerivativesEquations, arg0::SpacecraftState, arg1::Vector{jdouble})
    return jcall(obj, "computeDerivatives", Vector{jdouble}, (SpacecraftState, Vector{jdouble}), arg0, arg1)
end

function get_mapper(obj::DSSTPartialDerivativesEquations)
    return jcall(obj, "getMapper", DSSTJacobiansMapper, ())
end

function get_name(obj::DSSTPartialDerivativesEquations)
    return jcall(obj, "getName", JString, ())
end

function get_selected_parameters(obj::DSSTPartialDerivativesEquations)
    return jcall(obj, "getSelectedParameters", ParameterDriversList, ())
end

function init(obj::AdditionalEquations, arg0::SpacecraftState, arg1::AbsoluteDate)
    return jcall(obj, "init", void, (SpacecraftState, AbsoluteDate), arg0, arg1)
end

function set_initial_jacobians(obj::DSSTPartialDerivativesEquations, arg0::SpacecraftState)
    return jcall(obj, "setInitialJacobians", SpacecraftState, (SpacecraftState,), arg0)
end

function set_initial_jacobians(obj::DSSTPartialDerivativesEquations, arg0::SpacecraftState, arg1::Vector{Vector{jdouble}}, arg2::Vector{Vector{jdouble}})
    return jcall(obj, "setInitialJacobians", SpacecraftState, (SpacecraftState, Vector{Vector{jdouble}}, Vector{Vector{jdouble}}), arg0, arg1, arg2)
end


text:
using D3TypeTrees
using Test

@test TypeTree(Number).children == Array{Int64,1}[[2, 26], [3, 18, 23, 25], [4, 5, 11], [], [6, 7, 8, 9, 10], [], [], [], [], [], [12, 13, 14, 15, 16, 17], [], [], [], [], [], [], [19, 20, 21, 22], [], [], [], [], [24], [], [], []]
@test TypeTree(Number).text == ["Number", "Real", "Integer", "Bool", "Unsigned", "UInt16", "UInt128", "UInt8", "UInt32", "UInt64", "Signed", "Int32", "Int128", "Int8", "BigInt", "Int64", "Int16", "AbstractFloat", "Float16", "Float64", "Float32", "BigFloat", "AbstractIrrational", "Irrational", "Rational", "Complex"]
@test TypeTree(Number).tooltip == ["Abstract", "Abstract", "Abstract", "", "Abstract", "", "", "", "", "", "Abstract", "", "", "", "alloc\nsize\nd", "", "", "Abstract", "", "", "", "prec\nsign\nexp\nd\n_d", "Abstract", "", "num\nden", "re\nim"]


text:
function ls_update!(out::AbstractArray, base::AbstractArray, step::AbstractArray, α)
    length(out) == length(base) == length(step) || throw(DimensionMismatch("all arrays must have the same length, got $(length(out)), $(length(base)), $(length(step))"))
    @. out  = base + α*step
end


text:
# Utilities for working with HSA agents

mutable struct HSAAgent
    agent::HSA.Agent

    # Cached information
    type::Symbol
    name::String
    productname::String
    uuid::String

    function HSAAgent(handle::HSA.Agent)
        agent = new(handle)
        agent.type = device_type(agent)
        agent.name = get_name(agent)
        agent.productname = get_product_name(agent)
        agent.uuid = get_uuid(agent)

        agent
    end
end

Base.:(==)(agent1::HSAAgent, agent2::HSAAgent) = agent1.agent == agent2.agent

const DEFAULT_AGENT = Ref{HSAAgent}()
const AGENTS = IdDict{UInt64, HSAAgent}() # Map from agent handles to HSAAgent structs

### @cfunction callbacks ###

function agent_iterate_isas_cb(isa::HSA.ISA, isas)
    push!(isas, isa)
    return HSA.STATUS_SUCCESS
end

function iterate_agents_cb(agent::HSA.Agent, agents)
    push!(agents, HSAAgent(agent))
    return HSA.STATUS_SUCCESS
end

"""
    get_agents() -> Vector{HSAAgent}

Returns the list of HSA agents available on the system.
"""
function get_agents()
    agents = Ref(Vector{HSAAgent}())
    GC.@preserve agents begin
        func = @cfunction(iterate_agents_cb, HSA.Status,
                (HSA.Agent, Ref{Vector{HSAAgent}}))
        HSA.iterate_agents(func, agents) |> check
        _agents = agents[]
    end

    # Update the entries in the agent handle dictionary
    for agent in _agents
        AGENTS[agent.agent.handle] = agent
    end

    return _agents
end
get_agents(kind::Symbol) =
    filter(agent->device_type(agent)==kind, get_agents())

"""
    get_default_agent() -> HSAAgent

Returns the default agent, which is used for all kernel and array operations
when one is not explicitly specified. May be changed with
[`set_default_agent!`](@ref).
"""
function get_default_agent()
    if !isassigned(DEFAULT_AGENT)
        error("No GPU agents detected!\nPlease consider rebuilding AMDGPU")
    end
    DEFAULT_AGENT[]
end
"""
    set_default_agent!(agent::HSAAgent) -> HSAAgent

Sets the default agent to `agent`. See [`get_default_agent`](@ref) for more
details.
"""
function set_default_agent!(agent::HSAAgent)
    DEFAULT_AGENT[] = agent
end

"""
    device(kind::Symbol=:gpu) -> Int

Returns the numeric ID of the current default device, which is in the range of
`1:length(AMDGPU.get_agents(kind))`. This number should be stable for all
processes on the same node, so long as any device filtering is consistently
applied (such as `ROCR_VISIBLE_DEVICES`). The [`device!`](@ref) function
accepts the same numeric ID that is produced by this function.
"""
device(kind::Symbol=:gpu) = something(findfirst(a->a==get_default_agent(), get_agents(kind)))
"""
    device!(idx::Integer, kind::Symbol=:gpu)

Sets the default device to `AMDGPU.get_agents(kind)[idx]`. See [`device`](@ref)
for details on the numbering semantics.
"""
device!(idx::Integer, kind::Symbol=:gpu) = set_default_agent!(get_agents(kind)[idx])

function get_name(agent::HSAAgent)
    #len = Ref(0)
    #hsa_agent_get_info(agent.agent, HSA.AGENT_INFO_NAME_LENGTH, len) |> check
    #FIXME: this could be better?
    name = Vector{UInt8}(undef, 64)
    getinfo(agent.agent, HSA.AGENT_INFO_NAME, name) |> check
    return rstrip(String(name), '\0')
end

"""
Return all agents available to the runtime.
"""
agents() = collect(values(AGENTS))

# Pretty-printing
function Base.show(io::IO, agent::HSAAgent)
    name = agent.uuid

    name *= if agent.name == agent.productname
        " [$(agent.name)]"
    else
        " [$(agent.productname) ($(agent.name))]"
    end

    print(io, name)
end

### Agent details

function get_product_name(agent::HSAAgent)
    name = Vector{UInt8}(undef, 64)
    getinfo(agent.agent, HSA.AMD_AGENT_INFO_PRODUCT_NAME, name) |> check
    rstrip(String(name), '\0')
end

function get_uuid(agent::HSAAgent)
    uuid = Base.zeros(UInt8, 64) # Doesn't appear to write zero-terminator
    getinfo(agent.agent, HSA.AMD_AGENT_INFO_UUID, uuid) |> check
    rstrip(String(uuid), '\0')
end

function profile(agent::HSAAgent)
    profile = Ref{HSA.Profile}()
    getinfo(agent.agent, HSA.AGENT_INFO_PROFILE, profile) |> check
    return profile[]
end

function device_type(agent::HSAAgent)
    devtype = Ref{UInt32}()
    getinfo(agent.agent, HSA.AGENT_INFO_DEVICE, devtype) |> check

    if HSA.DeviceType(devtype[]) == HSA.DEVICE_TYPE_CPU
        return :cpu
    elseif HSA.DeviceType(devtype[]) == HSA.DEVICE_TYPE_GPU
        return :gpu
    elseif HSA.DeviceType(devtype[]) == HSA.DEVICE_TYPE_DSP
        return :dsp
    else
        return :unknown
    end
end

### ISAs

function isas(agent::HSAAgent)
    isas = Ref(HSA.ISA[])
    func = @cfunction(agent_iterate_isas_cb, HSA.Status, (HSA.ISA, Ref{Vector{HSA.ISA}}))
    HSA.agent_iterate_isas(agent.agent, func, isas) |> check
    isas[]
end

# TODO: PCRE regexes are not thread-safe
const isa_regex = r"([a-z]*)-([a-z]*)-([a-z]*)--([a-z0-9]*)([a-zA-Z0-9+\-:]*)"
function parse_isa(isa::HSA.ISA)
    len = Ref{Cuint}(0)
    getinfo(isa, HSA.ISA_INFO_NAME_LENGTH, len) |> check
    name = Vector{UInt8}(undef, len[])
    getinfo(isa, HSA.ISA_INFO_NAME, name) |> check
    name = String(name)
    m = match(isa_regex, name)
    @assert m !== nothing "Failed to match ISA name pattern: $name"
    m
end

function llvm_arch_features(isa::HSA.ISA)
    m = parse_isa(isa)
    arch = m.captures[4]
    features = join(map(x->x[1:end-1],
                        filter(x->!isempty(x) && (x[end]=='+'),
                               split(m.captures[5], ':'))),
                    ",+")
    if !isempty(features)
        features = '+'*features
    end
    if Base.libllvm_version < v"12"
        features = replace(features, "sramecc"=>"sram-ecc")
    end
    return (arch, features)
end
architecture(isa::HSA.ISA) = llvm_arch_features(isa)[1]
features(isa::HSA.ISA) = llvm_arch_features(isa)[2]

get_first_isa_string(agent::HSAAgent) = architecture(first(isas(agent)))
get_first_feature_string(agent::HSAAgent) = features(first(isas(agent)))

function max_group_size(isa::HSA.ISA)
    size = Ref{UInt32}(0)
    getinfo(isa, HSA.ISA_INFO_WORKGROUP_MAX_SIZE, size)
    size[]
end

### Regions

struct HSARegion
    region::HSA.Region
end

function iterate_regions_cb(region::HSA.Region, regions)
    push!(regions, region)
    return HSA.STATUS_SUCCESS
end

function regions(agent::HSAAgent)
    regions = Ref(HSA.Region[])
    func = @cfunction(iterate_regions_cb, HSA.Status, (HSA.Region, Ref{Vector{HSA.Region}}))
    HSA.agent_iterate_regions(agent.agent, func, regions) |> check
    map(HSARegion, regions[])
end

function getinfo(region::HSA.Region, attribute::HSA.RegionInfo,
                 value::Union{Vector,Base.RefValue,String})
    HSA.region_get_info(region, attribute, value) |> check
end

function region_segment(region::HSARegion)
    segment = Ref{HSA.RegionSegment}()
    getinfo(region.region, HSA.REGION_INFO_SEGMENT, segment)
    segment[]
end
function region_global_flags(region::HSARegion)
    flags = Ref{HSA.RegionGlobalFlag}()
    getinfo(region.region, HSA.REGION_INFO_GLOBAL_FLAGS, flags)
    flags[]
end
function region_size(region::HSARegion)
    size = Ref{Csize_t}(0)
    getinfo(region.region, HSA.REGION_INFO_SIZE, size)
    size[]
end
function region_alloc_max_size(region::HSARegion)
    size = Ref{Csize_t}(0)
    getinfo(region.region, HSA.REGION_INFO_ALLOC_MAX_SIZE, size)
    size[]
end
function region_alloc_max_private_workgroup_size(region::HSARegion)
    size = Ref{Csize_t}(0)
    getinfo(region.region, HSA.REGION_INFO_ALLOC_MAX_PRIVATE_WORKGROUP_SIZE, size)
    size[]
end
function region_runtime_alloc_allowed(region::HSARegion)
    runtime_alloc = Ref{Bool}(false)
    getinfo(region.region, HSA.REGION_INFO_RUNTIME_ALLOC_ALLOWED, runtime_alloc)
    runtime_alloc[]
end
function region_runtime_alloc_granule(region::HSARegion)
    runtime_alloc_granule = Ref{Csize_t}(0)
    getinfo(region.region, HSA.REGION_INFO_RUNTIME_ALLOC_GRANULE, runtime_alloc_granule)
    runtime_alloc_granule[]
end
function region_runtime_alloc_alignment(region::HSARegion)
    runtime_alloc_align = Ref{Csize_t}(0)
    getinfo(region.region, HSA.REGION_INFO_RUNTIME_ALLOC_ALIGNMENT, runtime_alloc_align)
    runtime_alloc_align[]
end
function region_host_accessible(region::HSARegion)
    host_accessible = Ref(false)
    getinfo(region.region,
            reinterpret(HSA.RegionInfo, HSA.AMD_REGION_INFO_HOST_ACCESSIBLE),
            host_accessible)
    host_accessible[]
end

function get_region(agent::HSAAgent, kind::Symbol)
    _regions = regions(agent)
    flag = if kind == :finegrained
        HSA.REGION_GLOBAL_FLAG_FINE_GRAINED
    elseif kind == :coarsegrained || kind == :coarsegrained_host
        HSA.REGION_GLOBAL_FLAG_COARSE_GRAINED
    elseif kind == :kernarg
        HSA.REGION_GLOBAL_FLAG_KERNARG
    else
        error("Region kind $kind not supported")
    end
    _regions = filter(region->region_global_flags(region) & flag > 0, _regions)
    if kind == :coarsegrained
        _regions = filter(region->!region_host_accessible(region), _regions)
    end
    @assert !isempty(_regions) "No region of kind $kind in agent $agent"
    return first(_regions)
end

function Base.show(io::IO, region::HSARegion)
    segment_map = Dict(HSA.REGION_SEGMENT_GLOBAL   => :global,
                       HSA.REGION_SEGMENT_READONLY => :readonly,
                       HSA.REGION_SEGMENT_PRIVATE  => :private,
                       HSA.REGION_SEGMENT_GROUP    => :group,
                       HSA.REGION_SEGMENT_KERNARG  => :kernarg)
    segment = segment_map[region_segment(region)]

    _flags = region_global_flags(region)
    flags = Symbol[]
    flag_map = Dict(UInt32(HSA.REGION_GLOBAL_FLAG_KERNARG)        => :kernarg,
                    UInt32(HSA.REGION_GLOBAL_FLAG_FINE_GRAINED)   => :finegrained,
                    UInt32(HSA.REGION_GLOBAL_FLAG_COARSE_GRAINED) => :coarsegrained)
    for (flag, flag_name) in CEnum.namemap(HSA.RegionGlobalFlag)
        if flag & _flags > 0
            push!(flags, flag_map[flag])
        end
    end
    flags = "($(join(flags, ", ")))"

    size = Base.format_bytes(region_size(region))

    private_workgroup_size = segment == :private ? Base.format_bytes(region_alloc_max_private_workgroup_size(region)) : nothing

    runtime_alloc = region_runtime_alloc_allowed(region)

    alloc_granule = Base.format_bytes(region_runtime_alloc_granule(region))

    alloc_align = Base.format_bytes(region_runtime_alloc_alignment(region))

    max_size = Base.format_bytes(region_alloc_max_size(region))

    host_access = region_host_accessible(region)

    print(io, "HSARegion @ $(repr(region.region.handle)): Segment $segment, Flags $flags, Size $size ($max_size max allocation), ")
    if segment == :private
        print(io, "Workgroup Max Alloc: $private_workgroup_size, ")
    end
    print(io, "Runtime Alloc: "); printstyled(io, "$runtime_alloc"; color=runtime_alloc ? :green : :red); print(io, " ($alloc_granule granularity, $alloc_align alignment), ")
    print(io, "Host Accessible: "); printstyled(io, "$host_access"; color=host_access ? :green : :red)
end

### Memory Pools

struct HSAMemoryPool
    pool::HSA.AMDMemoryPool
end

function iterate_pools_cb(pool::HSA.AMDMemoryPool, pools)
    push!(pools, pool)
    return HSA.STATUS_SUCCESS
end

function memory_pools(agent::HSAAgent)
    pools = Ref(HSA.AMDMemoryPool[])
    func = @cfunction(iterate_pools_cb, HSA.Status, (HSA.AMDMemoryPool, Ref{Vector{HSA.AMDMemoryPool}}))
    HSA.amd_agent_iterate_memory_pools(agent.agent, func, pools) |> check
    map(HSAMemoryPool, pools[])
end

function getinfo(pool::HSA.AMDMemoryPool, attribute::HSA.AMDMemoryPoolInfo,
                 value::Union{Vector,Base.RefValue,String})
    HSA.amd_memory_pool_get_info(pool, attribute, value) |> check
end

function pool_segment(pool::HSAMemoryPool)
    segment = Ref{HSA.AMDSegment}()
    getinfo(pool.pool, HSA.AMD_MEMORY_POOL_INFO_SEGMENT, segment)
    segment[]
end
function pool_global_flags(pool::HSAMemoryPool)
    flags = Ref{HSA.AMDMemoryPoolGlobalFlag}()
    getinfo(pool.pool, HSA.AMD_MEMORY_POOL_INFO_GLOBAL_FLAGS, flags)
    flags[]
end
function pool_size(pool::HSAMemoryPool)
    size = Ref{Csize_t}(0)
    getinfo(pool.pool, HSA.AMD_MEMORY_POOL_INFO_SIZE, size)
    size[]
end
function pool_runtime_alloc_allowed(pool::HSAMemoryPool)
    runtime_alloc = Ref{Bool}(false)
    getinfo(pool.pool, HSA.AMD_MEMORY_POOL_INFO_RUNTIME_ALLOC_ALLOWED, runtime_alloc)
    runtime_alloc[]
end
function pool_runtime_alloc_granule(pool::HSAMemoryPool)
    runtime_alloc_granule = Ref{Csize_t}(0)
    getinfo(pool.pool, HSA.AMD_MEMORY_POOL_INFO_RUNTIME_ALLOC_GRANULE, runtime_alloc_granule)
    runtime_alloc_granule[]
end
function pool_runtime_alloc_alignment(pool::HSAMemoryPool)
    runtime_alloc_align = Ref{Csize_t}(0)
    getinfo(pool.pool, HSA.AMD_MEMORY_POOL_INFO_RUNTIME_ALLOC_ALIGNMENT, runtime_alloc_align)
    runtime_alloc_align[]
end
function pool_accessible_by_all(pool::HSAMemoryPool)
    accessible_all = Ref{Bool}(false)
    getinfo(pool.pool, HSA.AMD_MEMORY_POOL_INFO_ACCESSIBLE_BY_ALL, accessible_all)
    accessible_all[]
end
function pool_alloc_max_size(pool::HSAMemoryPool)
    size = Ref{Csize_t}(0)
    getinfo(pool.pool, HSA.AMD_MEMORY_POOL_INFO_ALLOC_MAX_SIZE, size)
    size[]
end

function Base.show(io::IO, pool::HSAMemoryPool)
    segment_map = Dict(HSA.AMD_SEGMENT_GLOBAL   => :global,
                       HSA.AMD_SEGMENT_READONLY => :readonly,
                       HSA.AMD_SEGMENT_PRIVATE  => :private,
                       HSA.AMD_SEGMENT_GROUP    => :group)
    segment = segment_map[pool_segment(pool)]

    _flags = pool_global_flags(pool)
    flags = Symbol[]
    flag_map = Dict(UInt32(HSA.AMD_MEMORY_POOL_GLOBAL_FLAG_KERNARG_INIT)   => :kernarg,
                    UInt32(HSA.AMD_MEMORY_POOL_GLOBAL_FLAG_FINE_GRAINED)   => :finegrained,
                    UInt32(HSA.AMD_MEMORY_POOL_GLOBAL_FLAG_COARSE_GRAINED) => :coarsegrained)
    for (flag, flag_name) in CEnum.namemap(HSA.AMDMemoryPoolGlobalFlag)
        if flag & _flags > 0
            push!(flags, flag_map[flag])
        end
    end
    flags = "($(join(flags, ", ")))"

    size = Base.format_bytes(pool_size(pool))

    runtime_alloc = pool_runtime_alloc_allowed(pool)

    alloc_granule = Base.format_bytes(pool_runtime_alloc_granule(pool))

    alloc_align = Base.format_bytes(pool_runtime_alloc_alignment(pool))

    accessible_all = pool_accessible_by_all(pool)

    max_size = Base.format_bytes(pool_alloc_max_size(pool))

    print(io, "HSAMemoryPool @ $(repr(pool.pool.handle)): Segment $segment, Flags $flags, Size $size ($max_size max allocation), ")
    print(io, "Runtime Alloc: "); printstyled(io, "$runtime_alloc"; color=runtime_alloc ? :green : :red); print(io, " ($alloc_granule granularity, $alloc_align alignment), ")
    print(io, "All Accessible: "); printstyled(io, "$accessible_all"; color=accessible_all ? :green : :red)
end

function get_memory_pool(agent::HSAAgent, kind::Symbol)
    _pools = memory_pools(agent)
    flag = if kind == :finegrained
        HSA.AMD_MEMORY_POOL_GLOBAL_FLAG_FINE_GRAINED
    elseif kind == :coarsegrained || kind == :coarsegrained_host
        HSA.AMD_MEMORY_POOL_GLOBAL_FLAG_COARSE_GRAINED
    elseif kind == :kernarg
        HSA.AMD_MEMORY_POOL_GLOBAL_FLAG_KERNARG_INIT
    else
        error("Region kind $kind not supported")
    end
    _pools = filter(pool->pool_global_flags(pool) & flag > 0, _pools)
    if kind == :coarsegrained
        _pools = filter(pool->!pool_accessible_by_all(pool), _pools)
    end
    @assert !isempty(_pools) "No memory pool of kind $kind in agent $agent"
    return first(_pools)
end


text:
"
    gather(w::AbstractMatrix{T}, xs::OneHotArray) where

getting vector at the given onehot encoding.
"
gather(w::AbstractMatrix{T}, xs::OneHotArray) where T = gather(w, onehot2indices(xs))

"
    gather(w::AbstractMatrix{T}, xs) where

getting vector at the given indices, `xs` is a array of indices(`Int` type).
"
function gather(w::AbstractMatrix{T}, xs::AbstractArray{Int}) where T
    ys = similar(w, size(w, 1), size(xs)...)

    Threads.@threads for i = 1:length(xs)
        ind = Tuple(CartesianIndices(xs)[i])
        @inbounds ys[:, ind...] .= w[:, xs[i]]
    end
    return ys
end

"
    gather(w::AbstractArray{T}, xs) where

getting vector at the given indices, `xs` is a array of cartesian indices(`Tuple{Int}` type).
"
function gather(w::AbstractArray{T}, xs::AbstractArray{<:Tuple}) where T
    ys = similar(w, size(w, 1), size(xs)...)

    Threads.@threads for i = 1:length(xs)
        ind = Tuple(CartesianIndices(xs)[i])
        @inbounds ys[:, ind...] .= w[:, xs[i]...]
    end
    return ys
end

# cpu gather back
function ∇gather(Δ::AbstractArray{T}, w::AbstractMatrix{T}, xs::AbstractArray{Int}) where T
    ys = fill!(similar(w), zero(T))

    Threads.@threads for i = 1:length(xs)
        ind = Tuple(CartesianIndices(xs)[i])
        @inbounds ys[:, xs[i]] .+= Δ[:, ind...]
    end

    return ys
end

function ∇gather(Δ::AbstractArray{T}, w::AbstractArray{T}, xs::AbstractArray{<:Tuple}) where T
    ys = fill!(similar(w), zero(T))

    Threads.@threads for i = 1:length(xs)
        ind = Tuple(CartesianIndices(xs)[i])
        @inbounds ys[:, xs[i]] .+= Δ[:, ind...]
    end

    return ys
end

@init @require CuArrays="3a865a2d-5b23-5a0f-bc46-62713ec82fae" begin
    using .CuArrays
    using .CuArrays.CUDAnative

    # gpu gather
    gather(w::CuMatrix{T}, xs::OneHotArray) where T = gather(w, onehot2indices(xs))
    function gather(w::CuMatrix{T}, xs::CuArray{Int}) where T
        ys = CuArray{T}(undef, size(w, 1), size(xs)...)

        function kernel!(ys::CuDeviceArray{T}, w::CuDeviceArray{T}, xs)
            li = threadIdx().y + (blockIdx().y - 1) * blockDim().y
            i = threadIdx().x + (blockIdx().x - 1) * blockDim().x

            if li <= length(xs)
                ind = Tuple(CartesianIndices(xs)[li])
                ys[i, ind...] = w[i, xs[li]]
            end

            return
        end

        max_threads = 256
        threads_x = min(max_threads, size(ys,1))
        threads_y = min(max_threads ÷ threads_x, length(xs))
        threads = (threads_x, threads_y)
        blocks = ceil.(Int, (size(ys,1), length(xs)) ./ threads)

        CuArrays.@cuda blocks=blocks threads=threads kernel!(ys, w, xs)
        return ys
    end

    #gpu gather back
    function ∇gather(Δ::CuArray{T}, w::CuMatrix{T}, xs::CuArray{Int}) where T
        ys = fill!(similar(w), zero(T))

        function kernel!(ys::CuDeviceArray{T}, Δ::CuDeviceArray{T}, xs)
            xi = threadIdx().x + (blockIdx().x - 1) * blockDim().x

            if xi <= size(ys, 1)
                for i = 1:length(xs)
                    ind = Tuple(CartesianIndices(xs)[i])
                    ys[xi, xs[i]] += Δ[xi, ind...]
                end
            end

            return
        end

        max_threads = 256
        threads = max_threads
        blocks = ceil(Int, size(ys, 1) / threads)

        CuArrays.@cuda blocks=blocks threads=threads kernel!(ys, Δ, xs)
        return ys
    end

    # Not atomic to do this
    # function kernel!(ys::CuDeviceArray{T}, Δ::CuDeviceArray{T}, xs)
    #     li = threadIdx().y + (blockIdx().y - 1) * blockDim().y
    #     i = threadIdx().x + (blockIdx().x - 1) * blockDim().x
    #     if li <= length(xs)
    #         ind = Tuple(CartesianIndices(xs)[li])
    #         ys[i, xs[li]] += Δ[i, ind...]
    #     end

    #     return
    # end
    # max_threads = 256
    # threads_x = min(max_threads, size(ys,1))
    # threads_y = min(max_threads ÷ threads_x, length(xs))
    # threads = (threads_x, threads_y)
    # blocks = ceil.(Int, (size(ys,1), length(xs)) ./ threads)
    # @cuda blocks=blocks threads=threads kernel!(ys, Δ, xs)

    function gather(W::CuArray{T}, xs::CuArray{<:Tuple}) where T
        ys = CuArray{T}(undef, size(W, 1), size(xs)...)

        function kernel!(ys::CuDeviceArray{T}, w::CuDeviceArray{T}, xs)
            li = threadIdx().y + (blockIdx().y - 1) * blockDim().y
            i = threadIdx().x + (blockIdx().x - 1) * blockDim().x

            if li <= length(xs)
                ind = Tuple(CartesianIndices(xs)[li])
                ys[i, ind...] = w[i, xs[li]...]
            end

            return
        end

        max_threads = 256
        threads_x = min(max_threads, size(ys,1))
        threads_y = min(max_threads ÷ threads_x, length(xs))
        threads = (threads_x, threads_y)
        blocks = ceil.(Int, (size(ys,1), length(xs)) ./ threads)

        CuArrays.@cuda blocks=blocks threads=threads kernel!(ys, W, xs)
        return ys
    end

    function ∇gather(Δ::CuArray{T}, W::CuArray{T}, xs::CuArray{<:Tuple}) where T
        ys = fill!(similar(W), zero(T))

        function kernel!(ys::CuDeviceArray{T}, Δ::CuDeviceArray{T}, xs)
            xi = threadIdx().x + (blockIdx().x - 1) * blockDim().x

            if xi <= size(ys, 1)
                for i = 1:length(xs)
                    ind = Tuple(CartesianIndices(xs)[i])
                    ys[xi, xs[i]...] += Δ[xi, ind...]
                end
            end

            return
        end

        max_threads = 256
        threads = max_threads
        blocks = ceil(Int, size(ys, 1) / threads)

        CuArrays.@cuda blocks=blocks threads=threads kernel!(ys, Δ, xs)
        return ys
    end
end

using Flux: Tracker, TrackedArray, TrackedMatrix, data
using Flux.Tracker: @grad, track

gather(w::TrackedMatrix, xs::OneHotArray) = gather(w, onehot2indices(xs))
gather(w::TrackedMatrix, xs::AbstractArray{Int}) = track(gather, w, xs)
gather(w::TrackedArray, xs::AbstractArray{<:Tuple}) = track(gather, w, xs)

@grad gather(w::TrackedArray, xs) = gather(data(w), xs), Δ->(∇gather(Δ, data(w), xs),nothing)


text:
"""
    GradDescent(ρ,β)

Gradient descent with backtracking linesearch. Updates parameters
according to `x = x - ρ⋅∇x` and shrinks the step size `ρ` by a
multiplicative factor `0 < β < 1` whenever this parameter change
increases the objective function.
"""
type GradDescent{T<:AbstractFloat} <: Optimizer
    ρ::T                # step size
    β::T                # linesearch param
    function GradDescent(ρ,β)
        0<β<1 || error("β must be in interval (0,1)")
        ρ>0 || error("ρ must be postive")
        new(ρ,β)
    end
end
GradDescent{T<:AbstractFloat}(ρ::T=1.0,β::T=0.5) = GradDescent{T}(ρ,β)

function update!(
        x::AbstractVector,
        ∇::AbstractVector,
        f::Real,
        model::Model,
        opt::GradDescent
    )
    axpy!(-opt.ρ,∇x,x)          # take gradient step
    while f_next > f
        s = opt.ρ*opt.β         # smaller stepsize
        axpy!(opt.ρ-s,∇x,x)     # backtrack
        opt.ρ = s               # update stepsize
        f_next = value(model)   # check objective value again
        opt.ρ < 1e-12 && warn("linesearch failed") && break
    end
    return f_next
end

max_stars_repo_name: JuliaML/StochasticOptimization

max_issues_repo_name: JuliaML/StochasticOptimization

max_forks_repo_name: JuliaML/StochasticOptimization


text:
#!/usr/bin/env julia

using Shearlab, Test
@time @testset "Wavelet Transform Size" begin include("wavelet_trans_size.jl") end
@time @testset "Wavelet Inverse Transform Size" begin include("wavelet_inv_size.jl") end
@time @testset "Shearlet Transform Size" begin include("shearlet_trans_size.jl") end
@time @testset "Shearlet Inverse Transform Size" begin include("shearlet_inv_size.jl") end


text:
struct tikzStyle

    set::Bool

    name::String

    color::String
    linestyle::String
    lw::Float64

    extracommands::String

end

function tikzStyle( )

    return tikzStyle( false, "", "", "", 0.0, "" )

end

function tikzStyle( name::String;
                    color = "",
                    linestyle = "",
                    lw = 0.0, extracommands="" )

    return tikzStyle( true, name, color, linestyle, lw, extracommands )

end

max_stars_repo_name: mopg/julia-tikz

max_issues_repo_name: mopg/julia-tikz

max_forks_repo_name: mopg/julia-tikz


text:
module CTA

using ThermistorHW

"""
    PulsedCTA(Rt::Thermistor, Ri=200.0, Rf=10e3, Cf=10e-6, Vi=24.0, freq=490.0, duty=0.5)

Modela os componentes do circuito primário do anemômetro  de termistor pulsado.

"""
struct PulsedCTA
    "Termistor usado como sensor"
    Rt::Thermistor
    "Resistência usada para media a corrente"
    Ri::Float64
    "Resistência do filtro passa baixa"
    Rf::Float64
    "Capacitor do filtro passa baixa"
    Cf::Float64
    "Tensão de alimentação"
    Vi::Float64
    "Frequência do PWM"
    freq::Float64
    PulsedCTA(Rt::Thermistor; Ri=100.0, Rf=10e3, Cf=47e-6, Vi=24.0,
              freq=1000.0) = new(Rt, Ri, Rf, Cf, Vi, freq)
end

"""
    voltage(cta::PulsedCTA, t=0.0, duty=0.5)

Calcula a tensão ao longo do tempo dado o duty cycle do PWM
"""
function voltage(cta::PulsedCTA, t=0.0, duty=0.5, tmin=0.0)
    if t < tmin
        return cta.Vi
    end

    T = 1/cta.freq
    p = mod(t, T) / T

    return cta.Vi * ((p<=duty) ? 1.0 : 0.0)
end

(cta::PulsedCTA)(t=0.0, duty=0.5, tmin=0.0) = voltage(cta, t, duty, tmin)

"""
    HeatTrans(Dmm, Ta=20.0, Pa=101.325)

Principais parâmetros de transferência de calor.
"""
struct HeatTrans
    "Temperatura de operação do termistor"
    Tw::Float64
    "Diâmetro do termistor"
    D::Float64
    "Temperatura ambiente"
    Ta::Float64
    "Pressão atmosférica"
    Pa::Float64
    "Área superficial externa do termistos"
    A::Float64
    "Propriedades de transferência de calor do termistor"
    S::Solid
    m::Float64
    mcp::Float64
    function HeatTrans(Tw=85.0; Dmm=2.0,  Ta=20.0, Pa=93.0, solid=Fe₂O₃)
        D = Dmm/1000
        R = D/2
        A = 4π*R^2
        Vol = A*R/3
        m = Vol * solid.ρ
        mcp = m * solid.cₚ

        new(Tw, D, Ta, Pa, A, solid, m, mcp)
    end

end

"""
    hafun(htrans::HeatTrans, U, T)

Calcula `h⋅A` onde `h` é o coeficiente de convecção e `A` é a área externa do termistor.
"""
hafun(htrans::HeatTrans, U, T) = hconvect(U, htrans.D, T, htrans.Ta, htrans.Pa) * htrans.A
(htrans::HeatTrans)(U,T) = hafun(htrans, U, T)

"""
    mcpfun(s::Solid, D)

Calcula `m⋅cₚ` onde `m` é a massa do termistor e `cₚ` é o calor específico do termistor.
"""
mcpfun(s::Solid, D) = s.cₚ * s.ρ * 4π/3*(D/2)^3

"""
    pulsedcta(dy, y, p, t)

Monta o sistema de equações diferenciais do CTA pulsado:

```math
dEₐᵢ/dt = 1/RC⋅( EᵢRᵢ / (Rₜ(T) +Rᵢ) - Eₐᵢ )

```
```math
dT/dt = -hA/mcₚ⋅(T - Tₐ)  + 1/mcₚ * Eᵢ²(t) / Rₜ(T)

```

"""
function pulsedcta(dy, y, p, t)
    cta = p[1]
    duty = p[2]
    U = p[3](t)
    htrans = p[4]
    mcp = p[5]
    tmin = p[6]

    hA = htrans(U, y[2])
    Rt = cta.Rt(y[2])
    #Rt = (Rt < 20.0)?20.0 : Rt

    Ta = htrans.Ta

    Ei = cta(t, duty, tmin)
    Ri = cta.Ri
    RC = cta.Rf * cta.Cf
    I = Ei / (Rt + Ri)

    dy[1] = 1/RC * ( Ei * Ri/(Rt + Ri) - y[1] )
    dy[2] = -hA/mcp * (y[2] - Ta) + Rt/mcp * I*I

    return
end

abstract type AbstractVelocity end

struct VelConst <: AbstractVelocity
    v::Float64
    VelConst(v=10.0) = new(v)
end

velocity(v::VelConst, t=0.0) = v.v
(v::VelConst)(t=0.0) = velocity(v,t)

struct VelFreq <: AbstractVelocity
    vmean::Float64
    freq::Float64
    ampl::Float64
    VelFreq(vmean=10.0, freq=0.5, ampl=0.5) = new(vmean, freq, ampl)
end
velocity(v::VelFreq, t=0.0) = v.vmean + v.ampl * sin(2π*v.freq*t)
(v::VelFreq)(t=0.0) = velocity(v,t)

struct VelRamp <: AbstractVelocity
    α::Float64
    VelRamp(α=1.0) = new(α)
end

velocity(v::VelRamp, t=0.0) = v.α * t
(v::VelRamp)(t=0.0) = velocity(v,t)

struct VelRampConst <: AbstractVelocity
    t0::Float64
    V0::Float64
    VelRampConst(V0=10.0, t0=10.0) = new(V0, t0)
end
velocity(v::VelRampConst, t=0.0) = (t < v.t0) ? t*v.V0/v.t0 : v.V0
(v::VelRampConst)(t=0.0) = velocity(v, t)

struct VelStep <: AbstractVelocity
    t0::Float64
    V0::Float64
    V1::Float64
    VelStep(t0=10.0, V1=10.0, V0=0.0) = new(t0, V0, V1)
end
velocity(v::VelStep, t=0.0) = (t < v.t0) ? v.V0 : v.V1
(v::VelStep)(t=0.0) = velocity(v, t)

struct VelUpDown <: AbstractVelocity
    t0::Float64
    t1::Float64
    t2::Float64
    V0::Float64
    VelUpDown(V0=10.0, t0=6.0, t1=12, t2=18.0) = new(t0, t1, t2, V0)
end
function velocity(v::VelUpDown, t=0.0)
    if t < v.t0
        return v.V0*t/v.t0
    elseif t >= v.t0 && t < v.t1
        return v.V0
    elseif t >= v.t1 && t < v.t2
        return (1.0 - (t-v.t1)/(v.t2-v.t1)) * v.V0
    else
        return 0.0
    end
end
(v::VelUpDown)(t=0.0) = velocity(v, t)

hamcpfun(htrans::HeatTrans, vel::AbstractVelocity, t=0.0, Tw=htrans.Tw) = hafun(htrans, vel(t), Tw)/htrans.mcp

hamcpfun(htrans::HeatTrans, U::Number, t=0.0, Tw=htrans.Tw) = hafun(htrans, U, Tw)/htrans.mcp

end


text:
error("This code should not be included")

tr2blas(X::BlasArray) = 'N', X
tr2blas(X::BlasTranspose) = 'T', parent(X)
tr2blas(X::BlasAdjoint) = 'C', parent(X)
tr2blas(X::BlasAdjoint{T}) where T <: BlasReal = 'T', parent(X)

#  GEMM and SYMM =======================

do_gemm!(α, TA, A, TB, B, β, C) = BLAS.gemm!(TA, TB, α, A, B, β, C)      # C ← αAB + βC  (with transpositions as specified)
do_gemm!(α, TA, A::Symmetric, TB, B, β, C) = BLAS.symm!('L', A.uplo, α, blasnode(A), B, β, C)
do_gemm!(α, TA, A, TB, B::Symmetric, β, C) = BLAS.symm!('R', B.uplo, α, blasnode(B), A, β, C)

function gemm_αABβC!(α, A, B, β, C::BlasMatrix{T}) where T
   try
        C===A && α==1 && β==0 && return rmul!(A,B)
        C===B && α==1 && β==0 && return lmul!(A,B)
        (C===A || C===B) && β≠0 && ip_error("multiplicative and additive updates cannot be combined.")
        (C===A || C===B) && α≠1 && ip_error("scaled multiplicative update not available for these types.")
        α, β = convert.(T, (α, β))
        do_gemm!(α, tr2blas(A)..., tr2blas(B)..., β, C)
   catch err
       err isa InplaceException ? rethrow(err) : ip_error(err)
   end
end

C_AB!(C::BlasMatrix{T}, β::Number, α::Number, A::BlasMatrix{T}, B::BlasMatrix{T}) where T = gemm_αABβC!(α, A, B, β, C)
C_AB!(C::BlasMatrix{T}, β::Number, A::BlasMatrix{T}, α::Number, B::BlasMatrix{T}) where T = gemm_αABβC!(α, A, B, β, C)
C_AB!(C::BlasMatrix{T}, β::Number, A::BlasMatrix{T}, B::BlasMatrix{T}, α::Number) where T = gemm_αABβC!(α, A, B, β, C)

# AXPY and AXPBY and copyto!  =====================================================================

function C_AB!(y::AxpyVec{T}, β::Number, α1::Number, α2::Number, x::AxpyVec{T}) where T
    α = α1*α2
    try
        α, β = convert.(T, (α, β))
        β==0 && α==1 && return copyto!(y,x)
        β==1 ? BLAS.axpy!(α,x,y) : BLAS.axpby!(α,x,β,y)
    catch err
        ip_error(err)
    end
end

# GEMV and SYMV =====================================================================================

do_gemv!(α, TA, A, B, β, C) = BLAS.gemv!(TA, α, A, B, β, C)      # C ← αAB + βC  (with transposition as specified)
do_gemv!(α, TA, A::Symmetric, B, β, C) = BLAS.symv!(A.uplo, α, blasnode(A), B, β, C)

function gemv_αABβC!(α, A, B, β, C::BlasVector{T}) where T
    try
        C===A && α==1 && β==0 && return rmul!(A,B)
        C===B && α==1 && β==0 && return lmul!(A,B)
        (C===A || C===B) && β≠0 && ip_error("multiplicative and additive updates cannot be combined.")
        (C===A || C===B) && α≠1 && ip_error("scaled multiplicative update not available for these types (α==1 required).")
        α, β = convert.(T, (α, β))
        do_gemv!(α, tr2blas(A)..., B, β, C)
    catch err
        err isa InplaceException ? rethrow(err) : ip_error(err)
    end
end

C_AB!(C::BlasVector{T}, β::Number, α::Number, A::BlasMatrix{T}, B::BlasVector{T}) where T = gemv_αABβC!(α, A, B, β, C)
C_AB!(C::BlasVector{T}, β::Number, A::BlasMatrix{T}, α::Number, B::BlasVector{T}) where T = gemv_αABβC!(α, A, B, β, C)
C_AB!(C::BlasVector{T}, β::Number, A::BlasMatrix{T}, B::BlasVector{T}, α::Number) where T = gemv_αABβC!(α, A, B, β, C)

# SYRK =============================================
function do_syrk!(α, A, B, β, C::Symmetric{T}) where T
    TA, A = tr2blas(A)
    TB, B = tr2blas(B)
    cond1 = A===B
    cond2 = (TA=='N' && TB=='T') || (TA=='T' && TB=='N')
    ( cond1 && cond2) || ip_error("conditions violated for update to Symmetric LHS.")
    try
        α, β = convert.(T, (α, β))
        BLAS.syrk!(C.uplo, TA, α, A, β, blasnode(C))
        return C  # syrk! returns parent(C), which is usually not symmetric
    catch err
        ip_error(err)
    end
end

C_AB!(C::Symmetric{T}, β::Number, α::Number, A::BlasMatrix{T}, B::BlasMatrix{T}) where T = do_syrk!(α, A, B, β, C)
C_AB!(C::Symmetric{T}, β::Number, A::BlasMatrix{T}, α::Number, B::BlasMatrix{T}) where T = do_syrk!(α, A, B, β, C)
C_AB!(C::Symmetric{T}, β::Number, A::BlasMatrix{T}, B::BlasMatrix{T}, α::Number) where T = do_syrk!(α, A, B, β, C)

# HERK =============================================
function do_herk!(α, A, B, β, C::Hermitian{T}) where T  <: Complex{F} where F
    TA, A = tr2blas(A)
    TB, B = tr2blas(B)
    cond1 = A===B
    cond2 = (TA=='N' && TB=='C') || (TA=='C' && TB=='N')
    ( cond1 && cond2) || ip_error("conditions violated for update to Hermitian LHS.")
    try
        α, β = convert.(F, (α, β))
        BLAS.herk!(C.uplo, TA, α, A, β, blasnode(C))
        return C  # herk! returns parent(C)
    catch err
        ip_error(err)
    end
end

C_AB!(C::Hermitian{T}, β::Number, α::Number, A::BlasMatrix{T}, B::BlasMatrix{T}) where T <: Complex =
    do_herk!(α, A, B, β, C)
#
C_AB!(C::Hermitian{T}, β::Number, A::BlasMatrix{T}, α::Number, B::BlasMatrix{T}) where T <: Complex =
    do_herk!(α, A, B, β, C)
#
C_AB!(C::Hermitian{T}, β::Number, A::BlasMatrix{T}, B::BlasMatrix{T}, α::Number) where T <: Complex =
    do_herk!(α, A, B, β, C)
#

# SYR =============================================
function do_syr!(α, A, B, β, C::Symmetric{T}) where T
    β==1 || ip_error("symmetric rank 1 update cannot prescale LHS (β==1 required).")
    TB, B = tr2blas(B)
    (A===B && TB=='T') || ip_error("conditions violated for update to Symmetric LHS.")
    try
        α = convert(T, α)
        BLAS.syr!(C.uplo, α, A, blasnode(C))
        return C  # syr! returns parent(C), which is usually not symmetric
    catch err
        ip_error(err)
    end
end

C_AB!(C::Symmetric{T}, β::Number, α::Number, A::BlasVector{T}, B::BlasRow{T}) where T = do_syr!(α, A, B, β, C)
C_AB!(C::Symmetric{T}, β::Number, A::BlasVector{T}, α::Number, B::BlasRow{T}) where T = do_syr!(α, A, B, β, C)
C_AB!(C::Symmetric{T}, β::Number, A::BlasVector{T}, B::BlasRow{T}, α::Number) where T = do_syr!(α, A, B, β, C)

# HER =============================================
function do_her!(α, A, B, β, C::Hermitian{T}) where T  <: Complex{F} where F
    β==1 || ip_error("symmetric rank 1 update cannot prescale LHS (β==1 required).")
    A===parent(B) || ip_error("conditions violated for update to Hermitian LHS.")
    try
        α = convert(F, α)
        BLAS.her!(C.uplo, α, A, blasnode(C))
        return C  # her! returns parent(C)
    catch err
        ip_error(err)
    end
end

C_AB!(C::Hermitian{T}, β::Number, α::Number, A::BlasVector{T}, B::BlasAdjRow{T}) where T <: Complex =
    do_her!(α, A, B, β, C)
#
C_AB!(C::Hermitian{T}, β::Number, A::BlasVector{T}, α::Number, B::BlasAdjRow{T}) where T <: Complex =
    do_her!(α, A, B, β, C)
#
C_AB!(C::Hermitian{T}, β::Number, A::BlasVector{T}, B::BlasAdjRow{T}, α::Number) where T <: Complex =
    do_her!(α, A, B, β, C)
#

# TRMM ====================================
do_trmm!(α, B, side, A::SimpleTriangular)  = BLAS.trmm!(side, A.uplo, A.trans, A.diag, α, A.blasnode, B)
do_trmm!(α, B, side, A::UniformScaling) = rmul!(B,α*A.λ)
do_trmm!(α, B, side, A::Number) = rmul!(B,α*A)
#do_trmm!(B, α, A::InverseTriangular) = BLAS.trmm!(side, A.uplo, A.trans, A.diag, α, A.blasnode, B)

function trmm_αABβC!(α,A,B,β,C::BlasMatrix{T},side) where T
    β==0 || ip_error("additive update to LHS not available for triangular multiplier (β==0 required).")
    try
        α = convert(T,α)
        C===B || copyto!(C,B)
        do_trmm!(α, C, side, A)
    catch err
        ip_error(err)
    end

end

C_AB!(C::BlasMatrix{T}, β::Number, α::Number, A::BlasTriangular{T}, B::BlasMatrix{T}) where T =
    trmm_αABβC!(α, A, B, β, C, 'L')
#
C_AB!(C::BlasMatrix{T}, β::Number, A::BlasTriangular{T}, α::Number, B::BlasMatrix{T}) where T =
    trmm_αABβC!(α, A, B, β, C, 'L')
#
C_AB!(C::BlasMatrix{T}, β::Number, A::BlasTriangular{T}, B::BlasMatrix{T}, α::Number) where T =
    trmm_αABβC!(α, A, B, β, C, 'L')
#

C_AB!(C::BlasMatrix{T}, β::Number, α::Number, B::BlasMatrix{T}, A::BlasTriangular{T}) where T =
    trmm_αABβC!(α, A, B, β, C, 'R')
#
C_AB!(C::BlasMatrix{T}, β::Number, B::BlasMatrix{T}, α::Number, A::BlasTriangular{T}) where T =
    trmm_αABβC!(α, A, B, β, C, 'R')
#
C_AB!(C::BlasMatrix{T}, β::Number, B::BlasMatrix{T}, A::BlasTriangular{T}, α::Number) where T =
    trmm_αABβC!(α, A, B, β, C, 'R')
#

# TRMV ====================================
# This is handled by lmul!, which calls TRMV


text:
# infinit
rstudy001,cstudy001=simpleprover("study/study001.cnf",12,3)
rstudy001n,cstudy001n=simpleprover("study/study001n.cnf",12,3)
rstudy001n2,cstudy001n2=simpleprover("study/study001n2.cnf",12,3)
rstudy001n3,cstudy001n3=simpleprover("study/study001n3.cnf",12,3)

# finite
rstudy002,cstudy002=simpleprover("study/study002.cnf",4,3)
rstudy003,cstudy003=simpleprover("study/study003.cnf",4,3)

#
rstudy004,cstudy004=simpleprover("study/study004.cnf",10,3)
rstudy005,cstudy005=simpleprover("study/study005.cnf",5,5)
#rstudy005,cstudy005=simpleprover("study/study005.cnf",20,15)

max_stars_repo_name: sazare/cheapViews

max_issues_repo_name: sazare/cheapViews

max_forks_repo_name: sazare/cheapViews


text:
module IICGP

using CartesianGeneticProgramming
using Cambrian
using FileIO
using Logging
using Dates
import Formatting
import JSON
import YAML

include("individual.jl") # before evolution.jl
include("game.jl") # before evolution.jl
include("evolution.jl")
include("evaluation.jl")
include("functions.jl")
include("reducer.jl")
include("populate.jl")
include("config.jl")
include("process.jl")
include("postproc.jl")
include("utils.jl")
include("init.jl")
include("saving.jl")
include("visual.jl")
include("ucea.jl")

end


text:
## Solve the classical GP Regression ##
mutable struct Analytic{T<:Real} <: AbstractInference{T}
    ϵ::T # Convergence criteria
    n_iter::Int # Number of steps performed
    batchsize::Int
    HyperParametersUpdated::Bool #To know if the inverse kernel matrix must updated
    function Analytic{T}(ϵ::T) where {T}
        return new{T}(ϵ, 0, 0, false)
    end
end

"""
    Analytic(;ϵ::T=1e-5)

Analytic inference structure for solving the classical GP regression with Gaussian noise

## Keyword arguments
- `ϵ::Real` : convergence criteria (not used at the moment)
"""
Analytic

function Analytic(; ϵ::T=1e-5) where {T<:Real}
    return Analytic{T}(ϵ)
end

function Base.show(io::IO, ::Analytic)
    return print(io, "Analytic Inference")
end

function init_inference(
    i::Analytic{T}, nSamples::Integer, xview::TX, yview::TY
) where {T<:Real,TX,TY}
    return Analytic{T}(conv_crit(i), nSamples, xview, yview)
end

function analytic_updates(m::GP{T}, state, y) where {T}
    f = getf(m)
    l = likelihood(m)
    K = state.kernel_matrices.K
    f.post.Σ = K + first(l.σ²) * I
    f.post.α .= cov(f) \ (y - pr_mean(f, input(m.data)))
    if !isnothing(l.opt_noise)
        g = 0.5 * (norm(mean(f), 2) - tr(inv(cov(f))))
        state_σ², Δlogσ² = Optimisers.apply(
            l.opt_noise, state.local_vars.state_σ², l.σ², g .* l.σ²
        )
        local_vars = merge(state.local_vars, (; state_σ²))
        state = merge(state, (; local_vars))
        l.σ² .= exp.(log.(l.σ²) .+ Δlogσ²)
    end
    return state
end

ρ(::Analytic{T}) where {T} = one(T)

is_stochastic(::Analytic) = false


text:
module Trajectories

using Parameters
using Vec, LinearAlgebra
using Convex, SCS, ECOS
using NearestNeighbors
using GraphUtils

using ..Utils
using ..GridPaths

export
    TimeInterval,
    verify,
    get_start_time,
    get_end_time,
    get_Δt,
    TrajectoryPoint,

    AbstractTrajectory,
    TrajectoryPrimitive,
    AbstractTrackingTrajectory,
    AbstractStabilizingTrajectory,
    AbstractPivotingTrajectory,
    CompositeTrajectory,
    EmptyPrimitive,

    get_Δt,
    get_start_pt,
    get_end_pt,
    get_length,
    get_dist,
    get_position,
    get_heading,
    get_vel,
    get_yaw_rate,
    verify_abstract_traj,

    get_time_from_arc_length,
    get_time_from_pt,
    get_trajectory_point_by_time,
    get_trajectory_point_by_pt,

    StraightTrajectory,
    ArcTrajectory,
    WaitTrajectory,
    PivotTrajectory,
    ReverseTrajectory,

    Trajectory,
    get_active_segment_idx,
    get_active_segment,
    cap_trajectory,
    push_with_reverse_flag!,
    construct_trajectory,
    optimize_velocity_profile,
    optimize_velocity_profile_traj_only,

    DenseTrajectory,
    get_index_time,
    SimpleTrajectory,

    stitch_trajectories

"""
    `TimeInterval`

    Defines the start and end time (the speed profile, essentially) for a
    particular AbstractTrajectory
"""
struct TimeInterval
    t1::Float64
    t2::Float64
end
get_start_time(interval::TimeInterval) = interval.t1
get_end_time(interval::TimeInterval) = interval.t2
function get_Δt(i::TimeInterval,t::Float64)
    dt = (i.t2 - i.t1)
    if dt == 0.0
        return 0.0
    end
    return (t - i.t1) / dt
 end
function verify(i::TimeInterval)
    @assert (
        get_start_time(i) <= get_end_time(i)
        ) "end time is before start time"
end
"""
    `TrajectoryPoint`

    The feed forward command (2D rigid body state) associated with a particular
    point on a trajectory.
"""
@with_kw struct TrajectoryPoint
    pos::VecE2          = VecE2()
    heading::VecE2      = VecE2()
    vel::VecE2          = VecE2()
    yaw_rate::Float64   = 0.0
    t::Float64          = NaN
end
function Utils.interpolate(pt1::TrajectoryPoint,pt2::TrajectoryPoint,t::Float64)
    θ1 = atan(pt1.heading)
    θ2 = atan(pt2.heading)
    Δθ = get_angular_offset(θ1,θ2)
    θ = interpolate(θ1,θ1+Δθ,t)
    heading = VecE2(cos(θ),sin(θ))
    TrajectoryPoint(
        pos         = interpolate(pt1.pos, pt2.pos, t),
        heading     = heading,
        vel         = interpolate(pt1.vel, pt2.vel, t),
        yaw_rate    = interpolate(pt1.yaw_rate, pt2.yaw_rate, t),
        t           = interpolate(pt1.t, pt2.t, t)
    )
end
function verify(pt::TrajectoryPoint)
    @assert (!isnan(pt.t)) "time is NaN"
    @assert (!any(isnan,pt.pos)) "pos is NaN"
    @assert (!any(isnan,pt.heading)) "heading is NaN"
    @assert (!any(isnan,pt.vel)) "vel is NaN"
    @assert (!isnan(pt.yaw_rate)) "yaw_rate is NaN"
end

abstract type AbstractTrajectory end
abstract type TrajectoryPrimitive <: AbstractTrajectory end
abstract type AbstractTrackingTrajectory <: TrajectoryPrimitive end
abstract type AbstractStabilizingTrajectory <: TrajectoryPrimitive end
abstract type AbstractPivotingTrajectory <: TrajectoryPrimitive end
abstract type CompositeTrajectory <: AbstractTrajectory end
struct EmptyPrimitive <: AbstractTrajectory end
"""
    `get_start_time`

    Returns the start time of a given AbstractTrajectory.
"""
function get_start_time(traj::T) where {T <: TrajectoryPrimitive}
    return get_start_time(traj.interval)
end
"""
    `get_end_time`

    Returns the end time of a given AbstractTrajectory.
"""
function get_end_time(traj::T) where {T <: TrajectoryPrimitive}
    return get_end_time(traj.interval)
end
"""
    `get_Δt`

    Returns the fraction Δt by which t interpolates `get_start_time(traj)` and
    `get_end_time(traj)`
"""
function get_Δt(traj::T,t::Float64) where {T <: TrajectoryPrimitive}
    get_Δt(traj.interval,t)
end

"""
    `get_trajectory_point_by_time`

    Returns the TrajectoryPoint associated with a particular time along a
    AbstractTrajectory.
"""
function get_trajectory_point_by_time(traj::T,t::Float64) where {T <: AbstractTrajectory}
    pos         = get_position(traj,t)
    heading     = get_heading(traj,t)
    vel         = get_vel(traj,t)
    yaw_rate    = get_yaw_rate(traj,t)
    TrajectoryPoint(pos,heading,vel,yaw_rate,t)
end
"""
    `get_time_from_arc_length(traj::T,s::Float64)`

    Returns the time associated with a particular arc length
"""
function get_time_from_arc_length(traj::T,s::Float64) where {T <: AbstractTrajectory}
    Δt = get_end_time(traj) - get_start_time(traj)
    if get_length(traj) == 0.0
        return get_start_time(traj)
    end
    return get_start_time(traj) + Δt*(s / get_length(traj))
end
"""
    `get_trajectory_point_by_pt`

    Returns the TrajectoryPoint associated with a particular location along a
    AbstractTrajectory.
"""
function get_trajectory_point_by_pt(traj::T,pt::VecE2) where {T <: AbstractTrajectory}
    get_trajectory_point_by_time(traj,get_time_from_pt(traj,pt))
end
"""
    `get_closest_pt(traj::T, pt::VecE2) where {T <: AbstractTrajectory}`

    Finds the closest point on `traj` to `pt` (based on Euclidean distance)
"""
function get_closest_pt(traj::T, pt::VecE2) where {T <: AbstractTrajectory}
    t = get_time_from_pt(traj,pt)
    t = min(max(t,get_start_time(traj)),get_end_time(traj))
    get_trajectory_point_by_time(traj, t)
end
"""
    `get_start_pt`

    Returns the start point of a given AbstractTrajectory.
"""
function get_start_pt end
"""
    `get_end_pt`

    Returns the end point of a given AbstractTrajectory.
"""
function get_end_pt end
"""
    `get_length`

    Return the length of a AbstractTrajectory
"""
function get_length end
"""
    `get_dist`

    Return the distance along a AbstractTrajectory from the beginning up to a
    given time.
"""
function get_dist end
"""
    `get_position`

    Return the position associated with a particular time along a
    AbstractTrajectory
"""
function get_position end
"""
    `get_heading`

    Return the position associated with a particular time along a
    AbstractTrajectory
"""
function get_heading end
"""
    `get_vel`

    Return the speed associated with a particular time along a
    AbstractTrajectory
"""
function get_vel end
"""
    `get_yaw_rate`

    Return the yaw rate associated with a particular time along a
    AbstractTrajectory
"""
function get_yaw_rate end
"""
    `get_primitive`
"""
function get_primitive end
function verify_abstract_traj(traj::T) where {T<:AbstractTrajectory}
    t_mid = (get_start_time(traj)+get_end_time(traj))/2.0
    @assert !isnan(get_yaw_rate(traj,t_mid)) "get_yaw_rate is NaN"
    @assert !isnan(get_dist(traj,t_mid)) "get_dist is NaN"
    @assert !any(isnan,get_position(traj,t_mid)) "get_position is NaN"
    @assert !any(isnan,get_heading(traj,t_mid)) "get_heading is NaN"
    @assert !any(isnan,get_vel(traj,t_mid)) "get_vel is NaN"
end

"""
    `get_active_segment(traj::T,t::Float64) where {T <: AbstractTrajectory}`

    Returns the trajectory active at time `t`. For a base motion primitive (e.g.
    `StraightTrajectory`, `ArcTrajectory`, etc.) This is just `traj`.
    The function must be implemented for Composite Trajectory types such that
    a call to `get_active_segment` will recurse all the way down to the "leaf"
    trajectory primitives, thus permitting hierarchical definition of
    trajectories.
"""
get_active_segment(traj::T,t::Float64) where {T <: AbstractTrajectory} = traj

"""
    `StraightTrajectory`
"""
struct StraightTrajectory <: AbstractTrackingTrajectory
    pt1::VecE2 # start point
    # pt2::VecE2 # end point
    heading::VecE2
    length::Float64
    interval::TimeInterval
end
function StraightTrajectory(pt1::VecE2,pt2::VecE2,interval::TimeInterval)
    L = norm(pt2-pt1)
    StraightTrajectory(pt1,(pt2-pt1)/L,L,interval)
end
get_start_pt(traj::StraightTrajectory) = traj.pt1
get_end_pt(traj::StraightTrajectory) = traj.pt1 + traj.length * traj.heading
function verify(traj::StraightTrajectory)
    @assert (!any(isnan, get_start_pt(traj))) "start point is NaN"
    @assert (!any(isnan, get_end_pt(traj))) "end point is NaN"
    verify_abstract_traj(traj)
    verify(traj.interval)
end
function get_length(traj::StraightTrajectory)
    norm(get_end_pt(traj) - get_start_pt(traj))
end
function get_dist(traj::StraightTrajectory, t::Float64)
    dt = get_end_time(traj) - get_start_time(traj)
    return get_length(traj) * get_Δt(traj,t)
end
function get_position(traj::StraightTrajectory,t::Float64)
    p1 = get_start_pt(traj)
    p2 = get_end_pt(traj)
    return p1 + get_Δt(traj,t) * (p2 - p1)
end
function get_heading(traj::StraightTrajectory,t::Float64)
    # p1 = get_start_pt(traj)
    # p2 = get_end_pt(traj)
    # return (p2-p1) / norm(p2-p1)
    return traj.heading
end
function get_vel(traj::StraightTrajectory, t::Float64)
    # vec = get_end_pt(traj) - get_start_pt(traj)
    vec = get_heading(traj,t) * get_length(traj)
    dt = get_end_time(traj) - get_start_time(traj)
    if get_length(traj) == 0
        return VecE2(0.0,0.0)
    end
    return vec / dt
end
function get_yaw_rate(traj::StraightTrajectory,t::Float64)
    return 0.0
end
function get_time_from_pt(traj::StraightTrajectory,pt::VecE2)
    p1 = get_start_pt(traj)
    p2 = get_end_pt(traj)
    s = proj(pt-p1,get_heading(traj,get_end_time(traj)),Float64) * get_length(traj)
    get_time_from_arc_length(traj,s)
end

"""
    `ArcTrajectory`
"""
struct ArcTrajectory <: AbstractTrackingTrajectory
    center::VecE2
    radius::Float64 #
    heading1::VecE2
    θ1::Float64 # start angle of vector from center to pt1
    Δθ::Float64
    θ2::Float64 # end angle of vector from center to pt1
    interval::TimeInterval
end
function ArcTrajectory(center::VecE2,pt1::VecE2,heading1::VecE2,
    Δθ::Float64,interval::TimeInterval)
    radius = norm(pt1-center)
    sgn = sign(cross([(pt1-center)..., 0],[heading1..., 0 ])[end])
    if sgn == 0
        θ1 = atan(heading1) - π/2
    else
        θ1 = atan(heading1) - sgn*π/2
    end
    return ArcTrajectory(center,radius,heading1,mod(θ1,2π),Δθ,mod(θ1,2π)+Δθ,interval)
end
function ArcTrajectory(center::VecE2,radius::Float64,θ1::Float64,
    Δθ::Float64,interval::TimeInterval)
    if sign(Δθ) == 0
        heading1 = VecE2(NaN,NaN) # undefined
    else
        heading1 = VecE2(-sin(θ1),cos(θ1)) * sign(Δθ)
    end
    return ArcTrajectory(center,radius,heading1,mod(θ1,2π),Δθ,mod(θ1,2π)+Δθ,interval)
end
function verify(traj::ArcTrajectory)
    @assert !any(isnan, traj.center) "center is NaN"
    @assert (0.0 <= traj.θ1 <= 2π) "θ1 is outside of [0,2π]"
    # @assert (0.0 <= traj.θ2 <= 2π) "θ2 is outside of [0,2π]"
    @assert (traj.radius >= 0.0) "radius is negative"
    verify_abstract_traj(traj)
    verify(traj.interval)
end
get_start_pt(traj::ArcTrajectory) = traj.center + traj.radius*VecE2(cos(traj.θ1),sin(traj.θ1))
get_end_pt(traj::ArcTrajectory) = traj.center + traj.radius*VecE2(cos(traj.θ2),sin(traj.θ2))
function get_length(traj::ArcTrajectory)
    arclength = traj.radius * abs(traj.Δθ)
end
function get_dist(traj::ArcTrajectory, t::Float64)
    dt = get_end_time(traj) - get_start_time(traj)
    return get_length(traj) * get_Δt(traj,t) #(t-get_start_time(traj)) / dt
end
function get_position(traj::ArcTrajectory,t::Float64)
    θ = traj.θ1 + get_Δt(traj,t) * traj.Δθ
    return traj.center + traj.radius*VecE2(cos(θ),sin(θ))
end
function get_heading(traj::ArcTrajectory,t::Float64)
    Δθ = get_Δt(traj,t) * traj.Δθ
    heading = rot(traj.heading1,Δθ)
end
function get_vel(traj::ArcTrajectory, t::Float64)
    dt = get_end_time(traj) - get_start_time(traj)
    return get_heading(traj,t) * get_length(traj) / dt
end
function get_yaw_rate(traj::ArcTrajectory,t::Float64)
    dt = get_end_time(traj) - get_start_time(traj)
    return traj.Δθ / dt
end
function get_time_from_pt(traj::ArcTrajectory,pt::VecE2)
    if traj.Δθ == 0
        return get_start_time(traj)
    end
    θ = atan(pt-traj.center)
    Δθ = get_angular_offset(traj.θ1,θ)
    while sign(Δθ) != sign(traj.Δθ)
        Δθ += 2π*sign(traj.Δθ)
    end
    t = Δθ/traj.Δθ
end

"""
    ` WaitTrajectory <: AbstractTrajectory`

    When the robot should not be moving
"""
struct WaitTrajectory <: AbstractStabilizingTrajectory
    pt1::VecE2 # start point
    heading::VecE2
    interval::TimeInterval
end
function StraightTrajectory(traj::WaitTrajectory)
    StraightTrajectory(traj.pt1,traj.heading,0.0,traj.interval)
end
get_start_pt(traj::WaitTrajectory) = get_start_pt(StraightTrajectory(traj))
get_end_pt(traj::WaitTrajectory) = get_end_pt(StraightTrajectory(traj))
verify(traj::WaitTrajectory) = verify(StraightTrajectory(traj))
get_length(traj::WaitTrajectory) = 0.0
get_dist(traj::WaitTrajectory, t::Float64) = 0.0
get_position(traj::WaitTrajectory,t::Float64) = traj.pt1
get_heading(traj::WaitTrajectory,t::Float64) = traj.heading
get_vel(traj::WaitTrajectory, t::Float64) = VecE2(0.0,0.0)
get_yaw_rate(traj::WaitTrajectory,t::Float64) = get_yaw_rate(StraightTrajectory(traj),t)
get_time_from_pt(traj::WaitTrajectory,pt::VecE2) = get_time_from_pt(StraightTrajectory(traj),pt)

"""
    `PivotTrajectory`

    When the robot simply turns in place.
"""
struct PivotTrajectory <: AbstractPivotingTrajectory
    pt1::VecE2
    heading1::VecE2
    Δθ::Float64
    interval::TimeInterval
end
function ArcTrajectory(traj::PivotTrajectory)
    ArcTrajectory(traj.pt1,traj.pt1,traj.heading1,traj.Δθ,traj.interval)
end
get_start_pt(traj::PivotTrajectory) = get_start_pt(ArcTrajectory(traj))
get_end_pt(traj::PivotTrajectory) = get_end_pt(ArcTrajectory(traj))
verify(traj::PivotTrajectory) = verify(ArcTrajectory(traj))
get_length(traj::PivotTrajectory) = get_length(ArcTrajectory(traj))
get_dist(traj::PivotTrajectory, t::Float64) = get_dist(ArcTrajectory(traj), t)
get_position(traj::PivotTrajectory,t::Float64) = traj.pt1
get_heading(traj::PivotTrajectory,t::Float64) = get_heading(ArcTrajectory(traj),t)
get_vel(traj::PivotTrajectory, t::Float64) = VecE2(0.0,0.0)
get_yaw_rate(traj::PivotTrajectory,t::Float64) = get_yaw_rate(ArcTrajectory(traj),t)
get_time_from_pt(traj::PivotTrajectory,pt::VecE2) = get_time_from_pt(ArcTrajectory(traj),pt)

for T = (:StraightTrajectory, :ArcTrajectory, :WaitTrajectory, :PivotTrajectory)
    @eval get_primitive(traj::$T) = traj
end

export
    get_reversed_trajectory_point_by_time

"""
    `ReverseTrajectory`

    When the robot is supposed to be driving in reverse
"""
struct ReverseTrajectory{T} <: TrajectoryPrimitive
    traj::T
end
for op = (:get_start_time, :get_end_time, :get_start_pt, :get_end_pt, :verify, :get_length)
    @eval $op(traj::R) where {R<:ReverseTrajectory} = $op(traj.traj)
end
get_dist(traj::R,t::Float64) where {R<:ReverseTrajectory}      = get_dist(traj.traj, t)
get_position(traj::R,t::Float64) where {R<:ReverseTrajectory}   = get_position(traj.traj, t)
get_heading(traj::R,t::Float64) where {R<:ReverseTrajectory}    = get_heading(traj.traj, t)
get_vel(traj::R,t::Float64) where {R<:ReverseTrajectory}       = get_vel(traj.traj, t)
get_yaw_rate(traj::R,t::Float64) where {R<:ReverseTrajectory}   = get_yaw_rate(traj.traj, t)
get_time_from_pt(traj::R, pt::VecE2) where {R<:ReverseTrajectory} = get_time_from_pt(traj.traj, pt)
get_primitive(traj::R) where {R<:ReverseTrajectory} = get_primitive(traj.traj)
function get_reversed_trajectory_point_by_time(traj::R,t::Float64) where {R<:ReverseTrajectory}
    ref = get_trajectory_point_by_time(traj,t)
    TrajectoryPoint(
        pos     = ref.pos,
        heading = ref.heading,
        vel     = ref.vel,
        yaw_rate= ref.yaw_rate,
        t       = ref.t
    )
end

"""
    `DenseTrajectory`

    Contains an underlying `AbstractTrajectory` with extra information about the
    acceleration profile (and the resultant velocity and position profiles).
"""
struct DenseTrajectory{T<:AbstractTrajectory} <: CompositeTrajectory
    traj::T
    t_vec::Vector{Float64}
    accel::Vector{Float64}
    vel::Vector{Float64}
    pos::Vector{Float64}
end
for op = (:get_start_time, :get_end_time, :get_start_pt, :get_end_pt, :get_length)
    @eval $op(traj::R) where {R<:DenseTrajectory} = $op(traj.traj)
end
function verify(traj::DenseTrajectory)
    verify(traj.traj)
    @assert isapprox(traj.t_vec[1], get_start_time(traj.traj);atol=1e-10) "start times do not match"
    @assert isapprox(traj.t_vec[end], get_end_time(traj.traj);atol=1e-10) "end times do not match"
    @assert isapprox(traj.pos[end], get_length(traj.traj);atol=1e-10) "lengths do not match"
    @assert length(traj.vel) == length(traj.pos) == length(traj.t_vec) "t_vec, vel, pos have different lengths"
    @assert length(traj.accel) == max(0,length(traj.t_vec)-1) "vel and pos have different lengths"
    verify_abstract_traj(traj)
end
function get_dist(traj::DenseTrajectory, t::Float64)
    idx, Δe = linear_interp(traj.t_vec, t)
    # This could be replaced with precise integration of accel and vel signals...
    if traj.pos[idx] == traj.pos[idx+1]
        return traj.pos[idx]
    end
    s = interpolate(traj.pos[idx], traj.pos[idx+1], Δe)
    return s
end
"""
    `get_index_time(traj::DenseTrajectory, t::Float64)`
"""
function get_index_time(traj::DenseTrajectory, t::Float64)
    idx, Δe = linear_interp(traj.t_vec, t)
    ds = (traj.pos[idx+1] - traj.pos[idx])
    s = traj.pos[idx]+Δe*ds
    dt = (traj.t_vec[idx+1] - traj.t_vec[idx])
    if isapprox(ds,0.0;atol=1e-5)
        return traj.t_vec[idx] + Δe*dt
    end
    return get_time_from_arc_length(traj.traj, s)
end
function get_position(traj::DenseTrajectory, t::Float64)
    t_idx = get_index_time(traj,t)
    get_position(traj.traj, t_idx)
end
function get_heading(traj::DenseTrajectory, t::Float64)
    t_idx = get_index_time(traj,t)
    get_heading(traj.traj, t_idx)
end
function get_vel(traj::DenseTrajectory, t::Float64)
    idx, Δe = linear_interp(traj.t_vec, t)
    v = interpolate(traj.vel[idx], traj.vel[idx+1],Δe)
    return v * get_heading(traj,t)
end
function get_accel(traj::DenseTrajectory, t::Float64)
    idx, Δe = linear_interp(traj.t_vec, t)
    a = interpolate(traj.accel[idx], traj.accel[idx+1],Δe)
    return a
end
function get_yaw_rate(traj::DenseTrajectory,t::Float64)
    v_true = norm(get_vel(traj, t))
    t_idx = get_index_time(traj,t)
    v_nominal = norm(get_vel(traj.traj, t_idx))
    yw_nominal = get_yaw_rate(traj.traj, t_idx)
    if v_nominal != 0
        return yw_nominal * (v_true / v_nominal)
    else
        return yw_nominal
    end
end
get_primitive(traj::R) where {R<:DenseTrajectory} = get_primitive(traj.traj)

"""
    `Trajectory`
"""
@with_kw struct Trajectory <: CompositeTrajectory
    t_vec::Vector{Float64}               = Vector{Float64}() # time vector
    s_vec::Vector{Float64}               = Vector{Float64}() # arc length vector
    segments::Vector{AbstractTrajectory} = Vector{AbstractTrajectory}()
end
function Trajectory(segments::Vector{AbstractTrajectory})
    t_vec = map(seg->get_start_time(seg), segments)
    push!(t_vec, get_end_time(segments[end]))
    s_vec = cumsum([0.0, map(seg->get_length(seg), segments)...])
    Trajectory(t_vec,s_vec,segments)
end
Trajectory(v::Vector{T}) where {T <: AbstractTrajectory} = Trajectory(Vector{AbstractTrajectory}(v))
function Base.push!(traj::Trajectory,seg::T) where {T <: AbstractTrajectory}
    if length(traj.segments) == 0
        push!(traj.t_vec, get_start_time(seg))
        push!(traj.s_vec, 0.0)
    end
    push!(traj.t_vec, get_end_time(seg))
    push!(traj.s_vec, traj.s_vec[end] + get_length(seg))
    push!(traj.segments, seg)
    traj
end
function push_with_reverse_flag!(traj::Trajectory,seg::T,flag::Bool=false) where {T <: AbstractTrajectory}
    if flag
        push!(traj,ReverseTrajectory(seg))
    else
        push!(traj,seg)
    end
    traj
end
function verify(traj::Trajectory)
    for (i,p) in enumerate(traj.segments)
        if i < length(traj.segments)
            @assert (
                get_start_time(traj.segments[i+1]) == get_end_time(p) == traj.t_vec[i+1]
            ) "segment start and end times are not properly aligned"
            @assert (
                norm(get_start_pt(traj.segments[i+1]) - get_end_pt(p)) < 0.0000001
            ) "segment start and end positions are not properly aligned"
            @assert (
                length(traj.s_vec) == length(traj.t_vec)
            ) "s_vec and t_vec are not the same length"
        end
        verify(p)
    end
    verify_abstract_traj(traj)
end
get_length(traj::Trajectory) = sum([get_length(seg) for seg in traj.segments])
"""
    `get_active_segment_idx`

    Returns the integer index identifying which mode of the trajectory is active
"""
function get_active_segment_idx(traj::Trajectory,t::Float64)
    idx,Δe = linear_interp(traj.t_vec,t)
    return idx
end
function get_active_segment(traj::Trajectory,t::Float64)
    idx = get_active_segment_idx(traj,t)
    get_active_segment(traj.segments[idx],t)
end
function get_time_from_arc_length(traj::Trajectory,s::Float64)
    idx,Δe = linear_interp(traj.s_vec,s)
    seg = traj.segments[idx]
    get_time_from_arc_length(seg, s-traj.s_vec[idx])
end
for op = (:get_dist, :get_position, :get_heading, :get_vel, :get_yaw_rate)
    @eval $op(traj::T,t::Float64) where {T<:Trajectory} = $op(get_active_segment(traj,t),t)
end
for op = (:get_start_time, :get_start_pt)
    @eval begin
        function $op(traj::T) where {T<:Trajectory}
            if length(traj.segments) > 0
                return $op(traj.segments[1])
            else
                return NaN
            end
        end
    end
end
for op = (:get_end_time, :get_end_pt)
    @eval begin
        function $op(traj::T) where {T<:Trajectory}
            if length(traj.segments) > 0
                return $op(traj.segments[end])
            else
                return NaN
            end
        end
    end
end

"""
    `cap_trajectory(traj::Trajectory)`
"""
function cap_trajectory(traj::Trajectory)
    start = WaitTrajectory(
        get_start_pt(traj),
        get_heading(traj,get_start_time(traj)),
        TimeInterval(get_start_time(traj),get_start_time(traj))
        )
    finish = WaitTrajectory(
        get_end_pt(traj),
        get_heading(traj,get_end_time(traj)),
        TimeInterval(get_end_time(traj),get_end_time(traj))
        )
    new_traj = Trajectory()
    push!(new_traj,start)
    for seg in traj.segments
        push!(new_traj,seg)
    end
    push!(new_traj,finish)
    new_traj
end

"""
    `construct_trajectory(path::GridWorldPath;cap=true)`

    Returns a Trajectory that matches the sequence of vertices visited by `path`
    Inputs:
    - path::GridWorldPath - contains information about the initial starting
    position and time of the robot, as well as the series of transitions that
    the robot must make.
    - cap::Bool - when `true`, calls the `cap_trajectory` function on the traj
    before returning it.
"""
function construct_trajectory(path::GridWorldPath;cap::Bool=true)
    N = length(path.waypoints)
    @assert (N > 0) "path is empty"

    traj = Trajectory()
    # trajectory head: position, orientation, time
    pos     = path.start_pt
    ctr_pos = pos
    h       = VecE2(NaN,NaN)
    t       = path.start_time
    ctr_t   = t
    reverse_flag = false

    i = 1
    while i <= N
        w = path.waypoints[i]
        a = w.transition
        h_next = get_heading_vector(a)
        if i == 1
            # Stay in place if a == WAIT
            while (a == WAIT) && (i < N)
                # increment head (time only)
                t = w.t
                # skip to next action
                i += 1
                w = path.waypoints[i]
                a = w.transition
            end
            ctr_pos = w.pt
            ctr_t   = w.t
            # straight to boundary between cells
            seg = StraightTrajectory(pos,get_heading_vector(a),path.cellwidth/2,TimeInterval(t,(t+ctr_t)/2))
            push_with_reverse_flag!(traj, seg, reverse_flag)
        elseif a == WAIT
            ctr_pos_0 = ctr_pos
            ctr_t_0 = ctr_t
            while i <= N
                w = path.waypoints[i]
                a = w.transition
                h_next = get_heading_vector(a)
                if a != WAIT
                    if abs(dot(h,h_next)) != 1 # abs(Δθ) > 0
                        radius = path.cellwidth / 2
                        center = pos + radius * h_next
                        Δθ = get_angular_offset(atan(h),atan(h_next))
                        # halfway t->ctr_t_0
                        seg = ArcTrajectory(center,get_end_pt(traj),get_heading(traj,get_end_time(traj)),Δθ/2,TimeInterval(t,ctr_t_0))
                        push_with_reverse_flag!(traj, seg, reverse_flag)
                        # wait ctr_t_0 -> ctr_t
                        # seg = ArcTrajectory(center,get_end_pt(traj),get_heading(traj,get_end_time(traj)),0.0,TimeInterval(ctr_t_0,ctr_t))
                        seg = WaitTrajectory(get_end_pt(traj),get_heading(traj,get_end_time(traj)),TimeInterval(ctr_t_0,ctr_t))
                        push_with_reverse_flag!(traj, seg, reverse_flag)
                        # halfway again ctr_t -> (ctr_t+w.t)/2
                        seg = ArcTrajectory(center,get_end_pt(traj),get_heading(traj,get_end_time(traj)),Δθ/2,TimeInterval(ctr_t,(ctr_t + w.t)/2))
                        push_with_reverse_flag!(traj, seg, reverse_flag)
                    else # straight and wait
                        # straight to center of cell
                        seg = StraightTrajectory(pos,h,path.cellwidth/2,TimeInterval(t,ctr_t_0))
                        push_with_reverse_flag!(traj, seg, reverse_flag)
                        # wait
                        # seg = StraightTrajectory(ctr_pos_0,h_next,0.0,TimeInterval(ctr_t_0,ctr_t))
                        seg = WaitTrajectory(get_end_pt(traj),get_heading(traj,get_end_time(traj)),TimeInterval(ctr_t_0,ctr_t))
                        push_with_reverse_flag!(traj, seg, reverse_flag)
                        # straight on
                        if dot(h,h_next) < 0
                            reverse_flag = !reverse_flag
                        end
                        seg = StraightTrajectory(ctr_pos,h_next,path.cellwidth/2,TimeInterval(ctr_t,(ctr_t+w.t)/2))
                        push_with_reverse_flag!(traj, seg, reverse_flag)
                    end
                    break
                else
                    ctr_pos = w.pt
                    ctr_t = w.t
                    # Handle the case where the robot is just waiting until the end
                    if i == N
                        # straight to center of cell
                        seg = StraightTrajectory(pos,h,path.cellwidth/2,TimeInterval(t,ctr_t_0))
                        push_with_reverse_flag!(traj, seg, reverse_flag)
                        # wait
                        # seg = WaitTrajectory(get_end_pt(traj),get_heading(traj,get_end_time(traj)),TimeInterval(ctr_t_0,ctr_t))
                        # push_with_reverse_flag!(traj, seg, reverse_flag)
                        if cap
                            return cap_trajectory(traj)
                        end
                        return traj
                    end
                    i += 1
                end
            end
        elseif dot(h,h_next) == 1 # abs(Δθ) == 0
            # straight to next boundary
            seg = StraightTrajectory(pos,h,path.cellwidth/2,TimeInterval(t,ctr_t))
            push_with_reverse_flag!(traj, seg, reverse_flag)
            seg = StraightTrajectory(ctr_pos,h_next,path.cellwidth/2,TimeInterval(ctr_t,(ctr_t + w.t)/2))
            push_with_reverse_flag!(traj, seg, reverse_flag)
        elseif dot(h,h_next) == 0 # abs(Δθ) == π/2
            # Turn to next boundary
            radius = path.cellwidth / 2
            center = pos + radius * h_next
            Δθ = get_angular_offset(atan(h),atan(h_next))
            seg = ArcTrajectory(center,pos,get_heading(traj,get_end_time(traj)),Δθ/2,TimeInterval(t,ctr_t))
            push_with_reverse_flag!(traj, seg, reverse_flag)
            seg = ArcTrajectory(center,pos,get_heading(traj,get_end_time(traj)),Δθ/2,TimeInterval(ctr_t,(ctr_t + w.t)/2))
            push_with_reverse_flag!(traj, seg, reverse_flag)
        elseif dot(h,h_next) == -1 # abs(Δθ) == π
            # reverse: straight to interior point, then backward to boundary
            # TODO this is still a problem!!! Need a flag telling the robot to
            # reverse its coordinate frame
            seg = StraightTrajectory(pos,ctr_pos,TimeInterval(t,ctr_t))
            push_with_reverse_flag!(traj, seg, reverse_flag)
            seg = WaitTrajectory(ctr_pos,h_next,TimeInterval(ctr_t,ctr_t))
            push_with_reverse_flag!(traj, seg, reverse_flag)
            # switch flag
            reverse_flag = !reverse_flag
            seg = StraightTrajectory(ctr_pos,pos,TimeInterval(ctr_t,(ctr_t + w.t)/2))
            push_with_reverse_flag!(traj, seg, reverse_flag)
        end
        pos = get_end_pt(traj)
        h   = get_heading_vector(a)
        t   = get_end_time(traj)
        ctr_pos = w.pt
        ctr_t   = w.t
        i += 1
    end
    # final half piece
    seg = StraightTrajectory(pos,get_heading(traj,get_end_time(traj)),
        path.cellwidth/2,TimeInterval(t,ctr_t))
    push_with_reverse_flag!(traj, seg, reverse_flag)
    if cap
        return cap_trajectory(traj)
    end
    return traj
end

struct SimpleTrajectory <: AbstractTrajectory
    pts ::Vector{TrajectoryPoint}
    t   ::Vector{Float64}
    s   ::Vector{Float64}
end
function SimpleTrajectory(traj::T,t_range) where {T <: AbstractTrajectory}
    pts     = Vector{TrajectoryPoint}()
    t_vec   = Vector{Float64}()
    s       = Vector{Float64}()
    for (i,t) in enumerate(t_range)
        push!(pts, get_trajectory_point_by_time(traj,t))
        push!(s, get_dist(traj,t))
        push!(t_vec, t)
    end
    SimpleTrajectory(pts,t_vec,s)
end
function SimpleTrajectory(traj::T,Δt::Float64) where{T <: AbstractTrajectory}
    t0 = get_start_time(traj)
    tf = get_end_time(traj)
    t_range = collect(t0:Δt:tf)
    SimpleTrajectory(traj,t_range)
end
function SimpleTrajectory(traj::T,N::Int) where{T <: AbstractTrajectory}
    t0 = get_start_time(traj)
    tf = get_end_time(traj)
    t_range = [t0 + (tf-t0)*(i/(N-1)) for i in 0:N-1]
    SimpleTrajectory(traj,t_range)
end
function verify(traj::SimpleTrajectory)
    @assert length(traj.t) == length(traj.s) == length(traj.pts) "lengths do not match"
    verify_abstract_traj(traj)
end
get_start_time(traj::SimpleTrajectory)  = traj.t[1]
get_end_time(traj::SimpleTrajectory)    = traj.t[end]
get_start_pt(traj::SimpleTrajectory)    = traj.pts[1]
get_end_pt(traj::SimpleTrajectory)      = traj.pts[end]
function get_trajectory_point_by_time(traj::SimpleTrajectory,t::Float64)
    idx,Δe = linear_interp(traj.t,t)
    pt = interpolate(traj.pts[idx],traj.pts[idx+1],Δe)
end
function get_dist(traj::SimpleTrajectory, t::Float64)
    idx, Δe = linear_interp(traj.t, t)
    interpolate(traj.s[idx],traj.s[idx+1], Δe)
end
function get_position(traj::SimpleTrajectory,t::Float64)
    get_trajectory_point_by_time(traj,t).pos
end
function get_vel(traj::SimpleTrajectory,t::Float64)
    get_trajectory_point_by_time(traj,t).vel
end
function get_heading(traj::SimpleTrajectory,t::Float64)
    get_trajectory_point_by_time(traj,t).heading
end
function get_yaw_rate(traj::SimpleTrajectory,t::Float64)
    get_trajectory_point_by_time(traj,t).yaw_rate
end

"""
    `optimize_velocity_profile(traj::Trajectory)`

    Given the constraints on robot position as a function of time, compute
    a dynamically feasible velocity profile that minimizes (the objective can
    be modified) the squared control effort.

    Params:
    * m::Int - the number of discrete control command blocks per trajectory.
    * n::Int - the polynomial order of the command signal within each command
    window

    Returns:
    - t_vec ::Vector{Float64} - a vector of time indices of length N*m + 1
    - accel ::Vector{Float64} - a vector of accel values of length N*m
    - vel   ::Vector{Float64} - a vector of speed values of length N*m + 1
    - pos   ::Vector{Float64} - a vector of position values of length N*m + 1

    TODO: make the optimization objective tunable via keyword parameters
"""
function optimize_velocity_profile(traj::Trajectory;
    m::Int=8,
    n::Int = 0,
    a_max::Float64 = 2.0,
    verbose=false
    )
    # # t = 0.5
    # # n = 3
    # # a = ones(n) # coefficients
    # # Polynomial basis vector
    # tv(t,n) = [t^j for j in 0:n]
    # # a(t) = a[1] + a[2]*t + a[3]*t^2 + ... + a[n]*t^(n-1)
    # #      = a'*tv
    # # Differentiation
    # Dmat(n) = vcat([[i*(i == j-1) for j in 1:n+1]' for i in 1:n+1]...)
    # # d/dt a(t) = a[2] + 2*a[3]*t + ... + (n-1)*a[n]*t^(n-2)
    # #           = (Dmat*a)'*tv
    # # Integration from 0 to t
    # Imat(n,d=1) = vcat([[(1/prod([k for k in j:j+d-1]))*(i == j+1) for j in 1:n+1]' for i in 2:n+2]...);
    # # ∫ a(t) = a[1]*t + (1/2)*a[2]*t^2 + ... + (1/n)*a[n]*t^n
    # #        = (Imat*a)'*t*tv

    N = length(traj.segments)
    stop_idxs = map(seg->norm(get_vel(seg,get_start_time(seg))) == 0, traj.segments) # true if v == 0 along this trajectory
    d = [get_length(seg) for seg in traj.segments]
    cd = cumsum(d)
    t = [[get_start_time(seg) for seg in traj.segments]..., get_end_time(traj)]
    dt = diff(t)
    v0 = 0.0
    vT = 0.0
    s0 = 0.0
    sT = get_length(traj)

    # n = 1 # order of acceleration polynomial: a[i] = a_0 + a_1*t + a_2*t^2 + ... a_n*t^n
    # m = 8 # num accel windows (m per seg, equally spaced in time)
    # each trajectory has a m-phase acceleration profile
    a = [Variable(m) for t in 1:N] # accel cmd coefficients
    v = [Variable(m+1) for t in 1:N]
    s = [Variable(m+1) for t in 1:N]

    constraints = [
        [a[i] <= a_max for i in 1:N]...,
        [a[i] >= -a_max for i in 1:N]...,
        v[1][1] == v0,    # initial conditions
        v[end][end] == vT,  # final conditions
        [v[i] >= 0.0 for i in 1:N]...,
        [v[i] == 0.0 for i in 1:N if stop_idxs[i] == true]...,
        [v[i][end] == v[i+1][1] for i in 1:N-1]...,
        [v[i][j+1] == v[i][j] + a[i][j]*(dt[i]/m) for i in 1:N for j in 1:m]...,# dynamics
        s[1][1] == s0,
        # s[end][end] == sT,
        [s[i][end] == cd[i] for i in 1:N]...,
        [s[i][end] == s[i+1][1] for i in 1:N-1]...,
        [s[i][j+1] == s[i][j] + v[i][j]*dt[i]/m + (1/2)*a[i][j]*(dt[i]/m)^2 for i in 1:N for j in 1:m]...,
    ]

    problem = minimize(sum([sumsquares(a[i]) for i in 1:N]),constraints);
    solve!(problem, ECOSSolver(verbose=verbose))
    # solve!(problem, SCSSolver(verbose=verbose))

    @assert !(problem.status in Set([:Infeasible,:Error,:Unbounded,:Indeterminate])) string("Optimization failed: problem.status == ",problem.status)

    new_traj = Trajectory()
    s0 = 0.0
    for (i,seg) in enumerate(traj.segments)
        t_vec = t[i] .+ (dt[i]/m)*collect(0:m)
        accel = a[i].value[:]
        vel = v[i].value[:]
        pos = s[i].value[:] .- s0
        push!(new_traj, DenseTrajectory(deepcopy(seg),t_vec,accel,vel,pos)) # using deepcopy to prevent the old pointers from lingering when embedding in c
        s0 += pos[end]
    end
    # return new_traj
    t_vec = [0, vcat([t[i] .+ (dt[i]/m)*collect(1:m) for i in 1:N]...)...]
    accel = vcat([a[i].value[:] for i in 1:N]...)
    vel = [vcat([v[i].value[1:end-1] for i in 1:N]...)..., v[end].value[end]]
    pos = [vcat([s[i].value[1:end-1] for i in 1:N]...)..., s[end].value[end]]

    return new_traj, t_vec, accel, vel, pos
end

function optimize_velocity_profile_traj_only(traj::Trajectory;
    m::Int=8,
    n::Int = 0,
    a_max::Float64 = 2.0,
    verbose=false
    )
    new_traj, t_vec, accel, vel, pos = optimize_velocity_profile(traj;
        m=m,n=n,a_max=a_max,verbose=verbose)
    return new_traj
end

"""
    `stitch_trajectories(traj1::Trajectory,traj2::Trajectory;buffer=[0.0,0.0])`

    Combines two trajectories with a `PivotTrajectory` sandwiched between two
    `WaitTrajectory` segments.
"""
function stitch_trajectories(traj1::Trajectory,traj2::Trajectory;buffer=[0.0,0.0])
    @assert norm(get_end_pt(traj1) - get_start_pt(traj2)) < 0.000001 "start and end points do not align"
    @assert buffer[1]+buffer[2] <= get_start_time(traj2) - get_end_time(traj1)
    θ1 = atan(get_heading(traj1,get_end_time(traj1)))
    θ2 = atan(get_heading(traj2,get_start_time(traj2)))
    Δθ = get_angular_offset(θ1,θ2)
    traj = Trajectory()
    for seg in traj1.segments
        push!(traj, seg)
    end
    push!(traj, WaitTrajectory(
        get_end_pt(traj),
        get_heading(traj,get_end_time(traj)),
        TimeInterval(get_end_time(traj),get_end_time(traj)+buffer[1]))
        )
    push!(traj, PivotTrajectory(
        get_end_pt(traj),
        get_heading(traj,get_end_time(traj)),
        Δθ,TimeInterval(get_end_time(traj),get_start_time(traj2)-buffer[2]))
        )
    push!(traj, WaitTrajectory(
        get_end_pt(traj),
        get_heading(traj,get_end_time(traj)),
        TimeInterval(get_end_time(traj),get_start_time(traj2)))
        )
    for seg in traj2.segments
        push!(traj, seg)
    end
    return traj
end

end # module Trajectories


text:
using UnPack, OrderedCollections, ForwardDiff, JLD2

mutable struct TextbookNK{T <: Real}
    β::T
    σ::T
    ψ::T
    η::T
    ϵ::T
    ϕ::T
    ρₐ::T
    σₐ::T
    ρᵢ::T
    σᵢ::T
    ϕ_π::T
    π̃_ss::T
end

function TextbookNK(; β::T = .99, σ::T = 2., ψ::T = 1., η::T = 1., ϵ::T = 4.45, ϕ::T = .7,
                    ρₐ::T = 0.9, σₐ::T = .004, ρᵢ::T = .7, σᵢ::T = .025 / 4.,
                    ϕ_π::T = 1.5, π̃_ss::T = 0.) where {T <: Real}
    return TextbookNK{T}(β, σ, ψ, η, ϵ, ϕ, ρₐ, σₐ, ρᵢ, σᵢ, ϕ_π, π̃_ss)
end

function textbook_nk(m::TextbookNK{T}) where {T <: Real}
    @unpack β, σ, ψ, η, ϵ, ϕ, ρₐ, σₐ, ρᵢ, σᵢ, ϕ_π, π̃_ss = m
    ĩ_ss = π̃_ss - log(β)

    # On notation: x̃ = log(1 + x); x′ = 𝔼ₜ[xₜ₊₁]
    S  = OrderedDict{Symbol, Int}(:a => 1, :ĩ₋₁ => 2, :v₋₁ => 3, :i_sh => 4) # State Variables
    J  = OrderedDict{Symbol, Int}(:c => 1, :π̃ => 2, :n => 3, :w => 4, :mc => 5, :v => 6,
                                  :x₁ => 7, :x₂ => 8, :ĩ => 9) # Jump variables
    E  = OrderedDict{Symbol, Int}(:euler => 1, :mrs => 2, :eq_mc => 3, :output => 4,
                                  :dispersion => 5, :phillips_curve => 6, :eq_x₁ => 7,
                                  :eq_x₂ => 8, :eq_mp => 9) # Equations
    SH = OrderedDict{Symbol, Int}(:εₐ => 1, :εᵢ => 2) # Exogenous shocks

    @unpack a, ĩ₋₁, v₋₁, i_sh = S
    @unpack c, π̃, n, w, mc, v, x₁, x₂, ĩ = J
    @unpack euler, mrs, eq_mc, output, dispersion, phillips_curve, eq_x₁, eq_x₂, eq_mp = E
    @unpack εₐ, εᵢ = SH

    Nz = length(S)
    Ny = length(J)
    Nε = length(SH)

    function μ(F, z, y)
        F_type  = eltype(F)
        F[a]    = ρₐ * z[a]
        F[ĩ₋₁]  = y[ĩ]
        F[v₋₁]  = y[v]
        F[i_sh] = zero(F_type)
    end

    function ξ(F, z, y)
        F_type = eltype(F)
        π̃_star = log(ϵ / (ϵ - 1.)) + y[π̃] + (y[x₁] - y[x₂])
        F[euler] = log(β) + σ * y[c] + y[ĩ]
        F[mrs] = log(ψ) + η * y[n] - (-σ * y[n] + y[w])
        F[eq_mc] = y[w] - (z[a] + y[mc])
        F[output] = y[c] - (z[a] + y[n] - y[v])
        F[dispersion] = y[v] - (ϵ * y[π̃] + log((1. - ϕ) * exp(π̃_star)^(-ϵ) + ϕ * exp(z[v₋₁])))
        F[phillips_curve] = (1. - ϵ) * y[π̃] - log((1. - ϕ) * exp(π̃_star)^(1 - ϵ) + ϕ)
        F[eq_x₁] = log(ϕ) + log(β) - log(exp(y[x₁]) - exp((1. - σ) * y[c] + y[mc]))
        F[eq_x₂] = log(ϕ) + log(β) - log(exp(y[x₂]) - exp((1. - σ) * y[c]))
        F[eq_mp] = y[ĩ] - ((1. - ρᵢ) * ĩ_ss + ρᵢ * z[ĩ₋₁]  + (1 - ρᵢ) * ϕ_π * (y[π̃] - π̃_ss) + z[i_sh])
    end

    # The cache is initialized as zeros so we only need to fill non-zero elements
    Λ = zeros(T, Nz, Ny)

    # The cache is initialized as zeros so we only need to fill non-zero elements
    function Σ(F, z)
        F[a, εₐ]    = σₐ
        F[i_sh, εᵢ] = σᵢ
    end

    function ccgf(F, α, z)
        # F .= .5 * diag(α * α') # slower but this is the underlying math
        sum!(F, α.^2) # faster implementation
        F .*= .5
    end

    Γ₅ = zeros(T, Ny, Nz)

    Γ₆ = zeros(T, Ny, Ny)
    Γ₆[euler, c] = -σ
    Γ₆[euler, π̃] = -one(T)
    Γ₆[eq_x₁, x₁] = one(T)
    Γ₆[eq_x₁, π̃] = ϵ
    Γ₆[eq_x₂, x₂] = one(T)
    Γ₆[eq_x₂, π̃] = (ϵ - 1.)

    Ψ = zeros(T, Ny, Nz)

    # Deterministic steady state as initial guess

    # z
    a0    = 0.
    ĩ₋₁0  = ĩ_ss
    v₋₁0  = 0.
    i_sh0 = 0.
    z     = [a0, ĩ₋₁0, v₋₁0, i_sh0]

    # y
    ĩ0  = ĩ_ss
    π̃0  = π̃_ss
    v0  = 0.
    mc0 = log((ϵ - 1.) / ϵ)
    x₁0 = log(exp(π̃0) * exp(mc0) / (1. - ϕ * β))
    x₂0 = log(1. / (1. - ϕ * β))
    n0  = (1 / (η + σ)) * log(1. / ψ * (exp(v0))^σ * exp(mc0))
    c0  = n0 - v0
    w0  = a0 + mc0
    y   = [c0, π̃0, n0, w0, mc0, v0, x₁0, x₂0, ĩ0]

    return RiskAdjustedLinearization(μ, Λ, Σ, ξ, Γ₅, Γ₆, ccgf, vec(z), vec(y), Ψ, Nε)
end

nk_cₜ(m, zₜ) = exp(m.y[1] + (m.Ψ * (zₜ - m.z))[1])

# Evaluates euler equation in log terms
function nk_logSDFxR(m, zₜ, εₜ₊₁, Cₜ; β::T = .99, σ::T = 2.) where {T <: Real}
    yₜ = m.y + m.Ψ * (zₜ - m.z)
    zₜ₊₁, yₜ₊₁ = simulate(m, εₜ₊₁, zₜ)

    return log(β) - σ * (yₜ₊₁[1] - log(Cₜ)) + yₜ[9] - yₜ₊₁[2]
end

# Calculate Euler equation via quadrature
std_norm_mean = zeros(2)
std_norm_sig  = ones(2)
nk_𝔼_quadrature(f::Function) = gausshermite_expectation(f, std_norm_mean, std_norm_sig, 10)


text:
using Test
using ArgTools
using Downloads
using Downloads.Curl

include("json.jl")

if VERSION < v"1.5"
    contains(haystack, needle) = occursin(needle, haystack)
end

function download_body(url::AbstractString, headers = Union{}[])
    sprint() do io
        Downloads.download(url, io, headers = headers)
    end
end

function download_json(url::AbstractString, headers = Union{}[])
    json.parse(download_body(url, headers))
end

function request_body(multi::Multi, url::AbstractString, headers = Union{}[])
    resp = nothing
    body = sprint() do io
        req = Request(io, url, headers)
        resp = Downloads.request(req, multi)
    end
    return resp, body
end

function request_json(multi::Multi, url::AbstractString, headers = Union{}[])
    resp, body = request_body(multi, url, headers)
    return resp, json.parse(body)
end

function header(hdrs::Dict, hdr::AbstractString)
    hdr = lowercase(hdr)
    for (key, values) in hdrs
        lowercase(key) == hdr || continue
        values isa Vector || error("header value should be a vector")
        length(values) == 1 || error("header value should have length 1")
        return values[1]
    end
    error("header not found")
end

function test_response_string(response::AbstractString, status::Integer)
    m = match(r"^HTTP/\d+(?:\.\d+)?\s+(\d+)\b", response)
    @test m !== nothing
    @test parse(Int, m.captures[1]) == status
end

macro exception(ex)
    quote
        try $(esc(ex))
        catch err
            err
        end
    end
end

# URL escape & unescape

function is_url_safe_byte(byte::UInt8)
    0x2d ≤ byte ≤ 0x2e ||
    0x30 ≤ byte ≤ 0x39 ||
    0x41 ≤ byte ≤ 0x5a ||
    0x61 ≤ byte ≤ 0x7a ||
    byte == 0x5f ||
    byte == 0x7e
end

function url_escape(str::Union{String, SubString{String}})
    sprint(sizehint = ncodeunits(str)) do io
        for byte in codeunits(str)
            if is_url_safe_byte(byte)
                write(io, byte)
            else
                write(io, '%', string(byte, base=16, pad=2))
            end
        end
    end
end
url_escape(str::AbstractString) = url_escape(String(str))

const multi = Multi()
const server = "https://httpbingo.julialang.org"


text:
@testset "test helpers" begin
    @test SparseTimeSeries.neighbors(1:3) |> collect == [(1,2), (2,3)]
    @test SparseTimeSeries.neighbors(['A','B','C']) |> collect == [('A','B'), ('B','C')]
end

@testset "test Event, TaggedEventConstructor" begin
    e = Event(1,2)
    @test SparseTimeSeries.value(e) == 2
    @test SparseTimeSeries.timestamp(e) == 1
    te = SparseTimeSeries.TaggedEvent(:a, 1, 2)
    te_ = SparseTimeSeries.TaggedEvent(:a, e)
    e_ = Event(te_)

    for x in [te,te_, e_]
        @test SparseTimeSeries.value(e) == SparseTimeSeries.value(x)
        @test SparseTimeSeries.timestamp(e) == SparseTimeSeries.timestamp(x)
    end

    @test SparseTimeSeries.tag(e) == nothing
    @test SparseTimeSeries.tag(te) == :a
    @test SparseTimeSeries.tag(te_) == :a
    @test SparseTimeSeries.tag(e_) == nothing
    @test SparseTimeSeries.tag(nothing) == nothing
    @test SparseTimeSeries.timestamp(nothing) == nothing

end

@testset "constructing EventSeries" begin
    values_ = [:a, 1.0, "hello", 2]
    time_ = [1, 2, 3, 4]

    ts = EventSeries(time_, values_)
    @test ts.timestamps === time_
    @test ts.values === values_

    # not equal lenght of input
    @test_throws AssertionError EventSeries([1, 2, 3], values_)

    # not sorted times
    @test_throws AssertionError EventSeries([1, 2, 3, -1], values_)

    # TrustInput avoids assertions
    time_ = [1, 2, -1]
    @test EventSeries(time_, values_, SparseTimeSeries.TrustInput()).timestamps === time_

    # Equal times works
    @test EventSeries(ones(4), values_).timestamps == ones(4)

    v = ['A','A','A','B','C','C']
    t = 1:length(v)
    y = EventSeries(t,v)
    @test y.values == ['A', 'B', 'C', 'C']

    y = EventSeries(t, v, keep_end=false)
    @test y.values == ['A', 'B', 'C']

    y = EventSeries(t, v, drop_repeated=false)
    @test y.values == ['A','A','A','B','C','C']

    y = EventSeries(t, v, drop_repeated=false, keep_end=false)
    @test y.values == ['A','A','A','B','C','C']
end

@testset "push!, append! and getindex, to EventSeries" begin
    values_ = [:a, 1.0, "hello", 2]
    time_ = [1, 2, 3, 4]

    ts = EventSeries(copy(time_), values_)
    @test SparseTimeSeries.timestamptype(ts) == Int
    @test ts[3] == Event(timestamp=3, value="hello")

    # decreasing timestep
    @test_throws AssertionError push!(ts, Event(3, 3))

    # errs when pushing incopatible time type
    let err = nothing
        try
             push!(ts, Event(31.2, 3))
        catch err
        end
        @test err isa Exception
    end

    t, v =  6, :foo
    push!(ts, Event(t,v))
    @test last(ts.timestamps) == t
    @test last(ts.values) == v

    ts2 = EventSeries(collect(6:8), rand(3))
    append!(ts, ts2)
    @test ts.timestamps == [time_; [t];  6:8]

    ts3 = ts[end-2:end]
    @test ts3.timestamps == ts2.timestamps

    # setindex should throw error
    @test_throws ErrorException ts3[3] = :b
end

@testset "fill forward EventSeries" begin
    values_ = [:a, 1.0, "hello", 2]
    time_ = [1, 2, 3, 4]
    ts = EventSeries(copy(time_), values_)

    @test fill_forward_event(ts, 0.9) == nothing
    @test fill_forward_event(ts, 1) == Event(timestamp=1, value=:a)
    @test fill_forward_event(ts, 4) == Event(timestamp=4, value=2)
    @test fill_forward_event(ts, 5.123) == Event(timestamp=4, value=2)
    @test fill_forward_event(ts, 2) == Event(timestamp=2, value=1.0)
    @test fill_forward_event(ts, 2.5) == Event(timestamp=2, value=1.0)

    @test fill_forward_value(ts, 0.9) == nothing
    @test fill_forward_value(ts, 1) == :a
    @test fill_forward_value(ts, 4) == 2
    @test fill_forward_value(ts, 5.123) == 2
    @test fill_forward_value(ts, 2) == 1.0
    @test fill_forward_value(ts, 2.5) == 1.0
end

@testset  "TaggedEventSeries" begin
    l1 = 'A':'C'
    l2 = 'a':'c'
    t = 1:length(l1)
    y1 = EventSeries(t, l1)
    y2 = EventSeries(t .+ 0.5, l2)
    y = SparseTimeSeries.TaggedEventSeries()
    y[:capital] = y1
    y[:small] = y2
    @test value.(SparseTimeSeries.tagged_events(y)) == ['A', 'a', 'B', 'b','C','c']

    @test value.(EventSeries(y)) == [
    (capital='A', small=nothing),
    (capital='A', small='a'),
    (capital='B', small='a'),
    (capital='B', small='b'),
    (capital='C', small='b'),
    (capital='C', small='c'),
    ]

    ff = fill_forward_event(y, 2)
    @test ff.capital == Event(2, 'B')
    @test ff.small == Event(1.5, 'a')

    ff = fill_forward_value(y, 2)
    @test ff.capital ==  'B'
    @test ff.small ==  'a'

    @test issorted(SparseTimeSeries.timestamps(y))
    tidx = collect(SparseTimeSeries.sorted_tag_idx(y))
    @test length(tidx) == length(y)
    @test length(y) == 3+3
end

@testset  "TaggedEventSeries construct by kwargs" begin
    l1 = 'A':'C'
    l2 = 'a':'c'
    t = 1:length(l1)
    y1 = EventSeries(t, l1)
    y2 = EventSeries(t .+ 0.5, l2)
    y = SparseTimeSeries.TaggedEventSeries(capital=y1, small=y2)

    @test issorted(SparseTimeSeries.timestamps(y))
    tidx = collect(SparseTimeSeries.sorted_tag_idx(y))
    @test length(tidx) == length(y)
    @test length(y) == 3+3
end