text:
using HullGMP
using Test
using LazySets

nnet = read_nnet("nnet/dnn2.nnet")

delta1 = 0.1
delta2 = 0.05
delta3 = 0.025
delta4 = 0.0125
delta5 = 0.00625

solver11 = SpeGuid(delta1)
solver12 = SpeGuid(delta2)
solver13 = SpeGuid(delta3)
solver14 = SpeGuid(delta4)
solver15 = SpeGuid(delta5)

solver21 = HullSearch(delta1)
solver22 = HullSearch(delta2)
solver23 = HullSearch(delta3)
solver24 = HullSearch(delta4)
solver25 = HullSearch(delta5)

center = fill(0.5, 3) # a 2-dimensional vector filled by 5.0
radius = fill(0.5, 3)
inputSet = Hyperrectangle(center, radius)

output_center = fill(0.0, 2)
output_radius = fill(0.15, 2)
outputSet = Hyperrectangle(output_center, output_radius) #output constraint

problem = Problem(nnet, inputSet, outputSet)

file = open("group2_dnn2.txt", "a")
print(file, "Test Result of group2_dnn2:\n\n")

#solver11
time11 = 0
solve(solver11, problem)
for i = 1:10
    timed_result =@timed solve(solver11, problem)
    print(file, "SpeGuid - test " * string(i) * " - Time: " * string(timed_result.time) * " s")
    print(file, " - Output: " * string(timed_result.value) * "\n")
    global time11 += timed_result.time
end
print(file, "delta = $(delta1). Average time: " * string(time11/10) * " s\n\n")

#solver12
time12 = 0
solve(solver12, problem)
for i = 1:10
    timed_result =@timed solve(solver12, problem)
    print(file, "SpeGuid - test " * string(i) * " - Time: " * string(timed_result.time) * " s")
    print(file, " - Output: " * string(timed_result.value) * "\n")
    global time12 += timed_result.time
end
print(file, "delta = $(delta2). Average time: " * string(time12/10) * " s\n\n")

#solver13
time13 = 0
solve(solver13, problem)
for i = 1:10
    timed_result =@timed solve(solver13, problem)
    print(file, "SpeGuid - test " * string(i) * " - Time: " * string(timed_result.time) * " s")
    print(file, " - Output: " * string(timed_result.value) * "\n")
    global time13 += timed_result.time
end
print(file, "delta = $(delta3). Average time: " * string(time13/10) * " s\n\n")

#solver14
time14 = 0
solve(solver14, problem)
for i = 1:10
    timed_result =@timed solve(solver14, problem)
    print(file, "SpeGuid - test " * string(i) * " - Time: " * string(timed_result.time) * " s")
    print(file, " - Output: " * string(timed_result.value) * "\n")
    global time14 += timed_result.time
end
print(file, "delta = $(delta4). Average time: " * string(time14/10) * " s\n\n")

#solver15
time15 = 0
solve(solver15, problem)
for i = 1:10
    timed_result =@timed solve(solver15, problem)
    print(file, "SpeGuid - test " * string(i) * " - Time: " * string(timed_result.time) * " s")
    print(file, " - Output: " * string(timed_result.value) * "\n")
    global time15 += timed_result.time
end
print(file, "delta = $(delta5). Average time: " * string(time15/10) * " s\n\n")

#solver21
time21 = 0
solve(solver21, problem)
for i = 1:10
    timed_result =@timed solve(solver21, problem)
    print(file, "HullSearch - test " * string(i) * " - Time: " * string(timed_result.time) * " s")
    print(file, " - Output: " * string(timed_result.value) * "\n")
    global time21 += timed_result.time
end
print(file, "delta = $(delta1). Average time: " * string(time21/10) * " s\n\n")

#solver22
time22 = 0
solve(solver22, problem)
for i = 1:10
    timed_result =@timed solve(solver22, problem)
    print(file, "HullSearch - test " * string(i) * " - Time: " * string(timed_result.time) * " s")
    print(file, " - Output: " * string(timed_result.value) * "\n")
    global time22 += timed_result.time
end
print(file, "delta = $(delta2). Average time: " * string(time22/10) * " s\n\n")

#solver23
time23 = 0
solve(solver23, problem)
for i = 1:10
    timed_result =@timed solve(solver23, problem)
    print(file, "HullSearch - test " * string(i) * " - Time: " * string(timed_result.time) * " s")
    print(file, " - Output: " * string(timed_result.value) * "\n")
    global time23 += timed_result.time
end
print(file, "delta = $(delta3). Average time: " * string(time23/10) * " s\n\n")

#solver24
time24 = 0
solve(solver24, problem)
for i = 1:10
    timed_result =@timed solve(solver24, problem)
    print(file, "HullSearch - test " * string(i) * " - Time: " * string(timed_result.time) * " s")
    print(file, " - Output: " * string(timed_result.value) * "\n")
    global time24 += timed_result.time
end
print(file, "delta = $(delta4). Average time: " * string(time24/10) * " s\n\n")

#solver25
time25 = 0
solve(solver25, problem)
for i = 1:10
    timed_result =@timed solve(solver25, problem)
    print(file, "HullSearch - test " * string(i) * " - Time: " * string(timed_result.time) * " s")
    print(file, " - Output: " * string(timed_result.value) * "\n")
    global time25 += timed_result.time
end
print(file, "delta = $(delta5). Average time: " * string(time25/10) * " s\n\n")

close(file)


text:
using SubstitutionTilings
using SubstitutionTilings.Penrose
import SubstitutionTilings.Penrose: ψ
using Test
using BenchmarkTools
using Plots

using Luxor

const L = Penrose.Qζ

setantialias(6)

#collars = SubstitutionTilings.CoreDefs.collars(penrose(), 6);
#length(collars)

width = 1280
height = 800
sc = 200
@draw begin
    colors = ["#DD93FC", "#E7977A", "#9B70AF", "#A0644F",]
    first_tile = hkite()
    tiling = substitute(penrose(), [first_tile], 1, Penrose.in_bounds, (w=width/sc, h=height/sc))
    println(typeof(tiling))
    setline(1)

    for tile in tiling
        origin()
        draw(tile, sc, colors[Penrose.color(tile)], :fill)
        origin()
        setcolor("black")
        scale(sc)
        #circle(embed_center(tile[1]), 0.1, :fill)
        sethue("black")
    end
    origin()
    setline(1)
    #draw(first_tile, sc/0.618, "black", :stroke)
end width height #"penrose.png"

width = 1280*2
height = 800*2
sc = 400
@draw begin
    colors = ["#DD93FC", "#E7977A", "#9B70AF", "#A0644F",]

    quadrants = Tiler(width, height, 1, 2, margin=5)

    begin
        pos = quadrants[1][1]
        first_tile = hkite()
        tiling = substitute(penrose(), [first_tile], 0, Penrose.in_bounds, (w=width/sc, h=height/sc))
        println(typeof(tiling))
        setline(1)

        for tile in tiling
            origin()
            Luxor.translate(pos)
            draw(tile, sc, colors[Penrose.color(tile)], :fill)
        end
        origin()
        Luxor.translate(pos)
        scale(sc)
        setline(2)
        setcolor("black")
        Luxor.line(Penrose.embed_nf_p(ζ^4),Penrose.embed_nf_p(L(1)), :stroke)
        Luxor.arrow(Penrose.embed_nf_p(ζ^4),Penrose.embed_nf_p(ζ^4+(L(1)-ζ^4)*(7//16)), arrowheadlength=0.1)
        Luxor.arrow(Penrose.embed_nf_p(ζ^4),Penrose.embed_nf_p(ζ^4+(L(1)-ζ^4)*(9//16)), arrowheadlength=0.1)
        Luxor.line(Penrose.embed_nf_p(ζ^6),Penrose.embed_nf_p(L(1)), :stroke)
        Luxor.arrow(Penrose.embed_nf_p(L(1)),Penrose.embed_nf_p(ζ^6+(L(1)-ζ^6)//2), arrowheadlength=0.1)
        Luxor.arrow(Penrose.embed_nf_p(L(1)),Penrose.embed_nf_p(ζ^6+(L(1)-ζ^6)*(3//8)), arrowheadlength=0.1)
        Luxor.arrow(Penrose.embed_nf_p(L(1)),Penrose.embed_nf_p(ζ^6+(L(1)-ζ^6)*(5//8)), arrowheadlength=0.1)
        Luxor.line(Penrose.embed_nf_p(ζ^4),Penrose.embed_nf_p(ζ^6), :stroke)
        Luxor.arrow(Penrose.embed_nf_p(ζ^6),Penrose.embed_nf_p(ζ^4+(ζ^6-ζ^4)*(1//2)), arrowheadlength=0.1)
    end

    begin
        pos = quadrants[2][1]
        first_tile = hdart()
        tiling = substitute(penrose(), [first_tile], 0, Penrose.in_bounds, (w=width/sc, h=height/sc))
        println(typeof(tiling))
        setline(1)

        for tile in tiling
            origin()
            Luxor.translate(pos)
            draw(tile, sc, colors[Penrose.color(tile)], :fill)
        end
        origin()
        Luxor.translate(pos)
        scale(sc)
        setline(2)
        setcolor("black")
        Luxor.line(Penrose.embed_nf_p(ζ^2-ζ^0//ϕ),Penrose.embed_nf_p(L(1-ζ^0//ϕ)), :stroke)
        Luxor.arrow(Penrose.embed_nf_p(ζ^2-ζ^0//ϕ),Penrose.embed_nf_p(ζ^2-ζ^0//ϕ+(L(1-ζ^0//ϕ)-(ζ^2-ζ^0//ϕ))//2), arrowheadlength=0.1)
        Luxor.line(Penrose.embed_nf_p(L(1-ζ^0//ϕ)),Penrose.embed_nf_p(ζ^8-ζ^0//ϕ), :stroke)
        Luxor.arrow(Penrose.embed_nf_p(L(1-ζ^0//ϕ)),Penrose.embed_nf_p(1-ζ^0//ϕ+((ζ^8-ζ^0//ϕ)-(1-ζ^0//ϕ))*(3//16)), arrowheadlength=0.1)
        Luxor.arrow(Penrose.embed_nf_p(L(1-ζ^0//ϕ)),Penrose.embed_nf_p(1-ζ^0//ϕ+((ζ^8-ζ^0//ϕ)-(1-ζ^0//ϕ))*(5//16)), arrowheadlength=0.1)
        Luxor.arrow(Penrose.embed_nf_p(L(1-ζ^0//ϕ)),Penrose.embed_nf_p(1-ζ^0//ϕ+((ζ^8-ζ^0//ϕ)-(1-ζ^0//ϕ))*(7//16)), arrowheadlength=0.1)
        Luxor.arrow(Penrose.embed_nf_p(L(1-ζ^0//ϕ)),Penrose.embed_nf_p(1-ζ^0//ϕ+((ζ^8-ζ^0//ϕ)-(1-ζ^0//ϕ))*(9//16)), arrowheadlength=0.1)
        Luxor.line(Penrose.embed_nf_p(ζ^8-ζ^0//ϕ),Penrose.embed_nf_p(ζ^2-ζ^0//ϕ), :stroke)
        Luxor.arrow(Penrose.embed_nf_p(ζ^8-ζ^0//ϕ),Penrose.embed_nf_p(ζ^8-ζ^0//ϕ-(ζ^8-ζ^2)*(7//16)), arrowheadlength=0.1)
        Luxor.arrow(Penrose.embed_nf_p(ζ^8-ζ^0//ϕ),Penrose.embed_nf_p(ζ^8-ζ^0//ϕ-(ζ^8-ζ^2)*(9//16)), arrowheadlength=0.1)

   end
end width height #"penrose.png"

pentagon = (([
    PenroseElem(mod(k+s,10),s,L(1))*PenroseElem(0,0,L(-1)) => Penrose.Hkite
    for k=0:2:10 for s=0:1
]));

width = 300
height = 300
sc = 50
@png begin
    colors = ["#DD93FC", "#E7977A", "#9B70AF", "#A0644F",]
    first_tile = hkite(0, false, L(0))
    tiling = pentagon

    for tile in tiling
        origin()
        draw(tile, sc, colors[Penrose.color(tile)], :fill)
    end
    origin()
    setline(1)
    draw(first_tile, sc, "black", :stroke)
end width height "pentagon.png"

trapezoid = Dict([
    hkite(0,0,L(0)),
    hdart(2,1, ζ^8//ϕ )])
width = 300
height = 300
sc = 50
@png begin
    colors = ["#DD93FC", "#E7977A", "#9B70AF", "#A0644F",]
    first_tile = hkite(0, false, L(0))
    tiling = trapezoid

    for tile in tiling
        origin()
        draw(tile, sc, colors[Penrose.color(tile)], :fill)
    end
    origin()
    setline(1)
    draw(first_tile, sc, "black", :stroke)
end width height "trapezoid.png"

# Comparison of Array and Dict operations
@assert substitute(penrose(), Dict([hkite(0,0,L(0))]), 20, Penrose.in_bounds, (w=50, h=50)) == Dict(substitute(penrose(), ([hkite(0,0,L(0))]), 20, Penrose.in_bounds, (w=50, h=50)))

t_dict = zeros(35)
n_dict = zeros(35)
for i in 1:35
    p = substitute(penrose(), Dict([hkite(0,0,L(0))]), 20, Penrose.in_bounds, (w=i*2, h=i*2));
    time = @timed empirical_frequency(pentagon, Dict(p))
    t_dict[i] = time.time
    n_dict[i] = length(p)
end # essentially linear in the amont of tiles: pretty good!

t_arr = zeros(35)
n_arr = zeros(35)
for i in 1:35
    p = substitute(penrose(), ([hkite(0,0,L(0))]), 20, Penrose.in_bounds, (w=i*2, h=i*2));
    time = @timed empirical_frequency(pentagon, (p))
    t_arr[i] = time.time
    n_arr[i] = length(p)
end
plot(n_dict,t_dict)
plot!(n_arr, t_arr)
plot((n_dict), (t_dict))
plot(log.(n_arr), log.(t_arr))
# Frequency counting with arrays is exponential in the amount of tiles
# with Dicts it is more like linear

tiling = substitute(penrose(), Dict([hkite(0,0,L(0))]), 20, Penrose.in_bounds, (w=50, h=50))
empirical_frequency(pentagon, tiling)

Penrose.frequency(pentagon, 4)
@testset "Frequencies" begin
    @test Penrose.frequency([hkite()], 4) == ψ
    @test Penrose.frequency([hdart()], 4) == 1-ψ
    @test Penrose.frequency(pentagon, 4) == 7*ψ - 4
    @test Penrose.frequency(pentagon, 5) == 7*ψ - 4
    @test Penrose.frequency(pentagon, 6) == 7*ψ - 4
end

ext: jl


text:
export rule

@rule NormalMixture((:p, k), Marginalisation) (q_out::Any, q_switch::Any, q_m::UnivariateNormalDistributionsFamily) = begin
    m_mean_k, v_mean_k = mean(q_m), cov(q_m)
    m_out, v_out = mean(q_out), cov(q_out)
    z_bar = probvec(q_switch)
    return GammaShapeRate(1.0 + 0.5 * z_bar[k], 0.5 * z_bar[k] * (v_out + v_mean_k + abs2(m_out - m_mean_k)))
end

@rule NormalMixture((:p, k), Marginalisation) (q_out::Any, q_switch::Any, q_m::MultivariateNormalDistributionsFamily) = begin
    m_mean_k, v_mean_k = mean(q_m), cov(q_m)
    m_out, v_out       = mean(q_out), cov(q_out)
    z_bar = probvec(q_switch)
    d = length(m_mean_k)
    return Wishart(1.0 + z_bar[k] + d, cholinv(z_bar[k]*( v_out + v_mean_k + (m_out - m_mean_k)*(m_out - m_mean_k)')))
end


text:
"""
DistCtrl4DistMan (C) 2020, Martin Gurtner
Numerical experiments for a paper on distributed optimization for distributed manipulation.
"""
module DistCtrl4DistMan

include("LinSolvers.jl")
include("ActuatorArrayModule.jl")
include("ObjectAgentModule.jl")
include("ConvAnalysisModule.jl")
include("ADMMModule.jl")
include("PlottingModule.jl")
include("SimulationModule.jl")
include("CentralizedSolutionModule.jl")

using LinearAlgebra
using Statistics
using Plots
using Printf

using .LinSolvers
using .ActuatorArrayModule
using .ObjectAgentModule
using .ADMMModule
using .ConvAnalysisModule
using .PlottingModule
using .SimulationModule
using .CentralizedSolutionModule

"""
    genRandomPositions(N, params, xlim, ylim, minMutualDist)

Generates random positions for `N` agents. The random positions respect the
limits specified by tuples `xlim` and 'ylim'. The dictionary `params` must
contain a key "space_dim" containing a number 2 or 3 depending on whether the
positions are to be generated in 2D or 3D. If in 3D, the third positional
coordinate is copied from `params["z0"]`.

# Examples
```julia-repl
julia> genRandomPositions(3, Dict{String, Any}("space_dim" => 2), (-1,1), (-1,1), 0.1)
3-element Array{Tuple{Float64,Float64},1}:
 (0.1794808388697966, -0.12069979900680572)
 (0.21384769250194813, 0.33873868561424914)
 (0.608968185187885, -0.03527899822165992)
```
"""
function genRandomPositions(N, params, xlim, ylim, minMutualDist)
    @assert (params["space_dim"] == 2 || params["space_dim"] == 3) "Unsupported dimension."

    F = typeof(minMutualDist);
    oa_pos = NTuple{params["space_dim"], F}[];

    N_attempts = 5000;
    for l in 1:N_attempts
        # Generate a random position for kth agent
        xrand = (xlim[2] - xlim[1])*rand(F) + xlim[1];
        yrand = (ylim[2] - ylim[1])*rand(F) + ylim[1];
        if params["space_dim"] == 2
            oa_posk = ( xrand, yrand );
        else params["space_dim"] == 3
            oa_posk = ( xrand, yrand, params["z0"]);
        end

        # oa_pos = ((n-1)*aa.dx/2, (n-1)*aa.dx/2, z0);
        # Check whether outher poitns are far enough from the kth point
        theyAreFarEnough = true;
        for pos in oa_pos
            if norm(pos .- oa_posk) < minMutualDist
                theyAreFarEnough = false;
                continue;
            end
        end
        # if they are far enough, break from the loop
        if theyAreFarEnough
            push!(oa_pos, oa_posk);

            length(oa_pos) == N && break
        end
    end

    if length(oa_pos) != N
        error("Failed to generate random positions of the agents.")
    end

    oa_pos;
end

"""
    initAgents(aa, oa_pos, params[, Fdes])

Returns an array of agents located at positions given by in `oa_pos`. Each agents
is initialized with a list of actuators it considers in the force model and with
a list of actuators it optimizes over. These lists are created based on agent's
position and values in `params["maxDist"]``. If `Fdes` argument is missing, the
desired force is generated randomly.
"""
function initAgents(
    aa::ActuatorArray{T, U},
    oa_pos::Union{Array{Tuple{T,T},1}, Array{Tuple{T,T,T}, 1}},
    params::Dict{String,Any};
    algorithm=:admm,
    Fdes = missing) where {T<:Real, U<:Unsigned}

    N = size(oa_pos)[1];

    if params["platform"] == :DEP
        agents = Array{ObjectAgent_DEP{T, U}, 1}();
        randActuatorCommands = 2*π*rand(T, aa.nx, aa.ny);
        for k in 1:N
            # Generate list of used actuators for each object agent
            aL, a_used = genActList(aa, oa_pos[k], params["maxDist"][1], params["maxDist"][2]);
            if ismissing(Fdes)
                Fdes_i = calcDEPForce(aa, oa_pos[k], randActuatorCommands).*(1/2,1/2,1/2);
            else
                Fdes_i = Fdes[k];
            end
            push!(agents, ObjectAgent_DEP("Agent $k", oa_pos[k], (Fdes_i[1], Fdes_i[2], Fdes_i[3]), aa, aL, a_used));
        end
    elseif params["platform"] == :MAG
        agents = Array{ObjectAgent_MAG{T, U}, 1}();
        randActuatorCommands = rand(T, aa.nx, aa.ny);
        for k in 1:N
            # Generate list of used actuators for each object agent
            aL, a_used = genActList(aa, (oa_pos[k][1], oa_pos[k][2], T(0)), params["maxDist"][1], params["maxDist"][2]);
            if ismissing(Fdes)
                Fdes_i = calcMAGForce(aa, oa_pos[k], randActuatorCommands)./2;
            else
                Fdes_i = Fdes[k];
            end
            push!(agents, ObjectAgent_MAG("Agent $k", oa_pos[k], (Fdes_i[1], Fdes_i[2]), aa, aL, a_used, params["λ"]));
        end
    elseif params["platform"] == :ACU
        agents = Array{ObjectAgent_ACU{T, U}, 1}();
        for k in 1:N
            # Generate list of used actuators for each object agent
            aL, a_used = genActList(aa, oa_pos[k], params["maxDist"][1], params["maxDist"][2]);

            push!(agents, ObjectAgent_ACU("Agent $k", oa_pos[k], params["Pdes"], aa, aL, a_used));
        end
    else
        error("Unsupported platform")
    end

    return agents;
end

"""
    averageActuatorCommands(agents, aa)

Averages the required actuators commands from individual agents in the `agents`
array and returns actuator commands that can be applied to the actuator array.
"""
function averageActuatorCommands(agents, aa)
    ActuatorCommands = fill(NaN, aa.nx, aa.ny);
    actuatorIsUsedByN = zeros(size(ActuatorCommands))
    for agent in agents
        for (k, (i, j)) in enumerate(agent.actList)
            actuatorIsUsedByN[i,j] += 1;
            if isnan(ActuatorCommands[i,j])
                ActuatorCommands[i,j] = agent.zk[k];
            else
                ActuatorCommands[i,j] += 1/actuatorIsUsedByN[i,j] * (agent.zk[k] - ActuatorCommands[i,j]);
            end
        end
    end

    return ActuatorCommands;
end

"""
    printErrStats(exp_data, params)

Print some error statistics saying how good is the result of the distributed
optimization.
"""
function printErrStats(exp_data, params)
        println("Req.  \t\t\t | Dev.")

        printf(fmt::String,args...) = @eval @printf($fmt,$(args...));

        for (k, agnt) in enumerate(exp_data["Agents"])
            if exp_data["Platform"] == :ACU
                printf("%4.1f \t\t\t%4.1f\n", agnt.Pdes, params["calcForce"](exp_data["ActuatorArray"], agnt.pos, exp_data["ActuatorCommands"]))
            else
                force_dim = length(agnt.Fdes);
                if force_dim > 1
                    Fdev = params["calcForce"](exp_data["ActuatorArray"], agnt.pos, exp_data["ActuatorCommands"]) ./ agnt.Fdes_sc;
                    printf("[" * "% 1.3f, "^(force_dim-1) *"% 1.3f]\t [" * "% 1.3f, "^(force_dim-1) *"% 1.3f]\n", agnt.Fdes..., Fdev...)
                else
                    error("Unsupported platform")
                end
            end
        end
        # Value of the cost function in the last iteration
        println("\nFinal-itreation error")
        # println([exp_data["conv_data"][end][i].cost for i in 1:size(exp_data)[3]])
        println([agent.cost for agent in exp_data["conv_data"][end]])

        nothing
end

"""
    showPlots(exp_data, params; <keyword arguments>)

Visualize the results of the optimization: agents on the actuator array together
with the desired and developed forces.

# Arguments
- `plotConvAnalysis::Bool=false`: make also plots with values of the cost function
of individual agents and the global convergence measure.
- `plotActuatorCommands::Bool=false`: plot also values of the actuator commands.
- `showCircles::Bool=false`: plot circles encircling the used actuators.

# Examples
```julia-repl
julia> params, exps_data = run_exp();
julia> showPlots(exps_data[1], params, plotConvAnalysis=true);
```
"""
function showPlots(exp_data, params::Dict{String,Any}; plotConvAnalysis=false, plotActuatorCommands=false, showCircles=false)
    if showCircles
        distCircles = params["maxDist"];
    else
        distCircles = missing;
    end

    ## Plot the actuator array together with the objects
    if exp_data["Platform"] == :ACU
        if !plotConvAnalysis
            Plots.display(plot(plot(exp_data["ActuatorArray"], agents=exp_data["Agents"], legend=nothing, distCircles=distCircles),
            heatmap(exp_data["ActuatorArray"], params["z0"], exp_data["ActuatorCommands"], agents=exp_data["Agents"], N=100, box=:grid, colorbar=false),
            layout=@layout [a b]));
        else
            Plots.display(plot(plot(exp_data["ActuatorArray"], agents=exp_data["Agents"], legend=nothing, distCircles=distCircles),
            heatmap(exp_data["ActuatorArray"], params["z0"], exp_data["ActuatorCommands"], agents=exp_data["Agents"], N=100, box=:grid, colorbar=false),
            plot(errf(exp_data["conv_data"]), leg=false, yticks=0:0.5:1, border=:origin, title="Error convergence", titlefontsize=8),
            plot(conv_measure(exp_data["conv_data"]), leg=false, title="Error convergence", titlefontsize=8, yscale=:log10), size=(500,500), layout=@layout [[a b]; c{0.1h}; d{0.1h}]));
        end
    else
        Fdev_arr = Array{NTuple{params["force_dim"], Float64}, 1}();
        for agnt in exp_data["Agents"]
            Fdev = params["calcForce"](exp_data["ActuatorArray"], agnt.pos, exp_data["ActuatorCommands"]);
            push!(Fdev_arr, Fdev);
        end

        if plotActuatorCommands
            actuatorCommands = exp_data["ActuatorCommands"];
        else
            actuatorCommands = fill(NaN, exp_data["ActuatorArray"].nx, exp_data["ActuatorArray"].ny);
        end

        if !plotConvAnalysis
            Plots.display(plot(exp_data["ActuatorArray"], actuatorCommands = actuatorCommands, agents=exp_data["Agents"], dev_forces=Fdev_arr, showReqForces=true, Fscale=params["Fscale"], legend=nothing, size=(500,500), distCircles=distCircles));
        else
            Plots.display(plot(plot(exp_data["ActuatorArray"], actuatorCommands = actuatorCommands, agents=exp_data["Agents"], dev_forces=Fdev_arr, showReqForces=true, Fscale=params["Fscale"], legend=nothing, distCircles=distCircles),
            plot(errf(exp_data["conv_data"]), leg=false, yticks=0:0.5:1, border=:origin, title="Error convergence", titlefontsize=8),
            plot(conv_measure(exp_data["conv_data"]), leg=false, title="Error convergence", titlefontsize=8, yscale=:log10),  size=(500,600), layout=@layout [a; b{0.1h};c{0.1h}]));
        end
    end

    nothing
end

"""
    savePlots(exp_data, params; <keyword arguments>)

Save figure visualizing the results of the optimization: agents on the actuator
array together with the desired and developed forces.

# Arguments
- `plotActuatorCommands::Bool=false`: plot also values of the actuator commands.
- `filename::String="TestArray.pdf"`: filename of the saved figure
- `showCircles::Bool=false`: plot circles encircling the used actuators.

# Examples
```julia-repl
julia> params, exps_data = run_exp();
julia> savePlots(exps_data[1], params, plotConvAnalysis=true);
```
"""
function savePlots(exp_data, params::Dict{String,Any}; plotActuatorCommands=false, filename="TestArray.pdf", showCircles=false)
    if showCircles
        distCircles = params["maxDist"];
    else
        distCircles = missing;
    end

    if exp_data["Platform"] == :ACU
        plot(plot(exp_data["ActuatorArray"], agents=exp_data["Agents"], legend=nothing, distCircles=distCircles),
        heatmap(exp_data["ActuatorArray"], params["z0"], exp_data["ActuatorCommands"], agents=exp_data["Agents"], N=120, box=:grid, colorbar=false),
        size=(1000,500), layout=@layout [a b]);
        annotate!([(agnt.pos[1]+exp_data["ActuatorArray"].dx/6, agnt.pos[2]+exp_data["ActuatorArray"].dx/6, text(string(round(calcPressure(exp_data["ActuatorArray"], agnt.pos, exp_data["ActuatorCommands"]))), 12, :white, :left, :bottom, :bold)) for agnt in exp_data["Agents"]], subplot=2);
        savefig(filename);
    else
        Fdev_arr = Array{NTuple{params["force_dim"], Float64}, 1}();
        for agnt in exp_data["Agents"]
            Fdev = params["calcForce"](exp_data["ActuatorArray"], agnt.pos, exp_data["ActuatorCommands"]);
            push!(Fdev_arr, Fdev);
        end

        if plotActuatorCommands
            actuatorCommands = exp_data["ActuatorCommands"];
        else
            actuatorCommands = fill(NaN, exp_data["ActuatorArray"].nx, exp_data["ActuatorArray"].ny);
        end

        plot(exp_data["ActuatorArray"], actuatorCommands=actuatorCommands, agents=exp_data["Agents"], dev_forces=Fdev_arr, showReqForces=true, Fscale=params["Fscale"], legend=nothing, size=(500,500), lwidth=8, distCircles=distCircles);
        savefig(filename);
    end

    nothing
end

const DEP_params = Dict{String, Any}("platform" => :DEP,
    "dx" => 100.0e-6,
    "λ" => 1.0e4,
    "ρ" => 1.0e-4,
    "space_dim" => 3,
    "force_dim" => 3,
    "z0" => 100e-6,
    "maxDist" => (300e-6, 550e-6),
    "calcForce" => calcDEPForce,
    "Fscale" => 1e10);

const MAG_params = Dict{String, Any}("platform" => :MAG,
    "dx" => 25.0e-3,
    "λ" => 1.3,
    "ρ" => 1.5,
    "space_dim" => 2,
    "force_dim" => 2,
    "z0" => 0,
    "maxDist" => (50e-3, 75e-3),
    "calcForce" => calcMAGForce,
    "Fscale" => 20);

const ACU_params = Dict{String, Any}("platform" => :ACU,
    "dx" => 10.0e-3,
    "λ" => 1.0e4,
    "ρ" => 1.0e-4,
    "space_dim" => 3,
    "force_dim" => 1,
    "z0" => -65e-3,
    "maxDist" => (35e-3, 100e-3),
    "calcForce" => calcPressure,
    "Pdes" => 1500.0);

"""
    runExp(<keyword arguments>)

Run a numerical experiment testing the distributed optimization solver.

# Arguments
- `platform::Symbol=:MAG`: the distributed manipulation platform. Must be one of these three: `:MAG`, `:DEP`, `:ACU`.
- `N_exps::Int=1`: number of numerical experiments to be carried out
- `N_iter::Int=50`: number of iterations of the solver
- `N_acts::Tuple{Int,Int}=(8,8)`: the actuator array used in experiments is a `N_acts`by``N_acts matrix of actuators
- `N_agnts::Int=4`: number of agents (i.e. manipulated objects)
- `method::Symbol=:freedir`: decides the variant of the optimization problem. Either `:freedir` for minimizing the norm of the difference between the desired and developed force or `:fixdir` for generating the desired direction and minimizing the difference in the magnitude of the developed and desired force.
- `algorithm::Symbol=:admm`: chooses the centralized (:centralized) or distributed (:admm) optimization algorithm.
- `showplots::Bool=true`: plot the results of the numerical experiments
- `plotActuatorCommands::Bool=false`: plot the results of the numerical experiments
- `plotNeighbtCircles::Bool=false`: encircle the used actuators by each agent
- `saveplots::Bool=false`: save the plot. Applicable only when `N_exps=1`.
- `errstats::Bool=false`: print some error statistics of each experiment
- `plotConvergenceRates::Bool=false`: plot the global convergence measures of all experiments in one figure
- `plotConvergenceRatesIndividual::Bool=false`: plot the convergence measures for each experiment. `convanalysis` must be true for this to be working.
- `convanalysis::Bool=false`: if `true`, data from all iterations form all experiments are stored in `exps_data` array which is returned by `runExp()` function
- `figfilename::String="test_fig.pdf"`: the file name of the saved plot
- `display::Symbol=:iter`: ∈{:none, :iter, :final}
- `agent_positions::Union{Array{Tuple{Float64,Float64},1}, Array{Tuple{Float64,Float64,Float64},1}}=missing`: array of positions of the agents. If `missing`, the positions are generated randomly.
- `Fdes::Union{Array{Tuple{Float64,...,Float64},1}=missing`: array of desired forces. If `missing`, the forces are generated randomly.
- `params::Dict{String, Any}=Dict{String, Any}()`: a dictionary of parameters used of the experiments. Check the code for details.

# Examples
```julia-repl
julia> runExp(platform=:DEP, N_iter=25, N_agnts=6, N_acts=16);
```
"""
function runExp(;platform=:MAG,
    N_acts=(8,8),
    N_iter=50, N_exps = 1, N_agnts = 4,
    method = :freedir,
    algorithm = :admm,
    showplots = true,
    plotActuatorCommands = false,
    plotNeighbtCircles = false,
    saveplots = false,
    errstats = false,
    plotConvergenceRates = false,
    plotConvergenceRatesIndividual = false,
    convanalysis = true,
    figfilename = "test_fig.pdf",
    display = :iter, # display = {:none, :iter, :final}
    agent_positions = missing,
    Fdes = missing,
    ignore_list = Array{Tuple{Int64, Int64},1}(),
    params = Dict{String, Any}(),
    stopping_value = Inf)

    # Used number types
    F = Float64;  # Doesnt work with FLoat32
    U = UInt64;

    @assert ismissing(agent_positions) || (length(agent_positions) == N_agnts) "Number of positions does not match the number of agents"

    if platform == :DEP
        params = merge(DEP_params, params);

        aa = ActuatorArray(N_acts[1], N_acts[2], params["dx"], params["dx"]/2, :square, ignore_list);

        xlim = (2*aa.dx, (aa.nx-3)*aa.dx);
        ylim = (2*aa.dx, (aa.ny-3)*aa.dx);

        minMutualDist = 1.5*aa.dx;   # Set the minimum mutual distance between the agents
    elseif platform == :MAG
        params = merge(MAG_params, params);

        aa = ActuatorArray(N_acts[1], N_acts[2], params["dx"], params["dx"], :coil, ignore_list);

        xlim = (aa.dx, (aa.nx-2)*aa.dx);
        ylim = (aa.dx, (aa.ny-2)*aa.dx);

        minMutualDist = 1.1*aa.dx;   # Set the minimum mutual distance between the agents
    elseif platform == :ACU
        params = merge(ACU_params, params);

        aa = ActuatorArray(N_acts[1], N_acts[2], params["dx"], params["dx"], :circle, ignore_list);

        xlim = (2*aa.dx, (aa.nx-3)*aa.dx);
        ylim = (2*aa.dx, (aa.ny-3)*aa.dx);

        minMutualDist = 3.5*aa.dx;   # Set the minimum mutual distance between the agents
    end

    t_elapsed = Array{F}(undef, N_exps);

    exps_data = Any[];

    # Stopping criteria
    if stopping_value != Inf
        # stopping_criteria = history -> (last(conv_measure(history)) < stopping_value)
        stopping_criteria = history -> (last(errf_sum(history)) < stopping_value)
    else
        stopping_criteria = history -> false
    end

    for i in 1:N_exps
        display==:iter && @printf("------ %s - %s - Experiment #%d, λ: %f, ρ: %f ------\n", platform, method, i, params["λ"], params["ρ"])

        # Initialize the actuator commands to NaN
        actuatorCommands = fill(F(NaN), aa.nx, aa.ny);

        # Randomly generate agent positions if they are not provided
        if ismissing(agent_positions)
            agnts_pos = genRandomPositions( N_agnts, params, xlim, ylim, minMutualDist);
        else
            agnts_pos = agent_positions;
        end

        # Initialize the agents
        agents = initAgents(aa, agnts_pos, params, algorithm = algorithm, Fdes = Fdes);

        exps_data_i = Dict{String, Any}("Agents" => agents, "ActuatorArray" => aa, "Platform" => platform, "params" => params);
        convanalysis && push!(exps_data, exps_data_i);
        convanalysis && push!(exps_data_i, "conv_data" => ConvAnalysis_Data());

        t_elapsed[i] = 0
        if algorithm == :admm
            elapsed = @elapsed resolveNeighbrRelations!(agents);
            # @printf("resolveNeighbrRelations: %3.2f ms.\n", 1e3*elapsed)

            t_elapsed[i] += elapsed

            elapsed, hist = admm(agents,
                λ = params["λ"], ρ = params["ρ"],
                log = convanalysis,
                maxiter = N_iter,
                method = method,
                stoping_criteria = stopping_criteria
                );

            t_elapsed[i] += elapsed
        elseif algorithm == :centralized
            actuatorCommands, t_elapsed[i], hist = centralized_solution(
                agents,
                aa,
                λ = params["λ"],
                log = convanalysis,
                maxiter = N_iter,
                stoping_criteria = stopping_criteria
                );

                # actuatorCommands = copy(transpose(reshape(actuatorCommands, (aa.nx, aa.ny))))
        else
            error("Only :admm or :centralized algorithms are supported.")
        end

        # Average the actuator commands over the agents
        actuatorCommands = averageActuatorCommands(agents, aa);

        convanalysis && push!(exps_data_i, "conv_data" => hist);
        (convanalysis || showplots || saveplots) && push!(exps_data_i, "ActuatorCommands" => actuatorCommands);
        errstats && printErrStats(exps_data_i, params);
        showplots && showPlots(exps_data_i, params, plotConvAnalysis=(convanalysis && plotConvergenceRatesIndividual), plotActuatorCommands=plotActuatorCommands, showCircles=plotNeighbtCircles);
        saveplots && savePlots(exps_data_i, params, plotActuatorCommands=plotActuatorCommands, filename=figfilename, showCircles=plotNeighbtCircles);

        display==:iter && @printf("Time elapsed: %3.2f ms.\n", 1e3*t_elapsed[i])
    end

    (display==:iter || display==:final) && @printf("%s - %s - λ: %f, ρ: %f - Time elapsed - mean: %3.2f ms, meadian: %3.2f ms, std: %3.2f ms.\n", platform, method, params["λ"], params["ρ"], 1e3*mean(t_elapsed), 1e3*median(t_elapsed), 1e3*std(t_elapsed));

    if plotConvergenceRates && convanalysis
        Plots.display(plot([conv_measure(exps_datak["conv_data"]) for exps_datak in exps_data], legend=nothing, linecolor=:red, linealpha=0.3,
        leg=false, size=(700,200), yscale=:log10, xlabel="Iteration", ylabel="Convergence measure"))

        Plots.display(plot!( mean([conv_measure(exps_datak["conv_data"]) for exps_datak in exps_data]), legend=nothing, linecolor=:blue, linewidth=3))

        savefig("TestConv_all.pdf")
    end

    convanalysis ? (params, exps_data, t_elapsed) : (params);
end

end


text:
using FromPython
using Test

include("ast.jl")


text:
type BrownianBridge{I <: Integer}
  size_::I
  t::Vector{Float64}
  sqrtdt::Vector{Float64}
  bridgeIndex::Vector{I}
  leftIndex::Vector{I}
  rightIndex::Vector{I}
  leftWeight::Vector{Float64}
  rightWeight::Vector{Float64}
  stdDev::Vector{Float64}
end

function BrownianBridge(tg::TimeGrid)
  size_ = length(tg.times) - 1
  t = zeros(size_)
  for i in eachindex(t)
    t[i] = tg[i + 1]
  end
  bb = BrownianBridge(size_, t, zeros(size_), zeros(Int, size_), zeros(Int, size_), zeros(Int, size_), zeros(size_), zeros(size_), zeros(size_))
  initialize!(bb)

  return bb
end

function BrownianBridge(steps::Int)
  t = Vector{Float64}(steps)
  for i in eachindex(t)
    t[i] = float(i)
  end

  bb = BrownianBridge(steps, t, Vector{Float64}(steps), ones(Int, steps), ones(Int, steps), ones(Int, steps), Vector{Float64}(steps), Vector{Float64}(steps), Vector{Float64}(steps))
  initialize!(bb)

  return bb
end

function initialize!(bb::BrownianBridge)
  bb.sqrtdt[1] = sqrt(bb.t[1])
  for i = 2:bb.size_
    bb.sqrtdt[i] = sqrt(bb.t[i] - bb.t[i-1])
  end

  # mapVec is used to indicate which points are already constructed.
  # if mapVec[i] is zero, path point i is yet unconstructed
  # mapVec[i] - 1 is the index of the variate that constructs path point # i
  mapVec = zeros(Int, bb.size_)

  # the first point in the construction is the global step
  mapVec[bb.size_] = 1
  # the global step is constructed from the first variate
  bb.bridgeIndex[1] = bb.size_
  # the variance of the global step
  bb.stdDev[1] = sqrt(bb.t[bb.size_])
  # the global step to the last point in time is special
  bb.leftWeight[1] = bb.rightWeight[1] = 0.0

  j = 0
  for i = 1:bb.size_ - 1
    # find the next unpopulated entry in the map
    while mapVec[j+1] != 0
      j += 1
    end
    k = j
    # find the next populated entry from there
    while mapVec[k+1] == 0
      k += 1
    end
    # l - 1 is now the index of the point to be constructed next
    l = j + ((k - 1 - j) >> 1)
    mapVec[l+1] = i
    # the i-th Gaussian variate will be used to set point l - 1
    bb.bridgeIndex[i+1] = l + 1
    bb.leftIndex[i+1] = j + 1
    bb.rightIndex[i+1] = k + 1

    if j != 0
      bb.leftWeight[i+1] = (bb.t[k+1] - bb.t[l+1]) / (bb.t[k+1] - bb.t[j])
      bb.rightWeight[i+1] = (bb.t[l+1] - bb.t[j]) / (bb.t[k+1] - bb.t[j])
      bb.stdDev[i+1] = sqrt(((bb.t[l+1] - bb.t[j]) * (bb.t[k+1] - bb.t[l+1])) / (bb.t[k+1] - bb.t[j]))
    else
      bb.leftWeight[i+1] = (bb.t[k+1] - bb.t[l+1]) / bb.t[k+1]
      bb.rightWeight[i+1] = bb.t[l+1] / bb.t[k+1]
      bb.stdDev[i+1] = sqrt(bb.t[l+1] * (bb.t[k+1] - bb.t[l+1]) / bb.t[k+1])
    end
    j = k + 1
    if j >= bb.size_
      # wrap around
      j = 0
    end
  end

  return bb
end

function transform!(bb::BrownianBridge, randomVec::Vector{Float64}, output::Vector{Float64})
  length(randomVec) == bb.size_ || error("size mismatch $(bb.size_) $(length(randomVec))")
  # we use the output to store the path
  output[bb.size_] = bb.stdDev[1] * randomVec[1]

  for i = 2:bb.size_
    j = bb.leftIndex[i]
    k = bb.rightIndex[i]
    l = bb.bridgeIndex[i]

    if j != 1
      output[l] = bb.leftWeight[i] * output[j-1] + bb.rightWeight[i] * output[k] + bb.stdDev[i] * randomVec[i]
    else
      output[l] = bb.rightWeight[i] * output[k] + bb.stdDev[i] * randomVec[i]
    end
  end

  # after which we calculate the variations and normalize to unit times
  for i = bb.size_:-1:2
    output[i] -= output[i-1]
    output[i] /= bb.sqrtdt[i]
  end

  output[1] /= bb.sqrtdt[1]

  return output
end


text:
# ----------------------------------------------------------------
# Fast processing using same memory space
# ----------------------------------------------------------------

# What's the size again?
#=
julia> size(valuation)
(10000, 3, 100000)
=#

using Statistics: std

# Find standard deviation of each attribute for each security
function std_by_security(valuation)
    (nstates, nattr, n) = size(valuation)
    result = zeros(n, nattr)
    for i in 1:n
        for j in 1:nattr
            result[i, j] = std(valuation[:, j, i])
        end
    end
    return result
end

# does it work properly?

result = std_by_security(valuation);
result[1:5, :]
#=
julia> result = std_by_security(valuation);

julia> result[1:5, :]
5×3 Array{Float64,2}:
 0.286829  0.287704  0.285188
 0.286607  0.288997  0.28929
 0.290648  0.287691  0.288957
 0.288234  0.291172  0.290039
 0.290519  0.289879  0.288602
=#

# performance check: ~5s

@benchmark std_by_security($valuation) seconds=30
#=
julia> @benchmark std_by_security($valuation) seconds=30
BenchmarkTools.Trial:
  memory estimate:  22.38 GiB
  allocs estimate:  600002
  --------------
  minimum time:     4.983 s (5.22% GC)
  median time:      5.002 s (5.35% GC)
  mean time:        5.021 s (5.34% GC)
  maximum time:     5.125 s (5.74% GC)
  --------------
  samples:          6
  evals/sample:     1
=#

# Let's try the distributed way.

# Ensure all processes have loaded necessary packages
@everywhere using Statistics: std

# Distribution version
function std_by_security2(valuation)
    (nstates, nattr, n) = size(valuation)
    result = SharedArray{Float64}(n, nattr)
    @sync @distributed for i in 1:n
        for j in 1:nattr
            result[i, j] = std(valuation[:, j, i])
        end
    end
    return result
end

@benchmark std_by_security2($valuation) seconds=30
#=
julia> result = std_by_security2(valuation);

julia> result[1:5, :]
5×3 Array{Float64,2}:
 0.286829  0.287704  0.285188
 0.286607  0.288997  0.28929
 0.290648  0.287691  0.288957
 0.288234  0.291172  0.290039
 0.290519  0.289879  0.288602

julia> @benchmark std_by_security2($valuation) seconds=30
BenchmarkTools.Trial:
  memory estimate:  228.67 KiB
  allocs estimate:  4812
  --------------
  minimum time:     880.181 ms (0.00% GC)
  median time:      908.827 ms (0.00% GC)
  mean time:        910.734 ms (0.00% GC)
  maximum time:     940.846 ms (0.00% GC)
  --------------
  samples:          33
  evals/sample:     1
=#

# how much faster?
#=
julia> 4983 / 880
5.6625
=#


text:
# ---
# title: 1582. Special Positions in a Binary Matrix
# id: problem1582
# author: Tian Jun
# date: 2020-10-31
# difficulty: Easy
# categories: Array
# link: <https://leetcode.com/problems/special-positions-in-a-binary-matrix/description/>
# hidden: true
# ---
#
# Given a `rows x cols` matrix `mat`, where `mat[i][j]` is either `0` or `1`,
# return _the number of special positions in`mat`._
#
# A position `(i,j)` is called **special**  if `mat[i][j] == 1` and all other
# elements in row `i` and column `j` are `0` (rows and columns are **0-indexed**
# ).
#
#
#
# **Example 1:**
#
#
#
#     Input: mat = [[1,0,0],
#                   [0,0, **1** ],
#                   [1,0,0]]
#     Output: 1
#     Explanation: (1,2) is a special position because mat[1][2] == 1 and all other elements in row 1 and column 2 are 0.
#
#
# **Example 2:**
#
#
#
#     Input: mat = [[ **1** ,0,0],
#                   [0, **1** ,0],
#                   [0,0, **1** ]]
#     Output: 3
#     Explanation: (0,0), (1,1) and (2,2) are special positions.
#
#
# **Example 3:**
#
#
#
#     Input: mat = [[0,0,0, **1** ],
#                   [ **1** ,0,0,0],
#                   [0,1,1,0],
#                   [0,0,0,0]]
#     Output: 2
#
#
# **Example 4:**
#
#
#
#     Input: mat = [[0,0,0,0,0],
#                   [ **1** ,0,0,0,0],
#                   [0, **1** ,0,0,0],
#                   [0,0, **1** ,0,0],
#                   [0,0,0,1,1]]
#     Output: 3
#
#
#
#
# **Constraints:**
#
#   * `rows == mat.length`
#   * `cols == mat[i].length`
#   * `1 <= rows, cols <= 100`
#   * `mat[i][j]` is `0` or `1`.
#
#
## @lc code=start
using LeetCode

## add your code here:
## @lc code=end


text:
function GaussianRandomGenerator(arg0::RandomGenerator)
    return GaussianRandomGenerator((RandomGenerator,), arg0)
end

function next_normalized_double(obj::GaussianRandomGenerator)
    return jcall(obj, "nextNormalizedDouble", jdouble, ())
end


text:
# This file is a part of SimilaritySearch.jl

using SearchModels, Random
import SearchModels: combine, mutate, config_type

@with_kw struct BeamSearchSpace <: AbstractSolutionSpace
    bsize = 1:4:16
    origin::BeamSearch
end

function _create_bs(o::BeamSearch, bsize)
    BeamSearch(hints=o.hints, bsize=bsize, beam=o.beam, vstate=o.vstate)
end

Base.eltype(::BeamSearchSpace) = BeamSearch
Base.rand(space::BeamSearchSpace) = _create_bs(space.origin, rand(space.bsize))
combine(a::BeamSearch, b::BeamSearch) = _create_bs(a, div(a.bsize + b.bsize, 2))
mutate(space::BeamSearchSpace, c::BeamSearch, iter) = _create_bs(c, SearchModels.scale(c.bsize, s=1.1, lower=1))

@with_kw struct IHCSearchSpace <: AbstractSolutionSpace
    restarts = 1:4:16
    origin::IHCSearch
end

function _create_ihc(o::IHCSearch, restarts)
    IHCSearch(hints=o.hints, restarts=restarts, localimprovements=o.localimprovements, vstate=o.vstate)
end

Base.eltype(::IHCSearchSpace) = IHCSearch
Base.rand(space::IHCSearchSpace) = _create_ihc(space.origin, rand(space.restarts))
combine(a::IHCSearch, b::IHCSearch) = _create_ihc(a, div(a.restarts + b.restarts, 2))
mutate(space::IHCSearchSpace, c::IHCSearch, iter) = _create_ihc(c, SearchModels.scale(c.restarts, s=1.1, lower=1))

"""
    callback(opt::OptimizeParametersCallback, index)

SearchGraph's callback for adjunting search parameters
"""
function callback(opt::OptimizeParametersCallback, index)
    sample = unique(rand(1:length(index), opt.numqueries))
    queries = index[sample]

    error_function = if opt.error === :recall
        seq = ExhaustiveSearch(index.dist, index.db; ksearch=opt.ksearch)
        perf = Performance(seq, queries, opt.ksearch; popnearest=true)
        function error_function1(c)
            copy!(index.search_algo, c)
            p = probe(perf, index)
            index.verbose && println(stderr, "== SearchGraph optimizing recall, err: ", p, ", using configuration: ", c)
            1 / (p.macrorecall + 1)
        end
    elseif opt.error === :distance
        function error_function2(c)
            d = 0.0
            copy!(index.search_algo, c)
            for q in queries
                d += maximum(keys(search(index, q, opt.ksearch)))
            end
            d /= length(queries)

            index.verbose && println(stderr, "== SearchGraph optimizing covering radius, err: ", d, ", using configuration: ", c)
            d
        end
    elseif opt.error == :distance_and_searchtime
        function error_function3(c)
            d = 0.0
            copy!(index.search_algo, c)
            t = time()
            for q in queries
                d += maximum(search(index, q, opt.ksearch))
            end
            t = time() - t
            d *= t
            index.verbose && println(stderr, "== SearchGraph optimizing distance and searchtime, err: ", d, " using configuration: ", c)
            d
        end
    else
        error("unknown $(opt.error), valid options are :recall, :distance, and :distance_and_searchtime")
    end

    space = index.search_algo isa BeamSearch ? BeamSearchSpace(origin=index.search_algo) : IHCSearchSpace(origin=index.search_algo)
    bestlist = search_models(space, error_function, opt.initialpopulation; maxpopulation=opt.maxpopulation, maxiters=opt.maxiters, tol=opt.tol)
    config, err = bestlist[1]
    copy!(index.search_algo, config)
    index.verbose && println(stderr, "== finished optimization SearchGraphdistance, err: ", err, ", with configuration: ", config)
    index
end


text:
using DiffEqFlux, Optim, OrdinaryDiffEq, DiffEqSensitivity, Test

function f(du,u,p,t)
  du[1] = u[2]
  du[2] = -p[1]
end

function condition(u,t,integrator) # Event when event_f(u,t) == 0
  u[1]
end

function affect!(integrator)
  integrator.u[2] = -integrator.p[2]*integrator.u[2]
end

cb = ContinuousCallback(condition,affect!)
u0 = [50.0,0.0]
tspan = (0.0,15.0)
p = [9.8, 0.8]
prob = ODEProblem(f,u0,tspan,p)
sol = solve(prob,Tsit5(),callback=cb)

function loss(θ)
  sol = solve(prob,Tsit5(),p=[9.8,θ[1]],callback=cb,sensealg=ForwardDiffSensitivity())
  target = 20.0
  abs2(sol[end][1] - target)
end

res = DiffEqFlux.sciml_train(loss,[0.8],BFGS())
@test loss(res.minimizer) < 1

function loss(θ)
  sol = solve(prob,Tsit5(),p=[9.8,θ[1]],callback=cb,sensealg=ReverseDiffAdjoint())
  target = 20.0
  abs2(sol[end][1] - target)
end

res = DiffEqFlux.sciml_train(loss,[0.8],BFGS())
@test loss(res.minimizer) < 1


text:
using BitBasis, LegibleLambdas

export @λ, @lambda

# same with nbatch
Base.length(r::AbstractRegister{B}) where {B} = B

addbits!(n::Int) = @λ(register -> addbits!(register, n))
insert_qubits!(loc::Int; nqubits::Int = 1) =
    @λ(register -> insert_qubits!(register, loc; nqubits = n))

nremain(r::AbstractRegister) = nqubits(r) - nactive(r)
nbatch(r::AbstractRegister{B}) where {B} = B

"""
    focus(f, register, locs...)

Call a callable `f` under the context of `focus`. See also [`focus!`](@ref).

# Example

print the focused register

```julia
julia> r = ArrayReg(bit"101100")
ArrayReg{1,Complex{Float64},Array...}
    active qubits: 6/6

julia> focus(x->(println(x);x), r, 1, 2);
ArrayReg{1,Complex{Float64},Array...}
    active qubits: 2/6
```
"""
focus!(f::Base.Callable, r::AbstractRegister, locs::Int...) = focus(f, r, locs)

"""
    focus!(locs...) -> f(register) -> register

Lazy version of [`focus!`](@ref), this returns a lambda which requires a register.
"""
focus!(locs::Int...) = focus!(locs)
focus!(locs::NTuple{N,Int}) where {N} = @λ(register -> focus!(register, locs))
focus!(locs::UnitRange) = @λ(register -> focus!(register, locs))
focus!(f::Base.Callable, r::AbstractRegister, loc::Int) = focus(f, r, (loc,))
focus!(f::Base.Callable, r::AbstractRegister, locs) =
    relax!(f(focus!(r, locs)), locs; to_nactive = nqubits(r))

relax!(r::AbstractRegister; to_nactive::Int = nqubits(r)) =
    relax!(r, (); to_nactive = to_nactive)

"""
    relax!(locs::Int...; to_nactive=nqubits(register)) -> f(register) -> register

Lazy version of [`relax!`](@ref), it will be evaluated once you feed a register
to its output lambda.
"""
relax!(locs::Int...; to_nactive::Union{Nothing,Int} = nothing) =
    relax!(locs; to_nactive = to_nactive)

function relax!(locs::NTuple{N,Int}; to_nactive::Union{Nothing,Int} = nothing) where {N}
    lambda = function (r::AbstractRegister)
        if to_nactive === nothing
            return relax!(r, locs; to_nactive = nqubits(r))
        else
            return relax!(r, locs; to_nactive = to_nactive)
        end
    end

    @static if VERSION < v"1.1.0"
        return LegibleLambda("(register->relax!(register, locs...; to_nactive))", lambda)
    else
        return LegibleLambda(:(register -> relax!(register, locs...; to_nactive)), lambda)
    end
end

## Measurement
measure!(postprocess::PostProcess, op, reg::AbstractRegister; kwargs...) =
    measure!(postprocess, op, reg, AllLocs(); kwargs...)
measure!(postprocess::PostProcess, reg::AbstractRegister, locs; kwargs...) =
    measure!(postprocess, ComputationalBasis(), reg, locs; kwargs...)
measure!(postprocess::PostProcess, reg::AbstractRegister; kwargs...) =
    measure!(postprocess, ComputationalBasis(), reg, AllLocs(); kwargs...)
measure!(op, reg::AbstractRegister, args...; kwargs...) =
    measure!(NoPostProcess(), op, reg, args...; kwargs...)
measure!(reg::AbstractRegister, args...; kwargs...) =
    measure!(NoPostProcess(), reg, args...; kwargs...)

measure(op, reg::AbstractRegister; kwargs...) = measure(op, reg, AllLocs(); kwargs...)
measure(reg::AbstractRegister, locs; kwargs...) =
    measure(ComputationalBasis(), reg, locs; kwargs...)
measure(reg::AbstractRegister; kwargs...) =
    measure(ComputationalBasis(), reg, AllLocs(); kwargs...)

# focus! to specify locations, we that we only need to consider full-space measure in the future.
function measure!(
    postprocess::PostProcess,
    op,
    reg::AbstractRegister,
    locs;
    kwargs...,
) where {MODE}
    nbit = nactive(reg)
    focus!(reg, locs)
    res = measure!(postprocess, op, reg, AllLocs(); kwargs...)
    if postprocess isa RemoveMeasured
        relax!(reg; to_nactive = nbit - length(locs))
    else
        relax!(reg, locs; to_nactive = nbit)
    end
    res
end

function measure(op, reg::AbstractRegister, locs; kwargs...) where {MODE}
    nbit = nactive(reg)
    focus!(reg, locs)
    res = measure(op, reg, AllLocs(); kwargs...)
    relax!(reg, locs; to_nactive = nbit)
    res
end

"""
    basis(register) -> UnitRange

Returns an `UnitRange` of the all the bits in the Hilbert space of given register.
"""
BitBasis.basis(r::AbstractRegister) = basis(nqubits(r))

invorder!(r::AbstractRegister) = reorder!(r, Tuple(nactive(r):-1:1))

"""
    partial_tr(locs) -> f(register)

Curried version of `partial_tr(register, locs)`.
"""
partial_tr(locs) = @λ(register -> partial_tr(register, locs))

"""
    select!(b::Integer) -> f(register)

Lazy version of [`select!`](@ref). See also [`select`](@ref).
"""
select!(bits...) = @λ(register -> select!(register, bits...))

function Base.iterate(it::AbstractRegister{B}, state = 1) where {B}
    if state > B
        return nothing
    else
        return viewbatch(it, state), state + 1
    end
end

# fallback printing
function Base.show(io::IO, reg::AbstractRegister)
    summary(io, reg)
    print(io, "\n    active qubits: ", nactive(reg), "/", nqubits(reg))
end

ρ(x) = density_matrix(x)


text:
####
#### prognostic_to_primitive! and primitive_to_prognostic!
####

####
#### Wrappers (entry point)
####

"""
    prognostic_to_primitive!(atmos::AtmosModel, prim::Vars, prog::Vars, aux::Vars)

Convert prognostic variables `prog` to primitive
variables `prim` for the atmos model `atmos`.

!!! note
    The only field in `aux` required for this
    method is the geo-potential.
"""
prognostic_to_primitive!(atmos::AtmosModel, prim::Vars, prog::Vars, aux) =
    prognostic_to_primitive!(
        atmos,
        atmos.moisture,
        prim,
        prog,
        Thermodynamics.internal_energy(
            prog.ρ,
            prog.ρe,
            prog.ρu,
            gravitational_potential(atmos.orientation, aux),
        ),
    )

"""
    primitive_to_prognostic!(atmos::AtmosModel, prog::Vars, prim::Vars, aux::Vars)

Convert primitive variables `prim` to prognostic
variables `prog` for the atmos model `atmos`.

!!! note
    The only field in `aux` required for this
    method is the geo-potential.
"""
primitive_to_prognostic!(atmos::AtmosModel, prog::Vars, prim::Vars, aux) =
    primitive_to_prognostic!(
        atmos,
        atmos.moisture,
        prog,
        prim,
        gravitational_potential(atmos.orientation, aux),
    )

####
#### prognostic to primitive
####

function prognostic_to_primitive!(
    atmos,
    moist::DryModel,
    prim::Vars,
    prog::Vars,
    e_int::AbstractFloat,
)
    ts = PhaseDry(atmos.param_set, e_int, prog.ρ)
    prim.ρ = prog.ρ
    prim.u = prog.ρu ./ prog.ρ
    prim.p = air_pressure(ts)
end

function prognostic_to_primitive!(
    atmos,
    moist::EquilMoist,
    prim::Vars,
    prog::Vars,
    e_int::AbstractFloat,
)
    FT = eltype(prim)
    ts = PhaseEquil(
        atmos.param_set,
        e_int,
        prog.ρ,
        prog.moisture.ρq_tot / prog.ρ,
        # 20,       # can improve test error with better convergence
        # FT(1e-3), # can improve test error with better convergence
    )
    prim.ρ = prog.ρ
    prim.u = prog.ρu ./ prog.ρ
    prim.p = air_pressure(ts)
    prim.moisture.q_tot = PhasePartition(ts).tot
end

function prognostic_to_primitive!(
    atmos,
    moist::NonEquilMoist,
    prim::Vars,
    prog::Vars,
    e_int::AbstractFloat,
)
    q_pt = PhasePartition(
        prog.moisture.ρq_tot / prog.ρ,
        prog.moisture.ρq_liq / prog.ρ,
        prog.moisture.ρq_ice / prog.ρ,
    )
    ts = PhaseNonEquil(atmos.param_set, e_int, prog.ρ, q_pt)
    prim.ρ = prog.ρ
    prim.u = prog.ρu ./ prog.ρ
    prim.p = air_pressure(ts)
    prim.moisture.q_tot = PhasePartition(ts).tot
    prim.moisture.q_liq = PhasePartition(ts).liq
    prim.moisture.q_ice = PhasePartition(ts).ice
end

####
#### primitive to prognostic
####

function primitive_to_prognostic!(
    atmos,
    moist::DryModel,
    prog::Vars,
    prim::Vars,
    e_pot::AbstractFloat,
)
    ts = PhaseDry_ρp(atmos.param_set, prim.ρ, prim.p)
    e_kin = prim.u' * prim.u / 2

    prog.ρ = prim.ρ
    prog.ρu = prim.ρ .* prim.u
    prog.ρe = prim.ρ * total_energy(e_kin, e_pot, ts)
end

function primitive_to_prognostic!(
    atmos,
    moist::EquilMoist,
    prog::Vars,
    prim::Vars,
    e_pot::AbstractFloat,
)
    ts = PhaseEquil_ρpq(
        atmos.param_set,
        prim.ρ,
        prim.p,
        prim.moisture.q_tot,
        true,
    )
    e_kin = prim.u' * prim.u / 2

    prog.ρ = prim.ρ
    prog.ρu = prim.ρ .* prim.u
    prog.ρe = prim.ρ * total_energy(e_kin, e_pot, ts)
    prog.moisture.ρq_tot = prim.ρ * PhasePartition(ts).tot
end

function primitive_to_prognostic!(
    atmos,
    moist::NonEquilMoist,
    prog::Vars,
    prim::Vars,
    e_pot::AbstractFloat,
)
    q_pt = PhasePartition(
        prim.moisture.q_tot,
        prim.moisture.q_liq,
        prim.moisture.q_ice,
    )
    ts = PhaseNonEquil_ρpq(atmos.param_set, prim.ρ, prim.p, q_pt)
    e_kin = prim.u' * prim.u / 2

    prog.ρ = prim.ρ
    prog.ρu = prim.ρ .* prim.u
    prog.ρe = prim.ρ * total_energy(e_kin, e_pot, ts)
    prog.moisture.ρq_tot = prim.ρ * PhasePartition(ts).tot
    prog.moisture.ρq_liq = prim.ρ * PhasePartition(ts).liq
    prog.moisture.ρq_ice = prim.ρ * PhasePartition(ts).ice
end

max_stars_repo_name: jm-c/CLIMA

max_issues_repo_name: jm-c/CLIMA

max_forks_repo_name: jm-c/CLIMA


text:
abstract type VLAbstractModelObject end

# concrete types -
struct VLResult{T}
    value::T
end

struct VLJuliaModelObject <: VLAbstractModelObject

    # data -
    path_to_model_file::String
    path_to_output_dir::String
    defaults_file_name::String
    model_type::Symbol

    # constructor -
    function VLJuliaModelObject(path_to_model_file, path_to_output_dir;
        defaults_file_path::String="Defaults.toml", model_type::Symbol=:dynamic)

        # build new model object -
        this = new(path_to_model_file, path_to_output_dir, defaults_file_path, model_type)
    end
end

function check(result::VLResult;
    logger::Union{Nothing,SimpleLogger}=nothing)::(Union{Nothing,T} where T <: Any)

    # ok, so check, do we have an error object?
    # Yes: log the error if we have a logger, then throw the error.
    # No: return the result.value

    try

        # Error case -
        if (isa(result.value, Exception) == true)

            # get the error object -
            error_object = result.value

            # get the error message as a String -
            error_message = sprint(showerror, error_object)

            # log -
            if (isnothing(logger) == false)
                with_logger(logger) do
                    @error(error_message)
                end
            end

            # throw -
            throw(result.value)
        end

        # default -
        return result.value

    catch error
        rethrow(error)
    end
end


text:
#####
##### Masked operators
#####

"""
    shrink_mask(mask)

Shrinks the subdomains where `mask == 1`.

Example:

```julia
using Test
mask = Bool[0, 0, 0, 1, 1, 1, 0, 0, 1, 1]
shrunken_mask = Bool[0, 0, 0, 0, 1, 0, 0, 0, 0, 1]
@test shrink_mask(mask) == shrunken_mask
```
"""
function shrink_mask(mask)
    return map(enumerate(mask)) do (i, m)
        if i == 1 || i == length(mask)
            m
        elseif m == 1 && mask[i - 1] == 0
            m = 0
        elseif m == 1 && mask[i + 1] == 0
            m = 0
        else
            m
        end
    end
end


text:
# AUTO GENERATED FILE - DO NOT EDIT

export ''_draggabledashboard

"""
    ''_draggabledashboard(;kwargs...)
    ''_draggabledashboard(children::Any;kwargs...)
    ''_draggabledashboard(children_maker::Function;kwargs...)

A DraggableDashboard component.
DraggableDashboard is a component for building
dashboards with draggable and resizable items.
It takes a list of children and display them in
div elements that can be moved around the page.
The initial size of each element can either be
defined with the layout argument or by wrapping
each element with the DashboardItem component.
By default, DraggableDashboard will saved the
position of the elements on client side, when
moved on the page. But you can also save it
on server side by defining a callback with :
`Input("<my-id>", "layout")`.
Keyword arguments:
- `children` (Array of a list of or a singular dash component, string or numbers | a list of or a singular dash component, string or number; optional): Children is a list of the elements to drag and resize on the dashboard.
It can be a list of dash Components and/or DashboardItem.
- `id` (String; optional): (string) The ID used to identify this component in Dash callbacks.
The id is also used to automatically save the layout on client side.
- `layout` (Array of Dicts; optional): Layout is a list of dictionnary with the format:
{x: number, y: number, w: number, h: number}
The index into the layout must match the id used on each item component with DashboardItem.
If you choose to use custom keys, you can specify that key in the layout
array objects like so:
{i: string, x: number, y: number, w: number, h: number}
The ID used to identify this component in Dash callbacks.
The id is also used to automatically save the layout on client side.
- `save` (Bool; optional): (bool) If False, then the layout is not saved in the browser.
Default value is True.
- `clearSavedLayout` (Bool; optional): (bool) If set to true, then the layout saved in the client browser
will be cleared on page load.
- `ncols` (Real; optional): (number) the default number of columns by item.
Default value is 6.
- `nrows` (Real; optional): (number) the default number of row by item.
Default value is 8.
- `width` (Real; optional): (number) width (in px).
Default value is 1200.
- `height` (Real; optional): (number) height of a row (in px).
Default value is 30.
- `className` (String; optional): (string) class passed to the react-grid-layout component
- `style` (Dict; optional): (dict) css style passed to the react-grid-layout component
"""
function ''_draggabledashboard(; kwargs...)
        available_props = Symbol[:children, :id, :layout, :save, :clearSavedLayout, :ncols, :nrows, :width, :height, :className, :style]
        wild_props = Symbol[]
        return Component("''_draggabledashboard", "DraggableDashboard", "dash_draggable", available_props, wild_props; kwargs...)
end

''_draggabledashboard(children::Any; kwargs...) = ''_draggabledashboard(;kwargs..., children = children)
''_draggabledashboard(children_maker::Function; kwargs...) = ''_draggabledashboard(children_maker(); kwargs...)

max_stars_repo_name: tcgvn/dash-draggable

max_issues_repo_name: tcgvn/dash-draggable

max_forks_repo_name: tcgvn/dash-draggable


text:
using KNITRO
using Test

@testset "Test old API" begin
    include("oldexamples.jl")
end

if KNITRO.KNITRO_VERSION >= v"11.0"
    @testset "Test C API" begin
        include("knitroapi.jl")
    end

    @testset "Test examples" begin
        include("testexamples.jl")
    end

    @testset "Test MathOptInterface" begin
        include("MOI_wrapper.jl")
        include("MOI_additional.jl")
    end

    @testset "Test JuMP" begin
        include("jump_soc.jl")
    end
end


text:
"""
    print_infos(ata_model)

# Description

Print the results of each build step of the ATA model.

# Arguments

- **`ata_model::AbstractModel`** : Required. Model processed with build functions.
"""
function print_infos(ata_model::AbstractModel)
    for m in ata_model.output.infos
        if m[1] == "danger"
            printstyled(m[2]; color = :red)
        else
            printstyled(m[2]; color = :green)
        end
    end
end

"""
    print_last_info(ata_model)

# Description

Print info of the last build step.

# Arguments

- **`ata_model::AbstractModel`** : Required. Model processed with build functions.
"""
function print_last_info(ata_model::AbstractModel)
    m = ata_model.output.infos[end]
    if m[1] == "danger"
        printstyled(m[2]; color = :red)
    else
        printstyled(m[2]; color = :green)
    end
end


text:
using ParameterizedFunctions, Test
using OrdinaryDiffEq, Calculus, ForwardDiff

f = @ode_def begin
    dx = -a
    dy = b
end a b

cb = ContinuousCallback((u,t,i) -> u[1], (integrator)->(println("Stopped.");integrator.p[2]=zero(integrator.p[2])))
function test_f(p)
  prob = ODEProblem(f,eltype(p).([1.0,0.0]),eltype(p).((0.0,1.0)),copy(p))
  integrator = init(prob,Tsit5(),abstol=1e-14,reltol=1e-14,callback=cb)
  step!(integrator)
  solve!(integrator).u[end]
end
p = [2.0, 1.0]
findiff = Calculus.finite_difference_jacobian(test_f,p)
fordiff = ForwardDiff.jacobian(test_f,p)
@test findiff ≈ fordiff

f2 = @ode_def begin
    dx = -x
    dy = b
end a b

function test_f2(p)
  prob = ODEProblem(f2,eltype(p).([1.0,0.0]),eltype(p).((0.0,1.0)),copy(p))
  integrator = init(prob,Tsit5(),abstol=1e-14,reltol=1e-14,callback=cb)
  step!(integrator)
  solve!(integrator).u[end]
end
p = [2.0, 1.0]
findiff = Calculus.finite_difference_jacobian(test_f2,p)
fordiff = ForwardDiff.jacobian(test_f2,p)
@test findiff ≈ fordiff

# Gradients and Hessians

function myobj(θ)
  f(u,p,t) = -θ[1]*u
  u0, _ = promote(10.0, θ[1])
  prob = ODEProblem(f, u0, (0.0, 1.0))
  sol = solve(prob, Tsit5())
  diff = sol.u - 10*exp.(-sol.t)
  return diff'diff
end

ForwardDiff.gradient(myobj, [1.0])
ForwardDiff.hessian(myobj, [1.0])

function myobj2(θ)
  f(du,u,p,t) = (du[1]=-θ[1]*u[1])
  u0, _ = promote(10.0, θ[1])
  prob = ODEProblem(f, [u0], (0.0, 1.0))
  sol = solve(prob, Tsit5())
  diff = sol[:,1] .- 10 .*exp.(-sol.t)
  return diff'diff
end

ForwardDiff.gradient(myobj2, [1.0])
ForwardDiff.hessian(myobj2, [1.0])

function myobj3(θ)
  f(u,p,t) = -θ[1]*u
  u0, _ = promote(10.0, θ[1])
  tspan_end, _ = promote(1.0, θ[1])
  prob = ODEProblem(f, u0, (0.0, tspan_end))
  sol = solve(prob, Tsit5())
  diff = sol.u - 10*exp.(-sol.t)
  return diff'diff
end

ForwardDiff.gradient(myobj3, [1.0])
ForwardDiff.hessian(myobj3, [1.0])

function myobj4(θ)
  f(du,u,p,t) = (du[1] = -θ[1]*u[1])
  u0, _ = promote(10.0, θ[1])
  tspan_end, _ = promote(1.0, θ[1])
  prob = ODEProblem(f, [u0], (0.0, tspan_end))
  sol = solve(prob, Tsit5())
  diff = sol[:,1] .- 10 .* exp.(-sol.t)
  return diff'diff
end

ForwardDiff.gradient(myobj4, [1.0])
ForwardDiff.hessian(myobj4, [1.0])


text:
module StructuredLoggers

using Logging, Dates, Match
import Crayons

export StructuredLogger,
    global_logger

struct StructuredLogger <: AbstractLogger
    app_name::String
    io::IO
    coloured::Bool
    StructuredLogger(app_name="", io=stderr, coloured=(io isa Base.TTY)) = new(app_name, io, coloured)
end

function Logging.handle_message(logger::StructuredLogger, level, message, _module, group, id, file, line; kwds...)
    # Dodgy hack for colours
    C(name) = logger.coloured ? getproperty(Crayons.Box, name) : identity

    # Note: because of the colors, rpad gets confused. So need to pad before the colour is applied.
    level_len = 7
    level_str = @match level begin
        Logging.Info => C(:GREEN_FG)(rpad("info", level_len))
        Logging.Debug => C(:GREEN_FG)(rpad("debug", level_len))
        Logging.Warn => C(:LIGHT_YELLOW_FG)(rpad("warning", level_len))
        Logging.Error => C(:LIGHT_RED_FG)(rpad("error", level_len))
        _ => C(:WHITE_FG)(rpad(string(level), level_len))
    end

    if logger.app_name != ""
        message = string(C(:LIGHT_GRAY_FG)("$(logger.app_name): ")) * message
    end
    println(logger.io, join([C(:LIGHT_GRAY_FG)(Dates.format(now(), "yyyy-mm-ddTHH:MM:SS:sss")),
                             "[$level_str]",
                             rpad(message, 30),
                             (map(collect(kwds)) do (key,val)
                              string(C(:LIGHT_CYAN_FG)(string(key))) * "=" * string(C(:MAGENTA_FG)(string(val)))
                              end)...], " "))
    flush(logger.io)
    nothing
end

Logging.shouldlog(::StructuredLogger, level, _module, group, id) = true
Logging.min_enabled_level(::StructuredLogger) = Logging.Info

end


text:
export ReedsSheppMetricSpace, DubinsQuasiMetricSpace
export ReedsSheppExact, DubinsExact, SimpleCarMetric

### (Quasi)Metric Typedefs/Evaluation
struct ReedsSheppExact{T<:AbstractFloat} <: Metric
    r::T
    s::T
    p::Vector{StepControl{T,2}}    # scratch space to avoid extra memory allocation

    ReedsSheppExact{T}(r::T, s::T) where T = new(r, s, Array(StepControl{T,2}, 5))
end
ReedsSheppExact{T<:AbstractFloat}(r::T = T(1), s::T = T(1)) = ReedsSheppExact{T}(r, s)
struct DubinsExact{T<:AbstractFloat} <: QuasiMetric
    r::T
    s::T
    p::Vector{StepControl{T,2}}    # scratch space to avoid extra memory allocation

    DubinsExact{T}(r::T, s::T) where T = new(r, s, Array(StepControl{T,2}, 3))
end
DubinsExact{T<:AbstractFloat}(r::T = T(1), s::T = T(1)) = DubinsExact{T}(r, s)
const SimpleCarMetric{T} = Union{ReedsSheppExact{T}, DubinsExact{T}}

evaluate(RS::ReedsSheppExact, v::SE2State, w::SE2State) = reedsshepp(v, w, RS.r, RS.s, RS.p)[1]
evaluate(D::DubinsExact, v::SE2State, w::SE2State) = dubins(v, w, D.r, D.s, D.p)[1]
changeprecision{T<:AbstractFloat}(::Type{T}, dist::ReedsSheppExact) = ReedsSheppExact(T(dist.r, dist.s))
changeprecision{T<:AbstractFloat}(::Type{T}, dist::DubinsExact) = DubinsExact(T(dist.r, dist.s))

## (Quasi)Metric Space Instantiation
function ReedsSheppMetricSpace{T}(r::T, s::T = T(1), lo::AbstractVector = zeros(SVector{2,T}),
                                                     hi::AbstractVector = ones(SVector{2,T}))
    BoundedStateSpace(SE2State([lo; T(0)]), SE2State([hi; 2*T(pi)]),
                      ChoppedMetric(ReedsSheppExact(r, s), Euclidean(), T(Inf)),
                      VectorView(1:2))
end
function DubinsQuasiMetricSpace{T}(r::T, s::T = T(1), lo::AbstractVector = zeros(SVector{2,T}),
                                                      hi::AbstractVector = ones(SVector{2,T}))
    BoundedStateSpace(SE2State([lo; T(0)]), SE2State([hi; 2*T(pi)]),
                      ChoppedQuasiMetric(DubinsExact(r, s), Euclidean(), T(Inf)),
                      VectorView(1:2))
end

function helper_data_structures{S<:SE2State,R<:ReedsSheppExact}(V::Vector{S}, M::ChoppedMetric{R})
    TreeDistanceDS(KDTree(statevec2mat(V)[1:2,:], M.lowerbound; reorder=false)), EmptyControlCache()
end
function helper_data_structures{S<:SE2State,R<:DubinsExact}(V::Vector{S}, M::ChoppedQuasiMetric{R})
    DS = TreeDistanceDS(KDTree(statevec2mat(V)[1:2,:], M.lowerbound; reorder=false))
    US = EmptyControlCache()   # TODO: non-empty ControlDS
    DS, US, DS, US
end
# Required for CLBM
evaluate(M::Euclidean, v::SE2State, w::SE2State) = norm(v[(1,2)] - w[(1,2)])
inrange{V,T<:AbstractFloat}(t::NearestNeighbors.NNTree{V}, v::SE2State{T}, r::T, s=false) = inrange(t, v[(1,2)], r, s)

### Steering
function propagate{T}(M::SimpleCarMetric{T}, v::SE2State{T}, u::StepControl{T,2})
    s = u.u[1]
    invr = u.u[2]
    if abs(u.t*s*invr) > 10*eps(T)
        SE2State(v.x + (sin(v.θ + u.t*s*invr) - sin(v.θ))/invr,
                 v.y + (cos(v.θ) - cos(v.θ + u.t*s*invr))/invr,
                 mod2piF(v.θ + u.t*s*invr))
    else
        SE2State(v.x + u.t*s*cos(v.θ),
                 v.y + u.t*s*sin(v.θ),
                 mod2piF(v.θ + u.t*s*invr))
    end
end
steering_control(RS::ReedsSheppExact, v::SE2State, w::SE2State) = copy(reedsshepp(v, w, RS.r, RS.s, RS.p)[2])
steering_control(D::DubinsExact, v::SE2State, w::SE2State) = copy(dubins(v, w, D.r, D.s, D.p)[2])
function collision_waypoints{T}(M::SimpleCarMetric{T}, v::SE2State{T}, u::StepControl{T,2})
    s = u.u[1]
    invr = u.u[2]
    θres = T(pi)/12
    m = floor(Int, u.t*s*invr / θres)
    if m == 0
        [v]
    else
        unshift!([SE2State(v.x + (sin(v.θ + i*θres) - sin(v.θ))/invr,
                           v.y + (cos(v.θ) - cos(v.θ + i*θres))/invr,
                           mod2piF(v.θ + i*θres)) for i in 1:m], v)
    end
end

### Simple Car Steering Nuts and Bolts
carsegment2stepcontrol{T}(t::Int, d::T) = StepControl(abs(d), SVector(T(sign(d)), T(t)))
scaleradius{T}(u::StepControl{T,2}, r::T) = StepControl(u.t*r, SVector(u.u[1], u.u[2]/r))
scalespeed{T}(u::StepControl{T,2}, s::T) = StepControl(u.t/s, SVector(u.u[1]*s, u.u[2]))
function scaleradius!(u::ZeroOrderHoldControl, r)
    for i in 1:length(u)
        u[i] = scaleradius(u[i], r)
    end
    u
end
function scalespeed!(u::ZeroOrderHoldControl, s)
    for i in 1:length(u)
        u[i] = scalespeed(u[i], s)
    end
    u
end

## Dubins Steering Nuts and Bolts
function dubinsLSL!{T}(d::T, a::T, b::T, c::T, path::Vector{StepControl{T,2}})
    ca, sa, cb, sb = cos(a), sin(a), cos(b), sin(b)
    tmp = 2 + d*d - 2*(ca*cb + sa*sb - d*(sa - sb))
    tmp < 0 && return c
    th = atan2(cb - ca, d + sa - sb)
    t = mod2piF(-a + th)
    p = sqrt(max(tmp, T(0)))
    q = mod2piF(b - th)
    cnew = t + p + q
    c <= cnew && return c
    path[1] = carsegment2stepcontrol(1, t)
    path[2] = carsegment2stepcontrol(0, p)
    path[3] = carsegment2stepcontrol(1, q)
    cnew
end

function dubinsRSR!{T}(d::T, a::T, b::T, c::T, path::Vector{StepControl{T,2}})
    ca, sa, cb, sb = cos(a), sin(a), cos(b), sin(b)
    tmp = 2 + d*d - 2*(ca*cb + sa*sb - d*(sb - sa))
    tmp < 0 && return c
    th = atan2(ca - cb, d - sa + sb)
    t = mod2piF(a - th)
    p = sqrt(max(tmp, T(0)))
    q = mod2piF(-b + th)
    cnew = t + p + q
    c <= cnew && return c
    path[1] = carsegment2stepcontrol(-1, t)
    path[2] = carsegment2stepcontrol(0, p)
    path[3] = carsegment2stepcontrol(-1, q)
    cnew
end

function dubinsRSL!{T}(d::T, a::T, b::T, c::T, path::Vector{StepControl{T,2}})
    ca, sa, cb, sb = cos(a), sin(a), cos(b), sin(b)
    tmp = d * d - 2 + 2 * (ca*cb + sa*sb - d * (sa + sb))
    tmp < 0 && return c
    p = sqrt(max(tmp, T(0)))
    th = atan2(ca + cb, d - sa - sb) - atan2(T(2), p)
    t = mod2piF(a - th)
    q = mod2piF(b - th)
    cnew = t + p + q
    c <= cnew && return c
    path[1] = carsegment2stepcontrol(-1, t)
    path[2] = carsegment2stepcontrol(0, p)
    path[3] = carsegment2stepcontrol(1, q)
    cnew
end

function dubinsLSR!{T}(d::T, a::T, b::T, c::T, path::Vector{StepControl{T,2}})
    ca, sa, cb, sb = cos(a), sin(a), cos(b), sin(b)
    tmp = -2 + d * d + 2 * (ca*cb + sa*sb + d * (sa + sb))
    tmp < 0 && return c
    p = sqrt(max(tmp, T(0)))
    th = atan2(-ca - cb, d + sa + sb) - atan2(-T(2), p)
    t = mod2piF(-a + th)
    q = mod2piF(-b + th)
    cnew = t + p + q
    c <= cnew && return c
    path[1] = carsegment2stepcontrol(1, t)
    path[2] = carsegment2stepcontrol(0, p)
    path[3] = carsegment2stepcontrol(-1, q)
    cnew
end

function dubinsRLR!{T}(d::T, a::T, b::T, c::T, path::Vector{StepControl{T,2}})
    ca, sa, cb, sb = cos(a), sin(a), cos(b), sin(b)
    tmp = (6 - d * d  + 2 * (ca*cb + sa*sb + d * (sa - sb))) / 8
    abs(tmp) >= 1 && return c
    p = 2*T(pi) - acos(tmp)
    th = atan2(ca - cb, d - sa + sb)
    t = mod2piF(a - th + p/2)
    q = mod2piF(a - b - t + p)
    cnew = t + p + q
    c <= cnew && return c
    path[1] = carsegment2stepcontrol(-1, t)
    path[2] = carsegment2stepcontrol(1, p)
    path[3] = carsegment2stepcontrol(-1, q)
    cnew
end

function dubinsLRL!{T}(d::T, a::T, b::T, c::T, path::Vector{StepControl{T,2}})
    ca, sa, cb, sb = cos(a), sin(a), cos(b), sin(b)
    tmp = (6 - d * d  + 2 * (ca*cb + sa*sb - d * (sa - sb))) / 8
    abs(tmp) >= 1 && return c
    p = 2*T(pi) - acos(tmp)
    th = atan2(-ca + cb, d + sa - sb)
    t = mod2piF(-a + th + p/2)
    q = mod2piF(b - a - t + p)
    cnew = t + p + q
    c <= cnew && return c
    path[1] = carsegment2stepcontrol(1, t)
    path[2] = carsegment2stepcontrol(-1, p)
    path[3] = carsegment2stepcontrol(1, q)
    cnew
end

function dubins{T}(s1::SE2State{T}, s2::SE2State{T}, r::T = T(1), s::T = T(1), pmin = Array(StepControl{T,2}, 3))
    v = (s2[(1,2)] - s1[(1,2)]) / r
    d = norm(v)
    th = atan2(v)
    a = mod2piF(s1.θ - th)
    b = mod2piF(s2.θ - th)

    cmin = T(Inf)
    cmin = dubinsLSL!(d, a, b, cmin, pmin)
    cmin = dubinsRSR!(d, a, b, cmin, pmin)
    cmin = dubinsRSL!(d, a, b, cmin, pmin)
    cmin = dubinsLSR!(d, a, b, cmin, pmin)
    cmin = dubinsRLR!(d, a, b, cmin, pmin)
    cmin = dubinsLRL!(d, a, b, cmin, pmin)
    cmin*r, scalespeed!(scaleradius!(pmin, r), s)
end

# function save_dubins_cache(Xmax = 5., Ymax = 5., Nx = 101, Ny = 101, Nt = 101,
#                            fname = joinpath(Pkg.dir("MotionPlanning"), "src", "statespaces",
#                                             @sprintf("Dubins_%.2f_%.2f_%d_%d_%d.gz", Xmax, Ymax, Nx, Ny, Nt)))
#     segment_to_string(s) = (s.t == 1 ? " L " : s.t == 0 ? " S " : " R ") * @sprintf("%.3f", s.d)
#     v = SE2State(0., 0., 0.)
#     fh = GZip.open(fname, "w")
#     for θ in [2pi*(i-1)/Nt for i in 1:Nt], y in linspace(-Ymax, Ymax, Ny), x in linspace(-Xmax, Xmax, Nx)
#         c, p = dubins(v, SE2State(x, y, θ))
#         cacheline = @sprintf("%.6f ", c) * prod([segment_to_string(s) for s in p]) * "\n"
#         write(fh, cacheline)
#     end
#     GZip.close(fh)
# end

## Reeds-Shepp Steering Nuts and Bolts
# Utilities (pedantic about typing to guard against problems)
R{T}(x::T, y::T) = sqrt(x*x + y*y), atan2(y, x)
function M{T}(t::T)
    m = mod2piF(t)
    m > pi ? m - 2*T(pi) : m
end
function Tau{T}(u::T, v::T, E::T, N::T)
    delta = M(u - v)
    A = sin(u) - sin(delta)
    B = cos(u) - cos(delta) - 1
    r, θ = R(E*A + N*B, N*A - E*B)
    t = 2*cos(delta) - 2*cos(v) - 2*cos(u) + 3
    t < 0 ? M(θ + pi) : M(θ)
end
Omega{T}(u::T, v::T, E::T, N::T, t::T) = M(Tau(u, v, E, N) - u + v - t)
timeflip{T}(s::SE2State{T}) = SE2State(-s.x, s.y, -s.θ)
reflect{T}(s::SE2State{T}) = SE2State(s.x, -s.y, -s.θ)
backwards{T}(s::SE2State{T}) = SE2State(s.x*cos(s.θ) + s.y*sin(s.θ), s.x*sin(s.θ) - s.y*cos(s.θ), s.θ)
function timeflip!(u::ZeroOrderHoldControl)
    for i in 1:length(u)
        u[i] = StepControl(u[i].t, SVector(-u[i].u[1], u[i].u[2]))
    end
    u
end
function reflect!(u::ZeroOrderHoldControl)
    for i in 1:length(u)
        u[i] = StepControl(u[i].t, SVector(u[i].u[1], -u[i].u[2]))
    end
    u
end
backwards!(u::ZeroOrderHoldControl) = reverse!(u)

# TODO: something about an enum type in Julia v0.4?
const POST, POST_T, POST_R, POST_B, POST_R_T, POST_B_T, POST_B_R, POST_B_R_T = 0, 1, 2, 3, 4, 5, 6, 7

# Gotta check 'em all
function reedsshepp{T}(s1::SE2State{T}, s2::SE2State{T}, r::T = T(1), s::T = T(1),  p = Array(StepControl{T,2}, 5))
    dx, dy = (s2[(1,2)] - s1[(1,2)]) / r
    ct, st = cos(s1.θ), sin(s1.θ)
    target = SE2State(dx*ct + dy*st, -dx*st + dy*ct, mod2piF(s2.θ - s1.θ))

    tTarget = timeflip(target)
    rTarget = reflect(target)
    trTarget = reflect(tTarget)
    bTarget = backwards(target)
    btTarget = timeflip(bTarget)
    brTarget = reflect(bTarget)
    btrTarget = reflect(btTarget)

    c, l = T(Inf), 0
    # Only Tim Wheeler knows what the hell is happening down below
    # (8.1) C S C
    b, c, l = LpSpLp!(target, c, l, p); b && (post = POST)
    b, c, l = LpSpLp!(tTarget, c, l, p); b && (post = POST_T)
    b, c, l = LpSpLp!(rTarget, c, l, p); b && (post = POST_R)
    b, c, l = LpSpLp!(trTarget, c, l, p); b && (post = POST_R_T)

    # (8.2) C S C
    b, c, l = LpSpRp!(target, c, l, p); b && (post = POST)
    b, c, l = LpSpRp!(tTarget, c, l, p); b && (post = POST_T)
    b, c, l = LpSpRp!(rTarget, c, l, p); b && (post = POST_R)
    b, c, l = LpSpRp!(trTarget, c, l, p); b && (post = POST_R_T)

    # (8.3) C|C|C
    b, c, l = LpRmLp!(target, c, l, p); b && (post = POST)
    # b, c, l = LpRmLp!(tTarget, c, l, p); b && (post = POST_T) # (redundant)
    b, c, l = LpRmLp!(rTarget, c, l, p); b && (post = POST_R)
    # b, c, l = LpRmLp!(trTarget, c, l, p); b && (post = POST_R_T) # (redundant)

    # (8.4) C|C C
    b, c, l = LpRmLm!(target, c, l, p); b && (post = POST)
    b, c, l = LpRmLm!(tTarget, c, l, p); b && (post = POST_T)
    b, c, l = LpRmLm!(rTarget, c, l, p); b && (post = POST_R)
    b, c, l = LpRmLm!(trTarget, c, l, p); b && (post = POST_R_T)
    b, c, l = LpRmLm!(bTarget, c, l, p); b && (post = POST_B)
    b, c, l = LpRmLm!(btTarget, c, l, p); b && (post = POST_B_T)
    b, c, l = LpRmLm!(brTarget, c, l, p); b && (post = POST_B_R)
    b, c, l = LpRmLm!(btrTarget, c, l, p); b && (post = POST_B_R_T)

    # (8.7) C Cu|Cu C
    b, c, l = LpRpuLmuRm!(target, c, l, p); b && (post = POST)
    b, c, l = LpRpuLmuRm!(tTarget, c, l, p); b && (post = POST_T)
    b, c, l = LpRpuLmuRm!(rTarget, c, l, p); b && (post = POST_R)
    b, c, l = LpRpuLmuRm!(trTarget, c, l, p); b && (post = POST_R_T)

    # (8.8) C|Cu Cu|C
    b, c, l = LpRmuLmuRp!(target, c, l, p); b && (post = POST)
    b, c, l = LpRmuLmuRp!(tTarget, c, l, p); b && (post = POST_T)
    b, c, l = LpRmuLmuRp!(rTarget, c, l, p); b && (post = POST_R)
    b, c, l = LpRmuLmuRp!(trTarget, c, l, p); b && (post = POST_R_T)

    # (8.9)
    b, c, l = LpRmSmLm!(target, c, l, p); b && (post = POST)
    b, c, l = LpRmSmLm!(tTarget, c, l, p); b && (post = POST_T)
    b, c, l = LpRmSmLm!(rTarget, c, l, p); b && (post = POST_R)
    b, c, l = LpRmSmLm!(trTarget, c, l, p); b && (post = POST_R_T)
    b, c, l = LpRmSmLm!(bTarget, c, l, p); b && (post = POST_B)
    b, c, l = LpRmSmLm!(btTarget, c, l, p); b && (post = POST_B_T)
    b, c, l = LpRmSmLm!(brTarget, c, l, p); b && (post = POST_B_R)
    b, c, l = LpRmSmLm!(btrTarget, c, l, p); b && (post = POST_B_R_T)

    # (8.10)
    b, c, l = LpRmSmRm!(target, c, l, p); b && (post = POST)
    b, c, l = LpRmSmRm!(tTarget, c, l, p); b && (post = POST_T)
    b, c, l = LpRmSmRm!(rTarget, c, l, p); b && (post = POST_R)
    b, c, l = LpRmSmRm!(trTarget, c, l, p); b && (post = POST_R_T)
    b, c, l = LpRmSmRm!(bTarget, c, l, p); b && (post = POST_B)
    b, c, l = LpRmSmRm!(btTarget, c, l, p); b && (post = POST_B_T)
    b, c, l = LpRmSmRm!(brTarget, c, l, p); b && (post = POST_B_R)
    b, c, l = LpRmSmRm!(btrTarget, c, l, p); b && (post = POST_B_R_T)

    # (8.11) C|Cpi/2 S Cpi/2|C
    b, c, l = LpRmSmLmRp!(target, c, l, p); b && (post = POST)
    b, c, l = LpRmSmLmRp!(tTarget, c, l, p); b && (post = POST_T)
    b, c, l = LpRmSmLmRp!(rTarget, c, l, p); b && (post = POST_R)
    b, c, l = LpRmSmLmRp!(trTarget, c, l, p); b && (post = POST_R_T)

    u = scalespeed!(scaleradius!(p[1:l], r), s)
    if post == POST_T
        timeflip!(u)
    elseif post == POST_R
        reflect!(u)
    elseif post == POST_B
        backwards!(u)
    elseif post == POST_R_T
        reflect!(timeflip!(u))
    elseif post == POST_B_T
        backwards!(timeflip!(u))
    elseif post == POST_B_R
        backwards!(reflect!(u))
    elseif post == POST_B_R_T
        backwards!(reflect!(timeflip!(u)))
    end
    c*r, u
end

## Where the magic happens
function LpSpLp!{T}(tx::T, ty::T, tt::T, c::T, l::Int, path::Vector{StepControl{T,2}})
    r, θ = R(tx - sin(tt), ty - 1 + cos(tt))
    u = r
    t = mod2piF(θ)
    v = mod2piF(tt - t)
    cnew = t + u + v
    c <= cnew && return false, c, l
    path[1] = carsegment2stepcontrol(1, t)
    path[2] = carsegment2stepcontrol(0, u)
    path[3] = carsegment2stepcontrol(1, v)
    true, cnew, 3
end

function LpSpRp!{T}(tx::T, ty::T, tt::T, c::T, l::Int, path::Vector{StepControl{T,2}})
    r, θ = R(tx + sin(tt), ty - 1 - cos(tt))
    r*r < 4 && return false, c, l
    u = sqrt(r*r - 4)
    r1, θ1 = R(u, T(2))
    t = mod2piF(θ + θ1)
    v = mod2piF(t - tt)
    cnew = t + u + v
    c <= cnew && return false, c, l
    path[1] = carsegment2stepcontrol(1, t)
    path[2] = carsegment2stepcontrol(0, u)
    path[3] = carsegment2stepcontrol(-1, v)
    true, cnew, 3
end

function LpRmLp!{T}(tx::T, ty::T, tt::T, c::T, l::Int, path::Vector{StepControl{T,2}})
    E = tx - sin(tt)
    N = ty + cos(tt) - 1
    E*E + N*N > 16 && return false, c, l
    r, θ = R(E, N)
    u = acos(1 - r*r/8)
    t = mod2piF(θ - u/2 + pi)
    v = mod2piF(pi - u/2 - θ + tt)
    u = -u
    cnew = t - u + v
    c <= cnew && return false, c, l
    path[1] = carsegment2stepcontrol(1, t)
    path[2] = carsegment2stepcontrol(-1, u)
    path[3] = carsegment2stepcontrol(1, v)
    true, cnew, 3
end

function LpRmLm!{T}(tx::T, ty::T, tt::T, c::T, l::Int, path::Vector{StepControl{T,2}})
    E = tx - sin(tt)
    N = ty + cos(tt) - 1
    E*E + N*N > 16 && return false, c, l
    r, θ = R(E, N)
    u = acos(1 - r*r/8)
    t = mod2piF(θ - u/2 + pi)
    v = mod2piF(pi - u/2 - θ + tt) - 2*T(pi)
    u = -u
    cnew = t - u - v
    c <= cnew && return false, c, l
    path[1] = carsegment2stepcontrol(1, t)
    path[2] = carsegment2stepcontrol(-1, u)
    path[3] = carsegment2stepcontrol(1, v)
    true, cnew, 3
end

function LpRpuLmuRm!{T}(tx::T, ty::T, tt::T, c::T, l::Int, path::Vector{StepControl{T,2}})
    E = tx + sin(tt)
    N = ty - cos(tt) - 1
    p = (2 + sqrt(E*E + N*N)) / 4
    (p < 0 || p > 1) && return false, c, l
    u = acos(p)
    t = mod2piF(Tau(u, -u, E, N))
    v = mod2piF(Omega(u, -u, E, N, tt)) - 2*T(pi)
    cnew = t + 2*u - v
    c <= cnew && return false, c, l
    path[1] = carsegment2stepcontrol(1, t)
    path[2] = carsegment2stepcontrol(-1, u)
    path[3] = carsegment2stepcontrol(1, -u)
    path[4] = carsegment2stepcontrol(-1, v)
    true, cnew, 4
end

function LpRmuLmuRp!{T}(tx::T, ty::T, tt::T, c::T, l::Int, path::Vector{StepControl{T,2}})
    E = tx + sin(tt)
    N = ty - cos(tt) - 1
    p = (20 - E*E - N*N) / 16
    (p < 0 || p > 1) && return false, c, l
    u = -acos(p)
    t = mod2piF(Tau(u, u, E, N))
    v = mod2piF(Omega(u, u, E, N, tt))
    cnew = t - 2*u + v
    c <= cnew && return false, c, l
    path[1] = carsegment2stepcontrol(1, t)
    path[2] = carsegment2stepcontrol(-1, u)
    path[3] = carsegment2stepcontrol(1, u)
    path[4] = carsegment2stepcontrol(-1, v)
    true, cnew, 4
end

function LpRmSmLm!{T}(tx::T, ty::T, tt::T, c::T, l::Int, path::Vector{StepControl{T,2}})
    E = tx - sin(tt)
    N = ty + cos(tt) - 1
    D, β = R(E, N)
    D < 2 && return false, c, l
    γ = acos(2/D)
    F = sqrt(D*D/4 - 1)
    t = mod2piF(pi + β - γ)
    u = 2 - 2*F
    u > 0 && return false, c, l
    v = mod2piF(-3*T(pi)/2 + γ + tt - β) - 2*T(pi)
    cnew = t + T(pi)/2 - u - v
    c <= cnew && return false, c, l
    path[1] = carsegment2stepcontrol(1, t)
    path[2] = carsegment2stepcontrol(-1, -T(pi)/2)
    path[3] = carsegment2stepcontrol(0, u)
    path[4] = carsegment2stepcontrol(1, v)
    true, cnew, 4
end

function LpRmSmRm!{T}(tx::T, ty::T, tt::T, c::T, l::Int, path::Vector{StepControl{T,2}})
    E = tx + sin(tt)
    N = ty - cos(tt) - 1
    D, β = R(E, N)
    D < 2 && return false, c, l
    t = mod2piF(β + T(pi)/2)
    u = 2 - D
    u > 0 && return false, c, l
    v = mod2piF(-pi - tt + β) - 2*T(pi)
    cnew = t + T(pi)/2 - u - v
    c <= cnew && return false, c, l
    path[1] = carsegment2stepcontrol(1, t)
    path[2] = carsegment2stepcontrol(-1, -T(pi)/2)
    path[3] = carsegment2stepcontrol(0, u)
    path[4] = carsegment2stepcontrol(-1, v)
    true, cnew, 4
end

function LpRmSmLmRp!{T}(tx::T, ty::T, tt::T, c::T, l::Int, path::Vector{StepControl{T,2}})
    E = tx + sin(tt)
    N = ty - cos(tt) - 1
    D, β = R(E, N)
    D < 2 && return false, c, l
    γ = acos(2/D)
    F = sqrt(D*D/4 - 1)
    t = mod2piF(pi + β - γ)
    u = 4 - 2*F
    u > 0 && return false, c, l
    v = mod2piF(pi + β - tt - γ)
    cnew = t + pi - u + v
    c <= cnew && return false, c, l
    path[1] = carsegment2stepcontrol(1, t)
    path[2] = carsegment2stepcontrol(-1, -T(pi)/2)
    path[3] = carsegment2stepcontrol(0, u)
    path[4] = carsegment2stepcontrol(1, -T(pi)/2)
    path[5] = carsegment2stepcontrol(-1, v)
    true, cnew, 5
end

for f in (:LpSpLp!, :LpSpRp!, :LpRmLp!, :LpRmLm!, :LpRpuLmuRm!, :LpRmuLmuRp!, :LpRmSmLm!, :LpRmSmRm!, :LpRmSmLmRp!)
    @eval $f{T}(s::SE2State{T}, c::T, l::Int, p::Vector{StepControl{T,2}}) = $f(s.x, s.y, s.θ, c, l, p)
end


text:
using LsqFit

@testset "qpdf for GaussianStateBHD" begin
    r, θ, dim = 0.8, 2π, 100
    ρ = SqueezedState(r, θ, Matrix, dim=dim)
    d = GaussianStateBHD(ρ)

    m = 10
    n = 1000
    θs = LinRange(0, 2π, m)
    xs = LinRange(-3, 3, n)

    g_pdf = Vector{Float64}(undef, n)

    for i in 1:n
        g_pdf[i] = qpdf(d, π/3, xs[i])
    end

    g_pdfs = qpdf(d, θs, xs)

    moments = [QuantumStateDistributions.mean(d, π/3), QuantumStateDistributions.std(d, π/3)]
    @. model(x, p) = 1 / (p[2] * √(2π)) * exp(-(x - p[1])^2 / (2 * p[2]^2))
    p0 = [0.5, 0.5]

    g_fit = curve_fit(model, xs, g_pdf, p0)
    @test coef(g_fit) ≈ moments

    for i in 1:m
        g_fits = curve_fit(model, xs, g_pdfs[i, :], p0)
        moments = [QuantumStateDistributions.mean(d, θs[i]), QuantumStateDistributions.std(d, θs[i])]

        @test coef(g_fits) ≈ moments
    end
end

@testset "qpdf for QuantumStateBHD" begin
    r, θ, dim = 0.8, 2π, 100
    ρ = SqueezedState(r, θ, Matrix, dim=dim)
    q = QuantumStateBHD(ρ)
    d = GaussianStateBHD(ρ)

    m = 10
    n = 1000
    θs = LinRange(0, 2π, m)
    xs = LinRange(-3, 3, n)

    q_pdf = Vector{Float64}(undef, n)

    for i in 1:n
        q_pdf[i] = qpdf(q, π/3, xs[i])
    end

    q_pdfs = qpdf(q, θs, xs)

    moments = [QuantumStateDistributions.mean(d, π/3), QuantumStateDistributions.std(d, π/3)]
    @. model(x, p) = 1 / (p[2] * √(2π)) * exp(-(x - p[1])^2 / (2 * p[2]^2))
    p0 = [0.5, 0.5]

    q_fit = curve_fit(model, xs, q_pdf, p0)
    @test coef(q_fit) ≈ moments

    for i in 1:m
        q_fits = curve_fit(model, xs, q_pdfs[i, :], p0)
        moments = [QuantumStateDistributions.mean(d, θs[i]), QuantumStateDistributions.std(d, θs[i])]

        @test coef(q_fits) ≈ moments
    end
end

@testset "pdf for density matrix" begin
    r, θ, dim = 0.8, 2π, 100
    ρ = SqueezedState(r, θ, Matrix, dim=dim)
    d = GaussianStateBHD(ρ)

    m = 10
    n = 1000
    θs = LinRange(0, 2π, m)
    xs = LinRange(-3, 3, n)

    ρ_pdf = Vector{Float64}(undef, n)

    for i in 1:n
        ρ_pdf[i] = qpdf(ρ, π/3, xs[i])
    end

    ρ_pdfs = qpdf(ρ, θs, xs)

    moments = [QuantumStateDistributions.mean(d, π/3), QuantumStateDistributions.std(d, π/3)]
    @. model(x, p) = 1 / (p[2] * √(2π)) * exp(-(x - p[1])^2 / (2 * p[2]^2))
    p0 = [0.5, 0.5]

    ρ_fit = curve_fit(model, xs, ρ_pdf, p0)
    @test coef(ρ_fit) ≈ moments

    for i in 1:m
        ρ_fits = curve_fit(model, xs, ρ_pdfs[i, :], p0)
        moments = [QuantumStateDistributions.mean(d, θs[i]), QuantumStateDistributions.std(d, θs[i])]

        @test coef(ρ_fits) ≈ moments
    end
end


text:
println("Compile on main node...")
include("mixst_model.jl")

using Distributed
rmprocs(workers())
addprocs(20)
println("Compile on workers...")
@everywhere include("mixst_model.jl")

## Generate data.
Random.seed!(0);
@everywhere true_data_dist = cde.Util.SkewT(2, 1, 7, -10)
@everywhere y = rand(true_data_dist, 1000);

# Plot data.
imgdir = joinpath(resultsdir, "img")
mkpath(imgdir)

# Data Histogram.
histogram(y, bins=100, label=nothing, la=0);
plot!(size=plotsize); ylabel!("density")
savefig(joinpath(imgdir, "data-hist.pdf"))
closeall()

# True data density.
@everywhere grid = collect(range(-4.5, 2.5, length=100))
ypdf = pdf.(true_data_dist, grid)
plot(grid, ypdf, label=nothing);
plot!(size=plotsize); ylabel!("density")
savefig(joinpath(imgdir, "data-density.pdf"))
closeall()

# Setup
@everywhere begin
  sims = dict_list(Dict(:K=>collect(1:5),
                        :skew=>[true, false], :tdist=>[true, false]))
  getsavedir(sim) = joinpath(resultsdir, savename(sim))
  getsavepath(sim) = joinpath(getsavedir(sim), "results.jls")
end

@everywhere function run(sim)
  Random.seed!(1);
  savedir = getsavedir(sim)
  mkpath(savedir)
  savepath = getsavepath(sim)

  K = sim[:K]
  skew = sim[:skew]
  tdist = sim[:tdist]

  v, zeta = make_aux(y)
  m = MixST(y, K, v, zeta)
  spl = make_sampler(y, K, v, zeta, skew=skew, tdist=tdist)

  chain = sample(m, spl, 3000, discard_initial=8000, thinning=2, save_state=true);
  serialize(savepath, chain)
end
println("Fit in parallel ...")
res = pmap(run, sims, on_error=identity)

println("Status of runs:")
foreach(z -> println(z[1], " => ", z[2]), zip(res, savename.(sims)))

# Send results to aws.
cde.Util.s3sync(from=resultsdir, to=awsbucket, tags=`--exclude '*.nfs'`)

# Postprocess
# TODO:
# - [X] plot data (observed and truth).
# - [X] plot psterior estimate and point-wise ci.
# - [x] get posterior distributions and trace of each parameter.
# - [X] compute dic for all models.
# - [X] see how many componenets are needed for best fit for each model
# - [X] see which model is best overall

@everywhere function postprocess(sim)
  savepath = getsavepath(sim)
  savedir = getsavedir(sim)
  chain = deserialize(savepath);
  plot_posterior(chain, savedir, y, grid, true_data_dist)
end

# foreach(postprocess, ProgressBar(sims))
ppres = pmap(postprocess, sims, on_error=identity)

# Plot DIC for all models.
function plot_metrics(sims, colors=palette(:tab10),
                      marker=[:rect, :circle, :utriangle, :pentagon])
  unique_K = unique(getindex.(sims, :K))

  for metric in ["dic", "mean_deviance"]
    plot(size=plotsize)
    plotidx = 0
    for tdist in [false, true]
      for skew in [false, true]
        plotidx +=1
        sim_subset = filter(s -> s[:tdist]==tdist && s[:skew]==skew, sims)

        metric_paths = [joinpath(getsavedir(s), "img/$(metric).txt")
                        for s in sim_subset]
        metrics = parse.(Float64, [open(f->read(f, String), metric_path)
                                   for metric_path in metric_paths])
        label = (skew ? "skew-" : "") * (tdist ? "t" : "Normal") * " mixture"
        plot!(unique_K, metrics, label=label,
              # marker=marker[plotidx], color=:grey, ms=6)
              marker=marker[plotidx], ms=4, lw=3, color=colors[plotidx])
      end
    end
    metric == "dic" && ylims!(1900, 3000)

    ylabel!(replace(metric, "_" => " "))
    xlabel!("K")
    savefig(joinpath(imgdir, "$(metric).pdf"))
    closeall()
  end
end
plot_metrics(sims)

# Send all results to aws.
cde.Util.s3sync(from=resultsdir, to=awsbucket, tags=`--exclude '*.nfs'`)

max_stars_repo_name: luiarthur/CytofDensityEstimation

max_issues_repo_name: luiarthur/CytofDensityEstimation

max_forks_repo_name: luiarthur/CytofDensityEstimation


text:
using Iex
using Test

#  using tokens in API examples on iexcloud.io
@testset "Iex.jl" begin
    include("Profiles_test.jl")
    include("Prices_test.jl")
end

max_stars_repo_name: GodolphinCapitalManagement/Iex

max_issues_repo_name: GodolphinCapitalManagement/Iex

max_forks_repo_name: GodolphinCapitalManagement/Iex


text:
### A Pluto.jl notebook ###
# v0.11.4

using Markdown
using InteractiveUtils

# ╔═╡ cce045ac-d373-11ea-29af-5be52ed80e36
md"## Confounding triangles notebook"

# ╔═╡ 30cfc76c-d276-11ea-1e1a-6567e0173d69
using DrWatson

# ╔═╡ cfa1909e-d278-11ea-215d-4f34edbb8294
using StructuralCausalModels

# ╔═╡ dde39c20-d272-11ea-00b1-71e8b3c09e79
cd(joinpath(homedir(), ".julia/dev/StructuralCausalModels"))

# ╔═╡ 3ed7655e-d276-11ea-185b-29e9192ba4f0
@quickactivate "StructuralCausalModels"

# ╔═╡ 742ca656-d276-11ea-04ab-f36281c92836
d_string = "dag {A -> {E Z}; B -> {D Z}; Z -> {D E}; E -> D}";

# ╔═╡ 739fdd34-d276-11ea-0632-13aeca3d3aa5
dag = DAG("conf_triangles", d_string);

# ╔═╡ 98ce6cf6-d276-11ea-15d5-856fba9a3c50
dag.a

# ╔═╡ 182e0140-d374-11ea-254b-8f4a175f4f06
dag.d

# ╔═╡ eb94dbbc-d278-11ea-2f78-9bd8a49d14d9
fname = projectdir() * "/conf_triangles.dot";

# ╔═╡ eafca4d2-d278-11ea-08ce-11367ae96f69
to_graphviz(dag, fname);

# ╔═╡ 59229f0e-d277-11ea-0bc8-cb68e38ae3b3
Sys.isapple() && run(`open -a GraphViz.app $(fname)`);

# ╔═╡ 1223d8ee-d279-11ea-2ba4-7dbae68007ff
to_ggm(dag)

# ╔═╡ 11b28448-d279-11ea-1d52-2544424ea533
bs = basis_set(dag)

# ╔═╡ 8fd4685c-d374-11ea-3b9c-670bb9da8a7c
bs.bs

# ╔═╡ 2b60ff06-d2cd-11ea-0bf6-050ad1720b31
f = :A

# ╔═╡ 4dc673b4-d2cd-11ea-1ca4-219bfa2bf57d
d = :B

# ╔═╡ 582b9c98-d279-11ea-2daf-93e7a13c9966
d_separation(dag, f, :B)

# ╔═╡ 57534fc8-d279-11ea-0b9b-a99db30bc7b4
d_separation(dag, d, :E)

# ╔═╡ 5712eff0-d279-11ea-3978-2106eb0feb76
d_separation(dag, d, :E; c=[:A, :Z])

# ╔═╡ 49e58f04-d8c8-11ea-19e0-cb45efe39493
m_separation(dag, d, :E; c=[:A, :Z])

# ╔═╡ Cell order:
# ╠═cce045ac-d373-11ea-29af-5be52ed80e36
# ╠═dde39c20-d272-11ea-00b1-71e8b3c09e79
# ╠═30cfc76c-d276-11ea-1e1a-6567e0173d69
# ╠═3ed7655e-d276-11ea-185b-29e9192ba4f0
# ╠═742ca656-d276-11ea-04ab-f36281c92836
# ╠═cfa1909e-d278-11ea-215d-4f34edbb8294
# ╠═739fdd34-d276-11ea-0632-13aeca3d3aa5
# ╠═98ce6cf6-d276-11ea-15d5-856fba9a3c50
# ╠═182e0140-d374-11ea-254b-8f4a175f4f06
# ╠═eb94dbbc-d278-11ea-2f78-9bd8a49d14d9
# ╠═eafca4d2-d278-11ea-08ce-11367ae96f69
# ╠═59229f0e-d277-11ea-0bc8-cb68e38ae3b3
# ╠═1223d8ee-d279-11ea-2ba4-7dbae68007ff
# ╠═11b28448-d279-11ea-1d52-2544424ea533
# ╠═8fd4685c-d374-11ea-3b9c-670bb9da8a7c
# ╠═2b60ff06-d2cd-11ea-0bf6-050ad1720b31
# ╠═4dc673b4-d2cd-11ea-1ca4-219bfa2bf57d
# ╠═582b9c98-d279-11ea-2daf-93e7a13c9966
# ╠═57534fc8-d279-11ea-0b9b-a99db30bc7b4
# ╠═5712eff0-d279-11ea-3978-2106eb0feb76
# ╠═49e58f04-d8c8-11ea-19e0-cb45efe39493


text:
# Portions translated from SLICOT-Reference distribution
# Copyright (c) 2002-2020 NICONET e.V.
function run_mb05md(datfile, io=stdout)
    NIN = 5
    NOUT = 6
    NMAX = 20
    LDA = NMAX
    LDV = NMAX
    LDY = NMAX
    LDWORK = 4*NMAX
    A = Array{Float64,2}(undef, LDA,NMAX)
    V = Array{Float64,2}(undef, LDV,NMAX)
    Y = Array{Float64,2}(undef, LDY,NMAX)
    VALR = Array{Float64,1}(undef, NMAX)
    VALI = Array{Float64,1}(undef, NMAX)
    DWORK = Array{Float64,1}(undef, LDWORK)
    IWORK = Array{BlasInt,1}(undef, NMAX)
    f = open(datfile,"r")
    readline(f)
    BALANC = 'N'
    vs = split(readline(f))
    N = parse(BlasInt, vs[1])
    DELTA = parse(Float64, replace(vs[2],'D'=>'E'))
    if ( N<=0 || N>NMAX )
        @error "Illegal N=$N"
    end

    vs = String[]
    _isz,_jsz = (N,N)
    while length(vs) < _isz*_jsz
        append!(vs, replace.(split(readline(f)),'D'=>'E'))
    end
    for i in 1:_isz
       _i0 = (i-1)*_jsz
       A[i,1:_jsz] .= parsex.(Float64, vs[_i0+1:_i0+_jsz])
    end
    close(f)
    # interp call 1

    INFO = SLICOT.mb05md!(BALANC, N, DELTA, A, V, Y, VALR, VALI, LDWORK)
    @test INFO == 0
    INFO == 0 || return

# interp output 1
    println(io, "A:")
    _nc = N
    _nr = N
    show(io, "text/plain", A[1:_nr,1:_nc])
    println(io)

# unable to translate write loop:
#  write (I), VALI(I), I = 1,N
# interp output 2
    println(io, "eigvals:")
    show(io, "text/plain", VALR[1:N]+im*VALI[1:N])
    println(io)

# interp output 3
    println(io, "V:")
    _nc = N
    _nr = N
    show(io, "text/plain", V[1:_nr,1:_nc])
    println(io)

# interp output 4
    println(io, "Y:")
    _nc = N
    _nr = N
    show(io, "text/plain", Y[1:_nr,1:_nc])
    println(io)

end # run_mb05md()


text:
using Test
using Recommenders:
    Recall, Precision, DCG, NDCG, MeanRecall, MeanPrecision, MeanDCG, MeanNDCG, _dcg

@testset "Recall and precision" begin
    recall1 = Recall(1)

    recommends = [[1, 2, 3], [4, 5], [], nothing]
    ys = [[1], [2, 5], [4], [5]]

    expected_recall1 = [1.0, 0.0, 0.0, 0.0]
    expected_precision1 = [1.0, 0.0, 0.0, 0.0]
    expected_recall2 = [1.0, 0.5, 0.0, 0.0]
    expected_precision2 = [0.5, 0.5, 0.0, 0.0]

    recall1 = Recall(1)
    recall2 = Recall(2)
    precision1 = Precision(1)
    precision2 = Precision(2)

    for i in eachindex(recommends)
        @test recall1(recommends[i], ys[i]) == expected_recall1[i]
        @test recall2(recommends[i], ys[i]) == expected_recall2[i]
        @test precision1(recommends[i], ys[i]) == expected_precision1[i]
        @test precision2(recommends[i], ys[i]) == expected_precision2[i]
    end

    # mean_recall1 = MeanMetric(Recall(1))
    mean_recall1 = MeanRecall(1)
    @test mean_recall1(recommends, ys) == 1 / 4

    mean_recall12 = MeanRecall(2)
    @test mean_recall12(recommends, ys) == (1 + 0.5) / 4

    mean_prec1 = MeanPrecision(1)
    @test mean_prec1(recommends, ys) == 1 / 4

    mean_prec2 = MeanPrecision(2)
    @test mean_prec2(recommends, ys) == (0.5 + 0.5) / 4

end

@testset "DCG" begin
    @test _dcg([1]) ≈ 1.0
    relevances = [1.0, 0.0, 3.0]
    @test _dcg(relevances) ≈ 1 + 3.5

    dcg3 = DCG(3)
    @test dcg3([1], [1]) ≈ 1.0
    @test dcg3([1, 2, 3], [1, 2, 3], [1.0, 0.0, 3.0]) ≈ 4.5
    @test dcg3([1, 2, 3, 4], [1, 2, 3, 4], [1.0, 0.0, 3.0]) ≈ 4.5
    @test dcg3([1, 4, 2], [1, 2, 3], [1.0, 3.0, 5.0]) ≈ 4.5
    @test dcg3([1, 4, 2], [1, 2, 3], [1.0, 7.0, 5.0]) ≈ 1 + 127 / 2.0

    ndcg3 = NDCG(3)
    @test ndcg3([1], [1]) ≈ 1.0
    @test ndcg3([1, 2, 3], [1, 2, 3], [1, 1, 1]) ≈ 1.0
    @test ndcg3([1, 2, 3], [1, 2], [1, 1]) ≈ 1.0
    @test ndcg3([1, 2, 3], [2, 1], [1, 1]) ≈ 1.0
    @test ndcg3([1, 2, 3], [1, 2, 3, 4]) ≈ 1.0
    @test ndcg3([6, 7, 8], [1, 2, 3, 4]) ≈ 0.0

    mean_dcg3 = MeanDCG(3)
    @test mean_dcg3([[1], [1, 2, 3]], [[1], [1]]) ≈ 1.0

    mean_ndcg3 = MeanNDCG(3)
    @test mean_ndcg3([[1, 2, 3], [6, 7, 8]], [[1, 2, 3], [1]]) ≈ 0.5
end