text:
# GCM Initial Base State
# This file contains helpers and lists currely avaiable options

abstract type AbstractBaseState end
struct ZeroBaseState <: AbstractBaseState end
struct BCWaveBaseState <: AbstractBaseState end
struct HeldSuarezBaseState <: AbstractBaseState end

# Helper for parsing `--init-base-state`` command line argument
function parse_base_state_arg(arg)
    if arg === nothing
        base_state = nothing
    elseif arg == "bc_wave"
        base_state = BCWaveBaseState()
    elseif arg == "heldsuarez"
        base_state = HeldSuarezBaseState()
    elseif arg == "zero"
        base_state = ZeroBaseState()
    else
        error("unknown base state: " * arg)
    end

    return base_state
end

# Initial base state from rest, independent of the model reference state
function init_base_state(::ZeroBaseState, bl, state, aux, coords, t)
    FT = eltype(state)
    _R_d = R_d(bl.param_set)::FT
    _grav = grav(bl.param_set)::FT
    _a = planet_radius(bl.param_set)::FT
    _p_0::FT = MSLP(bl.param_set)
    T_initial = FT(255)
    r = norm(coords, 2)
    h = r - _a

    scale_height = _R_d * FT(T_initial) / _grav
    p = FT(_p_0) * exp(-h / scale_height)
    u_ref, v_ref, w_ref = (FT(0), FT(0), FT(0))
    return T_initial, p, u_ref, v_ref, w_ref
end

# Initial base state from rest, consistent with the model reference state
function init_base_state(::HeldSuarezBaseState, bl, state, aux, coords, t)
    FT = eltype(state)

    # T_v is dry ref state
    T_v = aux.ref_state.T
    p = aux.ref_state.p
    u_ref, v_ref, w_ref = (FT(0), FT(0), FT(0))

    return T_v, p, u_ref, v_ref, w_ref
end

# Initial base state following
# Ullrich et al. (2016) Dynamical Core Model Intercomparison Project (DCMIP2016) Test Case Document
function init_base_state(::BCWaveBaseState, bl, state, aux, coords, t)
    FT = eltype(state)

    # general parameters
    _grav = grav(bl.param_set)::FT
    _R_d = R_d(bl.param_set)::FT
    _Ω = Omega(bl.param_set)::FT
    _a = planet_radius(bl.param_set)::FT
    _p_0 = MSLP(bl.param_set)::FT

    # grid
    φ = latitude(bl, aux)
    z = altitude(bl, aux)
    γ::FT = 1 # set to 0 for shallow-atmosphere case and to 1 for deep atmosphere case

    # base state parameters
    k::FT = 3
    T_E::FT = 310
    T_P::FT = 240
    T_0::FT = 0.5 * (T_E + T_P)
    Γ::FT = 0.005
    A::FT = 1 / Γ
    B::FT = (T_0 - T_P) / T_0 / T_P
    C::FT = 0.5 * (k + 2) * (T_E - T_P) / T_E / T_P
    b::FT = 2
    H::FT = _R_d * T_0 / _grav
    z_t::FT = 15e3
    λ_c::FT = π / 9
    φ_c::FT = 2 * π / 9
    d_0::FT = _a / 6
    V_p::FT = 1

    # convenience functions for temperature and pressure
    τ_z_1::FT = exp(Γ * z / T_0)
    τ_z_2::FT = 1 - 2 * (z / b / H)^2
    τ_z_3::FT = exp(-(z / b / H)^2)
    τ_1::FT = 1 / T_0 * τ_z_1 + B * τ_z_2 * τ_z_3
    τ_2::FT = C * τ_z_2 * τ_z_3
    τ_int_1::FT = A * (τ_z_1 - 1) + B * z * τ_z_3
    τ_int_2::FT = C * z * τ_z_3
    I_T::FT =
        (cos(φ) * (1 + γ * z / _a))^k -
        k / (k + 2) * (cos(φ) * (1 + γ * z / _a))^(k + 2)

    # base state virtual temperature and pressure
    T_v::FT = (τ_1 - τ_2 * I_T)^(-1)
    p::FT = _p_0 * exp(-_grav / _R_d * (τ_int_1 - τ_int_2 * I_T))

    # base state velocity
    U::FT =
        _grav * k / _a *
        τ_int_2 *
        T_v *
        (
            (cos(φ) * (1 + γ * z / _a))^(k - 1) -
            (cos(φ) * (1 + γ * z / _a))^(k + 1)
        )
    u_ref::FT =
        -_Ω * (_a + γ * z) * cos(φ) +
        sqrt((_Ω * (_a + γ * z) * cos(φ))^2 + (_a + γ * z) * cos(φ) * U)
    v_ref::FT = 0
    w_ref::FT = 0

    return T_v, p, u_ref, v_ref, w_ref
end

max_stars_repo_name: mwarusz/CLIMA

max_issues_repo_name: mwarusz/CLIMA

max_forks_repo_name: mwarusz/CLIMA


text:
"""
    function preproc_imagenet(img)

Image preprocessing for pre-trained ImageNet examples.
Preprocessing includes
+ bring RGB colour values into a range 0-255
+ standardise of colour values by substracting mean colour values
    (103.939, 116.779, 123.68) from RGB
+ changing colour channel sequence from RGB to BGR

Resize is **not** done, because this may be part of the
augmentation pipeline.

### Examples:
The function can be used with the image loader; for prediction
with a trained model as:
```julia
pipl = CropRatio(ratio=1.0) |> Resize(224,224)
images = mk_image_minibatch("./example_pics", 16;
                    shuffle=false, train=false,
                    aug_pipl=pipl,
                    pre_proc=preproc_imagenet)
```

And for training something like:
```julia
pipl = Either(1=>FlipX(), 1=>FlipY(), 2=>NoOp()) |>
       Rotate(-5:5) |>
       ShearX(-5:5) * ShearY(-5:5) |>
       RCropSize(224,224)

dtrn, dvld = mk_image_minibatch("./example_pics", 16;
                    split=true, fr=0.2, balanced=false,
                    shuffle=true, train=true,
                    aug_pipl=pipl,
                    pre_proc=preproc_imagenet)
```
"""
function preproc_imagenet(img)

    (r, g, b) = (103.939, 116.779, 123.68)
    img = img .* 255.0

    y = zeros(Float32, size(img))
    y[:,:,1] .= img[:,:,3] .- r
    y[:,:,2] .= img[:,:,2] .- g
    y[:,:,3] .= img[:,:,1] .- b
    return y
end

"""
    function get_imagenet_classes()

Return a list of all 1000 ImageNet class labels.
"""
function get_imagenet_classes()

    IMAGENET_CLASSES = joinpath(@__DIR__, "..",
                             "data", "imagenet", "classes.txt")

    if isfile(IMAGENET_CLASSES)
        classes = readlines(IMAGENET_CLASSES)
    else
        println("File with ImageNet class labels not found at")
        println("$IMAGENET_CLASSES.")

        classes = repeat(["?"], 1000)
    end
    return classes
end

"""
    function predict_imagenet(mdl, x; top_n=5)

Predict the ImageNet-class of images from the
predefined list of class labels.
"""
function predict_imagenet(mdl, x; top_n=5)

    classes = get_imagenet_classes()
    return predict_top5(mdl, x; top_n=top_n, classes=classes)
end

ext: jl


text:
using KernelAbstractions.Extras.LoopInfo: @unroll

#####
##### Periodic boundary conditions
#####

@kernel function fill_periodic_west_and_east_halo!(c, H::Int, N)
  j, k = @index(Global, NTuple)
  @unroll for i = 1:H
      @inbounds begin
          c[i, j, k] = c[N+i, j, k]     # west
          c[N+H+i, j, k] = c[H+i, j, k] # east
      end
  end
end

@kernel function fill_periodic_south_and_north_halo!(c, H::Int, N)
  i, k = @index(Global, NTuple)
  @unroll for j = 1:H
      @inbounds begin
          c[i, j, k] = c[i, N+j, k]     # south
          c[i, N+H+j, k] = c[i, H+j, k] # north
      end
  end
end

@kernel function fill_periodic_bottom_and_top_halo!(c, H::Int, N)
  i, j = @index(Global, NTuple)
  @unroll for k = 1:H
      @inbounds begin
          c[i, j, k] = c[i, j, N+k]        # top
          c[i, j, N+H+k] = c[i, j, H+k] # bottom
      end
  end
end

function fill_west_and_east_halo!(c, ::PBC, ::PBC, arch, dep, grid, args...; kw...)
  c_parent = parent(c)
  yz_size = size(c_parent)[[2, 3]]
  event = launch!(arch, grid, yz_size, fill_periodic_west_and_east_halo!, c_parent, grid.Hx, grid.Nx; dependencies=dep, kw...)
  return validate_event(event)
end

function fill_south_and_north_halo!(c, ::PBC, ::PBC, arch, dep, grid, args...; kw...)
  c_parent = parent(c)
  xz_size = size(c_parent)[[1, 3]]
  event = launch!(arch, grid, xz_size, fill_periodic_south_and_north_halo!, c_parent, grid.Hy, grid.Ny; dependencies=dep, kw...)
  return validate_event(event)
end

function fill_bottom_and_top_halo!(c, ::PBC, ::PBC, arch, dep, grid, args...; kw...)
  c_parent = parent(c)
  xy_size = size(c_parent)[[1, 2]]
  event = launch!(arch, grid, xy_size, fill_periodic_bottom_and_top_halo!, c_parent, grid.Hz, grid.Nz; dependencies=dep, kw...)
  return validate_event(event)
end

####
#### Implement single periodic directions (for Distributed memory architectures)
####

  fill_west_halo!(c, ::PBC, H::Int, N) = @views @. c.parent[1:H, :, :] = c.parent[N+1:N+H, :, :]
 fill_south_halo!(c, ::PBC, H::Int, N) = @views @. c.parent[:, 1:H, :] = c.parent[:, N+1:N+H, :]
fill_bottom_halo!(c, ::PBC, H::Int, N) = @views @. c.parent[:, :, 1:H] = c.parent[:, :, N+1:N+H]

 fill_east_halo!(c, ::PBC, H::Int, N) = @views @. c.parent[N+H+1:N+2H, :, :] = c.parent[1+H:2H, :, :]
fill_north_halo!(c, ::PBC, H::Int, N) = @views @. c.parent[:, N+H+1:N+2H, :] = c.parent[:, 1+H:2H, :]
  fill_top_halo!(c, ::PBC, H::Int, N) = @views @. c.parent[:, :, N+H+1:N+2H] = c.parent[:, :, 1+H:2H]

# Generate interface functions for periodic boundary conditions
sides = (:west, :east, :south, :north, :top, :bottom)
coords = (:x, :x, :y, :y, :z, :z)

for (x, side) in zip(coords, sides)
   name = Symbol(:fill_, side, :_halo!)
   H = Symbol(:H, x)
   N = Symbol(:N, x)
   @eval $name(c, bc::PBC, arch, dep, grid, args...; kw...) = $name(c, bc, grid.$(H), grid.$(N))
end


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

using Markdown
using InteractiveUtils

# This Pluto notebook uses @bind for interactivity. When running this notebook outside of Pluto, the following 'mock version' of @bind gives bound variables a default value (instead of an error).
macro bind(def, element)
    quote
        local el = $(esc(element))
        global $(esc(def)) = Core.applicable(Base.get, el) ? Base.get(el) : missing
        el
    end
end

# ╔═╡ c5af5d4a-455b-11eb-0b57-4d8d63615b85
begin
	using SimilaritySearch  # this is the only package needed
	using MLDatasets # this dataset allows access the dataset
	using Colors # allows to show images in the Pluto notebook
	using PlutoUI
end

# ╔═╡ 5cd87a9e-5506-11eb-2744-6f02144677ff
md"""
# Using SimilaritySearch with synthetic images

This example shows how to search on synthetic images

## Loading the required packages for the examples
"""

# ╔═╡ d8d27dbc-5507-11eb-20e9-0f16ddba080b
md"""
### Creating the dataset

We will create a $n$ random matrices (a.k.a. synthetic image)

"""

# ╔═╡ c6ca5d24-616c-11eb-1031-4b943ecc633c
function create_object(dim)
	s = randn(Float32, dim, dim)
	s .= max.(s, 0.0)
	s .= 1 .- s ./ maximum(s) # just to get many ~white pixels
	s
end

# ╔═╡ a23b0cae-455d-11eb-0e50-4dc31c050cc1
begin
	n = 10_000
	dim = 7
	X = [create_object(dim) for i in 1:n]
	length(X), size(X[1]), length(X[1])
	X[1]
end

# ╔═╡ a2984e1a-6165-11eb-20e0-6d16394dc05a
md"""
## Creating the index
Firstly, we need to create an index; please uncomment any index you want to try
"""

# ╔═╡ 1ce583f6-54fb-11eb-10ad-b5dc9328ca3b
index = ExhaustiveSearch(L2Distance(), X);
#index = PivotedSearch(L2Distance(), X, 16);
#index = sss(L2Distance(), X);
#index = distant_tournament(L2Distance(), X, 16, 3)
#index = Kvp(L2Distance(), X);
#index = SearchGraph(SqL2Distance(), X)

# ╔═╡ e600bf26-6167-11eb-08b8-4904cd4d787f
md"""## Querying
Some indexes support changing the querying distance (ExhaustiveSearch & SearchGraph in the previous list), others will just work but without guarantee
"""

# ╔═╡ 5b743cbc-54fa-11eb-1be4-4b619e1070b2

begin
	qsel = @bind qid Slider(1:n, default=10, show_value=true)
	dsel = @bind distname Select([
			"L2" => "Euclidean",
			"L1" => "Manhattan",
			"LInf" => "Chevyshev",
			"Angle" => "Angle",
			"Cosine" => "Cosine",
			], default="L2")

	md"""
	select the query object: $(qsel)
	$(dsel)
	"""
end

# ╔═╡ def63abc-45e7-11eb-231d-11d94709acd3
begin
	dist = Dict(
		"L1" => L1Distance(),
		"L2" => L2Distance(),
		"LInf" => LInftyDistance(),
		"Angle" => AngleDistance(),
		"Cosine" => CosineDistance()
	)[distname]
	index_ = copy(index, dist=dist) # makes a copy of the index with a different distance function
	res = KnnResult(7)
	q = X[qid]
	sep = ones(Float32, dim, 2)
	R = [q, sep, sep]
	with_terminal() do
		@time search(index_, X[qid], res)

		for (i, (id_, dist_)) in enumerate(res)
			push!(R, Float32.(X[id_]))
			push!(R, sep)
			println("$(i)nn: $(id_) => $(dist_)")
		end
	end

	h = hcat(R...)

	md""" $(size(h))

Query Id: $(qid)
database size: $(length(X))
dist: $(distname)

# Result:
$(Gray.(h))

	"""
end

# ╔═╡ 8559b97b-7a34-4283-9b5c-cce77f08fdb4

# ╔═╡ 00000000-0000-0000-0000-000000000001
PLUTO_PROJECT_TOML_CONTENTS = """
[deps]
Colors = "5ae59095-9a9b-59fe-a467-6f913c188581"
MLDatasets = "eb30cadb-4394-5ae3-aed4-317e484a6458"
PlutoUI = "7f904dfe-b85e-4ff6-b463-dae2292396a8"
SimilaritySearch = "053f045d-5466-53fd-b400-a066f88fe02a"

[compat]
Colors = "~0.12.8"
MLDatasets = "~0.5.8"
PlutoUI = "~0.7.9"
SimilaritySearch = "~0.6.2"
"""

# ╔═╡ 00000000-0000-0000-0000-000000000002
PLUTO_MANIFEST_TOML_CONTENTS = """
# This file is machine-generated - editing it directly is not advised

[[ArgTools]]
uuid = "0dad84c5-d112-42e6-8d28-ef12dabb789f"

[[Artifacts]]
uuid = "56f22d72-fd6d-98f1-02f0-08ddc0907c33"

[[Base64]]
uuid = "2a0f44e3-6c83-55bd-87e4-b1978d98bd5f"

[[BinDeps]]
deps = ["Libdl", "Pkg", "SHA", "URIParser", "Unicode"]
git-tree-sha1 = "1289b57e8cf019aede076edab0587eb9644175bd"
uuid = "9e28174c-4ba2-5203-b857-d8d62c4213ee"
version = "1.0.2"

[[BinaryProvider]]
deps = ["Libdl", "Logging", "SHA"]
git-tree-sha1 = "ecdec412a9abc8db54c0efc5548c64dfce072058"
uuid = "b99e7846-7c00-51b0-8f62-c81ae34c0232"
version = "0.5.10"

[[Blosc]]
deps = ["Blosc_jll"]
git-tree-sha1 = "84cf7d0f8fd46ca6f1b3e0305b4b4a37afe50fd6"
uuid = "a74b3585-a348-5f62-a45c-50e91977d574"
version = "0.7.0"

[[Blosc_jll]]
deps = ["Artifacts", "JLLWrappers", "Libdl", "Lz4_jll", "Pkg", "Zlib_jll", "Zstd_jll"]
git-tree-sha1 = "e747dac84f39c62aff6956651ec359686490134e"
uuid = "0b7ba130-8d10-5ba8-a3d6-c5182647fed9"
version = "1.21.0+0"

[[BufferedStreams]]
deps = ["Compat", "Test"]
git-tree-sha1 = "5d55b9486590fdda5905c275bb21ce1f0754020f"
uuid = "e1450e63-4bb3-523b-b2a4-4ffa8c0fd77d"
version = "1.0.0"

[[CodecZlib]]
deps = ["TranscodingStreams", "Zlib_jll"]
git-tree-sha1 = "ded953804d019afa9a3f98981d99b33e3db7b6da"
uuid = "944b1d66-785c-5afd-91f1-9de20f533193"
version = "0.7.0"

[[ColorTypes]]
deps = ["FixedPointNumbers", "Random"]
git-tree-sha1 = "024fe24d83e4a5bf5fc80501a314ce0d1aa35597"
uuid = "3da002f7-5984-5a60-b8a6-cbb66c0b333f"
version = "0.11.0"

[[Colors]]
deps = ["ColorTypes", "FixedPointNumbers", "Reexport"]
git-tree-sha1 = "417b0ed7b8b838aa6ca0a87aadf1bb9eb111ce40"
uuid = "5ae59095-9a9b-59fe-a467-6f913c188581"
version = "0.12.8"

[[Compat]]
deps = ["Base64", "Dates", "DelimitedFiles", "Distributed", "InteractiveUtils", "LibGit2", "Libdl", "LinearAlgebra", "Markdown", "Mmap", "Pkg", "Printf", "REPL", "Random", "SHA", "Serialization", "SharedArrays", "Sockets", "SparseArrays", "Statistics", "Test", "UUIDs", "Unicode"]
git-tree-sha1 = "dc7dedc2c2aa9faf59a55c622760a25cbefbe941"
uuid = "34da2185-b29b-5c13-b0c7-acf172513d20"
version = "3.31.0"

[[DataAPI]]
git-tree-sha1 = "ee400abb2298bd13bfc3df1c412ed228061a2385"
uuid = "9a962f9c-6df0-11e9-0e5d-c546b8b5ee8a"
version = "1.7.0"

[[DataDeps]]
deps = ["BinaryProvider", "HTTP", "Libdl", "Reexport", "SHA", "p7zip_jll"]
git-tree-sha1 = "4f0e41ff461d42cfc62ff0de4f1cd44c6e6b3771"
uuid = "124859b0-ceae-595e-8997-d05f6a7a8dfe"
version = "0.7.7"

[[DataStructures]]
deps = ["Compat", "InteractiveUtils", "OrderedCollections"]
git-tree-sha1 = "4437b64df1e0adccc3e5d1adbc3ac741095e4677"
uuid = "864edb3b-99cc-5e75-8d2d-829cb0a9cfe8"
version = "0.18.9"

[[Dates]]
deps = ["Printf"]
uuid = "ade2ca70-3891-5945-98fb-dc099432e06a"

[[DelimitedFiles]]
deps = ["Mmap"]
uuid = "8bb1440f-4735-579b-a4ab-409b98df4dab"

[[Distances]]
deps = ["LinearAlgebra", "Statistics", "StatsAPI"]
git-tree-sha1 = "abe4ad222b26af3337262b8afb28fab8d215e9f8"
uuid = "b4f34e82-e78d-54a5-968a-f98e89d6e8f7"
version = "0.10.3"

[[Distributed]]
deps = ["Random", "Serialization", "Sockets"]
uuid = "8ba89e20-285c-5b6f-9357-94700520ee1b"

[[Downloads]]
deps = ["ArgTools", "LibCURL", "NetworkOptions"]
uuid = "f43a241f-c20a-4ad4-852c-f6b1247861c6"

[[FixedPointNumbers]]
deps = ["Statistics"]
git-tree-sha1 = "335bfdceacc84c5cdf16aadc768aa5ddfc5383cc"
uuid = "53c48c17-4a7d-5ca2-90c5-79b7896eea93"
version = "0.8.4"

[[GZip]]
deps = ["Libdl"]
git-tree-sha1 = "039be665faf0b8ae36e089cd694233f5dee3f7d6"
uuid = "92fee26a-97fe-5a0c-ad85-20a5f3185b63"
version = "0.5.1"

[[HDF5]]
deps = ["Blosc", "Compat", "HDF5_jll", "Libdl", "Mmap", "Random", "Requires"]
git-tree-sha1 = "1d18a48a037b14052ca462ea9d05dee3ac607d23"
uuid = "f67ccb44-e63f-5c2f-98bd-6dc0ccc4ba2f"
version = "0.15.5"

[[HDF5_jll]]
deps = ["Artifacts", "JLLWrappers", "LibCURL_jll", "Libdl", "OpenSSL_jll", "Pkg", "Zlib_jll"]
git-tree-sha1 = "fd83fa0bde42e01952757f01149dd968c06c4dba"
uuid = "0234f1f7-429e-5d53-9886-15a909be8d59"
version = "1.12.0+1"

[[HTTP]]
deps = ["Base64", "Dates", "IniFile", "Logging", "MbedTLS", "NetworkOptions", "Sockets", "URIs"]
git-tree-sha1 = "c6a1fff2fd4b1da29d3dccaffb1e1001244d844e"
uuid = "cd3eb016-35fb-5094-929b-558a96fad6f3"
version = "0.9.12"

[[IniFile]]
deps = ["Test"]
git-tree-sha1 = "098e4d2c533924c921f9f9847274f2ad89e018b8"
uuid = "83e8ac13-25f8-5344-8a64-a9f2b223428f"
version = "0.5.0"

[[InteractiveUtils]]
deps = ["Markdown"]
uuid = "b77e0a4c-d291-57a0-90e8-8db25a27a240"

[[JLLWrappers]]
deps = ["Preferences"]
git-tree-sha1 = "642a199af8b68253517b80bd3bfd17eb4e84df6e"
uuid = "692b3bcd-3c85-4b1f-b108-f13ce0eb3210"
version = "1.3.0"

[[JSON]]
deps = ["Dates", "Mmap", "Parsers", "Unicode"]
git-tree-sha1 = "81690084b6198a2e1da36fcfda16eeca9f9f24e4"
uuid = "682c06a0-de6a-54ab-a142-c8b1cf79cde6"
version = "0.21.1"

[[LibCURL]]
deps = ["LibCURL_jll", "MozillaCACerts_jll"]
uuid = "b27032c2-a3e7-50c8-80cd-2d36dbcbfd21"

[[LibCURL_jll]]
deps = ["Artifacts", "LibSSH2_jll", "Libdl", "MbedTLS_jll", "Zlib_jll", "nghttp2_jll"]
uuid = "deac9b47-8bc7-5906-a0fe-35ac56dc84c0"

[[LibGit2]]
deps = ["Base64", "NetworkOptions", "Printf", "SHA"]
uuid = "76f85450-5226-5b5a-8eaa-529ad045b433"

[[LibSSH2_jll]]
deps = ["Artifacts", "Libdl", "MbedTLS_jll"]
uuid = "29816b5a-b9ab-546f-933c-edad1886dfa8"

[[Libdl]]
uuid = "8f399da3-3557-5675-b5ff-fb832c97cbdb"

[[LinearAlgebra]]
deps = ["Libdl"]
uuid = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e"

[[Logging]]
uuid = "56ddb016-857b-54e1-b83d-db4d58db5568"

[[Lz4_jll]]
deps = ["Artifacts", "JLLWrappers", "Libdl", "Pkg"]
git-tree-sha1 = "5d494bc6e85c4c9b626ee0cab05daa4085486ab1"
uuid = "5ced341a-0733-55b8-9ab6-a4889d929147"
version = "1.9.3+0"

[[MAT]]
deps = ["BufferedStreams", "CodecZlib", "HDF5", "SparseArrays"]
git-tree-sha1 = "5c62992f3d46b8dce69bdd234279bb5a369db7d5"
uuid = "23992714-dd62-5051-b70f-ba57cb901cac"
version = "0.10.1"

[[MLDatasets]]
deps = ["BinDeps", "ColorTypes", "DataDeps", "DelimitedFiles", "FixedPointNumbers", "GZip", "MAT", "Requires"]
git-tree-sha1 = "454056087be66c6b802a2bf15a4a662622910ece"
uuid = "eb30cadb-4394-5ae3-aed4-317e484a6458"
version = "0.5.8"

[[Markdown]]
deps = ["Base64"]
uuid = "d6f4376e-aef5-505a-96c1-9c027394607a"

[[MbedTLS]]
deps = ["Dates", "MbedTLS_jll", "Random", "Sockets"]
git-tree-sha1 = "1c38e51c3d08ef2278062ebceade0e46cefc96fe"
uuid = "739be429-bea8-5141-9913-cc70e7f3736d"
version = "1.0.3"

[[MbedTLS_jll]]
deps = ["Artifacts", "Libdl"]
uuid = "c8ffd9c3-330d-5841-b78e-0817d7145fa1"

[[Missings]]
deps = ["DataAPI"]
git-tree-sha1 = "4ea90bd5d3985ae1f9a908bd4500ae88921c5ce7"
uuid = "e1d29d7a-bbdc-5cf2-9ac0-f12de2c33e28"
version = "1.0.0"

[[Mmap]]
uuid = "a63ad114-7e13-5084-954f-fe012c677804"

[[MozillaCACerts_jll]]
uuid = "14a3606d-f60d-562e-9121-12d972cd8159"

[[NetworkOptions]]
uuid = "ca575930-c2e3-43a9-ace4-1e988b2c1908"

[[OpenSSL_jll]]
deps = ["Artifacts", "JLLWrappers", "Libdl", "Pkg"]
git-tree-sha1 = "15003dcb7d8db3c6c857fda14891a539a8f2705a"
uuid = "458c3c95-2e84-50aa-8efc-19380b2a3a95"
version = "1.1.10+0"

[[OrderedCollections]]
git-tree-sha1 = "85f8e6578bf1f9ee0d11e7bb1b1456435479d47c"
uuid = "bac558e1-5e72-5ebc-8fee-abe8a469f55d"
version = "1.4.1"

[[Parameters]]
deps = ["OrderedCollections", "UnPack"]
git-tree-sha1 = "2276ac65f1e236e0a6ea70baff3f62ad4c625345"
uuid = "d96e819e-fc66-5662-9728-84c9c7592b0a"
version = "0.12.2"

[[Parsers]]
deps = ["Dates"]
git-tree-sha1 = "c8abc88faa3f7a3950832ac5d6e690881590d6dc"
uuid = "69de0a69-1ddd-5017-9359-2bf0b02dc9f0"
version = "1.1.0"

[[Pkg]]
deps = ["Artifacts", "Dates", "Downloads", "LibGit2", "Libdl", "Logging", "Markdown", "Printf", "REPL", "Random", "SHA", "Serialization", "TOML", "Tar", "UUIDs", "p7zip_jll"]
uuid = "44cfe95a-1eb2-52ea-b672-e2afdf69b78f"

[[PlutoUI]]
deps = ["Base64", "Dates", "InteractiveUtils", "JSON", "Logging", "Markdown", "Random", "Reexport", "Suppressor"]
git-tree-sha1 = "44e225d5837e2a2345e69a1d1e01ac2443ff9fcb"
uuid = "7f904dfe-b85e-4ff6-b463-dae2292396a8"
version = "0.7.9"

[[Preferences]]
deps = ["TOML"]
git-tree-sha1 = "00cfd92944ca9c760982747e9a1d0d5d86ab1e5a"
uuid = "21216c6a-2e73-6563-6e65-726566657250"
version = "1.2.2"

[[Printf]]
deps = ["Unicode"]
uuid = "de0858da-6303-5e67-8744-51eddeeeb8d7"

[[REPL]]
deps = ["InteractiveUtils", "Markdown", "Sockets", "Unicode"]
uuid = "3fa0cd96-eef1-5676-8a61-b3b8758bbffb"

[[Random]]
deps = ["Serialization"]
uuid = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c"

[[Reexport]]
git-tree-sha1 = "5f6c21241f0f655da3952fd60aa18477cf96c220"
uuid = "189a3867-3050-52da-a836-e630ba90ab69"
version = "1.1.0"

[[Requires]]
deps = ["UUIDs"]
git-tree-sha1 = "4036a3bd08ac7e968e27c203d45f5fff15020621"
uuid = "ae029012-a4dd-5104-9daa-d747884805df"
version = "1.1.3"

[[SHA]]
uuid = "ea8e919c-243c-51af-8825-aaa63cd721ce"

[[SearchModels]]
deps = ["Distributed", "Random", "StatsBase"]
git-tree-sha1 = "c552a0cb8bb76ec290c9820521b3ea27e74ee6a2"
uuid = "0e966ebe-b704-4a65-8279-db954bfe5da0"
version = "0.2.3"

[[Serialization]]
uuid = "9e88b42a-f829-5b0c-bbe9-9e923198166b"

[[SharedArrays]]
deps = ["Distributed", "Mmap", "Random", "Serialization"]
uuid = "1a1011a3-84de-559e-8e89-a11a2f7dc383"

[[SimilaritySearch]]
deps = ["Dates", "Distances", "LinearAlgebra", "Parameters", "Random", "SearchModels", "StatsBase", "Test"]
git-tree-sha1 = "0cff1fc0558bffdca16b0d73bb87332fcd295573"
uuid = "053f045d-5466-53fd-b400-a066f88fe02a"
version = "0.6.2"

[[Sockets]]
uuid = "6462fe0b-24de-5631-8697-dd941f90decc"

[[SortingAlgorithms]]
deps = ["DataStructures"]
git-tree-sha1 = "b3363d7460f7d098ca0912c69b082f75625d7508"
uuid = "a2af1166-a08f-5f64-846c-94a0d3cef48c"
version = "1.0.1"

[[SparseArrays]]
deps = ["LinearAlgebra", "Random"]
uuid = "2f01184e-e22b-5df5-ae63-d93ebab69eaf"

[[Statistics]]
deps = ["LinearAlgebra", "SparseArrays"]
uuid = "10745b16-79ce-11e8-11f9-7d13ad32a3b2"

[[StatsAPI]]
git-tree-sha1 = "1958272568dc176a1d881acb797beb909c785510"
uuid = "82ae8749-77ed-4fe6-ae5f-f523153014b0"
version = "1.0.0"

[[StatsBase]]
deps = ["DataAPI", "DataStructures", "LinearAlgebra", "Missings", "Printf", "Random", "SortingAlgorithms", "SparseArrays", "Statistics", "StatsAPI"]
git-tree-sha1 = "2f6792d523d7448bbe2fec99eca9218f06cc746d"
uuid = "2913bbd2-ae8a-5f71-8c99-4fb6c76f3a91"
version = "0.33.8"

[[Suppressor]]
git-tree-sha1 = "a819d77f31f83e5792a76081eee1ea6342ab8787"
uuid = "fd094767-a336-5f1f-9728-57cf17d0bbfb"
version = "0.2.0"

[[TOML]]
deps = ["Dates"]
uuid = "fa267f1f-6049-4f14-aa54-33bafae1ed76"

[[Tar]]
deps = ["ArgTools", "SHA"]
uuid = "a4e569a6-e804-4fa4-b0f3-eef7a1d5b13e"

[[Test]]
deps = ["InteractiveUtils", "Logging", "Random", "Serialization"]
uuid = "8dfed614-e22c-5e08-85e1-65c5234f0b40"

[[TranscodingStreams]]
deps = ["Random", "Test"]
git-tree-sha1 = "7c53c35547de1c5b9d46a4797cf6d8253807108c"
uuid = "3bb67fe8-82b1-5028-8e26-92a6c54297fa"
version = "0.9.5"

[[URIParser]]
deps = ["Unicode"]
git-tree-sha1 = "53a9f49546b8d2dd2e688d216421d050c9a31d0d"
uuid = "30578b45-9adc-5946-b283-645ec420af67"
version = "0.4.1"

[[URIs]]
git-tree-sha1 = "97bbe755a53fe859669cd907f2d96aee8d2c1355"
uuid = "5c2747f8-b7ea-4ff2-ba2e-563bfd36b1d4"
version = "1.3.0"

[[UUIDs]]
deps = ["Random", "SHA"]
uuid = "cf7118a7-6976-5b1a-9a39-7adc72f591a4"

[[UnPack]]
git-tree-sha1 = "387c1f73762231e86e0c9c5443ce3b4a0a9a0c2b"
uuid = "3a884ed6-31ef-47d7-9d2a-63182c4928ed"
version = "1.0.2"

[[Unicode]]
uuid = "4ec0a83e-493e-50e2-b9ac-8f72acf5a8f5"

[[Zlib_jll]]
deps = ["Libdl"]
uuid = "83775a58-1f1d-513f-b197-d71354ab007a"

[[Zstd_jll]]
deps = ["Artifacts", "JLLWrappers", "Libdl", "Pkg"]
git-tree-sha1 = "cc4bf3fdde8b7e3e9fa0351bdeedba1cf3b7f6e6"
uuid = "3161d3a3-bdf6-5164-811a-617609db77b4"
version = "1.5.0+0"

[[nghttp2_jll]]
deps = ["Artifacts", "Libdl"]
uuid = "8e850ede-7688-5339-a07c-302acd2aaf8d"

[[p7zip_jll]]
deps = ["Artifacts", "Libdl"]
uuid = "3f19e933-33d8-53b3-aaab-bd5110c3b7a0"
"""

# ╔═╡ Cell order:
# ╟─5cd87a9e-5506-11eb-2744-6f02144677ff
# ╟─c5af5d4a-455b-11eb-0b57-4d8d63615b85
# ╟─d8d27dbc-5507-11eb-20e9-0f16ddba080b
# ╟─c6ca5d24-616c-11eb-1031-4b943ecc633c
# ╟─a23b0cae-455d-11eb-0e50-4dc31c050cc1
# ╟─a2984e1a-6165-11eb-20e0-6d16394dc05a
# ╟─1ce583f6-54fb-11eb-10ad-b5dc9328ca3b
# ╟─e600bf26-6167-11eb-08b8-4904cd4d787f
# ╟─5b743cbc-54fa-11eb-1be4-4b619e1070b2
# ╟─def63abc-45e7-11eb-231d-11d94709acd3
# ╠═8559b97b-7a34-4283-9b5c-cce77f08fdb4
# ╟─00000000-0000-0000-0000-000000000001
# ╟─00000000-0000-0000-0000-000000000002


text:
export toggle!, local_complement!, interlace

"""
`toggle!(G,x,y)` deletes edge `xy` if present or adds edge `xy`
if absent. **No error checking is done**.
"""
function toggle!(G::SimpleGraph, x, y)
    if has(G, x, y)
        SimpleGraphs.delete!(G, x, y)
    else
        add!(G, x, y)
    end
    nothing
end

"""
`super_toggle!(G,A,B)` toggles all edges/nonedges `ab`
where `a` is in `A` and `b` is in `B`.
**No error checking is done**.
"""
function super_toggle!(G::SimpleGraph, A::Set, B::Set)
    for a in A
        for b in B
            toggle!(G, a, b)
        end
    end
    nothing
end

"""
`local_complement!(G,v)` complements the edges in the neighborhood of `v`.
That is, if `u` and `w` are neighbors of `v` then we toggle the edge/nonedge
`uw`, modifying the graph.
"""
function local_complement!(G::SimpleGraph, v)
    if !has(G, v)
        error("Vertex $v is not in this graph")
    end
    Nv = G[v]  # get neighbors
    dv = length(Nv)

    for i = 1:dv-1
        u = Nv[i]
        for j = i+1:dv
            w = Nv[j]
            toggle!(G, u, w)
        end
    end
    nothing
end

function pivot!(G::SimpleGraph, a, b, debug::Bool = false)
    if !has(G, a, b)
        error("Edge ($a,$b) is not in this graph")
    end
    NA = Set(G[a])
    delete!(NA, b)
    NB = Set(G[b])
    delete!(NB, a)
    if debug
        println("N[$a] = $NA")
        println("N[$b] = $NB")
    end

    A = setdiff(NA, NB)
    B = setdiff(NB, NA)
    C = intersect(NA, NB)

    if debug
        println("A = $A")
        println("B = $B")
        println("C = $C")
    end

    super_toggle!(G, A, B)
    super_toggle!(G, A, C)
    super_toggle!(G, B, C)
end

"""
`interlace(G)` returns the interlace polynomial of the graph `G`.
"""
function interlace(G::SimpleGraph, saver::Bool = true)
    if saver && cache_check(G, :interlace)
        return cache_recall(G, :interlace)
    end
    if NE(G) == 0
        x = getx()
        return x^NV(G)
    end

    if is_connected(G)
        e = first(G.E)
        a = e[1]
        b = e[2]

        G1 = deepcopy(G)
        SimpleGraphs.delete!(G1, a)
        p1 = interlace(G1, false)

        G2 = deepcopy(G)
        pivot!(G2, a, b)
        SimpleGraphs.delete!(G2, b)
        p2 = interlace(G2, false)

        p = p1 + p2
        if saver
            cache_save(G, :interlace, p)
        end

        return p
    end

    # if not connected, break into components and use multiplicativity
    comps = parts(components(G))
    p = SimplePolynomial(1)
    for S in comps
        H = induce(G, S)
        pH = interlace(H, false)
        p *= pH
    end
    if saver
        cache_save(G, :interlace, p)
    end
    return p

end


text:
# ---
# title: 701. Insert into a Binary Search Tree
# id: problem701
# author: zhwang
# date: 2022-02-22
# difficulty: Medium
# categories: Tree
# link: <https://leetcode.com/problems/insert-into-a-binary-search-tree/description/>
# hidden: true
# ---
#
# You are given the `root` node of a binary search tree (BST) and a `value` to
# insert into the tree. Return _the root node of the BST after the insertion_.
# It is **guaranteed** that the new value does not exist in the original BST.
#
# **Notice**  that there may exist multiple valid ways for the insertion, as
# long as the tree remains a BST after insertion. You can return **any of
# them**.
#
#
#
# **Example 1:**
#
# ![](https://assets.leetcode.com/uploads/2020/10/05/insertbst.jpg)
#
#
#
#     Input: root = [4,2,7,1,3], val = 5
#     Output: [4,2,7,1,3,5]
#     Explanation: Another accepted tree is:
#     ![](https://assets.leetcode.com/uploads/2020/10/05/bst.jpg)
#
#
# **Example 2:**
#
#
#
#     Input: root = [40,20,60,10,30,50,70], val = 25
#     Output: [40,20,60,10,30,50,70,null,null,25]
#
#
# **Example 3:**
#
#
#
#     Input: root = [4,2,7,1,3,null,null,null,null,null,null], val = 5
#     Output: [4,2,7,1,3,5]
#
#
#
#
# **Constraints:**
#
#   * The number of nodes in the tree will be in the range `[0, 104]`.
#   * `-108 <= Node.val <= 108`
#   * All the values `Node.val` are **unique**.
#   * `-108 <= val <= 108`
#   * It's **guaranteed** that `val` does not exist in the original BST.
#
#
## @lc code=start
using LeetCode

insert_into_bst(::Nothing, val::Int) = TreeNode(val)
function insert_into_bst(root::TreeNode, val::Int)::TreeNode
    if root.val > val
        root.left = insert_into_bst(root.left, val)
    else
        root.right = insert_into_bst(root.right, val)
    end
    root
end

## @lc code=end
## @lc test=start
@testset "701.insert-into-a-binary-search-tree.jl" begin
    @test insert_into_bst(TreeNode{Int}([4,2,7,1,3]),5) == TreeNode{Int}([4,2,7,1,3,5])
    @test insert_into_bst(TreeNode{Int}([40,20,60,10,30,50,70]), 25) == TreeNode{Int}([40,20,60,10,30,50,70,nothing,nothing,25])
    @test insert_into_bst(TreeNode{Int}([4,2,7,1,3,nothing,nothing,nothing,nothing,nothing,nothing]), 5) == TreeNode{Int}([4,2,7,1,3,5])
end
## @lc test=end

## @lc info=start
# link: [solution to 701](https://leetcode-cn.com/problems/insert-into-a-binary-search-tree/solution/701-er-cha-sou-suo-shu-zhong-de-cha-ru-c-ca85/)
## @lc info=end


text:
using Plots

x̄, ū = unpack(z̄, prob)

# Position trajectory
Px = [x̄[t][1] for t = 1:T]
Py = [x̄[t][2] for t = 1:T]

pts = Plots.partialcircle(0.0, 2.0 * π, 100, 0.1)
cx, cy = Plots.unzip(pts)
cx1 = [_cx + circles[1][1] for _cx in cx]
cy1 = [_cy + circles[1][2] for _cy in cy]
cx2 = [_cx + circles[2][1] for _cx in cx]
cy2 = [_cy + circles[2][2] for _cy in cy]
cx3 = [_cx + circles[3][1] for _cx in cx]
cy3 = [_cy + circles[3][2] for _cy in cy]
cx4 = [_cx + circles[4][1] for _cx in cx]
cy4 = [_cy + circles[4][2] for _cy in cy]

plt = plot(Shape(cx1, cy1), color = :black, label = "", linecolor = :black)
plt = plot!(Shape(cx2, cy2), color = :black, label = "", linecolor = :black)
plt = plot!(Shape(cx3, cy3), color = :black, label = "", linecolor = :black)
plt = plot!(Shape(cx4, cy4), color = :black, label = "", linecolor = :black)
plt = plot!(Px, Py, aspect_ratio = :equal, xlabel = "x", ylabel = "y",
    width = 4.0, label = "TO", color = :cyan, legend = :topleft)

# DPO
x, u = unpack(z, prob_dpo.prob.prob.nom)

# Position trajectory
Px_dpo = [x[t][1] for t = 1:T]
Py_dpo = [x[t][2] for t = 1:T]
plt = plot!(Px_dpo, Py_dpo, width = 4.0, label = "DPO", color = :orange)

# Position trajectory (samples)
x_sample = []
for i = 1:2 * model.n
	z_sample = z[prob_dpo.prob.idx.sample[i]]
	_x_sample, u_sample = unpack(z_sample, prob_dpo.prob.prob.nom)
	push!(x_sample, _x_sample)
	Px_sample = [_x_sample[t][1] for t = 1:T]
	Py_sample = [_x_sample[t][2] for t = 1:T]
	plt = plot!(Px_sample, Py_sample, width = 2.0, label = "sample", color = :magenta)
end
display(plt)

# using PGFPlots
# const PGF = PGFPlots
#
# # TO trajectory
# p_nom = PGF.Plots.Linear(hcat(x̄...)[1,:], hcat(x̄...)[2,:],
#     mark = "none",
# 	style = "color=cyan, line width=2pt, solid",
# 	legendentry = "TO")
#
# # DPO trajectory
# p_dpo = PGF.Plots.Linear(hcat(x...)[1,:], hcat(x...)[2,:],
#     mark = "none",
# 	style = "color=orange, line width=2pt, solid",
# 	legendentry = "DPO")
#
# # obstacles
# pc1 = PGF.Plots.Circle(circles[1]...,
# 	style = "color=black, fill=black")
# pc2 = PGF.Plots.Circle(circles[2]...,
# 	style = "color=black, fill=black")
# pc3 = PGF.Plots.Circle(circles[3]...,
# 	style = "color=black, fill=black")
# pc4 = PGF.Plots.Circle(circles[4]...,
# 	style = "color=black, fill=black")
#
# a = Axis([
#     p_nom;
#     p_dpo;
# 	pc1;
# 	pc2;
# 	pc3;
# 	pc4],
#     xmin = -0.4, ymin = -0.1, xmax = 1.4, ymax = 1.1,
#     axisEqualImage = true,
#     hideAxis = true,
# 	# ylabel = "y",
# 	# xlabel = "x",
# 	legendStyle = "{at={(0.01, 0.99)}, anchor = north west}")
#
# # Save to tikz format
# PGF.save(joinpath(@__DIR__, "car_obstacles.tikz"), a, include_preamble = false)

# Animation
include(joinpath(pwd(), "models/visualize.jl"))
vis = Visualizer()
# render(vis)
open(vis)
default_background!(vis)
settransform!(vis["/Cameras/default"],
	compose(Translation(0.0, 0.0, -1.0),LinearMap(RotY(-pi/2.5))))
shift = [-0.5; -0.5; 0.0]

for i = 1:4
    cyl = Cylinder(Point3f0(circles[i][1] - 0.5,circles[i][2] - 0.5,0), Point3f0(circles[i][1] - 0.5,circles[i][2] -0.5,0.1),convert(Float32,0.035))
    setobject!(vis["cyl$i"],cyl,MeshPhongMaterial(color=RGBA(0,0,0,1.0)))
end

q_to = deepcopy(x̄)

# interpolate traj
T = length(q_to)
T_sim = 4 * T
times = [(t - 1) * h for t = 1:T-1]
tf = h * (T-1)
t_sim = range(0, stop = tf, length = T_sim)
t_ctrl = range(0, stop = tf, length = T)
dt_sim = tf / (T_sim - 1)
A_state = hcat(q_to...)
z_cubic = zero(q_to[1])

for t = 1:T_sim
	for i = 1:length(q_to[1])
		interp_cubic = CubicSplineInterpolation(t_ctrl, A_state[i,:])
		z_cubic[i] = interp_cubic(t_sim[t])
	end
	setobject!(vis["traj_to$t"],
		Cylinder(Point3f0(0.0, 0.0, -0.001),
			Point3f0(0.0, 0.0, 0.0),
			convert(Float32,0.065)),
			MeshPhongMaterial(color=RGBA(0.0,255.0/255.0,255.0/255.0,1.0)))

		# HyperSphere(Point3f0(0),
		# convert(Float32,0.075)),
		# MeshPhongMaterial(color=RGBA(0.0,255.0/255.0,255.0/255.0,1.0)))
	settransform!(vis["traj_to$t"], Translation((z_cubic[1] - 0.5,z_cubic[2] -0.5,0.0)))
	setvisible!(vis["traj_to$t"],false)
end

q_dpo = deepcopy(x)
A_state = hcat(q_dpo...)
z_cubic = zero(q_dpo[1])
for t = 1:T_sim
	for i = 1:length(q_dpo[1])
		interp_cubic = CubicSplineInterpolation(t_ctrl, A_state[i,:])
		z_cubic[i] = interp_cubic(t_sim[t])
	end
	setobject!(vis["traj_dpo$t"], Cylinder(Point3f0(0.0, 0.0, -0.001),
		Point3f0(0.0, 0.0, 0.001),
		convert(Float32,0.065)),
		MeshPhongMaterial(color=RGBA(255.0/255.0,127.0/255.0,0.0,1.0)))
	settransform!(vis["traj_dpo$t"], Translation((z_cubic[1] - 0.5,z_cubic[2] - 0.5,0.0)))
	setvisible!(vis["traj_dpo$t"],true)
end

for i = 1:2 * model.n
	# q_dpo = deepcopy(x_sample[i])
	A_state = hcat(q_dpo...)
	z_cubic = zero(q_dpo[1])
	for t = 1:T_sim
		for i = 1:length(q_dpo[1])
			interp_cubic = CubicSplineInterpolation(t_ctrl, A_state[i,:])
			z_cubic[i] = interp_cubic(t_sim[t])
		end
		setobject!(vis["traj_sample$t$i"], Cylinder(Point3f0(0.0, 0.0, -0.001),
			Point3f0(0.0, 0.0, 0.000),
			convert(Float32,0.065)),
			MeshPhongMaterial(color=RGBA(255.0/255.0,0.0,255.0,1.0)))
		settransform!(vis["traj_sample$t$i"], Translation((z_cubic[1] - 0.5,z_cubic[2] - 0.5,0.0)))
		setvisible!(vis["traj_sample$t$i"],false)
	end
end

q = [[x[1] for i = 1:25]..., x..., [x[T] for i = 1:25]...]
q = [[x̄[1] for i = 1:25]..., x̄..., [x̄[T] for i = 1:25]...]

obj_path = joinpath(pwd(),"/home/taylor/Research/motion_planning/models/cybertruck/cybertruck.obj")
mtl_path = joinpath(pwd(),"/home/taylor/Research/motion_planning/models/cybertruck/cybertruck.mtl")
m = ModifiedMeshFileObject(obj_path, mtl_path, scale = 0.05)
setobject!(vis["car"], m)
settransform!(vis["car"], compose(Translation([q[t][1];q[t][2];0.0] + shift),LinearMap(RotZ(q[t][3]+pi)*RotX(pi/2.0))))

anim = MeshCat.Animation(convert(Int, floor(1.0 / h)))

for t = 1:length(q)
	MeshCat.atframe(anim,t) do
		settransform!(vis["car"],
			compose(Translation(q[t][1] - 0.5, q[t][2] - 0.5, 0.0),
				LinearMap(RotZ(q[t][3] + pi) * RotX(pi / 2.0))))
	end
end
MeshCat.setanimation!(vis, anim)

ctm = ModifiedMeshFileObject(obj_path, mtl_path, scale=0.05)
t = 1
setobject!(vis["ct1"],ctm)
settransform!(vis["ct1"], compose(Translation([q[t][1];q[t][2];0.0] + shift),LinearMap(RotZ(q[t][3]+pi)*RotX(pi/2.0))))
setvisible!(vis["ct1"],true)
t = 1
setobject!(vis["ct2"],ctm)
settransform!(vis["ct2"], compose(Translation([q[t][1];q[t][2];0.0] + shift),LinearMap(RotZ(q[t][3]+pi)*RotX(pi/2.0))))
setvisible!(vis["ct2"],true)
t = 20#17#20
setobject!(vis["ct3"],ctm)
settransform!(vis["ct3"], compose(Translation([q[t][1];q[t][2];0.0] + shift),LinearMap(RotZ(q[t][3]+pi)*RotX(pi/2.0))))
setvisible!(vis["ct3"],true)

t = 34#36#34
setobject!(vis["ct4"],ctm)
settransform!(vis["ct4"], compose(Translation([q[t][1];q[t][2];0.0] + shift),LinearMap(RotZ(q[t][3]+pi)*RotX(pi/2.0))))
setvisible!(vis["ct4"],true)
t = T
setobject!(vis["ct5"],ctm)
settransform!(vis["ct5"], compose(Translation([q[t][1];q[t][2];0.0] + shift),LinearMap(RotZ(q[t][3]+pi)*RotX(pi/2.0))))
setvisible!(vis["ct5"],true)

settransform!(vis["/Cameras/default"],
	compose(Translation(0.0, 0.0, 1.0),LinearMap(RotY(-pi/2.5))))

open(vis)


text:
#CMDimCircuts: Circuit analysis tools operating on CMDimData datasets.
#-------------------------------------------------------------------------------
__precompile__(true)
#=
TAGS:
	#WANTCONST, HIDEWARN_0.7
=#

module CMDimCircuits

using MDDatasets
using CMDimData
using NumericIO

const rootpath = realpath(joinpath(@__DIR__, ".."))
const demoplotcfgscript = joinpath(rootpath, "sample", "cfgplots4demo.jl")

include("physics.jl")
include("dsvfiles.jl")
include("CircuitAnalysis/CircuitAnalysis.jl")
include("NetwAnalysis/NetwAnalysis.jl") #Depends on CircuitAnalysis
include("EDAData/EDAData.jl") #Depends on NetwAnalysis
include("SignalProcessing/SignalProcessing.jl")

#==Convenience macros
===============================================================================#

#"using" C-Data environment into the caller's module:
macro using_CData()
	m = quote
		using MDDatasets
		using CMDimData
		using CMDimData.EasyPlot
		#using CMDimData.EasyData #TODO: Cannot yet because EasyData is "included".
		#using CMDimCircuits #Already there
		using CMDimCircuits.CircuitAnalysis
		using CMDimCircuits.NetwAnalysis
		using CMDimCircuits.SignalProcessing
		using CMDimCircuits.EDAData
	end
	return esc(m) #esc: Evaluate in calling module
end

#==Functions
===============================================================================#
SI(v; ndigits=3) = NumericIO.formatted(v, :SI, ndigits=ndigits)

#==Exported symbols
===============================================================================#
export SI

#==Initialization
===============================================================================#
function __init__()
	return
end

end # module


text:
# ---
# jupyter:
#   jupytext:
#     formats: ipynb,jl:hydrogen
#     text_representation:
#       extension: .jl
#       format_name: hydrogen
#       format_version: '1.3'
#       jupytext_version: 1.11.2
#   kernelspec:
#     display_name: Julia 1.6.2
#     language: julia
#     name: julia-1.6
# ---

# %%
using QuadGK

f(x) = (x + 2) * x * (x - 2)
g(x) = -f(5 - x)

@show quadgk(f, 0, 5)
@show quadgk(f, 5, 0)
@show quadgk(g, 0, 5);

# %%

max_stars_repo_path: 0020/quadgk example.jl

max_stars_repo_name: genkuroki/public

max_issues_repo_path: 0020/quadgk example.jl

max_issues_repo_name: genkuroki/public

max_forks_repo_path: 0020/quadgk example.jl

max_forks_repo_name: genkuroki/public


text:
function go_front(tar::Entity)
	l=steplength(tar)
	move!(tar,l*sin(tar.θ),-l*cos(tar.θ))
end
function go_back(tar::Entity)
	l=steplength(tar)
	move!(tar,-l*sin(tar.θ),l*cos(tar.θ))
end
go_north(tar::Entity)=move!(tar,0,-steplength(tar))
go_south(tar::Entity)=move!(tar,0,steplength(tar))
go_west(tar::Entity)=move!(tar,-steplength(tar),0)
go_east(tar::Entity)=move!(tar,steplength(tar),0)
turn_left(tar::Entity)=tar.θ-=π/12
turn_right(tar::Entity)=tar.θ+=π/12
"pos是已经过处理的地图坐标"
function mousedown(pos::Pair{Float,Float})
	bx=floor(pos.first)
	by=floor(pos.second)
	if !c_reach(ply,bx,by) return end
	plyclickblk(bx,by)
end
function plyclickblk(x::Int,y::Int,p::玩家=ply)
	blk=getblk(x,y)
	if isa(blk,气体)||isa(blk,液体)
		plyfill(x,y)
		return
	end
	tm=destroy_with(p.ib[p.chosen],blk)
	if (!lsetting[:break_all]) && tm==-1
		info_help(:game,:not_destroyable)
		return
	end
	if tm<4
		plydestroy(x,y)
		return
	end
	# 摧毁进程
	cou=0
	mouse_up=Channel{Nothing}(1)
	id=signal_connect(dcon,:mouse_release_event) do widget,event
		put!(mouse_up,nothing)
	end
	sur=CairoImageSurface(loadedimgs[:dig0])
	after_show[:d]=()->begin
		pos=get_show_pos(x,y)
		move_to(dcon,pos.first,pos.second)
		set_source_surface(dcon,sur)
	end
	t=@task begin
		while true
			sleep(0.5)
			cou+=1
			if !isempty(mouse_up)
				if cou>tm plydestroy(x,y)
				else info_help(:game,:not_enough_time)
				end
				break
			elseif cou>tm wait(mouse_up)
			elseif cou>tm>>2 sur=CairoImageSurface(loadedimgs[:dig1])
			elseif cou>tm>>1 sur=CairoImageSurface(loadedimgs[:dig2])
			end
		end
	end
	schedule(t)
	delete!(after_show,:d)
	signal_handler_disconnect(dcon,id)
end
function plyfill(x::Int,y::Int,p::玩家=ply)
	it=p.ib.i[p.chosen]
	blk=t_blk(it)
	if blk==missing return false
	else
		setblk(x,y,blk;player_put=true)
		if !lsetting[:inf_itm]
			reduce!(p.ib,p.chosen,1)
			updg(p)
		end
		return true
	end
end
function plydestroy(x::Int,y::Int,p::玩家=ply)
	setblk(x,y,空气())
end

function env_light()
#=
0.05s一帧，一昼夜32768帧，即27min 18.4s
round(sin(t/16384*π)*8+7.5)
=#
	t=UInt16(lsetting[:t]&32767)
	te=(t>16384 ? t-32768 : t)>>1
	res= te<327||te>7865 ? 0x0 :
	te<659||te>7533 ? 0x1 :
	te<1003||te>7189 ? 0x2 :
	te<1366||te>6826 ? 0x3 :
	te<1761||te>6431 ? 0x4 :
	te<2212||te>5980 ? 0x5 :
	te<2779||te>5413 ? 0x6 : 0x7
    return (t>16384) ? 0x7-res : res+0x8
end


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

using Markdown
using InteractiveUtils

# ╔═╡ ca32437b-65ce-401b-93fc-1f3b7011ae88
begin
    import Pkg
    # activate the shared project environment
    Pkg.activate(Base.current_project())
    using Omega, Distributions, UnicodePlots
end

# ╔═╡ daa9fd86-5a36-4aa2-842e-477cdb19b8d5
md"# Probability Dilution
Probability dilution is refers to scenarios where one can increase one's confidence by increasing the noisyness of one's measurement device.

Here we'll use an example on microcrip certification to demonstrate."

# ╔═╡ 62257a75-2949-4afc-8543-fe3fa4073be5
md"## Example: Chip certification

Imagine you're a microchip certification engineer.  You probe microchips coming off the supply line to see if they are good or bad.  A microchip is bad if its power pin (which is the first pin) has a zero voltage.  If any of the other pins have zero voltage it's salvageable, and we'll say it's a good pin.  For simplicitly, we assume that only one (and exactly one) pin is faulty."

# ╔═╡ 53572568-9d02-465a-baba-1c624d79d4b5
md"We'll express our prior belief over which pin is faulty using a discrete uniform distribution over the 10 pins:"

# ╔═╡ f0f80b1a-d090-11eb-2b0d-d11fd8196311
#using Omega, Distributions, UnicodePlots

# ╔═╡ 4936c45a-a5bf-49cb-bfdc-c05883d5982b
faulty_pin = @~ DiscreteUniform(1, 10)

# ╔═╡ 57c308fa-a2e7-49fb-b52d-41346ee7ea0d
md"The probe tells us which pin is faulty.  Unfortunately, the probe is a little error prone.  Under normal operation it gives back the true answer, but with some probability it just gives back a random answer between 1 and 10"

# ╔═╡ d3af8df6-5375-4a52-9432-41ef6227af0c
error_prob = Variable(ω -> 0.01) # constant random variable

# ╔═╡ b0769639-7b6a-4d95-bc8d-bdedd5d27c4f
error_occurred = @~ Bernoulli(error_prob)

# ╔═╡ 8e2fb0c5-ca50-4bcb-85bf-d78c06ea5653
alternative_pin = @~ DiscreteUniform(1, 10)

# ╔═╡ ad3ec928-dc20-4987-aaed-78e8c4141d3e
function measure(ω)
	if error_occurred(ω)
		alternative_pin(ω)
	else
		faulty_pin(ω)
	end
end

# ╔═╡ 87199546-b427-4908-9531-28d40995efa8
md"As described above, a chip is bad if the faulty_pin is pin 1"

# ╔═╡ 4b8285ca-0fa8-4c38-a2a9-870169a2c1d7
bad_chip = faulty_pin ==ₚ 1

# ╔═╡ 021546fa-267d-4789-b6a5-b69d7eea01e6
md"We'll define a function `prob` to estimate the probability that an event is true:"

# ╔═╡ 0ed2799b-0e1b-434a-85be-99a3869a89c7
prob(x; n = 1000) = mean(randsample(x, n))

# ╔═╡ 505f497c-2852-44f2-bf17-8bfb12886554
md"The probability that it's a bad chip is:"

# ╔═╡ fc03484c-206d-4c3d-90f9-773e3b9b0a90
prob(bad_chip)

# ╔═╡ 413d05ce-ed65-406e-be97-32a2bad22d92
md"Now suppose we use our probe and observe a measurement of 1"

# ╔═╡ ff0aced3-466f-44e8-8d84-9521fca7c7c4
evidence = measure ==ₚ 1;

# ╔═╡ 233e3ee2-e56c-4c4d-a53d-83c144c303d0
md"Given the evidence, we can look at our conditional belief over which pin is faulty:"

# ╔═╡ 5774b63a-d492-490e-862a-0c3645722885
histogram(randsample(cnd(faulty_pin, evidence), 1000))

# ╔═╡ c6d93cdd-a2bd-4e5b-b835-1509cc582978
md"And compute the conditional probability that it is indeed a bad chip (faulty pin == 1)"

# ╔═╡ 32b3da83-0fb4-4a99-9742-46f9ad13eb8b
conditional_bad_chip = cnd(bad_chip, evidence);

# ╔═╡ 5ddfb321-d696-4853-bb9a-f3b8593f074d
md"Given the evidence, the conditional probability the chip is bad is high, at:"

# ╔═╡ ff904fa3-898b-4c35-a1c4-8a08ca5f82df
prob_bad = prob(conditional_bad_chip)

# ╔═╡ ae28acc2-fb0f-4189-bdaa-574a3e090f75
md"The certification handbook says a chip can be ceritifed if there's less than a 20% chance that it is faulty"

# ╔═╡ dbe58fac-bc39-4ee1-8930-6bb4e8453317
can_certify(p) = p < 0.3

# ╔═╡ 03ac41b7-10fd-4466-976d-f2815fe655a2
md"Clearly, we cannot certify this chip:"

# ╔═╡ 6b9ac9fa-56c3-453f-aba7-69cd193d91c3
can_certify(prob_bad)

# ╔═╡ 9ef8cafd-49e5-439a-9723-479babd7e6d7
md"### A Noisier Probe
\"Not to worry!\" your colleague, Sneaky Simon says.  \"Just use the bad probe.\""

# ╔═╡ 8c128635-e46d-418e-8c51-d74fc55973e4
md"We'll use `intervene` in Omega to construct a different model where the error rate is different"

# ╔═╡ aa15703f-6137-4d67-bb25-bec070d3d193
error_prob_noisy = 0.9

# ╔═╡ da0608ac-94bd-4ba1-81be-1cde40bd4cae
conditional_bad_chip_noisy_probe = intervene(conditional_bad_chip, error_prob => error_prob_noisy);

# ╔═╡ 828161b0-c32e-4ae8-9206-1f7ce95dfda4
prob_bad_noisy = prob(conditional_bad_chip_noisy_probe)

# ╔═╡ 1e1a3659-0a82-46c1-9b6e-0f8de142f9be
md"Job done?"

# ╔═╡ b6470b88-3ac0-4ccf-81df-c6f80e409a64
can_certify(prob_bad_noisy)

# ╔═╡ abe64bc4-cdef-4cb5-80cc-2e99e792ecab
md"Apparently, Sneaky Simon was right.  Using the bad probe has allowed us to certify the chip.  Is this right?  If not, what has gone wrong?"

# ╔═╡ 672b6bfa-54ce-4b12-80f0-ee930a777cbc
probwow = pw(prob, rid(conditional_bad_chip_noisy_probe, (faulty_pin, alternative_pin)))

# ╔═╡ 36a6cb8a-f81e-4e05-9dd0-a475e88ab62c
# histogram(randsample(probwow, 100))

# ╔═╡ Cell order:
# ╟─daa9fd86-5a36-4aa2-842e-477cdb19b8d5
# ╟─62257a75-2949-4afc-8543-fe3fa4073be5
# ╟─53572568-9d02-465a-baba-1c624d79d4b5
# ╠═f0f80b1a-d090-11eb-2b0d-d11fd8196311
# ╠═ca32437b-65ce-401b-93fc-1f3b7011ae88
# ╠═4936c45a-a5bf-49cb-bfdc-c05883d5982b
# ╟─57c308fa-a2e7-49fb-b52d-41346ee7ea0d
# ╠═d3af8df6-5375-4a52-9432-41ef6227af0c
# ╠═b0769639-7b6a-4d95-bc8d-bdedd5d27c4f
# ╠═8e2fb0c5-ca50-4bcb-85bf-d78c06ea5653
# ╠═ad3ec928-dc20-4987-aaed-78e8c4141d3e
# ╟─87199546-b427-4908-9531-28d40995efa8
# ╠═4b8285ca-0fa8-4c38-a2a9-870169a2c1d7
# ╟─021546fa-267d-4789-b6a5-b69d7eea01e6
# ╠═0ed2799b-0e1b-434a-85be-99a3869a89c7
# ╟─505f497c-2852-44f2-bf17-8bfb12886554
# ╠═fc03484c-206d-4c3d-90f9-773e3b9b0a90
# ╟─413d05ce-ed65-406e-be97-32a2bad22d92
# ╠═ff0aced3-466f-44e8-8d84-9521fca7c7c4
# ╟─233e3ee2-e56c-4c4d-a53d-83c144c303d0
# ╠═5774b63a-d492-490e-862a-0c3645722885
# ╟─c6d93cdd-a2bd-4e5b-b835-1509cc582978
# ╠═32b3da83-0fb4-4a99-9742-46f9ad13eb8b
# ╟─5ddfb321-d696-4853-bb9a-f3b8593f074d
# ╠═ff904fa3-898b-4c35-a1c4-8a08ca5f82df
# ╟─ae28acc2-fb0f-4189-bdaa-574a3e090f75
# ╠═dbe58fac-bc39-4ee1-8930-6bb4e8453317
# ╟─03ac41b7-10fd-4466-976d-f2815fe655a2
# ╠═6b9ac9fa-56c3-453f-aba7-69cd193d91c3
# ╟─9ef8cafd-49e5-439a-9723-479babd7e6d7
# ╟─8c128635-e46d-418e-8c51-d74fc55973e4
# ╠═aa15703f-6137-4d67-bb25-bec070d3d193
# ╠═da0608ac-94bd-4ba1-81be-1cde40bd4cae
# ╠═828161b0-c32e-4ae8-9206-1f7ce95dfda4
# ╟─1e1a3659-0a82-46c1-9b6e-0f8de142f9be
# ╠═b6470b88-3ac0-4ccf-81df-c6f80e409a64
# ╟─abe64bc4-cdef-4cb5-80cc-2e99e792ecab
# ╠═672b6bfa-54ce-4b12-80f0-ee930a777cbc
# ╠═36a6cb8a-f81e-4e05-9dd0-a475e88ab62c

max_stars_repo_name: zenna/expect

max_issues_repo_name: zenna/expect

max_forks_repo_name: zenna/expect


text:
module Operation
   ## Общие функции
   # Поменять местами строки
   function swaprows(A::Array{Float64,2},row::Int64,row2::Int64)
      for j = 1:size(A)[end]
         r = A[row,j]
         A[row,j] = A[row2,j]
         A[row2,j] = r
      end
      return A
   end
   #Поменять местами столбцы
   function swapcols(A::Array{Float64,2},col::Int64,col2::Int64)
      for i = 1:size(A)[1]
         c = A[i,col]
         A[i,col] = A[i,col2]
         A[i,col2] = c
      end
      return A
   end

   #Поиск элемента с указанным или отличным значением, возвращает координаты этого элемента
   function search(sr::Int64,A::Array{Float64,2},startr::Int64,startc::Int64,same::Bool)
      for i = startr+1:size(A)[1], j = startc+1:size(A)[end]
         if (same == true)
            if (A[i,j] == sr)
               return (i,j)
            end
         else
            if (A[i,j] != sr)
               return (i,j)
            end
         end
      end
      return 0
   end
   #Функция для представления в qml
   function transH(str, rows, cols)
      A = str2num(str, rows, cols)
      S = Array{Float64,2}(undef,size(A)[end],size(A)[1])
      for i=1:size(A)[1]
         for j=1:size(A)[end]
            S[j,i] = A[i,j]
         end
      end
      return S
   end
   #Строка в массив
   function str2num(A, rows, cols)
      c = 1
      S = Array{Float64,2}(undef,rows,cols)
      for i=1:rows
         for j=1:cols
            f = findnext(isequal(' '), A, c)
            if f == nothing
               help = A[c:length(A)]
               S[i,j] = tryparse(Float64,help)
            else
               help = A[c:f]
               S[i,j] = tryparse(Float64,help)
               c = f + 1
            end
         end
      end
      return S
   end

   ## Транспонирование
   function transfake(A,rows,cols)
      B = str2num(A,rows,cols)
      return B
   end
   ## Определитель
   # Функция получения матрицы без i-ой строки и j-ого столбца
   function dethelp(A::Array{Float64,2},str::Int64,stlb::Int64)
      S = Array{Float64,2}(undef,size(A)[1]-1,size(A)[end]-1)
      di=0
      for i = 1:size(A)[1]-1
         if (i == str)
            di = 1
         end
         dj = 0
         for j = 1:size(A)[1] - 1
            if (j == stlb)
               dj = 1
            end
             S[i,j] = A[i + di ,j + dj];
         end
      end
      return S
   end

   # Рекурсивная функция определителя
   function det(S,rows,cols)
      A = str2num(S,rows,cols)
      if (size(A)[end] != size(A)[1])
         d = "Матрица должна быть квадратной"
         return d
      end
      d = 0
      k = 1
      if (size(A)[end] == 1)
         d=A[1,1]
         return d
      end
      if (size(A)[end] == 2)
         d = A[1,1] * A[2,2] - A[2,1] * A[1,2]
         return d
      end
      if (size(A)[end] > 2)
         for i = 1:size(A)[1]
            S = dethelp(A,i,1)
            str = ""
            for i=1:length(S)
               str = str * string(S[i]) * " "
            end
            d = d + k * A[i,1] * det(str,size(S)[1],size(S)[end])
            k = -k
         end
      end

      return d
   end
   ## След
   function tr(A,rows,cols)
      B = str2num(A,rows,cols)
      if size(B)[end] != size(B)[1]
         g = "Матрица должна быть квадратной"
         return g
      end
      g = 0
      for i = 1:size(B)[1]
         g += B[i,i]
      end
      return g
   end
   ##Ранг
   function rang(str,rows,cols)
      A = str2num(str,rows,cols)
      iter = min(size(A)[end],size(A)[1])
      for i = 1:iter
         if (A[i,i] == 0)
            check = search(0,A,i,i,false)
            if (!(typeof((1,1))==typeof(check)))
               break
            end
            if (i != check[1])
               swaprows(A,i,check[1])
            end
            if (i != check[2])
               swapcols(A,i,check[2])
            end
         end
         tmp=A[i,i]
         for x = i:size(A)[end]
            A[i,x] = div(A[i,x],tmp)
         end
         for y = (i+1):size(A)[1]
            tmp = A[y,i]
            for x = i:size(A)[end]
               A[y,x] -= (A[i,x] * tmp)
            end
         end
         for x = (i+1):size(A)[end]
            tmp = A[i,x]
            for y = i:size(A)[1]
               A[y,x] -= (A[y,i] * tmp)
            end
         end
      end
      cnt = 0
      for i = 1:iter
         if (A[i,i] == 0)
            break
         else
            cnt += 1
         end
      end
      if (cnt == 0)
         cnt += 1
      end
      return cnt
   end
   ##Обратная матрица
   function obr(str,rows,cols)
      A = str2num(str,rows,cols)
      if rows != cols
         wrn = "Матрица должна быть квадратной"
         return wrn
      end
      if det(str,rows,cols) == 0
         wrn = "Определитель равен нулю"
         return wrn
      end
      B = convert(Array{Float64,2}, A)
      S = zeros(size(B)[1],size(B)[1])
      for i = 1:size(B)[1]
         S[i,i] = 1
      end
      for k = 1:size(B)[1]
         tmp = B[k,k]
         for j = 1:size(B)[1]
            B[k,j] = B[k,j]/tmp
            S[k,j] = S[k,j]/tmp
         end
         for i = k + 1:size(B)[1]
            tmp = B[i,k]
            for j = 1:size(B)[1]
               B[i,j] -= B[k,j] * tmp;
               S[i,j] -= S[k,j] * tmp;
            end
         end
      end
      k = size(B)[1]
      while (k > 1)
         i = k - 1
         while (i >= 1)
            tmp = B[i,k]
            for j=1:size(B)[1]
               B[i,j] -= B[k,j] * tmp;
               S[i,j] -= S[k,j] * tmp;
            end
            i -= 1
         end
         k -= 1
      end
      return S
   end
   ## Умножение
   function mul(strA,rowsA,colsA,strB,rowsB,colsB)
      A = str2num(strA,rowsA,colsA)
      B = str2num(strB,rowsB,colsB)
      if size(A)[end] != size(B)[1]
         wrn = "Количество столбцов первой матрицы должно быть равно количеству строк второй"
         return wrn
      end
      S = zeros(size(A)[1],size(B)[end])
      for i = 1:size(A)[1], j = 1:size(B)[end]
         S[i,j] = 0
         for k = 1:size(A)[end]
            S[i,j] += A[i,k] * B[k,j]
         end
      end
      return S
   end
   ##Сложение матриц
   function plus(strA,rowsA,colsA,strB,rowsB,colsB)
      A = str2num(strA,rowsA,colsA)
      B = str2num(strB,rowsB,colsB)
      if (rowsA != rowsB) || (colsA != colsB)
         wrn = "Размеры матриц должны совпадать"
         return wrn
      end
      S = Array{Float64,2}(undef,size(A)[1],size(A)[end])
      for i = 1:size(A)[1], j = 1:size(A)[end]
         S[i,j] = A[i,j] + B[i,j]
      end
      return S
   end

   ##Разность матриц
   function minus(strA,rowsA,colsA,strB,rowsB,colsB)
      A = str2num(strA,rowsA,colsA)
      B = str2num(strB,rowsB,colsB)
      if (rowsA != rowsB) || (colsA != colsB)
         wrn = "Размеры матриц должны совпадать"
         return wrn
      end
      S = Array{Float64,2}(undef,size(A)[1],size(A)[end])
      for i = 1:size(A)[1], j = 1:size(A)[end]
         S[i,j] = A[i,j] - B[i,j]
      end
      return S
   end
   ##Перевод десятичной дроби в обычную
   function float_to_frac(x,error=0.00000001)
      n = convert(Int64,round(x,RoundDown))
      x -= n
      if x < error
         return n
      elseif 1 - error < x
         return n+1
      end
      lower_n = 0
      lower_d = 1
      upper_n = 1
      upper_d = 1
      while (true)
         middle_n = lower_n + upper_n
         middle_d = lower_d + upper_d
         if middle_d * (x + error) < middle_n
            upper_n = middle_n
            upper_d = middle_d
         elseif middle_n < (x - error) * middle_d
            lower_n = middle_n
            lower_d = middle_d
         else
            s = ""
            help = n * middle_d + middle_n
            s = string(help) * "/" * string(middle_d)
            return s
         end
      end
   end
end

max_stars_repo_name: NikitaKutelev/kurs

max_issues_repo_name: NikitaKutelev/kurs

max_forks_repo_name: NikitaKutelev/kurs


text:
#!/bin/bash
# -*- mode: julia -*-
#=
# A hack to pass default options to julia:
exec julia --color=yes --startup-file=no "${BASH_SOURCE[0]}" "$@"
=#

#=
CLI to run pytest within Julia process.

All arguments are passed to pytest.

Examples:

    cd HERE
    ./pytest.jl
    ./pytest.jl -x --pdb
    ./pytest.jl pybase_jl/tests/test_wrappers.py
=#

# Do not use matplotlib GUI backend during tests.
ENV["MPLBACKEND"] = "agg"

using PyBase
code = PyBase.pytest(
    `$ARGS`;
    inprocess = true,
    # revise = false,
    check = false,
)
exit(code)


text:
dtc = 0.02;                                                                         # Euler integration time interval [s]
dtr = 0.1;                                                                          # replanning time interval [s]
dtexec = [0.0, 0.001, 0.002, 0.004, 0.008, 0.016, 0.02, 0.04, 0.08];                # control insertion duration [s]
tcalc = 0.1;                                                                        # pre-allocated control computation time [s] (< dtr)
u_norm_max = 5.0;                                                                   # maximum control norm [m/s^2]
u_nominal_base = [0.0, 0.0];                                                        # nominal control base.
#β = 1.6;                                                                            # penalty for waiting time
improvement_threshold = 0.0;
constraint_time = nothing
u_nominal_cand = append!([u_nominal_base],
                         [round.([a*cos(deg2rad(θ)), a*sin(deg2rad(θ))], digits=5)
                          for a = [2., 4.] for θ = 0.:45.:(360. - 45.)])            # nominal control candidate value [ax, ay] [m/s^2]
nominal_search_depth = 1;
#improvement_threshold = 0.1;
#constraint_time = tcalc + dtr;


text:
# These functions are needed by dns.jl but are not part of julia.Base.
# Function ndgrid is taken from julia examples.
# fftfreq is implemented following scipy/numpy.

module utils

"numpy.mgrid[v1, v2]"
function ndgrid{T}(v1::AbstractVector{T}, v2::AbstractVector{T})
    m, n = length(v1), length(v2)
    v1 = reshape(v1, m, 1)
    v2 = reshape(v2, 1, n)
    (repmat(v1, 1, n), repmat(v2, m, 1))
end

"Helper"
function ndgrid_fill(a, v, s, snext)
    for j = 1:length(a)
        a[j] = v[div(rem(j-1, snext), s)+1]
    end
end

"numpy.mgrid[v1, v2, v3, ...]"
function ndgrid{T}(vs::AbstractVector{T}...)
    n = length(vs)
    sz = map(length, vs)
    out = ntuple(i->Array{T}(sz), n)
    s = 1
    for i=1:n
        a = out[i]::Array
        v = vs[i]
        snext = s*size(a,i)
        ndgrid_fill(a, v, s, snext)
        s = snext
    end
    out
end

"numpy.fft.fftfreq"
function fftfreq(n::Int, d::Real=1.0)
    val = 1.0/(n*d)
    results = Array{Int}(n)
    N = (n-1)÷2 + 1
    p1 = 0:(N-1)
    results[1:N] = p1
    p2 = -n÷2:-1
    results[N+1:end] = p2
    results * val
end

"numpy.fft.rfftfreq"
function rfftfreq(n::Int, d::Real=1.0)
    val = 1.0/(n*d)
    N = (n)÷2 + 1
    results = Array{Int}(N)
    results[1:N] = 0:(N-1)
    results * val
end

"Modifies while loop such that ivar and avar returns mpi-reduced minimal and
maximal times that it takes to execute once the body of the loop."
macro mpitime(loop, ivar, avar)
    _mpi_time(loop, ivar, avar)
end

function _mpi_time(loop, ivar, avar)
    loop.head != :while && error("Not a while loop")

    tstep = symbol(string(ivar), string(avar))

    # Modify internals of the loop
    internals = loop.args[2]
    internals.head == :quote && error("I did not expect this")
    insert!(internals.args, 1, :(tic()))          # Time the internals
    push!(internals.args, :($tstep = toq()))
    push!(internals.args, :($ivar = min($tstep, $ivar)))  # Update min/max
    push!(internals.args, :($avar = max($tstep, $avar)))

    body = quote
        $ivar, t_max = NaN, 0
        $loop
        $ivar = MPI.Reduce($ivar, MPI.MIN, 0, MPI.COMM_WORLD)
        $avar = MPI.Reduce($avar, MPI.MAX, 0, MPI.COMM_WORLD)
    end

    ex = Expr(:escape, body)
    ex
end

view(k::Int, N::Int=4) = [fill(Colon(), N-1)..., k]

export ndgrid, fftfreq, rfftfreq, view, @mpitime

end


text:
using Test
@testset "ODE" begin let
	import DifferentiableBackwardEuler
	DBE = DifferentiableBackwardEuler
	using DifferentialEquations
	import LinearAlgebra
	import SparseArrays
	import Zygote

	f(u, p, t) = f_u(u, p, t) * u
	f_u(u, p, t) = SparseArrays.spdiagm(0=>p)
	f_p(u, p, t) = SparseArrays.spdiagm(0=>u)
	f_t(u, p, t) = zero(u)

	#test the forward solver analytically
	n = 2
	p = collect(1.0:n)
	u0 = ones(n)
	us = [u0]
	numsteps = 10 ^ 3
	ts = range(0, 1; length=numsteps)
	for i = 1:length(ts) - 1
		push!(us, DBE.step(us[end], ts[i + 1] - ts[i], f, f_u, f_p, f_t, p, ts[i + 1]))
	end
	@test isapprox(exp.(p), us[end]; atol=1e2 / numsteps, atol=1e2 / numsteps)

	#test gradients involving step(...)
	function naivegradient(f, x0s...; delta::Float64=1e-8)
		f0 = f(x0s...)
		gradient = map(x->zeros(size(x)), x0s)
		for j = 1:length(x0s)
			x0 = copy(x0s[j])
			for i = 1:length(x0)
				x = copy(x0)
				x[i] += delta
				xs = (x0s[1:j - 1]..., x, x0s[j + 1:length(x0s)]...)
				fval = f(xs...)
				gradient[j][i] = (fval - f0) / delta
			end
		end
		return gradient
	end
	g(p, u0) = sum(DBE.step(u0, ts[2] - ts[1], f, f_u, f_p, f_t, p, ts[2]))
	dgdp_zygote = Zygote.gradient((p, u0)->g(p, u0), p, u0)
	dgdp_naive = naivegradient((p, u0)->g(p, u0), p, u0)
	@test isapprox(dgdp_zygote[1], dgdp_naive[1]; atol=1e-6, rtol=1e-6)
	@test isapprox(dgdp_zygote[2], dgdp_naive[2]; atol=1e-6, rtol=1e-6)

	#test gradients involving steps(..., ts)
	numsteps = 10 ^ 1
	ts = collect(range(0, 1e0; length=numsteps))
	g(p, u0) = sum(DBE.steps(u0, f, f_u, f_p, f_t, p, ts)[:, end])
	@test isapprox(sum(exp.(p * ts[end])), g(p, u0); atol=1e2 / numsteps, rtol=1e2 / numsteps)
	dgdp_zygote = Zygote.gradient((p, u0)->g(p, u0), p, u0)
	dgdp_naive = naivegradient((p, u0)->g(p, u0), p, u0)
	@test isapprox(dgdp_zygote[1], dgdp_naive[1]; atol=1e-4, rtol=1e-4)
	@test isapprox(dgdp_zygote[2], dgdp_naive[2]; atol=1e-4, rtol=1e-4)

	#test the forward solver against DifferentialEquations.jl
	u0 = [1 / 2, 1 / 3]
	f(u, p, t) = f_u(u, p, t) * u
	f_u(u, p, t) = LinearAlgebra.Diagonal(copy(p))
	tspan = (0.0, 1.0)
	prob = ODEProblem(f, u0, tspan, p)
	sol = solve(prob, ImplicitEuler())
	dbesol = DBE.steps(u0, f, f_u, nothing, nothing, p, sol.t)
	for i = 1:length(sol.t)
		@test isapprox(dbesol[:, i], sol[:, i])
	end

	#test the forward runs involving steps(..., t0, final) are working
	dbesol = DBE.steps(u0, f, f_u, nothing, nothing, p, tspan[1], tspan[2])
	for i = 1:length(sol.t)
		@test isapprox(dbesol[:, i], sol[:, i])
	end

	#test gradients involving steps(..., t0, tfinal)
	t0 = 0.0
	tfinal = 1e0
	g(p, u0) = sum(DBE.steps(u0, f, f_u, f_p, f_t, p, t0, tfinal)[:, end])
	g_val = g(p, u0)
	@test isapprox(sum(u0 .* exp.(p * ts[end])), g_val; atol=1e2 / numsteps, rtol=1e2 / numsteps)
	dgdp_zygote = Zygote.gradient(g, p, u0)
	dgdp_naive = naivegradient(g, p, u0; delta=1e-2)
	@test isapprox(dgdp_zygote[2], dgdp_naive[2]; atol=1e-4, rtol=1e-4)
	@test isapprox(dgdp_zygote[1], dgdp_naive[1]; atol=1e-2, rtol=1e-2)
end end#end begin and let


text:
using Plots; pyplot()
using Reproduce
using FileIO
using Statistics
using ProgressMeter
using JLD2
using LaTeXStrings
using Statistics

data_key = :oned_tmaze_old_error
folder_name = "oned_control"
data_home = "../data/OneDTMaze_Control"
experiment_folders = [data_home]
include("./plot_utils.jl")
GPU = GeneralPlotUtils
LU = LabelUtils

ic = ItemCollection(joinpath(experiment_folders[1], "data"))

algo_divisor_keys = ["behaviour_learner", "demon_learner"]
sweep_params = ["eta"]

algo_specs_full = GPU.split_algo(ic, algo_divisor_keys)

all_algos_ics = [search(ic,algo_spec) for algo_spec in algo_specs_full]

#Plot Sensitivity of eta.
p = plot()
for algo in all_algos_ics
    eta_vals = diff(algo)["eta"]

    line = []
    err_bars = []
    for eta in eta_vals
        algo_for_eta = search(algo, Dict("eta" => eta))
        best_algo_for_eta = GPU.get_best_final_perf(algo_for_eta, ["alpha_init"], data_key,0.1)

        data = GPU.load_results(best_algo_for_eta,data_key)

        # rmse,rmse_std = GPU.get_stats(data)
        rmse = mean(sum(data, dims = 1),dims = [2,3])[]
        rmse_std = std(mean(data, dims = [1,2]),dims = 3)[]

        num_runs = size(data)[3]

        CI = 1.97 * rmse_std / sqrt(num_runs)

        push!(line, mean(rmse))
        push!(err_bars, CI)
    end
    plot!(line, yerror=err_bars)
end
display(p)

max_stars_repo_name: MatthewMcLeod/curiosity

max_issues_repo_name: MatthewMcLeod/curiosity

max_forks_repo_name: MatthewMcLeod/curiosity


text:
# ------------------------------------------------------------
# NOTE BROKEN AND POSSIBLY BEYOND REPAIR                     |
# explosion of likelihoods seems to make inference impossible|
# ------------------------------------------------------------

SRC_DIR = joinpath(Base.source_dir(), "..", "..", "src")
OUT_DIR = joinpath(Base.source_dir(), "..", "..", "output")
mkpath(OUT_DIR)

#include(joinpath(SRC_DIR, "BridgeSDEInference.jl"))
#using Main.BridgeSDEInference
include(joinpath(SRC_DIR, "BridgeSDEInference_for_tests.jl"))

using StaticArrays
using Distributions # to define priors
using Random        # to seed the random number generator

# Let's generate the data
# -----------------------
using Bridge
#import Main.BridgeSDEInference.forcedSolve
include(joinpath(SRC_DIR, "auxiliary", "data_simulation_fns.jl"))
Random.seed!(4)
θ₀ = [0.05, √2.0]
Pˣ = RadialOU(θ₀...)

x0, dt, T = ℝ{1}(0.5), 1/5000, 1.0
tt = 0.0:dt:T
XX, _ = simulateSegment(0.0, x0, Pˣ, tt)

num_obs = 11
skip = div(length(tt), num_obs-1)
obs_time, obs_vals = collect(tt)[1:skip:end], XX.yy[1:skip:end]
P̃ = [RadialOUAux(θ₀..., t₀, u[1], T, v[1]) for (t₀,T,u,v)
     in zip(obsTime[1:end-1], obsTime[2:end], obsVals[1:end-1], obsVals[2:end])]
L = @SMatrix [1.]
Σ = @SMatrix [10^(-3)]

setup = MCMCSetup(Pˣ, P̃, PartObs())
set_observations!(setup, [L for _ in P̃], [Σ for _ in P̃])
set_imputation_grid!(setup, 1/1000)
set_transition_kernels!(setup,
                        [RandomWalk([0.002, 0.1], [true, true]),
                         RandomWalk([0.002, 0.1], [true, true])],
                        0.975, true, [[1],[2]],
                        (MetropolisHastingsUpdt(),
                         MetropolisHastingsUpdt(),
                        ))
set_priors!(setup, Priors((ImproperPrior(), ImproperPrior())), KnownStartingPt(x0))
set_mcmc_params!(setup, 1*10^1, 1*10^0, 10^2, 10^0, 0)
set_blocking!(setup)
set_solver!(setup, Vern7(), NoChangePt())
initialise!(eltype(x0), setup)

Random.seed!(4)
out, elapsed = @timeit mcmc(setup)
display(out.accpt_tracker)

include(joinpath(SRC_DIR, DIR, "plotting_fns.jl"))
plot_chains(out; truth=[0.05, √2.0])
plot_paths(out; obs=(times=obs_time[2:end], vals=obs_vals[2:end], indices=1))


text:
struct Static{T}
  α::T
end

find_steplength(ls::Static, φ, λ::T) where T = T(ls.α), φ(T(ls.α)), true


text:
# Iterative distributed computing example
# To run with 5 workers (rank 0 is the coordinator):
# mpirun -n 6 julia --project ./examples/iterative_example.jl

using MPI, MPIAsyncPools

MPI.Init()
const comm = MPI.COMM_WORLD
const root = 0
const rank = MPI.Comm_rank(comm)
const isroot = MPI.Comm_rank(comm) == root
const data_tag = 0
const control_tag = 1
const COORDINATOR_TX_BYTES = 100
const WORKER_TX_BYTES = 100

function coordinator_main()

    # worker pool and communication buffers
    nworkers = MPI.Comm_size(comm) - 1
    nwait = 1
    pool = MPIAsyncPool(nworkers)

    # combined buffer for data received from all workers
    recvbuf = zeros(UInt8, nworkers*WORKER_TX_BYTES)

    # buffer for data being broadcast to the workers
    sendbuf = zeros(UInt8, COORDINATOR_TX_BYTES)

    # communication buffers for use internally in MPIAsyncPool
    isendbuf = similar(sendbuf, nworkers*length(sendbuf))
    irecvbuf = similar(recvbuf)

    # views into recvbuf corresponding to each worker
    n = div(length(recvbuf), nworkers)
    recvbufs = [view(recvbuf, (i-1)*n+1:i*n) for i in 1:nworkers]

    for epoch in 1:10
        sends = Vector{UInt8}("hello from coordinator on $(gethostname()), epoch $epoch")
        sendbuf[1:length(sends)] .= sends
        repochs = asyncmap!(pool, sendbuf, recvbuf, isendbuf, irecvbuf, comm; epoch, nwait, tag=data_tag)
        for i in 1:nworkers
            if repochs[i] == epoch
                recs = String(recvbufs[i])
                println("[coordinator]\t\treceived from worker $i:\t\t$recs")
            end
        end
    end

    # signal all workers to close
    waitall!(pool, recvbuf, irecvbuf)
    for i in pool.ranks
        MPI.Send(zeros(1), i, control_tag, comm)
    end
end

function worker_main()

    # control channel, to tell the workers when to exit
    crreq = MPI.Irecv!(zeros(1), root, control_tag, comm)

    # buffer for data received from the coordinator
    recvbuf = zeros(UInt8, COORDINATOR_TX_BYTES)

    # buffer for data being sent to the coordinator
    sendbuf = zeros(UInt8, WORKER_TX_BYTES)

    # handle messages received from the coordinator until a message is received on the crreq channel
    i = 0
    while true
        rreq = MPI.Irecv!(recvbuf, root, data_tag, comm)
        index, _ = MPI.Waitany!([crreq, rreq])
        if index == 1 # exit message on control channel
            MPI.Cancel!(rreq) # mark the rreq for cancellation
            MPI.Test!(rreq) # cleanup the data receive request
            break
        end
        sleep(rand()) # simulate performing a computation
        recs = String(recvbuf[:])
        println("[worker $rank]\t\treceived from coordinator\t$recs")
        sends = Vector{UInt8}("hello from worker $rank on $(gethostname()), iteration $i")
        sendbuf[1:length(sends)] .= sends
        MPI.Send(sendbuf, root, data_tag, comm)
        i += 1
    end
end

if isroot
    coordinator_main()
else
    worker_main()
end
MPI.Barrier(comm)
MPI.Finalize()


text:
"""
     AbstractMutableComp

Abstract base type for computation units (read layers) which may mutate.
"""
abstract type AbstractMutableComp end
Base.Broadcast.broadcastable(m::AbstractMutableComp) = Ref(m)

# Generic forwarding methods. Just implement layer(t::Type) and wrapped(t::Type) to enable

# Not possible in julia <= 1.1. See #14919
# (m::AbstractMutableComp)(x...) = layer(m)(x...)
layertype(m::AbstractMutableComp) = layertype(layer(m))

NaiveNASlib.nin(m::AbstractMutableComp) = nin(layer(m))
NaiveNASlib.nout(m::AbstractMutableComp) = nout(layer(m))

# Leave some room to override clone
NaiveNASlib.clone(m::AbstractMutableComp;cf=clone) = typeof(m)(map(cf, getfield.(m, fieldnames(typeof(m))))...)

NaiveNASlib.mutate_inputs(m::AbstractMutableComp, inputs::AbstractArray{<:Integer,1}...) = mutate_inputs(wrapped(m), inputs...)
NaiveNASlib.mutate_outputs(m::AbstractMutableComp, outputs) = mutate_outputs(wrapped(m), outputs)

mutate_weights(m::AbstractMutableComp, w) = mutate_weights(wrapped(m), w)

NaiveNASlib.minΔninfactor(m::AbstractMutableComp) = minΔninfactor(layertype(m), layer(m))
NaiveNASlib.minΔnoutfactor(m::AbstractMutableComp) = minΔnoutfactor(layertype(m), layer(m))

NaiveNASlib.compconstraint!(s, m::AbstractMutableComp, data) = NaiveNASlib.compconstraint!(s, layertype(m), data)

"""
    MutableLayer

Wraps a layer in order to allow for mutation as layer structures are typically immutable
"""
mutable struct MutableLayer <: AbstractMutableComp
    layer
end
(m::MutableLayer)(x...) = layer(m)(x...)
wrapped(m::MutableLayer) = m.layer
layer(m::MutableLayer) = wrapped(m)
layertype(m::MutableLayer) = layertype(layer(m))

function NaiveNASlib.mutate_inputs(m::MutableLayer, inputs::AbstractArray{<:Integer,1}...)
    @assert length(inputs) == 1 "Only one input per layer!"
    mutate(layertype(m), m, inputs=inputs[1])
end

NaiveNASlib.mutate_outputs(m::MutableLayer, outputs) = mutate(layertype(m), m, outputs=outputs)

mutate_weights(m::MutableLayer, w) = mutate(layertype(m), m, other=w)

mutate(m::MutableLayer; inputs, outputs, other = l -> ()) = mutate(layertype(m), m, inputs=inputs, outputs=outputs, other=other)

function mutate(::FluxParLayer, m::MutableLayer; inputs=1:nin(m), outputs=1:nout(m), other= l -> ())
    l = layer(m)
    otherdims = other(l)
    w = select(weights(l), outdim(l) => outputs, indim(l) => inputs, otherdims...)
    b = select(bias(l), 1 => outputs)
    newlayer(m, w, b, otherpars(other, l))
end
otherpars(o, l) = ()

function mutate(::FluxDepthwiseConv, m::MutableLayer; inputs=1:nin(m), outputs=1:nout(m), other= l -> ())
    l = layer(m)
    otherdims = other(l)
    weightouts = map(Iterators.partition(outputs, length(inputs))) do group
        all(group .< 0) && return group[1]
        return (maximum(group) - 1) ÷ length(inputs) + 1
    end

    w = select(weights(l), outdim(l) => weightouts, indim(l) => inputs, otherdims...)
    b = select(bias(l), 1 => outputs)
    newlayer(m, w, b, otherpars(other, l))
end

function mutate(t::FluxRecurrent, m::MutableLayer; inputs=1:nin(m), outputs=1:nout(m), other=missing)
    l = layer(m)
    outputs_scaled = mapfoldl(vcat, 1:outscale(l)) do i
        offs = (i-1) * nout(l)
        return map(x -> x > 0 ? x + offs : x, outputs)
    end

    wi = select(weights(l), outdim(l) => outputs_scaled, indim(l) => inputs)
    wh = select(hiddenweights(l), 1 => outputs_scaled, 2 => outputs)
    b = select(bias(l), 1 => outputs_scaled)
    mutate_recurrent_state(t, m, outputs, wi, wh, b)
end

function mutate_recurrent_state(::FluxRecurrent, m::MutableLayer, outputs, wi, wh, b)
    l = layer(m)
    h = select(hiddenstate(l), 1 => outputs)
    s = select(state(l), 1 => outputs)

    cellnew = setproperties(layer(m).cell, (Wi=param(wi), Wh=param(wh), b = param(b), h = param(h)))
    lnew = setproperties(layer(m), (cell=cellnew, state = param(s)))
    m.layer = lnew
end

function mutate_recurrent_state(::FluxLstm, m::MutableLayer, outputs, wi, wh, b)
    l = layer(m)
    hcurr, scurr = hiddenstate(l), state(l)
    hc = select.(hcurr, repeat([1 => outputs], length(hcurr)))
    s = select.(scurr, repeat([1 => outputs], length(scurr)))

    cellnew = setproperties(layer(m).cell, (Wi=param(wi), Wh=param(wh), b = param(b), h = param(hc[1]), c = param(hc[2])))
    lnew = setproperties(layer(m), (cell=cellnew, state = tuple(param.(s)...)))
    m.layer = lnew

    return (h = hc[1], c = hc[2]), tuple(param.(s))
end

function mutate(t::FluxParInvLayer, m::MutableLayer; inputs=missing, outputs=missing, other=missing)
    @assert any(ismissing.((inputs, outputs))) || inputs == outputs "Try to mutate $inputs and $outputs for invariant layer $(m)!"
    ismissing(inputs) || return mutate(t, m, inputs)
    ismissing(outputs) || return mutate(t, m, outputs)
end

function mutate(::FluxDiagonal, m::MutableLayer, inds)
    l = layer(m)
    w = select(weights(l), 1 => inds)
    b = select(bias(l), 1 => inds)
    newlayer(m, w, b)
end

function mutate(::FluxLayerNorm, m::MutableLayer, inds)
    # LayerNorm is only a wrapped Diagonal. Just mutate the Diagonal and make a new LayerNorm of it
    proxy = MutableLayer(layer(m).diag)
    mutate(proxy, inputs=inds, outputs=inds, other=l->())
    m.layer = LayerNorm(layer(proxy))
end

function mutate(::FluxParNorm, m::MutableLayer, inds)
    # Good? bad? I'm the guy who assumes mean and std type parameters will be visited in a certain order and uses a closure for that assumption
    ismean = false
    parselect = function(x)
        ismean = !ismean
        return select(x, 1 => inds; insval = (ismean ? 0 : 1))
    end
    m.layer = Flux.mapchildren(parselect, m.layer)
end

function mutate(::FluxGroupNorm, m::MutableLayer, inds)

    l = m.layer
    ngroups = l.G
    nchannels = length(inds)
    # Make a "best fit" to new group size, but don't allow 1 as group size
    g_alts = filter(ga -> ga > 1, findall(r -> r == 0, nchannels .% (1:nchannels)))
    ngroups = g_alts[argmin(abs.(g_alts .- ngroups))]

    nchannels_per_group = div(nchannels, ngroups)
    # Map indices to (new) groups
    inds_groups = ceil.(Int, map(ind -> ind > 0 ? ind / nchannels_per_group : ind, inds))
    # TODO: Select the most commonly occuring index in each column (except -1)
    inds_groups = reshape(inds_groups, nchannels_per_group, ngroups)[1,:]

    sizetoinds = Dict(nin(l) => inds, l.G => inds_groups)

    ismean = false
    parselect = function(x)
        ismean = !ismean
        return select(x, 1 => sizetoinds[length(x)]; insval = (ismean ? 0 : 1))
    end
    m.layer = Flux.mapchildren(parselect, m.layer)
    m.layer.G = ngroups
end

newlayer(m::MutableLayer, w, b, other=nothing) = m.layer = newlayer(layertype(m), m, w, b, other)

newlayer(::FluxDense, m::MutableLayer, w, b, other) = Dense(param(w), param(b), deepcopy(layer(m).σ))
newlayer(::FluxConvolutional, m::MutableLayer, w, b, other) = setproperties(layer(m), (weight=param(w), bias=param(b), σ=deepcopy(layer(m).σ), other...))
newlayer(::FluxDiagonal, m::MutableLayer, w, b, other) = Flux.Diagonal(param(w), param(b))

"""
    LazyMutable

Lazy version of MutableLayer in the sense that it does not perform any mutations
until invoked to perform a computation.

This is done by calling the method trigger_mutation on the mutable, meaning that
there must exist a method for the the mutable type in order for something to
happen.

This reduces the need to garbage collect intermediate parameters when both inputs
and outputs are mutated.

Also useable for factory-like designs where the actual layers of a computation graph
are not instantiated until the graph is used.

# Examples
```julia-repl
julia> using NaiveNASflux

julia> struct DenseConfig end

julia> lazy = LazyMutable(DenseConfig(), 2,3)
LazyMutable(DenseConfig(), 1:2, 1:3)

julia> function NaiveNASflux.dispatch(m::LazyMutable, ::DenseConfig, x)
       m.mutable = MutableLayer(Dense(nin(m), nout(m), relu))
       return m.mutable(x)
       end

julia> lazy(Float32[1,2])
Tracked 3-element Array{Float32,1}:
 1.568902f0
 0.556749f0
 2.0417972f0

julia> lazy
LazyMutable(MutableLayer(Dense(2, 3, NNlib.relu)), 1:2, 1:3)
```
"""
mutable struct LazyMutable <: AbstractMutableComp
    mutable
    inputs::AbstractVector{<:Integer}
    outputs::AbstractVector{<:Integer}
    other
end
LazyMutable(m::AbstractMutableComp) = LazyMutable(m, nin(m), nout(m))
LazyMutable(m, nin::Integer, nout::Integer) = LazyMutable(m, 1:nin, 1:nout, m -> ())

wrapped(m::LazyMutable) = m.mutable
layer(m::LazyMutable) = layer(wrapped(m))

treelike_fields(T::Type{LazyMutable}) = (:mutable,)

(m::LazyMutable)(x...) = dispatch!(m, m.mutable, x...)
dispatch!(m::LazyMutable, mutable::AbstractMutableComp, x...) = mutable(x...)

NaiveNASlib.nin(m::LazyMutable) = length(m.inputs)
NaiveNASlib.nout(m::LazyMutable) = length(m.outputs)

function NaiveNASlib.mutate_inputs(m::LazyMutable, inputs::AbstractArray{<:Integer,1}...)
    @assert length(inputs) == 1 "Only one input per layer!"
    m.inputs == inputs[1] && return

    m.mutable = ResetLazyMutable(trigger_mutation(m.mutable))
    m.inputs = select(m.inputs, 1 => inputs[1], insval=-1)
end

function NaiveNASlib.mutate_outputs(m::LazyMutable, outputs::AbstractArray{<:Integer,1})
    outputs == m.outputs && return

    m.mutable = ResetLazyMutable(trigger_mutation(m.mutable))
    m.outputs = select(m.outputs, 1=>outputs, insval = -1)
end

function mutate_weights(m::LazyMutable, w)
    m.other == w && return

    m.mutable = ResetLazyMutable(trigger_mutation(m.mutable))
    m.other = w
end

NaiveNASlib.mutate_inputs(m::LazyMutable, nin::Integer...) = mutate_inputs(m, trunc_or_pad.(length(m.inputs), nin)...)

NaiveNASlib.mutate_outputs(m::LazyMutable, nout::Integer) = mutate_outputs(m, trunc_or_pad(length(m.outputs), nout))

function trunc_or_pad(maxselect, size)
    res = -ones(Int, size)
    lastselected = min(size, maxselect)
    res[1:lastselected] = 1:lastselected
    return res
end

"""
    MutationTriggered

Dispatches mutation for LazyMutable.
"""
struct MutationTriggered
    wrapped
end
# Functionality is opt-in
trigger_mutation(m) = m
trigger_mutation(m::AbstractMutableComp) = MutationTriggered(m)

function dispatch!(lm::LazyMutable, m::MutationTriggered, x...)
    mutate(m.wrapped; inputs=lm.inputs, outputs=lm.outputs, other=lm.other)
    lm.mutable = m.wrapped
    return lm(x...)
end

layer(m::MutationTriggered) = layer(m.wrapped)
layertype(m::MutationTriggered) = layertype(layer(m))

Flux.@treelike MutationTriggered

"""
    ResetLazyMutable

Reset a `LazyMutable` when dispatching.
"""
struct ResetLazyMutable
    wrapped
end
ResetLazyMutable(r::ResetLazyMutable) = r

function dispatch!(lm::LazyMutable, m::ResetLazyMutable, x...)
    lm.mutable = m.wrapped
    output = lm(x...)
    lm.inputs = 1:nin(lm)
    lm.outputs = 1:nout(lm)
    lm.other = m -> ()
    return output
end

layer(m::ResetLazyMutable) = layer(m.wrapped)
layertype(m::ResetLazyMutable) = layertype(layer(m))

Flux.@treelike ResetLazyMutable

"""
    NoParams

Ignores size mutation.

Useful for layers which don't have parameters.
"""
struct NoParams
    layer
end

(i::NoParams)(x...) = layer(i)(x...)
layer(i::NoParams) = i.layer
layertype(i::NoParams) = layertype(layer(i))

LazyMutable(m::NoParams) = m

function NaiveNASlib.mutate_inputs(::NoParams, inputs) end
function NaiveNASlib.mutate_outputs(::NoParams, outputs) end
function mutate_weights(::NoParams, w) end
NaiveNASlib.minΔninfactor(m::NoParams) = minΔninfactor(layertype(m), layer(m))
NaiveNASlib.minΔnoutfactor(m::NoParams) = minΔnoutfactor(layertype(m), layer(m))


text:
using SFGAnalysis
using Documenter

makedocs(;
    modules=[SFGAnalysis],
    authors="Michael Lackner <lacknersmichael@gmail.com",
    repo="https://github.com/MLackner/SFGAnalysis.jl/blob/{commit}{path}#L{line}",
    sitename="SFGAnalysis.jl",
    format=Documenter.HTML(;
        prettyurls=get(ENV, "CI", "false") == "true",
        assets=String[],
    ),
    pages=[
        "Home" => "index.md",
    ],
)
deploydocs(
    repo   = "github.com/MLackner/SFGAnalysis.jl",
    branch = "gh-pages",
    devbranch = "master",
    devurl = "dev",
)


text:
import GraphIdx: neighbors_lambda, Grid.GridGraph

a = neighbors_lambda(GridGraph(2, 2, 2))
@time a = neighbors_lambda(GridGraph(1000, 1000, 2))

max_stars_repo_name: EQt/graphidx

max_issues_repo_name: EQt/graphidx

max_forks_repo_name: EQt/graphidx


text:
# For recreating ComponentArrays with a different copy_with_eltype
function copy_with_eltype(x, T)
    x_new = similar(x, T)
    x_new .= x
    return x_new
end


text:
include("maxsat.jl")

mutable struct CFiniteSolver <: NLSolver
    vars::Dict{Symbol,Type}
    hard_clauses::ClauseSet
    cfin_clauses::ClauseSet
    soft_clauses::ClauseSet
    function CFiniteSolver()
        new(Dict(), ClauseSet(), ClauseSet(), ClauseSet())
    end
end

function add!(s::CFiniteSolver, cs::ClauseSet)
    for c in cs
        if length(c) == 1 && first(c) isa CFiniteConstraint{EQ}
            s.cfin_clauses &= c
            # limit = min(3, length(first(c).us))
            s.hard_clauses &= expand(first(c), 1)
        else
            cl = Clause()
            for cstr in c
                cl |= expand(cstr)
            end
            s.hard_clauses &= cl
        end
    end
end
add_soft!(s::CFiniteSolver, cs::ClauseSet) = s.soft_clauses &= cs
add_vars!(s::CFiniteSolver, d::Dict{Symbol,Type}) = push!(s.vars, d...)

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

const Partition = Vector{Vector{Int}}
const SPartition = Set{Set{Int}}

struct PartitionManager
    stack::Stack{Partition}
    history::Set{SPartition}
    unsat_cores::Vector{SPartition}
end

PartitionManager() = PartitionManager(Stack{Partition}(), Set{SPartition}(), Vector{SPartition}())

function _already_visited(m::PartitionManager, P::Partition)
    S = Set(Set(x) for x in P)
    for core in m.unsat_cores
        if issubset(core, S)
            return true
        end
    end
    S in m.history
end
function _visit!(m::PartitionManager, P::Partition)
    S = Set(Set(x) for x in P)
    push!(m.history, S)
end

# function backtrack!(m::PartitionManager, level::Integer)
#     for i in level+1:length(m.stacks)
#         empty!(g_partition_histories[i])
#         empty!(g_partition_stacks[i])
#     end
# end

function Base.pop!(m::PartitionManager)
    while true
        isempty(m.stack) && return nothing
        P = pop!(m.stack)
        if !_already_visited(m, P)
            _visit!(m, P)
            return P
        end
    end
end

function Base.push!(m::PartitionManager, iter)
    for P in iter
        push!(m, P)
    end
end

function Base.push!(m::PartitionManager, P::Partition)
    push!(m.stack, P)
end

function learn_core!(m::PartitionManager, C::SPartition)
    @debug "learn $C"
    push!(m.unsat_cores, C)
end

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

function find_partition(model::Model, ms)
    ps = Dict{Z3Expr,Vector{Int}}()
    for (i, m) in enumerate(ms)
        mval = Z3.eval(model, m, false)
        if haskey(ps, mval)
            push!(ps[mval], i)
        else
            push!(ps, mval=>Int[i])
        end
    end
    values(ps)
end

function move_iterator(P::Partition, set_idx, elem)
    P = deepcopy(P)
    deleteat!(P[set_idx], findfirst(x->x==elem, P[set_idx]))
    deleteat!(P, findall(x->isempty(x), P))
    iter = ([
        (if i == j
            [Y; elem]
        else
            Y
        end) for (j, Y) in enumerate(P)
    ] for i = 1:length(P) if i != set_idx)
    # (i for i in iter if !already_visited(i))
    # @debug "" P elem [P; [[elem]]]
    Iterators.flatten((iter, [[P; [[elem]]]]))
end

function distinct_iterator(P::Partition, idx1, elem1, idx2, elem2)
    # display(P)
    P = deepcopy(P)
    deleteat!(P[idx1], findfirst(x->x==elem1, P[idx1]))
    deleteat!(P[idx2], findfirst(x->x==elem2, P[idx2]))
    deleteat!(P, findall(x->isempty(x), P))
    iter = ([
        (if i == j
            [Y; elem1; elem2]
        else
            Y
        end) for (j, Y) in enumerate(P)
    ] for i = 1:length(P))
    # (i for i in iter if !already_visited(i))
    iter
end

function learn_partitions(ms, us, partition, partition_map, core)
    # @debug "Learn next partitions" string(partition) partition_map core
    Iterators.flatten((if is_eq(x)
        # @debug "" partition_map[id(x)]
        (p1, i1), (p2, i2) = partition_map[id(x)]
        # index1, index2 = partition[p1][i1], partition[p2][i2]
        it1 = move_iterator(partition, p1, i1)
        it2 = move_iterator(partition, p2, i2)
        Iterators.flatten((it1, it2))
    elseif is_distinct(x)
        (p1, i1), (p2, i2) = partition_map[id(x)]
        x = distinct_iterator(partition, p1, i1, p2, i2)
        @debug "" collect(x) p1 i1 p2 i2
        x
    else
        error("No equality/inequality: $(x)")
    end for x in core))
end

function solve_constraints(s::Solver, mss, uss, partitions, current=1)
    @debug("Number of constraints to solve: $(length(mss))")
    @debug "" partitions
    m = PartitionManager()
    push!(m, partitions[current])
    # push(s)
    while true
        push(s)
        @debug "New iteration at level: $current ======================================="
        partition = pop!(m)
        isnothing(partition) && return false

        ms, us = mss[current], uss[current]
        assumptions = Z3Expr[]
        usums = Z3Expr[]
        partition_map = Dict{UInt32, Pair{Pair{Int,Int},Pair{Int,Int}}}()
        for (t, indices) in enumerate(partition)
            usum = us[indices[1]]
            # all m in one set of partition are equal
            for i = 2:length(indices)
                j, k = indices[i-1], indices[i]
                ass = (ms[j] == ms[k])
                push!(assumptions, ass)
                usum += us[k]
                @assert !haskey(partition_map, id(ass))
                partition_map[id(ass)] = (t=>j) => (t=>k)
            end
            # the sum of all u in one set of partition has to be 0
            push!(usums, usum == 0)
            # if there is more than one set in the partition,
            # then the m in different sets have to be different
            if t > 1
                j, k = first(partition[t-1]), first(indices)
                ass = ms[j] != ms[k]
                push!(assumptions, ass)
                @assert !haskey(partition_map, id(ass))
                partition_map[id(ass)] = (t-1=>j) => (t=>k)
            end
        end

        for su in usums
            add(s, su)
        end
        res = check(s, assumptions)
        @debug "Assumptions" string(partition) usums assumptions res == Z3.unsat unsat_core(s)
        if res == Z3.unsat && length(unsat_core(s)) == 0
            pop(s, 1)
            res = check(s, usums)
            @debug "" res == Z3.unknown reason_unknown(s)
            @assert res == Z3.unsat
            core = unsat_core(s)
            pcore = SPartition(Set{Int}(partition[findfirst(x->isequal(x, ex), usums)]) for ex in core)
            learn_core!(m, pcore)
            core_indices = (findfirst(x->isequal(x, ex), usums) for ex in core)
            Ps = (move_iterator(partition, i, elem) for i in core_indices for elem in partition[i])
            push!(m, Iterators.flatten(Ps))
        elseif res == Z3.sat
            if current == length(mss)
                # all C-finite constraints are satisfied
                # add assumptions to get back values for Z3
                # TODO: is this really needed?
                for a in assumptions
                    add(s, a)
                end
                check(s)
                @debug "" check(s) == Z3.sat get_model(s)
                return true
            else
                # there are C-finite constraints left; continue with current+1
                for a in assumptions
                    add(s, a)
                end
                solve_constraints(s, mss, uss, partitions, current+1) && return true
                pop(s, 1)
            end
        elseif res == Z3.unsat
            Ps = learn_partitions(ms, us, partition, partition_map, unsat_core(s)) |> collect
            push!(m, Ps)
            pop(s, 1)
        else
            return false
        end
    end
end

function check_cfinite(s::CFiniteSolver, z3::Solver, vars, soft_clauses)
    uss = [[Z3Expr(ctx(z3), vars, x) for x in first(c).us] for c in soft_clauses]
    mss = [[Z3Expr(ctx(z3), vars, x) for x in first(c).ms] for c in soft_clauses]

    soft_clauses = Z3Expr[u == 0 for u in Iterators.flatten(uss)]
    @debug "Soft clauses" soft_clauses

    res, cost, model = maxsat(z3, soft_clauses)

    res == Z3.sat && iszero(cost) && return Z3.sat, model # soft clauses are all satisfiable
    res != Z3.sat && return res, nothing                  # hard clauses not satisfiable or timeout

    partitions = [collect(find_partition(model, ms)) for ms in mss]
    solve_constraints(z3, mss, uss, partitions) && return Z3.sat, get_model(z3)

    return Z3.unsat, nothing
end

function solve(s::CFiniteSolver; timeout::Int=-1)
    @debug("Solve")
    set_param("unsat_core", true)
    ctx = main_ctx()
    if timeout > 0
        set(ctx, "timeout", timeout*1000)
    end
    z3 = mk_solver()
    # add variables
    z3vars = Dict{Symbol,Z3Expr}()
    for (k,v) in s.vars
        push!(z3vars, k=>real_const(ctx, string(k)))
    end
    # add hard clauses
    for cl in s.hard_clauses
        add(z3, Z3Expr(ctx, z3vars, cl))
    end
    @debug "" [length(first(cl).us) for cl in s.cfin_clauses]
    # C-finite clauses
    sclauses = ClauseSet()
    for cl in s.cfin_clauses
        # if clause consists of just one term then add it as a hard clause
        if length(first(cl).us) == 1
            add(z3, Z3Expr(ctx, z3vars, first(cl).us[1]) == 0)
        else
            push!(sclauses, cl)
        end
    end

    elapsed_sec = @elapsed res, model = check_cfinite(s, z3, z3vars, sclauses)
    @debug "CFiniteSolver" res == Z3.sat reason_unknown(z3)
    elapsed = Millisecond(round(elapsed_sec*1000))
    if res == Z3.sat
        return NLSat.sat, elapsed, parse_model(model)
    elseif res == Z3.unsat
        return NLSat.unsat, elapsed, nothing
    elseif res == Z3.unknown
        reason = reason_unknown(z3)
        if reason == "timeout"
            return NLSat.timeout, elapsed, nothing
        end
        return NLSat.unknown, elapsed, nothing
    end
end


text:
############################################################################################
#                                  IMPLICIT METHODS 								       #
############################################################################################

# Hermite Simpson
function evaluate!(vals::Vector{<:AbstractVector}, con::DynamicsConstraint{HermiteSimpson},
		Z::Traj, inds=1:length(Z)-1)
	N = length(Z)
	model = con.model
	fVal = con.fVal
	xMid = con.xMid

	for k = inds[1]:inds[end]+1
		fVal[k] = RobotDynamics.dynamics(model, Z[k])
	end
	for k in inds
		xMid[k] = (state(Z[k]) + state(Z[k+1]))/2 + Z[k].dt/8 * (fVal[k] - fVal[k+1])
	end
	for k in inds
		Um = (control(Z[k]) + control(Z[k+1]))*0.5
		fValm = RobotDynamics.dynamics(model, xMid[k], Um)
		vals[k] .= state(Z[k]) - state(Z[k+1]) + Z[k].dt*(fVal[k] + 4*fValm + fVal[k+1])/6
	end
end

function jacobian!(∇c::Matrix{<:SizedMatrix}, con::DynamicsConstraint{HermiteSimpson,L,n,m},
		Z::Traj, inds=1:length(Z)-1) where {L,n,m}
	N = length(Z)
	model = con.model
	∇f = con.∇f
	A = con.A
	B = con.B
	xMid = con.xMid
	In = Diagonal(@SVector ones(n))
	Fmid = con.∇fMid[1]
	Amid = con.Am[1]
	Bmid = con.Bm[1]

	xi = Z[1]._x
	ui = Z[1]._u

	# Compute dynamics Jacobian at each knot point
	for k = inds.start:inds.stop+1
		jacobian!(∇f[k], model, Z[k])
	end

	# TODO: write an in-place version for large arrays where this will probably die at compile time
	for k in inds
		Um = (control(Z[k]) + control(Z[k+1]))*0.5
		zMid = StaticKnotPoint([xMid[k]; Um], xi, ui, Z[k].dt, Z[k].t)
		jacobian!(Fmid, model, zMid)
		A1 = SMatrix{n,n}(A[k])
		B1 = SMatrix{n,m}(B[k])
		Am = SMatrix{n,n}(Amid)
		Bm = SMatrix{n,m}(Bmid)
		A2 = SMatrix{n,n}(A[k+1])
		B2 = SMatrix{n,m}(B[k+1])
		dt = Z[k].dt
		A_ = dt/6*(A1 + 4Am*( dt/8*A1 + In/2)) + In
		B_ = dt/6*(B1 + 4Am*( dt/8*B1) + 2Bm)
		C_ = dt/6*(A2 + 4Am*(-dt/8*A2 + In/2)) - In
		D_ = dt/6*(B2 + 4Am*(-dt/8*B2) + 2Bm)
		∇c[k,1] .= [A_ B_]
		∇c[k,2] .= [C_ D_]
	end
end

function cost(obj, dyn_con::DynamicsConstraint{HermiteSimpson}, Z)
	N = length(Z)
	model = dyn_con.model
    xMid = dyn_con.xMid
	fVal = dyn_con.fVal
	for k = 1:N
		fVal[k] = dynamics(model, Z[k])
	end
	for k = 1:N-1
		xMid[k] = (state(Z[k]) + state(Z[k+1]))/2 + Z[k].dt/8 * (fVal[k] - fVal[k+1])
	end
	J = 0.0
	for k = 1:N-1
		Um = (control(Z[k]) + control(Z[k+1]))*0.5
		J += Z[k].dt/6 * (stage_cost(obj[k], state(Z[k]), control(Z[k])) +
					    4*stage_cost(obj[k], xMid[k], Um) +
					      stage_cost(obj[k], state(Z[k+1]), control(Z[k+1])))
	end
	J += stage_cost(obj[N], state(Z[N]))
	return J
end

function cost_gradient!(E, obj, dyn_con::DynamicsConstraint{HermiteSimpson},
		Z::Vector{<:KnotPoint{<:Any,n,m}}) where {n,m}
	N = length(Z)
	xi = Z[1]._x
	ui = Z[1]._u

	model = dyn_con.model
	fVal = dyn_con.fVal
	xMid = dyn_con.xMid
	∇f = dyn_con.∇f
	A = dyn_con.A
	B = dyn_con.B
	grad = dyn_con.grad
	Fm = dyn_con.∇fMid[1]
	Amid = dyn_con.Am[1]
	Bmid = dyn_con.Bm[1]

	for k = 1:N
		fVal[k] = dynamics(model, Z[k])
	end
	for k = 1:N-1
		xMid[k] = (state(Z[k]) + state(Z[k+1]))/2 + Z[k].dt/8 * (fVal[k] - fVal[k+1])
	end
	for k = 1:N
		jacobian!(∇f[k], model, Z[k])
		E[k].x .*= 0
		E[k].u .*= 0
	end

	for k in 1:N-1
		x1, u1 = state(Z[k]),   control(Z[k])
		x2, u2 = state(Z[k+1]), control(Z[k+1])
		xm, um = xMid[k], 0.5*(u1 + u2)

		zMid = StaticKnotPoint([xm; um], xi, ui, Z[k].dt, Z[k].t)
		jacobian!(Fm, model, zMid)
		A1 = SMatrix{n,n}(A[k])
		B1 = SMatrix{n,m}(B[k])
		Am = SMatrix{n,n}(Amid)
		Bm = SMatrix{n,m}(Bmid)
		A2 = SMatrix{n,n}(A[k+1])
		B2 = SMatrix{n,m}(B[k+1])
		dt = Z[k].dt

		gradient!(grad[1], obj[k], x1, u1)
		gradient!(grad[2], obj[k], x2, u2)
		gradient!(grad[3], obj[k], xm, um)

		∇x1,∇u1 = grad[1].x, grad[1].u #gradient(obj[k], x1, u1)
		∇x2,∇u2 = grad[2].x, grad[2].u #gradient(obj[k], x2, u2)
		∇xm,∇um = grad[3].x, grad[3].u #gradient(obj[k], xm, um)

		E[k].x   .+= dt/6 * (∇x1 + 4*( dt/8 * A1 + I/2)'∇xm)
		E[k].u   .+= dt/6 * (∇u1 + 4*( ( dt/8 * B1)'∇xm + 0.5I'*∇um))
		E[k+1].x .+= dt/6 * (∇x2 + 4*(-dt/8 * A2 + I/2)'∇xm)
		E[k+1].u .+= dt/6 * (∇u2 + 4*( (-dt/8 * B2)'∇xm + 0.5I'*∇um))
	end

	gradient!(grad[1], obj[N], state(Z[N]), control(Z[N]))
	E[N].x .+= grad[1].x #gradient(obj[N], state(Z[N]), control(Z[N]))[1]
	return nothing
end