text:
import Base: copy, get

"""
    IntervalMatrixPower{T}

A wrapper for the matrix power that can be incremented.

### Fields

- `M`  -- the original matrix
- `Mᵏ` -- the current matrix power, i.e., ``M^k``
- `k`  -- the current power index

### Notes

The wrapper should only be accessed using the interface functions.
The internal representation (such as the fields) are subject to future changes.

### Examples

```jldoctest
julia> A = IntervalMatrix([2.0..2.0 2.0..3.0; 0.0..0.0 -1.0..1.0])
2×2 IntervalMatrix{Float64, Interval{Float64}, Matrix{Interval{Float64}}}:
 [2, 2]   [2, 3]
 [0, 0]  [-1, 1]

julia> pow = IntervalMatrixPower(A);

julia> increment!(pow)
2×2 IntervalMatrix{Float64, Interval{Float64}, Matrix{Interval{Float64}}}:
 [4, 4]  [2, 9]
 [0, 0]  [0, 1]

julia> increment(pow)
2×2 IntervalMatrix{Float64, Interval{Float64}, Matrix{Interval{Float64}}}:
  [8, 8]  [-1, 21]
 [-0, 0]   [-1, 1]

julia> get(pow)
2×2 IntervalMatrix{Float64, Interval{Float64}, Matrix{Interval{Float64}}}:
 [4, 4]  [2, 9]
 [0, 0]  [0, 1]

julia> index(pow)
2

julia> base(pow)
2×2 IntervalMatrix{Float64, Interval{Float64}, Matrix{Interval{Float64}}}:
 [2, 2]   [2, 3]
 [0, 0]  [-1, 1]

```
"""
mutable struct IntervalMatrixPower{T}
    M::IntervalMatrix{T}
    Mᵏ::IntervalMatrix{T}
    k::Int

    function IntervalMatrixPower(M::IntervalMatrix{T}, Mᵏ::IntervalMatrix{T},
                                 k::Int) where {T}
        @assert k >= 1 "matrix powers must be positive"
        return new{T}(M, Mᵏ, k)
    end
end

function IntervalMatrixPower(M::IntervalMatrix{T}) where {T}
    return IntervalMatrixPower(M, M, 1)
end

function IntervalMatrixPower(M::IntervalMatrix{T}, k::Int;
                             algorithm::String="power") where {T}
    @assert k >= 1 "matrix powers must be positive"
    pow = IntervalMatrixPower(M, M, 1)
    @inbounds for i in 1:(k-1)
        increment!(pow; algorithm=algorithm)
    end
    return pow
end

function copy(pow::IntervalMatrixPower)
    return IntervalMatrixPower(pow.M, pow.Mᵏ, pow.k)
end

const default_algorithm = "intersect"

"""
    increment!(pow::IntervalMatrixPower; [algorithm=default_algorithm])

Increment a matrix power in-place (i.e., storing the result in `pow`).

### Input

- `pow`       -- wrapper of a matrix power (modified in this function)
- `algorithm` -- (optional; default: `default_algorithm`) algorithm to compute
                 the matrix power; available options:
    * `"multiply"` -- fast computation using `*` from the previous result
    * `"power"` -- recomputation using `^`
    * `"decompose_binary"` -- decompose `k = 2a + b`
    * `"intersect"` -- combination of `"multiply"`/`"power"`/`"decompose_binary"`

### Output

The next matrix power, reflected in the modified wrapper.

### Notes

Independent of `"algorithm"`, if the index is a power of two, we compute the
exact result using squaring.
"""
function increment!(pow::IntervalMatrixPower;
                    algorithm::String=default_algorithm)
    pow.k += 1
    if _isapoweroftwo(pow.k)
        pow.Mᵏ = _eval_poweroftwo(pow)
    elseif algorithm == "multiply"
        pow.Mᵏ = _eval_multiply(pow)
    elseif algorithm == "power"
        pow.Mᵏ = _eval_power(pow)
    elseif algorithm == "decompose_binary"
        pow.Mᵏ = _eval_decompose_binary(pow)
    elseif algorithm == "intersect"
        pow.Mᵏ = _eval_intersect(pow)
    else
        throw(ArgumentError("algorithm $algorithm is not available; choose " *
            "from 'multiply', 'power', 'decompose_binary', 'intersect'"))
    end
    return pow.Mᵏ
end

"""
    increment(pow::IntervalMatrixPower; [algorithm=default_algorithm])

Increment a matrix power without modifying `pow`.

### Input

- `pow` -- wrapper of a matrix power
- `algorithm` -- (optional; default: `default_algorithm`) algorithm to compute
                 the matrix power; see [`increment!`](@ref) for available options

### Output

The next matrix power.
"""
function increment(pow::IntervalMatrixPower;
                   algorithm::String=default_algorithm)
    return increment!(copy(pow), algorithm=algorithm)
end

# checks whether a number is a power of 2
function _isapoweroftwo(k::Integer)
    # see https://stackoverflow.com/a/600306
    return (k & (k - 1)) == 0
end

# compute the (exact) matrix power for a power of two
function _eval_poweroftwo(pow::IntervalMatrixPower)
    Mᵏ = pow.M
    @inbounds for i in 1:Int(log(2, pow.k))
        Mᵏ = square(Mᵏ)
    end
    return Mᵏ
end

function _eval_multiply(pow::IntervalMatrixPower)
    return pow.Mᵏ * pow.M
end

function _eval_power(pow::IntervalMatrixPower)
    return pow.M^pow.k
end

function _eval_decompose_binary(pow::IntervalMatrixPower)
    return _eval_decompose_binary_helper(pow.M, pow.k)
end

# decompose k = 2a + b with a, b being positive integers and a being maximal
function _eval_decompose_binary_helper(M, k)
    if k == 1
        Mᵏ = M
    else
        Mᵏ = square(_eval_decompose_binary_helper(M, div(k, 2)))
        if k % 2 == 1
            Mᵏ *= M
        end
    end
    return Mᵏ
end

function _eval_intersect(pow::IntervalMatrixPower)
    return intersect(intersect(_eval_multiply(pow), _eval_power(pow)),
                     _eval_decompose_binary(pow))
end

"""
    base(pow::IntervalMatrixPower)

Return the original matrix represented by a wrapper of a matrix power.

### Input

- `pow` -- wrapper of a matrix power

### Output

The matrix ``M`` being the basis of the matrix power ``M^k`` represented by the
wrapper.
"""
function base(pow::IntervalMatrixPower)
    return pow.M
end

"""
    get(pow::IntervalMatrixPower)

Return the matrix represented by a wrapper of a matrix power.

### Input

- `pow` -- wrapper of a matrix power

### Output

The matrix power represented by the wrapper.
"""
function get(pow::IntervalMatrixPower)
    return pow.Mᵏ
end

"""
    index(pow::IntervalMatrixPower)

Return the current index of the wrapper of a matrix power.

### Input

- `pow` -- wrapper of a matrix power

### Output

The index `k` of the wrapper representing ``M^k``.
"""
function index(pow::IntervalMatrixPower)
    return pow.k
end


text:
let
    for shape in [(1,1),(2,1),(1,2),(2,2),(3,2)]
        for idx in 1:config_tuple_to_index(shape,shape)
            tup = config_index_to_tuple(shape,idx)
            config_idx = config_tuple_to_index(shape,tup)
            # @show shape, tup, idx, config_idx
            @test idx == config_idx
        end
        idx = 0
        for i in 1:shape[1]
            for j in 1:shape[2]
                idx += 1
                @test (i,j) == config_index_to_tuple(shape,idx)
            end
        end
    end
end
let
    dims = (2,2)
    # initialize a dense grid graph
    G = initialize_grid_graph_from_vtx_grid(initialize_dense_vtx_grid(dims...))
    # construct the corresponding vertex coordinate list
    vtxs = [(i,j) for i in 1:dims[1] for j in 1:dims[2]]
    vtx_map = construct_vtx_map(vtxs,dims)
    edge_cache = construct_edge_cache(vtxs,vtx_map)
    @test validate_edge_cache(G,vtxs,edge_cache)
end
let
    grid = initialize_regular_vtx_grid(;n_obstacles_x=1,n_obstacles_y=1)
    dims = size(grid)
    base_vtxs = vtx_list_from_vtx_grid(grid)
    # 1   2   3   4   5   6
    # 7   8   9  10  11  12
    # 13  14   0   0  15  16
    # 17  18   0   0  19  20
    # 21  22  23  24  25  26
    # 27  28  29  30  31  32
    indicator_grid = IndicatorGrid(grid)
    s = (2,2)
    filtered_grid = convolve_with_occupancy_kernel(indicator_grid,ones(Int,s))
    vtx_map = VtxGrid(filtered_grid)
    vtx_list = vtx_list_from_vtx_grid(vtx_map)
    graph = initialize_grid_graph_from_vtx_grid(vtx_map)
    sm = SparseDistanceMatrix(graph)
    @test sm(1,2) == 1
    @test sm(4,2) == 2

    m = RemappedDistanceMatrix(sm,grid,base_vtxs,vtx_map,vtx_list,s)
    remap_idx(m,1,1)
    remap_idx(m,2,2)
    remap_idx(m,7,3)
    remap_idx(m,8,4)
    @test m(1,2,(1,1)) == 1
    @test m(9,10,(2,2)) == 1
    @test m(9,10,4) == 1
    @test m(4,2,(1,1)) == m(4,2) == 2
end
let
    grid = initialize_regular_vtx_grid()
    dims = size(grid)
    vtxs = [(i,j) for i in 1:dims[1] for j in 1:dims[2] if grid[i,j] > 0]
    sort!(vtxs, by=vtx->grid[vtx...])
    zones = [v for (v,vtx) in enumerate(vtxs) if sum(grid[clip(vtx[1]-1,1,dims[1]):clip(vtx[1]+1,1,dims[1]),clip(vtx[2]-1,1,dims[2]):clip(vtx[2]+1,1,dims[2])] .> 0) == 7]
    vtx_map = construct_vtx_map(vtxs,dims)
    expanded_zones = construct_expanded_zones(vtxs,vtx_map,zones)
    @test GraphUtils.validate_expanded_zones(vtx_map,expanded_zones)
end
let
    grid = initialize_regular_vtx_grid(;n_obstacles_x=1,n_obstacles_y=1)
    dims = size(grid)
    vtxs = [(i,j) for i in 1:dims[1] for j in 1:dims[2] if grid[i,j] > 0]
    # 1   2   3   4   5   6
    # 7   8   9  10  11  12
    # 13  14   0   0  15  16
    # 17  18   0   0  19  20
    # 21  22  23  24  25  26
    # 27  28  29  30  31  32
    dist_mtx_map = DistMatrixMap(grid,vtxs)
    dist_matrix = get_dist_matrix(initialize_grid_graph_from_vtx_grid(grid))
    v1 = 13
    v2 = 15
    @test get_distance(dist_mtx_map,v1,v2,(1,1)) == dist_matrix[v1,v2]
    @test get_distance(dist_mtx_map,v1,v2,(1,2)) == dist_matrix[v1,v2]
    @test get_distance(dist_mtx_map,v1,v2,(2,1)) == dist_matrix[v1,v2] + 2
    @test get_distance(dist_mtx_map,v1,v2,(2,2)) == dist_matrix[v1,v2] + 2
    v3 = 18
    v4 = 20
    shape = (2,2)
    config = (2,2)
    @test get_distance(dist_mtx_map,v3,v4,shape,config) == dist_matrix[v1,v2] + 2
    @test get_distance(dist_mtx_map,v3,v4,shape,4) == dist_matrix[v1,v2] + 2
end
let
    env = construct_regular_factory_world()
    get_x_dim(env)
    get_y_dim(env)
    get_cell_width(env)
    get_transition_time(env)
    get_vtxs(env)
    get_pickup_zones(env)
    get_dropoff_zones(env)
    get_obstacles(env)
    get_pickup_vtxs(env)
    get_dropoff_vtxs(env)
    get_obstacle_vtxs(env)
    GridFactoryEnvironment(env,env.graph)
    Base.zero(env)
    LightGraphs.edges(env)
    LightGraphs.edgetype(env)
    LightGraphs.has_edge(env,1,2)
    LightGraphs.has_vertex(env,1)
    LightGraphs.inneighbors(env,1)
    LightGraphs.is_directed(env)
    LightGraphs.ne(env)
    LightGraphs.nv(env)
    LightGraphs.outneighbors(env,1)
    LightGraphs.vertices(env)
    get_num_free_vtxs(env)
    get_free_zones(env)

    @test validate_expanded_zones(env.vtx_map,env.expanded_zones)

    filename = "env.toml"
    open(filename,"w") do io
        TOML.print(io, TOML.parse(env))
    end
    env2 = read_env(filename)

    @test get_vtxs(env) == get_vtxs(env2)
    @test nv(env.graph) == nv(env2.graph)
    @test ne(env.graph) == ne(env2.graph)

    construct_factory_env_from_vtx_grid(env.vtx_map)
    # construct_random_factory_world()
    # sample_random_robot_locations(env,10)
end
let
    xdim = 4
    ydim = 4
    vtx_grid = initialize_dense_vtx_grid(xdim,ydim)
    env_graph = initialize_grid_graph_from_vtx_grid(vtx_grid)
    vtxs = [(i,j) for i in 1:xdim for j in 1:ydim]
    factory_env = GridFactoryEnvironment(
        graph = env_graph,
        x_dim=xdim,
        y_dim=ydim,
        vtxs=vtxs
    )
end
# test custom envs for different-sized robot sizes
let
    vtx_grid = initialize_dense_vtx_grid(4,4)
    #  1   2   3   4
    #  5   6   7   8
    #  9  10  11  12
    # 13  14  15  16
    env = construct_factory_env_from_vtx_grid(vtx_grid)
    for i in 1:3
        for j in 1:3
            # @show env.expanded_zones[vtx_grid[i,j]]
        end
    end
end
let
    factory_env = construct_regular_factory_world(;n_obstacles_x=1,n_obstacles_y=1)
    # 1   2   3   4   5   6
    # 7   8   9  10  11  12
    # 13  14   0   0  15  16
    # 17  18   0   0  19  20
    # 21  22  23  24  25  26
    # 27  28  29  30  31  32
    dist_matrix = get_dist_matrix(factory_env)
    dist_mtx_map = factory_env.dist_function

    v1 = 13
    v2 = 15
    @test get_distance(dist_mtx_map,v1,v2) == dist_matrix[v1,v2]
    @test get_distance(dist_mtx_map,v1,v2,(1,1)) == dist_matrix[v1,v2]
    @test get_distance(dist_mtx_map,v1,v2,(1,2)) == dist_matrix[v1,v2]
    @test get_distance(dist_mtx_map,v1,v2,(2,1)) == dist_matrix[v1,v2] + 2
    @test get_distance(dist_mtx_map,v1,v2,(2,2)) == dist_matrix[v1,v2] + 2

    v3 = 18
    v4 = 20
    config = 4
    @test get_distance(dist_mtx_map,v3,v4,(2,2),config) == dist_matrix[v1,v2] + 2
end
# modifying graphs and distance matrices
let
    vtx_grid = initialize_dense_vtx_grid(4,4)
    #  1   2   3   4
    #  5   6   7   8
    #  9  10  11  12
    # 13  14  15  16
    env = construct_factory_env_from_vtx_grid(vtx_grid)
    m = SparseDistanceMatrix(env.graph)
    vtxs = [6,7,10,11]
    edge_set = edge_cover(m.graph,vtxs)
    remove_edges!(env,edge_set)
    for v in vtxs
        @test indegree(env.graph,v) == 0
        @test outdegree(env.graph,v) == 0
    end
    for e in edge_set
        @test !has_edge(env.graph,e)
    end
    add_edges!(env,edge_set)
    for e in edge_set
        @test has_edge(env.graph,e)
    end

    D = get_dist_matrix(m.graph)
    remove_edges!(m,edge_set)
    @test !all(D .== get_dist_matrix(m.graph))
    for v in vtxs
        for v2 in vertices(m.graph)
            if v != v2
                @test m(v,v2) == typemax(Int)
            end
        end
    end
    add_edges!(m,edge_set)
    @test all(D .== get_dist_matrix(m.graph))
end

ext: jl


text:
# https://en.wikipedia.org/wiki/Hofstadter_sequence#Hofstadter_Female_and_Male_sequences

function f(n)
    if n == 0
        1
    else
        n - m(f(n - 1))
    end
end

function m(n)
    if n == 0
        0
    else
        n - f(m(n - 1))
    end
end

# les deux séquences diffèrent si et seulement si n+1 est un nb de Fibonacci

for n = 0 : 50
    if f(n) != m(n)
        println(n + 1)
    end
end

max_stars_repo_name: gabriel-doriath-dohler/pjulia

max_issues_repo_name: gabriel-doriath-dohler/pjulia

max_forks_repo_name: gabriel-doriath-dohler/pjulia


text:
# ## model
include("trajopt_model_v2.jl")

# ## horizon
h = 0.1
T = 16
Tm = 4 # mid point for a swing / stance change

# ## centroidal_quadruped
s = get_simulation("centroidal_quadruped", "flat_3D_lc", "flat")
model = s.model
env = s.env

nx = 2 * model.nq
nc = 4 #model.nc
nu = model.nu + nc + 4 * nc + nc + 4 * nc + 1
nθ = 53

# ## model
d1 = DTO.Dynamics((y, x, u, w) -> centroidal_quadruped_dyn1(model, env, [h], y, x, u, w), nx + nθ + nx, nx, nu)
dt = DTO.Dynamics((y, x, u, w) -> centroidal_quadruped_dynt(model, env, [h], y, x, u, w), nx + nθ + nx, nx + nθ + nx, nu)

dyn = [d1, [dt for t = 2:T-1]...]

# ## initial conditions
body_height = 0.3
foot_x = 0.17
foot_y = 0.17
foot_height = 0.08

q1 = zeros(model.nq)

# body position
q1[1:3] = [0.0; 0.0; body_height]
q1[4:6] = [0.0; 0.0; 0.0]

# foot1
q1[7:9] = [foot_x; foot_y; 0]

# foot2
q1[10:12] = [foot_x; -foot_y; 0]

# foot3
q1[13:15] = [-foot_x; foot_y; 0]

# foot4
q1[16:18] = [-foot_x; -foot_y; 0]

# qM1 FL and RR swing
# qM2 FR and RL swing
qM1 = deepcopy(q1)
qM2 = deepcopy(q1)
qM1[6 + 3] += foot_height # front left
qM1[15 + 3] += foot_height # back right
qM2[9 + 3] += foot_height # front right
qM2[12 + 3] += foot_height # back left

# Terminal Q
qT = copy(q1)

vis = Visualizer()
render(vis)
visualize!(vis, model, [qM2], Δt=h);

# q_ref = [q1, qM1, qT,
#          q1, qM2, qM2, qT]

q_ref = [q1, linear_interpolation(q1, qM1, Tm)..., linear_interpolation(qM1, qT, Tm)...,
         linear_interpolation(q1, qM2, Tm)..., linear_interpolation(qM2, qT, Tm)...]
x_ref = [[q_ref[t]; q_ref[t+1]] for t = 1:T]
x1 = x_ref[1]
xM = x_ref[Tm]
xT = x_ref[T]

visualize!(vis, model, q_ref, Δt=h);

plot(hcat(q_ref...)', labels="")

# ## objective
obj = DTO.Cost{Float64}[]
for t = 1:T
    if t == T
        function objT(x, u, w)
            J = 0.0
            v = (x[model.nq .+ (1:model.nq)] - x[1:model.nq]) ./ h
            J += 0.5 * 1.0e-3 * dot(v, v)
            J += 100 * transpose(x[1:nx] - x_ref[t]) * Diagonal(1000.0 * ones(nx)) * (x[1:nx] - x_ref[t])
            return J / T
        end
        push!(obj, DTO.Cost(objT, nx + nθ + nx, 0))
    elseif t == 1
        function obj1(x, u, w)
            J = 0.0
            v = (x[model.nq .+ (1:model.nq)] - x[1:model.nq]) ./ h
            J += 0.5 * 1.0e-3 * dot(v, v)
            J += 100 * transpose(x[1:nx] - x_ref[t]) * Diagonal(1000.0 * ones(nx)) * (x[1:nx] - x_ref[t])
            J += 0.5 * transpose(u[1:model.nu]) * Diagonal(1.0e-3 * ones(model.nu)) * u[1:model.nu]
            J += 0.5 * transpose(u[model.nu + 4 .+ (1:20)]) * Diagonal(1.0 * ones(20)) * u[model.nu + 4 .+ (1:20)]

            J += 1000.0 * u[end] # slack
            return J / T
        end
        push!(obj, DTO.Cost(obj1, nx, nu))
    else
        function objt(x, u, w)
            J = 0.0
            v = (x[model.nq .+ (1:model.nq)] - x[1:model.nq]) ./ h
            J += 0.5 * 1.0e-1 * dot(v, v)
            u_previous = x[nx .+ (1:53)]
            u_control = u
            w = (u_control - u_previous) ./ h
            J += 0.5 * 1.0e-3 * dot(w, w)
            J += 100 * transpose(x[1:nx] - x_ref[t]) * Diagonal(1000.0 * ones(nx)) * (x[1:nx] - x_ref[t])
            J += 0.5 * transpose(u[1:model.nu]) * Diagonal(1.0e-3 * ones(model.nu)) * u[1:model.nu]
            J += 0.5 * transpose(u[model.nu + 4 .+ (1:20)]) * Diagonal(1.0 * ones(20)) * u[model.nu + 4 .+ (1:20)]
            J += 1000.0 * u[end] # slack
            return J / T
        end
        push!(obj, DTO.Cost(objt, nx + nθ + nx, nu))
    end
end

# ## constraints
# initial condition
xl1 = [q1; q1]
xu1 = [q1; q1]
xlt = [-Inf * ones(nx); -Inf * ones(nθ); -Inf * ones(nx)]
xut = [Inf * ones(nx); Inf * ones(nθ); Inf * ones(nx)]

# final condition
xlT = [-Inf * ones(nq); qT; -Inf * ones(nθ); -Inf * ones(nx)]
xuT = [Inf * ones(nq); qT; Inf * ones(nθ); Inf * ones(nx)]

ul = [-Inf * ones(model.nu); zeros(nu - model.nu)]
uu = [Inf * ones(model.nu); Inf * ones(nu - model.nu)]

# bnd1 = DTO.Bound(nx, nu, state_lower=xl1, state_upper=xu1, action_lower=ul, action_upper=uu)
# bndt = DTO.Bound(nx + nθ + nx, nu, state_lower=xlt, state_upper=xut, action_lower=ul, action_upper=uu)
# bndT = DTO.Bound(nx + nθ + nx, 0, state_lower=xlT, state_upper=xuT)
# bnds = [bnd1, [bndt for t = 2:T-1]..., bndT];

bnds = DTO.Bound{Float64}[]
push!(bnds, DTO.Bound(nx, nu, state_lower=xl1, state_upper=xu1, action_lower=ul, action_upper=uu))
for t = 2:T-1
    push!(bnds, DTO.Bound(nx + nθ + nx, nu,
        state_lower=[-Inf * ones(nq); -Inf * ones(nq); -Inf * ones(nθ); -Inf * ones(nx)],
        state_upper=[Inf * ones(nq); Inf * ones(nq); Inf * ones(nθ); Inf * ones(nx)],
        action_lower=ul, action_upper=uu))
end
push!(bnds, DTO.Bound(nx + nθ + nx, 0, state_lower=xlT, state_upper=xuT))

cons = DTO.Constraint{Float64}[]
for t = 1:T
    if t == 1
        function constraints_1(x, u, w)
            [
            # equality (16)
            contact_constraints_equality(model, env, h, x, u, w);
            # inequality (28)
            contact_constraints_inequality_1(model, env, h, x, u, w);

            # body/feet constraints
            x[3] - x_ref[t][3]; # body height
            # x[model.nq + 3] - x_ref[t][model.nq + 3]; # body height
            # x[9:3:18] - x_ref[t][9:3:18];
            ]
        end
        push!(cons, DTO.Constraint(constraints_1, nx, nu, idx_ineq=collect(16 .+ (1:28))))
    elseif t == T
        function constraints_T(x, u, w)
            [
            # inequality (8)
            contact_constraints_inequality_T(model, env, h, x, u, w);

            # body/feet constraints
            x[3] - x_ref[t][3]; # body height
            # x[model.nq + 3] - x_ref[t][model.nq + 3]; # body height
            # x[9:3:18] - x_ref[t][9:3:18];
            ]
        end
        push!(cons, DTO.Constraint(constraints_T, nx + nθ + nx, nu, idx_ineq=collect(0 .+ (1:8))))
    else
        function constraints_t(x, u, w)
            [
            # equality (16)
            contact_constraints_equality(model, env, h, x, u, w);
            # inequality (32)
            contact_constraints_inequality_t(model, env, h, x, u, w);

            # body/feet constraints
            x[3] - x_ref[t][3]; # body height
            # x[model.nq + 3] - x_ref[t][model.nq + 3]; # body height
            # x[9:3:18] - x_ref[t][9:3:18];
            ]
        end
        push!(cons, DTO.Constraint(constraints_t, nx + nθ + nx, nu, idx_ineq=collect(16 .+ (1:32))) )
    end
end

# ## problem
tolerance = 1.0e-3
p = DTO.solver(dyn, obj, cons, bnds,
    options=DTO.Options(
        max_iter=2000,
        tol=tolerance,
        constr_viol_tol=tolerance,
        ))

# ## initialize
x_interpolation = [x_ref[1], [[x_ref[t]; zeros(nθ); zeros(nx)] for t = 2:T]...]
u_guess = [1.0e-1 * rand(nu) for t = 1:T-1] # may need to run more than once to get good trajectory
DTO.initialize_states!(p, x_interpolation)
DTO.initialize_controls!(p, u_guess)

# ## solve
@time DTO.solve!(p)

# ## solution
x_sol, u_sol = DTO.get_trajectory(p)
@show x_sol[1]
@show x_sol[T]
maximum([u[end] for u in u_sol[1:end-1]])

# ## visualize
vis = Visualizer()
render(vis)
visualize!(vis, model, [x_sol[1][1:nq], [x[nq .+ (1:nq)] for x in x_sol]...], Δt=h);

q_opt = [x_sol[1][1:model.nq], [x[model.nq .+ (1:model.nq)] for x in x_sol]...]
v_opt = [(x[model.nq .+ (1:model.nq)] - x[0 .+ (1:model.nq)]) ./ h for x in x_sol]
u_opt = [u[1:model.nu] for u in u_sol]
λ_opt = [u[model.nu .+ (1:4)] for u in u_sol]
b_opt = [u[model.nu + 4 .+ (1:16)] for u in u_sol]

q_opt = [x_sol[1][1:model.nq], [x[model.nq .+ (1:model.nq)] for x in x_sol]...]
v_opt = [(x[model.nq .+ (1:model.nq)] - x[0 .+ (1:model.nq)]) ./ h for x in x_sol]
u_opt = [u[1:model.nu] for u in u_sol]
γ_opt = [u[model.nu .+ (1:4)] for u in u_sol]
b_opt = [u[model.nu + 4 .+ (1:16)] for u in u_sol]
ψ_opt = [u[model.nu + 4 + 16 .+ (1:4)] for u in u_sol]
η_opt = [u[model.nu + 4 + 16 + 4 .+ (1:16)] for u in u_sol]

qm = copy(q_opt)
um = copy(u_opt)
γm = copy(γ_opt)
bm = copy(b_opt)
ψm = copy(ψ_opt)
ηm = copy(η_opt)
μm = model.μ_world

hm = h
timesteps = range(0.0, stop=(h * (length(qm) - 2)), length=(length(qm) - 2))
plot(hcat(qm...)', labels="")

plot(timesteps, hcat(qm[2:end-1]...)', labels="")
plot(timesteps, hcat(um...)', labels="")
plot(timesteps, hcat(γm...)', labels="")
plot(timesteps, hcat(bm...)', labels="")
plot(timesteps, hcat(ψm...)', labels="")
plot(timesteps, hcat(ηm...)', labels="")

using JLD2
@save joinpath(@__DIR__, "inplace_trot_10Hz_double.jld2") qm um γm bm ψm ηm μm hm
@load joinpath(@__DIR__, "inplace_trot_10Hz_double.jld2") qm um γm bm ψm ηm μm hm


text:
"""
    AbstractExtent

Abstract supertype for `Extent` objects, that hold all variables related
to space and time in a simulation. Usually the field of an output.
"""
abstract type AbstractExtent{I,M,A,PV} end

init(e::AbstractExtent) = e.init
mask(e::AbstractExtent) = e.mask
aux(e::AbstractExtent) = e.aux
@inline aux(e::AbstractExtent, key) = aux(aux(e), key)
@noinline aux(::Nothing, key) =
    throw(ArgumentError("No aux data available. Pass a NamedTuple to the `aux=` keyword of the Output"))
padval(e::AbstractExtent) = e.padval
tspan(e::AbstractExtent) = e.tspan # Never type-stable, only access in `modifyrule` methods
gridsize(extent::AbstractExtent) = gridsize(init(extent))

const EXTENT_KEYWORDS = """
- `init`: initialisation `Array`/`NamedTuple` for grid/s.
- `mask`: `BitArray` for defining cells that will/will not be run.
- `aux`: NamedTuple of arbitrary input data. Use `aux(data, Aux(:key))` to access from
    a `Rule` in a type-stable way.
- `padval`: padding value for grids with neighborhood rules. The default is
    `zero(eltype(init))`.
- `tspan`: Time span range. Never type-stable, only access this in `modifyrule` methods
"""

"""
    Extent <: AbstractExtent

    Extent(init, mask, aux, padval, tspan)
    Extent(; init, tspan, mask=nothing, aux=nothing, padval=zero(eltype(init)), kw...)

Container for extensive variables: spatial and timeseries data.
These are kept separate from rules to allow application
of rules to alternate spatial and temporal contexts.

Extent is not usually constructed directly by users, but it can be passed
to `Output` constructors instead of `init`, `mask`, `aux` and `tspan`.

# Arguments/Keywords

$EXTENT_KEYWORDS
"""
mutable struct Extent{I<:Union{AbstractArray,NamedTuple},
                      M<:Union{AbstractArray,Nothing},
                      A<:Union{NamedTuple,Nothing},
                      PV} <: AbstractExtent{I,M,A,PV}
    init::I
    mask::M
    aux::A
    padval::PV
    tspan::AbstractRange
    function Extent(init::I, mask::M, aux::A, padval::PV, tspan::T) where {I,M,A,PV,T}
        # Check grid sizes match
        gridsize = if init isa NamedTuple
            size_ = size(first(init))
            if !all(map(i -> size(i) == size_, init))
                throw(ArgumentError("`init` grid sizes do not match"))
            end
            size_
        else
            size(init)
        end
        if (mask !== nothing) && (size(mask) != gridsize)
            throw(ArgumentError("`mask` size do not match `init`"))
        end
        new{I,M,A,PV}(init, mask, aux, padval, tspan)
    end
end
Extent(; init, mask=nothing, aux=nothing, padval=_padval(init), tspan, kw...) =
    Extent(init, mask, aux, padval, tspan)

settspan!(e::Extent, tspan) = e.tspan = tspan

_padval(init::NamedTuple) = map(_padval, init)
_padval(init::AbstractArray{T}) where T = zero(T)

# An immutable `Extent` object, for internal use.
struct StaticExtent{I<:Union{AbstractArray,NamedTuple},
                    M<:Union{AbstractArray,Nothing},
                    A<:Union{NamedTuple,Nothing},
                    PV,T} <: AbstractExtent{I,M,A,PV}
    init::I
    mask::M
    aux::A
    padval::PV
    tspan::T
end
StaticExtent(e::Extent) = StaticExtent(init(e), mask(e), aux(e), padval(e), tspan(e))


text:
using Weber
using DataFrames: writetable, DataFrame

# HOW TO USE: Setup a video camera in front of your computer monitor and then
# run this script. Try as best you can to center the the monitor in the camera's
# view. Press record on the camera and then hit spacebar to start the
# test. Ideally you should increase the frame rate of your camera to at least 60
# fps. You can then use analyze_videotiming.jl to the extract onsets from the
# recorded image, and determine the timing accuracy of video playback.

timing = 0.5abs.(randn(Float64,100)) + 0.3
writetable("video_timing.csv",DataFrame(times = cumsum(timing),lengths = timing/2))

exp = Experiment()
setup(exp) do
  addbreak(instruct("Test is ready."))
  square = visual(ones(200,200),duration=0.2)
  blackness = visual(colorant"black",duration=Inf,priority=-1)

  black = moment(t -> display(blackness))
  moments = map(timing) do delta
    moment(delta,t -> display(square))
  end
  addtrial(black,moments)
end

run(exp)


text:
using PyCall

export read_data, read_hod, read_my_hod, read_abacus

function read_hod(run, snapshot)
    mimicus = pyimport("mimicus")
    pos, vel = mimicus.read_raw_data(run=run, snapshot=snapshot,hod=true)
    return permutedims(pos), permutedims(vel)
end

function read_my_hod(run, snapshot, galaxy_type)
    dq = pyimport("dq")
    pos, vel = dq.read_hod_data(run=run, snapshot=snapshot,galaxy_type=galaxy_type)
    return permutedims(pos), permutedims(vel)
end

function read_data(run, snapshot, number_density, boxsize)
    dq = pyimport("dq")
    pos, vel, m200c = dq.read_halo_data(run=run, snapshot=snapshot)
    pos, vel = dq.cut_by_number_density(pos, vel, m200c, number_density, boxsize)
    return permutedims(pos), permutedims(vel)
end

function read_data(run, snapshot, min_mass, max_mass, boxsize)
    dq = pyimport("dq")
    pos, vel, m200c = dq.read_halo_data(run=run, snapshot=snapshot)
    mask = (max_mass .>= m200c .>= min_mass)
    pos = pos[mask,:]
    vel = vel[mask,:]
    return permutedims(pos), permutedims(vel)
end

function read_abacus()
    pypairvel = pyimport("pypairvel")
    pos, vel, boxsize, redshift = pypairvel.read_abacus()
    pos = permutedims(pos)
    vel = permutedims(vel)
    return pos, vel, boxsize, redshift
end


text:
module CFLPLib

export CFLPData, CFLPFacility, CFLPCustomer, loadCFLP

const data_path = joinpath(pkgdir(CFLPLib), "data")

using ZipFile

include("Data.jl")
include("Loader.jl")

end


text:
using PEGParser

@grammar calc3 begin
  start = expr{ _1 }

  expr_op = term + op1 + expr
  expr = expr_op | term
  term_op = factor + op2 + term

  term = term_op | factor
  factor = number | pfactor
  pfactor = (lparen + expr + rparen){ _2 }
  op1 = add | sub
  op2 = mult | div

  number = (-space + float){ parse(Float64, _1.value) } | (-space + integer){
    parse(Int, _1.value)
  }
  add = (-space + "+"){ symbol(_1.value) }
  sub = (-space + "-"){ symbol(_1.value) }
  mult = (-space + "*"){ symbol(_1.value) }
  div = (-space + "/"){ symbol(_1.value) }

  lparen = (-space + "("){ _1 }
  rparen = (-space + ")"){ _1 }
  space = r"[ \n\r\t]*"
end

data = "3.145+5*(6-4.0)"
(ast, pos, error) = parse(calc3, data)
println(ast)

toexpr(node, cnodes, ::MatchRule{:default}) = cnodes
toexpr(node, cnodes, ::MatchRule{:term_op}) = Expr(:call, cnodes[2], cnodes[1], cnodes[3])
toexpr(node, cnodes, ::MatchRule{:expr_op}) = Expr(:call, cnodes[2], cnodes[1], cnodes[3])

code = transform(toexpr, ast)
dump(code)
println(eval(code))


text:
using NavAbilitySDK
using DistributedFactorGraphs
using RoME
using JSON

function exists(dfg::NavAbilityDFG, label::String)
  context = Client(dfg.userId,dfg.robotId,dfg.sessionId)
  gqlVariable = NavAbilitySDK.getVariable(dfg.navabilityClient, context, label) |> fetch
  if haskey(gqlVariable,"label") && gqlVariable["label"] == label
    return true
  end
  gqlFactor = NavAbilitySDK.getFactor(dfg.navabilityClient, context, label) |> fetch
  if haskey(gqlFactor,"label") && gqlFactor["label"] == label
    return true
  end
  return false
end


text:
immutable Block1{T}
  vec::AbstractVector{T}
  startidx::Int
end
block{T<:Number}(vec::AbstractVector{T}, startidx::Int) = Block1{T}(vec, startidx)

startidx(block::Block1) = block.startidx
endidx(block::Block1) = block.startidx + length(block.vec) - 1

# --------------------------------------------------------------------------------------------------

# you can create a sparse representation of blocked vectors with default val in the empty spaces
# example: this is a nice way to create the input vector to a regression that has a bias term: "v = view(length(x)+1, x)"
type BlockVector{T} <: AbstractVector{T}
  blocks::Vector{Block1{T}}
  defaultval::T
  len::Int
end

# function ArrayViews.view{T<:Number}(vs::AbstractVector{T}...; def::T = zero(T), len::Int = 0)
#   blocks = Array(Block1{T}, length(vs))
#   totlen = 0
#   for (i,v) in enumerate(vs)
#     blocks[i] = block(v, totlen + 1)
#     totlen += length(v)
#   end
#   BlockVector{T}(blocks, def, max(totlen, len))
# end

# # ArrayViews.view{T<:Number}(len::Int, vs::AbstractVector{T}...) = view(zero(T), len, vs...)
# # ArrayViews.view{T<:Number}(vs::AbstractVector{T}...) = view(zero(T), 0, vs...)

# --------------------------------------------------------------------------------------------------

function computeStartIndices{T}(vs::AbstractVector{T}...)
  indices = Array(Int, length(vs))
  totlen = 0
  for (i,v) in enumerate(vs)
    indices[i] = totlen + 1
    totlen += length(v)
  end
  indices
end

# methods to create the BlockVector with a list of start indices

# function ArrayViews.view{T<:Number}(indices::AbstractVector{Int}, vs::AbstractVector{T}...; def::T = zero(T), len::Int = 0)
# function ArrayViews.view{T<:Number}(vpairs::Pair{Int,AbstractVector{T}}...; def::T = zero(T), len::Int = 0)
function ArrayViews.view{T<:Number}(vs::AbstractVector{T}...; def::T = zero(T), len::Int = 0, indices::AbstractVector{Int} = computeStartIndices(vs...))
  @assert length(vs) == length(indices)
  blocks = Array(Block1{T}, length(vs))
  totlen = 0
  for (i,v) in enumerate(vs)
    startidx = indices[i]
    @assert startidx > totlen
    blocks[i] = block(v, startidx)
    totlen = startidx + length(v) - 1
  end
  BlockVector{T}(blocks, def, max(totlen, len))
end

# ArrayViews.view{T<:Number}(len::Int, indices::AbstractVector{Int}, vs::AbstractVector{T}...) = view(zero(T), indices, len, vs...)
# ArrayViews.view{T<:Number}(indices::AbstractVector{Int}, vs::AbstractVector{T}...) = view(zero(T), indices, 0, vs...)

# --------------------------------------------------------------------------------------------------

function Base.getindex(blockvec::BlockVector, i::Int)
  checkbounds(blockvec, i)
  for block in blockvec.blocks
    sidx = startidx(block)
    i < sidx && break
    i < sidx + length(block.vec) && return block.vec[i - sidx + 1]
  end
  return blockvec.defaultval
end

setindex_error() = error("You're calling BlockVector::setindex for an index without an underlying array... not currently allowed")

function Base.setindex!{T<:Number}(blockvec::BlockVector{T}, data::T, i::Int)
  checkbounds(blockvec, i)
  for block in blockvec.blocks
    sidx = startidx(block)
    i < sidx && setindex_error()
    if i < sidx + length(block.vec)
      block.vec[i - sidx + 1] = data
      return
    end
  end
  setindex_error()
end

Base.length(blockvec::BlockVector) = blockvec.len
Base.size(blockvec::BlockVector) = (length(blockvec),)

# --------------------------------------------------------------------------------------------------


text:
using CubedSphere
using JLD2
using OffsetArrays
using Adapt
using Adapt: adapt_structure

using Oceananigans

struct ConformalCubedSphereFaceGrid{FT, TX, TY, TZ, A, R, Arch} <: AbstractHorizontallyCurvilinearGrid{FT, TX, TY, TZ, Arch}
    architecture :: Arch
    Nx :: Int
    Ny :: Int
    Nz :: Int
    Hx :: Int
    Hy :: Int
    Hz :: Int
    λᶜᶜᵃ :: A
    λᶠᶜᵃ :: A
    λᶜᶠᵃ :: A
    λᶠᶠᵃ :: A
    φᶜᶜᵃ :: A
    φᶠᶜᵃ :: A
    φᶜᶠᵃ :: A
    φᶠᶠᵃ :: A
    zᵃᵃᶜ :: R
    zᵃᵃᶠ :: R
    Δxᶜᶜᵃ :: A
    Δxᶠᶜᵃ :: A
    Δxᶜᶠᵃ :: A
    Δxᶠᶠᵃ :: A
    Δyᶜᶜᵃ :: A
    Δyᶜᶠᵃ :: A
    Δyᶠᶜᵃ :: A
    Δyᶠᶠᵃ :: A
    Δz    :: FT
    Azᶜᶜᵃ :: A
    Azᶠᶜᵃ :: A
    Azᶜᶠᵃ :: A
    Azᶠᶠᵃ :: A
    radius :: FT

    function ConformalCubedSphereFaceGrid{TX, TY, TZ}(architecture::Arch,
                                                      Nx, Ny, Nz,
                                                      Hx, Hy, Hz,
                                                       λᶜᶜᵃ :: A,  λᶠᶜᵃ :: A,  λᶜᶠᵃ :: A,  λᶠᶠᵃ :: A,
                                                       φᶜᶜᵃ :: A,  φᶠᶜᵃ :: A,  φᶜᶠᵃ :: A,  φᶠᶠᵃ :: A, zᵃᵃᶜ :: R, zᵃᵃᶠ :: R,
                                                      Δxᶜᶜᵃ :: A, Δxᶠᶜᵃ :: A, Δxᶜᶠᵃ :: A, Δxᶠᶠᵃ :: A,
                                                      Δyᶜᶜᵃ :: A, Δyᶜᶠᵃ :: A, Δyᶠᶜᵃ :: A, Δyᶠᶠᵃ :: A, Δz :: FT,
                                                      Azᶜᶜᵃ :: A, Azᶠᶜᵃ :: A, Azᶜᶠᵃ :: A, Azᶠᶠᵃ :: A,
                                                      radius :: FT) where {TX, TY, TZ, FT, A, R, Arch}

        return new{FT, TX, TY, TZ, A, R, Arch}(architecture,
                                               Nx, Ny, Nz,
                                               Hx, Hy, Hz,
                                               λᶜᶜᵃ, λᶠᶜᵃ, λᶜᶠᵃ, λᶠᶠᵃ,
                                               φᶜᶜᵃ, φᶠᶜᵃ, φᶜᶠᵃ, φᶠᶠᵃ, zᵃᵃᶜ, zᵃᵃᶠ,
                                               Δxᶜᶜᵃ, Δxᶠᶜᵃ, Δxᶜᶠᵃ, Δxᶠᶠᵃ,
                                               Δyᶜᶜᵃ, Δyᶜᶠᵃ, Δyᶠᶜᵃ, Δyᶠᶠᵃ, Δz,
                                               Azᶜᶜᵃ, Azᶠᶜᵃ, Azᶜᶠᵃ, Azᶠᶠᵃ, radius)
    end
end

function ConformalCubedSphereFaceGrid(arch::AbstractArchitecture = CPU(),
                                      FT::DataType = Float64;
                                      size,
                                      z,
                                      topology = (Bounded, Bounded, Bounded),
                                      ξ = (-1, 1),
                                      η = (-1, 1),
                                      radius = R_Earth,
                                      halo = (1, 1, 1),
                                      rotation = nothing)

    TX, TY, TZ = topology
    Nξ, Nη, Nz = size
    Hx, Hy, Hz = halo
    Arch = typeof(arch)

    ## Use a regular rectilinear grid for the face of the cube

    ξη_grid = RectilinearGrid(arch, FT, topology=topology, size=(Nξ, Nη, Nz), x=ξ, y=η, z=z, halo=halo)

    ξᶠᵃᵃ = xnodes(Face, ξη_grid)
    ξᶜᵃᵃ = xnodes(Center, ξη_grid)
    ηᵃᶠᵃ = ynodes(Face, ξη_grid)
    ηᵃᶜᵃ = ynodes(Center, ξη_grid)

    ## The vertical coordinates can come out of the regular rectilinear grid!

    Δz = ξη_grid.Δzᵃᵃᶜ
    zᵃᵃᶠ = ξη_grid.zᵃᵃᶠ
    zᵃᵃᶜ = ξη_grid.zᵃᵃᶜ

    ## Compute staggered grid Cartesian coordinates (X, Y, Z) on the unit sphere.

    Xᶜᶜᵃ = zeros(Nξ,   Nη  )
    Xᶠᶜᵃ = zeros(Nξ+1, Nη  )
    Xᶜᶠᵃ = zeros(Nξ,   Nη+1)
    Xᶠᶠᵃ = zeros(Nξ+1, Nη+1)

    Yᶜᶜᵃ = zeros(Nξ,   Nη  )
    Yᶠᶜᵃ = zeros(Nξ+1, Nη  )
    Yᶜᶠᵃ = zeros(Nξ,   Nη+1)
    Yᶠᶠᵃ = zeros(Nξ+1, Nη+1)

    Zᶜᶜᵃ = zeros(Nξ,   Nη  )
    Zᶠᶜᵃ = zeros(Nξ+1, Nη  )
    Zᶜᶠᵃ = zeros(Nξ,   Nη+1)
    Zᶠᶠᵃ = zeros(Nξ+1, Nη+1)

    ξS = (ξᶜᵃᵃ, ξᶠᵃᵃ, ξᶜᵃᵃ, ξᶠᵃᵃ)
    ηS = (ηᵃᶜᵃ, ηᵃᶜᵃ, ηᵃᶠᵃ, ηᵃᶠᵃ)
    XS = (Xᶜᶜᵃ, Xᶠᶜᵃ, Xᶜᶠᵃ, Xᶠᶠᵃ)
    YS = (Yᶜᶜᵃ, Yᶠᶜᵃ, Yᶜᶠᵃ, Yᶠᶠᵃ)
    ZS = (Zᶜᶜᵃ, Zᶠᶜᵃ, Zᶜᶠᵃ, Zᶠᶠᵃ)

    for (ξ, η, X, Y, Z) in zip(ξS, ηS, XS, YS, ZS)
        for i in 1:length(ξ), j in 1:length(η)
            @inbounds X[i, j], Y[i, j], Z[i, j] = conformal_cubed_sphere_mapping(ξ[i], η[j])
        end
    end

    ## Rotate the face if it's not the +z face (the one containing the North Pole).

    if !isnothing(rotation)
        for (ξ, η, X, Y, Z) in zip(ξS, ηS, XS, YS, ZS)
            for i in 1:length(ξ), j in 1:length(η)
                @inbounds X[i, j], Y[i, j], Z[i, j] = rotation * [X[i, j], Y[i, j], Z[i, j]]
            end
        end
    end

    ## Compute staggered grid latitude-longitude (φ, λ) coordinates.

    λᶜᶜᵃ = OffsetArray(zeros(Nξ + 2Hx,     Nη + 2Hy    ), -Hx, -Hy)
    λᶠᶜᵃ = OffsetArray(zeros(Nξ + 2Hx + 1, Nη + 2Hy    ), -Hx, -Hy)
    λᶜᶠᵃ = OffsetArray(zeros(Nξ + 2Hx,     Nη + 2Hy + 1), -Hx, -Hy)
    λᶠᶠᵃ = OffsetArray(zeros(Nξ + 2Hx + 1, Nη + 2Hy + 1), -Hx, -Hy)

    φᶜᶜᵃ = OffsetArray(zeros(Nξ + 2Hx,     Nη + 2Hy    ), -Hx, -Hy)
    φᶠᶜᵃ = OffsetArray(zeros(Nξ + 2Hx + 1, Nη + 2Hy    ), -Hx, -Hy)
    φᶜᶠᵃ = OffsetArray(zeros(Nξ + 2Hx,     Nη + 2Hy + 1), -Hx, -Hy)
    φᶠᶠᵃ = OffsetArray(zeros(Nξ + 2Hx + 1, Nη + 2Hy + 1), -Hx, -Hy)

    λS = (λᶜᶜᵃ, λᶠᶜᵃ, λᶜᶠᵃ, λᶠᶠᵃ)
    φS = (φᶜᶜᵃ, φᶠᶜᵃ, φᶜᶠᵃ, φᶠᶠᵃ)

    for (ξ, η, X, Y, Z, λ, φ) in zip(ξS, ηS, XS, YS, ZS, λS, φS)
        for i in 1:length(ξ), j in 1:length(η)
            @inbounds φ[i, j], λ[i, j] = cartesian_to_lat_lon(X[i, j], Y[i, j], Z[i, j])
        end
    end

    any(any.(isnan, λS)) &&
        @warn "Your cubed sphere face contains a grid point at a pole so its longitude λ is undefined (NaN)."

    ## Not sure how to compute these right now so how about zeros?

    Δxᶜᶜᵃ = OffsetArray(zeros(Nξ + 2Hx,     Nη + 2Hy    ), -Hx, -Hy)
    Δxᶠᶜᵃ = OffsetArray(zeros(Nξ + 2Hx + 1, Nη + 2Hy    ), -Hx, -Hy)
    Δxᶜᶠᵃ = OffsetArray(zeros(Nξ + 2Hx,     Nη + 2Hy + 1), -Hx, -Hy)
    Δxᶠᶠᵃ = OffsetArray(zeros(Nξ + 2Hx + 1, Nη + 2Hy + 1), -Hx, -Hy)

    Δyᶜᶜᵃ = OffsetArray(zeros(Nξ + 2Hx,     Nη + 2Hy    ), -Hx, -Hy)
    Δyᶜᶠᵃ = OffsetArray(zeros(Nξ + 2Hx + 1, Nη + 2Hy    ), -Hx, -Hy)
    Δyᶠᶜᵃ = OffsetArray(zeros(Nξ + 2Hx,     Nη + 2Hy + 1), -Hx, -Hy)
    Δyᶠᶠᵃ = OffsetArray(zeros(Nξ + 2Hx + 1, Nη + 2Hy + 1), -Hx, -Hy)

    Azᶜᶜᵃ = OffsetArray(zeros(Nξ + 2Hx,     Nη + 2Hy    ), -Hx, -Hy)
    Azᶠᶜᵃ = OffsetArray(zeros(Nξ + 2Hx + 1, Nη + 2Hy    ), -Hx, -Hy)
    Azᶜᶠᵃ = OffsetArray(zeros(Nξ + 2Hx,     Nη + 2Hy + 1), -Hx, -Hy)
    Azᶠᶠᵃ = OffsetArray(zeros(Nξ + 2Hx + 1, Nη + 2Hy + 1), -Hx, -Hy)

    return ConformalCubedSphereFaceGrid{TX, TY, TZ}(arch, Nξ, Nη, Nz, Hx, Hy, Hz,
                                                     λᶜᶜᵃ,  λᶠᶜᵃ,  λᶜᶠᵃ,  λᶠᶠᵃ,
                                                     φᶜᶜᵃ,  φᶠᶜᵃ,  φᶜᶠᵃ,  φᶠᶠᵃ, zᵃᵃᶜ, zᵃᵃᶠ,
                                                    Δxᶜᶜᵃ, Δxᶠᶜᵃ, Δxᶜᶠᵃ, Δxᶠᶠᵃ,
                                                    Δyᶜᶜᵃ, Δyᶜᶠᵃ, Δyᶠᶜᵃ, Δyᶠᶠᵃ, Δz,
                                                    Azᶜᶜᵃ, Azᶠᶜᵃ, Azᶜᶠᵃ, Azᶠᶠᵃ, radius)
end

# architecture = CPU() default, assuming that a DataType positional arg
# is specifying the floating point type.
ConformalCubedSphereFaceGrid(FT::DataType; kwargs...) = ConformalCubedSphereFaceGrid(CPU(), FT; kwargs...)

function load_and_offset_cubed_sphere_data(file, FT, arch, field_name, loc, topo, N, H)

    ii = interior_indices(loc[1], topo[1], N[1])
    jj = interior_indices(loc[2], topo[2], N[2])

    interior_data = arch_array(arch, file[field_name][ii, jj])

    underlying_data = zeros(FT, arch,
                            total_length(loc[1], topo[1], N[1], H[1]),
                            total_length(loc[2], topo[2], N[2], H[2]))

    ip = interior_parent_indices(loc[1], topo[1], N[1], H[1])
    jp = interior_parent_indices(loc[2], topo[2], N[2], H[2])

    view(underlying_data, ip, jp) .= interior_data

    return offset_data(underlying_data, loc, topo, N, H)
end

function ConformalCubedSphereFaceGrid(filepath::AbstractString, architecture = CPU(), FT = Float64;
                                      face, Nz, z,
                                      topology = (Bounded, Bounded, Bounded),
                                        radius = R_Earth,
                                          halo = (1, 1, 1),
                                      rotation = nothing)

    TX, TY, TZ = topo = topology
    Hx, Hy, Hz = halo

    ## Use a regular rectilinear grid for the vertical grid
    ## The vertical coordinates can come out of the regular rectilinear grid!

    ξη_grid = RectilinearGrid(architecture, FT, topology=topology, size=(1, 1, Nz), x=(0, 1), y=(0, 1), z=z, halo=halo)

    Δz = ξη_grid.Δzᵃᵃᶜ
    zᵃᵃᶠ = ξη_grid.zᵃᵃᶠ
    zᵃᵃᶜ = ξη_grid.zᵃᵃᶜ

    ## Read everything else from the file

    file = jldopen(filepath, "r")["face$face"]
    Nξ, Nη = size(file["λᶠᶠᵃ"]) .- 1

    N = (Nξ, Nη, Nz)
    H = halo

    loc_cc = (Center, Center)
    loc_cf = (Center, Face)
    loc_fc = (Face, Center)
    loc_ff = (Face, Face)

    topo_bbb = (Bounded, Bounded, Bounded)

    λᶜᶜᵃ = load_and_offset_cubed_sphere_data(file, FT, architecture, "λᶜᶜᵃ", loc_cc, topo_bbb, N, H)
    λᶠᶠᵃ = load_and_offset_cubed_sphere_data(file, FT, architecture, "λᶠᶠᵃ", loc_ff, topo_bbb, N, H)

    φᶜᶜᵃ = load_and_offset_cubed_sphere_data(file, FT, architecture, "φᶜᶜᵃ", loc_cc, topo_bbb, N, H)
    φᶠᶠᵃ = load_and_offset_cubed_sphere_data(file, FT, architecture, "φᶠᶠᵃ", loc_ff, topo_bbb, N, H)

    Δxᶜᶜᵃ = load_and_offset_cubed_sphere_data(file, FT, architecture, "Δxᶜᶜᵃ", loc_cc, topo_bbb, N, H)
    Δxᶠᶜᵃ = load_and_offset_cubed_sphere_data(file, FT, architecture, "Δxᶠᶜᵃ", loc_fc, topo_bbb, N, H)
    Δxᶜᶠᵃ = load_and_offset_cubed_sphere_data(file, FT, architecture, "Δxᶜᶠᵃ", loc_cf, topo_bbb, N, H)
    Δxᶠᶠᵃ = load_and_offset_cubed_sphere_data(file, FT, architecture, "Δxᶠᶠᵃ", loc_ff, topo_bbb, N, H)

    Δyᶜᶜᵃ = load_and_offset_cubed_sphere_data(file, FT, architecture, "Δyᶜᶜᵃ", loc_cc, topo_bbb, N, H)
    Δyᶠᶜᵃ = load_and_offset_cubed_sphere_data(file, FT, architecture, "Δyᶠᶜᵃ", loc_fc, topo_bbb, N, H)
    Δyᶜᶠᵃ = load_and_offset_cubed_sphere_data(file, FT, architecture, "Δyᶜᶠᵃ", loc_cf, topo_bbb, N, H)
    Δyᶠᶠᵃ = load_and_offset_cubed_sphere_data(file, FT, architecture, "Δyᶠᶠᵃ", loc_ff, topo_bbb, N, H)

    Azᶜᶜᵃ = load_and_offset_cubed_sphere_data(file, FT, architecture, "Azᶜᶜᵃ", loc_cc, topo_bbb, N, H)
    Azᶠᶜᵃ = load_and_offset_cubed_sphere_data(file, FT, architecture, "Azᶠᶜᵃ", loc_fc, topo_bbb, N, H)
    Azᶜᶠᵃ = load_and_offset_cubed_sphere_data(file, FT, architecture, "Azᶜᶠᵃ", loc_cf, topo_bbb, N, H)
    Azᶠᶠᵃ = load_and_offset_cubed_sphere_data(file, FT, architecture, "Azᶠᶠᵃ", loc_ff, topo_bbb, N, H)

    ## Maybe we won't need these?
    Txᶠᶜ = total_length(loc_fc[1], topology[1], N[1], H[1])
    Txᶜᶠ = total_length(loc_cf[1], topology[1], N[1], H[1])
    Tyᶠᶜ = total_length(loc_fc[2], topology[2], N[2], H[2])
    Tyᶜᶠ = total_length(loc_cf[2], topology[2], N[2], H[2])

    λᶠᶜᵃ = offset_data(zeros(FT, architecture, Txᶠᶜ, Tyᶠᶜ), loc_fc, topology, N, H)
    λᶜᶠᵃ = offset_data(zeros(FT, architecture, Txᶜᶠ, Tyᶜᶠ), loc_cf, topology, N, H)
    φᶠᶜᵃ = offset_data(zeros(FT, architecture, Txᶠᶜ, Tyᶠᶜ), loc_fc, topology, N, H)
    φᶜᶠᵃ = offset_data(zeros(FT, architecture, Txᶜᶠ, Tyᶜᶠ), loc_cf, topology, N, H)

    return ConformalCubedSphereFaceGrid{TX, TY, TZ}(architecture, Nξ, Nη, Nz, Hx, Hy, Hz,
         λᶜᶜᵃ,  λᶠᶜᵃ,  λᶜᶠᵃ,  λᶠᶠᵃ,  φᶜᶜᵃ,  φᶠᶜᵃ,  φᶜᶠᵃ,  φᶠᶠᵃ, zᵃᵃᶜ, zᵃᵃᶠ,
        Δxᶜᶜᵃ, Δxᶠᶜᵃ, Δxᶜᶠᵃ, Δxᶠᶠᵃ, Δyᶜᶜᵃ, Δyᶜᶠᵃ, Δyᶠᶜᵃ, Δyᶠᶠᵃ, Δz,
        Azᶜᶜᵃ, Azᶠᶜᵃ, Azᶜᶠᵃ, Azᶠᶠᵃ, radius)
end

function on_architecture(arch::AbstractArchitecture, grid::ConformalCubedSphereFaceGrid)
    horizontal_coordinates = (:λᶜᶜᵃ,
                              :λᶠᶜᵃ,
                              :λᶜᶠᵃ,
                              :λᶠᶠᵃ,
                              :φᶜᶜᵃ,
                              :φᶠᶜᵃ,
                              :φᶜᶠᵃ,
                              :φᶠᶠᵃ)

    horizontal_grid_spacings = (:Δxᶜᶜᵃ,
                                :Δxᶠᶜᵃ,
                                :Δxᶜᶠᵃ,
                                :Δxᶠᶠᵃ,
                                :Δyᶜᶜᵃ,
                                :Δyᶜᶠᵃ,
                                :Δyᶠᶜᵃ,
                                :Δyᶠᶠᵃ)

    horizontal_areas = (:Azᶜᶜᵃ,
                        :Azᶠᶜᵃ,
                        :Azᶜᶠᵃ,
                        :Azᶠᶠᵃ)

    horizontal_grid_spacing_data = Tuple(arch_array(arch, getproperty(grid, name)) for name in horizontal_grid_spacings)
    horizontal_coordinate_data = Tuple(arch_array(arch, getproperty(grid, name)) for name in horizontal_coordinates)
    horizontal_area_data = Tuple(arch_array(arch, getproperty(grid, name)) for name in horizontal_areas)

    zᵃᵃᶜ = arch_array(arch, grid.zᵃᵃᶜ)
    zᵃᵃᶠ = arch_array(arch, grid.zᵃᵃᶠ)

    TX, TY, TZ = topology(grid)

    new_grid = ConformalCubedSphereFaceGrid{TX, TY, TZ}(arch,
                                                        grid.Nx, grid.Ny, grid.Nz,
                                                        grid.Hx, grid.Hy, grid.Hz,
                                                        horizontal_coordinate_data..., zᵃᵃᶜ, zᵃᵃᶠ,
                                                        horizontal_grid_spacing_data..., grid.Δz,
                                                        horizontal_area_data..., grid.radius)

    return new_grid
end

function Adapt.adapt_structure(to, grid::ConformalCubedSphereFaceGrid)
    TX, TY, TZ = topology(grid)
    return ConformalCubedSphereFaceGrid{TX, TY, TZ}(nothing,
                                                    grid.Nx, grid.Ny, grid.Nz,
                                                    grid.Hx, grid.Hy, grid.Hz,
                                                    adapt(to, grid.λᶜᶜᵃ),
                                                    adapt(to, grid.λᶠᶜᵃ),
                                                    adapt(to, grid.λᶜᶠᵃ),
                                                    adapt(to, grid.λᶠᶠᵃ),
                                                    adapt(to, grid.φᶜᶜᵃ),
                                                    adapt(to, grid.φᶠᶜᵃ),
                                                    adapt(to, grid.φᶜᶠᵃ),
                                                    adapt(to, grid.φᶠᶠᵃ),
                                                    adapt(to, grid.zᵃᵃᶜ),
                                                    adapt(to, grid.zᵃᵃᶠ),
                                                    adapt(to, grid.Δxᶜᶜᵃ),
                                                    adapt(to, grid.Δxᶠᶜᵃ),
                                                    adapt(to, grid.Δxᶜᶠᵃ),
                                                    adapt(to, grid.Δxᶠᶠᵃ),
                                                    adapt(to, grid.Δyᶜᶜᵃ),
                                                    adapt(to, grid.Δyᶜᶠᵃ),
                                                    adapt(to, grid.Δyᶠᶜᵃ),
                                                    adapt(to, grid.Δyᶠᶠᵃ),
                                                    grid.Δz,
                                                    adapt(to, grid.Azᶜᶜᵃ),
                                                    adapt(to, grid.Azᶠᶜᵃ),
                                                    adapt(to, grid.Azᶜᶠᵃ),
                                                    adapt(to, grid.Azᶠᶠᵃ),
                                                    grid.radius)
end

function Base.show(io::IO, g::ConformalCubedSphereFaceGrid{FT}) where FT
    print(io, "ConformalCubedSphereFaceGrid{$FT}\n",
              "        size (Nx, Ny, Nz): ", (g.Nx, g.Ny, g.Nz), '\n',
              "        halo (Hx, Hy, Hz): ", (g.Hx, g.Hy, g.Hz))
end

@inline xnode(::Face,   ::Face,   LZ, i, j, k, grid::ConformalCubedSphereFaceGrid) = @inbounds grid.λᶠᶠᵃ[i, j]
@inline xnode(::Face,   ::Center, LZ, i, j, k, grid::ConformalCubedSphereFaceGrid) = @inbounds grid.λᶠᶜᵃ[i, j]
@inline xnode(::Center, ::Face,   LZ, i, j, k, grid::ConformalCubedSphereFaceGrid) = @inbounds grid.λᶜᶠᵃ[i, j]
@inline xnode(::Center, ::Center, LZ, i, j, k, grid::ConformalCubedSphereFaceGrid) = @inbounds grid.λᶜᶜᵃ[i, j]

@inline ynode(::Face,   ::Face,   LZ, i, j, k, grid::ConformalCubedSphereFaceGrid) = @inbounds grid.φᶠᶠᵃ[i, j]
@inline ynode(::Face,   ::Center, LZ, i, j, k, grid::ConformalCubedSphereFaceGrid) = @inbounds grid.φᶠᶜᵃ[i, j]
@inline ynode(::Center, ::Face,   LZ, i, j, k, grid::ConformalCubedSphereFaceGrid) = @inbounds grid.φᶜᶠᵃ[i, j]
@inline ynode(::Center, ::Center, LZ, i, j, k, grid::ConformalCubedSphereFaceGrid) = @inbounds grid.φᶜᶜᵃ[i, j]

@inline znode(LX, LY, ::Face,   i, j, k, grid::ConformalCubedSphereFaceGrid) = @inbounds grid.zᵃᵃᶠ[k]
@inline znode(LX, LY, ::Center, i, j, k, grid::ConformalCubedSphereFaceGrid) = @inbounds grid.zᵃᵃᶜ[k]


text:
export Camera

struct Camera{T}
    origin::Point3{T}
    lower_left_corner::Point3{T}
    horizontal::Vec3{T}
    vertical::Vec3{T}
end

function Camera{T}() where T
    aspect_ratio = T(16) / T(9)
    viewport_height = T(2)
    viewport_width = aspect_ratio * viewport_height
    focal_length = T(1)

    origin = Point3{T}(0, 0, 0)
    horizontal = Vec3{T}(viewport_width, 0, 0)
    vertical = Vec3{T}(0, viewport_height, 0)
    lower_left_corner = origin - horizontal/T(2) - vertical/T(2) - Vec3{T}(0, 0, focal_length)

    return Camera(origin, lower_left_corner, horizontal, vertical)
end

function get_ray(cam, u, v)
    return Ray(cam.origin, cam.lower_left_corner + u*cam.horizontal + v*cam.vertical - cam.origin)
end


text:
@assert begin
    bfun=_get_gradfunc(:(1.0-xi-eta-zeta), [:xi, :eta, :zeta], 2)

    grad=bfun(zeros(Float64, 3)...)

    all(
        @. isapprox(grad, -1.0)
    )
end

@assert begin
    intp=InterpolationFunction(
        :(a0+xi*a1+eta*a2),
        [
            :xi, :eta
        ],
        [
            :a0, :a1, :a2
        ],
        [
            [0, 0]
        ]
    )

    dmn=Domain(
        intp,
        [
            (
                0.0, 1.0
            ),
            (
                0.0, :(1.0-xi)
            )
        ],
        [
            [0.0, 0.0],
            [1.0, 0.0],
            [0.0, 1.0]
        ]
    )

    vals=[1.0, 0.0, 0.0]
    coefs=regress(dmn, vals)

    isapprox(coefs, [1.0, -1.0, -1.0])
end


text:
using Maker
using Base.Test

## Scenarios to test
##  - basic
##  - run make(), mimic restart (kill TARGETS), run make()
##  - run make(), mimic restart (kill TARGETS), redefine "c", run make()
##  - run make(), mimic restart (kill TARGETS), redefine "e", run make()
##  - run make(), mimic fresh start (kill jld & TARGETS), redefine "c", run make()
##  - run make(), mimic fresh start (kill jld & TARGETS), redefine "e", run make()

Maker.rm("c.csv")
Maker.rm("e.csv")
Maker.rm(".maker-cache.jld")
x = [ 3.14159   6.28319   9.42478
      12.5664   15.708    18.8496
      21.9911   25.1327   28.2743 ]
writecsv("in1.csv", x)
writecsv("in2.csv", 3x)

println("=== Base ===")
module X
using Maker
using Base.Test
COUNT = 0
empty!(Maker.TARGETS)
include("targets.jl")
make("e.csv")
@test COUNT == 7
end # module

println("=== Steps ===")
Maker.rm("c.csv")
Maker.rm("e.csv")
module X
using Maker
using Base.Test
COUNT = 0
empty!(Maker.TARGETS)
include("targets.jl")
make("a")
@test COUNT == 1
make("b")
@test COUNT == 2
make("c.csv")
@test COUNT == 3
make("c")
@test COUNT == 4
make("d")
@test COUNT == 5
make("e")
@test COUNT == 6
make("e.csv")
@test COUNT == 7
end # module

println("=== Partial Steps ===")
Maker.rm("c.csv")
Maker.rm("e.csv")
module X
using Maker
using Base.Test
COUNT = 0
empty!(Maker.TARGETS)
include("targets.jl")
make(["a", "d"])
@test COUNT == 2
make("e")
@test COUNT == 6
end # module

println("=== Redo ===")
module X
using Maker
using Base.Test
COUNT = 0
empty!(Maker.TARGETS)
include("targets.jl")
make("e.csv", verbose = true)
@test COUNT == 6
end # module

println("=== Remove e.csv ===")
Maker.rm("e.csv")
module X
using Maker
using Base.Test
COUNT = 0
empty!(Maker.TARGETS)
include("targets.jl")
make("e.csv")
@test COUNT == 6
end # module

println("=== Redefine c.csv dependencies ===")
module X
using Maker
using Base.Test
COUNT = 0
empty!(Maker.TARGETS)
include("targets.jl")
# change the dependencies of one of the tasks
Maker.file("c.csv", ["a", "b", "a"]) do
    global COUNT += 1
    writecsv("c.csv", a * b)
end
make("e.csv")
@test COUNT == 7
end # module

println("=== Redefine c.csv ===")
module X
using Maker
using Base.Test
COUNT = 0
empty!(Maker.TARGETS)
include("targets.jl")
# change one of the tasks
Maker.file("c.csv", ["a", "b"]) do
    global COUNT += 1
    a = 1
    writecsv("c.csv", a * b)
end
make("e.csv")
@test COUNT == 7
end # module

println("=== Change var ===")
module X
using Maker
using Base.Test
COUNT = 0
empty!(Maker.TARGETS)
include("targets.jl")
make("e.csv")
@test COUNT == 7
c = 27
make("e.csv")
@test COUNT == 8
@test c != 27
end # module

println("=== Redefine var ===")
module X
using Maker
using Base.Test
COUNT = 0
empty!(Maker.TARGETS)
include("targets.jl")
make("e.csv")
@test COUNT == 5
include("targets.jl")
make("e.csv")
@test COUNT == 5
COUNT = 0
Maker.variable("c", "c.csv") do # redefine action
    global COUNT += 1
    -readcsv("c.csv")
end
make("e.csv")
@test COUNT == 3
COUNT = 0
Maker.variable("c", ["c.csv", "a"]) do # redefine dependencies
    global COUNT += 1
    -readcsv("c.csv")
end
make("e.csv")
@test COUNT == 3
COUNT = 0
Maker.variable("c", ["c.csv", "a"]) do # redefine to same (shouldn't rerun)
    global COUNT += 1
    -readcsv("c.csv")
end
make("e.csv")
@test COUNT == 0
end # module

Maker.rm("c.csv")
Maker.rm("e.csv")
Maker.rm("in1.csv")
Maker.rm("in2.csv")


text:
type SRacos
    rc::RacosCommon
    function SRacos()
        return new(RacosCommon())
    end
end

# SRacos's optimization function
# Default strategy is WR(worst sracos_replace!)
# Default uncertain_bits is 1, but actually ub will be set either by user or by RacosOptimization automatically.
function sracos_opt!(sracos::SRacos, objective::Objective, parameter::Parameter;
    ub=1)
    strategy = parameter.replace_strategy
    rc = sracos.rc
    rc_clear!(rc)
    rc.objective = objective
    rc.parameter = parameter
    init_attribute!(rc)
    i = 0
    iteration_num = rc.parameter.budget - rc.parameter.train_size
    time_log1 = now()
    while i < iteration_num
        i += 1
        if rand(rng, Float64) < rc.parameter.probability
            classifier = RacosClassification(rc.objective.dim, rc.positive_data,
            rc.negative_data, ub=ub)
            mixed_classification(classifier)
            solution, distinct_flag = distinct_sample_classifier(rc, classifier, data_num=rc.parameter.train_size)
        else
            solution, distinct_flag = distinct_sample(rc, rc.objective.dim)
        end
        #painc stop
        if isnull(solution)
            return rc.best_solution
        end
        if !distinct_flag
            zoolog("distinct_error")
            continue
        end
        # evaluate the solution
        obj_eval(objective, solution)
        bad_ele = sracos_replace!(rc.positive_data, solution, "pos")
        sracos_replace!(rc.negative_data, bad_ele, "neg", strategy=strategy)
        rc.best_solution = rc.positive_data[1]
        if i == 4
            time_log2 = now()
            expected_time = (parameter.budget - parameter.train_size) *
              (Dates.value(time_log2 - time_log1) / 1000) / 5
            if !isnull(rc.parameter.time_limit)
                expected_time = min(expected_time, rc.parameter.time_limit)
            end
            if expected_time > 5
              zoolog(string("expected remaining running time: ", convert_time(expected_time)))
            end
        end
        # time budget check
        if !isnull(rc.parameter.time_limit)
            if expected_time >= rc.parameter.time_limit
              zoolog("Exceed time limit")
              return rc.best_solution
            end
        end
        # terminal_value check
        if !isnull(rc.parameter.terminal_value)
            if rc.best_solution.value <= rc.parameter.terminal_value
                zoolog("terminal_value function value reached")
                return rc.best_solution
            end
        end
    end
    return rc.best_solution
end

function sracos_replace!(iset, x, iset_type; strategy="WR")
    if strategy == "WR"
        return strategy_wr(iset, x, iset_type)
    elseif strategy == "RR"
        return strategy_rr(iset, x)
    elseif strategy == "LM"
        best_sol, best_index = find_min(iset)
        return strategy_lm(iset, best_sol, x)
    else
        zoolog("No such strategy")
    end
end

# Find first element larger than x
function binary_search(iset, x, ibegin::Int64, iend::Int64)
    x_value = x.value
    if x_value <= iset[ibegin].value
        return ibegin
    end
    if x_value >= iset[end].value
        return iend + 1
    end
    if iend == ibegin + 1
        return iend
    end
    mid = div(ibegn + iend, 2)
    if x_value <= iset[mid].value
        return binary_search(iset, x, ibegin, mid)
    else
        return binary_search(iset, x, mid, iend)
    end
end

# Worst replace
function strategy_wr(iset, x, iset_type)
    if iset_type == "pos"
        index = binary_search(iset, x, 1, length(iset))
        insert!(iset, index, x)
        worst_ele = pop!(iset)
    else
        worst_ele, worst_index = find_max(iset)
        if worst_ele.value > x.value
            iset[worst_index] = x
        else
            worst_ele = x
        end
    end
    return worst_ele
end

# Random replace
function strategy_rr(iset, x)
    len_iset = length(iset)
    replace_index = rand(rng, 1:len_iset)
    replace_ele = iset[replace_index]
    iset[replace_index] = x
    return replace_ele
end

# replace the farthest solution from best_sol
function strategy_lm(iset, best_sol, sol)
    farthest_dis = 0
    farthest_index = 0
    for i in 1:length(iset)
        dis = mydistance(iset[i].x, best_sol.x)
        if dis > farthest_dis
            farthest_dis = dis
            farthest_index = i
        end
    end
    farthest_ele = iset[farthest_index]
    iset[farthest_index] = sol
    return farthest_ele
end


text:
"""
TEST FILE FOR VARIOUS TEMPORAL META GRAPH THINGS
"""

using QuNet
using LightGraphs, SimpleWeightedGraphs, GraphPlot, MetaGraphs, Plots, Colors, Statistics
using LaTeXStrings
using JLD
using Parameters

datafile = "data/timedepth_test"

num_trials = 100::Int64
min_depth = 5
max_depth = 10::Int64
num_pairs = 50::Int64
grid_size = 10::Int64
async_cost = 0.01
src_layer= 5
dst_layer= -1

perf_data = []
perf_err = []
path_data = []
path_err = []

for i in min_depth:max_depth

    # Debug
    # global dst_layer
    # if dst_layer >= i
    #     spcl_dst_layer = i
    # else
    #     spcl_dst_layer = dst_layer
    # end

    # global src_layer
    # if src_layer < i
    #     spcl_src_layer = i
    # else
    #     spcl_src_layer = src_layer
    # end

    println("Collecting for time depth $i")
    G = GridNetwork(grid_size, grid_size)
    # Create a Temporal Graph from G with timedepth i
    T = QuNet.TemporalGraph(G, i, memory_costs = zero_costvector())
    # Get random pairs of asynchronus nodes
    user_pairs = make_user_pairs(T, num_pairs)
    # Get data
    p, p_e, pat, pat_e = net_performance(T, num_trials, num_pairs, max_paths=4,
    async_cost=async_cost, src_layer=src_layer, dst_layer=dst_layer)
    push!(perf_data, p)
    push!(perf_err, p_e)
    push!(path_data, pat)
    push!(path_err, pat_e)
end

# Collect data for horizontal lines:
# println("Collecting data for asymptote")
# as = asymptotic_costs(grid_size)
# e_as = ones(length(min_depth:max_depth)) * as[1]
# f_as = ones(length(min_depth:max_depth)) * as[2]

# Get values for x axis
# x = collect(1:(max_depth-min_depth))
x = collect(min_depth:max_depth-1)

# Extract from performance data
loss = collect(map(x->x["loss"], perf_data))
z = collect(map(x->x["Z"], perf_data))
loss_err = collect(map(x->x["loss"], perf_err))
z_err = collect(map(x->x["Z"], perf_err))

# Extract from path data
P0 = [path_data[i][1]/num_pairs for i in 1:(max_depth-min_depth)]
P1 = [path_data[i][2]/num_pairs for i in 1:(max_depth-min_depth)]
P2 = [path_data[i][3]/num_pairs for i in 1:(max_depth-min_depth)]
P3 = [path_data[i][4]/num_pairs for i in 1:(max_depth-min_depth)]
P4 = [path_data[i][5]/num_pairs for i in 1:(max_depth-min_depth)]

# P0e = [path_err[i][1]/num_pairs for i in 1:(max_depth-min_depth)]
# P1e = [path_err[i][2]/num_pairs for i in 1:(max_depth-min_depth)]
# P2e = [path_err[i][3]/num_pairs for i in 1:(max_depth-min_depth)]
# P3e = [path_err[i][4]/num_pairs for i in 1:(max_depth-min_depth)]
# P4e = [path_err[i][5]/num_pairs for i in 1:(max_depth-min_depth)]

println(P0)
println(P1)
println(P2)
println(P3)
println(P4)

# Plot
# after seriestype: marker = (5)
plot(x, loss, ylims=(0,1), xlims=(min_depth, max_depth), seriestype = :scatter, yerror = loss_err, label=L"$\eta$",
legend=:right, xguidefontsize=14, tickfontsize=12, legendfontsize=10, fontfamily="computer modern",
markersize=5, color=:DodgerBlue, markershape =:utriangle)
plot!(x, z, seriestype = :scatter, yerror = z_err, label=L"$F$", markersize=5, color=:Crimson)

# Plot asymptote
# plot!(x, e_as, linestyle=:dot, color=:blue, linewidth=2, label=L"$\textrm{Asymptotic } \eta$")
# plot!(x, f_as, linestyle=:dot, color=:red, linewidth=2, label=L"$\textrm{Asymptotic } F$")
xaxis!(L"$\textrm{Time Depth of Tempral Meta-Graph}$")

savefig("scrap/test_costtemporal.png")

plot(x, P0, ylims=(0,1), xlims=(min_depth, max_depth), seriestype=:scatter, yerr = P0e, label=L"$P_0$", legend= :right,
xguidefontsize=14, tickfontsize=12, legendfontsize=10, fontfamily="computer modern",
markersize=5, color_palette = palette(:plasma, 10))
plot!(x, P0, linewidth=1, label=false)
plot!(x, P1, seriestype=:scatter, yerr = P1e, label=L"$P_1$", markersize=5)
plot!(x, P1, linewidth=1, label=false)
plot!(x, P2, seriestype=:scatter, yerr = P2e, label=L"$P_2$", markersize=5)
plot!(x, P2, linewidth=1, label=false)
plot!(x, P3, seriestype=:scatter, yerr = P3e, label=L"$P_3$", markersize=5)
plot!(x, P3, linewidth=1, label=false)
plot!(x, P4, seriestype=:scatter, yerr = P4e, label=L"$P_4$", markersize=5)
plot!(x, P4, linewidth=1, label=false)
xaxis!(L"$\textrm{Time Depth of Temporal Meta-Graph}$")

savefig("scrap/test_pathtemporal.png")

max_stars_repo_name: FalafelGood/QuNet

max_issues_repo_name: FalafelGood/QuNet

max_forks_repo_name: FalafelGood/QuNet


text:
const AU = 1.495978700e11     # Astronomical Unit [m]
const G = 6.6740831e-11       # Gravitational constant [m^3/kg/s^2]
const GM☉ = 1.32712440018e20
const M☉ = GM☉ / G
const SOLAR_CONST = 1366.0    # Solar constant, Φ☉ [W/m^2]
const c₀ = 299792458.0        # speed of light [m/s]
const σ_SB = 5.670374419e-8   # Stefan–Boltzmann constant [W/m^2/K^4]

const GM_Mercury = 2.2032e13
const GM_Venus   = 3.24859e14
const GM_Earth   = 3.986004418e14
const GM_Moon    = 4.9048695e12
const GM_Mars    = 4.282837e13
const GM_Ceres   = 6.26325e10
const GM_Jupiter = 1.26686534e17
const GM_Saturn  = 3.7931187e16
const GM_Uranus  = 5.793939e15
const GM_Neptune = 6.836529e15
const GM_Pluto   = 8.71e11
const GM_Eris    = 1.108e12

const M_Mercury = GM_Mercury / G
const M_Venus   = GM_Venus   / G
const M_Earth   = GM_Earth   / G
const M_Moon    = GM_Moon    / G
const M_Mars    = GM_Mars    / G
const M_Ceres   = GM_Ceres   / G
const M_Jupiter = GM_Jupiter / G
const M_Saturn  = GM_Saturn  / G
const M_Uranus  = GM_Uranus  / G
const M_Neptune = GM_Neptune / G
const M_Pluto   = GM_Pluto   / G
const M_Eris    = GM_Eris    / G

max_stars_repo_name: MasanoriKanamaru/Astroshaper

max_issues_repo_name: MasanoriKanamaru/Astroshaper

max_forks_repo_name: MasanoriKanamaru/Astroshaper


text:
#this is the julia file that creates the AP test with the questions.
include("graphicAP.jl")
struct frq
	figs::Union{Vector{AbstractString},Nothing}
	Qtype::Vector{AbstractString}

end

function docHeader()
"""

	<title>Question #1</title>
	<meta name="viewport" content="width=device-width">

"""
end

function leadingText(q::frq)
	txt="""
	Question 1:
	\n
	Let \\(f\\) be a differentiable function. The graph is shown above."""
	# for qPart in q.Qtype
		# if qPart == graphic
			# txt = txt+ "Let "
		# end
	# end
	txt
end

function preText(q::frq)
	#inserting images/etc..
	pT = """

	"""
	for fig in q.figs
		pT = pT * """"""
	end
	pT
end

#to mildly create questions in html format
function createQuestion()
	q = frq(["question.png"],[""])
	file = touch("question.html")
	f = open(file,"w")
	println(f,docHeader())
	println(f,preText(q))
	println(f,leadingText(q))

	close(f)

end


text:
using Parquet
using Test
using Random

if VERSION < v"1.3"
    using Missings: nonmissingtype
end

Random.seed!(1234567)

function test_written(tbl, tmpfile)
    pf = Parquet.File(tmpfile)
    N = length(tbl.int32)

    # the file is very small so only one rowgroup
    col_chunks = columns(pf, 1)

    for (colnum, col_chunk) in enumerate(col_chunks)
        correct_vals = tbl[colnum]
        coltype = eltype(correct_vals)
        jtype = Parquet.elemtype(Parquet.elem(schema(pf), colname(pf,col_chunk)))
        ccpv = Parquet.ColumnChunkPageValues(pf, col_chunk, jtype)
        resultdata,nextpos = iterate(ccpv)

        if Missing <: coltype
            @test ismissing.(correct_vals) == (resultdata.defn_level.data .== 0)
        end

        non_missing_vals = collect(skipmissing(correct_vals))

        if nonmissingtype(coltype) == String
            @test all(non_missing_vals .== String.(resultdata.value.data))
        else
            @test all(non_missing_vals .== resultdata.value.data)
        end
    end

    # test with BatchedColumnsCursor
    cc = Parquet.BatchedColumnsCursor(pf)
    vals, _ = iterate(cc)
    @test length(vals) == 12
    @test length(vals.boolm) == N
    for idx in 1:N
        @test ismissing(vals.boolm[idx]) == ismissing(tbl.boolm[idx])
        if !ismissing(vals.boolm[idx])
            @test vals.boolm[idx] == tbl.boolm[idx]
        end
    end

    # clean up
    close(pf)
end

function make_table(N::Int=1000)
    tbl = (
        int32 = rand(Int32, N),
        int64 = rand(Int64, N),
        float32 = rand(Float32, N),
        float64 = rand(Float64, N),
        bool = rand(Bool, N),
        string = [randstring(8) for i in 1:N],
        int32m = rand([missing, rand(Int32, 10)...], N),
        int64m = rand([missing, rand(Int64, 10)...], N),
        float32m = rand([missing, rand(Float32, 10)...], N),
        float64m = rand([missing, rand(Float64, 10)...], N),
        boolm = rand([missing, true, false], N),
        stringm = rand([missing, "abc", "def", "ghi"], N)
    )
end

function test_write()
    tbl = make_table()
    tmpfile = tempname()*".parquet"
    write_parquet(tmpfile, tbl)
    test_written(tbl, tmpfile)
end

function test_write_via_buffer()
    tbl = make_table()
    tmpfile = tempname()*".parquet"

    # write to io buffer first
    io = IOBuffer()
    write_parquet(io, tbl)

    # then dump io buffer to disk
    open(tmpfile, "w") do f
        write(f, take!(io))
    end

    #and now test round-trip via file
    test_written(tbl, tmpfile)
end

@testset "writer" begin
    test_write()
    test_write_via_buffer()
end


text:
# # Introduction to Julio

using Julio
using Test

# (Note: Currently there's no macro-based syntactic sugar. This is for making
# sure all the components are composable during the design process.)

# ## Tasks and task groups
#
# Julio manages tasks in *task groups*. Spawning a task requires creating a task
# group first.

function test_simple_spawn()
    Julio.withtaskgroup() do tg
        task = Julio.spawn!(tg) do
            3 + 4
        end
        @test (1 + 2) + fetch(task) == 10
    end
end

# This style is useful when combined with the resource management using the
# `open(...) do` idiom (scope-based resource management).  See [black box
# rule](@id black-box) for more information.

# ## Channels

function test_channel()
    Julio.withtaskgroup() do tg
        #=
        A channel can be created using `Julio.channel`. It returns the
        *endpoints* for the send and receive sides.
        =#
        send_endpoint, receive_endpoint = Julio.channel()

        #=
        The `send_endpoint` supports `put!` function. It can be called from
        arbitrary tasks safely.
        =#
        Julio.spawn!(tg) do
            try
                for i in 1:10
                    put!(send_endpoint, i)
                end
            finally
                close(send_endpoint)  # signaling that there are no more items
            end
        end

        #=
        The `receive_endpoint` supports `take!`. It can be called from arbitrary
        tasks. It also supports the iteration protocol.  The following
        `collect(receive_endpoint)` continues until the child task calls
        `close(send_endpoint)`.
        =#
        try
            @test collect(receive_endpoint) == 1:10
        finally
            close(receive_endpoint)  # signaling the child task if something went wrong
        end
        #=
        ---
        =#
    end
end

# Use a task group to wait for multiple tasks before closing the endpoint:

function test_channel_open_many_scoped()
    send_endpoint, receive_endpoint = Julio.channel()
    local task
    Julio.withtaskgroup() do tg0
        task = Julio.spawn!(tg0) do
            try
                sort!(collect(receive_endpoint))
            finally
                close(receive_endpoint)
            end
        end
        try
            Julio.withtaskgroup() do tg1
                for i in 1:10
                    Julio.spawn!(tg1) do
                        put!(send_endpoint, i)
                    end
                end
            end
        finally
            close(send_endpoint)
        end
    end
    @test fetch(task) == 1:10
end

# Note that `Julio.channel` is always unbuffered. Use `Julio.queue` and
# `Julio.stack` for buffered channels.

function test_queue()
    send_endpoint, receive_endpoint = Julio.queue()
    put!(send_endpoint, 111)
    put!(send_endpoint, 222)
    @test take!(receive_endpoint) == 111
    @test take!(receive_endpoint) == 222
end

# ## Timeout
#
# Julio can introduce a timeout for arbitrary code blocks.  The timeout is
# triggered whenever the code is blocked by a Julia API.

function test_timeout()
    send_endpoint, receive_endpoint = Julio.channel()
    Julio.withtimeout(0.1) do
        put!(send_endpoint, nothing)  # never completes
    end
end

# ## Automatic cancellation
#
# Julio cancells the tasks within the same task group if one of them (including
# the parent task) throws an exception.

function produce!(input)
    i = 0
    while true
        i += 1
        put!(input, i)
    end
end

function test_cancellation()
    send_endpoint, receive_endpoint = Julio.channel()
    try
        task = nothing
        err = try
            Julio.withtaskgroup() do tg
                task = Julio.spawn!(tg) do
                    try
                        produce!(send_endpoint)
                    finally
                        close(send_endpoint)
                    end
                end
                for i in 1:3
                    @test take!(receive_endpoint) == i
                end
                error("cancel")
            end
            nothing
        catch err
            err
        end
        @test err isa Exception
        @test istaskdone(task)
    finally
        close(receive_endpoint)
    end
end

# ## [Manual cancellation](@id man-cancel-scope)
#
# ### Cancel scope
#
# Cancellation of Julio tasks can also be triggered manually.

function test_cancel_scope()
    _, receive_endpoint = Julio.channel()
    #=
    The parts of code that are cancelled together can be managed by
    `Julio.cancelscope`:
    =#
    scope = Julio.cancelscope()
    #=
    Let's see how it works with a nested task tree:
    =#
    Julio.withtaskgroup() do tg0
        Julio.spawn!(tg0) do
            #=
            The cancel scope can be installed at different places.  For example,
            it can be manually `open`ed.  The blocking calls inside the `do`
            block now checks the cancellation signal whenever Julio's blocking
            method is invoked.
            =#
            open(scope) do
                take!(receive_endpoint)  # blocks
            end
            #=
            ---
            =#
        end  # Julio.spawn!(tg0) do
        Julio.spawn!(tg0) do
            #=
            The cancel scope can be also be passed to `Julio.withtaskgroup` to
            associate the cancellation scope to the tasks managed by it.
            =#
            Julio.withtaskgroup(scope) do tg1
                Julio.spawn!(tg1) do
                    take!(receive_endpoint)  # blocks
                end
                take!(receive_endpoint)  # blocks
            end
            #=
            ---
            =#
        end  # Julio.spawn!(tg0) do
        #=
        Cancellation can be triggered by `Julio.cancel!`.
        =#
        Julio.cancel!(scope)
        #=
        Since the cancellation signal unblocks all the blocking calls
        `take!(receive_endpoint)`, this code reliably synchronizes all
        sub-tasks.
        =#
    end  # Julio.withtaskgroup() do tg0
end

# ### Interop with other cancellation mechanisms
#
# Julio can be used with other cancellation mechanisms. For example, several
# `Base` API supports cancellation by concurrent `close` on a "resource" object
# (e.g., `Base.Channel`, `Timer`, files). We can hook the `close` call into
# Julio's cancellation token by calling `Julio.onclose(close, resource)`.

function test_cancel_interop()
    result = @timed try
        Julio.withtaskgroup() do tg
            Julio.spawn!(tg) do
                timer = Timer(60)
                Julio.oncancel(close, timer)  # call `close(timer)` on cancellation
                wait(timer)  # this can be interrupted by `close(timer)`
            end
            error("cancelling")
        end
        false
    catch
        true
    end
    @test result.value  # terminated by the execption
    @test result.time < 30  # it didn't wait 60 seconds
end

# ## Event selection
#
# Julio supports executing one (and exactly one) of synchronizable *events*.
# Here, an event means a possibly blocking operations such as taking an item
# from a channel.

function test_select()
    Julio.withtaskgroup() do tg
        #=
        Suppose that we have two channels, but only one of them are available:
        =#
        send_endpoint1, receive_endpoint1 = Julio.channel()
        send_endpoint2, receive_endpoint2 = Julio.channel()
        Julio.spawn!(tg) do
            put!(send_endpoint1, 111)
        end

        #=
        We can select an available "event" (here, `take!`) using `Julio.select`
        function.
        =#
        selected = nothing
        Julio.select(
            (take!, receive_endpoint1) => item -> begin
                selected = item  # result of `take!(receive_endpoint1)`
            end,
            (take!, receive_endpoint2) => item -> begin
                selected = item  # result of `take!(receive_endpoint2)` (unreachable)
            end,
        )
        #=
        Since only `receive_endpoint1` has a task at the input endpoint,
        `take!(receive_endpoint1)` is chosen:
        =#
        @test selected == 111
        #=
        ---
        =#
    end
end

# Note that `Julio.select` works for various synchronizable events and not just
# channels.  See [select](@ref man-select) for more information.


text:
function getCoefficientsSymplecticEulerForward()
    a = [[0.0 0.0]
         [1.0 0.0]]
    b = [1.0, 0.0]
    c = [0.0, 1.0]

    o = 1

    CoefficientsRK(:symplectic_euler_forward, o, a, b, c)
end

function getCoefficientsSymplecticEulerBackward()
    a = [[0.0 0.0]
         [0.0 1.0]]
    b = [0.0, 1.0]
    c = [0.0, 1.0]

    o = 1

    CoefficientsRK(:symplectic_euler_backward, o, a, b, c)
end

"Tableau for symplectic Euler-A method"
function getTableauSymplecticEulerA()
    TableauEPRK(:symplectic_euler_a, 1, getCoefficientsSymplecticEulerForward(), getCoefficientsSymplecticEulerBackward())
end

"Tableau for symplectic Euler-B method"
function getTableauSymplecticEulerB()
    TableauEPRK(:symplectic_euler_b, 1, getCoefficientsSymplecticEulerBackward(), getCoefficientsSymplecticEulerForward())
end

"Tableau for Gauss-Lobatto IIIAIIIB method with s=2 stages"
function getTableauLobattoIIIAIIIB2()
    TableauEPRK(:lobatto_IIIA_IIIB_2, 2, getCoefficientsLobIIIA2(), getCoefficientsLobIIIB2())
end

"Tableau for Gauss-Lobatto IIIBIIIA method with s=2 stages"
function getTableauLobattoIIIBIIIA2()
    TableauEPRK(:lobatto_IIIB_IIIA_2, 2, getCoefficientsLobIIIB2(), getCoefficientsLobIIIA2())
end


text:
## Testing for type stability
## This code is type stable
## Check: julia> @code_warntype test()

function f(g::Function, x::Int64)
     return g(x)
end

g1(x)::Function = x^2
g2(x)::Function = (1 - x)^2
const garr = (g1, g2)

function test()
    for k = 1 : 2
           f(garr[k], 20)
    end
end

max_stars_repo_name: sofiabelen/Ideal-Gas-Compressible-Fluid

max_issues_repo_name: sofiabelen/Ideal-Gas-Compressible-Fluid

max_forks_repo_name: sofiabelen/Two-Phase-Filtration


text:
import GeometryTypes.Point
export Point
export HRepElement, HalfSpace, HyperPlane
export VRepElement, AbstractPoint, AbstractRay, Ray, Line
#export SymPoint
export islin, isray, ispoint, coord, lift, simplify

const MyVec{N,T} = Union{Vec{N,T},AbstractVector{T}}
_vec(::Type{T}, a::AbstractVector) where {T} = AbstractArray{T}(a)
_vec(::Type{T}, a::AbstractVector{T}) where {T} = a
function _vec(::Type{T}, a::StaticArrays.SVector{N}) where {N,T}
    StaticArrays.SVector{N,T}(a)
end
_vec(::Type{T}, a::StaticArrays.SVector{N,T}) where {N,T} = a
_vec(::Type{T}, a::Vec{N}) where {N,T} = Vec{N,T}(a)
_vec(::Type{T}, a::Vec{N,T}) where {N,T} = a

abstract type HRepElement{N,T} end

"""
    struct HalfSpace{N, T, AT} <: HRepElement{N, T}
        a::AT
        β::T
    end

An halfspace defined by the set of points ``x`` such that ``\\langle a, x \\rangle \\le \\beta``.
"""
struct HalfSpace{N, T, AT<:MyVec{N, T}} <: HRepElement{N,T}
    a::AT
    β::T
    function HalfSpace{N, T, AT}(a::AT, β::T) where {N, T, AT<:MyVec{N, T}}
        new{N, T, AT}(a, β)
    end
end

HalfSpace{N, T}(a::AT, β::T) where {N, T, AT <: MyVec{N, T}} = HalfSpace{N, T, AT}(a, β)
HalfSpace{N, T}(a::MyVec, β) where {N, T} = HalfSpace{N, T}(_vec(T, a), T(β))

"""
    struct HyperPlane{N, T, AT} <: HRepElement{N, T}
        a::AT
        β::T
    end

An hyperplane defined by the set of points ``x`` such that ``\\langle a, x \\rangle = \\beta``.
"""
struct HyperPlane{N, T, AT<:MyVec{N, T}} <: HRepElement{N,T}
    a::AT
    β::T
    function HyperPlane{N, T, AT}(a::AT, β::T) where {N, T, AT<:MyVec{N, T}}
        new{N, T, AT}(a, β)
    end
end

HyperPlane{N, T}(a::AT, β::T) where {N, T, AT <: MyVec{N, T}} = HyperPlane{N, T, AT}(a, β)
HyperPlane{N, T}(a::MyVec, β) where {N, T} = HyperPlane{N, T}(_vec(T, a), T(β))

_HalfSpace(a::MyVec{N, T}, β::T, d::FullDim{N}) where {N, T} = HalfSpace{N, T}(a, β)
_HalfSpace(a::MyVec{N, S}, β::T, d::FullDim{N}) where {N, S, T} = HalfSpace{N, promote_type(S, T)}(a, β)
HalfSpace(a, β) = _HalfSpace(a, β, FullDim(a))
_HyperPlane(a::MyVec{N, T}, β::T, d::FullDim{N}) where {N, T} = HyperPlane{N, T}(a, β)
_HyperPlane(a::MyVec{N, S}, β::T, d::FullDim{N}) where {N, S, T} = HyperPlane{N, promote_type(S, T)}(a, β)
HyperPlane(a, β) = _HyperPlane(a, β, FullDim(a))

Base.convert(::Type{HalfSpace{N, T, AT}}, h::HalfSpace{N}) where {N, T, AT} = HalfSpace{N, T, AT}(AT(h.a), T(h.β))
Base.convert(::Type{HyperPlane{N, T, AT}}, h::HyperPlane{N}) where {N, T, AT} = HyperPlane{N, T, AT}(AT(h.a), T(h.β))
Base.convert(::Type{HalfSpace{N, T, AT}}, h::HalfSpace{N, T, AT}) where {N, T, AT} = h
Base.convert(::Type{HyperPlane{N, T, AT}}, h::HyperPlane{N, T, AT}) where {N, T, AT} = h

islin(::Union{HalfSpace, Type{<:HalfSpace}}) = false
islin(::Union{HyperPlane, Type{<:HyperPlane}}) = true

Base.:-(h1::HRepElement, h2::HRepElement) = HalfSpace(h1.a - h2.a, h1.β - h2.β)
Base.:-(h1::HyperPlane, h2::HyperPlane) = HyperPlane(h1.a - h2.a, h1.β - h2.β)

Base.:(*)(h::HyperPlane, α::Real) = HyperPlane(h.a * α, h.β * α)
Base.:(*)(α::Real, h::HyperPlane) = HyperPlane(α * h.a, α * h.β)
Base.:(*)(h::HalfSpace, α::Real) = HalfSpace(h.a * α, h.β * α)
Base.:(*)(α::Real, h::HalfSpace) = HalfSpace(α * h.a, α * h.β)

function Base.:(/)(h::ElemT, P::Matrix) where {N, T, ElemT<:HRepElement{N, T}}
    Tout = _promote_type(T, eltype(P))
    ElemTout = similar_type(ElemT, FullDim{size(P, 2)}(), Tout)
    ElemTout(Matrix{Tout}(P) * _vec(Tout, h.a), Tout(h.β))
end
function zeropad(h::ElemT, n::Integer) where {N, T, ElemT<:HRepElement{N, T}}
    if n == 0
        h
    else
        ElemTout = similar_type(ElemT, FullDim{N+abs(n)}())
        if n < 0
            aout = [zeros(T, -n); h.a]
        else
            aout = [h.a; zeros(T, n)]
        end
        ElemTout(aout, h.β)
    end
end

# Point: -> A same Rep should always return the same of the two types so that when points and sympoints will have different accessors it will be type stable
# Point{N, T} or AbstractVector{T}
# Linear Point:
# SymPoint{N,T}
# Ray:
# Ray{N, T}
# Linear Ray:
# Line{N, T}

"""
    const AbstractPoint{N, T} = Union{Point{N, T}, AbstractVector{T}}

A point in dimension `N` and of coefficient type `T`.
"""
const AbstractPoint{N, T} = Union{Point{N, T}, AbstractVector{T}}

origin(::Type{<:SparseVector{T}}, ::FullDim{N}) where {N, T} = spzeros(T, N)
origin(::Type{Vector{T}}, ::FullDim{N}) where {N, T} = zeros(T, N)
origin(VT::Type{<:AbstractVector}, ::FullDim) = zeros(VT)
# Canonical basis vector
function basis(::Type{Vector{T}}, ::FullDim{N}, i::Int) where {N, T}
    v = zeros(T, N)
    v[i] = one(T)
    v
end
function basis(::Type{SVector{N, T}}, ::FullDim{N}, i::Int) where {N, T}
    SVector{N, T}(ntuple(j -> j == i ? one(T) : zero(T), Val{N}))
end

#"""
#    struct SymPoint{N, T, AT <: AbstractPoint{N, T}}
#        a::AT
#    end
#
#The convex hull of `a` and `-a`.
#"""
#struct SymPoint{N, T, AT <: AbstractPoint{N, T}}
#    a::AT
#    function SymPoint{N, T, AT}(a::AT) where {N, T, AT<:AbstractPoint{N, T}}
#        new{N, T, AT}(a)
#    end
#end
#SymPoint{N, T, AT}(sympoint::SymPoint) where {N, T, AT} = SymPoint{N, T, AT}(AT(sympoint.a))
#SymPoint{N, T}(a::AbstractPoint{N, T}) where {N, T} = SymPoint{N, T, typeof(a)}(a)
#SymPoint(a::AbstractPoint) = SymPoint{fulldim(a), eltype(a)}(a)

#const AnyPoint{N, T} = Union{SymPoint{N, T}, AbstractPoint{N, T}}
const AnyPoint{N, T} = AbstractPoint{N, T}

"""
    struct Ray{N, T, AT <: MyVec{N, T}}
        a::AT
    end

The conic hull of `a`, i.e. the set of points `λa` where `λ` is any nonnegative real number.
"""
struct Ray{N, T, AT <: MyVec{N, T}}
    a::AT
    function Ray{N, T, AT}(a::AT) where {N, T, AT<:MyVec{N, T}}
        new{N, T, AT}(a)
    end
end
Ray{N, T, AT}(ray::Ray) where {N, T, AT} = Ray{N, T, AT}(AT(ray.a))
Base.convert(::Type{Ray{N, T, AT}}, ray::Ray) where {N, T, AT} = Ray{N, T, AT}(ray)
Ray{N, T}(a::AT) where {N, T, AT<:MyVec{N, T}} = Ray{N, T, AT}(a)
Ray(a::MyVec) = Ray{fulldim(a), eltype(a)}(a)

"""
    struct Line{N, T, AT <: MyVec{N, T}}
        a::AT
    end

The conic hull of `a` and `-a`, i.e. the set of points `λa` where `λ` is any real number.
"""
struct Line{N, T, AT<:MyVec{N, T}}
    a::AT
    function Line{N, T, AT}(a::AT) where {N, T, AT<:MyVec{N, T}}
        new{N, T, AT}(a)
    end
end
Line{N, T, AT}(line::Line) where {N, T, AT} = Line{N, T, AT}(AT(line.a))
Base.convert(::Type{Line{N, T, AT}}, line::Line) where {N, T, AT} = Line{N, T, AT}(line)
Line{N, T}(a::AT) where {N, T, AT<:MyVec{N, T}} = Line{N, T, AT}(a)
Line(a::MyVec) = Line{fulldim(a), eltype(a)}(a)

#const VStruct{N, T} = Union{SymPoint{N, T}, Line{N, T}, Ray{N, T}}
const VStruct{N, T} = Union{Line{N, T}, Ray{N, T}}

Base.:(==)(a::T, b::T) where T<:VStruct = coord(a) == coord(b)
Base.getindex(x::VStruct, i) = x.a[i]
Base.vec(x::VStruct) = vec(x.a)

Base.:-(h::ElemT) where {ElemT<:Union{HyperPlane, HalfSpace}} = ElemT(-h.a, -h.β)
Base.:-(elem::ElemT) where ElemT<:VStruct = ElemT(-coord(elem))
# Used in remproj
Base.:-(p::AbstractVector, l::Line) = p - coord(l)
# Ray - Line is done in remproj
Base.:-(r::Ray, s::Union{Ray, Line}) = Ray(r.a - s.a)
Base.:+(r::Ray, s::Ray) = Ray(r.a + s.a)
Base.:+(p::AbstractPoint, r::Ray) = p + coord(r)

for op in [:dot, :cross]
    @eval begin
        Compat.LinearAlgebra.$op(x::VStruct, y) = $op(x.a, y)
        Compat.LinearAlgebra.$op(x, y::VStruct) = $op(x, y.a)
        Compat.LinearAlgebra.$op(x::VStruct, y::VStruct) = $op(x.a, y.a)
    end
end

for ElT in (:HyperPlane, :HalfSpace, :Line, :Ray)
    @eval begin
        Base.promote_rule(::Type{$ElT{N, T, AT}}, ::Type{$ElT{N, T, AT}}) where {N, T, AT} = $ElT{N, T, AT}
        # Allowing mixing e.g. sparse vector with vector would not be helpful as code meant
        # to use sparse polyhedra would lose sparsity silently. Same thing for SVector
        Base.promote_rule(::Type{$ElT{N, T, VT}}, ::Type{$ElT{N, T, WT}}) where {N, T, VT, WT} = error("Cannot mix Polyhedra elements of vector type $VT and $WT")
        function Base.promote_rule(::Type{$ElT{N, S, AS}}, ::Type{$ElT{N, T, AT}}) where {N, S, T, AS, AT}
            U = promote_type(S, T)
            promote_type($ElT{N, U, similar_type(AS, U)}, $ElT{N, U, similar_type(AT, U)})
        end
    end
end

Base.:(*)(α, r::T) where T<:VStruct = T(α * r.a)
Base.:(*)(r::T, α) where T<:VStruct = T(r.a * α)
Base.:(/)(r::T, α) where T<:VStruct = T(r.a / α)

const AbstractRay{N, T} = Union{Ray{N, T}, Line{N, T}}
const FixedVRepElement{N,T} = Union{Point{N, T}, VStruct{N, T}}
const VRepElement{N,T} = Union{FixedVRepElement{N,T}, AbstractVector{T}}
const RepElement{N,T} = Union{HRepElement{N,T}, VRepElement{N,T}}
const FixedRepElement{N,T} = Union{HRepElement{N,T}, FixedVRepElement{N,T}}
const Element{N, T} = Union{HRepElement{N, T}, VRepElement{N, T}}

FullDim(::Union{FixedRepElement{N}, Type{<:FixedRepElement{N}}}) where {N} = FullDim{N}()
MultivariatePolynomials.coefficienttype(::Union{FixedRepElement{N, T}, Type{<:FixedRepElement{N, T}}}) where {N, T} = T

#islin(::Union{SymPoint, Line, Type{<:Union{SymPoint, Line}}}) = true
islin(::Union{Line, Type{<:Line}}) = true
islin(::Union{AbstractPoint, Ray, Type{<:Union{AbstractPoint, Ray}}}) = false
ispoint(::Union{AnyPoint, Type{<:AnyPoint}}) = true
ispoint(::Union{Line, Ray, Type{<:Union{Line, Ray}}}) = false
isray(v) = !ispoint(v)

coord(v::ElemT) where {ElemT<:Union{Point,AbstractVector}} = v
coord(v::ElemT) where {ElemT<:Union{HRepElement,VStruct}} = v.a

function Base.:*(P::AbstractMatrix, v::ElemT) where {N, T, ElemT<:VStruct{N, T}}
      Tout = _promote_type(T, eltype(P))
      ElemTout = similar_type(ElemT, FullDim{size(P, 1)}(), Tout)
      return ElemTout(P * v.a)
end
# FIXME there seem to be a Julia bug, with Vector, it does not recognize that the zeropad method
#       works
function _zeropad(v, n::Integer)
    if n == 0
        v
    else
        T = MultivariatePolynomials.coefficienttype(v)
        ElemTout = similar_type(typeof(v), FullDim(v) + FullDim{abs(n)}())
        if n < 0
            aout = [zeros(T, -n); coord(v)]
        else
            aout = [coord(v); zeros(T, n)]
        end
        ElemTout(aout)
    end
end
zeropad(v::ElemT, n::Integer) where {N, T, ElemT <: VRepElement{N, T}} = _zeropad(v, n)
zeropad(v::AbstractVector, n::Integer) = _zeropad(v, n)

for ElemT in [:HalfSpace, :HyperPlane, :Ray, :Line] # , :SymPoint
    @eval begin
        similar_type(::Type{$ElemT{N,T,AT}}, dout::FullDim{Nout}, ::Type{Tout}) where {N,T,AT,Nout,Tout} = $ElemT{Nout,Tout,similar_type(AT, dout, Tout)}
    end
end

ininterior(r::Ray{N}, h::HalfSpace{N}) where {N} = _neg(h.a ⋅ r)
ininterior(l::Line{N}, h::HalfSpace{N}) where {N} = _neg(h.a ⋅ l)
ininterior(p::Point{N}, h::HalfSpace{N}) where {N} = _lt(h.a ⋅ p, h.β)
ininterior(p::AbstractVector, h::HalfSpace{N}) where {N} = _lt(h.a ⋅ p, h.β)
#ininterior{N}(p::SymPoint{N}, h::HalfSpace{N}) = _lt(h.a ⋅ p.p, h.β)

inrelativeinterior(p::VRepElement, h::HalfSpace) = ininterior(p, h)

Base.in(r::Ray{N}, h::HalfSpace{N}) where N = _nonpos(h.a ⋅ r)
Base.in(l::Line{N}, h::HalfSpace{N}) where N = _nonpos(h.a ⋅ l)
Base.in(p::Point{N}, h::HalfSpace{N}) where N = _leq(h.a ⋅ p, h.β)
Base.in(p::AbstractVector, h::HalfSpace{N}) where N = _leq(h.a ⋅ p, h.β)
#function Base.in(p::SymPoint{N}, h::HalfSpace{N}) where N
#    ap = h.a ⋅ p.p
#    _leq(ap, h.β) && _leq(-ap, h.β)
#end

ininterior(p::VRepElement, h::HyperPlane) = false
inrelativeinterior(p::VRepElement, h::HyperPlane) = p in h

Base.in(r::Ray{N}, h::HyperPlane{N}) where {N} = isapproxzero(h.a ⋅ r)
Base.in(l::Line{N}, h::HyperPlane{N}) where {N} = isapproxzero(h.a ⋅ l)
Base.in(p::Point{N}, h::HyperPlane{N}) where {N} = _isapprox(h.a ⋅ p, h.β)
Base.in(p::AbstractVector, h::HyperPlane{N}) where {N} = _isapprox(h.a ⋅ p, h.β)
#Base.in(p::SymPoint{N}, h::HyperPlane{N}) where {N} = isapproxzero(h.β) && isapproxzero(h.a ⋅ p.a)

function Base.vec(x::FixedVector{N,T}) where {N,T}
    y = Vector{T}(N)
    for i in 1:N
        y[i] = x[i]
    end
    y
end
#Base.vec{ElemT::VStruct}(x::ElemT) = ElemT(vec(x.a))

function pushbefore(a::AbstractSparseVector{T}, β::T) where T
    b = spzeros(T, length(a)+1)
    b[1] = β
    b[2:end] = a
    b
end
pushbefore(a::AbstractVector, β) = [β; a]
function pushbefore(a::ElemT, β, ElemTout = similar_type(ElemT, FullDim{N+1}())) where {N, ElemT<:FixedVector{N}}
    ElemTout([β; vec(a)])
end

function lift(h::HRepElement{N, T}) where {N, T}
    similar_type(typeof(h), FullDim{N+1}(), T)(pushbefore(h.a, -h.β), zero(T))
end
lift(h::Ray{N,T}) where {N,T} = Ray{N+1,T}(pushbefore(h.a, zero(T)))
lift(h::Line{N,T}) where {N,T} = Line{N+1,T}(pushbefore(h.a, zero(T)))
lift(h::Point{N,T}) where {N,T} = pushbefore(h, one(T))
lift(h::AbstractVector{T}) where {T} = pushbefore(h, one(T))
#lift(h::SymPoint{N,T}) where {N,T} = SymPoint{N+1,T}(pushbefore(h.a, one(T)))

#translate(p::Union{Point,AbstractVector,SymPoint}, v) = p + v
translate(p::Union{Point,AbstractVector}, v) = p + v
translate(r::Union{Ray,Line}, v) = r

translate(h::ElemT, p) where {ElemT<:HRepElement} = ElemT(h.a, h.β + h.a ⋅ p)

_simplify(a::AbstractVector) = a
_simplify(a::AbstractVector, β) = a, β

iszo(g) = iszero(g) || g == 1 # TODO isone in v0.7

function _simplify(a::AbstractVector{<:Integer})
    g = gcd(a)
    if !iszo(g)
        div.(a, g)
    else
        a
    end
end
function _simplify(a::AbstractVector{<:Integer}, β::Integer)
    g = gcd(gcd(a), β)
    if !iszo(g)
        div.(a, g), div(β, g)
    else
        a, β
    end
end
function _simplify(a::AbstractVector{<:Rational})
    g = gcd(numerator.(a))
    if !iszo(g)
        a = a ./ g
    end
    g = gcd(denominator.(a))
    if !iszo(g)
        a .* g
    else
        a
    end
end
function _simplify(a::AbstractVector{<:Rational}, β::Rational)
    g = gcd(gcd(numerator.(a)), numerator(β))
    if !iszo(g)
        a = a ./ g
        β = β / g
    end
    g = gcd(gcd(denominator.(a)), denominator(β))
    if !iszo(g)
        a .* g, β * g
    else
        a, β
    end
end

simplify(h::HalfSpace{N, T}) where {N, T} = HalfSpace{N, T}(_simplify(h.a, h.β)...)
simplify(h::HyperPlane{N, T}) where {N, T} = HyperPlane{N, T}(_simplify(h.a, h.β)...)
simplify(r::T) where {T<:Union{Ray,Line}} = T(_simplify(coord(r)))
# Cannot scale points
simplify(p::AnyPoint) = p


text:
using ProjectAssigner
using Documenter

DocMeta.setdocmeta!(ProjectAssigner, :DocTestSetup, :(using ProjectAssigner); recursive=true)

makedocs(;
    modules=[ProjectAssigner],
    authors="Zachary Sunberg <zachary.sunberg@colorado.edu> and contributors",
    repo="https://github.com/zsunberg/ProjectAssigner.jl/blob/{commit}{path}#{line}",
    sitename="ProjectAssigner.jl",
    format=Documenter.HTML(;
        prettyurls=get(ENV, "CI", "false") == "true",
        canonical="https://zsunberg.github.io/ProjectAssigner.jl",
        assets=String[],
    ),
    pages=[
        "Home" => "index.md",
        "Usage" => "usage.md",
        "Data Format" => "data.md"
    ],
)

deploydocs(;
    repo="github.com/zsunberg/ProjectAssigner.jl",
    devbranch="main",
)


text:
__precompile__()

module Utils

export gaussian_pdf, get_nn_pairs

function gaussian_pdf(n, μ, σ)
    x = collect(1:1<<n)
    pl = @. 1 / sqrt(2pi * σ^2) * exp(-(x - μ)^2 / (2 * σ^2))
    pl / sum(pl)
end

function get_nn_pairs(n)
    pairs = []
    for inth in 1:2
        for i in inth:2:n
            push!(pairs, i=>i % n + 1)
        end
    end
    pairs
end

end

max_stars_repo_name: Roger-luo/QCBM

max_issues_repo_name: Roger-luo/QCBM

max_forks_repo_name: Roger-luo/QCBM