text:
# Submit coverage to Coveralls.io
export Coveralls
"""
Coverage.Coveralls Module

This module provides functionality to push coverage information to the Coveralls
web service. It exports the `submit` and `submit_local` methods.
"""
module Coveralls
    using Coverage
    using CoverageTools
    using HTTP
    using JSON
    using LibGit2
    using MbedTLS

    export submit, submit_token, submit_local

    #=
    JSON structure for Coveralls
    Accessed 2015/07/24:
    https://coveralls.zendesk.com/hc/en-us/articles/201774865-API-Introduction
    {
      "service_job_id": "1234567890",
      "service_name": "travis-ci",
      "source_files": [
        {
          "name": "example.rb",
          "source": "def four\n  4\nend",
          "coverage": [null, 1, null]
        },
        {
          "name": "lib/two.rb",
          "source": "def seven\n  eight\n  nine\nend",
          "coverage": [null, 1, 0, null]
        }
      ]
    }
    =#

    # to_json
    # Convert a FileCoverage instance to its Coveralls JSON representation
    to_json(fc::FileCoverage) = Dict("name"          => fc.filename,
                                     "source_digest" => digest(MD_MD5, fc.source, "secret"),
                                     "coverage"      => fc.coverage)

    # Format the body argument to HTTP.post
    makebody(data::Dict) =
        Dict("json_file" => HTTP.Multipart("json_file", IOBuffer(JSON.json(data)),
                                           "application/json"))

    """
        submit(fcs::Vector{FileCoverage}; kwargs...)

    Take a vector of file coverage results (produced by `process_folder`),
    and submits them to Coveralls. Assumes that this code is being run
    on TravisCI, AppVeyor or Jenkins. If running locally, use `submit_local`.
    """
    function submit(fcs::Vector{FileCoverage}; kwargs...)
        if haskey(kwargs, :verbose)
            Base.depwarn("The verbose keyword argument is deprecated, set the environment variable " *
                         "JULIA_DEBUG=Coverage for verbose output", :submit_generic)
            verbose = kwargs[:verbose]
        else
            verbose = false
        end
        data = prepare_request(fcs, false)
        post_request(data, verbose)
    end

    function prepare_request(fcs::Vector{FileCoverage}, local_env, git_info=query_git_info)
        data = Dict{String,Any}("source_files" => map(to_json, fcs))

        if local_env
            # Attempt to parse git info via git_info, unless the user explicitly disables it by setting git_info to nothing
            data["git"] = parse_git_info(git_info)
        elseif lowercase(get(ENV, "APPVEYOR", "false")) == "true"
            data["service_job_id"] = ENV["APPVEYOR_JOB_ID"]
            data["service_name"] = "appveyor"
            appveyor_pr = get(ENV, "APPVEYOR_PULL_REQUEST_NUMBER", "")
            isempty(appveyor_pr) || (data["service_pull_request"] = appveyor_pr)
        elseif lowercase(get(ENV, "TRAVIS", "false")) == "true"
            data["service_job_id"] = ENV["TRAVIS_JOB_ID"]
            data["service_name"] = "travis-ci"
            travis_pr = get(ENV, "TRAVIS_PULL_REQUEST", "")
            isempty(travis_pr) || (data["service_pull_request"] = travis_pr)
        elseif lowercase(get(ENV, "JENKINS", "false")) == "true"
            data["service_job_id"] = ENV["BUILD_ID"]
            data["service_name"] = "jenkins-ci"
            data["git"] = parse_git_info(git_info)

            # get the name of the branch if not a pull request
            if get(ENV, "CI_PULL_REQUEST", "false") == "false"
                data["git"]["branch"] = split(ENV["GIT_BRANCH"], "/")[2]
            end
        elseif haskey(ENV, "GITHUB_ACTION")
            data["git"] = parse_git_info(git_info)
        else
            error("No compatible CI platform detected")
        end

        data = add_repo_token(data, local_env)
        if get(ENV, "COVERALLS_PARALLEL", "false") == "true"
            data["parallel"] = "true"
        end
        return data
    end

    function parse_git_info(git_info::Function)
        result = nothing
        try
            result = git_info()
        catch ex
            @warn "Parse of git information failed" exception=e, catch_backtrace()
        end
        return result
    end

    parse_git_info(git_info::Dict) = git_info

    # query_git_info
    # Pulls information about the repository that isn't available if we
    # are running somewhere other than TravisCI
    function query_git_info(dir=pwd())
        repo            = LibGit2.GitRepoExt(dir)
        head            = LibGit2.head(repo)
        head_cmt        = LibGit2.peel(head)
        head_oid        = LibGit2.GitHash(head_cmt)
        commit_sha      = string(head_oid)
        author_name     = string(LibGit2.author(head_cmt).name)
        author_email    = string(LibGit2.author(head_cmt).email)
        committer_name  = string(LibGit2.committer(head_cmt).name)
        committer_email = string(LibGit2.committer(head_cmt).email)
        message         = LibGit2.message(head_cmt)
        remote_name     = "origin"
        branch          = LibGit2.shortname(head)

        # determine remote url, but only if repo is not in detached state
        remote = ""
        if branch != "HEAD"
            LibGit2.with(LibGit2.get(LibGit2.GitRemote, repo, remote_name)) do rmt
                remote = LibGit2.url(rmt)
            end
        end
        LibGit2.close(repo)

        return Dict(
            "branch"    => branch,
            "remotes"   => [
                Dict(
                    "name"  => remote_name,
                    "url"   => remote
                )
            ],
            "head" => Dict(
                "id" => commit_sha,
                "author_name"       => author_name,
                "author_email"      => author_email,
                "committer_name"    => committer_name,
                "committer_email"   => committer_email,
                "message"           => message
            )
        )
    end

    """
        submit_local(fcs::Vector{FileCoverage}, git_info=query_git_info; kwargs...)

    Take a `Vector` of file coverage results (produced by `process_folder`),
    and submits them to Coveralls. For submissions not from CI.

    git_info can be either a `Dict` or a function that returns a `Dict`.
    """
    function submit_local(fcs::Vector{FileCoverage}, git_info=query_git_info; kwargs...)
        if haskey(kwargs, :verbose)
            Base.depwarn("The verbose keyword argument is deprecated, set the environment variable " *
                         "JULIA_DEBUG=Coverage for verbose output", :submit_generic)
            verbose = kwargs[:verbose]
        else
            verbose = false
        end
        data = prepare_request(fcs, true, git_info)
        post_request(data, verbose)
    end

    # posts the actual request given the data
    function post_request(data, verbose)
        verbose && @info "Submitting data to Coveralls..."
        req = HTTP.post("https://coveralls.io/api/v1/jobs", HTTP.Form(makebody(data)))
        verbose && @debug "Result of submission:\n" * String(req.body)
        nothing
    end

    # adds the repo token to the data
    function add_repo_token(data, local_submission)
        repo_token =
                get(ENV, "COVERALLS_TOKEN") do
                    get(ENV, "REPO_TOKEN") do #backward compatibility
                        # error unless we are on Travis
                        if local_submission || (data["service_name"] != "travis-ci")
                            error("Coveralls submission requires a COVERALLS_TOKEN environment variable")
                        end
                    end
                end
        if repo_token !== nothing
            data["repo_token"] = repo_token
        end
        return data
    end
    @deprecate submit_token submit_local

end  # module Coveralls


text:
export ReportConfig, ReportStatus, ReportProblem
#TODO add ability to save report every X iterations
#TODO read report file to warm-start solver

const tab1 = "\t"
const tab2 = "\t\t"
const tab3 = "\t\t\t"

struct ReportSection
    title::String
    entries::Vector{Union{Pair{String,Any},Nothing}}
end

"""
    ReportConfig(p::DSProblem)

Print the configuration options currently used by `p`.
"""
ReportConfig(p::DSProblem) = print(report_config(p))

"""
    ReportStatus(p::DSProblem)

Print the current non-problem-specific status information of `p`.
"""
ReportStatus(p::DSProblem) = print(report_status(p))

"""
    ReportProblem(p::DSProblem)

Print the current problem specific status information of `p`.
"""
ReportProblem(p::DSProblem) = print(report_problem(p))

"""
    Base.print(p::DSProblem)

Print the output of [`ReportConfig`](@ref), [`ReportStatus`](@ref), and [`ReportProblem`](@ref) in a list.
"""
Base.print(p::DSProblem) = format_problem(p)
Base.println(p::DSProblem) = print(p)
function format_problem(p::DSProblem)
    print(report_config(p))
    println()
    print(report_status(p))
    println()
    print(report_problem(p))
end

Base.println(s::ReportSection) = print(s)
Base.print(s::ReportSection) = print(format(s))
function format(s::ReportSection)
    str = ""
    spacenumber = max(length.([e.first for e in s.entries if e != nothing])...) + 4
    maxwidth = max(length.([string(e.second) for e in s.entries if e != nothing])...) + spacenumber

    str *= join(["=" for _ in 1:maxwidth]) * "\n"
    str *= s.title * "\n"
    str *= join(["-" for _ in 1:maxwidth]) * "\n"

    for e in s.entries
        if e == nothing
            str *= "\n"
        else
            str *= e.first
            str *= join([" " for _ in 1:spacenumber - length(e.first)])
            str *= string(e.second)
            str *= "\n"
        end
    end

    return str
end

"""
    report_config(p::DSProblem)::ReportSection

Format the contents of `p.config` to a DS.ReportSection. This information details
the configuration used by `p`.
"""
function report_config(p::DSProblem)::ReportSection
    entries = []
    push!(entries, "Search" => typeof(p.config.search))
    push!(entries, "Poll" => typeof(p.config.poll))
    push!(entries, "Mesh" => typeof(p.config.mesh))
    push!(entries, "Mesh Scale" => p.config.meshscale)
    push!(entries, "Opportunistic" => p.config.opportunistic)
    push!(entries, "Number of processes" => p.config.num_procs)
    push!(entries, "Max simultanious evaluations" => p.config.max_simultaneous_evaluations)
    return ReportSection("Config", entries)
end

"""
    report_status(p::DSProblem)::ReportSection

Format the contents of `p.status` to a DS.ReportSection. This information details
the current state of the solver in regards to non-problem specific information.
"""
function report_status(p::DSProblem)::ReportSection
    entries = []
    push!(entries, "Function Evaluations" => p.status.function_evaluations)
    push!(entries, "Iterations" => p.status.iteration)
    push!(entries, "Cache hits" => p.status.cache_hits)
    push!(entries, "Optimization Status" => p.status.optimization_status)
    push!(entries, "Optimization Status String" => p.status.optimization_status_string)

    push!(entries, nothing)

    push!(entries, "Runtime" => p.status.runtime_total)
    push!(entries, "Search Time" => p.status.search_time_total)
    push!(entries, "Poll Time" => p.status.poll_time_total)
    push!(entries, "Blackbox Evaluation Time" => p.status.blackbox_time_total)
    return ReportSection("Status", entries)
end

"""
    report_problem(p::DSProblem)::ReportSection

Format the contents of `p` to a DS.ReportSection. This information details
the current problem-specific state of the solver.
"""
function report_problem(p::DSProblem)::ReportSection
    entries = []
    push!(entries, "Variables" => p.N)
    push!(entries, "Initial Point" => p.user_initial_point)
    push!(entries, "Sense" => p.sense)

    push!(entries, nothing)

    push!(entries, "Feasible Solution" => p.x)
    push!(entries, "Feasible Cost" => p.x_cost)
    push!(entries, "Infeasible Solution" => p.i)
    push!(entries, "Infeasible Cost" => p.i_cost)
    return ReportSection("Optimization Problem", entries)
end

"""
    export_points(p::DSProblem)

Gives the trace of considered points during optimisation. Note that this is unavailable
if tracking of points has been disabled
"""
function export_points(p::DSProblem)
    for x in p.cache.order
        println("$(x)\t$(p.cache.costs[x])")
    end
end

ReportConstraints(p::DSProblem) = ReportConstraints(p.constraints)
function ReportConstraints(c::Constraints{T}) where T
    constraint_report=
"""
---------------------------------
Constraints
---------------------------------
Constraint Collections:     $(c.count)
Populated Collections:      $(length([col for col in c.collections if col.count > 0]))
Total Constraints:          $(sum([col.count for col in c.collections]))
"""
    print(constraint_report)
    for (index, collection) in enumerate(c.collections)
        println("\n$(CollectionIndex(index)) : $(typeof(collection))")
        ReportConstraintCollection(collection)
    end
end

function ReportConstraintCollection(c::ConstraintCollection{T, C}) where {T,C}
    collection_report =
"""
Constraints:       $(c.count)
Ignored:           $(c.ignore)
h_max:             $(c.h_max)
h_max update
function handle:   $(c.h_max_update)
result aggregate
function handle:   $(c.result_aggregate)
"""
    print(collection_report)
end

ReportFinal(p::DSProblem) = print(report_final(p))

function report_final(p::DSProblem)::ReportSection
    entries = []
    push!(entries, "Feasible Solution" => p.x)
    push!(entries, "Feasible Cost" => p.x_cost)
    push!(entries, "Infeasible Solution" => p.i)
    push!(entries, "Infeasible Cost" => p.i_cost)

    push!(entries, nothing)

    push!(entries, "Iterations" => p.status.iteration)
    push!(entries, "Function Evaluations" => p.status.function_evaluations)
    push!(entries, "Cache hits" => p.status.cache_hits)
    push!(entries, "Optimization Status" => p.status.optimization_status_string)

    push!(entries, nothing)

    push!(entries, "Runtime" => p.status.runtime_total)
    push!(entries, "Search Time" => p.status.search_time_total)
    push!(entries, "Poll Time" => p.status.poll_time_total)
    push!(entries, "Blackbox Evaluation Time" => p.status.blackbox_time_total)

    return ReportSection("MADS Run Summary", entries)
end

function OutputIterationDetails(p::DSProblem)
    str = ""

    title = "Iteration #$(p.status.iteration):"
    border = join(["=" for _ in 1:length(title)])

    str *= border * "\n"
    str *= title * "\n"
    str *= border * "\n"

    str *= "\n"

    str *= "Status:" * "\n"

    str *= tab1 * "Number of blackbox evaluations: $(p.status.function_evaluations)" * "\n"
    str *= tab1 * "Number of cache hits: $(p.status.cache_hits)" * "\n"

    str *= "\n"

    str *= PointDetails(p)

    str *= MeshDetails(p)

    print(str)
end

MeshDetails(p::DSProblem) = MeshDetails(p.config.mesh)
function MeshDetails(m::Mesh)::String
    str = ""
    str *= tab1 * "Mesh:" * "\n"
    str *= tab2 * "Mesh size: $(m.δ)" * "\n"
    str *= tab2 * "Poll size: $(m.Δ)" * "\n"
    str *= tab2 * "Mesh index: $(m.l)" * "\n"
    str *= "\n"
    return str
end

function PointDetails(p::DSProblem)::String
    str = ""

    str *= tab1 * "Feasible point: " * "\n"
    str *= tab2 * "x = $(p.x)" * "\n"
    str *= tab2 * "f(x) = $(p.x_cost)" * "\n"

    str *= "\n"

    str *= tab1 * "Infeasible point: " * "\n"
    str *= tab2 * "i = $(p.i)" * "\n"
    str *= tab2 * "f(i) = $(p.i_cost)" * "\n"

    str *= "\n"

    return str
end

function InitialPointEvaluationOutput(p::DSProblem, feasibility::ConstraintOutcome)
    str = ""

    title = "Evaluating initial point:"
    border = join(["=" for _ in 1:length(title)])

    str *= border * "\n"
    str *= title * "\n"
    str *= border * "\n"

    str *= "\n"

    if feasibility == Feasible
        str *= tab1 * "x₀ = $(p.x)" * "\n"
        str *= tab1 * "f(x₀) = $(p.x_cost)" * "\n"
        str *= tab1 * "Feasibility: Feasible" * "\n"
    else
        str *= tab1 * "x₀ = $(p.i)" * "\n"
        str *= tab1 * "f(x₀) = $(p.i_cost)" * "\n"
        str *= tab1 * "Feasibility: Weak Infeasible" * "\n"
    end

    str *= "\n"

    print(str)
end

function OutputSearchStep(p::DSProblem{T}, points::Vector{Vector{T}}) where T
    str = ""

    if p.config.search isa NullSearch
        str *= tab1 * "Skipping Search step." * "\n"
    else
        str *= tab1 * "Generated search points:" * "\n"
        for i=1:length(points)
            str *= tab2 * "Search point $i: $(points[i])" * "\n"
        end
    end

    str *= "\n"

    print(str)
end

function OutputPollStep(points::Vector{Vector{T}}, directions::Matrix{T}) where T
    str = ""

    str *= tab1 * "Generated directions:" * "\n"
    for i=1:size(directions,2)
        str *= tab2 * "Direction $i: $(directions[:,i])" * "\n"
    end

    str *= "\n"

    str *= tab1 * "Generated poll points:\n" * "\n"
    for i=1:length(points)
        str *= tab2 * "Poll point $i: $(points[i])" * "\n"
    end

    str *= "\n"

    print(str)
end

function OutputPointEvaluation(i::Int, point::Vector{T}, cost::T, h::T, is_from_cache::Bool, threadId::Union{Int, Nothing}=nothing) where T
    str = ""

    if threadId !== nothing
        str *= tab2 * "Evaluating point $i with Thread $threadId:" * "\n"
    else
        str *= tab2 * "Evaluating point $i:" * "\n"
    end

    str *= tab3 * "x = $point" * "\n"
    str *= tab3 * "f(x) = $cost" * "\n"
    str *= tab3 * "h(x) = $h" * "\n"
    if is_from_cache
        str *= tab3 * "Point was found in cache." * "\n"
    end
    str *= "\n"

    print(str)
end

ext: jl


text:
################################################################################
##############################    Tuple Stuff    ###############################
################################################################################
for op in [:typemin,:typemax]
    @eval Base.$op(::Type{Tuple{A,B,C,D,E,F}}) where {A,B,C,D,E,F}    = map(i->$op(i),(A,B,C,D,E,F))
    @eval Base.$op(::Type{Tuple{A,B,C,D,E}}) where {A,B,C,D,E}        = map(i->$op(i),(A,B,C,D,E))
    @eval Base.$op(::Type{Tuple{A,B,C,D}}) where {A,B,C,D}            = map(i->$op(i),(A,B,C,D))
    @eval Base.$op(::Type{Tuple{A,B,C}}) where {A,B,C}                = map(i->$op(i),(A,B,C))
    @eval Base.$op(::Type{Tuple{A,B}}) where {A,B}                    = map(i->$op(i),(A,B))
    @eval Base.$op(::Type{Tuple{A}}) where {A}                        = map(i->$op(i),(A,))
end
Base.Tuple{A,B,C,D,E,F}() where {A,B,C,D,E,F}    = map(i->i(0),(A,B,C,D,E,F))
Base.Tuple{A,B,C,D,E}() where {A,B,C,D,E}        = map(i->i(0),(A,B,C,D,E))
Base.Tuple{A,B,C,D}() where {A,B,C,D}            = map(i->i(0),(A,B,C,D))
Base.Tuple{A,B,C}() where {A,B,C}                = map(i->i(0),(A,B,C))
Base.Tuple{A,B}() where {A,B}                    = map(i->i(0),(A,B))
Base.Tuple{A}() where {A}                        = map(i->i(0),(A))

Base.typemin(::Type{NTuple{N,R}}) where {N,R} = NTuple{N,R}(map(i->typemin(R),1:N))
Base.typemax(::Type{NTuple{N,R}}) where {N,R} = NTuple{N,R}(map(i->typemax(R),1:N))
Base.NTuple{N,R}() where {N,R} = NTuple{N,R}(map(i->R(0), 1:N))
Base.NTuple{N,R}(val::R) where {N,R} = NTuple{N,R}(map(i->val, 1:N))
Base.typemin(c::Tuple) = typemin(typeof(c))
Base.typemax(c::Tuple) = typemax(typeof(c))

for op = (:(>), :(<), :(>=), :(<=))
    eval(quote
        Base.$op(a::T,b::R) where {N,T<:Tuple,R<:Real} = $op(a[1],b)
        Base.$op(b::R,a::T) where {N,T<:Tuple,R<:Real} = $op(b,a[1])
    end)
end

################################################################################
############################### Cost Model Stuff ###############################
################################################################################
export
    get_initial_cost,
    get_infeasible_cost,
    compute_path_cost,
    # get_transition_cost,
    accumulate_cost,
    add_heuristic_cost,
    aggregate_costs,

    AggregationFunction,
    MaxCost,
    SumCost,

    FullCostModel,
    get_aggregation_function,
    get_cost_model,
    cost_type,

    MetaCostModel,
    MetaCost,
    CompositeCostModel,
    construct_composite_cost_model,

    LowLevelCostModel,

    TravelTime,
    TravelDistance,
    FinalTime,
    NullCost,
    MakeSpan,
    SumOfTravelDistance,
    SumOfTravelTime

"""
    get_initial_cost(model)

Part of cost model interface. Defaults to zero.
"""
get_initial_cost(model)     = cost_type(model)(0)
# get_initial_cost(model::AbstractCostModel)     = cost_type(model)(0)

"""
    get_infeasible_cost(model)

Part of cost model interface. Defaults to zero.
"""
get_infeasible_cost(model)  = typemax(cost_type(model))
# get_infeasible_cost(model::AbstractCostModel)  = typemax(cost_type(model))

"""
    compute_path_cost(model,env,path,i)

Compute the cost of a path from scratch.
"""
compute_path_cost(model,env,path,i) = cost_type(model)(0)

# """
#     get_transition_cost(model,env,s,a,sp)
#     get_transition_cost(env,s,a,sp)
#     get_transition_cost(env,s,a)
#
# Part of cost model interface. Defines the cost of transitioning from state `s`
# to state `sp` via action `a` in environment `env` under cost model `model`.
# """
# function get_transition_cost end

"""
    accumulate_cost(model,current_cost,transition_cost)

Defines the way that a `transition_cost` updates the `current_cost` of a `Path`.
"""
function accumulate_cost end

"""
    add_heuristic_cost(cost_model,heuristic_model,cost,h_cost)

Defines the output heuristic cost that results from combining the current path
cost `cost` with a heuristic "cost-to-go" `h_cost`. Gener
"""
function add_heuristic_cost end
add_heuristic_cost(::C, cost, h_cost) where {C<:AbstractCostModel} = cost + h_cost
add_heuristic_cost(m::C, ::E, cost, h_cost) where {C<:AbstractCostModel,E<:AbstractLowLevelEnv} = add_heuristic_cost(m,cost,h_cost)
add_heuristic_cost(m::C, ::H, cost, h_cost) where {C<:AbstractCostModel,H<:AbstractCostModel}   = add_heuristic_cost(m,cost,h_cost)
add_heuristic_cost(m::C, h::H, ::E, cost, h_cost) where {C<:AbstractCostModel,H<:AbstractCostModel,E<:AbstractLowLevelEnv} = add_heuristic_cost(m,h,cost,h_cost)
add_heuristic_cost(env::E, cost, h_cost) where {E<:AbstractLowLevelEnv} = add_heuristic_cost(get_cost_model(env),get_heuristic_model(env),env,cost,h_cost)

"""
    compute_heuristic_cost(env,cost,sp)

Defaults to `add_heuristic_cost(env,cost,get_heuristic_cost(env,sp))`
Can be overridden so that state info can inform the heuristic cost directly.
"""
compute_heuristic_cost(m::C,h::H,env::E,cost,sp) where {C<:AbstractCostModel,H<:AbstractCostModel,E<:AbstractLowLevelEnv} = add_heuristic_cost(m,h,env,cost,get_heuristic_cost(h,env,sp))
compute_heuristic_cost(m::C,env::E,cost,sp) where {C<:AbstractCostModel,E<:AbstractLowLevelEnv} = add_heuristic_cost(m,env,cost,get_heuristic_cost(env,sp))
compute_heuristic_cost(env::E,args...) where {E<:AbstractLowLevelEnv} = compute_heuristic_cost(get_cost_model(env),get_heuristic_model(env),env,args...)

get_initial_cost(env::E) where {E<:AbstractLowLevelEnv}     = get_initial_cost(get_cost_model(env))
get_infeasible_cost(env::E) where {E<:AbstractLowLevelEnv}  = get_infeasible_cost(get_cost_model(env))
accumulate_cost(env::E, cost, transition_cost) where {E<:AbstractLowLevelEnv} = accumulate_cost(get_cost_model(env), cost, transition_cost)
# add_heuristic_cost(env::E, cost, h_cost) where {E<:AbstractLowLevelEnv} = add_heuristic_cost(env,get_cost_model(env),cost,h_cost)
get_transition_cost(env::E,s,a) where {E<:AbstractLowLevelEnv} = get_transition_cost(env,s,a,get_next_state(env,s,a))
get_transition_cost(env::E,s,a,sp) where {E<:AbstractLowLevelEnv} = get_transition_cost(get_cost_model(env),env,s,a,sp)

"""
    aggregate_costs(model, costs::Vector{T}) where {T}

Defines how costs from multiple distinct paths are combined into a single cost
for the whole solution. For example, the aggregation function for a
`SumOfTravelTime` objective is the `sum` of individual cost.
"""
function aggregate_costs end

"""
    a special version of aggregate_costs for the meta_env
"""
aggregate_costs_meta(m::AbstractCostModel,args...) = aggregate_costs(m,args...)

abstract type AggregationFunction end
struct MaxCost <: AggregationFunction end
(f::MaxCost)(costs...) = maximum(costs...)
struct SumCost <: AggregationFunction end
(f::SumCost)(costs...) = sum(costs...)

"""
    FullCostModel{F,T,M<:AbstractCostModel{T}} <: AbstractCostModel{T}

The `FullCostModel` defines all the behavior required for running CBS-based
algorithms.

Elements:
- f::F must be callable, and defines how a vector of path costs (e.g., the
    paths of a solution) should be combined into a single cost that reflects
    the cost of the entire path group together
- model::M<:AbstractCostModel{T} defines how the cost is computed during low
level search (when individual paths are being compared against each other).
"""
struct FullCostModel{F,T,M<:AbstractCostModel{T}} <: AbstractCostModel{T}
    f::F        # the aggregation function
    model::M    # the low level cost model
end
get_aggregation_function(m::FullCostModel)  = m.f
aggregate_costs(m::FullCostModel, costs)    = m.f(costs)
get_cost_model(m::FullCostModel)            = m.model
for op in [:accumulate_cost,:get_initial_cost,:compute_path_cost,
    :get_infeasible_cost,:add_heuristic_cost,:get_transition_cost]
    @eval $op(model::FullCostModel,args...) = $op(model.model,args...)
end
for T in [:FullCostModel]
    @eval begin
        add_heuristic_cost(m::$T, h::H, env::E, args...) where {H<:AbstractCostModel,E<:AbstractLowLevelEnv} = add_heuristic_cost(m.model, h, env, args...)
    end
end

"""
    CompositeCostModel{T}

Combines multiple cost models in a specific order (i.e., to use for cascaded
tie-breaking).
"""
struct CompositeCostModel{M<:Tuple,T<:Tuple} <: AbstractCostModel{T}
    cost_models::M
end
function construct_composite_cost_model(args...)
    models = Tuple(args)
    for m in models
        @assert typeof(m) <: AbstractCostModel
    end
    cost_types = map(m->cost_type(m),models)
    CompositeCostModel{typeof(models),Tuple{cost_types...}}(models)
end
# function get_transition_cost(model::C,env,s,a,sp) where {C<:CompositeCostModel}
#     cost_type(model)(map(m->get_transition_cost(m,env,s,a,sp), model.cost_models))
# end
function accumulate_cost(model::C, cost::T, transition_cost::T) where {T,M,C<:CompositeCostModel{M,T}}
    new_cost = map(x->accumulate_cost(x[1],x[2],x[3]),
    zip(model.cost_models, cost, transition_cost))
    T(new_cost)
end
function aggregate_costs(model::C, costs::Vector{T}) where {T,M,C<:CompositeCostModel{M,T}}
    aggregated_costs = map(
        i->aggregate_costs(model.cost_models[i], map(c->c[i], costs)), 1:length(model.cost_models))
    T(aggregated_costs)
end
function aggregate_costs_meta(model::C, costs::Vector{T}) where {T,M,C<:CompositeCostModel{M,T}}
    aggregated_costs = map(
        i->aggregate_costs_meta(model.cost_models[i], map(c->c[i], costs)), 1:length(model.cost_models))
    T(aggregated_costs)
end
for op in [:get_initial_cost,:get_infeasible_cost,:get_transition_cost,
    :compute_path_cost]
    @eval $op(model::CompositeCostModel,args...) = cost_type(model)(map(m->$op(m,args...),model.cost_models))
end
function add_heuristic_cost(model::C, cost::T, h_cost) where {T,M,C<:CompositeCostModel{M,T}}
    T(map(
        i->add_heuristic_cost(model.cost_models[i], cost[i], h_cost[i]),
        1:length(cost)
        ))
end
function compute_heuristic_cost(m::C,h::H,env::E,cost,s) where {C<:CompositeCostModel,H<:AbstractCostModel,E<:AbstractLowLevelEnv}
    # T([compute_heuristic_cost(env,m,c,s) for (m,c) in zip(model.cost_models,cost)])
    cost_type(m)(map(
        i->compute_heuristic_cost(
            m.cost_models[i],
            h.cost_models[i],
            env,
            cost[i],
            s),
        1:length(cost)
        ))
end

"""
    MetaCost

`MetaCost` maintaining separate costs for individual agents that have been
combined into a MetaAgent.
- independent_costs::Vector{T} a vector of costs, 1 per agent
- total_cost::T the total cost, which reflects the combined cost (its
    interpretation depends on the `MetaCostModel` used to define cost-
    upating behavior)
"""
struct MetaCost{T}
    independent_costs::Vector{T}
    total_cost::T
end
Base.isless(m1::MetaCost,m2::MetaCost) = m1.total_cost < m2.total_cost

"""
    MetaCostModel

Defines the cost-updating behavior of `MetaCost` for MetaAgent applications.
"""
struct MetaCostModel{T,M<:AbstractCostModel{T}} <: AbstractCostModel{MetaCost{T}}
    model::M
    num_agents::Int
end
aggregate_costs(m::C, costs::Vector{T}) where {T,C<:MetaCostModel} = aggregate_costs(m.model, costs)
# function add_heuristic_cost(m::C, cost, h_cost) where {C<:MetaCostModel}
#     costs = map(i->add_heuristic_cost(
#         m.model,
#         cost.independent_costs[i],
#         h_cost[i]),1:m.num_agents)
#     MetaCost(costs, aggregate_costs_meta(m.model, costs))
# end
function accumulate_cost(model::M, cost::MetaCost{T}, transition_cost::Vector{T}) where {T,M<:MetaCostModel}
    new_costs = Vector{T}()
    for (i,(c1,c2)) in enumerate(zip(
        cost.independent_costs,
        transition_cost))
        push!(new_costs, accumulate_cost(model.model, c1, c2))
    end
    total_cost = aggregate_costs_meta(model.model,new_costs)
    new_cost = MetaCost{T}(new_costs,total_cost)
    return new_cost
end
function accumulate_cost(model::M, cost::MetaCost{T}, transition_cost::MetaCost{T}) where {T,M<:MetaCostModel}
    accumulate_cost(model, cost, transition_cost.independent_costs)
end
function get_initial_cost(model::C) where {C<:MetaCostModel}
    costs = map(a->get_initial_cost(model.model),1:model.num_agents)
    MetaCost(costs,aggregate_costs(model,costs))
end
function get_infeasible_cost(model::C) where {C<:MetaCostModel}
    costs = map(a->get_infeasible_cost(model.model),1:model.num_agents)
    MetaCost(costs,aggregate_costs(model,costs))
end

################################################################################
############################## Atomic Cost Models ##############################
################################################################################

"""
    LowLevelCostModel{C}

The low level cost model defines the objective to be optimized by the
solver at the low level. An optimal low level solver will return a path if a
feasible path exists) of minimal cost under the objective specified by the
associated LowLevelCostModel.
The parameter `C` defines the `cost_type` of the objective. The following
functions must be implemented for a `LowLevelCostModel` to be used:
* `get_initial_cost(model::LowLevelCostModel,env)` - returns
the initial_cost for a path
* `get_transition_cost(model::LowLevelCostModel{C},path::Path,s::S,a::A,
    sp::S) where {S,A,C}` - defines the cost associated with taking action
    `a` from state `s` to arrive in state `sp` according to the objective
    defined by `model` given that `s` is the "tip" of `path`.
* `accumulate_cost(model::LowLevelCostModel{C}, current_cost::C,
    transition_cost::C)` - defines how cost accumulates as new `PathNode`s
    are added to a Path.
"""
abstract type LowLevelCostModel{T} <: AbstractCostModel{T} end

export
    TransformCostModel

"""
    TransformCostModel{T,M<:LowLevelCostModel{T}} <: LowLevelCostModel{T}

Applies a transformation to an underlying cost model.
    e.g., `TransformCostModel(c->2*c, TravelTime())`
"""
struct TransformCostModel{T,M<:LowLevelCostModel{T}} <: LowLevelCostModel{T}
    f::Function
    model::M
end
# get_transition_cost(m::TransformCostModel,args...) = m.f(get_transition_cost(m.model,args...))
# accumulate_cost(m::TransformCostModel, args...)     = accumulate_cost(m.model,args...)
# get_initial_cost(m::TransformCostModel,args...)     = get_initial_cost(m.model,args...)
# get_infeasible_cost(m::TransformCostModel)          = get_infeasible_cost(m.model)
# add_heuristic_cost(m::TransformCostModel,args...)   = add_heuristic_cost(m.model,args...)
for op in [:accumulate_cost,:get_initial_cost,
    :get_infeasible_cost,:add_heuristic_cost,:get_transition_cost]
    @eval $op(model::TransformCostModel,args...) = model.f($op(model.model,args...))
end
# function compute_heuristic_cost(m::TransformCostModel,env::E,args...) where {E<:AbstractLowLevelEnv}
#     m.f(compute_heuristic_cost(m.model,env,args...))
# end
for op in [:add_heuristic_cost,:compute_heuristic_cost]
    @eval begin
        function $op(m::TransformCostModel, h::H, env::E, args...) where {H<:AbstractCostModel,E<:AbstractLowLevelEnv}
            m.f($op(m.model, h, env, args...))
        end
        function $op(m::TransformCostModel, env::E, args...) where {E<:AbstractLowLevelEnv}
            m.f($op(m.model, env, args...))
        end
    end
end
# add_heuristic_cost(m::TransformCostModel, h::H, env::E, args...) where {H<:AbstractCostModel,E<:AbstractLowLevelEnv} = m.f(add_heuristic_cost(m.model, h, env, args...))
# compute_heuristic_cost(m::TransformCostModel, h::H, env::E, args...) where {H<:AbstractCostModel,E<:AbstractLowLevelEnv} = m.f(add_heuristic_cost(m.model, h, env, args...))

"""
    TravelTime <: LowLevelCostModel{Float64}

Cost model that assigns cost equal to the duration of a path.
"""
struct TravelTime       <: LowLevelCostModel{Float64} end
accumulate_cost(model::TravelTime,      cost,transition_cost) = cost+transition_cost

"""
    TravelDistance <: LowLevelCostModel{Float64}

Cost model that assigns cost equal to the length (distance) of a path.
"""
struct TravelDistance   <: LowLevelCostModel{Float64} end
accumulate_cost(model::TravelDistance,  cost,transition_cost) = cost+transition_cost

struct FinalTime        <: LowLevelCostModel{Float64} end
accumulate_cost(model::FinalTime,       cost,transition_cost) = cost+transition_cost

"""
    NullCost <: LowLevelCostModel{Float64}

Cost equal to `0.0`.
"""
struct NullCost         <: LowLevelCostModel{Float64} end
get_transition_cost(model::F,env,s,a,sp) where {F<:NullCost} = 0.0
accumulate_cost(model::NullCost,        cost,transition_cost) = cost

export
    DeadlineCost,
    set_deadline!,
    FullDeadlineCost,
    # WeightedDeadlinesCost,
    MultiDeadlineCost,
    SumOfMakeSpans

abstract type AbstractDeadlineCost <: LowLevelCostModel{Float64} end
"""
    DeadlineCost

Identical to `TravelTime`, except for the behavior of
`add_heuristic_cost`.

add_heuristic_cost: `c = max(0.0, t + Δt - deadline)`
"""
mutable struct DeadlineCost     <: AbstractDeadlineCost
    deadline::Float64 # deadline
    m::TravelTime
end
DeadlineCost(deadline::R) where {R<:Real} = DeadlineCost(deadline,TravelTime())
function set_deadline!(m::DeadlineCost,t_max)
    m.deadline = minimum(t_max)
    return m
end
set_deadline!(m::C,args...) where {C<:AbstractCostModel} = nothing
set_deadline!(m::C,args...) where {C<:FullCostModel} = set_deadline!(m.model,args...)
set_deadline!(m::C,args...) where {C<:MetaCostModel} = set_deadline!(m.model,args...)
function set_deadline!(m::C,args...) where {C<:CompositeCostModel}
    for model in m.cost_models
        set_deadline!(model,args...)
    end
end
add_heuristic_cost(m::C, cost, h_cost) where {C<:DeadlineCost} = max(0.0, cost + h_cost - m.deadline) # assumes heuristic is PerfectHeuristic
for op in [:accumulate_cost,:get_initial_cost,:get_transition_cost,:compute_path_cost]
    @eval $op(model::AbstractDeadlineCost,args...) = $op(model.m,args...)
end
FullDeadlineCost(model::DeadlineCost) = FullCostModel(costs->max(0.0, maximum(costs)),model)

"""
    MultiDeadlineCost

Combines multiple deadlines according to some specified aggregation function.
"""
struct MultiDeadlineCost{F} <: AbstractDeadlineCost
    f::F # aggregation function
    tF::Vector{Float64}
    root_nodes::Vector{Int} # weights and deadlines correspond to root_nodes only
    weights::Vector{Float64}
    deadlines::Vector{Float64}
    m::TravelTime
end
const SumOfMakeSpans = MultiDeadlineCost{SumCost}
const MakeSpan = MultiDeadlineCost{MaxCost}
SumOfMakeSpans(tF,root_nodes,weights,deadlines) = MultiDeadlineCost(SumCost(),Float64.(tF),root_nodes,Float64.(weights),Float64.(deadlines),TravelTime())
SumOfMakeSpans() = MultiDeadlineCost(SumCost(),Float64[],Int[],Float64[],Float64[],TravelTime())
MakeSpan(tF,root_nodes,weights,deadlines) = MultiDeadlineCost(MaxCost(),Float64.(tF),root_nodes,Float64.(weights),Float64.(deadlines),TravelTime())
MakeSpan() = MultiDeadlineCost(MaxCost(),Float64[],Int[],Float64[],Float64[],TravelTime())
function set_deadline!(m::M,t_max) where {M<:MultiDeadlineCost}
    m.deadlines .= t_max
    return m
end
add_heuristic_cost(m::C, cost, h_cost) where {C<:MultiDeadlineCost} = m.f(m.weights .* max.(0.0, cost .+ h_cost .- m.deadlines)) # assumes heuristic is PerfectHeuristic
aggregate_costs(m::C, costs::Vector{T}) where {T,C<:MultiDeadlineCost}  = m.f(m.tF[m.root_nodes] .* m.weights)
aggregate_costs_meta(m::C, costs::Vector{T}) where {T,C<:MultiDeadlineCost}  = maximum(costs)

# MakeSpan(model::FinalTime=FinalTime()) = FullCostModel(maximum,model)
SumOfTravelDistance(model::TravelDistance=TravelDistance()) = FullCostModel(sum,model)
SumOfTravelTime(model::TravelTime=TravelTime()) = FullCostModel(sum, model)

export EnvDeadlineCost
"""
    EnvDeadlineCost

Deadlines come from an external environment
"""
@with_kw struct EnvDeadlineCost{E,F} <: AbstractDeadlineCost
    env::E = nothing
    f::F   = SumCost()          # aggregation function
    m::TravelTime = TravelTime()
end
# EnvDeadlineCost() = EnvDeadlineCost(SumCost(),nothing,TravelTime())
EnvDeadlineCost{E,F}() where {E,F} = EnvDeadlineCost(env=E(),f=F())
aggregate_costs(::EnvDeadlineCost,costs) = maximum(costs)

################################################################################
############################# HardConflictHeuristic ############################
################################################################################
export
    AbstractConflictTable,
    HardConflictTable,
    get_time_horizon,
    get_planned_vtx,
    reset_path!,
    set_path!,
    partially_set_path!,
    get_conflict_value,
    get_conflicting_paths,
    construct_empty_lookup_table

abstract type AbstractConflictTable end

"""
    HardConflictTable

Stores a lookup table of planned paths for all agents.
When agent `i` queries the table, `table.paths[i]` (the existing path plan
for agent `i`) must be subtracted so that the agent does not try to avoid
conflicts with itself.
"""
@with_kw struct HardConflictTable{V<:AbstractVector,M<:AbstractMatrix} <: AbstractConflictTable
    paths   ::Vector{V} = Vector{SparseVector{Int,Int}}()
    CAT     ::M         = SparseMatrixCSC(zeros(2,2)) # global table
    start_time::Int     = 0
end
get_time_horizon(h::T) where {T<:HardConflictTable} = size(h.CAT,2)
function get_planned_vtx(h::T,agent_idx::Int,t::Int) where {T<:HardConflictTable}
    t_idx = t + (1-h.start_time)
    if agent_idx == -1
        return 0
    else
        return get(h.paths[agent_idx], t_idx, -1)
    end
end
function reset_path!(h::T,path_idx::Int) where {V,M,T<:HardConflictTable{V,M}}
    # first remove the old path from the lookup table
    for (t,vtx) in enumerate(h.paths[path_idx])
        if vtx > 0
            h.CAT[vtx,t] = h.CAT[vtx,t] - 1
        end
    end
    # initialize an empty new path
    h.paths[path_idx] = V(zeros(Int,get_time_horizon(h)))
    return h
end
function set_path!(h::T,path_idx::Int,path::Vector{Int},start_time::Int=0) where {V,M,T<:HardConflictTable{V,M}}
    # println("Updating conflict table with path ", path, " for agent ",path_idx)
    reset_path!(h,path_idx)
    # now add new path vtxs to new path and lookup table
    for (i,vtx) in enumerate(path)
        t = (start_time-h.start_time) + i
        h.paths[path_idx][t] = vtx
        h.CAT[vtx,t] = h.CAT[vtx,t] + 1
    end
    h
end
"""
    partially_set_path!

Only replaces the cached path from start_time to length(path). Useful if you
want the remainder of the cached path to stay in the lookup table (i.e. for
repairing an existing plan).
"""
function partially_set_path!(h::T,path_idx::Int,path::Vector{Int},start_time::Int=0) where {V,M,T<:HardConflictTable{V,M}}
    # now add new path vtxs to new path and lookup table
    t0 = (start_time-h.start_time)
    for (i,vtx) in enumerate(path)
        t = t0 + i
        # try
        checkbounds(Bool,h.paths[path_idx],t) ? nothing : throw(SolverException("t = $(t) Out of bounds in partially_set_path! with start_time = $(h.start_time)"))
        old_vtx = h.paths[path_idx][t]
        if old_vtx != 0
            h.CAT[old_vtx,t] = h.CAT[old_vtx,t] - 1
        end
        h.paths[path_idx][t] = vtx
        h.CAT[vtx,t] = h.CAT[vtx,t] + 1
        # catch e
        #     throw(e)
        # end
    end
    h
end
function get_conflict_value(h::HardConflictTable,agent_idx::Int,vtx::Int,t::Int)
    t_idx = t + (1-h.start_time)
    try
        # c = h.CAT[vtx,t_idx]
        c = get(h.CAT, (vtx,t_idx), 0)
        if get_planned_vtx(h,agent_idx,t) == vtx # conflict with own trajectory
            c = c - 1
        end
        return c
    catch e
        @show vtx,t_idx,size(h.CAT)
        rethrow(e)
    end
end

"""
    get_conflicting_paths

Operates over a lookup table and returns a dictionary mapping path index to the
time index at which the conflict occurred
"""
function get_conflicting_paths(ct::T) where {T<:HardConflictTable}
    conflict_idxs = map(idx->Int.([idx.I...]), findall(ct.CAT .> 1))
    agent_id_to_time_idx = Dict{Int,Int}()
    for idx in conflict_idxs
        vtx = idx[1]
        t = idx[2]+ct.start_time-1
        for (agent_id,path) in enumerate(ct.paths)
            if get_planned_vtx(ct,agent_id,t) == vtx
                agent_id_to_time_idx[agent_id] = t
            end
        end
    end
    agent_id_to_time_idx
end

"""
    construct_empty_lookup_table(G,T::Int)

Returns an empty lookup table.
"""
construct_empty_lookup_table(V::Int,T::Int) = SparseMatrixCSC(zeros(V,T))
construct_empty_lookup_table(graph::G,T::Int) where {G<:AbstractGraph} = construct_empty_lookup_table(nv(graph),T)
function HardConflictTable(graph::G,T::Int,num_agents::Int) where {G<:AbstractGraph}
    HardConflictTable(
        paths = map(i->SparseVector(zeros(Int,T)),1:num_agents),
        CAT = construct_empty_lookup_table(graph,T)
        )
end
function HardConflictTable(graph::G,T::Float64,num_agents::Int) where {G<:AbstractGraph}
    @assert abs(T - Int(round(T))) < 0.01
    HardConflictTable(graph,Int(round(T)),num_agents)
end

################################################################################
############################### SoftConflictHeuristic ##############################
################################################################################
export
    SoftConflictTable,
    get_fat_path,
    add_fat_path_to_table!,
    populate_soft_lookup_table!

"""
    SoftConflictTable
"""
@with_kw struct SoftConflictTable{V,M<:AbstractMatrix} <: AbstractConflictTable
    paths   ::Vector{V} = Vector{SparseMatrixCSC{Float64,Int}}()
    CAT     ::M         = SparseMatrixCSC(zeros(2,2)) # global table
    start_time::Int     = 0
end
get_time_horizon(h::SoftConflictTable) = size(h.CAT,2)
get_conflict_value(h::SoftConflictTable,vtx::Int,t::Int) = h.CAT[vtx,t]
get_conflict_value(h::SoftConflictTable,agent_idx::Int,vtx::Int,t::Int) = h.CAT[vtx,t] - h.paths[agent_idx][vtx,t]

"""
    get_fat_path(G,D,start_vtx,goal_vtx)

returns a fat path through `G` from `start_vtx` to `goal_vtx`. Each set
of vertices in the fat path contains all vertices with distance d1 from
start_vtx and distance d2 to goal_vtx, where d1+d2 == the length of the
shortest path(s) from `start_vtx` to `goal_vtx`

G is a graph, D is the distance matrix
"""
function get_fat_path(G,D,start_vtx::Int,goal_vtx::Int)
    fat_path = Vector{Set{Int}}([Set{Int}(start_vtx)])
    for i in 1:D[start_vtx,goal_vtx]
        next_set = Set{Int}()
        for src_vtx in fat_path[end]
            for dst_vtx in outneighbors(G,src_vtx)
                if D[dst_vtx,goal_vtx] <= D[src_vtx,goal_vtx] - 1
                    push!(next_set, dst_vtx)
                end
            end
        end
        push!(fat_path, next_set)
    end
    fat_path
end

"""
    add_fat_path_to_table(CAT,fat_path)
"""
function add_fat_path_to_table!(CAT,fat_path,t0=0)
    for t in 1:length(fat_path)
        idxs = collect(fat_path[t])
        if t+t0 > 0
            CAT[idxs,t+t0] .+= 1.0/length(idxs)
        end
    end
end

"""
    populate_soft_lookup_table!(CAT,start_times,start_vtxs,goal_vtxs)
"""
function populate_soft_lookup_table!(CAT,G,D,start_vtxs,goal_vtxs,start_times=zeros(Int,length(start_vtxs)))
    for (s,t,g) in zip(start_vtxs,start_times,goal_vtxs)
        fat_path = get_fat_path(G,D,s,g)
        add_fat_path_to_table!(CAT,t0,fat_path)
    end
    CAT
end

"""
    construct_empty_lookup_table(graph,T::Int)

Returns a soft lookup table to encode possible paths for each agent through
`graph`. The argument `T` defines the time horizon of the lookup table.
"""
function SoftConflictTable(n_agents::Int,N::Int,T::Int,t0=0)
    SoftConflictTable(
        paths = map(i->spzeros(Float64,N,T),1:n_agents),
        CAT = spzeros(Float64,N,T),
        t0=t0
        )
end
function SoftConflictTable(n_agents::Int,N::Int,T::Float64,t0=0)
    @assert abs(T - Int(round(T))) < 0.01
    @assert abs(t0 - Int(round(t0))) < 0.01
    SoftConflictTable(n_agents,N,Int(round(T)),Int(round(t0)))
end

# """
#     `construct_empty_lookup_table(graph,T::Int)`
#
#     Returns a soft lookup table to encode possible paths for each agent through
#     `graph`. The argument `T` defines the time horizon of the lookup table.
# """
# function SoftConflictTable(graph,n_agents::Int,T::Int,t0=0)
#     SoftConflictTable(
#         paths = map(i->construct_empty_lookup_table(graph,T),1:n_agents),
#         CAT = construct_empty_lookup_table(graph,T),
#         t0=0
#         )
# end

# """
#     `construct_and_populate_soft_lookup_table!`
# """
# function SoftConflictTable(graph,
#         start_vtxs::Vector{Int},
#         goal_vtxs::Vector{Int},
#         start_times=zero(Int,length(start_vtxs),
#         T = Int(round(maximum(start_times) + nv(graph))),
#         )
#     )
#     CAT = construct_empty_lookup_table(graph,T)
#     D = get_dist_matrix(graph)
#     populate_soft_lookup_table!(CAT,graph,D,start_vtxs,goal_vtxs,start_times)
#     SoftConflictTable(CAT)
# end

################################################################################
############################# ConflictCostModel ############################
################################################################################
export
    ConflictCostModel,
    HardConflictCost,
    SoftConflictCost

struct ConflictCostModel{T<:Union{HardConflictTable,SoftConflictTable}} <: LowLevelCostModel{Float64}
    table::T
end
get_conflict_value(h::H, args...) where {H<:ConflictCostModel} = get_conflict_value(h.table, args...)
accumulate_cost(h::H,cost,transition_cost) where {H<:ConflictCostModel} = cost + transition_cost

HardConflictCost(args...) = FullCostModel(sum,ConflictCostModel(HardConflictTable(args...)))
SoftConflictCost(args...) = FullCostModel(sum,ConflictCostModel(SoftConflictTable(args...)))
SoftConflictCost(table::SoftConflictTable,args...) = FullCostModel(sum,ConflictCostModel(table))

get_time_horizon(h::H) where {H<:ConflictCostModel} = get_time_horizon(h.table)
get_planned_vtx(h::H,args...) where {T<:HardConflictTable,H<:ConflictCostModel}  = get_planned_vtx(h.table,args...)
reset_path!(h::H,args...) where {H<:ConflictCostModel}    = reset_path!(h.table,args...)
reset_path!(h::H,args...) where {H<:AbstractCostModel}    = nothing
reset_path!(h::FullCostModel{F,T,M},args...) where {F,T,M<:ConflictCostModel} = reset_path!(h.model,args...)
function reset_path!(h::H,args...) where {H<:CompositeCostModel}
    for m in h.cost_models
        reset_path!(m,args...)
    end
end
set_path!(h::H,args...) where {H<:ConflictCostModel}    = set_path!(h.table,args...)
set_path!(h::H,args...) where {H<:AbstractCostModel}    = nothing
set_path!(h::FullCostModel{F,T,M},args...) where {F,T,M<:ConflictCostModel} = set_path!(h.model,args...)
function set_path!(h::H,args...) where {H<:CompositeCostModel}
    for m in h.cost_models
        set_path!(m,args...)
    end
end
partially_set_path!(h::H,args...) where {H<:ConflictCostModel}    = partially_set_path!(h.table,args...)
partially_set_path!(h::H,args...) where {H<:AbstractCostModel}    = nothing
partially_set_path!(h::FullCostModel{F,T,M},args...) where {F,T,M<:ConflictCostModel} = partially_set_path!(h.model,args...)
function partially_set_path!(h::H,args...) where {H<:CompositeCostModel}
    for m in h.cost_models
        partially_set_path!(m,args...)
    end
end


text:
struct SolverState
    outputs::Dict{Arc{Rule}, Set{String}}
    derivations::Dict{Arc{Rule}, Dict{String, Vector{Vector{String}}}}
end

SolverState() = SolverState(Dict(), Dict())

@generated function _product(inputs, ::Val{N}) where {N}
    Expr(:call, :product, [:(inputs[$i]) for i in 1:N]...)
end

function apply(head::GrammaticalSymbol, args::Tuple{Vararg{GrammaticalSymbol, N}}, inputs) where {N}
    result = Set{String}()
    for input in _product(inputs, Val{N}())
        apply!(result, head, args, input)
    end
    result
end

function _apply(head::GrammaticalSymbol, args::Tuple{Vararg{GrammaticalSymbol, N}}, inputs::AbstractVector) where {N}
    apply(head, args, ntuple(i -> inputs[i], Val(N)))
end

function _apply(rule::Rule,
                constituents::AbstractVector)
    r = inner(rule)
    _apply(lhs(r), rhs(r), constituents)
end

solve!(state::SolverState, s::AbstractString) = Set([s])

function solve!(state::SolverState, arc::Arc{Rule})
    get!(state.outputs, arc) do
        inputs = [solve!(state, c) for c in constituents(arc)]
        _apply(rule(arc), inputs)
    end
end

struct SolvedArc
    arc::Arc{Rule}
    output::String
    similarity::Float64
end

function output_checker(len::Union{Integer, Nothing}, pattern::Regex)
    return word -> ((len === nothing || num_letters(word) == len) && is_word(word) && occursin(pattern, word))
end

function solve(clue;
        length::Union{Integer, Nothing} = nothing,
        pattern::Regex = r"",
        strategy = BottomUp(),
        min_grammar_score = 1e-6,
        should_continue = () -> true)
    state = SolverState()
    tokens = normalize.(split(clue))
    grammar = CrypticsGrammar()
    check = output_checker(length, pattern)

    function is_solvable(arc)
        if score(arc) < min_grammar_score
            return 0
        end
        outputs = solve!(state, arc)
        isempty(outputs)
        if isempty(outputs)
            return 0
        else
            return 1
        end
    end
    parser = ChartParser(tokens, grammar, BottomUp(),
                         is_solvable)
    solutions = SolvedArc[]
    lowest_score_seen = Inf
    for arc in parser
        if !should_continue()
            break
        end
        if !is_complete(arc, parser)
            continue
        end
        @assert score(arc) <= lowest_score_seen
        lowest_score_seen = score(arc)
        if score(arc) < min_grammar_score
            break
        end
        # TODO: probably don't need to call solve!() here
        outputs = solve!(state, arc)
        @assert !isempty(outputs)
        # for (output, inputs) in outputs
        for output in outputs
            if check(output)
                push!(solutions, SolvedArc(arc, output,
                                           solution_quality(arc, output)))
            end
        end
    end
    sort!(solutions, by=s -> s.similarity, rev=true)
    solutions, state
end

struct DerivedArc
    arc::Arc{Rule}
    output::String
    constituents::Vector{Union{DerivedArc, String}}
end

struct DerivedSolution
    derivation::DerivedArc
    output::String
    similarity::Float64
end

function derive(head::GrammaticalSymbol, args::Tuple{Vararg{GrammaticalSymbol, N}}, inputs, target) where {N}
    result = Vector{Vector{String}}()
    buffer = Vector{String}()
    for input in _product(inputs, Val{N}())
        empty!(buffer)
        apply!(buffer, head, args, input)
        input_vec = collect(input)
        for output in buffer
            if output == target
                push!(result, input_vec)
            end
        end
    end
    result
end

function _derive(head::GrammaticalSymbol, args::Tuple{Vararg{GrammaticalSymbol, N}}, inputs::AbstractVector, target::AbstractString) where {N}
    derive(head, args, ntuple(i -> inputs[i], Val(N)), target)
end

function _derive(rule::Rule,
                constituents::AbstractVector,
                target::AbstractString)
    r = inner(rule)
    _derive(lhs(r), rhs(r), constituents, target)
end

function find_derivations!(state::SolverState, arc::Arc{Rule}, target::AbstractString)
    arc_derivations = get!(state.derivations, arc) do
        Dict()
    end
    get!(arc_derivations, target) do
        inputs = [solve!(state, c) for c in constituents(arc)]
        _derive(rule(arc), inputs, target)
    end
end

derive!(state::SolverState, s::AbstractString, t::AbstractString) = [t]

function derive!(state::SolverState, arc::Arc{Rule}, target::AbstractString)
    result = DerivedArc[]
    input_lists = find_derivations!(state, arc, target)
    for inputs in input_lists
        for children in product(derive!.(Ref(state), constituents(arc), inputs)...)
            push!(result, DerivedArc(arc, target, collect(children)))
        end
    end
    result
end

function derive!(state::SolverState, solved::SolvedArc)
    derivations = derive!(state, solved.arc, solved.output)
    DerivedSolution.(derivations, Ref(solved.output), Ref(solved.similarity))
end

_show(io::IO, arc::DerivedArc) = print(io, arc)
_show(io::IO, s::AbstractString) = print(io, '"', s, '"')

function Base.show(io::IO, arc::DerivedArc)
    print(io, "($(lhs(rule(arc.arc))) -> ")
    for c in arc.constituents
        _show(io, c)
        print(io, " ")
    end
    print(io, "; $(score(arc.arc))) -> \"$(arc.output)\")")
end

function answer_similarity(word1::AbstractString, word2::AbstractString)
    if word2 in keys(CACHE.synonyms) && word1 in CACHE.synonyms[word2]
        1.0
    else
        SemanticSimilarity.similarity(word1, word2)
    end
end

function solution_quality(arc::Arc, output::AbstractString)
    @assert lhs(inner(rule(arc))) === Clue()
    answer_similarity(definition(arc), output)
end

tokens(arc::Arc) = join((tokens(x) for x in constituents(arc)), ' ')
tokens(s::AbstractString) = s

function definition(arc::Arc)
    @assert lhs(inner(rule(arc))) === Clue()
    tokens(first(x for x in constituents(arc) if lhs(inner(rule(x))) == Definition()))
end

definition(arc::SolvedArc) = definition(arc.arc)

num_letters(word::AbstractString) = count(!isequal(' '), word)


text:
"""
   Arrow
`enum` type that can take three values: `In`, `Out`, or `Neither`, representing a directionality
associated with an index, i.e. the index leg is directed into or out of a given tensor
"""
@enum Arrow In=-1 Out=1 Neither=0

"""
    -(dir::Arrow)
Reverse direction of a directed `Arrow`.
"""
function Base.:-(dir::Arrow)
  return Arrow(-Int(dir))
end


text:
#=
  Package: FishEBM
  File: agents.jl
  Justin Angevaare, Devin Rose
  Functions for agent-level model components used by other functions
  Created: May 2015
=#

"""
  Description: A function to create an empty agent database for the specified
    simulation length.

  Returns: Vector(EnviroAgent)

  Last update: March 2016
"""
function AgentDB(enviro::EnvironmentAssumptions)
  #Initialize the database
  agent_db = [EnviroAgent(0)]; init = false;
  length = (size(enviro.habitat)[1])*(size(enviro.habitat)[2])

  #Push new agents into the database for each enviro location
  for i = 1:length
    if enviro.habitat[i] > 0
      if init == false
        agent_db[1].locationID = i
        init = true
      else
        push!(agent_db, EnviroAgent(i))
      end
    end
  end

  return agent_db
end

"""
  Description: Function used to find the current life stage of a cohort from
    the current age using the agent assumptions growth vector.

  Returns: Int64

  Last update: September 2016
"""
function findCurrentStage(current_week::Int64, spawn_week::Int64, growth_age::Vector{Int64})
  #Initialize the life stage number to 4
  currentStage = 4
  q = length(growth_age)-1
  current_age = current_week - spawn_week

  #Most cohorts are likely to be adults, thus check stages from old to young
  while q > 0 && current_age < growth_age[q]
    currentStage = q
    q-=1
  end

  return currentStage
end

"""
  Description: Returns age of cohort. If cohort is not adult, returns 0.

    Returns: Int64

  Last Update: July 2016
"""
function getAge(current_week::Int64, spawn_week::Int64)
  age = floor((current_week - spawn_week)/52)
  if age < 2
    return 0
  elseif age > 8
    return 8
  else
    return Int(age)
  end
end

"""
  Description: Returns population of a specific age in an environment agent.
    Used for functions that requires age-specific population to be taken into
    account (such as spawning or harvest).

    Returns: Int64

  Last Update: September 2016
"""
function getAgeSpecificPop(age::Int64, current_week::Int64, alive::Vector{Int64}, weekNum::Vector{Int64}, a_a::AgentAssumptions)
  @assert(2 <= age <= 8, "Age argument must be between 2 and 8, inclusive (age = $age was passed).")
  classLength = length(weekNum)
  pop = 0
  for i = 1:classLength
    if findCurrentStage(current_week, weekNum[i], a_a.growth) == 4
      if age == 8
        if floor((current_week - weekNum[i]) / 52) >= age
          pop += alive[i]
        end
      else
        if floor((current_week - weekNum[i]) / 52) == age
          pop += alive[i]
        end #if floor
      end #if age == 8 else
    end #findCurrentStage
  end #for i = 1:classLength

  return pop
end

"""
  Description: Returns index of cohort of specified age. Used in harvest! to
    determine which cohort to subtract the harvest size from. If no cohort exist
    for the specified age, function will return 0 and so calling function should
    check if this function returns 0.

  Returns: Int64

  Last Update: September 2016
"""
function getCohortNumber(age::Int64, current_week::Int64, weekNum::Vector{Int64})
  classLength = length(weekNum)
  for i = 1:classLength
    if floor((current_week - weekNum[i]) / 52) == age
      return i
    end #if floor
  end #for i

  return 0
end

"""
  Description:  Used to get population of any of the stages (egg, larva,
    juvenile, adult). Will only return population of one stage at a time. To
    get total population of fish, loop through for i = 1:4.

  Returns: Int64

  Last update: September 2016
"""
function getStagePopulation(stage::Int64, current_week::Int64, agent_db::Vector{EnviroAgent}, a_a::AgentAssumptions)

  classLength = length((agent_db[1]).weekNum)
  pop = 0
  for i = 1:length(agent_db)
    if (isEmpty(agent_db[i]) == false)
      for j = 1:classLength
        if findCurrentStage(current_week, agent_db[i].weekNum[j], a_a.growth) == stage
          pop += agent_db[i].alive[j]
        end #findCurrentStage
      end #for j=1:classLength
    end #if isEmpty
  end #for i=1:length

  return pop
end

"""
  Description: This function is used for efficiently finding the Agent number
    from a known environment id.

  Returns: Int64

  Last update: September 2016
"""
function IDToAgentNum(a_db::Vector{EnviroAgent}, id_num::Int64, max_val::Int64, min_val::Int64)
  testVal = rand(min_val:max_val)

  #uses recursion
  if id_num == a_db[testVal].locationID
    return testVal
  elseif a_db[testVal].locationID > id_num
    testVal = IDToAgentNum(a_db, id_num, testVal, min_val)
  elseif a_db[testVal].locationID < id_num
    testVal = IDToAgentNum(a_db, id_num, max_val, testVal)
  end

  return testVal
end

"""
  Description: This function injects agents into the environment, this is
    function is mainly used for adding agents to the environment to test new
    functions.

    Returns: Operates directly on agent_db

  Last update: September 2016
"""
function injectAgents!(agent_db::Vector{EnviroAgent}, spawn_agents::Vector{Int64}, new_stock::Int64, week_num::Int64)
  @assert(length(new_stock)<=4, "There can only by four independent life stages of fish.")

  addToEach = round(Int, floor(new_stock/length(spawn_agents)))
  leftOver = new_stock%length(spawn_agents)
  randomAgent = rand(1:length(spawn_agents))

  #add a new population class to every agent
  for agentRef = 1:length(agent_db)
    push!((agent_db[agentRef]).alive, 0)
    push!((agent_db[agentRef]).weekNum, week_num)
  end

  #Only adds agents to the last (newest) cohort in the agent db spawn locations
  classLength = length((agent_db[1]).weekNum)
  for agentNum = 1:length(spawn_agents)
    addToAgent = addToEach
    if agentNum == randomAgent
      addToAgent += leftOver
    end

    (agent_db[spawn_agents[agentNum]]).alive[classLength] = addToAgent
  end
end

"""
  Description: This function is used to remove an empty spawn class once all
    agents in the entire spawn class have been removed.

  Returns: Operates directly on age_db

  Last Update: September 2016
"""
function removeEmptyClass!(age_db::Vector{EnviroAgent})
  if length(age_db[1].alive) > 1
    removeClass = true
    for i = 1:length(age_db)
      if (age_db[i]).alive[1] != 0
        removeClass = false
        i = length(age_db)
      end
    end

    if removeClass
      for j = 1:length(age_db)
        shift!((age_db[j]).alive)
        shift!((age_db[j]).weekNum)
      end
      removeEmptyClass!(age_db)
    end
  end
end


text:
# Julia wrapper for header: /home/jan/.julia/v0.5/IAEAPhsp/deps/iaea_phsp_Sept2013/iaea_phsp.h
# Automatically generated using Clang.jl wrap_c, version 0.0.0

function iaea_new_source(source_ID,header_file,access,result,hf_length::Cint)
    ccall((:iaea_new_source,IAEA_LIB),Void,(Ptr{IAEA_I32},Cstring,Ptr{IAEA_I32},Ptr{IAEA_I32},Cint),source_ID,header_file,access,result,hf_length)
end

function iaea_get_max_particles(id,_type,n_particle)
    ccall((:iaea_get_max_particles,IAEA_LIB),Void,(Ptr{IAEA_I32},Ptr{IAEA_I32},Ptr{IAEA_I64}),id,_type,n_particle)
end

function iaea_get_maximum_energy(id,Emax)
    ccall((:iaea_get_maximum_energy,IAEA_LIB),Void,(Ptr{IAEA_I32},Ptr{IAEA_Float}),id,Emax)
end

function iaea_get_extra_numbers(id,n_extra_float,n_extra_int)
    ccall((:iaea_get_extra_numbers,IAEA_LIB),Void,(Ptr{IAEA_I32},Ptr{IAEA_I32},Ptr{IAEA_I32}),id,n_extra_float,n_extra_int)
end

function iaea_set_extra_numbers(id,n_extra_float,n_extra_int)
    ccall((:iaea_set_extra_numbers,IAEA_LIB),Void,(Ptr{IAEA_I32},Ptr{IAEA_I32},Ptr{IAEA_I32}),id,n_extra_float,n_extra_int)
end

function iaea_set_type_extralong_variable(id,index,_type)
    ccall((:iaea_set_type_extralong_variable,IAEA_LIB),Void,(Ptr{IAEA_I32},Ptr{IAEA_I32},Ptr{IAEA_I32}),id,index,_type)
end

function iaea_set_type_extrafloat_variable(id,index,_type)
    ccall((:iaea_set_type_extrafloat_variable,IAEA_LIB),Void,(Ptr{IAEA_I32},Ptr{IAEA_I32},Ptr{IAEA_I32}),id,index,_type)
end

function iaea_get_type_extra_variables(id,result,extralong_types,extrafloat_types)
    ccall((:iaea_get_type_extra_variables,IAEA_LIB),Void,(Ptr{IAEA_I32},Ptr{IAEA_I32},Ptr{IAEA_I32},Ptr{IAEA_I32}),id,result,extralong_types,extrafloat_types)
end

function iaea_set_constant_variable(id,index,constant)
    ccall((:iaea_set_constant_variable,IAEA_LIB),Void,(Ptr{IAEA_I32},Ptr{IAEA_I32},Ptr{IAEA_Float}),id,index,constant)
end

function iaea_get_constant_variable(id,index,constant,result)
    ccall((:iaea_get_constant_variable,IAEA_LIB),Void,(Ptr{IAEA_I32},Ptr{IAEA_I32},Ptr{IAEA_Float},Ptr{IAEA_I32}),id,index,constant,result)
end

function iaea_get_used_original_particles(id,n_indep_particles)
    ccall((:iaea_get_used_original_particles,IAEA_LIB),Void,(Ptr{IAEA_I32},Ptr{IAEA_I64}),id,n_indep_particles)
end

function iaea_get_total_original_particles(id,number_of_original_particles)
    ccall((:iaea_get_total_original_particles,IAEA_LIB),Void,(Ptr{IAEA_I32},Ptr{IAEA_I64}),id,number_of_original_particles)
end

function iaea_set_total_original_particles(id,number_of_original_particles)
    ccall((:iaea_set_total_original_particles,IAEA_LIB),Void,(Ptr{IAEA_I32},Ptr{IAEA_I64}),id,number_of_original_particles)
end

function iaea_set_parallel(id,i_parallel,i_chunk,n_chunk,is_ok)
    ccall((:iaea_set_parallel,IAEA_LIB),Void,(Ptr{IAEA_I32},Ptr{IAEA_I32},Ptr{IAEA_I32},Ptr{IAEA_I32},Ptr{IAEA_I32}),id,i_parallel,i_chunk,n_chunk,is_ok)
end

function iaea_set_record(id,record_num,result)
    ccall((:iaea_set_record,IAEA_LIB),Void,(Ptr{IAEA_I32},Ptr{IAEA_I64},Ptr{IAEA_I32}),id,record_num,result)
end

function iaea_check_file_size_byte_order(id,result)
    ccall((:iaea_check_file_size_byte_order,IAEA_LIB),Void,(Ptr{IAEA_I32},Ptr{IAEA_I32}),id,result)
end

function iaea_get_particle(id,n_stat,_type,E,wt,x,y,z,u,v,w,extra_floats,extra_ints)
    ccall((:iaea_get_particle,IAEA_LIB),Void,(Ptr{IAEA_I32},Ptr{IAEA_I32},Ptr{IAEA_I32},Ptr{IAEA_Float},Ptr{IAEA_Float},Ptr{IAEA_Float},Ptr{IAEA_Float},Ptr{IAEA_Float},Ptr{IAEA_Float},Ptr{IAEA_Float},Ptr{IAEA_Float},Ptr{IAEA_Float},Ptr{IAEA_I32}),id,n_stat,_type,E,wt,x,y,z,u,v,w,extra_floats,extra_ints)
end

function iaea_write_particle(id,n_stat,_type,E,wt,x,y,z,u,v,w,extra_floats,extra_ints)
    ccall((:iaea_write_particle,IAEA_LIB),Void,(Ptr{IAEA_I32},Ptr{IAEA_I32},Ptr{IAEA_I32},Ptr{IAEA_Float},Ptr{IAEA_Float},Ptr{IAEA_Float},Ptr{IAEA_Float},Ptr{IAEA_Float},Ptr{IAEA_Float},Ptr{IAEA_Float},Ptr{IAEA_Float},Ptr{IAEA_Float},Ptr{IAEA_I32}),id,n_stat,_type,E,wt,x,y,z,u,v,w,extra_floats,extra_ints)
end

function iaea_destroy_source(source_ID,result)
    ccall((:iaea_destroy_source,IAEA_LIB),Void,(Ptr{IAEA_I32},Ptr{IAEA_I32}),source_ID,result)
end

function iaea_print_header(source_ID,result)
    ccall((:iaea_print_header,IAEA_LIB),Void,(Ptr{IAEA_I32},Ptr{IAEA_I32}),source_ID,result)
end

function iaea_copy_header(source_ID,destiny_ID,result)
    ccall((:iaea_copy_header,IAEA_LIB),Void,(Ptr{IAEA_I32},Ptr{IAEA_I32},Ptr{IAEA_I32}),source_ID,destiny_ID,result)
end

function iaea_update_header(source_ID,result)
    ccall((:iaea_update_header,IAEA_LIB),Void,(Ptr{IAEA_I32},Ptr{IAEA_I32}),source_ID,result)
end


text:
# Iterator State Placeholders
@inline _dummy_iterate_state(itr) = iterate(itr)[2]
@inline _dummy_iterate_state(itr::Array) = 0
@inline _dummy_iterate_state(itr::OrdinalRange{T}) where {T} = zero(T)
@inline _dummy_iterate_state(itr::StaticArray) = (Base.OneTo(1), 0)
@inline _dummy_iterate_state(itr::Base.Generator) = _dummy_iterate_state(itr.iter)
@inline _dummy_iterate_state(itr::Iterators.Filter) = _dummy_iterate_state(itr.itr)

@inline _dummy_iterate_result(itr) = iterate(itr)
@inline _dummy_iterate_result(itr::AbstractArray{T}) where {T} = (zero(T), _dummy_iterate_state(itr))

# ControlInterval Iteration
@inline Base.iterate(c::ControlInterval) = (c, nothing)
@inline Base.iterate(c::ControlInterval, ::Any) = nothing
@inline Base.eltype(::Type{CI}) where {CI<:ControlInterval} = CI
@inline Base.length(::ControlInterval) = 1

# Propagate
struct Propagate{D,S,C,T}
    f::D
    x0::S
    cs::C
    ss::T
end

Base.length(itr::Propagate) = length(itr.ss)
Base.eltype(::Type{Propagate{D,S,C,T}}) where {D,S,C,T} = S

@inline function _start(itr::Propagate)
    cs_iter_result = iterate(itr.cs)
    if (cs_done = cs_iter_result === nothing)
        cs_iter_result = _dummy_iterate_result(itr.cs)
    end
    ((itr.x0, zero(eltype(itr.ss)), cs_done, cs_iter_result),)
end

@inline function Base.iterate(itr::Propagate, state=_start(itr))
    x, t, cs_done, cs_iter_result = state[1][1], state[1][2], state[1][3], state[1][4]
    ss_state = Base.tail(state)

    ss_iter_result = iterate(itr.ss, ss_state...)
    ss_iter_result === nothing && return nothing
    s, ss_state = ss_iter_result

    c, cs_state = cs_iter_result
    if !cs_done
        while s >= t + duration(c)
            x = propagate(itr.f, x, c)
            t += duration(c)
            cs_iter_result = iterate(itr.cs, cs_state)
            cs_iter_result === nothing && return x, ((x, t, true, (c, cs_state)), ss_state)
            c, cs_state = cs_iter_result
        end
        y = propagate(itr.f, x, c, s - t)
    else
        y = x
    end

    y, ((x, t, cs_done, cs_iter_result), ss_state)
end

# InstantaneousControl
struct InstantaneousControl{C,T}
    cs::C
    ss::T
end

Base.length(itr::InstantaneousControl) = length(itr.ss)
function Base.eltype(::Type{InstantaneousControl{C,T}}) where {C,T}
    typeof(instantaneous_control(zero(eltype(C)), zero(eltype(T))))
end

@inline function _start(itr::InstantaneousControl)
    cs_iter_result = iterate(itr.cs)
    if (cs_done = cs_iter_result === nothing)
        cs_iter_result = _dummy_iterate_result(itr.cs)
    end
    ((zero(eltype(itr.ss)), cs_done, cs_iter_result),)
end

@inline function Base.iterate(itr::InstantaneousControl, state=_start(itr))
    t, cs_done, cs_iter_result = state[1][1], state[1][2], state[1][3]
    ss_state = Base.tail(state)

    ss_iter_result = iterate(itr.ss, ss_state...)
    ss_iter_result === nothing && return nothing
    s, ss_state = ss_iter_result

    c, cs_state = cs_iter_result
    if !cs_done
        while s >= t + duration(c)
            t += duration(c)
            cs_iter_result = iterate(itr.cs, cs_state)
            cs_iter_result === nothing && return (instantaneous_control(c, duration(c)),
                                                  ((t, true, (c, cs_state)), ss_state))
            c, cs_state = cs_iter_result
        end
        ic = instantaneous_control(c, s-t)
    else
        ic = instantaneous_control(c, duration(c))
    end

    ic, ((t, cs_done, cs_iter_result), ss_state)
end


text:
include("utils.jl")

struct NotImplementedReshaper <: Manifolds.AbstractReshaper end

function parray(M, x)
    return Manifolds.ProductArray(
        Manifolds.ShapeSpecification(Manifolds.StaticReshaper(), M.manifolds...),
        x,
    )
end

@testset "Product manifold" begin
    @test_throws MethodError ProductManifold()
    M1 = Sphere(2)
    M2 = Euclidean(2)
    @test (@inferred ProductManifold(M1, M2)) isa ProductManifold
    Mse = ProductManifold(M1, M2)
    @test Mse == M1 × M2
    @test Mse == ProductManifold(M1) × M2
    @test Mse == ProductManifold(M1) × ProductManifold(M2)
    @test Mse == M1 × ProductManifold(M2)
    @test injectivity_radius(Mse) ≈ π
    @test injectivity_radius(
        Mse,
        ProductRepr([0.0, 1.0, 0.0], [0.0, 0.0]),
        ProductRetraction(ExponentialRetraction(), ExponentialRetraction()),
    ) ≈ π
    @test injectivity_radius(
        Mse,
        ProductRepr([0.0, 1.0, 0.0], [0.0, 0.0]),
        ExponentialRetraction(),
    ) ≈ π
    @test is_default_metric(Mse, ProductMetric())
    @test Manifolds.default_metric_dispatch(Mse, ProductMetric()) === Val{true}()
    @test_throws ErrorException Manifolds.make_reshape(
        NotImplementedReshaper(),
        Int64,
        zeros(2, 3),
    )
    @test Manifolds.number_of_components(Mse) == 2
    types = [Vector{Float64}]
    TEST_FLOAT32 && push!(types, Vector{Float32})
    TEST_STATIC_SIZED && push!(types, MVector{5,Float64})

    retraction_methods = [Manifolds.ProductRetraction(
        ManifoldsBase.ExponentialRetraction(),
        ManifoldsBase.ExponentialRetraction(),
    )]
    inverse_retraction_methods = [Manifolds.InverseProductRetraction(
        ManifoldsBase.LogarithmicInverseRetraction(),
        ManifoldsBase.LogarithmicInverseRetraction(),
    )]

    reshapers = (Manifolds.StaticReshaper(), Manifolds.ArrayReshaper())

    @testset "get_component, set_component!, getindex and setindex!" begin
        p1 = ProductRepr([0.0, 1.0, 0.0], [0.0, 0.0])
        @test get_component(Mse, p1, 1) == p1.parts[1]
        @test get_component(Mse, p1, Val(1)) == p1.parts[1]
        @test p1[Mse, 1] == p1.parts[1]
        @test p1[Mse, Val(1)] == p1.parts[1]
        @test p1[Mse, 1] isa Vector
        @test p1[Mse, Val(1)] isa Vector
        p2 = [10.0, 12.0]
        set_component!(Mse, p1, p2, 2)
        @test get_component(Mse, p1, 2) == p2
        p1[Mse, 2] = 2 * p2
        @test p1[Mse, 2] == 2 * p2
        p3 = [11.0, 15.0]
        set_component!(Mse, p1, p3, Val(2))
        @test get_component(Mse, p1, Val(2)) == p3
        p1[Mse, Val(2)] = 2 * p3
        @test p1[Mse, Val(2)] == 2 * p3

        shape_a_se = Manifolds.ShapeSpecification(Manifolds.ArrayReshaper(), M1, M2)
        pra1 = Manifolds.ProductArray(shape_a_se, [0.0, 1.0, 0.0, 0.0, 0.0])
        @test get_component(Mse, pra1, 1) == p1.parts[1]
        @test get_component(Mse, pra1, Val(1)) == p1.parts[1]
        @test pra1[Mse, 1] == pra1.parts[1]
        @test pra1[Mse, Val(1)] == pra1.parts[1]
        @test pra1[Mse, 1] isa Vector
        @test pra1[Mse, Val(1)] isa Vector
        set_component!(Mse, pra1, p2, 2)
        @test get_component(Mse, pra1, 2) == p2
        pra1[Mse, 2] = 2 * p2
        @test pra1[Mse, 2] == 2 * p2
        set_component!(Mse, pra1, p3, Val(2))
        @test get_component(Mse, pra1, Val(2)) == p3
        pra1[Mse, Val(2)] = 2 * p3
        @test pra1[Mse, Val(2)] == 2 * p3

        shape_s_se = Manifolds.ShapeSpecification(Manifolds.StaticReshaper(), M1, M2)
        prs1 = Manifolds.ProductArray(shape_s_se, [0.0, 1.0, 0.0, 0.0, 0.0])
        @test get_component(Mse, prs1, 1) == p1.parts[1]
        @test get_component(Mse, prs1, Val(1)) == p1.parts[1]
        @test prs1[Mse, 1] == prs1.parts[1]
        @test prs1[Mse, Val(1)] == prs1.parts[1]
        @test prs1[Mse, 1] isa Vector
        @test prs1[Mse, Val(1)] isa Vector
        set_component!(Mse, prs1, p2, 2)
        @test get_component(Mse, prs1, 2) == p2
        prs1[Mse, 2] = 2 * p2
        @test prs1[Mse, 2] == 2 * p2
        set_component!(Mse, prs1, p3, Val(2))
        @test get_component(Mse, prs1, Val(2)) == p3
        prs1[Mse, Val(2)] = 2 * p3
        @test prs1[Mse, Val(2)] == 2 * p3
    end

    @testset "CompositeManifoldError" begin
        Mpr = Sphere(2) × Sphere(2)
        p1 = [1.0, 0.0, 0.0]
        p2 = [0.0, 1.0, 0.0]
        X1 = [0.0, 1.0, 0.2]
        X2 = [1.0, 0.0, 0.2]
        p = ProductRepr(p1, p2)
        X = ProductRepr(X1, X2)
        pf = ProductRepr(p1, X1)
        Xf = ProductRepr(X1, p2)
        @test is_manifold_point(Mpr, p, true)
        @test_throws CompositeManifoldError is_manifold_point(Mpr, X, true)
        @test_throws ComponentManifoldError is_tangent_vector(Mpr, pf, X, true)
        @test_throws ComponentManifoldError is_tangent_vector(Mpr, p, Xf, true)
    end

    @testset "arithmetic" begin
        Mee = ProductManifold(Euclidean(3), Euclidean(2))
        p1 = ProductRepr([0.0, 1.0, 0.0], [0.0, 1.0])
        p2 = ProductRepr([1.0, 2.0, 0.0], [2.0, 3.0])

        @test isapprox(Mee, p1 + p2, ProductRepr([1.0, 3.0, 0.0], [2.0, 4.0]))
        @test isapprox(Mee, p1 - p2, ProductRepr([-1.0, -1.0, 0.0], [-2.0, -2.0]))
        @test isapprox(Mee, -p1, ProductRepr([0.0, -1.0, 0.0], [0.0, -1.0]))
        @test isapprox(Mee, p1 * 2, ProductRepr([0.0, 2.0, 0.0], [0.0, 2.0]))
        @test isapprox(Mee, 2 * p1, ProductRepr([0.0, 2.0, 0.0], [0.0, 2.0]))
        @test isapprox(Mee, p1 / 2, ProductRepr([0.0, 0.5, 0.0], [0.0, 0.5]))
    end

    @testset "Show methods" begin
        Mse2 = ProductManifold(M1, M1, M2, M2)
        @test sprint(show, Mse2) == "ProductManifold($(M1), $(M1), $(M2), $(M2))"
        withenv("LINES" => 10, "COLUMNS" => 100) do
            @test sprint(show, "text/plain", ProductManifold(M1)) ==
                  "ProductManifold with 1 submanifold:\n $(M1)"
            @test sprint(show, "text/plain", Mse2) ==
                  "ProductManifold with 4 submanifolds:\n $(M1)\n $(M1)\n $(M2)\n $(M2)"
            return nothing
        end
        withenv("LINES" => 7, "COLUMNS" => 100) do
            @test sprint(show, "text/plain", Mse2) ==
                  "ProductManifold with 4 submanifolds:\n $(M1)\n ⋮\n $(M2)"
            return nothing
        end

        @test sprint(show, "text/plain", ProductManifold(Mse, Mse)) == """
        ProductManifold with 2 submanifolds:
         ProductManifold(Sphere(2, ℝ), Euclidean(2; field = ℝ))
         ProductManifold(Sphere(2, ℝ), Euclidean(2; field = ℝ))"""

        shape_se = Manifolds.ShapeSpecification(Manifolds.ArrayReshaper(), M1)
        p = Manifolds.ProductArray(shape_se, Float64[1, 0, 0])
        @test sprint(show, "text/plain", p) == """
        ProductArray with 1 submanifold component:
         Component 1 =
          3-element view(::$(sprint(show, Vector{Float64})), 1:3) with eltype Float64:
           1.0
           0.0
           0.0"""

        shape_se = Manifolds.ShapeSpecification(Manifolds.ArrayReshaper(), M1, M1, M2, M2)
        p = Manifolds.ProductArray(shape_se, Float64[1, 0, 0, 0, 1, 0, 1, 2, 3, 4])
        @test sprint(show, "text/plain", p) == """
        ProductArray with 4 submanifold components:
         Component 1 =
          3-element view(::$(sprint(show, Vector{Float64})), 1:3) with eltype Float64:
           1.0
           0.0
           0.0
         Component 2 =
          3-element view(::$(sprint(show, Vector{Float64})), 4:6) with eltype Float64:
           0.0
           1.0
           0.0
         Component 3 =
          2-element view(::$(sprint(show, Vector{Float64})), 7:8) with eltype Float64:
           1.0
           2.0
         Component 4 =
          2-element view(::$(sprint(show, Vector{Float64})), 9:10) with eltype Float64:
           3.0
           4.0"""

        shape_se =
            Manifolds.ShapeSpecification(Manifolds.ArrayReshaper(), M1, M1, M2, M2, M2)
        p = Manifolds.ProductArray(shape_se, Float64[1, 0, 0, 0, 1, 0, 1, 2, 3, 4, 5, 6])

        @test sprint(show, "text/plain", p) == """
        ProductArray with 5 submanifold components:
         Component 1 =
          3-element view(::$(sprint(show, Vector{Float64})), 1:3) with eltype Float64:
           1.0
           0.0
           0.0
         Component 2 =
          3-element view(::$(sprint(show, Vector{Float64})), 4:6) with eltype Float64:
           0.0
           1.0
           0.0
         ⋮
         Component 4 =
          2-element view(::$(sprint(show, Vector{Float64})), 9:10) with eltype Float64:
           3.0
           4.0
         Component 5 =
          2-element view(::$(sprint(show, Vector{Float64})), 11:12) with eltype Float64:
           5.0
           6.0"""

        p = Manifolds.ProductRepr(Float64[1, 0, 0])
        @test sprint(show, "text/plain", p) == """
        ProductRepr with 1 submanifold component:
         Component 1 =
          3-element $(sprint(show, Vector{Float64})):
           1.0
           0.0
           0.0"""

        p = Manifolds.ProductRepr(
            Float64[1, 0, 0],
            Float64[0, 1, 0],
            Float64[1, 2],
            Float64[3, 4],
        )
        @test sprint(show, "text/plain", p) == """
        ProductRepr with 4 submanifold components:
         Component 1 =
          3-element $(sprint(show, Vector{Float64})):
           1.0
           0.0
           0.0
         Component 2 =
          3-element $(sprint(show, Vector{Float64})):
           0.0
           1.0
           0.0
         Component 3 =
          2-element $(sprint(show, Vector{Float64})):
           1.0
           2.0
         Component 4 =
          2-element $(sprint(show, Vector{Float64})):
           3.0
           4.0"""

        p = Manifolds.ProductRepr(
            Float64[1, 0, 0],
            Float64[0, 1, 0],
            Float64[1, 2],
            Float64[3, 4],
            Float64[5, 6],
        )
        @test sprint(show, "text/plain", p) == """
        ProductRepr with 5 submanifold components:
         Component 1 =
          3-element $(sprint(show, Vector{Float64})):
           1.0
           0.0
           0.0
         Component 2 =
          3-element $(sprint(show, Vector{Float64})):
           0.0
           1.0
           0.0
         ⋮
         Component 4 =
          2-element $(sprint(show, Vector{Float64})):
           3.0
           4.0
         Component 5 =
          2-element $(sprint(show, Vector{Float64})):
           5.0
           6.0"""
    end

    for T in types, reshaper in reshapers
        if reshaper == reshapers[2] && T != Vector{Float64}
            continue
        end
        shape_se = Manifolds.ShapeSpecification(reshaper, M1, M2)
        @testset "Type $T" begin
            pts_base = [
                convert(T, [1.0, 0.0, 0.0, 0.0, 0.0]),
                convert(T, [0.0, 1.0, 0.0, 1.0, 0.0]),
                convert(T, [0.0, 0.0, 1.0, 0.0, 0.1]),
            ]
            pts = map(p -> Manifolds.ProductArray(shape_se, p), pts_base)
            distr_M1 = Manifolds.uniform_distribution(M1, pts_base[1][1:3])
            distr_M2 = Manifolds.projected_distribution(
                M2,
                Distributions.MvNormal(zero(pts_base[1][4:5]), 1.0),
            )
            distr_tv_M1 = Manifolds.normal_tvector_distribution(M1, pts_base[1][1:3], 1.0)
            distr_tv_M2 = Manifolds.normal_tvector_distribution(M2, pts_base[1][4:5], 1.0)
            @test injectivity_radius(Mse, pts[1]) ≈ π
            @test injectivity_radius(Mse) ≈ π
            @test injectivity_radius(Mse, pts[1], ExponentialRetraction()) ≈ π
            @test injectivity_radius(Mse, ExponentialRetraction()) ≈ π
            test_manifold(
                Mse,
                pts;
                test_reverse_diff = isa(T, Vector),
                test_musical_isomorphisms = true,
                test_injectivity_radius = true,
                test_project_point = true,
                test_project_tangent = true,
                retraction_methods = retraction_methods,
                inverse_retraction_methods = inverse_retraction_methods,
                test_mutating_rand = isa(T, Vector),
                point_distributions = [Manifolds.ProductPointDistribution(
                    distr_M1,
                    distr_M2,
                )],
                tvector_distributions = [Manifolds.ProductFVectorDistribution(
                    distr_tv_M1,
                    distr_tv_M2,
                )],
                is_tangent_atol_multiplier = 1,
                exp_log_atol_multiplier = 1,
            )
        end
    end

    M3 = Manifolds.Rotations(2)
    Mser = ProductManifold(M1, M2, M3)
    shape_ser = Manifolds.ShapeSpecification(reshapers[1], M1, M2, M3)

    @testset "ShapeSpecification detailed reshapers" begin
        shape_ser_3_test = Manifolds.ShapeSpecification(
            (reshapers[1], reshapers[2], reshapers[1]),
            M1,
            M2,
            M3,
        )
        @test shape_ser_3_test.reshapers[1] == reshapers[1]
        @test shape_ser_3_test.reshapers[2] == reshapers[2]
        @test shape_ser_3_test.reshapers[3] == reshapers[1]
    end

    @testset "high-dimensional product manifold" begin
        Mhigh = Euclidean(100000)
        shape_high = Manifolds.ShapeSpecification(reshapers[2], M1, Mhigh)
        a = Manifolds.ProductArray(shape_high, collect(1.0:100003.0))
        b = a.parts[2] .^ 2
        @test b[1] == 16
    end

    @test submanifold(Mser, 2) == M2
    @test submanifold(Mser, Val((1, 3))) == M1 × M3
    @test submanifold(Mser, 2:3) == M2 × M3
    @test submanifold(Mser, [1, 3]) == M1 × M3
    @inferred submanifold(Mser, Val((1, 3)))

    # testing the slower generic constructor
    Mprod4 = ProductManifold(M2, M2, M2, M2)
    shape4 = Manifolds.ShapeSpecification(reshapers[1], Mprod4.manifolds...)
    data_x4 = rand(8)
    @test Manifolds.ProductArray(shape4, data_x4).data === data_x4

    pts_sphere = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]
    pts_r2 = [[0.0, 0.0], [1.0, 0.0], [0.0, 0.1]]
    angles = (0.0, π / 2, 2π / 3)
    pts_rot = [[cos(ϕ) sin(ϕ); -sin(ϕ) cos(ϕ)] for ϕ in angles]
    pts = [
        Manifolds.prod_point(shape_ser, p[1], p[2], p[3])
        for p in zip(pts_sphere, pts_r2, pts_rot)
    ]
    test_manifold(
        Mser,
        pts,
        test_injectivity_radius = false,
        test_forward_diff = false,
        test_reverse_diff = false,
        is_tangent_atol_multiplier = 1,
        exp_log_atol_multiplier = 1,
    )

    @testset "product vector transport" begin
        p = ProductRepr([1.0, 0.0, 0.0], [0.0, 0.0])
        q = ProductRepr([0.0, 1.0, 0.0], [2.0, 0.0])
        X = log(Mse, p, q)
        m = ProductVectorTransport(ParallelTransport(), ParallelTransport())
        Y = vector_transport_to(Mse, p, X, q, m)
        Z = -log(Mse, q, p)
        @test isapprox(Mse, q, Y, Z)
    end

    @testset "prod_point" begin
        shape_se = Manifolds.ShapeSpecification(reshapers[1], M1, M2)
        Ts = SizedVector{3,Float64}
        Tr2 = SizedVector{2,Float64}
        T = SizedVector{5,Float64}
        pts_base = [
            convert(T, [1.0, 0.0, 0.0, 0.0, 0.0]),
            convert(T, [0.0, 1.0, 0.0, 1.0, 0.0]),
            convert(T, [0.0, 0.0, 1.0, 0.0, 0.1]),
        ]
        pts = map(p -> Manifolds.ProductArray(shape_se, p), pts_base)
        pts_sphere = [
            convert(Ts, [1.0, 0.0, 0.0]),
            convert(Ts, [0.0, 1.0, 0.0]),
            convert(Ts, [0.0, 0.0, 1.0]),
        ]
        pts_r2 =
            [convert(Tr2, [0.0, 0.0]), convert(Tr2, [1.0, 0.0]), convert(Tr2, [0.0, 0.1])]
        pts_prod =
            [Manifolds.prod_point(shape_se, p[1], p[2]) for p in zip(pts_sphere, pts_r2)]
        for p in zip(pts, pts_prod)
            @test isapprox(Mse, p[1], p[2])
        end
        for p in zip(pts_sphere, pts_r2, pts_prod)
            @test isapprox(M1, p[1], submanifold_component(p[3], 1))
            @test isapprox(M2, p[2], submanifold_component(p[3], 2))
        end
        @test submanifold_component(Mse, pts[1], 1) === pts[1].parts[1]
        @test submanifold_component(Mse, pts[1], Val(1)) === pts[1].parts[1]
        @test submanifold_component(pts[1], 1) === pts[1].parts[1]
        @test submanifold_component(pts[1], Val(1)) === pts[1].parts[1]
        @test submanifold_components(Mse, pts[1]) === pts[1].parts
        @test submanifold_components(pts[1]) === pts[1].parts

        p_inf = parray(Mse, randn(5))
        @test (@inferred ManifoldsBase.allocate_result_type(
            Mse,
            Manifolds.log,
            (p_inf, p_inf),
        )) === Float64
        @test (@inferred number_eltype(typeof(p_inf))) === Float64
        @test pts_prod[1] .+ fill(1.0, 5) == pts_prod[1] .+ 1.0
    end

    @testset "ProductRepr" begin

        Ts = SizedVector{3,Float64}
        Tr2 = SizedVector{2,Float64}
        pts_sphere = [
            convert(Ts, [1.0, 0.0, 0.0]),
            convert(Ts, [0.0, 1.0, 0.0]),
            convert(Ts, [0.0, 0.0, 1.0]),
        ]
        pts_r2 =
            [convert(Tr2, [0.0, 0.0]), convert(Tr2, [1.0, 0.0]), convert(Tr2, [0.0, 0.1])]

        pts = [ProductRepr(p[1], p[2]) for p in zip(pts_sphere, pts_r2)]
        basis_types = (
            DefaultOrthonormalBasis(),
            ProjectedOrthonormalBasis(:svd),
            get_basis(Mse, pts[1], DefaultOrthonormalBasis()),
            DiagonalizingOrthonormalBasis(ProductRepr([0.0, 1.0, 0.0], [1.0, 0.0])),
        )

        @test (@inferred convert(
            ProductRepr{Tuple{T,Float64,T} where T},
            ProductRepr(9, 10, 11),
        )) == ProductRepr(9, 10.0, 11)

        test_manifold(
            Mse,
            pts,
            test_injectivity_radius = false,
            test_musical_isomorphisms = true,
            test_tangent_vector_broadcasting = false,
            test_forward_diff = false,
            test_reverse_diff = false,
            test_project_tangent = true,
            test_project_point = true,
            test_default_vector_transport = true,
            vector_transport_methods = [
                ProductVectorTransport(ParallelTransport(), ParallelTransport()),
                ProductVectorTransport(SchildsLadderTransport(), SchildsLadderTransport()),
                ProductVectorTransport(PoleLadderTransport(), PoleLadderTransport()),
            ],
            basis_types_vecs = (basis_types[1], basis_types[3], basis_types[4]),
            basis_types_to_from = basis_types,
            is_tangent_atol_multiplier = 1,
            exp_log_atol_multiplier = 1,
        )
        @test number_eltype(pts[1]) === Float64
        @test submanifold_component(Mse, pts[1], 1) === pts[1].parts[1]
        @test submanifold_component(Mse, pts[1], Val(1)) === pts[1].parts[1]
        @test submanifold_component(pts[1], 1) === pts[1].parts[1]
        @test submanifold_component(pts[1], Val(1)) === pts[1].parts[1]
        @test submanifold_components(Mse, pts[1]) === pts[1].parts
        @test submanifold_components(pts[1]) === pts[1].parts
        @test (@inferred ManifoldsBase._get_vector_cache_broadcast(pts[1])) === Val(false)
    end

    @testset "vee/hat" begin
        M1 = Rotations(3)
        M2 = Euclidean(3)
        M = M1 × M2
        reshaper = Manifolds.ArrayReshaper()
        shape_se = Manifolds.ShapeSpecification(reshaper, M1, M2)

        e = Matrix{Float64}(I, 3, 3)
        x = Manifolds.prod_point(
            shape_se,
            exp(M1, e, hat(M1, e, [1.0, 2.0, 3.0])),
            [1.0, 2.0, 3.0],
        )
        v = [0.1, 0.2, 0.3, -1.0, 2.0, -3.0]
        V = hat(M, x, v)
        v2 = vee(M, x, V)
        @test isapprox(v, v2)

        xr = ProductRepr(exp(M1, e, hat(M1, e, [1.0, 2.0, 3.0])), [1.0, 2.0, 3.0])

        Vr = hat(M, xr, v)
        v2r = vee(M, xr, V)
        @test isapprox(v, v2r)
    end

    @testset "Basis printing" begin
        p = ProductRepr([1.0, 0.0, 0.0], [1.0, 0.0])
        B = DefaultOrthonormalBasis()
        Bc = get_basis(Mse, p, B)
        Bc_components_s = sprint.(show, "text/plain", Bc.data.parts)
        @test sprint(show, "text/plain", Bc) == """
        DefaultOrthonormalBasis(ℝ) for a product manifold
        Basis for component 1:
        $(Bc_components_s[1])
        Basis for component 2:
        $(Bc_components_s[2])
        """
    end

    @testset "Basis-related errors" begin
        a = ProductRepr([1.0, 0.0, 0.0], [0.0, 0.0])
        @test_throws ErrorException get_vector!(
            Mse,
            a,
            ProductRepr([1.0, 0.0, 0.0], [0.0, 0.0]),
            [1.0, 2.0, 3.0, 4.0, 5.0],
            CachedBasis(DefaultOrthonormalBasis(), []),
        )
    end
end


text:
# Based on https://www.ffmpeg.org/doxygen/trunk/encode_video_8c-example.html
#=
 * Copyright (c) 2001 Fabrice Bellard
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
 * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */
=#

using VideoIO, Printf

"""
encode(enc_ctx::Ptr{VideoIO.AVCodecContext}, frame, pktptr::Ptr{VideoIO.AVPacket}, io::IO)

Append and encode frame to output.
"""
function encode(enc_ctx::Ptr{VideoIO.AVCodecContext},
    frame, pktptr::Ptr{VideoIO.AVPacket}, io::IO)
    ret = VideoIO.avcodec_send_frame(enc_ctx, frame)
    if ret < 0
        error("Error $ret sending a frame for encoding")
    end

    while ret >= 0
        ret = VideoIO.avcodec_receive_packet(enc_ctx, pktptr)
        if (ret == -35 || ret == -541478725) # -35=EAGAIN -541478725=AVERROR_EOF
             return
        elseif (ret < 0)
            error("Error $ret during encoding")
        end
        pkt = unsafe_load(pktptr)
        println("Write packet $(pkt.pts) (size=$(pkt.size))")
        data = unsafe_wrap(Array,pkt.data,pkt.size)
        write(io,data)
        VideoIO.av_packet_unref(pktptr)
    end
end

filename = "video"
codec_name = "libx264"
framerate = 24

endcode = UInt8[0, 0, 1, 0xb7]

codec = VideoIO.avcodec_find_encoder_by_name(codec_name)
if codec == C_NULL
    error("Codec '$codec_name' not found")
end

c = Ptr{VideoIO.AVCodecContext}[VideoIO.avcodec_alloc_context3(codec)]
if c == [C_NULL]
    error("Could not allocate video codec context")
end

pktptr = Ptr{VideoIO.AVPacket}[VideoIO.av_packet_alloc()]
if pktptr == [C_NULL]
    error("av_packet_alloc() error")
end

codecContext = unsafe_load(c[1])

#resolution must be a multiple of two
codecContext.width = 352
codecContext.height = 288
# frames per second
codecContext.time_base = VideoIO.AVRational(1, framerate)
codecContext.framerate = VideoIO.AVRational(framerate, 1)
codecContext.pix_fmt = VideoIO.AV_PIX_FMT_YUV420P

unsafe_store!(c[1], codecContext)

codec_loaded = unsafe_load(codec)
if codec_loaded.id == VideoIO.AV_CODEC_ID_H264
    VideoIO.av_opt_set(codecContext.priv_data, "crf", "0", VideoIO.AV_OPT_SEARCH_CHILDREN)
    VideoIO.av_opt_set(codecContext.priv_data, "preset", "veryslow", VideoIO.AV_OPT_SEARCH_CHILDREN)
else
    ## PARAMETERS FROM ORIGINAL EXAMPLE
    # put sample parameters
    codecContext.bit_rate = 400000
    #= emit one intra frame every ten frames
     * check frame pict_type before passing frame
     * to encoder, if frame->pict_type is AV_PICTURE_TYPE_I
     * then gop_size is ignored and the output of encoder
     * will always be I frame irrespective to gop_size
    =#
    codecContext.gop_size = 10        #####
    codecContext.max_b_frames = 1
end

# open it
ret = VideoIO.avcodec_open2(c[1], codec, C_NULL)
if ret < 0
    error("Could not open codec: $(av_err2str(ret))")
end
f = open(string(filename,".h264"),"w")
frameptr = Ptr{VideoIO.AVFrame}[VideoIO.av_frame_alloc()]
if frameptr == [C_NULL]
    error("Could not allocate video frame")
end
frame = unsafe_load(frameptr[1])
frame.format = codecContext.pix_fmt
frame.width  = codecContext.width
frame.height = codecContext.height
unsafe_store!(frameptr[1],frame)

ret = VideoIO.av_frame_get_buffer(frameptr[1], 32)
if ret < 0
    error("Could not allocate the video frame data")
end

# frame_fields = map(x->fieldname(VideoIO.AVUtil.AVFrame,x),1:fieldcount(VideoIO.AVUtil.AVFrame))
# pos_data = findfirst(fields.==:data)

for i = 0:240
    flush(stdout)

    ret = VideoIO.av_frame_make_writable(frameptr[1])
    if ret < 0
        error("av_frame_make_writable() error")
    end

    frame = unsafe_load(frameptr[1]) #grab data from c memory
    Y = rand(UInt8,frame.width,frame.height)
    Cb = rand(UInt8,Int64(frame.width/2),Int64(frame.height/2))
    Cr = rand(UInt8,Int64(frame.width/2),Int64(frame.height/2))
    for y = 1:frame.height
        for x = 1:frame.width
            unsafe_store!(frame.data[1],rand(UInt8),((y-1)*frame.linesize[1])+x)
        end
    end
    for y = 1:Int64(frame.height/2)
        for x = 1:Int64(frame.width/2)
            unsafe_store!(frame.data[2],Cb[x,y],((y-1)*frame.linesize[2])+x)
            unsafe_store!(frame.data[3],Cr[x,y],((y-1)*frame.linesize[3])+x)
        end
    end

    frame.pts = i
    unsafe_store!(frameptr[1],frame) #pass data back to c memory

    encode(c[1], frameptr[1], pktptr[1], f)
    println(i)
end

# flush the encoder
encode(c[1], C_NULL, pktptr[1], f);
# add sequence end code to have a real MPEG file
write(f,endcode)
close(f)

VideoIO.avcodec_free_context(c)
VideoIO.av_frame_free(frameptr)
VideoIO.av_packet_free(pktptr)

overwrite = true
ow = overwrite ? `-y` : `-n`

muxout = VideoIO.collectexecoutput(`$(VideoIO.ffmpeg) $ow -framerate $framerate -i $filename.h264 -c copy $filename.mp4`)

filter!(x->!occursin.("Timestamps are unset in a packet for stream 0.",x),muxout)
if occursin("ffmpeg version ",muxout[1]) && occursin("video:",muxout[end])
    rm("$filename.h264")
    @info "Video file saved: $(pwd())/$filename.mp4"
    @info muxout[end-1]
    @info muxout[end]
    return
else
    rm("$filename.h264")
    @warn "Stream Muxing may have failed: $(pwd())/$filename.h264 into $(pwd())/$filename.mp4"
    println.(muxout)
end