text:
module ProjectAssigner

using CSV
using DataFrames
using GLPK
using JuMP

mutable struct Group
    members::Set{String}
    skills::Dict{String,Float64}
    preferences::Vector{String}
end

struct IndexModel
    c::Matrix{Float64}            # c[i, j] is the cost for assigning group i to project j
    r::Matrix{Float64}            # r[k, i] is the amount of skill k that group i contributes
    s::Vector{Float64}            # s[i] is the size of group i
    minskills::Matrix{Float64}    # minskills[k, j] is the minimum number of people needed for skill k on project j
    minsizes::Vector{Float64}     # minimum sizes for each project
    maxsizes::Vector{Float64}     # maximum sizes for each project
    force::Vector{Pair{Int, Int}} # i => j means we are forcing group i to be assigned to project j
end

function IndexModel(groups::Vector{Group}, projects::DataFrame; force=[])
    skills = map(n->n[findfirst(':', n)+1:end], filter(n->startswith(n, "skill:"), names(projects)))
    skillinds = Dict(n=>k for (k, n) in enumerate(skills))
    pinds = Dict(p=>j for (j, p) in enumerate(projects[!, :name]))

    c = calculate_costs(groups, pinds)

    r = zeros(length(skillinds), length(groups))
    for (i, g) in enumerate(groups)
        for skill in keys(g.skills)
            r[skillinds[skill], i] = g.skills[skill]
        end
    end

    s = [length(g.members) for g in groups]

    minskills = zeros(length(skillinds), length(pinds))
    minsizes = zeros(nrow(projects))
    maxsizes = zeros(nrow(projects))
    for (j, row) in enumerate(Tables.rows(projects))
        minsizes[j] = row[:min]
        maxsizes[j] = row[:max]
        for skill in keys(skillinds)
            minskills[skillinds[skill], j] = row["skill:"*skill]
        end
    end

    # map from student name to group index
    ginds = Dict(Iterators.flatten((m=>i for m in groups[i].members) for i in 1:length(groups)))
    force_indices = map(force) do pair
        ginds[first(pair)] => pinds[last(pair)]
    end

    return IndexModel(c, r, s, minskills, minsizes, maxsizes, force_indices)
end

"""
    ProjectAssigner.match(;students, projects, [output], [force], [optimizer])

Match students to projects and return a dataframe with the assignments.

All arguments are keyword arguments, and the function is not exported, so it must be called with the module explicitly `ProjectAssigner.match(...)`.

# Example
```julia-repl
julia> ProjectAssigner.match(students="students.csv", projects="projects.csv")
```

# Keyword Arguments

- `students`: A `DataFrame` or csv filename `String` specifying information about the students. See the Data Format section of the documentation for more information.
- `projects`: A `DataFrame` or csv filename `String` specifying information about the projects. See the Data Format section of the documentation for more information.
- `output::String`: optional name for a csv file that the assignments will be written to.
- `force`: optional vector of student-project pairs to force matches for, e.g. `force=["Bob"=>"Airplane"]` will force student Bob to be assigned to the Airplane project.
- `optimizer::Function`: optimizer factory to use for the optimization, e.g. `optimizer=Gurobi.Optimizer` will use Gurobi instead of the default `GLPK`

!!! warning "Scaling Issues"
    Due to the exponential costs in the objective, the optimization problem may be poorly-scaled for large classes. This can result in the default GLPK optimizer producing poor results even though it thinks that it has solved the problem. More powerful commercial solvers such as [Gurobi](https://github.com/jump-dev/Gurobi.jl) can handle this much better.
"""
function match(;students, projects,
                output=nothing,
                force::AbstractVector{Pair{String,String}}=Pair{String,String}[],
                optimizer=GLPK.Optimizer
    )

    students = as_dataframe(students)
    projects = as_dataframe(projects)

    lint_input(students, projects)

    @info("Found $(nrow(students)) students and $(nrow(projects)) projects.", students, projects)

    groups = group(students)

    @debug(groups)

    assignments = match_groups(groups, projects, force=force, optimizer=optimizer)

    out = DataFrame(name=String[], project=String[], preference=Union{Int,Missing}[])
    for (i, g) in enumerate(groups)
        project = assignments[i]
        pref = something(findfirst(==(project), g.preferences), missing)
        for name in g.members
            push!(out, (name=name, project=assignments[i], preference=pref))
        end
    end
    @info("Assignments:", out)
    histogram = DataFrame(preference=[], number=Int[])
    for r in 1:maximum(skipmissing(out[!, :preference]))
        push!(histogram, (preference=r, number=sum(skipmissing(out[!, :preference].==r))))
    end
    push!(histogram, (preference=missing, number=sum(ismissing, out[!, :preference])))
    @info("Frequencies by preference rank:", histogram)

    if !isnothing(output)
        @info("Writing to $output.")
        CSV.write(output, out)
    end

    return out
end

function match_groups(groups, projects; force=[], optimizer=GLPK.Optimizer)
    m = IndexModel(groups, projects, force=force)

    if (maximum(m.c) + minimum(m.c)) == maximum(m.c) && optimizer == GLPK.Optimizer
        @error("""This problem is too large for GLPK.

               The cost coefficients have too large a range, and the solution will likely be suboptimal.

               Recommend switching to the Gurobi optimizer with
               ProjectAssigner.match(..., optimizer=Gurobi.Optimizer).""", extrema(m.c))
    end

    opt = create_jump_model(m)
    set_optimizer(opt, optimizer)
    optimize!(opt)

    if termination_status(opt) != MOI.OPTIMAL
        @error(termination_status(opt))
    end
    assignments = String[]
    x_opt = value.(opt[:x])
    for i in 1:length(groups)
        j = only(findall(x_opt[i,:] .> 0))
        push!(assignments, projects[j, :name])
    end
    return assignments
end

as_dataframe(fname::AbstractString) = DataFrame(CSV.File(fname))
as_dataframe(df::DataFrame) = df

function group(students::DataFrame)
    skills = String[]
    teammate_keys = String[]
    project_names = String[]
    for n in names(students)
        if startswith(n, "skill:")
            push!(skills, n[findfirst(':', n)+1:end])
        elseif startswith(n, "teammate_")
            push!(teammate_keys, n)
        elseif n != "name"
            push!(project_names, n)
        end
    end

    # extract partners
    partners = Dict{String, Set{String}}()
    for row in Tables.rows(students)
        partners[row[:name]] = Set(skipmissing(row[k] for k in teammate_keys))
    end

    # validate groups
    groups = Group[]
    groupmap = Dict{String, Int}()
    for (k,v) in partners
        if k in keys(groupmap)
            continue
        else
            candidate_group = union!(Set((k,)), v)
            valid = true
            for s in candidate_group
                s_candidate_group = union!(Set((s,)), partners[s])
                if s_candidate_group != candidate_group
                    valid = false
                end
            end
            if valid
                g = Group(candidate_group, Dict(skill=>0.0 for skill in skills), [])
                push!(groups, g)
                for s in candidate_group
                    groupmap[s] = length(groups)
                end
                if length(candidate_group) > 1
                    @info("Creating group $(collect(candidate_group))")
                end
            else
                @warn("""
                      Invalid teammate request for $k
                      """, Dict(s=>collect(partners[s]) for s in candidate_group))
                g = Group(Set((k,)), Dict(skill=>0.0 for skill in skills), [])
                push!(groups, g)
                groupmap[k] = length(groups)
            end
        end
    end

    # fill data in groups
    for row in Tables.rows(students)
        g = groups[groupmap[row[:name]]]
        for skill in skills
            g.skills[skill] += row["skill:"*skill]
        end
        prefs = sort(filter(p->!ismissing(row[p]),project_names), by=p->row[p])
        if !isempty(g.preferences) && prefs != g.preferences
            @warn("Preferences for $(collect(g.members)) don't match.", prefs, g.preferences)
        end
        if length(prefs) > length(g.preferences)
            g.preferences = prefs
        end
    end

    return groups
end

function create_jump_model(m::IndexModel)
    n_groups, n_projects = size(m.c)

    opt = Model()

    # x[i, j] = 1 indicates that group i is in project j
    @variable(opt, x[1:n_groups, 1:n_projects], Bin)

    @objective(opt, Min, sum(m.c.*x))

    for i in 1:n_groups
        @constraint(opt, sum(x[i, :]) == 1)
    end

    for j in 1:n_projects
        # skills
        pr = m.r*x[:, j]
        @constraint(opt, pr .>= m.minskills[:, j])
        # sizes
        ps = m.s'*x[:, j]
        @constraint(opt, m.minsizes[j] <= ps <= m.maxsizes[j])
    end

    for pair in m.force
        @constraint(opt, x[pair...] == 1)
    end

    return opt
end

function calculate_costs(groups, pinds)
    # number of students
    n = sum(length(g.members) for g in groups)

    # number of projects
    m = length(pinds)

    # constant for calculating costs
    logn = ceil(Int, log2(n))

    # c[i, j] is the cost of connecting group i to project j
    c = zeros(length(groups), m)

    for (i, g) in enumerate(groups)
        # fill in preferenced
        for (r, p) in enumerate(g.preferences)
            c[i, pinds[p]] = 2.0^(logn*(r-div(m,2)))*length(g.members)
        end

        # fill in unpreferenced
        if length(g.preferences) < length(pinds)
            r = length(g.preferences)+1
            missing_cost = 2.0^(logn*(r-div(m,2)))*length(g.members)
            row = view(c, i, :)
            row[row.==0.0] .= missing_cost
        end
    end

    return c
end

function lint_input(students, projects)
    # students table
    if !("name" in names(students))
        @error("Required column 'name' missing from students table.", names(students))
    end

    student_skills = Set{String}()
    teammate_keys = String[]
    project_names = Set{String}()
    for n in names(students)
        if startswith(n, "skill:")
            push!(student_skills, n[findfirst(':', n)+1:end])
        elseif startswith(n, "teammate_")
            push!(teammate_keys, n)
        elseif n != "name"
            push!(project_names, n)
        end
    end

    student_names = Set(students[!, :name])
    for r in Tables.rows(students)
        for t in teammate_keys
            if !ismissing(r[t]) && !(r[t] in student_names)
                @error("Teammate requrest not in list of student names.", request=r[t], requester=r[:name])
            end
        end
    end

    # projects table
    for c in ["name", "min", "max"]
        if !(c in names(projects))
            @error("Required column '$c' missing from projects table.", names(projects))
        end
    end

    for c in ["min", "max"]
        if !(eltype(projects[!, c]) <: Integer)
            @error("The type of column '$c' in the projects table was not Integer; instead it was $(eltype(projects[!,c])). Check for extraneous space or other characters.")
        end
    end

    project_skills = Set{String}()
    for n in names(projects)
        if startswith(n, "skill:")
            push!(project_skills, n[findfirst(':', n)+1:end])
        end
    end

    # combination
    if student_skills != project_skills
        @error("Project and student skill columns do not match!", project_skills, student_skills)
    end

    if Set(projects[!,:name]) != project_names
        @error("Project columns in students table and names in project table do not match.", projects[!,:name], project_names)
    end
end

end


text:
# Writer
# ======

"""
    FASTQWriter(output::IO; quality_header=false, quality_encoding=:sanger)

Create a data writer of the FASTQ file format.

# Arguments
* `output`: data sink
* `quality_header=false`: output the title line at the third line just after '+'
* `quality_encoding=:sanger`: encoding of base qualities; see `FASTQReader` for available values
"""
type FASTQWriter{T<:IO} <: Bio.IO.AbstractWriter
    output::T

    # write sequence name and description header at the third line
    quality_header::Bool

    # quality encoding used for writing
    quality_encoding::QualityEncoding
end

function Bio.IO.stream(writer::FASTQWriter)
    return writer.output
end

function FASTQWriter(output::IO;
                     quality_header::Bool=false,
                     quality_encoding::Symbol=:sanger)
    return FASTQWriter(output, quality_header, sym2qualenc(quality_encoding))
end

function Base.write(writer::FASTQWriter, seqrec::FASTQSeqRecord)
    output = writer.output
    n = 0

    # header
    n += write(output, '@', seqrec.name)
    if !isempty(seqrec.metadata.description)
        n += write(output, ' ', seqrec.metadata.description)
    end
    n += write(output, '\n')

    # sequence
    for x in seqrec.seq
        n += write(output, Char(x))
    end
    n += write(output, '\n')

    # (optional) identifier and description
    n += write(output, '+')
    if writer.quality_header
        n += write(output, seqrec.name)
        if !isempty(seqrec.metadata.description)
            n += write(output, ' ', seqrec.metadata.description)
        end
    end
    n += write(output, '\n')

    for q in seqrec.metadata.quality
        n += write(output, Char(q + ascii_offset(writer.quality_encoding)))
    end
    n += write(output, '\n')

    return n
end

ext: jl


text:
using Base.Test
using Meganet

for TYPE = [Float64,Float32]
    K = getDenseKernel(TYPE,[24;14])

    @testset  "adjoint test $TYPE" begin
    theta = initTheta(K)
    A     = getOp(K,theta);
    v     = randn(TYPE,nFeatIn(K))
    w     = randn(TYPE,nFeatOut(K))

    t1    = dot(w,A*v)
    t2    = dot(v,A'*w)
    # println("adjointTest t1=$t1\t t2=$t2")
    @test norm(t1-t2)/norm(t1) < 1e3*eps(TYPE)
    end

    @testset "derivative Test $TYPE" begin
    th  = initTheta(K);
    dth = initTheta(K);
    nex = 1;
    Y  = randn(TYPE,nFeatIn(K),nex)+nex;
    Z  = randn(TYPE,nFeatOut(K),nex)-nex;

    t1 = vec(Z)'*vec(Jthetamv(K,dth,th,Y));
    t2 = vec(dth)'*vec(JthetaTmv(K,Z,th,Y));
    # println("derivativeTest t1=$t1\t t2=$t2")
    @test norm(t1-t2)/norm(t2) < 1e3*eps(TYPE)
    end
end


text:
# # Result Interface
# This file describes the methods that should or can be implemented
# for subtypes of `AbstractResult`.

# First of all, we make it so that any `AbstractResult` is broadcasted wholly:
Broadcast.broadcastable( res :: AbstractResult ) = Ref(res)

# ## Mandatory Methods

# The following getter methods should be implemented:
"""
    get_site( res :: AbstractResult{XT,YT} )

Return evaluation site of type `XT` associated with `res`.
"""
function get_site( :: AbstractResult{XT,YT} ) :: XT where {XT,YT}
	return XT()
end

"""
    get_value( res :: AbstractResult{XT,YT} )

Return evaluation value vector of type `YT` associated with `res`.
"""
function get_value( :: AbstractResult{XT,YT} ) :: YT where {XT,YT}
	return YT()
end

"""
	get_id( res :: AbstractResult ) :: Int

Return the `id` of a result such that for the database `db`
containing `res` it holds that `get_result(db, id) == res`.
"""
get_id( :: AbstractResult ) :: Int = -1

# Also define these setters:
set_site!(r :: AbstractResult, x) :: Nothing = nothing
set_value!(r :: AbstractResult, y) :: Nothing = nothing

# An `AbstractResult` is created with the `init_res` constructor taking
# an evaluation site `x`, a value vector `y` and a database id.
"""
    init_res( res_type, x, y, id)

Return of result of type `res_type` with site `x`, value `y` and database id `id`.
"""
function init_res( :: Type{<:AbstractResult}, :: Vec, :: Vec, :: Int )
	return nothing
end

# ## Derived Methods

# Based on the above definitions, there are some useful defaults defined.
# These can be overwritten for specific types but you don't have to:

"""
	_equal_vals( r1 :: AbstractResult, r2 :: AbstractResult )

Return `true` if both the site and the value vectors of `r1` and `r2` are equal.
"""
function _equal_vals( r1 :: AbstractResult, r2 :: AbstractResult )
	return get_site(r1) == get_site(r2) && get_value(r1) == get_value(r2)
end

"""
    has_valid_site( r :: AbstractResult )

Return `true` if the site vector of `r` is neither empty nor NaN.
"""
function has_valid_site( r :: AbstractResult )
	site = get_site(r)
	return !(isempty(site) || any( isnan.(site) ))
end

"""
    has_valid_value( r :: AbstractResult )

Return `true` if the value vector of `r` is neither empty nor NaN.
"""
function has_valid_value( r :: AbstractResult )
	value = get_value(r)
	isempty(value) && return false
	any( isnan.(value) ) && return false
	return true
end


text:
abstract type AbstractCR3BPIndirectParams end

# CR3BP Indirect parameters
mutable struct CR3BPIndirectParams <: AbstractCR3BPIndirectParams
    # Spacecraft parameters
    sp::SimpleSpacecraft

    # CR3BP parameters
    crp::CR3BPParams

    # Continuation variable
    ϵ::Float64

    # Thrust type
    utype::Int64

    function CR3BPIndirectParams(sp::Spacecraft, crp::CR3BPParams, ϵ)
        new(sp, crp, ϵ, 0)
    end
end

# CR3BP CoState Dynamics
function cr3bpCostateEom(u::AbstractArray, p::AbstractCR3BPIndirectParams, γ, homotopyFlag::MEMF)
    @inbounds begin
        # Get requirements
        TU  = p.crp.TU
        LU  = p.crp.LU
        MU  = p.crp.MU
        μ   = p.crp.μ

        # Scale requirements
        tMaxSc = p.sp.tMax * TU * TU / (MU*LU*1000.0)

        # Compute Requirements
        xpmu    = u[1] + μ
        xpmum1  = u[1] + μ - 1
        r1      = sqrt(xpmu*xpmu + u[2]*u[2] + u[3]*u[3])
        r2      = sqrt(xpmum1*xpmum1 + u[2]*u[2] + u[3]*u[3])
        invr13  = r1^(-3)
        invr15  = r1^(-5)
        invr23  = r2^(-3)
        invr25  = r2^(-5)
        λv      = sqrt(u[11]^2 + u[12]^2 + u[13]^2)

        # Compute G
        G11     = 1 - (1 - μ)*invr13 + 3*(1 - μ)*(u[1] + μ)^2*invr15 -
                    μ*invr23 + 3*μ*(u[1] + μ - 1)^2*invr25;
        G22     = 1 - (1 - μ)*invr13 + 3*(1 - μ)*u[2]^2*invr15 -
                    μ*invr23 + 3*μ*u[2]^2*invr25;
        G33     = -(1 - μ)*invr13 + 3*(1 - μ)*u[3]^2*invr15 -
                    μ*invr23 + 3*μ*u[3]^2*invr25;
        G12     = 3*(1 - μ)*(u[1] + μ)*u[2]*invr15 +
                    3*μ*(u[1] + μ - 1)*u[2]*invr25;
        G13     = 3*(1 - μ)*(u[1] + μ)*u[3]*invr15 +
                    3*μ*(u[1] + μ - 1)*u[3]*invr25;
        G23     = 3*(1 - μ)*u[2]*u[3]*invr15 +
                    3*μ*u[2]*u[3]*invr25;

        # Compute and return Dynamics
        dλ = @SVector [ -G11*u[11] - G12*u[12] - G13*u[13],
                        -G12*u[11] - G22*u[12] - G23*u[13],
                        -G13*u[11] - G23*u[12] - G33*u[13],
                        -u[8] + 2.0*u[12],
                        -u[9] - 2.0*u[11],
                        -u[10],
                        -λv*γ*tMaxSc / (u[7]*u[7])]
    end

    return dλ
end

function cr3bpCostateEom!(dλ::AbstractArray, u::AbstractArray,
                          p::AbstractCR3BPIndirectParams, γ,
                          homotopyFlag::MEMF)
    @inbounds begin
    # Get requirements
    TU  = p.crp.TU
    LU  = p.crp.LU
    MU  = p.crp.MU
    μ   = p.crp.μ

    # Scale requirements
    tMaxSc = p.sp.tMax * TU^2 / (MU*LU*1000.0)

    # Compute Requirements
    r1      = sqrt((u[1] + μ)^2 + u[2]^2 + u[3]^2)
    r2      = sqrt((u[1] + μ - 1)^2 + u[2]^2 + u[3]^2)
    invr13  = r1^(-3)
    invr15  = r1^(-5)
    invr23  = r2^(-3)
    invr25  = r2^(-5)
    λv      = sqrt(u[11]^2 + u[12]^2 + u[13]^2)

    # Compute G
    G11     = 1 - (1 - μ)*invr13 + 3*(1 - μ)*(u[1] + μ)^2*invr15 -
                μ*invr23 + 3*μ*(u[1] + μ - 1)^2*invr25;
    G22     = 1 - (1 - μ)*invr13 + 3*(1 - μ)*u[2]^2*invr15 -
                μ*invr23 + 3*μ*u[2]^2*invr25;
    G33     = -(1 - μ)*invr13 + 3*(1 - μ)*u[3]^2*invr15 -
                μ*invr23 + 3*μ*u[3]^2*invr25;
    G12     = 3*(1 - μ)*(u[1] + μ)*u[2]*invr15 +
                3*μ*(u[1] + μ - 1)*u[2]*invr25;
    G13     = 3*(1 - μ)*(u[1] + μ)*u[3]*invr15 +
                3*μ*(u[1] + μ - 1)*u[3]*invr25;
    G23     = 3*(1 - μ)*u[2]*u[3]*invr15 +
                3*μ*u[2]*u[3]*invr25;

    # Compute and return Dynamics
    dλ[1] = -G11*u[11] - G12*u[12] - G13*u[13]
    dλ[2] = -G12*u[11] - G22*u[12] - G23*u[13]
    dλ[3] = -G13*u[11] - G23*u[12] - G33*u[13]
    dλ[4] = -u[8] + 2.0*u[12]
    dλ[5] = -u[9] - 2.0*u[11]
    dλ[6] = -u[10]
    dλ[7] = -λv*γ*tMaxSc / (u[7]^2)
    end

    return @SVector [G11, G22, G33, G12, G13, G23]
end

# CR3BP Indirect EOMs
function cr3bpEomIndirect(u::AbstractVector, p::AbstractCR3BPIndirectParams, t, homotopyFlag::MEMF)
    @inbounds begin

    # Get requirements
    TU  = p.crp.TU
    LU  = p.crp.LU
    VU  = p.crp.VU
    MU  = p.crp.MU
    isp = p.sp.isp
    c   = p.sp.c

    # Scale requirements
    tMaxSc = p.sp.tMax * TU * TU / (MU*LU*1000.0)
    cSc = c*TU / (LU*1000.0)

    # Compute thrust direction
    λv = sqrt(u[11]*u[11] + u[12]*u[12] + u[13]*u[13])
    invλv = 1.0 / λv
    α = @SVector [-u[11]*invλv, -u[12]*invλv, -u[13]*invλv]

    # Compute throttline
    S = computeS(u, λv, cSc)
    γ = computeU(S, p.utype, p.ϵ)

    # Compute Thrust Acceleration
    atMag = γ*tMaxSc / u[7]
    at = @SVector [α[1]*atMag,
                   α[2]*atMag,
                   α[3]*atMag]

    # Derivatives
    dx = cr3bpEomControl(u,p.crp,t,at)
    dλ = cr3bpCostateEom(u,p, γ, homotopyFlag)
    du = @SVector [dx[1], dx[2], dx[3], dx[4], dx[5], dx[6], -γ*tMaxSc / cSc,
                   dλ[1], dλ[2], dλ[3], dλ[4], dλ[5], dλ[6], dλ[7]]

    return du
    end
end

function cr3bpEomIndirect!(du::AbstractVector, u::AbstractVector,
                           p::AbstractCR3BPIndirectParams, t, homotopyFlag::MEMF)
    @inbounds begin
    # Get requirements
    TU  = p.crp.TU
    LU  = p.crp.LU
    VU  = p.crp.VU
    MU  = p.crp.MU
    isp = p.sp.isp
    c   = p.sp.c

    # Scale requirements
    tMaxSc = p.sp.tMax * TU^2 / (MU*LU*1000.0)
    cSc = c*TU / (LU*1000.0)

    # Compute thrust direction
    λv = sqrt(u[11]^2 + u[12]^2 + u[13]^2)
    invλv = 1.0 / λv
    α = @SVector [-u[11]*invλv, -u[12]*invλv, -u[13]*invλv]

    # Compute throttline
    S = computeS(u, λv, cSc)
    γ = computeU(S, p.utype, p.ϵ)

    # Compute Thrust Acceleration
    atMag = γ*tMaxSc / u[7]
    at = @SVector [α[1]*atMag,
                   α[2]*atMag,
                   α[3]*atMag]

    # Compute dynamics
    cr3bpEomControl!(view(du,1:6), u, p.crp, t, at)
    du[7] = -γ*tMaxSc / cSc
    GVec  = cr3bpCostateEom!(view(du,8:14), u, p, γ, homotopyFlag)
    end
    return GVec
end

function cr3bpEomIndirectIntegralCost(u::AbstractVector, p::AbstractCR3BPIndirectParams, t, homotopyFlag::MEMF)
    @inbounds begin

    # Get requirements
    TU  = p.crp.TU
    LU  = p.crp.LU
    VU  = p.crp.VU
    MU  = p.crp.MU
    isp = p.sp.isp
    c   = p.sp.c

    # Scale requirements
    tMaxSc = p.sp.tMax * TU * TU / (MU*LU*1000.0)
    cSc = c*TU / (LU*1000.0)

    # Compute thrust direction
    λv = sqrt(u[11]*u[11] + u[12]*u[12] + u[13]*u[13])
    invλv = 1.0 / λv
    α = @SVector [-u[11]*invλv, -u[12]*invλv, -u[13]*invλv]

    # Compute throttline
    S = computeS(u, λv, cSc)
    γ = computeU(S, p.utype, p.ϵ)

    # Compute Thrust Acceleration
    atMag = γ*tMaxSc / u[7]
    at = @SVector [α[1]*atMag,
                   α[2]*atMag,
                   α[3]*atMag]

    # Derivatives
    dx      = cr3bpEomControl(u,p.crp,t,at)
    dλ      = cr3bpCostateEom(u,p, γ, homotopyFlag)
    usqr    = γ - p.ϵ*γ*(1.0 - γ)
    du = @SVector [dx[1], dx[2], dx[3], dx[4], dx[5], dx[6], -γ*tMaxSc / cSc,
                   dλ[1], dλ[2], dλ[3], dλ[4], dλ[5], dλ[6], dλ[7], usqr]

    return du
    end
end

function cr3bpEomIndirectIntegralCost!(du::AbstractArray, u::AbstractArray, p::AbstractCR3BPIndirectParams, t, homotopyFlag::MEMF)
    @inbounds begin
    # Get requirements
    TU  = p.crp.TU
    LU  = p.crp.LU
    VU  = p.crp.VU
    MU  = p.crp.MU
    isp = p.sp.isp
    c   = p.sp.c

    # Scale requirements
    tMaxSc = p.sp.tMax * TU^2 / (MU*LU*1000.0)
    cSc = c*TU / (LU*1000.0)

    # Compute thrust direction
    λv = sqrt(u[11]^2 + u[12]^2 + u[13]^2)
    invλv = 1.0 / λv
    α = @SVector [-u[11]*invλv, -u[12]*invλv, -u[13]*invλv]

    # Compute throttline
    S = computeS(u, λv, cSc)
    γ = computeU(S, p.utype, p.ϵ)

    # Compute Thrust Acceleration
    atMag = γ*tMaxSc / u[7]
    at = @SVector [α[1]*atMag,
                   α[2]*atMag,
                   α[3]*atMag]

    # Compute dynamics
    cr3bpEomControl!(view(du,1:6), u, p.crp, t, at)
    du[7]   = -γ*tMaxSc / cSc
    GVec    = cr3bpCostateEom!(view(du,8:14), u, p, γ, homotopyFlag)
    du[15]  = γ - p.ϵ*γ*(1.0 - γ)
    end
    return GVec
end

# Utility Functions
function computeS(x::AbstractVector, λv, cSc)
    @inbounds begin
        return -λv*cSc / x[7] - x[14] + 1.0
    end
end

function computeU(S, utype, ϵ)
    if utype == 2
        return 1.0
    elseif utype == 0
        return 0.0
    else
        return (ϵ - S) / (2.0*ϵ)
    end
end


text:
"Test the null of normality using the Jarque-Bera test statistic."
function jbtest(x::Vector)
  n = length(x)
  m1 = sum(x)/n
  m2 = sum((x .- m1).^2)/n
  m3 = sum((x .- m1).^3)/n
  m4 = sum((x .- m1).^4)/n
  b1 = (m3/m2^(3/2))^2
  b2 = (m4/m2^2)
  statistic = n * b1/6 + n*(b2 - 3)^2/24
  d = Chisq(2.)
  pvalue = 1. - cdf(d, statistic)
  statistic, pvalue
end


text:
using DifferentiableStateSpaceModels, HMCExamples, Symbolics
overwrite_model_cache = true
model_cache_location = joinpath(pkgdir(HMCExamples),"src/generated_models")

include(joinpath(pkgdir(HMCExamples),"deps/rbc.jl"))

# Save off the rbc_observables_benchmark example
H, mod_vals, _ = rbc()
model_name = "rbc"
@info "RBC parameters" mod_vals.p

make_perturbation_model(H; model_name = "rbc", model_cache_location, overwrite_model_cache, verbose = true, mod_vals...)

include(joinpath(pkgdir(HMCExamples),"deps/FVGQ20.jl"))

# The FVGQ20 is in this repo
H, mod_vals, _ = FVGQ20() # in this repo,
model_name = "FVGQ20"
@info "FVGQ20 parameters" mod_vals.p
make_perturbation_model(H; model_name = "FVGQ20", model_cache_location, overwrite_model_cache, verbose = true, mod_vals...)

max_stars_repo_name: HighDimensionalEconLab/HMCExamples

max_issues_repo_name: HighDimensionalEconLab/HMCExamples

max_forks_repo_name: HighDimensionalEconLab/HMCExamples


text:
import QuantumOptics
const qo = QuantumOptics
using Test, IonSim
using Suppressor

@suppress_err begin

# setup system
C = Ca40(["S-1/2", "D-1/2"])
chain = LinearChain(
    ions=[C, C], com_frequencies=(x=2,y=2,z=1), vibrational_modes=(x=[1], y=[], z=[1])
)
T = Trap(configuration=chain)
modes = get_vibrational_modes(chain)

@testset "operators -- VibrationalMode operators" begin
    # test creation of VibrationalMode functions by comparison with equiv. QO functions
    fb = qo.FockBasis(10)
    @test create(modes[1]).data == qo.create(fb).data
    @test destroy(modes[1]).data == qo.destroy(fb).data
    @test number(modes[1]).data == qo.number(fb).data
    n = rand(0:10)
    @test fockstate(modes[1], n).data == qo.fockstate(fb, n).data

    # displacement operator
    α = rand(0:1e-3:7) + im*rand(0:1e-3:7)
    @test displace(modes[1], α).data ≈ qo.displace(fb, α).data
    fb2 = qo.FockBasis(200)
    modes[1].N = 200
    i = rand(1:5); j = rand(1:5)
    @test displace(modes[1], α, method="analytic").data[i,j] ≈ qo.displace(fb2, α).data[i,j] rtol=1e-3

    # test that mean excitation of thermalstate is as expected
    modes[1].N = 500
    n̄ = abs(2randn())
    @test expect(number(modes[1]), thermalstate(modes[1], n̄)) ≈ n̄
    @test expect(number(modes[1]), thermalstate(modes[1], n̄, method="analytic")) ≈ n̄

    # test coherentstate matches QO results
    α = 10*(randn() + im*randn())
    coherentstate(modes[1], α).data == qo.coherentstate(fb, α).data

    # test coherenthermalstate
    N = 500
    modes[1].N = N
    n̄ = rand(0:1e-6:10)
    @test coherentthermalstate(modes[1], n̄, 0, method="analytic").data ≈ thermalstate(modes[1], n̄).data
    @test coherentthermalstate(modes[1], 0, α, method="analytic").data ≈ dm(coherentstate(modes[1], α)).data rtol=1e-3*N^2
    @test coherentthermalstate(modes[1], n̄, 0).data ≈ thermalstate(modes[1], n̄).data
    @test coherentthermalstate(modes[1], 0, α).data ≈ dm(coherentstate(modes[1], α)).data rtol=1e-3*N^2

    # shouldn't be able to have a mean phonon occupation greater than Hilbert space dimension
    @test_throws AssertionError coherentthermalstate(modes[1], N+1, 0)
    @test_throws AssertionError coherentthermalstate(modes[1], 0, N+1)
    @test_throws AssertionError coherentstate(modes[1], N+1)
    @test_throws AssertionError thermalstate(modes[1], N+1)
    @test_throws AssertionError displace(modes[1], N+1)
end

@testset "operators -- Ion operators" begin
    # test that ionstate constructs the appropriate state for a single ion
    @test ionstate(C, "S-1/2").data == ionstate(C, 1).data == ComplexF64[1; 0]
    @test ionstate(C, "D-1/2").data == ionstate(C, 2).data == ComplexF64[0; 1]

    # test ionstate for an IonConfiguration input
    @test ionstate(chain, "S-1/2", "D-1/2").data == kron(ComplexF64[0; 1], ComplexF64[1; 0])
    @test ionstate(chain, 1, 2).data == kron(ComplexF64[0; 1], ComplexF64[1; 0])

    # test ionstate for an Trap input
    @test ionstate(T, "S-1/2", "D-1/2").data == kron(ComplexF64[0; 1], ComplexF64[1; 0])

    # test sigma(ion::Ion, ψ1::T, ψ2::T) where {T<:Union{String,Int}}
    @test sigma(C, "S-1/2", "D-1/2").data == sigma(C, 1, 2).data == ComplexF64[0 1; 0 0]
    # test sigma(ion::Ion, ψ1::T<:Union{String,Int})
    @test sigma(C, "S-1/2").data == sigma(C, 1).data == ComplexF64[1 0; 0 0]

    # test ionprojector for IonConfiguration input
    ψ = ionprojector(chain, "S-1/2", "D-1/2", only_ions=true)
    @test  ψ.data == kron(ComplexF64[0; 1] * ComplexF64[0; 1]', ComplexF64[1; 0] * ComplexF64[1; 0]')
    @test ionprojector(chain, "S-1/2", "D-1/2") == ψ ⊗ one(modes[1]) ⊗ one(modes[2])
    @test ψ == ionprojector(T, "S-1/2", "D-1/2", only_ions=true)
end

@testset "operators -- internal functions" begin
    # test _pf(s, n, m)
    s = rand(1:12)
    n = rand(1:s)
    m = rand(1:s)
    v1 = IonSim._pf(s, n, m)
    v2 = 1im^n * (-1im)^m * factorial(s) / ((s+1) * √(factorial(m) * factorial(n)))
    isapprox(v1, v2)

    # test He(n) gives the correct Hermite polynomial for order 10
    IonSim._He(10) == [-945, 0, 4725, 0, -3150, 0, 630, 0, -45, 0, 1]

    # test fHe(x, He)
    IonSim._fHe(1, 10) == sum(IonSim._He(10))

    # _alaguerre
    L32 = (1/6) * (-2^3 + 3*(2+3)*2^2 - 3*(2+2)*(2+3)*2 + (2+1)*(2+2)*(2+3))
    @test IonSim._alaguerre(2, 3, 2) ≈ L32

    # _Dnm
    ξ = im * exp(2π*im)
    d = displace(modes[1], ξ).data
    diff = 0.0
    for i in 1:100, j in 1:100
        diff += abs(d[i, j] - IonSim._Dnm(ξ, i, j))
    end
    @test diff < 100  # <1% difference of L1 norm
                      # Note: displace() is an approximation, whereas _Dnm should not be
end
end  # end suppress


text:
using ApproxFun, Base.Test

c = rand(1000)
x=rand(10000)
f=Fun(c,Chebyshev)
y=f(x)
y=f(x)

@time y=f(x)
println("Clenshaw large coeffs, many points: Time should be ~0.024")
# 0.012482274  with unsafe_view
# 0.024306262 with inbounds

y=f(.1)
y=f(.1)
y=f(.1)

@time y=f(.1);
println("Clenshaw large coeffs, 1 point: Time should be ~6e-6")

# @time is 8.853e-6 seconds

f=Fun(exp)
x=sample(f,100000)
x=sample(f,100000)
@time x=sample(f,100000)
println("Sample: Time should be ~0.25")
# 0.213793292 with unsafe_view
# 0.268162181 with inbounds

f=Fun(x->cos(x),20)
roots(f)
roots(f)
@time for k=1:100
    roots(f)
end
println("Small roots: Time should be ~0.015")

f=Fun(x->cos(1000x),1000)
roots(f)
roots(f)
@time roots(f)
println("Roots: Time should be ~0.13")


text:
function PrintStats(x::T) where {T<:AbstractASTConsumer}
    @check_ptrs x
    return clang_ASTConsumer_PrintStats(x)
end

function HandleTranslationUnit(csr::T, ctx::ASTContext) where {T<:AbstractASTConsumer}
    @check_ptrs csr ctx
    return clang_ASTConsumer_HandleTranslationUnit(csr, ctx)
end


text:
using Yao
using Yao.Intrinsics
using LuxurySparse
using SparseArrays
using StaticArrays
using LinearAlgebra
using Yao.Blocks
using Test

using BenchmarkTools

println("##### Matrix #####")
nbit = 16
mmm = randn(ComplexF64, 2, 2)
println("u1mat, t ≈ 600us")
display(@benchmark u1mat(nbit, $mmm, 2))

println("\nunmat, t ≈ 1.1ms")
display(@benchmark unmat(nbit, $mmm, (2,)))
@test u1mat(nbit, mmm, 2) == unmat(nbit, mmm, (2,))

println("\n##### SparseMatrixCSC #####")
nbit = 16
mmm = mat(P1)
println("unmat, t ≈ 1.1ms")
display(@benchmark unmat(nbit, $mmm, (2,)))

println("\n######## PermMatrix #############")
nbit = 16
mmm = X |> mat
println("unmat, t ≈ 300us")
display(@benchmark unmat(nbit, $mmm, (2,)))

println("\n######## Diagonal #############")
nbit = 16
mmm = Z |> mat
println("unmat, t ≈ 400us")
display(@benchmark unmat(nbit, $mmm, (2,)))


text:
# https://github.com/invenia/Mocking.jl/issues/92
@testset "ContextVariableX logging incompatibility" begin
    generate_report() = "original"
    report_patch = @patch generate_report() = "mocked"

    apply(report_patch) do
        @test (@test_logs @mock generate_report()) == "mocked"
    end
end


text:
module SymbolServer

using Pkg, SHA
using Base: UUID

current_package_name = Symbol(ARGS[1])
current_package_version = VersionNumber(ARGS[2])
current_package_uuid = UUID(ARGS[3])
current_package_treehash = ARGS[4]

@info "Indexing package $current_package_name $current_package_version..."

# This path will always be mounted in the docker container in which we are running
store_path = "/symcache"

current_package_versionwithoutplus = replace(string(current_package_version), '+'=>'_')
cache_package_folder_path = joinpath(store_path, "v1", "packages", string(uppercase(string(current_package_name)[1])), "$(current_package_name)_$current_package_uuid")
filename_without_extension = "v$(current_package_versionwithoutplus)_$current_package_treehash"
filename_with_extension = "$filename_without_extension.jstore"
cache_path = joinpath(cache_package_folder_path, filename_with_extension)
cache_path_compressed = joinpath(cache_package_folder_path, "$filename_without_extension.tar.gz")

mkpath(cache_package_folder_path)

module LoadingBay end

try
    Pkg.add(name=string(current_package_name), version=current_package_version)
catch err
    @info "Could not install package, exiting"
    exit(20)
end

# TODO Make the code below ONLY write a cache file for the package we just added here.
include("faketypes.jl")
include("symbols.jl")
include("utils.jl")
include("serialize.jl")
using .CacheStore

# Load package
m = try
    LoadingBay.eval(:(import $current_package_name))
    getfield(LoadingBay, current_package_name)
catch e
    @info "Could not load package, exiting."
    exit(10)
end

# Get the symbols
env = getenvtree([current_package_name])
symbols(env, m)

 # Strip out paths
modify_dirs(env[current_package_name], f -> modify_dir(f, pkg_src_dir(Base.loaded_modules[Base.PkgId(current_package_uuid, string(current_package_name))]), "PLACEHOLDER"))

# There's an issue here - @enum used within CSTParser seems to add a method that is introduced from Enums.jl...

Pkg.PlatformEngines.probe_platform_engines!()

mktempdir() do path
    # Write them to a file
    open(joinpath(path, filename_with_extension), "w") do io
        CacheStore.write(io, Package(string(current_package_name), env[current_package_name], current_package_uuid, nothing))
    end

    # cp(joinpath(path, filename_with_extension), cache_path)
    Pkg.PlatformEngines.package(path, cache_path_compressed)
end

@info "Finished indexing."

# We are exiting with a custom error code to indicate success. This allows
# the parent process to distinguish between a successful run and one
# where the package exited the process.
exit(37)

end


text:
@inline function typeparser(::Type{T}, source, pos, len, b, code, options::Options{ignorerepeated, Q, debug, S, D, DF}) where {T <: Dates.TimeType, ignorerepeated, Q, debug, S, D, DF}
    df = options.dateformat === nothing ? Dates.default_format(T) : options.dateformat
    ret = mytryparsenext_internal(T, source, Int(pos), len, df)
    if debug
        if ret === nothing
            println("failed to parse $T")
        else
            println("parsed: $ret")
        end
    end
    if ret === nothing
        x = default(T)
        code |= INVALID
    else
        values, pos = ret
        x = T(values...)
        code |= OK
        if eof(source, pos, len)
            code |= EOF
        end
    end

    return x, code, pos
end

@generated function mytryparsenext_internal(::Type{T}, str, pos, len, df::DateFormat) where {T <: Dates.TimeType}
    letters = Dates.character_codes(df)

    tokens = Type[Dates.CONVERSION_SPECIFIERS[letter] for letter in letters]
    value_names = Symbol[Dates.genvar(t) for t in tokens]

    output_tokens = Dates.CONVERSION_TRANSLATIONS[T]
    output_names = Symbol[Dates.genvar(t) for t in output_tokens]
    output_defaults = Tuple(Dates.CONVERSION_DEFAULTS[t] for t in output_tokens)

    assign_defaults = Expr[
        quote
            $name = $default
        end
        for (name, default) in zip(output_names, output_defaults)
    ]
    value_tuple = Expr(:tuple, value_names...)

    return quote
        $(Expr(:meta, :inline))
        val = mytryparsenext_core(str, pos, len, df)
        val === nothing && return nothing
        values, pos, num_parsed = val
        $(assign_defaults...)
        $value_tuple = values
        return $(Expr(:tuple, output_names...)), pos
    end
end

@generated function mytryparsenext_core(str, pos, len, df::DateFormat)
    directives = Dates._directives(df)
    letters = Dates.character_codes(directives)

    tokens = Type[Dates.CONVERSION_SPECIFIERS[letter] for letter in letters]
    value_names = Symbol[Dates.genvar(t) for t in tokens]
    value_defaults = Tuple(Dates.CONVERSION_DEFAULTS[t] for t in tokens)

    assign_defaults = Expr[]
    for (name, default) in zip(value_names, value_defaults)
        push!(assign_defaults, quote
            $name = $default
        end)
    end

    vi = 1
    parsers = Expr[]
    for i = 1:length(directives)
        if directives[i] <: Dates.DatePart
            name = value_names[vi]
            vi += 1
            push!(parsers, quote
                pos > len && @goto done
                let val = Dates.tryparsenext(directives[$i], str, pos, len, locale)
                    if val === nothing
                        $i > 1 && @goto done
                        @goto error
                    end
                    $name, pos = val
                end
                num_parsed += 1
                directive_index += 1
            end)
        else
            push!(parsers, quote
                pos > len && @goto done
                let val = Dates.tryparsenext(directives[$i], str, pos, len, locale)
                    if val === nothing
                        $i > 1 && @goto done
                        @goto error
                    end
                    delim, pos = val
                end
                directive_index += 1
            end)
        end
    end

    return quote
        $(Expr(:meta, :inline))
        directives = df.tokens
        locale::Dates.DateLocale = df.locale
        num_parsed = 0
        directive_index = 1
        $(assign_defaults...)
        $(parsers...)
        @label done
        return $(Expr(:tuple, value_names...)), pos, num_parsed
        @label error
        return nothing
    end
end

@inline function Dates.tryparsenext(d::Dates.Delim{<:AbstractChar, N}, str::AbstractVector{UInt8}, i::Int, len) where N
    for j = 1:N
        i > len && return nothing
        next = iterate(str, i)
        @assert next !== nothing
        c, i = next
        c != UInt8(d.d) && return nothing
    end
    return true, i
end

@inline function Dates.tryparsenext(d::Dates.Delim{String, N}, str::AbstractVector{UInt8}, i::Int, len) where N
    i1 = i
    i2 = firstindex(d.d)
    for j = 1:N
        if i1 > len
            return nothing
        end
        next1 = iterate(str, i1)
        @assert next1 !== nothing
        c1, i1 = next1
        next2 = iterate(d.d, i2)
        @assert next2 !== nothing
        c2, i2 = next2
        if c1 != UInt8(c2)
            return nothing
        end
    end
    return true, i1
end

@inline function Dates.tryparsenext_base10(str::AbstractVector{UInt8}, i, len, min_width=1, max_width=0)
    i > len && return nothing
    min_pos = min_width <= 0 ? i : i + min_width - 1
    max_pos = max_width <= 0 ? len : min(i + max_width - 1, len)
    d::Int64 = 0
    @inbounds while i <= max_pos
        c, ii = iterate(str, i)
        if UInt8('0') <= c <= UInt8('9')
            d = d * 10 + (c - UInt8('0'))
        else
            break
        end
        i = ii
    end
    if i <= min_pos
        return nothing
    else
        return d, i
    end
end

@inline function Dates.tryparsenext_word(str::AbstractVector{UInt8}, i, len, locale, maxchars=0)
    word_start, word_end = i, 0
    max_pos = maxchars <= 0 ? len : min(len, i + (maxchars-1))
    @inbounds while i <= max_pos
        c, ii = iterate(str, i)
        if isletter(Char(c))
            word_end = i
        else
            break
        end
        i = ii
    end
    if word_end == 0
        return nothing
    else
        return unsafe_string(pointer(str, word_start), word_end - word_start + 1), i
    end
end


text: maximum_residual = 1e-2


text:
function createPointsIco(n::Int64)
    points,triangulation  = computeXYZico(n)
	return points,triangulation
end
function computeXYZico(n::Int64)
    # the vertices and faces of the regular icosahedron
    # with vertices on the unit sphere are hard coded
    # and stored in txt files
    vertices = readdlm("quadratures/icosahedron_vertices.txt",',',Float64)
    faces = readdlm("quadratures/icosahedron_faces.txt",',',Int)

    nfaces = 20;
    nvertices = 12;
    #close("all")

    n1,n2,n3 = faces[1,1],faces[1,2],faces[1,3]
    v1,v2,v3 = vertices[n1,:],vertices[n2,:],vertices[n3,:]

    p,t = getNodesAndTriangulationForSingleTriangle(n,v1,v2,v3)
   # plot3D(p[1,:],p[2,:],p[3,:],"x")

    ptsAll = deepcopy(p);
    trianglesAll = deepcopy(t);

    for f=2:20
        n1,n2,n3 = faces[f,1],faces[f,2],faces[f,3]
        v1,v2,v3 = vertices[n1,:],vertices[n2,:],vertices[n3,:]

        p,t = getNodesAndTriangulationForSingleTriangle(n,v1,v2,v3)
        nperface = size(p,2)
    #    plot3D(p[1,:],p[2,:],p[3,:],"x")
        ptsAll = deepcopy(hcat(ptsAll,p))
        trianglesAll = deepcopy(hcat(trianglesAll,t.+(f-1)*nperface))
    end
    #println(size(ptsAll))
    #println(size(trianglesAll))

    ptsAll,trianglesAll =unique(ptsAll,trianglesAll)

    # there will be n*(n+1)/2*20-30*n+1*12 points
	# and n*(n-2)*20+20 triangles
	ptsAll = permutedims(ptsAll)
	trianglesAll = permutedims(trianglesAll)
	return ptsAll,trianglesAll

end

function getNodesAndTriangulationForSingleTriangle(n::Int64,
    pt0::Array{Float64,1},pt1::Array{Float64,1},pt2::Array{Float64,1},slerpflag=true::Bool)

	if slerpflag
		# use slerp instead of linspace
		pts01 = slerp(pt0,pt1,n,true)
		pts02 = slerp(pt0,pt2,n,true)
	else
		pts01 = linspace(pt0,pt1,n) #type is: n-element LinSpace{Array{Float64,1}}:
		pts02 = linspace(pt0,pt2,n)
	end

	nptsoctant = Int64(n*(n+1)/2)
	pts = zeros(Float64,3,nptsoctant)

	# Creates points in planar geometry
	counter = 0;
	for i=1:n
		if slerpflag
			if i==1
				tmp = pts01[:,1]
			elseif i==n
				tmp = slerp(pts01[:,i],pts02[:,i],i,true)
			else
				tmp = slerp(pts01[:,i],pts02[:,i],i)
			end
		else
			tmp =linspace(pts01[i],pts02[i],i)
		end

		for j=1:i
			counter += 1
			if slerpflag
				pts[:,counter] = tmp[:,j]
			else
				pts[:,counter] = tmp[j]
			end
		end

	end
	# Project points onto sphere
	for i=1:nptsoctant
    	pts[:,i] = pts[:,i]/norm(pts[:,i])
	end

	# We now want to get the connectivity. Therefore enumerate all
	#the points (overkill from a computational point of view) and then
	#write the correct connectivity.

    # Matrix that assigns an ID to the points
   ids=zeros(Int64,n,n); # Too large, but not important.
   #Matrix that will later contain all triangles
   nTrianglesOctant = Int64(n*n-2*n+1);
   triangles = zeros(Int64,3,nTrianglesOctant);

	counter = 0
	for i=1:n
		for j=1:i
			counter +=1
			ids[i,j] = counter;
		end
	end

	# Now create triangles
	counter = 0
	tmp = zeros(Int64,1)
	for i=1:n
		for j=1:i-1
			tmp  = [ids[i,j],ids[i,j+1],ids[i-1,j]]
			counter += 1
			triangles[:,counter] = tmp;
		end
		if i<n
			for j=1:i-1
				tmp = [ids[i,j],ids[i,j+1],ids[i+1,j+1]]
				counter +=1
				triangles[:,counter] = tmp;
			end
		end
    end
    return pts,triangles;

end

max_stars_repo_name: vavrines/CSN

max_issues_repo_name: vavrines/CSN

max_forks_repo_name: vavrines/CSN


text:
using SpectralVis
using Test

@testset "Spectral environment test" begin
    env = SPECENV

    @test env.λmin == 390.0
    @test env.Δλ == 1.0
    @test env.λmax == 830.0
    @test env.cmf isa cie12_10
    @test env.ex == :linear

    for i in (:none, :boundary, :linear, :parabolic)
        set_extrap(env, i)
        @test env.ex == i
    end

    @test set_extrap(env, :cubic) == DomainError

    a, b, c = 400.0, 5.0, 800.0

    set_limits(env, a, b, c)
    @test env.λmin, env.Δλ, env.λmax == 400.0, 5.0, 800.0

    @test set_colormatch(env, :cie06_2).cmf isa CIE12_2
    @test set_colormatch(:cie06_10).cmf isa CIE12_10
end

@testset "Block spectrum generator mode 0 (block_spec) test" begin
    m = 0
    λmin=380.0
    λmax=780.0
    Δλ  =  1.0
    # 1.0 @ 381.0:382.0
    λs=381.0
    λe=382.0
    refl=zeros(length(collect(λmin:Δλ:λmax)))
    refl[2:3].=1.0
    gt=i_reflectance_spec(collect(λmin:Δλ:λmax),refl)
    @test block_spec(λmin,λmax,Δλ,λs,λe,m).r == gt.r
    # 1.0 @ 781.0
    λs=778.0
    λe=779.0
    refl=zeros(length(collect(λmin:Δλ:λmax)))
    refl[end-2:end-1].=1.0
    gt=i_reflectance_spec(collect(λmin:Δλ:λmax),refl)
    @test block_spec(λmin,λmax,Δλ,λs,λe,m).r == gt.r
end

@testset "Block spectrum generator mode 1 (block_spec) test" begin
    #Inverse block spectrum
    m = 1
    λmin=380.0
    λmax=780.0
    Δλ  =  1.0
    # 1.0 @ 381.0:382.0
    λs=381.0
    λe=382.0
    # 0.0 @ 381.0
    refl=ones(length(collect(λmin:Δλ:λmax)))
    refl[2:3].=0.0
    gt=i_reflectance_spec(collect(λmin:Δλ:λmax),refl)
    @test block_spec(λmin,λmax,Δλ,λs,λe,m).r == gt.r

    # 0.0 @ 781.0
    λs=778.0
    λe=779.0
    refl=ones(length(collect(λmin:Δλ:λmax)))
    refl[end-2:end-1].=0.0
    gt=i_reflectance_spec(collect(λmin:Δλ:λmax),refl)
    @test block_spec(λmin,λmax,Δλ,λs,λe,m).r == gt.r
end

# test of convert(::Type{CIE12_2},mf::LMS06_2)
@testset "LMS06_2 to CIE12_2 conversion" begin
    test02 = cmf(CMF2012_2)
    res02  = convert(CIE12_2,cmf(LMS2006_2))

    eps=5.0E-7

    delta2_1=test02.x.-res02.x
    delta2_2=test02.y.-res02.y
    delta2_3=test02.z.-res02.z

    @test maximum(broadcast(abs,(delta2_1))) ≈ 0 atol=eps
    @test maximum(broadcast(abs,(delta2_2))) ≈ 0 atol=eps
    @test maximum(broadcast(abs,(delta2_3))) ≈ 0 atol=eps
end

# test of convert(::Type{CIE12_10},mf::LMS06_10)
@testset "LMS06_10 to CIE12_10 conversion" begin
    test10 = cmf(CMF2012_10)
    res10  = convert(CIE12_10,cmf(LMS2006_10))

    eps=5.0E-7

    delta10_1=test10.x.-res10.x
    delta10_2=test10.y.-res10.y
    delta10_3=test10.z.-res10.z

    @test maximum(broadcast(abs,(delta10_1))) ≈ 0 atol=eps
    @test maximum(broadcast(abs,(delta10_2))) ≈ 0 atol=eps
    @test maximum(broadcast(abs,(delta10_3))) ≈ 0 atol=eps
end

@testset "Spectral operators test" begin
    @test 1==1

    luminance = D65_illuminant()

end


text:
function init_jacobians(solver,method)
    N,N_ = get_N(solver,method)
    n,m = get_sizes(solver)
    m̄, = get_num_controls(solver)
    if method == :trapezoid || method == :hermite_simpson_separated
        A = zeros(n,n+m̄,N_)
        B = zeros(0,0,N_)
    else
        A = zeros(n,n,N_)
        B = zeros(n,m,N_)
    end
    return A,B
end

function get_nG(solver::Solver,method::Symbol)
    n,m = get_sizes(solver)
    N,N_ = get_N(solver,method)
    m̄, = get_num_controls(solver)
    if method == :trapezoid || method == :hermite_simpson
        nC = 2(n+m̄)*(N-1)n
    elseif method == :hermite_simpson_separated
        nC = 3(n+m̄)*(N-1)n
    elseif method == :midpoint
        nC = (2n+m̄)*(N-1)n
    end
    solver.state.minimum_time ? nE = 2(N-2) : nE = 0

    # Custom constraints
    pI,pE = count_constraints(solver.obj, :custom)
    nP = (N-1)*(n+m̄)*(pE[1] + pI[1]) + n*(pE[2] + pI[2])

    nG = nC + nE + nP
    return nG, (collocation=nC, dt=nE, custom=nP)
end

function gen_usrfun_ipopt(solver::Solver,method::Symbol)
    N,N_ = TrajectoryOptimization.get_N(solver,method)
    n,m = get_sizes(solver)
    m̄,mm = get_num_controls(solver)
    NN = N*(n+m̄)

    # Initialize Variables
    fVal = zeros(n,N)
    gX_,gU_,fVal_ = init_traj_points(solver,fVal,method)
    weights = get_weights(method,N_)
    A,B = init_jacobians(solver,method)

    # Generate custom constraint functions
    if solver.state.constrained
        custom_constraints!, custom_constraint_jacobian!, custom_jacobian_sparsity = TrajectoryOptimization.gen_custom_constraint_fun(solver, method)
        pI_c, pE_c = count_constraints(solver.obj, :custom)
        PI_c = (N-1)pI_c[1] + pI_c[2]  # Total custom inequality constraints
        PE_c = (N-1)pE_c[1] + pE_c[2]  # Total custom equality constraints
        P_c = PI_c + PE_c              # Total custom constraints

        # Partition constraints
        n_colloc = (N-1)n  # Number of collocation constraints
        g_colloc = 1:n_colloc           # Collocation constraints
        g_custom = n_colloc.+(1:P_c)    # Custom constraints
        g_dt = (n_colloc+P_c).+(1:N-2)  # dt constraints (minimum time)

        nG,Gpart = get_nG(solver,method)
        jac_g_colloc = 1:Gpart.collocation
        jac_g_custom = Gpart.collocation .+ (1:Gpart.custom)
        jac_g_dt = Gpart.collocation + Gpart.custom + 1:nG
    end

    #################
    # COST FUNCTION #
    #################
    function eval_f(Z)
        # vars = DircolVars(Z,n,m̄,N)
        # X,U = vars.X, vars.U
        X,U = unpackZ(Z,(n,m̄,N))
        fVal = zeros(eltype(Z),n,N)
        X_ = zeros(eltype(Z),n,N_)
        U_ = zeros(eltype(Z),m̄,N_)
        X_,U_ = get_traj_points(solver,X,U,fVal,X_,U_,method,true)
        J = cost(solver,X_,U_,method)
        return J
    end

    ###########################
    # COLLOCATION CONSTRAINTS #
    ###########################
    function eval_g(Z, g)
        # vars = DircolVars(Z,n,m,N)
        # X,U = vars.X,vars.U
        X,U = unpackZ(Z,(n,m̄,N))
        fVal = zeros(eltype(Z),n,N)
        X_ = zeros(eltype(Z),n,N_)
        U_ = zeros(eltype(Z),m̄,N_)
        fVal_ = zeros(eltype(Z),n,N_)
        X_,U_ = get_traj_points(solver,X,U,fVal,X_,U_,method)
        get_traj_points_derivatives!(solver,X_,U_,fVal_,fVal, method)
        collocation_constraints!(solver::Solver, X_, U_, fVal_, view(g,g_colloc), method::Symbol)
        if solver.state.constrained
            custom_constraints!(view(g,g_custom),X,U)
            if solver.state.minimum_time
                dt_constraints!(solver, view(g,g_dt), view(U,m̄,1:N))
            end
        end
        return nothing
    end

    #################
    # COST GRADIENT #
    #################
    function eval_grad_f(Z, grad_f)
        # vars = DircolVars(Z,n,m,N)
        # X,U = vars.X,vars.U
        X,U = unpackZ(Z,(n,m̄,N))
        X_,U_ = get_traj_points(solver,X,U,fVal,gX_,gU_,method)
        # get_traj_points_derivatives!(solver,X_,U_,fVal_,fVal,method)
        update_jacobians!(solver,X_,U_,A,B,method,true)
        cost_gradient!(solver, X_, U_, fVal, A, B, weights, grad_f, method)
        return nothing
    end

    #######################
    # CONSTRAINT JACOBIAN #
    #######################
    function eval_jac_g(Z, mode, rows, cols, vals)
        if mode == :Structure  # TODO: Do this in place
            nG,Gpart = get_nG(solver,method)
            jacob_colloc = collocation_constraint_jacobian_sparsity(solver,method)
            jacob_custom = custom_jacobian_sparsity(Gpart.collocation)
            jacob_dt = time_step_constraint_jacobian_sparsity(solver,Gpart.collocation + Gpart.custom)
            jacob_g = [jacob_colloc;
                       jacob_custom;
                       jacob_dt]

            r,c,inds = findnz(jacob_g)
            v = sortperm(inds)
            rows .= r[v]
            cols .= c[v]
        else
            # vars = DircolVars(Z,n,m,N)
            # X,U = vars.X,vars.U
            X,U = unpackZ(Z,(n,m̄,N))
            X_,U_ = get_traj_points(solver,X,U,fVal,gX_,gU_,method)
            get_traj_points_derivatives!(solver,X_,U_,fVal_,fVal,method)
            update_jacobians!(solver,X_,U_,A,B,method)
            collocation_constraint_jacobian!(solver, X_,U_,fVal_, A,B, view(vals, jac_g_colloc), method)
            if solver.state.constrained
                custom_constraint_jacobian!(view(vals, jac_g_custom), X,U)
            end
            if solver.state.minimum_time
                time_step_constraint_jacobian!(view(vals, jac_g_dt), solver)
            end
        end
        return nothing
    end

    return eval_f, eval_g, eval_grad_f, eval_jac_g

end

function solve_ipopt(solver::Solver, X0::Matrix{Float64}, U0::Matrix{Float64}, method::Symbol; options::Dict{String,T}=Dict{String,Any}()) where T
    X0 = copy(X0)
    U0 = copy(U0)

    # Get Constants
    N,N_ = TrajectoryOptimization.get_N(solver,method)  # N=>Number of time steps for decision variables (may differ from solver.N)
    n,m = get_sizes(solver)                             # N_=>Number of time steps for "trajectory" or integration points
    m̄, = get_num_controls(solver)
    NN = N*(n+m̄)                                        # Total number of decision variables
    nG, = TrajectoryOptimization.get_nG(solver,method)   # Number of nonzero entries in Constraint Jacobian
    nH = 0                                              # Number of nonzeros entries in Hessian

    # Pack the variables
    if size(U0,2) == N-1
        U0 = [U0 U0[:,end]]
    end
    Z0 = packZ(X0,U0)
    @assert length(Z0) == NN

    # Generate functions
    eval_f, eval_g, eval_grad_f, eval_jac_g = gen_usrfun_ipopt(solver,method)

    # Get Bounds
    x_L, x_U, g_L, g_U = get_bounds(solver,method)
    P = length(g_L)  # Total number of constraints

    # Create Problem
    prob = Ipopt.createProblem(NN, x_L, x_U, P, g_L, g_U, nG, nH,
        eval_f, eval_g, eval_grad_f, eval_jac_g)  # Ipopt.jl method
    prob.x =  Z0  # Set initial state

    # Set options
    dir = root_dir()
    opt_file = joinpath(dir,"ipopt.opt")
    addOption(prob,"option_file_name",opt_file)
    for (opt,val) in pairs(options)
        addOption(prob,opt,val)
    end
    if solver.opts.verbose == false
        addOption(prob,"print_level",0)
    end

    # Solve
    t_eval = @elapsed status = solveProblem(prob)
    stats = parse_ipopt_summary()
    stats["info"] = Ipopt.ApplicationReturnStatus[status]
    stats["runtime"] = t_eval
    vars = DircolVars(prob.x,n,m̄,N)

    return vars, stats, prob
end

"""
$(SIGNATURES)
Extract important information from the SNOPT output file(s)
"""
function parse_ipopt_summary(file=joinpath(root_dir(),"logs","ipopt.out"))
    props = Dict()
    obj = Vector{Float64}()
    c_max = Vector{Float64}()
    iter_lines = false  # Flag true if it's parsing the iteration summary lines

    function stash_prop(ln::String,prop::String,prop_name::String=prop,vartype::Type=Float64)
        if occursin(prop,ln)
            loc = findfirst(prop,ln)
            if vartype <: Real
                val = convert(vartype,parse(Float64,split(ln)[end]))
                props[prop_name] = val
                return true
            end
        end
        return false
    end

    function store_itervals(ln::String)
        if iter_lines
            vals = split(ln)
            if length(vals) == 10 && vals[1] != "iter" && vals[1] != "Restoration" && vals[2] != "iteration"
                push!(obj, parse(Float64,vals[2]))
                push!(c_max, parse(Float64,vals[3]))
            end
        end
    end

    open(file) do f
        for ln in eachline(f)
            stash_prop(ln,"Number of Iterations..","iterations",Int64) ? iter_lines = false : nothing
            stash_prop(ln,"Total CPU secs in IPOPT (w/o function evaluations)","self time")
            stash_prop(ln,"Total CPU secs in NLP function evaluations","function time")
            stash_prop(ln,"Number of objective function evaluations","objective calls",Int64)
            length(ln) > 0 && split(ln)[1] == "iter" && iter_lines == false ? iter_lines = true : nothing
            store_itervals(ln)
        end
    end
    props["cost"] = obj
    props["c_max"] = c_max
    return props
end

function write_ipopt_options(
        optfile=joinpath(root_dir(),"ipopt.opt"),
        outfile=joinpath(root_dir(),"logs","ipopt.out"))
    f = open(optfile,"w")
    println(f,"# IPOPT Options for TrajectoryOptimization.jl\n")
    println(f,"# Use Quasi-Newton methods to avoid the need for the Hessian")
    println(f,"hessian_approximation limited-memory\n")
    println(f,"# Output file")
    println(f,"file_print_level 5")
    # println(f,"output_file $(wrap_quotes(outfile))")
    println(f,"output_file"*" "*"\""*"$(outfile)"*"\"")
    close(f)
end

function wrap_quotes(s::String)
    "\"" * s * "\""
end

#
# function eval_f2(Z)
#     vars = DircolVars(Z,n,m,N)
#     X,U = vars.X, vars.U
#     cost(solver,X,U,weights)
# end
#
# function eval_f3(Z)
#     results.Z .= Z
#     vars = DircolVars(Z,n,m,N)
#     update_derivatives!(solver,results,method)
#     get_traj_points!(solver,results,method)
#     cost(solver,results)
# end
#
#
# function eval_g1(Z, g)
#     X,U = unpackZ(Z)
#     g = reshape(g,n,N-1)
#     f1, f2 = zeros(n),zeros(n)
#     for k = 1:N-1
#         solver.fc(f1,X[:,k+1],U[:,k+1])
#         solver.fc(f2,X[:,k],U[:,k])
#         g[:,k] = dt*( f1 + f2 )/2 - X[:,k+1] + X[:,k]
#     end
#     # reshape(g,n*(N-1),1)
#     return nothing
# end
#
# function eval_g2(Z, g)
#     results.Z .= Z
#     update_derivatives!(solver,results,method)
#     get_traj_points!(solver,results,method)
#     get_traj_points_derivatives!(solver,results,method)
#     g .= collocation_constraints(solver::Solver,results, method::Symbol)
#     # reshape(g,n*(N-1),1)
#     return nothing
# end
#
# function eval_grad_f1(Z, grad_f)
#     results.Z .= Z
#     update_derivatives!(solver,results,method)
#     get_traj_points!(solver,results,method)
#     get_traj_points_derivatives!(solver,results,method)
#     grad_f .= cost_gradient(solver,results,method)
#     return nothing
# end
#
# function eval_jac_g1(Z, mode, rows, cols, vals)
#     if mode == :Structure
#         constraint_jacobian_sparsity(method,solver)
#     else
#         vars = DircolVars(Z,n,m,N)
#         X,U, = vars.X,vars.U
#         X_,U_ = get_traj_points(solver,X,U,fVal,gX_,gU_,method)
#         get_traj_points_derivatives!(solver,X_,U_,fVal_,fVal,method)
#         update_jacobians!(solver,X_,U_,A,B,method)
#         jacob_g = constraint_jacobian(solver,X_,U_,A,B,method)
#     end
# end


text:
function topological(tree::N, constraints::Dict) where N<:GeneralNode
    for key in keys(constraints)
        lca = find_lca(tree, constraints[key])
        lca.root && return false
    end
    true
end

# topological constraints fallback
function topological(tree::N, constraints::Missing) where N<:GeneralNode
    true
end


text:
# Note that this script can accept some limited command-line arguments, run
# `julia build_tarballs.jl --help` to see a usage message.
using BinaryBuilder

name = "binaryen"
version = v"89.0.0"

# Collection of sources required to build binaryen
sources = [
    "https://github.com/WebAssembly/binaryen.git" =>
    "e2f49d8227f2b71e4dede5cf4074bb9f65e3d77f",

]

# Bash recipe for building across all platforms
script = raw"""
cd $WORKSPACE/srcdir
cd binaryen/
sed -i -e 's/Windows.h/windows.h/g' src/tools/wasm-reduce.cpp
cmake -DCMAKE_INSTALL_PREFIX=$prefix -DCMAKE_TOOLCHAIN_FILE=/opt/$target/$target.toolchain .
make -j8
make install

"""

# These are the platforms we will build for by default, unless further
# platforms are passed in on the command line
platforms = [
    Linux(:i686, libc=:glibc),
    Linux(:x86_64, libc=:glibc),
    Linux(:aarch64, libc=:glibc),
    Linux(:armv7l, libc=:glibc, call_abi=:eabihf),
    Linux(:powerpc64le, libc=:glibc),
    Linux(:i686, libc=:musl),
    Linux(:x86_64, libc=:musl),
    Linux(:aarch64, libc=:musl),
    Linux(:armv7l, libc=:musl, call_abi=:eabihf),
    MacOS(:x86_64),
    Windows(:i686),
    Windows(:x86_64)
]

# The products that we will ensure are always built
products(prefix) = [
    ExecutableProduct(prefix, "wasm-as", :wasm_as),
    LibraryProduct(prefix, "libbinaryen", :libbinaryen),
    ExecutableProduct(prefix, "wasm-opt", :wasm_opt),
    ExecutableProduct(prefix, "wasm-shell", :wasm_shell),
    ExecutableProduct(prefix, "wasm2js", :wasm2js),
    ExecutableProduct(prefix, "wasm-reduce", :wasm_reduce),
    ExecutableProduct(prefix, "wasm-metadce", :wasm_metadce),
    ExecutableProduct(prefix, "wasm-dis", :wasm_dis),
    ExecutableProduct(prefix, "wasm-ctor-eval", :wasm_ctor_eval),
    ExecutableProduct(prefix, "wasm-emscripten-finalize", :wasm_emscripten_finalize),
    ExecutableProduct(prefix, "asm2wasm", :asm2wasm)
]

# Dependencies that must be installed before this package can be built
dependencies = [

]

# Build the tarballs, and possibly a `build.jl` as well.
build_tarballs(ARGS, name, version, sources, script, platforms, products, dependencies)


text:
T = Float64

# model
model = TrajectoryOptimization.Dynamics.pendulum
n = model.n; m = model.m
model_d = rk3(model)

# cost
Q = Array(1e-3*Diagonal(I,n))
R = Array(1e-3*Diagonal(I,m))
Qf = Q
x0 = zeros(n)
xf = [pi;0.0]

# options
verbose=false
opts_ilqr = iLQRSolverOptions{T}(verbose=false,live_plotting=:off)
opts_al = AugmentedLagrangianSolverOptions{T}(verbose=false,opts_uncon=opts_ilqr,iterations=50,penalty_scaling=10.0)
opts_altro = ALTROSolverOptions{T}(verbose=false,opts_al=opts_al,R_minimum_time=15.0,dt_max=0.15,dt_min=1.0e-3)

# constraints
u_bnd = 5.
bnd = BoundConstraint(n,m,u_min=-u_bnd,u_max=u_bnd)
goal_con = goal_constraint(xf)

# problem
N = 31
U = [ones(m) for k = 1:N-1]
constraints = Constraints(N)
for k = 1:N-1
    constraints[k] += bnd
end
constraints[N] += goal_con
obj = LQRObjective(Q,R,Qf,xf,N)

dt = 0.15
prob = Problem(model_d,obj,U,constraints=constraints,dt=dt,x0=x0)
solve!(prob,opts_altro)
tt = total_time(prob)

PC_mt = TrajectoryOptimization.mintime_constraints(prob)
@test length(PC_mt[1]) == 1
@test length(PC_mt[2]) == 2
@test length(PC_mt[N]) == 2

C = prob.constraints[1]
C2 = TrajectoryOptimization.update_constraint_set_jacobians(C, n, n+1, m)
@test length(C2) == 1

dt = 0.15/2.0
prob_mt = Problem(model_d,obj,prob.U,constraints=constraints,dt=dt,x0=x0,tf=:min)
solve!(prob_mt,opts_altro)
tt_mt = total_time(prob_mt)

n̄ = n+1
idx = [(1:n)...,((1:m) .+ n̄)...]
[collect(1:n);collect(1:m) .+ n̄]

@test tt_mt < 0.5*tt
@test tt_mt < 1.0

@test norm(prob_mt.X[end] - xf,Inf) < 1e-3
@test max_violation(prob_mt) < opts_al.constraint_tolerance

## Box parallel park
model = TrajectoryOptimization.Dynamics.car
n = model.n; m = model.m
model_d = discretize_model(model,:rk4)

# cost
x0 = [0.0;0.0;0.]
xf = [0.0;1.0;0.]
tf =  3.
Qf = 100.0*Diagonal(I,n)
Q = (1e-2)*Diagonal(I,n)
R = (1e-2)*Diagonal(I,m)

# options
verbose=false
opts_ilqr = iLQRSolverOptions{T}(verbose=false,live_plotting=:off)
opts_al = AugmentedLagrangianSolverOptions{T}(verbose=false,opts_uncon=opts_ilqr,
    iterations=30,penalty_scaling=10.0)
opts_altro = ALTROSolverOptions{T}(verbose=false,opts_al=opts_al,R_minimum_time=40.,
    dt_max=0.2,dt_min=1.0e-3)

# constraints
u_bnd = 2.
x_min = [-0.25; -0.001; -Inf]
x_max = [0.25; 1.001; Inf]
bnd1 = BoundConstraint(n,m,u_min=-u_bnd,u_max=u_bnd)
bnd2 = BoundConstraint(n,m,x_min=x_min,x_max=x_max,u_min=-u_bnd,u_max=u_bnd)

goal_con = goal_constraint(xf)

# problem
N = 51
U = [ones(m) for k = 1:N-1]
constraints = Constraints(N)
constraints[1] += bnd1
for k = 2:N-1
    constraints[k] += bnd2
end
constraints[N] += goal_con

obj = LQRObjective(Q,R,Qf,xf,N)

dt = 0.06
prob = Problem(model_d,obj,U,constraints=constraints,dt=dt,x0=x0)
solve!(prob,opts_altro)
tt = total_time(prob)

prob_mt = Problem(model_d,obj,prob.U,constraints=constraints,dt=dt,x0=x0,tf=:min)
solve!(prob_mt,opts_altro)
tt_mt = total_time(prob_mt)

@test tt_mt < 0.75*tt
@test tt_mt < 2.1

@test norm(prob_mt.X[end] - xf,Inf) < 1e-3
@test max_violation(prob_mt) < opts_al.constraint_tolerance

########################################
#            PARALLEL PARK             #
########################################

# options
T = Float64
max_con_viol = 1.0e-8
dt_max = 0.2
dt_min = 1.0e-3
verbose=false

opts_ilqr = iLQRSolverOptions{T}(verbose=verbose,live_plotting=:off)

opts_al = AugmentedLagrangianSolverOptions{T}(verbose=verbose,opts_uncon=opts_ilqr,
    iterations=30,penalty_scaling=10.0,constraint_tolerance=max_con_viol)

opts_pn = ProjectedNewtonSolverOptions{T}(verbose=verbose,feasibility_tolerance=max_con_viol)

opts_altro = ALTROSolverOptions{T}(verbose=verbose,opts_al=opts_al,opts_pn=opts_pn,R_minimum_time=12.5,
    dt_max=dt_max,dt_min=dt_min,projected_newton=true,projected_newton_tolerance=1.0e-4)

opts_ipopt = DIRCOLSolverOptions{T}(verbose=verbose,nlp=Ipopt.Optimizer(),
    opts=Dict(:print_level=>3,:tol=>max_con_viol,:constr_viol_tol=>max_con_viol))

# DIRCOL w/ Ipopt
prob_ipopt = copy(Problems.parallel_park)
rollout!(prob_ipopt)
prob_ipopt = update_problem(prob_ipopt,model=Dynamics.car) # get continuous time model
p2, s2 = solve(prob_ipopt, opts_ipopt)
@test max_violation_direct(p2) <= 1e-6

## Minimum Time
max_con_viol = 1.0e-6

opts_mt_ipopt = TO.DIRCOLSolverMTOptions{T}(verbose=verbose,nlp=Ipopt.Optimizer(),
    opts=Dict(:print_level=>0,:tol=>max_con_viol,:constr_viol_tol=>max_con_viol),
    R_min_time=10.0,h_max=dt_max,h_min=dt_min)

# DIRCOL w/ Ipopt
prob_mt_ipopt = update_problem(copy(Problems.parallel_park))
initial_controls!(prob_mt_ipopt,copy(p2.U))
rollout!(prob_mt_ipopt)
prob_mt_ipopt = update_problem(prob_mt_ipopt,model=Dynamics.car,tf=0.) # get continuous time model
p5, s5 = solve(prob_mt_ipopt, opts_mt_ipopt)
@test max_violation_direct(p5) <= 1e-6
@test total_time(p5) < 1.6

max_issues_repo_name: MehrdadZareian/GMARA

max_forks_repo_name: MehrdadZareian/GMARA


text:
"""
    NeighborLongitudinalResult

A structure to retrieve information about a neihbor in the longitudinal direction i.e. rear and front neighbors on the same lane.
If the neighbor index is equal to `nothing` it means there is no neighbor.

# Fields
- `ind::Union{Nothing, Int64}` index of the neighbor in the scene
- `Δs::Float64` positive distance along the lane between vehicles positions
"""
struct NeighborLongitudinalResult
    ind::Union{Nothing, Int64} # index in scene of the neighbor
    Δs::Float64 # positive distance along lane between vehicles' positions
end

function get_headway(veh_rear::Vehicle1D, veh_fore::Vehicle1D, roadway::StraightRoadway)
    return get_headway(get_front(veh_rear), get_rear(veh_fore), roadway)
end
function get_neighbor_fore(scene::Frame{Entity{State1D, D, I}}, vehicle_index::I, roadway::StraightRoadway) where {D, I}
    ego = scene[vehicle_index]
    best_ind = nothing
    best_gap = Inf
    for (i,veh) in enumerate(scene)
        if i != vehicle_index
            Δs = get_headway(ego, veh, roadway)
            if Δs < best_gap
                best_gap, best_ind = Δs, i
            end
        end
    end
    return NeighborLongitudinalResult(best_ind, best_gap)
end
function get_neighbor_rear(scene::Frame{Entity{State1D, D, I}}, vehicle_index::I, roadway::StraightRoadway) where {D, I}
    ego = scene[vehicle_index]
    best_ind = nothing
    best_gap = Inf
    for (i,veh) in enumerate(scene)
        if i != vehicle_index
            Δs = get_headway(veh, ego, roadway)
            if Δs < best_gap
                best_gap, best_ind = Δs, i
            end
        end
    end
    return NeighborLongitudinalResult(best_ind, best_gap)
end

abstract type VehicleTargetPoint end
struct VehicleTargetPointFront <: VehicleTargetPoint end
get_targetpoint_delta(::VehicleTargetPointFront, veh::Entity{S, D, I}) where {S,D<:AbstractAgentDefinition, I} = length(veh.def)/2*cos(posf(veh.state).ϕ)
struct VehicleTargetPointCenter <: VehicleTargetPoint end
get_targetpoint_delta(::VehicleTargetPointCenter, veh::Entity{S, D, I}) where {S,D<:AbstractAgentDefinition, I} = 0.0
struct VehicleTargetPointRear <: VehicleTargetPoint end
get_targetpoint_delta(::VehicleTargetPointRear, veh::Entity{S, D, I}) where {S,D<:AbstractAgentDefinition, I} = -length(veh.def)/2*cos(posf(veh.state).ϕ)

const VEHICLE_TARGET_POINT_CENTER = VehicleTargetPointCenter()

"""
    get_neighbor_fore_along_lane(scene::EntityFrame{S,D,I}, roadway::Roadway, tag_start::LaneTag, s_base::Float64, targetpoint_primary::VehicleTargetPoint, targetpoint_valid::VehicleTargetPoint;
                                 max_distance_fore::Float64 = 250.0, index_to_ignore::Int=-1) where {S,D<:AbstractAgentDefinition,I}

Return the index and the longitudinal distance of the vehicle that is in the same lane as scene[vehicle_index] and
in front of it with the smallest distance along the lane. The result is returned as a `NeighborLongitudinalResult` object.
The method will search on the current lane first, and if no vehicle is found it will continue to travel along the lane following next_lane(lane, roadway).
If no vehicle is found within `max_distance_fore,` the index takes a value of `nothing`.

# Notes on the optional arguments:
- `targetpoint_primary::VehicleTargetPoint` the reference point whose distance we want to minimize
- `targetpoint_valid::VehicleTargetPoint` the reference point, which if distance to is positive, we include the vehicle

"""
function get_neighbor_fore_along_lane(
    scene::EntityFrame{S,D,I},
    roadway::Roadway,
    tag_start::LaneTag,
    s_base::Float64,
    targetpoint_primary::VehicleTargetPoint, # the reference point whose distance we want to minimize
    targetpoint_valid::VehicleTargetPoint; # the reference point, which if distance to is positive, we include the vehicle
    max_distance_fore::Float64 = 250.0, # max distance to search forward [m]
    index_to_ignore::Int=-1,
    ) where {S,D<:AbstractAgentDefinition,I}
    best_ind = nothing
    best_dist = max_distance_fore
    tag_target = tag_start

    dist_searched = 0.0
    while dist_searched < max_distance_fore

        lane = roadway[tag_target]

        for (i,veh) in enumerate(scene)
            if i != index_to_ignore

                s_adjust = NaN
                if posf(veh.state).roadind.tag == tag_target
                    s_adjust = 0.0
                elseif is_between_segments_hi(posf(veh.state).roadind.ind, lane.curve) &&
                       is_in_entrances(roadway[tag_target], posf(veh.state).roadind.tag)
                    distance_between_lanes = norm(VecE2(roadway[tag_target].curve[1].pos - roadway[posf(veh.state).roadind.tag].curve[end].pos))
                    s_adjust = -(roadway[posf(veh.state).roadind.tag].curve[end].s + distance_between_lanes)

                elseif is_between_segments_lo(posf(veh.state).roadind.ind) &&
                       is_in_exits(roadway[tag_target], posf(veh.state).roadind.tag)
                    distance_between_lanes = norm(VecE2(roadway[tag_target].curve[end].pos - roadway[posf(veh.state).roadind.tag].curve[1].pos))
                    s_adjust = roadway[tag_target].curve[end].s + distance_between_lanes
                end

                if !isnan(s_adjust)
                    s_valid = posf(veh.state).s + get_targetpoint_delta(targetpoint_valid, veh) + s_adjust
                    dist_valid = s_valid - s_base + dist_searched
                    if dist_valid ≥ 0.0
                        s_primary = posf(veh.state).s + get_targetpoint_delta(targetpoint_primary, veh) + s_adjust
                        dist = s_primary - s_base + dist_searched
                        if dist < best_dist
                            best_dist = dist
                            best_ind = i
                        end
                    end
                end
            end
        end

        if best_ind != nothing
            break
        end

        if !has_next(lane) ||
           (tag_target == tag_start && dist_searched != 0.0) # exit after visiting this lane a 2nd time
            break
        end

        dist_searched += (lane.curve[end].s - s_base)
        s_base = -norm(VecE2(lane.curve[end].pos - next_lane_point(lane, roadway).pos)) # negative distance between lanes
        tag_target = next_lane(lane, roadway).tag
    end

    NeighborLongitudinalResult(best_ind, best_dist)
end
function get_neighbor_fore_along_lane(
    scene::EntityFrame{S,D,I},
    vehicle_index::Int,
    roadway::Roadway,
    targetpoint_ego::VehicleTargetPoint,
    targetpoint_primary::VehicleTargetPoint, # the reference point whose distance we want to minimize
    targetpoint_valid::VehicleTargetPoint; # the reference point, which if distance to is positive, we include the vehicle
    max_distance_fore::Float64 = 250.0 # max distance to search forward [m]
    ) where {S,D<:AbstractAgentDefinition,I}

    veh_ego = scene[vehicle_index]
    tag_start = posf(veh_ego.state).roadind.tag
    s_base = posf(veh_ego.state).s + get_targetpoint_delta(targetpoint_ego, veh_ego)

    get_neighbor_fore_along_lane(scene, roadway, tag_start, s_base,
        targetpoint_primary, targetpoint_valid,
        max_distance_fore=max_distance_fore, index_to_ignore=vehicle_index)
end
function get_neighbor_fore_along_left_lane(
    scene::EntityFrame{S,D,I},
    vehicle_index::Int,
    roadway::Roadway,
    targetpoint_ego::VehicleTargetPoint,
    targetpoint_primary::VehicleTargetPoint, # the reference point whose distance we want to minimize
    targetpoint_valid::VehicleTargetPoint; # the reference point, which if distance to is positive, we include the vehicle
    max_distance_fore::Float64 = 250.0 # max distance to search forward [m]
    ) where {S,D<:AbstractAgentDefinition,I}

    retval = NeighborLongitudinalResult(nothing, max_distance_fore)

    veh_ego = scene[vehicle_index]
    lane = get_lane(roadway, veh_ego)
    if n_lanes_left(lane, roadway) > 0
        lane_left = roadway[LaneTag(lane.tag.segment, lane.tag.lane + 1)]
        roadproj = proj(posg(veh_ego.state), lane_left, roadway)
        tag_start = roadproj.tag
        s_base = lane_left[roadproj.curveproj.ind, roadway].s + get_targetpoint_delta(targetpoint_ego, veh_ego)

        retval = get_neighbor_fore_along_lane(scene, roadway, tag_start, s_base,
                                              targetpoint_primary, targetpoint_valid,
                                              index_to_ignore=vehicle_index,
                                              max_distance_fore=max_distance_fore)
    end

    retval
end
function get_neighbor_fore_along_right_lane(
    scene::EntityFrame{S,D,I},
    vehicle_index::Int,
    roadway::Roadway,
    targetpoint_ego::VehicleTargetPoint,
    targetpoint_primary::VehicleTargetPoint, # the reference point whose distance we want to minimize
    targetpoint_valid::VehicleTargetPoint; # the reference point, which if distance to is positive, we include the vehicle
    max_distance_fore::Float64 = 250.0 # max distance to search forward [m]
    ) where {S,D<:AbstractAgentDefinition,I}

    retval = NeighborLongitudinalResult(nothing, max_distance_fore)

    veh_ego = scene[vehicle_index]
    lane = get_lane(roadway, veh_ego)
    if n_lanes_right(lane, roadway) > 0
        lane_right = roadway[LaneTag(lane.tag.segment, lane.tag.lane - 1)]
        roadproj = proj(posg(veh_ego.state), lane_right, roadway)
        tag_start = roadproj.tag
        s_base = lane_right[roadproj.curveproj.ind, roadway].s + get_targetpoint_delta(targetpoint_ego, veh_ego)

        retval = get_neighbor_fore_along_lane(scene, roadway, tag_start, s_base,
                                              targetpoint_primary, targetpoint_valid,
                                              index_to_ignore=vehicle_index,
                                              max_distance_fore=max_distance_fore)
    end

    retval
end

function get_neighbor_fore_along_lane(
    scene::EntityFrame{S,D,I},
    roadway::Roadway,
    tag_start::LaneTag,
    s_base::Float64;
    max_distance_fore::Float64 = 250.0, # max distance to search forward [m]
    index_to_ignore::Int=-1,
    ) where {S,D<:AbstractAgentDefinition,I}

    get_neighbor_fore_along_lane(scene, roadway, tag_start, s_base,
        VEHICLE_TARGET_POINT_CENTER, VEHICLE_TARGET_POINT_CENTER,
        max_distance_fore=max_distance_fore,
        index_to_ignore=index_to_ignore)
end
function get_neighbor_fore_along_lane(scene::EntityFrame{S,D,I}, vehicle_index::Int, roadway::Roadway;
    max_distance_fore::Float64 = 250.0 # max distance to search forward [m]
    ) where {S,D<:AbstractAgentDefinition,I}

    get_neighbor_fore_along_lane(scene, vehicle_index, roadway,
        VEHICLE_TARGET_POINT_CENTER, VEHICLE_TARGET_POINT_CENTER,
        VEHICLE_TARGET_POINT_CENTER, max_distance_fore=max_distance_fore)
end
function get_neighbor_fore_along_left_lane(scene::EntityFrame{S,D,I}, vehicle_index::Int, roadway::Roadway;
    max_distance_fore::Float64 = 250.0 # max distance to search forward [m]
    ) where {S,D<:AbstractAgentDefinition,I}

    get_neighbor_fore_along_left_lane(scene, vehicle_index, roadway,
        VEHICLE_TARGET_POINT_CENTER, VEHICLE_TARGET_POINT_CENTER,
        VEHICLE_TARGET_POINT_CENTER, max_distance_fore=max_distance_fore)
end
function get_neighbor_fore_along_right_lane(scene::EntityFrame{S,D,I}, vehicle_index::Int, roadway::Roadway;
    max_distance_fore::Float64 = 250.0 # max distance to search forward [m]
    ) where {S,D<:AbstractAgentDefinition,I}

    get_neighbor_fore_along_right_lane(scene, vehicle_index, roadway,
        VEHICLE_TARGET_POINT_CENTER, VEHICLE_TARGET_POINT_CENTER,
        VEHICLE_TARGET_POINT_CENTER, max_distance_fore=max_distance_fore)
end

function get_neighbor_rear_along_lane(
    scene::EntityFrame{S,D,I},
    roadway::Roadway,
    tag_start::LaneTag,
    s_base::Float64,
    targetpoint_primary::VehicleTargetPoint, # the reference point whose distance we want to minimize
    targetpoint_valid::VehicleTargetPoint; # the reference point, which if distance to is positive, we include the vehicle
    max_distance_rear::Float64 = 250.0, # max distance to search rearward [m]
    index_to_ignore::Int=-1,
    ) where {S,D<:AbstractAgentDefinition,I}

    best_ind = nothing
    best_dist = max_distance_rear
    tag_target = tag_start

    ignore = Set{I}()

    dist_searched = 0.0
    while dist_searched < max_distance_rear

        lane = roadway[tag_target]

        for (i,veh) in enumerate(scene)
            if i != index_to_ignore && !in(veh.id, ignore)

                s_adjust = NaN

                if posf(veh.state).roadind.tag == tag_target
                    s_adjust = 0.0

                elseif is_between_segments_hi(posf(veh.state).roadind.ind, lane.curve) &&
                       is_in_entrances(roadway[tag_target], posf(veh.state).roadind.tag)

                    distance_between_lanes = norm(VecE2(roadway[tag_target].curve[1].pos - roadway[posf(veh.state).roadind.tag].curve[end].pos))
                    s_adjust = -(roadway[posf(veh.state).roadind.tag].curve[end].s + distance_between_lanes)

                elseif is_between_segments_lo(posf(veh.state).roadind.ind) &&
                       is_in_exits(roadway[tag_target], posf(veh.state).roadind.tag)

                    distance_between_lanes = norm(VecE2(roadway[tag_target].curve[end].pos - roadway[posf(veh.state).roadind.tag].curve[1].pos))
                    s_adjust = roadway[tag_target].curve[end].s + distance_between_lanes
                end

                if !isnan(s_adjust)
                    s_valid = posf(veh.state).s + get_targetpoint_delta(targetpoint_valid, veh) + s_adjust
                    dist_valid = s_base - s_valid + dist_searched
                    if dist_valid ≥ 0.0
                        s_primary = posf(veh.state).s + get_targetpoint_delta(targetpoint_primary, veh) + s_adjust
                        dist = s_base - s_primary + dist_searched
                        if dist < best_dist
                            best_dist = dist
                            best_ind = i
                        end
                    else
                        push!(ignore, veh.id)
                    end
                end
            end
        end

        if best_ind != nothing
            break
        end

        if !has_prev(lane) ||
           (tag_target == tag_start && dist_searched != 0.0) # exit after visiting this lane a 2nd time
            break
        end

        dist_searched += s_base
        s_base = lane.curve[end].s + norm(VecE2(lane.curve[end].pos - prev_lane_point(lane, roadway).pos)) # end of prev lane plus crossover
        tag_target = prev_lane(lane, roadway).tag
    end

    NeighborLongitudinalResult(best_ind, best_dist)
end
function get_neighbor_rear_along_lane(
    scene::EntityFrame{S,D,I},
    vehicle_index::Int,
    roadway::Roadway,
    targetpoint_ego::VehicleTargetPoint,
    targetpoint_primary::VehicleTargetPoint, # the reference point whose distance we want to minimize
    targetpoint_valid::VehicleTargetPoint; # the reference point, which if distance to is positive, we include the vehicle
    max_distance_rear::Float64 = 250.0 # max distance to search rearward [m]
    ) where {S,D<:AbstractAgentDefinition,I}

    veh_ego = scene[vehicle_index]
    tag_start = posf(veh_ego.state).roadind.tag
    s_base = posf(veh_ego.state).s + get_targetpoint_delta(targetpoint_ego, veh_ego)

    get_neighbor_rear_along_lane(scene, roadway, tag_start, s_base,
        targetpoint_primary, targetpoint_valid,
        max_distance_rear=max_distance_rear, index_to_ignore=vehicle_index)
end
function get_neighbor_rear_along_left_lane(
    scene::EntityFrame{S,D,I},
    vehicle_index::Int,
    roadway::Roadway,
    targetpoint_ego::VehicleTargetPoint,
    targetpoint_primary::VehicleTargetPoint, # the reference point whose distance we want to minimize
    targetpoint_valid::VehicleTargetPoint; # the reference point, which if distance to is positive, we include the vehicle
    max_distance_rear::Float64 = 250.0 # max distance to search forward [m]
    ) where {S,D<:AbstractAgentDefinition,I}

    retval = NeighborLongitudinalResult(nothing, max_distance_rear)

    veh_ego = scene[vehicle_index]
    lane = get_lane(roadway, veh_ego)
    if n_lanes_left(lane, roadway) > 0
        lane_left = roadway[LaneTag(lane.tag.segment, lane.tag.lane + 1)]
        roadproj = proj(posg(veh_ego.state), lane_left, roadway)
        tag_start = roadproj.tag
        s_base = lane_left[roadproj.curveproj.ind, roadway].s + get_targetpoint_delta(targetpoint_ego, veh_ego)

        retval = get_neighbor_rear_along_lane(scene, roadway, tag_start, s_base,
                                              targetpoint_primary, targetpoint_valid,
                                              index_to_ignore=vehicle_index,
                                              max_distance_rear=max_distance_rear)
    end

    retval
end
function get_neighbor_rear_along_right_lane(
    scene::EntityFrame{S,D,I},
    vehicle_index::Int,
    roadway::Roadway,
    targetpoint_ego::VehicleTargetPoint,
    targetpoint_primary::VehicleTargetPoint, # the reference point whose distance we want to minimize
    targetpoint_valid::VehicleTargetPoint; # the reference point, which if distance to is positive, we include the vehicle
    max_distance_rear::Float64 = 250.0 # max distance to search forward [m]
    ) where {S,D<:AbstractAgentDefinition,I}

    retval = NeighborLongitudinalResult(nothing, max_distance_rear)

    veh_ego = scene[vehicle_index]
    lane = get_lane(roadway, veh_ego)
    if n_lanes_right(lane, roadway) > 0
        lane_right = roadway[LaneTag(lane.tag.segment, lane.tag.lane - 1)]
        roadproj = proj(posg(veh_ego.state), lane_right, roadway)
        tag_start = roadproj.tag
        s_base = lane_right[roadproj.curveproj.ind, roadway].s + get_targetpoint_delta(targetpoint_ego, veh_ego)

        retval = get_neighbor_rear_along_lane(scene, roadway, tag_start, s_base,
                                              targetpoint_primary, targetpoint_valid,
                                              index_to_ignore=vehicle_index,
                                              max_distance_rear=max_distance_rear)
    end

    retval
end

function get_neighbor_rear_along_lane(
    scene::EntityFrame{S,D,I},
    roadway::Roadway,
    tag_start::LaneTag,
    s_base::Float64;
    max_distance_rear::Float64 = 250.0, # max distance to search rearward [m]
    index_to_ignore::Int=-1,
    ) where {S,D<:AbstractAgentDefinition,I}

    get_neighbor_rear_along_lane(scene, roadway, tag_start, s_base,
        VEHICLE_TARGET_POINT_CENTER, VEHICLE_TARGET_POINT_CENTER,
        max_distance_rear=max_distance_rear,
        index_to_ignore=index_to_ignore)
end
function get_neighbor_rear_along_lane(scene::EntityFrame{S,D,I}, vehicle_index::Int, roadway::Roadway;
    max_distance_rear::Float64 = 250.0 # max distance to search forward [m]
    ) where {S,D<:AbstractAgentDefinition,I}

    get_neighbor_rear_along_lane(scene, vehicle_index, roadway,
        VEHICLE_TARGET_POINT_CENTER, VEHICLE_TARGET_POINT_CENTER,
        VEHICLE_TARGET_POINT_CENTER, max_distance_rear=max_distance_rear)
end
function get_neighbor_rear_along_left_lane(
    scene::EntityFrame{S,D,I},
    vehicle_index::Int,
    roadway::Roadway;
    max_distance_rear::Float64 = 250.0 # max distance to search forward [m]
    ) where {S,D<:AbstractAgentDefinition,I}

    get_neighbor_rear_along_left_lane(scene, vehicle_index, roadway,
        VEHICLE_TARGET_POINT_CENTER, VEHICLE_TARGET_POINT_CENTER,
        VEHICLE_TARGET_POINT_CENTER, max_distance_rear=max_distance_rear)
end
function get_neighbor_rear_along_right_lane(
    scene::EntityFrame{S,D,I},
    vehicle_index::Int,
    roadway::Roadway;
    max_distance_rear::Float64 = 250.0 # max distance to search forward [m]
    ) where {S,D<:AbstractAgentDefinition,I}

    get_neighbor_rear_along_right_lane(scene, vehicle_index, roadway,
        VEHICLE_TARGET_POINT_CENTER, VEHICLE_TARGET_POINT_CENTER,
        VEHICLE_TARGET_POINT_CENTER, max_distance_rear=max_distance_rear)
end

"""
    Project the given point to the same lane as the given RoadIndex.
This will return the projection of the point, along with the Δs along the lane from the RoadIndex.
"""
struct FrenetRelativePosition
    origin::RoadIndex
    target::RoadIndex
    Δs::Float64
    t::Float64
    ϕ::Float64
end
function get_frenet_relative_position(posG::VecSE2{Float64}, roadind::RoadIndex, roadway::Roadway;
    max_distance_fore::Float64 = 250.0, # max distance to search forward [m]
    max_distance_rear::Float64 = 250.0, # max distance to search backward [m]
    improvement_threshold::Float64 = 1e-4,
    )

    # project to current lane first
    tag_start = roadind.tag
    lane_start = roadway[tag_start]
    curveproj_start = proj(posG, lane_start, roadway, move_along_curves=false).curveproj
    curvept_start = lane_start[curveproj_start.ind, roadway]
    s_base = lane_start[roadind.ind, roadway].s
    s_proj = curvept_start.s
    Δs = s_proj - s_base
    sq_dist_to_curve = normsquared(VecE2(posG - curvept_start.pos))
    retval = FrenetRelativePosition(roadind,
                RoadIndex(curveproj_start.ind, tag_start), Δs, curveproj_start.t, curveproj_start.ϕ)

    # search downstream
    if has_next(lane_start)
        dist_searched = lane_start.curve[end].s - s_base
        s_base = -norm(VecE2(lane_start.curve[end].pos - next_lane_point(lane_start, roadway).pos)) # negative distance between lanes
        tag_target = next_lane(lane_start, roadway).tag
        while dist_searched < max_distance_fore

            lane = roadway[tag_target]
            curveproj = proj(posG, lane, roadway, move_along_curves=false).curveproj
            sq_dist_to_curve2 = normsquared(VecE2(posG - lane[curveproj.ind, roadway].pos))

            if sq_dist_to_curve2 < sq_dist_to_curve - improvement_threshold

                sq_dist_to_curve = sq_dist_to_curve2
                s_proj = lane[curveproj.ind, roadway].s
                Δs = s_proj - s_base + dist_searched
                retval = FrenetRelativePosition(
                            roadind, RoadIndex(curveproj.ind, tag_target),
                            Δs, curveproj.t, curveproj.ϕ)
            end

            if !has_next(lane)
                break
            end

            dist_searched += (lane.curve[end].s - s_base)
            s_base = -norm(VecE2(lane.curve[end].pos - next_lane_point(lane, roadway).pos)) # negative distance between lanes
            tag_target = next_lane(lane, roadway).tag

            if tag_target == tag_start
                break
            end
        end
    end

    # search upstream
    if has_prev(lane_start)
        dist_searched = s_base
        tag_target = prev_lane(lane_start, roadway).tag
        s_base = roadway[tag_target].curve[end].s + norm(VecE2(lane_start.curve[1].pos - prev_lane_point(lane_start, roadway).pos)) # end of the lane

        while dist_searched < max_distance_fore

            lane = roadway[tag_target]
            curveproj = proj(posG, lane, roadway, move_along_curves=false).curveproj
            sq_dist_to_curve2 = normsquared(VecE2(posG - lane[curveproj.ind, roadway].pos))

            if sq_dist_to_curve2 < sq_dist_to_curve - improvement_threshold

                sq_dist_to_curve = sq_dist_to_curve2
                s_proj = lane[curveproj.ind, roadway].s
                dist = s_base - s_proj  + dist_searched
                Δs = -dist
                retval = FrenetRelativePosition(
                            roadind, RoadIndex(curveproj.ind, tag_target),
                            Δs, curveproj.t, curveproj.ϕ)
            end

            if !has_prev(lane)
                break
            end

            dist_searched += s_base
            s_base = lane.curve[end].s + norm(VecE2(lane.curve[1].pos - prev_lane_point(lane, roadway).pos)) # length of this lane plus crossover
            tag_target = prev_lane(lane, roadway).tag

            if tag_target == tag_start
                break
            end
        end
    end

    retval
end
get_frenet_relative_position(veh_fore::Entity{S, D, I}, veh_rear::Entity{S, D, I}, roadway::Roadway) where {S,D, I} = get_frenet_relative_position(posg(veh_fore.state), posf(veh_rear.state).roadind, roadway)


text:
# sample from a harmonic potential in 1D using GJF
using Plots
using Printf
using Random
using LinearAlgebra
using ForwardDiff
using BasicMD

include("potentials.jl")

β = 5.0;
γ = 1.5;
# M = Float64[2.1];
M = 2.1;
q₀ = [0.0];
p₀ = [0.0];
Δt = 1e-1;
n_iters = 10^4;

V = x->Harmonic(x);
cfg = ForwardDiff.GradientConfig(V, q₀);
gradV! = (gradV, x)-> ForwardDiff.gradient!(gradV, V, x, cfg);

sampler = GJF(gradV!, β, γ, M, Δt)

Random.seed!(100);
Q₀ = copy(q₀);
P₀ = copy(p₀);
sample_trajectory!([Q₀, P₀], sampler, options=MDOptions(n_iters=n_iters));
@printf("In Place (Q,P) after %d iterations: (%g,%g)\n",n_iters, Q₀[1], P₀[1]);
#
Random.seed!(100);
Xvals = sample_trajectory([q₀, p₀], sampler, options=MDOptions(n_iters=n_iters,n_save_iters=n_iters));
Q = Xvals[end][1][1]
P = Xvals[end][2][1]
@printf("(Q,P) after %d iterations: (%g,%g)\n",n_iters, Q[1], P[1]);
#
Random.seed!(100);
Xvals = sample_trajectory([q₀, p₀], sampler, options=MDOptions(n_iters=n_iters));
histogram([X[1][1] for X in Xvals],label="Samples",normalize=true)
qq=LinRange(-2,2,200)
plot!(qq, sqrt((β)/(2*π))*exp.(-0.5 * β * qq.^2),label="Density")
xlabel!("q")
ylabel!("Frequency")

max_stars_repo_name: gideonsimpson/JuBasicMD


text:
"""
    "Truncate" the vertices of `g` to obtain a graph with every vertex of degree 3.
"""
function truncate(g::AbstractSimpleGraph)

end


text:
using Knet, CUDArt, Base.Test

a=rand(3,5)

info("KUdense")
b=convert(KUdense,a)
c=convert(Array,b)
@test a==c

info("GPU")
d=gpucopy(a)
e=cpucopy(d)
@test a==e

info("KUdense + GPU")
f=gpucopy(b)
g=cpucopy(f)
h=convert(Array,g)
@test a==h

info("GPU + KUdense")
i=convert(KUdense,d)
j=cpucopy(i)
k=convert(Array,j)
@test a==k

aa=sprand(3,5,.5)

info("KUsparse")
dd=convert(KUsparse,aa)
ee=convert(SparseMatrixCSC,dd)
@test aa==ee

info("KUsparse + GPU")
mm=gpucopy(dd)
nn=cpucopy(mm)
oo=convert(SparseMatrixCSC,nn)
@test oo==aa

# info("No GPU only conversion")


text:
# Modified from Shreyas Kowshik's implementation.

# Returns an episode's worth of experience
# @everywhere # TODO.
function run_episode(env::MDPEnvironment, policy::Union{CategoricalPolicy, DiagonalGaussianPolicy}, num_steps::Int)
    experience = []
    s = reset!(env)
    for i in 1:num_steps
        a = get_action(policy, s)
        ast_action = translate_ast_action(env.problem.sim, a, actiontype(env.problem))
        s′, r, done, _ = step!(env, ast_action)
        push!(experience, (s,a,r,s′))
        s = s′
        if done
            break
        end
    end
    return experience
end

function get_rollouts(env, policy, num_steps::Int)
    g = []
    for w in workers()
        push!(g, run_episode(env, policy, num_steps))
    end
    fetch.(g)
end

"""
Process and extraction information from rollouts
"""
function collect_rollouts!(env, solver, policy, episode_buffer::Buffer, num_steps::Int, stats_buffer::Buffer)
    rollouts = get_rollouts(env, policy, num_steps)

    # Process the variables
    states = []
    actions = []
    rewards = []
    next_states = []
    advantages = []
    returns = []
    log_probs = []
    kl_params = []

    # Logging statistics
    rollout_returns = []

    for ro in rollouts
        episode_states = []
        episode_actions = []
        episode_rewards = []
        episode_next_states = []

        for i in 1:length(ro)
            (episode_state, episode_action, episode_reward, episode_next_state) = ro[i]
            push!(episode_states, episode_state)
            push!(episode_actions, episode_action)
            push!(episode_rewards, episode_reward)
            push!(episode_next_states, episode_next_state)

            if typeof(episode_action) <: Int64
                push!(log_probs, log_prob(policy, reshape(episode_state, length(episode_state), 1), [episode_action]).data)
            elseif typeof(episode_action) <: Array
                push!(log_probs, log_prob(policy, reshape(episode_state, length(episode_state), 1), episode_action).data)
            end

            # Kl divergence variables
            if typeof(policy) <: CategoricalPolicy
                push!(kl_params, log_probs[end])
            elseif typeof(policy) <: DiagonalGaussianPolicy
                μ = policy.μ(reshape(episode_state, length(episode_state), 1)).data
                logΣ = policy.logΣ.data
                push!(kl_params, [μ, logΣ])
            end
        end

        episode_advantages = gae(policy, episode_states, episode_actions, episode_rewards, episode_next_states, num_steps; γ=solver.gae_γ, λ=solver.gae_λ)
        episode_advantages = normalize(episode_advantages)

        if solver.terminate_horizon
            episode_returns = disconunted_returns(episode_rewards)
        else
            solver.verbose ? println("\33[2J") : nothing # Clear screen
            solver.verbose ? println("\033[1;1H") : nothing # Move cursor to (1,1)
            solver.verbose ? println("Appending value of last state to returns") : nothing
            episode_returns = disconunted_returns(episode_rewards, policy.value_net(episode_states[end]).data[1])
        end

        push!(states, hcat(episode_states...))
        push!(actions, hcat(episode_actions...))
        push!(rewards, hcat(episode_rewards...))
        push!(next_states, hcat(episode_next_states...))
        push!(advantages, hcat(episode_advantages...))
        push!(returns, hcat(episode_returns...))

        # Variables for logging
        push!(rollout_returns, episode_returns)
    end

    # Normalize advantage across all processes
    # advantages = normalize_across_procs(hcat(advantages...), solver.episode_length)

    episode_buffer.exp_dict["states"] = hcat(states...)
    episode_buffer.exp_dict["actions"] = hcat(actions...)
    episode_buffer.exp_dict["rewards"] = hcat(rewards...)
    episode_buffer.exp_dict["next_states"] = hcat(next_states...)
    episode_buffer.exp_dict["advantages"] = hcat(advantages...)
    episode_buffer.exp_dict["returns"] = hcat(returns...)
    episode_buffer.exp_dict["log_probs"] = hcat(log_probs...)
    episode_buffer.exp_dict["kl_params"] = copy(kl_params)

    # Log the statistics
    add!(stats_buffer, "rollout_rewards", sum(hcat(rewards...)))

    solver.verbose ? println("Rollout rewards : $(sum(hcat(rewards...)))") : nothing
end