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

using Markdown
using InteractiveUtils

# ╔═╡ a6049db0-dd73-11eb-2757-05d672d68b4e
using Revise

# ╔═╡ 728b1147-6022-4787-9e75-0990b797a7a3
using PolaronMobility

# ╔═╡ 55073dc7-94dc-4e2f-ac15-a0a09bc8693b
using QuadGK

# ╔═╡ 51cc807b-cf2c-459c-a1b7-85c00f73b145
begin
	ħ = 1.05457162825e-34
	eV = 1.602176487e-19
	me = 9.10938188e-31
	kB =  1.3806504e-23
	ϵ_0 = 8.854E-12
	amu = 1.660_539_066_60e-27
	Ha = 4.35974820e-18
	Bohr = 5.29177249e-11
	γ = 0.577215664901532
end

# ╔═╡ 13d580e2-2ab8-4bb3-b658-94ef8def4d6e
# Parameters

# ╔═╡ b8654fc8-0b81-4ee2-866d-bc88679028e0
data = [
	"Material" "ϵ∞" "ϵ0" 	"m^*_⟂" 	"m^*_z" 	"ω_LO" 	"ZPR" 	"∂^2Σ/∂k_2⟂" "∂^2Σ/∂k_2z" "m^pol_⟂" "m^pol_z" "m^*_⟂/m^pol_⟂" "m^*_z/m^pol_z" "a_p" "a_p⟂" "a_pz";
	"AlAs"	9.49 	11.51 	0.24276 	0.89650 	47.3	-8.8 	-0.14802 	-0.02315 	0.25181 	0.91550 	0.96407 	0.97925 	105.67 	123.26 	77.68;
	"AlP"	8.12 	10.32 	0.25190		0.80934		59.9 	-14.0 	-0.17791	-0.03398 	0.26372 	0.83222		0.95519		0.97250		88.57	101.64	67.26;
	"AlSb"	12.02 	13.35 	0.22156		1.14183		39.8	-3.6	-0.08089	-0.00782	0.22561		1.15212		0.98208		0.99107		157.37 	190.81 	107.04;
	"BAs"	9.81 	9.89 	0.21642		1.09408		84.4	-0.5	-0.00557	-0.00055	0.21668		1.09474		0.99879		0.99939		1665.87	2015.38	1138.17;
	"BN"	4.52 	6.69	0.29866		0.89524		161.0	-67.9	-0.26823	-0.05659	0.32467		0.94301		0.91989		0.94934		30.38	34.58	23.45;
	"CdS"	6.21 	10.24 	0.11773		0.11773		34.4	-14.9	-0.61225	-0.61225	0.12688		0.12688		0.92792		0.92792		503.93	0.0		0.0;
	"CdSe"	7.83 	11.78 	0.05115		0.05115		23.6	-5.5	-0.75785	-0.75785	0.05322		0.05322		0.96124		0.96123		1716.44	0.0		0.0;
	"CdTe"	8.89 	12.37 	0.05178		0.05178		19.1	-3.7	-0.61860	-0.61860	0.05349		0.05349		0.96797		0.96797		2294.84	0.0		0.0;
	"GaAs"	15.31 	17.55 	0.00912		0.00912		33.5	-0.5	-0.29326	-0.29326	0.00914		0.00914		0.99733		0.99733		49463.6	0.0		0.0;
	"GaN"	6.13 	11.00 	0.14355		0.14355		86.0	-29.6	-0.39962	-0.39965	0.15229		0.15228		0.94263		0.94263		362.68	362.68	362.69;
	"GaP"	10.50 	12.53 	0.22958		1.06158		48.6	-7.4	-0.13113	-0.01483	0.23670		1.07856		0.96990		0.98426		95.73	114.62	66.78;
	"SiC"	6.97 	10.30 	0.22814		0.67709		117.0	-32.7	-0.23217	-0.04969	0.24090		0.70067		0.94703		0.96636		62.48	71.04	48.33;
	"ZnS"	5.97 	9.40 	0.16715		0.16715		40.6	-18.6	-0.45614	-0.45614	0.18094		0.18094		0.92376		0.92376		368.04	0.0		0.0;
	"ZnSe"	7.35 	10.73 	0.08932		0.08932		29.3	-8.1	-0.51498	-0.51498	0.09362		0.09362		0.95400		0.95400		982.25	0.0		0.0;
	"ZnTe"	9.05 	 11.99 	0.07644		0.07644		24.1	-4.3	-0.38801	-0.38801	0.07877		0.07877		0.97034		0.97034		1815.51	0.0		0.0;
	"BaO"	4.21 	92.43 	0.38040		1.19717		47.3	-132.0	-1.40050	-0.27555	0.81412		1.78651		0.46725		0.67012		7.01	8.02	5.35;
	"CaO"	3.77 	16.67 	0.44286		1.42415		66.8	-153.9	-0.99507	-0.18981	0.79178		1.95175		0.55932		0.72968		6.42	7.37	4.88;
	"Li2O"	2.9 	7.8 	0.43735		0.84989		86.3	-171.7	-0.82404	-0.32275	0.68378		1.17113		0.63960		0.72570		13.49	14.59	11.53;
	"MgO"	3.23 	11.14 	0.33954		0.33954		84.5	-137.3	-0.79790	-0.79790	0.46571		0.46571		0.72908		0.72908		50.37	0.0		0.0;
	"SrO"	3.77 	20.91 	0.40701		1.22525		55.4	-141.0	-1.18792	-0.24910	0.78801		1.76349		0.51650		0.69479		7.31	8.33	5.64;
]

# ╔═╡ 3d1f6989-12f1-44ee-8dcb-8a06cc83f6b1
ϵ_optic = data[2:end, 2]

# ╔═╡ 4bb07926-4ed8-4f36-9d4e-561f3693a801
ϵ_static = data[2:end, 3]

# ╔═╡ 3f7c4d03-3f7a-4d7a-af1a-fed238ea4ce7
phonon_freqs = data[2:end, 6] ./ 1e3 .* eV ./ ħ ./ 2π # Hz

# ╔═╡ 7af80fc4-e2a9-433c-bc98-f2497045bf3e
m_perp = data[2:end, 4] ./ Ha ./ Bohr^2 .* ħ^2 ./ me # me

# ╔═╡ 5cfd7351-ca1b-4757-8652-fc7147693101
m_z = data[2:end, 5] ./ Ha ./ Bohr^2 .* ħ^2 ./ me # me

# ╔═╡ 01eb80d6-8852-4ce3-a626-1ca4d1288520
# Alphas

# ╔═╡ 74598315-7ac3-4aa8-9e3c-f38bfe1e7205
α_perp = [frohlichalpha(ϵ∞, ϵ0, f, m) for (ϵ∞, ϵ0, f, m) in zip(ϵ_optic, ϵ_static, phonon_freqs, m_perp)]

# ╔═╡ 1b524ae3-60a4-47d6-8535-10e77abd2571
α_z = [frohlichalpha(ϵ∞, ϵ0, f, m) for (ϵ∞, ϵ0, f, m) in zip(ϵ_optic, ϵ_static, phonon_freqs, m_z)]

# ╔═╡ be0fd60e-633a-40e3-b07e-8bf98cd243f6
# Athermal Theory

# ╔═╡ 53103b1e-c993-480c-bcbc-004d6b56ac3a
begin
	var_perp_athermal = feynmanvw.(α_perp)
	v_perp_athermal = [i[1] for i in var_perp_athermal]
	w_perp_athermal = [i[2] for i in var_perp_athermal]
end

# ╔═╡ a932ff04-a4d5-4cab-baf5-79c6a763a132
v_perp_athermal

# ╔═╡ 0e89abc0-9332-42df-aec9-77f94f07c241
w_perp_athermal

# ╔═╡ add797ac-99c9-4ea8-8fda-bff4524fb4e0
begin
	var_z_athermal = feynmanvw.(α_z)
	v_z_athermal = [i[1] for i in var_z_athermal]
	w_z_athermal = [i[2] for i in var_z_athermal]
end

# ╔═╡ 64382d9f-2575-4521-9fb2-94bdef83833a
v_z_athermal

# ╔═╡ b5391e54-1b0f-4e73-9489-79c803f66acf
w_z_athermal

# ╔═╡ be65dc13-6c3b-4ecb-83d5-ed2f079b18c6
F_perp_athermal = F.(v_perp_athermal, w_perp_athermal, α_perp) .* 1e3 .* ħ .* 2π .* phonon_freqs ./ eV

# ╔═╡ b2e5b2d2-65bd-45e7-8f3f-ef82f343582b
F_z_athermal = F.(v_z_athermal, w_z_athermal, α_z) .* 1e3 .* ħ .* 2π .* phonon_freqs ./ eV

# ╔═╡ f8313eab-8792-4854-9288-f6f803e68e03
F_avg_athermal = (F_perp_athermal .* 2 .+ F_z_athermal) ./ 3

# ╔═╡ a5bce0fa-4df3-4e41-bd2f-8a221d1c808b
function feynamn_mass(α, v, w, m, ω)
	j(τ) = 1 + (v * ω / w^2) * (1 - w^2 / v^2) * (1 - exp(-v * τ / ω)) / τ
	integrand(τ) = exp(-τ) * τ^(1/2) * j(τ)^(-3/2)
	return m * (1 + α * (v / w)^3 / (3 * π^(1/2)) * quadgk(τ -> integrand(τ), 0.0, Inf)[1])
end

# ╔═╡ 72e71b1b-ca34-4f64-a561-a4c3e5fcb758
m_pol_perp_athermal = feynamn_mass.(α_perp, v_perp_athermal, w_perp_athermal, m_perp, phonon_freqs .* 2π) .* Ha .* Bohr^2 ./ ħ^2 .* me

# ╔═╡ 0bcdc70b-e564-4874-94ea-c10b7e638522
m_pol_z_athermal = feynamn_mass.(α_z, v_z_athermal, w_z_athermal, m_z, phonon_freqs .* 2π) .* Ha .* Bohr^2 ./ ħ^2 .* me

# ╔═╡ 73b1fe6d-771e-4eed-be9d-cc8eee02a7e1
m_perp_athermal_ratio = m_perp ./ m_pol_perp_athermal

# ╔═╡ 9a5479da-addc-4d74-a546-36105778798c
m_z_athermal_ratio = m_z ./ m_pol_z_athermal

# ╔═╡ 13cb532e-8b90-4a77-a4fc-28e037e5b64b
begin
	μ_perp = m_perp .* (m_pol_perp_athermal .- m_perp) ./ m_pol_perp_athermal
	size_perp_athermal = sqrt.(3 ./ v_perp_athermal ./ μ_perp) ./ sqrt.(me .* m_perp .* 2π .* phonon_freqs ./ ħ) ./ 2 ./ Bohr
end

# ╔═╡ 0dea92e7-5743-488b-99ca-c34ee90f658f
begin
	μ_z = m_z .* (m_pol_z_athermal .- m_z) ./ m_pol_z_athermal
	size_z_athermal = sqrt.(3 / 2 ./ v_z_athermal ./ μ_z) ./ sqrt.(2 .* me .* m_z .* 2π .* phonon_freqs ./ ħ) ./ Bohr
end

# ╔═╡ a965eff9-e19e-46aa-bd20-f735429cd273
size_avg_athermal = (size_perp_athermal.^2 .* size_z_athermal).^(1/3)

# ╔═╡ db39c08a-341a-4dc6-a872-e67d24daf32f
# Weak coupling limit

# ╔═╡ 0888ccfd-34c9-472e-89c9-6f6964695cbe
# Feynman Weak Limit

# ╔═╡ a1fb81d1-e1f5-48fa-adcd-a923f8516566
w_z_athermal_weak = 3

# ╔═╡ ccef5927-04c0-4eed-bfa1-271b58c3c488
w_perp_athermal_weak = 3

# ╔═╡ b7ab04b4-7ddf-4c58-a876-2c39f755baaa
v_z_athermal_weak = 3 .+ 2.22 .* α_z ./ 10 .+ 1.97 .* (α_z ./ 10).^2

# ╔═╡ e9dc6720-6deb-49b1-ab79-ca77ec8f73b2
v_perp_athermal_weak = 3 .+ 2.22 .* α_perp ./ 10 .+ 1.97 .* (α_perp ./ 10).^2

# ╔═╡ 7fd68186-4423-4efe-9ad8-bcbbcd37e127
E_z_weak = (-α_z .- 0.0123 .* α_z.^2 .- 0.00064 .* α_z.^3) .* 1e3 .* ħ .* 2π .* phonon_freqs ./ eV

# ╔═╡ 70e3324b-3cd4-4675-bdb2-78a5b971d697
E_perp_weak = (-α_perp .- 0.0123 .* α_perp.^2 .- 0.00064 .* α_perp.^3) .* 1e3 .* ħ .* 2π .* phonon_freqs ./ eV

# ╔═╡ 0ebba711-5d5a-49b5-a158-c5396593708e
E_avg_weak = (2 .* E_perp_weak .+ E_z_weak) ./ 3

# ╔═╡ 29d7ad21-2aca-43d9-9357-e6efc0e22fa0
m_pol_z_athermal_weak = m_z .* (1 .+ α_z ./ 6 .+ 0.025 .* α_z.^2) .* Ha .* Bohr^2 ./ ħ^2 .* me

# ╔═╡ 1297f732-6a2f-4a00-8e1f-e68340564400
m_pol_perp_athermal_weak = m_perp .* (1 .+ α_perp ./ 6 .+ 0.025 .* α_z.^2) .* Ha .* Bohr^2 ./ ħ^2 .* me

# ╔═╡ 5c7a55b2-66c1-41cf-87ab-10da25d0ac74
size_z_athermal_weak = 3 .* sqrt.(6 .* ħ ./ (α_z .* m_z .* 2π .* phonon_freqs .* me)) ./ 4 ./ Bohr

# ╔═╡ d578cf35-9e3d-49fe-b69a-1fffd1fa34ce
size_perp_athermal_weak = 3 .* sqrt.(6 .* ħ ./ (α_perp .* m_perp .* 2π .* phonon_freqs .* me)) ./ 4 ./ Bohr

# ╔═╡ b0b563e3-7e38-46cd-a895-2ab8dbba11e1
# Frohlich Weak Limit

# ╔═╡ 2745e641-26b1-4c91-825b-104883070ef3
E_z_weak_fr = (.- α_z .- 0.0159196220 .* α_z.^2 .- 0.000806070048 .* α_z.^3) .* 1e3 .* ħ .* 2π .* phonon_freqs ./ eV

# ╔═╡ 0524d4fd-b5e0-4d79-b9f9-0c4a6cde38c5
E_perp_weak_fr = (.- α_perp .- 0.0159196220 .* α_perp.^2 .- 0.000806070048 .* α_perp.^3) .* 1e3 .* ħ .* 2π .* phonon_freqs ./ eV

# ╔═╡ 29819ac9-a201-490e-92c5-67ffeb3fbb49
E_avg_weak_fr = (E_z_weak_fr .+ 2 .* E_perp_weak_fr) ./ 3

# ╔═╡ 225aadee-461e-4703-b860-60b58d8f7d39
m_pol_z_athermal_weak_fr = m_z .* (1 .+ α_z ./ 6 .+ 0.02362763 .* α_z.^2) .* Ha .* Bohr^2 ./ ħ^2 .* me

# ╔═╡ b01ecad1-1840-4bc5-91ff-873ec28c7d2f
m_pol_perp_athermal_weak_fr = m_perp .* (1 .+ α_perp ./ 6 .+ 0.02362763 .* α_perp.^2) .* Ha .* Bohr^2 ./ ħ^2 .* me

# ╔═╡ 7f54b2d5-6435-4736-b51c-a8f00437ab1b
size_z_athermal_weak_fr = sqrt.(2 .* ħ ./ (m_z .* me .* phonon_freqs .* 2π)) ./ Bohr

# ╔═╡ 6fc88ab3-7a5a-4252-aa68-af3c68014435
size_perp_athermal_weak_fr = sqrt.(2 .* ħ ./ (m_perp .* me .* phonon_freqs .* 2π)) ./ Bohr

# ╔═╡ 8250b9b5-80c1-4650-b5e8-9eaea9a03a1e
size_avg_athermal_weak_fr = (size_perp_athermal_weak_fr.^2 .* size_z_athermal_weak_fr).^(1/3)

# ╔═╡ 9b0d8873-f451-410e-9771-d8bd5c67f53c
# Strong coupling limit

# ╔═╡ 66c7fe8b-901e-4af0-98c6-6bb97d61354f
# Feynman Strong Limit

# ╔═╡ f48c8862-35d4-4592-8e00-7b598325a150
w_z_athermal_strong = 1

# ╔═╡ 4d6523b0-4dbe-45ee-b8ee-518580dfdf03
w_perp_athermal_strong = 1

# ╔═╡ 21dcd3a6-f322-456e-ab8e-7121b72b3773
v_z_athermal_strong = 4 .* α_z.^2 ./ (9 * π) .- 4 * (log(2) - 1)

# ╔═╡ ed0aac01-6b96-4236-8d7f-976a4fa310a2
v_perp_athermal_strong = 4 .* α_perp.^2 ./ (9 * π) .- 4 * (log(2) - 1)

# ╔═╡ a3be76e3-6426-41ed-9596-e28249fbeaac
E_z_strong = (-0.106 .* α_z.^2 .- 2.83) .* 1e3 .* ħ .* 2π .* phonon_freqs ./ eV

# ╔═╡ 8f093305-6dfd-42c7-848a-beeb416bd681
E_perp_strong = (-0.106 .* α_perp.^2 .- 2.83) .* 1e3 .* ħ .* 2π .* phonon_freqs ./ eV

# ╔═╡ 0b173c0b-2c86-47b5-a823-a8ff0920953f
E_avg_strong = (E_z_strong .+ 2 .* E_perp_strong) ./ 3

# ╔═╡ bcb54f1d-9c36-4448-a212-33d3544a9a07
m_pol_z_athermal_strong = m_z .* (1 .+ α_z.^4 .* 0.0202) .* Ha .* Bohr^2 ./ ħ^2 .* me

# ╔═╡ 29549691-1673-4902-bcbb-f4f0e44190b7
m_pol_perp_athermal_strong = m_perp .* (1 .+ α_perp.^4 .* 0.0202) .* Ha .* Bohr^2 ./ ħ^2 .* me

# ╔═╡ 0e32dde6-f203-4aa1-9ede-9d01a56ce8ea
size_z_athermal_strong = 3 .* α_z .* sqrt.(π * ħ ./ (m_z .* me .* 2π .* phonon_freqs)) ./ 2 ./ Bohr

# ╔═╡ 6ba5d70f-2bd5-4594-a9ed-71923c5e39b0
size_perp_athermal_strong = 3 ./ α_perp .* sqrt.(π * ħ ./ (m_perp .* me .* 2π .* phonon_freqs)) ./ 2 ./ Bohr

# ╔═╡ 34ab5243-9f7c-4250-a698-b8c54b6bcf49
# Frohlich Strong Limit

# ╔═╡ 0d27d4dd-c7d2-4292-abf7-258783440089
E_z_strong_fr = (-0.108513 .* α_z.^2 .- 2.836) .* 1e3 .* ħ .* 2π .* phonon_freqs ./ eV

# ╔═╡ 3f85e0d0-0016-4333-ada3-171ac606560b
E_perp_strong_fr = (-0.108513 .* α_perp.^2 .- 2.836) .* 1e3 .* ħ .* 2π .* phonon_freqs ./ eV

# ╔═╡ 64cdc83a-54a1-413a-a525-6d68fd6cd0bc
E_avg_strong_fr = (E_z_strong_fr .+ 2 .* E_perp_strong_fr) ./ 3

# ╔═╡ d8e299ac-b788-4263-b828-8417a84684f7
m_pol_z_athermal_strong_fr = m_z .* (1 .+ 0.0227019 .* α_z.^4) .* Ha .* Bohr^2 ./ ħ^2 .* me

# ╔═╡ f933ff3a-7fa9-434d-a52f-83f95b765c3b
m_pol_perp_athermal_strong_fr = m_perp .* (1 .+ 0.0227019 .* α_perp.^4) .* Ha .* Bohr^2 ./ ħ^2 .* me

# ╔═╡ e1d32f2a-fb54-45e0-bd0b-1d86859c7bbb
size_z_athermal_strong_fr = 3 / 2 * √π ./ sqrt.(m_z .* me .* phonon_freqs .* 2π .* α_z.^2 ./ ħ) ./ Bohr

# ╔═╡ f69a2f23-b43f-4dc2-83ba-ae281ee86a83
size_perp_athermal_strong_fr = 3 / 2 * √π ./ sqrt.(m_perp .* me .* phonon_freqs .* 2π .* α_perp.^2 ./ ħ) ./ Bohr

# ╔═╡ b03bddfa-8586-468f-beb3-3dd3ef456711
size_avg_athermal_strong_fr = (size_perp_athermal_strong_fr.^2 .* size_z_athermal_strong_fr).^(1/3)

# ╔═╡ 411691a5-53c7-40e5-8115-6d0ed0be07cb
# Thermal Theory

# ╔═╡ 66d7a406-7061-4714-9197-c65db6ef75cf
T = 300 # K

# ╔═╡ 50c5c581-2500-4746-8554-a51598c7aeea
β_red = ħ * 2π / kB / T .* phonon_freqs

# ╔═╡ 8fd12478-4320-4ffd-b686-45ba77b13e87
begin
	var_perp_thermal = feynmanvw.(α_perp, β_red)
	v_perp_thermal = [i[1] for i in var_perp_thermal]
	w_perp_thermal = [i[2] for i in var_perp_thermal]
end

# ╔═╡ 21ccb4da-ca25-4e50-9a3b-9eea67ff2699
begin
	var_z_thermal = feynmanvw.(α_z, β_red)
	v_z_thermal = [i[1] for i in var_z_thermal]
	w_z_thermal = [i[2] for i in var_z_thermal]
end

# ╔═╡ e9aa5e25-9467-427e-bd4b-7ad7d4906c22
F_perp_thermal = F.(v_perp_thermal, w_perp_thermal, β_red, α_perp) .* 1e3 .* ħ .* 2π .* phonon_freqs ./ eV

# ╔═╡ aa91544c-f776-4e2f-8fcf-e8c821bf5ac8
F_z_thermal = F.(v_z_thermal, w_z_thermal, β_red, α_z) .* 1e3 .* ħ .* 2π .* phonon_freqs ./ eV

# ╔═╡ 76d164e5-0e87-4aea-a40b-5e329371e848
F_avg_thermal = (F_perp_thermal .* 2 .+ F_z_thermal) / 3

# ╔═╡ ece97292-8325-4bf6-a18b-16f0a5be4621
m_pol_perp_thermal = m_perp ./ (1 .+ (v_perp_thermal.^2 .- w_perp_thermal.^2) ./ w_perp_thermal.^2 .* Ha .* Bohr^2 ./ ħ^2 .* me)

# ╔═╡ 67d60382-30d6-4b7f-9774-76a4d05e2098
m_pol_z_thermal = m_z ./ (1 .+ (v_z_thermal.^2 .- w_z_thermal.^2) ./ w_z_thermal.^2 .* Ha .* Bohr^2 ./ ħ^2 .* me)

# ╔═╡ 16d1552c-a7fe-4775-ac98-f97add75d1e4
m_perp_thermal_ratio = m_perp ./ m_pol_perp_thermal

# ╔═╡ f84c0d7f-13f7-49a0-9943-4f5c56527368
m_z_thermal_ratio = m_z ./ m_pol_z_thermal

# ╔═╡ 1b0d8352-7384-4355-ad13-2aae6aa6cf87
size_perp_thermal = sqrt.(3 ./ me ./ m_perp ./ (v_perp_thermal.^2 - w_perp_thermal.^2) .* v_perp_thermal ./ 2π ./ phonon_freqs .* ħ) ./ Bohr ./ 2

# ╔═╡ c274dd67-7e0c-417a-a972-26886fd32573
size_z_thermal = sqrt.(3 ./ me ./ m_z ./ (v_z_thermal.^2 - w_z_thermal.^2) .* v_z_thermal ./ 2π ./ phonon_freqs .* ħ) ./ Bohr ./ 2

# ╔═╡ 7e3aac26-62bf-410c-981f-654d918624bd
size_avg_thermal = (size_perp_thermal.^2 .* size_z_thermal).^(1/3)

# ╔═╡ Cell order:
# ╠═a6049db0-dd73-11eb-2757-05d672d68b4e
# ╠═728b1147-6022-4787-9e75-0990b797a7a3
# ╠═55073dc7-94dc-4e2f-ac15-a0a09bc8693b
# ╠═51cc807b-cf2c-459c-a1b7-85c00f73b145
# ╠═13d580e2-2ab8-4bb3-b658-94ef8def4d6e
# ╠═b8654fc8-0b81-4ee2-866d-bc88679028e0
# ╠═3d1f6989-12f1-44ee-8dcb-8a06cc83f6b1
# ╠═4bb07926-4ed8-4f36-9d4e-561f3693a801
# ╠═3f7c4d03-3f7a-4d7a-af1a-fed238ea4ce7
# ╠═7af80fc4-e2a9-433c-bc98-f2497045bf3e
# ╠═5cfd7351-ca1b-4757-8652-fc7147693101
# ╠═01eb80d6-8852-4ce3-a626-1ca4d1288520
# ╠═74598315-7ac3-4aa8-9e3c-f38bfe1e7205
# ╠═1b524ae3-60a4-47d6-8535-10e77abd2571
# ╠═be0fd60e-633a-40e3-b07e-8bf98cd243f6
# ╠═53103b1e-c993-480c-bcbc-004d6b56ac3a
# ╠═a932ff04-a4d5-4cab-baf5-79c6a763a132
# ╠═0e89abc0-9332-42df-aec9-77f94f07c241
# ╠═add797ac-99c9-4ea8-8fda-bff4524fb4e0
# ╠═64382d9f-2575-4521-9fb2-94bdef83833a
# ╠═b5391e54-1b0f-4e73-9489-79c803f66acf
# ╠═be65dc13-6c3b-4ecb-83d5-ed2f079b18c6
# ╠═b2e5b2d2-65bd-45e7-8f3f-ef82f343582b
# ╠═f8313eab-8792-4854-9288-f6f803e68e03
# ╠═a5bce0fa-4df3-4e41-bd2f-8a221d1c808b
# ╠═72e71b1b-ca34-4f64-a561-a4c3e5fcb758
# ╠═0bcdc70b-e564-4874-94ea-c10b7e638522
# ╠═73b1fe6d-771e-4eed-be9d-cc8eee02a7e1
# ╠═9a5479da-addc-4d74-a546-36105778798c
# ╠═13cb532e-8b90-4a77-a4fc-28e037e5b64b
# ╠═0dea92e7-5743-488b-99ca-c34ee90f658f
# ╠═a965eff9-e19e-46aa-bd20-f735429cd273
# ╠═db39c08a-341a-4dc6-a872-e67d24daf32f
# ╠═0888ccfd-34c9-472e-89c9-6f6964695cbe
# ╠═a1fb81d1-e1f5-48fa-adcd-a923f8516566
# ╠═ccef5927-04c0-4eed-bfa1-271b58c3c488
# ╠═b7ab04b4-7ddf-4c58-a876-2c39f755baaa
# ╠═e9dc6720-6deb-49b1-ab79-ca77ec8f73b2
# ╠═7fd68186-4423-4efe-9ad8-bcbbcd37e127
# ╠═70e3324b-3cd4-4675-bdb2-78a5b971d697
# ╠═0ebba711-5d5a-49b5-a158-c5396593708e
# ╠═29d7ad21-2aca-43d9-9357-e6efc0e22fa0
# ╠═1297f732-6a2f-4a00-8e1f-e68340564400
# ╠═5c7a55b2-66c1-41cf-87ab-10da25d0ac74
# ╠═d578cf35-9e3d-49fe-b69a-1fffd1fa34ce
# ╠═b0b563e3-7e38-46cd-a895-2ab8dbba11e1
# ╠═2745e641-26b1-4c91-825b-104883070ef3
# ╠═0524d4fd-b5e0-4d79-b9f9-0c4a6cde38c5
# ╠═29819ac9-a201-490e-92c5-67ffeb3fbb49
# ╠═225aadee-461e-4703-b860-60b58d8f7d39
# ╠═b01ecad1-1840-4bc5-91ff-873ec28c7d2f
# ╠═7f54b2d5-6435-4736-b51c-a8f00437ab1b
# ╠═6fc88ab3-7a5a-4252-aa68-af3c68014435
# ╠═8250b9b5-80c1-4650-b5e8-9eaea9a03a1e
# ╠═9b0d8873-f451-410e-9771-d8bd5c67f53c
# ╠═66c7fe8b-901e-4af0-98c6-6bb97d61354f
# ╠═f48c8862-35d4-4592-8e00-7b598325a150
# ╠═4d6523b0-4dbe-45ee-b8ee-518580dfdf03
# ╠═21dcd3a6-f322-456e-ab8e-7121b72b3773
# ╠═ed0aac01-6b96-4236-8d7f-976a4fa310a2
# ╠═a3be76e3-6426-41ed-9596-e28249fbeaac
# ╠═8f093305-6dfd-42c7-848a-beeb416bd681
# ╠═0b173c0b-2c86-47b5-a823-a8ff0920953f
# ╠═bcb54f1d-9c36-4448-a212-33d3544a9a07
# ╠═29549691-1673-4902-bcbb-f4f0e44190b7
# ╠═0e32dde6-f203-4aa1-9ede-9d01a56ce8ea
# ╠═6ba5d70f-2bd5-4594-a9ed-71923c5e39b0
# ╠═34ab5243-9f7c-4250-a698-b8c54b6bcf49
# ╠═0d27d4dd-c7d2-4292-abf7-258783440089
# ╠═3f85e0d0-0016-4333-ada3-171ac606560b
# ╠═64cdc83a-54a1-413a-a525-6d68fd6cd0bc
# ╠═d8e299ac-b788-4263-b828-8417a84684f7
# ╠═f933ff3a-7fa9-434d-a52f-83f95b765c3b
# ╠═e1d32f2a-fb54-45e0-bd0b-1d86859c7bbb
# ╠═f69a2f23-b43f-4dc2-83ba-ae281ee86a83
# ╠═b03bddfa-8586-468f-beb3-3dd3ef456711
# ╠═411691a5-53c7-40e5-8115-6d0ed0be07cb
# ╠═66d7a406-7061-4714-9197-c65db6ef75cf
# ╠═50c5c581-2500-4746-8554-a51598c7aeea
# ╠═8fd12478-4320-4ffd-b686-45ba77b13e87
# ╠═21ccb4da-ca25-4e50-9a3b-9eea67ff2699
# ╠═e9aa5e25-9467-427e-bd4b-7ad7d4906c22
# ╠═aa91544c-f776-4e2f-8fcf-e8c821bf5ac8
# ╠═76d164e5-0e87-4aea-a40b-5e329371e848
# ╠═ece97292-8325-4bf6-a18b-16f0a5be4621
# ╠═67d60382-30d6-4b7f-9774-76a4d05e2098
# ╠═16d1552c-a7fe-4775-ac98-f97add75d1e4
# ╠═f84c0d7f-13f7-49a0-9943-4f5c56527368
# ╠═1b0d8352-7384-4355-ad13-2aae6aa6cf87
# ╠═c274dd67-7e0c-417a-a972-26886fd32573
# ╠═7e3aac26-62bf-410c-981f-654d918624bd


text:
module CreditsTrigger

using ..Ahorn, Maple

const placements = Ahorn.PlacementDict(
    "Credits" => Ahorn.EntityPlacement(
        Maple.CreditsTrigger,
        "rectangle"
    )
)

end

ext: jl


text:
using GeometricIntegrators
using ChargedParticleDynamics.GuidingCenter4d.TokamakFastLoop
using GeometricExamples

const Δt     = 1.
const ntime  = 50000
const nloop  = 200
const nsave  = 50
const nplot  = 10
const run_id = "poincare_1st_fast_firk_dt1"

include("../guiding_center_4d_settings_firk.jl")

tableau_list = get_tableau_list_firk()

pinv = guiding_center_4d_ode_poincare_invariant_1st(Δt, nloop, ntime, nsave)

include("guiding_center_4d_fast_poincare_invariant_1st.jl")


text:
using Documenter, ConstraintProgrammingExtensions

# More than inspired by https://github.com/jump-dev/MathOptInterface.jl/blob/master/docs/make.jl.

makedocs(
    sitename="ConstraintProgrammingExtensions",
    format = Documenter.HTML(
        # See https://github.com/JuliaDocs/Documenter.jl/issues/868
        prettyurls = get(ENV, "CI", nothing) == "true",
        mathengine = Documenter.MathJax2(),
        collapselevel = 1,
    ),
    strict = true,
    modules = [ConstraintProgrammingExtensions],
    checkdocs = :exports,
    pages = [
        "Introduction" => "index.md",
        "Reference" => [
            "reference/sets.md",
            "reference/bridges_sets.md",
        ],
        "Comparison to other CP packages" => [
            "mappings/constraintsolver.md",
            "mappings/cplexcp.md",
            "mappings/disjunctive.md",
            "mappings/facile.md",
            "mappings/hakank.md",
            "mappings/juliaconstraints.md",
            "mappings/minizinc.md",
            "mappings/numberjack.md",
            "mappings/sas.md",
            "mappings/yalmip.md",
        ]
    ],
)

deploydocs(
    push_preview = true,
    repo = "github.com/dourouc05/ConstraintProgrammingExtensions.jl.git",
)


text:
using Requests
using Discourse

const BASE_URL = "https://api.stackexchange.com"
const LAST_CHECKED_FILE = expanduser("~/.sesync/last_checked")

function get_question(id::Integer)
    url = BASE_URL * "/2.2/questions/$id"
    query = Dict(:site => "stackoverflow",
                 :filter =>  "withbody")
    resp = get(url; query=query)
    return JSON.parse(String(resp.data))["items"][1]
end

function questions_from(fromdate::Int64)
    url = BASE_URL * "/2.2/questions"
    query = Dict(:page => 1,
                 :pagesize => 10,
                 :order => "desc",
                 :sort => "creation",
                 :tagged => "julia-lang",
                 :site => "stackoverflow",
                 :fromdate => fromdate)
    resp = get(url; query=query)
    return JSON.parse(String(resp.data))
end

function last_checked()
    if isfile(LAST_CHECKED_FILE)
        open(LAST_CHECKED_FILE) do f
            return parse(Int64, readstring(f))
        end
    else
        return round(Int64, time())
    end
end

function save_last_checked(ts::Int64)
    dirpath = dirname(LAST_CHECKED_FILE)
    if !isdir(dirpath)
        mkdir(dirpath)
    end
    open(LAST_CHECKED_FILE, "w") do f
        write(f, string(ts))
    end
end

function question2topic(q::Dict)
    title = q["title"]
    body = q["body"] * "\n\n**Link:** " * q["link"]
    return Dict(:title => title, :content => body)
end

function main()
    ds = DiscourseClient(;base_url="https://discourse.julialang.org")
    ts = last_checked()
    next_ts = round(Int64, time())
    qs_meta = questions_from(ts)
    for q_meta in qs_meta["items"]
        q = get_question(q_meta["question_id"])
        topic = question2topic(q)
        create_topic(ds, topic[:title], topic[:content]; category="Usage")
    end
    save_last_checked(next_ts)
end


text:
export sort_observations, add_offset!, fix_latent_features!,
       equilibrate_variance!, prob_scale!

### OBSERVATION TUPLES TO ARRAYS
@compat function sort_observations(obs::Array{Tuple{Int,Int},1}, m::Int, n::Int; check_empty=false)
    observed_features = Array{Int,1}[Int[] for i=1:m]
    observed_examples = Array{Int,1}[Int[] for j=1:n]
    for (i,j) in obs
        @inbounds push!(observed_features[i],j)
        @inbounds push!(observed_examples[j],i)
    end
    if check_empty && (any(map(x->length(x)==0,observed_examples)) ||
            any(map(x->length(x)==0,observed_features)))
        error("Every row and column must contain at least one observation")
    end
    return observed_features, observed_examples
end

### SCALINGS AND OFFSETS ON GLRM
function add_offset!(glrm::AbstractGLRM)
    glrm.rx, glrm.ry = map(lastentry1, glrm.rx), map(lastentry_unpenalized, glrm.ry)
    return glrm
end
function fix_latent_features!(glrm::AbstractGLRM, n)
    glrm.ry = Regularizer[fixed_latent_features(glrm.ry[i], glrm.Y[1:n,i])
                            for i in 1:length(glrm.ry)]
    return glrm
end

## equilibrate variance
# scale all columns inversely proportional to mean value of loss function
# makes sense when all loss functions used are nonnegative
function equilibrate_variance!(glrm::AbstractGLRM, columns_to_scale = 1:size(glrm.A,2))
    for i in columns_to_scale
        nomissing = glrm.A[glrm.observed_examples[i],i]
        if length(nomissing)>0
            varlossi = avgerror(glrm.losses[i], nomissing)
            varregi = var(nomissing) # TODO make this depend on the kind of regularization; this assumes QuadLoss
        else
            varlossi = 1
            varregi = 1
        end
        if varlossi > 0
            # rescale the losses and regularizers for each column by the inverse of the empirical variance
            scale!(glrm.losses[i], scale(glrm.losses[i])/varlossi)
        end
        if varregi > 0
            scale!(glrm.ry[i], scale(glrm.ry[i])/varregi)
        end
    end
    return glrm
end

## probabilistic scaling
# scale loss function to fit -loglik of joint distribution
# makes sense when all functions used are -logliks of sensible distributions
# todo: option to scale to account for nonuniform sampling in rows or columns or both
# skipmissing(Array with missing) gives an iterator.
function prob_scale!(glrm, columns_to_scale = 1:size(glrm.A,2))
    for i in columns_to_scale
        nomissing = glrm.A[glrm.observed_examples[i],i]
        if typeof(glrm.losses[i]) == QuadLoss && length(nomissing) > 0
            varlossi = var(skipmissing(glrm.A[i])) # estimate the variance
            if varlossi > TOL
    		  scale!(glrm.losses[i], 1/(2*varlossi)) # this is the correct -loglik of gaussian with variance fixed at estimate
    	    else
    		  warn("column $i has a variance of $varlossi; not scaling it to avoid dividing by zero.")
    	    end
        elseif typeof(glrm.losses[i]) == HuberLoss && length(nomissing) > 0
            varlossi = avgerror(glrm.losses[i], glrm.A[i]) # estimate the width of the distribution
            if varlossi > TOL
              scale!(glrm.losses[i], 1/(2*varlossi)) # this is not the correct -loglik of huber with estimates for variance and mean of poisson, but that's probably ok
            else
              warn("column $i has a variance of $varlossi; not scaling it to avoid dividing by zero.")
            end
        else # none of the other distributions have any free parameters to estimate, so this is the correct -loglik
            scale!(glrm.losses[i], 1)
        end
    end
    return glrm
end


text:
using Gadfly, DataArrays

set_default_plot_size(6inch, 3inch)

spy(randn((10, 10)), Coord.cartesian(fixed=true))


text:
"""
    AbstractEmbeddingType <: AbstractDecoratorType

A type used to specify properties of an [`AbstractEmbeddedManifold`](@ref).
"""
abstract type AbstractEmbeddingType <: AbstractDecoratorType end

"""
    AbstractEmbeddedManifold{𝔽,T<:AbstractEmbeddingType,𝔽} <: AbstractDecoratorManifold{𝔽}

This abstract type indicates that a concrete subtype is an embedded manifold with the
additional property, that its points are given in the embedding. This also means, that
the default implementation of [`embed`](@ref) is just the identity, since the points are
already stored in the form suitable for this embedding specified. This also holds true for
tangent vectors.

Furthermore, depending on the [`AbstractEmbeddingType`](@ref) different methods are
transparently used from the embedding, for example the [`inner`](@ref) product or even the
[`distance`](@ref) function. Specifying such an embedding type transparently passes the
compuation onwards to the embedding (note again, that no [`embed`](@ref) is required)
and hence avoids to reimplement these methods in the manifold that is embedded.

This should be used for example for [`check_point`](@ref) or [`check_vector`](@ref),
which should first invoke the test of the embedding and then test further constraints
the representation in the embedding has for these points to be valid.

Technically this is realised by making the [`AbstractEmbeddedManifold`](@ref) is a decorator
for the [`AbstractManifold`](@ref)s that are subtypes.
"""
abstract type AbstractEmbeddedManifold{𝔽,T<:AbstractEmbeddingType} <:
              AbstractDecoratorManifold{𝔽,T} end

"""
    DefaultEmbeddingType <: AbstractEmbeddingType

A type of default embedding that does not have any special properties.
"""
struct DefaultEmbeddingType <: AbstractEmbeddingType end

"""
    AbstractIsometricEmbeddingType <: AbstractEmbeddingType

Characterizes an embedding as isometric. For this case the [`inner`](@ref) product
is passed from the embedded manifold to the embedding.
"""
abstract type AbstractIsometricEmbeddingType <: AbstractEmbeddingType end

"""
    DefaultIsometricEmbeddingType <: AbstractIsometricEmbeddingType

An isometric embedding type that acts as a default, i.e. it has no specific properties
beyond its isometric property.
"""
struct DefaultIsometricEmbeddingType <: AbstractIsometricEmbeddingType end

"""
    TransparentIsometricEmbedding <: AbstractIsometricEmbeddingType

Specify that an embedding is the default isometric embedding. This even inherits
logarithmic and exponential map as well as retraction and inverse retractions from the
embedding.

For an example, see [`SymmetricMatrices`](@ref Main.Manifolds.SymmetricMatrices) which are
isometrically embedded in the Euclidean space of matrices but also inherit exponential
and logarithmic maps.
"""
struct TransparentIsometricEmbedding <: AbstractIsometricEmbeddingType end

"""
    EmbeddedManifold{𝔽, MT <: AbstractManifold, NT <: AbstractManifold} <: AbstractDecoratorManifold{𝔽}

A type to represent an explicit embedding of a [`AbstractManifold`](@ref) `M` of type `MT` embedded
into a manifold `N` of type `NT`.

!!! note
    This type is not required if a manifold `M` is to be embedded in one specific manifold `N`. One can then just implement
    [`embed!`](@ref) and [`project!`](@ref). Only for a second –maybe considered non-default–
    embedding, this type should be considered in order to dispatch on different embed
    and project methods for different embeddings `N`.

# Fields

* `manifold` the manifold that is an embedded manifold
* `embedding` a second manifold, the first one is embedded into

# Constructor

    EmbeddedManifold(M, N)

Generate the `EmbeddedManifold` of the [`AbstractManifold`](@ref) `M` into the
[`AbstractManifold`](@ref) `N`.
"""
struct EmbeddedManifold{𝔽,MT<:AbstractManifold{𝔽},NT<:AbstractManifold} <:
       AbstractDecoratorManifold{𝔽,AbstractDecoratorType}
    manifold::MT
    embedding::NT
end

function allocate_result(M::AbstractEmbeddedManifold, f::typeof(embed), x...)
    T = allocate_result_type(M, f, x)
    return allocate(x[1], T, representation_size(decorated_manifold(M)))
end

function allocate_result(M::AbstractEmbeddedManifold, f::typeof(project), x...)
    T = allocate_result_type(M, f, x)
    return allocate(x[1], T, representation_size(base_manifold(M)))
end

function allocate_result(M::EmbeddedManifold, f::typeof(embed), x...)
    T = allocate_result_type(M, f, x)
    return allocate(x[1], T, representation_size(get_embedding(M)))
end

function allocate_result(M::EmbeddedManifold, f::typeof(project), x...)
    T = allocate_result_type(M, f, x)
    return allocate(x[1], T, representation_size(base_manifold(M)))
end

"""
    base_manifold(M::AbstractEmbeddedManifold, d::Val{N} = Val(-1))

Return the base manifold of `M` that is enhanced with its embedding.
While functions like `inner` might be overwritten to use the (decorated) manifold
representing the embedding, the base_manifold is the manifold itself in the sense that
detemining e.g. the [`is_default_metric`](@ref) does not fall back to check with
the embedding but with the manifold itself. For this abstract case, just `M` is returned.
"""
base_manifold(M::AbstractEmbeddedManifold, ::Val{N} = Val(-1)) where {N} = M
"""
    base_manifold(M::EmbeddedManifold, d::Val{N} = Val(-1))

Return the base manifold of `M` that is enhanced with its embedding. For this specific
type the internally stored enhanced manifold `M.manifold` is returned.
"""
base_manifold(M::EmbeddedManifold, ::Val{N} = Val(-1)) where {N} = M.manifold

"""
    check_point(M::AbstractEmbeddedManifold, p; kwargs)

check whether a point `p` is a valid point on the [`AbstractEmbeddedManifold`](@ref),
i.e. that `embed(M, p)` is a valid point on the embedded manifold.
"""
function check_point(M::AbstractEmbeddedManifold, p; kwargs...)
    return invoke(
        check_point,
        Tuple{typeof(get_embedding(M)),typeof(p)},
        get_embedding(M),
        p;
        kwargs...,
    )
end

"""
    check_vector(M::AbstractEmbeddedManifold, p, X; kwargs...)

Check that `embed(M, p, X)` is a valid tangent to `embed(M, p)`.
"""
function check_vector(M::AbstractEmbeddedManifold, p, X; kwargs...)
    return invoke(
        check_vector,
        Tuple{typeof(get_embedding(M)),typeof(p),typeof(X)},
        get_embedding(M),
        p,
        X;
        kwargs...,
    )
end

decorated_manifold(M::EmbeddedManifold) = M.embedding

function embed(M::EmbeddedManifold, p)
    q = allocate_result(M, embed, p)
    embed!(M, q, p)
    return q
end

embed!(::AbstractEmbeddedManifold, q, p) = copyto!(q, p)
embed!(::AbstractEmbeddedManifold, Y, p, X) = copyto!(Y, X)

"""
    get_embedding(M::AbstractEmbeddedManifold)

Return the [`AbstractManifold`](@ref) `N` an [`AbstractEmbeddedManifold`](@ref) is embedded into.
"""
get_embedding(::AbstractEmbeddedManifold)

@decorator_transparent_function function get_embedding(M::AbstractEmbeddedManifold)
    return decorated_manifold(M)
end

"""
    get_embedding(M::EmbeddedManifold)

Return the [`AbstractManifold`](@ref) `N` an [`EmbeddedManifold`](@ref) is embedded into.
"""
get_embedding(::EmbeddedManifold)

function get_embedding(M::EmbeddedManifold)
    return M.embedding
end

function project(M::EmbeddedManifold, p)
    q = allocate_result(M, project, p)
    project!(M, q, p)
    return q
end

function show(
    io::IO,
    M::EmbeddedManifold{𝔽,MT,NT},
) where {𝔽,MT<:AbstractManifold{𝔽},NT<:AbstractManifold}
    return print(io, "EmbeddedManifold($(M.manifold), $(M.embedding))")
end

function default_decorator_dispatch(::EmbeddedManifold)
    return Val(true)
end

@doc raw"""
    default_embedding_dispatch(M::AbstractEmbeddedManifold)

This method indicates that an [`AbstractEmbeddedManifold`](@ref) is the default
and hence acts completely transparently and passes all functions transparently onwards.
This is used by the [`AbstractDecoratorManifold`](@ref) within
[`default_decorator_dispatch`](@ref).
By default this is set to `Val(false)`.
"""
default_embedding_dispatch(::AbstractEmbeddedManifold) = Val(false)

#
# Abstract intransparent – i.e. new implementations necessary
for f in
    [check_point, check_vector, embed!, exp!, inner, log!, manifold_dimension, project!]
    eval(
        quote
            function decorator_transparent_dispatch(
                ::typeof($f),
                ::AbstractEmbeddedManifold,
                args...,
            )
                return Val(:intransparent)
            end
        end,
    )
end
#
# Abstract parent – i.e. pass to embedding
for f in [
    copy,
    copyto!,
    embed,
    get_basis,
    get_coordinates,
    get_coordinates!,
    get_vector,
    get_vector!,
    inverse_retract!,
    mid_point!,
    project,
    retract!,
    vector_transport_along,
    vector_transport_direction,
    vector_transport_direction!,
    vector_transport_to,
]
    eval(
        quote
            function decorator_transparent_dispatch(
                ::typeof($f),
                ::AbstractEmbeddedManifold,
                args...,
            )
                return Val(:parent)
            end
        end,
    )
end
# Abstract generic isometric
for f in [inverse_retract!, retract!]
    eval(
        quote
            function decorator_transparent_dispatch(
                ::typeof($f),
                ::AbstractEmbeddedManifold{𝔽,<:AbstractIsometricEmbeddingType},
                args...,
            ) where {𝔽}
                return Val(:parent)
            end
        end,
    )
end
for f in [norm, inner]
    eval(
        quote
            function decorator_transparent_dispatch(
                ::typeof($f),
                ::AbstractEmbeddedManifold{𝔽,<:AbstractIsometricEmbeddingType},
                args...,
            ) where {𝔽}
                return Val(:transparent)
            end
        end,
    )
end
#
# Transparent Isometric Embedding – additionally transparent
for f in [
    distance,
    exp,
    exp!,
    inner,
    inverse_retract,
    inverse_retract!,
    log,
    log!,
    mid_point,
    mid_point!,
    project!,
    project,
    retract,
    retract!,
    vector_transport_along,
    vector_transport_direction,
    vector_transport_direction!,
    vector_transport_to,
]
    eval(
        quote
            function decorator_transparent_dispatch(
                ::typeof($f),
                ::AbstractEmbeddedManifold{𝔽,<:TransparentIsometricEmbedding},
                args...,
            ) where {𝔽}
                return Val(:transparent)
            end
        end,
    )
end
#
# For explicit EmbeddingManifolds the following have to be reimplemented (:intransparent)
for f in [embed, project]
    eval(
        quote
            function decorator_transparent_dispatch(
                ::typeof($f),
                ::EmbeddedManifold,
                args...,
            )
                return Val(:intransparent)
            end
        end,
    )
end

# unified vector transports for the three already implemented cases,
# where _direction! still has its nice fallback
for f in [vector_transport_along!, vector_transport_to!]
    eval(
        quote
            function decorator_transparent_dispatch(
                ::typeof($f),
                ::AbstractEmbeddedManifold{𝔽,<:E},
                Y,
                p,
                X,
                q,
                ::T,
            ) where {𝔽,T,E}
                return Val(:intransparent)
            end
            function decorator_transparent_dispatch(
                ::typeof($f),
                ::AbstractEmbeddedManifold{𝔽,<:TransparentIsometricEmbedding},
                Y,
                p,
                X,
                q,
                ::T,
            ) where {𝔽,T}
                return Val(:transparent)
            end
        end,
    )
    for m in [PoleLadderTransport, SchildsLadderTransport, ScaledVectorTransport]
        eval(
            quote
                function decorator_transparent_dispatch(
                    ::typeof($f),
                    ::AbstractEmbeddedManifold{𝔽,<:E},
                    Y,
                    p,
                    X,
                    q,
                    ::$m,
                ) where {𝔽,E}
                    return Val(:parent)
                end
                function decorator_transparent_dispatch(
                    ::typeof($f),
                    ::AbstractEmbeddedManifold{𝔽,<:TransparentIsometricEmbedding},
                    Y,
                    p,
                    X,
                    q,
                    ::$m,
                ) where {𝔽}
                    return Val(:parent)
                end
            end,
        )
    end
end


text:
macro inline_widget(ex)
    gname, name = ex.args
    quote
        function $(esc(name))(args...; props...)
            widget = $(esc(gname))(args...)
            length(props) > 0 && set!(widget; props...)
            return widget
        end
    end
end

macro container_widget(ex)
    gname, name = ex.args
    quote
        function $(esc(name))(children::Union{Function, GtkWidget, Tuple, Vector}, args...; props...)
            widget = $(esc(gname))(args...)
            length(props) > 0 && set!(widget; props...)
            add!(widget, children)
            return widget
        end
    end
end

function SpinButton(range::AbstractRange; init::Real = middle(range), props...)
    spin = GtkSpinButton(range)
    length(props) > 0 && set!(spin; props...)
    set_gtk_property!(GAccessor.adjustment(spin), :value, init)
    return spin
end

@inline_widget GtkLabel --> Label

macro label_str(text)
    :( Label($(esc(text))) )
end

@inline_widget GtkEntry --> Entry

macro entry_str(text)
    :( Entry(text = $(esc(text))) )
end

@inline_widget GtkScale         -->  Scale
@inline_widget GtkButton        -->  Button
@inline_widget GtkSpinner       -->  Spinner
@inline_widget GtkTextView      -->  TextView
@inline_widget GtkStatusbar     -->  StatusBar
@inline_widget Gtk.GtkSwitch    -->  Switch
@inline_widget GtkLinkButton    -->  LinkButton
@inline_widget GtkFontButton    -->  FontButton
@inline_widget GtkAppChooser    -->  AppChooser
@inline_widget GtkColorButton   -->  ColorButton
@inline_widget GtkProgressBar   -->  ProgressBar
@inline_widget GtkFileChooser   -->  FileChooser
@inline_widget GtkCheckButton   -->  CheckBox
@inline_widget GtkToggleButton  -->  ToggleButton
@inline_widget GtkComboBoxText  -->  TextList
@inline_widget GtkVolumeButton  -->  VolumeButton
@inline_widget GtkCanvas        -->  Canvas

function ColorButton(color::Colorant; props...)
    ColorButton(convert(Gtk.GdkRGBA, color); props...)
end

function ColorButton(color::String; props...)
    ColorButton(parse(Colorant, color); props...)
end

macro colorbtn_str(color)
    :( ColorButton($(esc(color))) )
end

function Slider(range::AbstractRange; init::Real = middle(range), props...)
    slider = Scale(false, range; props...)
    set_gtk_property!(GAccessor.adjustment(slider), :value, init)
    return slider
end

macro button_str(text)
    :( Button($(esc(text))) )
end

@container_widget GtkBox --> Box
@container_widget GtkFrame --> Frame
@container_widget GtkPaned --> Paned
@container_widget GtkWindow --> Window
@container_widget GtkMenuBar --> MenuBar
@container_widget GtkToolbar --> Toolbar
@container_widget GtkTreeView --> TreeView
@container_widget GtkNotebook --> Notebook
@container_widget GtkScrolledWindow --> ScrolledWindow
@container_widget GtkExpander --> Expander


text:
# general fallback
function logmean(a, b)
    (a-b) / (log(a/b))
end

"""
    logmean(a::Real, b::Real)

The logarithmic mean value (a - b) / ( log(a) - log(b) ).
"""
function logmean(_a::T, _b::T) where T<:Real
    a, b = minmax(_a,_b)
    if a < 0
        throw(DomainError("Cannot compute logarithmic mean of negative valuess."))
    end

    if a == b
        a
    else
        (a-b) / (log(a/b))
    end
end
logmean(a::Real, b::Real) = logmean(promote(a,b)...)

function logmean(_a::Float64, _b::Float64)
    a, b = minmax(_a,_b)
    if a < 0
        throw(DomainError("Cannot compute logarithmic mean of negative valuess."))
    end

    if a == b
        return a
    end

    # see Ismail, Roe (2009): Affordable, entropy consistent...
    ζ = a / b
    f = (ζ-1) / (ζ+1)
    u = f*f
    if u < 1.0e-2
        F = @evalpoly(u, 1, 1/3, 1/5, 1/7)
    else
        F = log(ζ)/(2f)
    end
    (a+b)/(2F)
end

"""
    integrate(func, u::AbstractArray{U,1}, meshx::AbstractMesh1D)

Map the function `func` to the coefficients `u` and integrate with respect to
the mesh `meshx`.
"""
function integrate(func, u::AbstractArray{U,1}, meshx::AbstractMesh1D) where U
    @boundscheck begin
        @assert numcells(meshx) == size(u,1)
    end

    res = zero(func(first(u)))
    @inbounds for ix in cell_indices(meshx)
        res += func(u[ix]) * volume(ix, meshx)
    end
    res
end

"""
    integrate(u::AbstractArray{U,1}, meshx::AbstractMesh1D)

Integrate the coefficients `u` with respect to the mesh `meshx`.
"""
function integrate(u::AbstractArray{U,1}, meshx::AbstractMesh1D) where U
    integrate(identity, u, meshx)
end

function integrate(func, u, fv::UniformPeriodicReconstructedFV1D)
    integrate(func, u, fv.meshx)
end

"""
    integrate(func, u::AbstractArray{U,2}, meshx::AbstractMesh1D, basis::NodalBasis)

Map the function `func` to the coefficients `u` and integrate with respect to
the mesh `meshx` and the polynomial basis `basis`.
"""
function integrate(func, u::AbstractArray{U,2}, meshx::AbstractMesh1D, basis::NodalBasis) where U
    @unpack weights = basis
    Pp1 = length(weights)
    @boundscheck begin
        @assert Pp1 == size(u,1)
        @assert numcells(meshx) == size(u,2)
    end

    res = zero(func(first(u)))
    @inbounds for ix in cell_indices(meshx)
        jac = volume(ix, meshx) / 2
        for nx in 1:Pp1
            res += func(u[nx,ix]) * weights[nx] * jac
        end
    end
    res
end

"""
    integrate(u::AbstractArray{U,2}, meshx::AbstractMesh1D, basis::NodalBasis)

Integrate the coefficients `u` with respect to the mesh `meshx` and the
polynomial basis `basis`.
"""
function integrate(u::AbstractArray{U,2}, meshx::AbstractMesh1D, basis::NodalBasis) where U
    integrate(identity, u, meshx, basis)
end

"""
    integrate(func, u::AbstractArray{U,4}, meshx::AbstractMesh1D, meshy::AbstractMesh1D, basis::NodalBasis)

Map the function `func` to the coefficients `u` and integrate with respect to
the meshes `meshx, meshy` and the polynomial basis `basis`.
"""
function integrate(func, u::AbstractArray{U,4}, meshx::AbstractMesh1D, meshy::AbstractMesh1D, basis::NodalBasis) where U
    @unpack weights = basis
    Pp1 = length(weights)
    @boundscheck begin
        @assert Pp1 == size(u,1) == size(u,2)
        @assert numcells(meshx) == size(u,3)
        @assert numcells(meshy) == size(u,4)
    end

    res = zero(func(first(u)))
    @inbounds for iy in cell_indices(meshy), ix in cell_indices(meshx)
        jac = volume(ix, meshx) * volume(iy, meshy) / 4
        for ny in 1:Pp1, nx in 1:Pp1
            res += func(u[nx,ny,ix,iy]) * weights[nx] * weights[ny] * jac
        end
    end
    res
end

"""
    integrate(u::AbstractArray{U,4}, meshx::AbstractMesh1D, meshy::AbstractMesh1D, basis::NodalBasis)

Integrate the coefficients `u` with respect to the meshes `meshx, meshy` and the
polynomial basis `basis`.
"""
function integrate(u::AbstractArray{U,4}, meshx::AbstractMesh1D, meshy::AbstractMesh1D, basis::NodalBasis) where U
    integrate(identity, u, meshx, meshy, basis)
end

"""
    integrate(func, u::AbstractArray{U,6}, meshx, meshy, meshz, basis::NodalBasis)

Map the function `func` to the coefficients `u` and integrate with respect to
the meshes `meshx, meshy, meshz` and the polynomial basis `basis`.
"""
function integrate(func, u::AbstractArray{U,6}, meshx::AbstractMesh1D, meshy::AbstractMesh1D, meshz::AbstractMesh1D, basis::NodalBasis) where U
    @unpack weights = basis
    Pp1 = length(weights)
    @boundscheck begin
        @assert Pp1 == size(u,1) == size(u,2) == size(u,3)
        @assert numcells(meshx) == size(u,4)
        @assert numcells(meshy) == size(u,5)
        @assert numcells(meshz) == size(u,6)
    end

    res = zero(func(first(u)))
    @inbounds for iz in cell_indices(meshz), iy in cell_indices(meshy), ix in cell_indices(meshx)
        jac = volume(ix, meshx) * volume(iy, meshy) * volume(iz, meshz)  / 8
        for nz in 1:Pp1, ny in 1:Pp1, nx in 1:Pp1
            res += func(u[nx,ny,nz,ix,iy,iz]) * weights[nx] * weights[ny] * weights[nz] * jac
        end
    end
    res
end

"""
    integrate(u::AbstractArray{U,6}, meshx, meshy, meshz, basis::NodalBasis)

Integrate the coefficients `u` with respect to the meshes `meshx, meshy, meshz`
and the polynomial basis `basis`.
"""
function integrate(u::AbstractArray{U,6}, meshx::AbstractMesh1D, meshy::AbstractMesh1D, meshz::AbstractMesh1D, basis::NodalBasis) where U
    integrate(identity, u, meshx, meshy, meshz, basis)
end


text:
@testset "Fearnhead style mixture model particles" begin

    @testset "Cutoff for automatic propogation" begin

        using Particles: cutoff_ascending

        # a stub to replace the original cutoff using cutoff_ascending
        function cutoff(ws, N)
            ws = sort(ws)
            i_keep, w = cutoff_ascending(ws, N)
            return length(ws) - i_keep + 2, w, sum(ws)
        end

        function cutoff_normalized(ws::Vector{<:Real}, N::Int)
            ws = ws ./ sum(ws)
            total = sum(ws)
            for i in eachindex(ws)
                if total / ws[i] + (i-1) >= N
                    return i, total/(N-i+1)
                end
                total -= ws[i]
            end
        end

        function cutoff_while(ws::Vector{<:Real}, N::Int)
            ws = ws ./ sum(ws)
            total = sum(ws)
            i = 1
            while total / ws[i] + (i-1) < N
                total, i = total - ws[i], i+1
                i > length(ws) && error("infinite loop??")
            end
            i, total / (N-i+1)
        end

        ws = sort([1e10, exp.(randn(19))...], rev=true)
        keep, cut, tot = cutoff(ws, 10)
        # @show cutoff_normalized(ws,10)[2], cut, tot,  cut/tot

        # check correctness of the algorithm:  should be the case that
        # \sum_i=1^M min( c*w_i, 1 ) = N
        @test isapprox(sum(min.(ws ./ cut, 1)), 10, atol=1e-5)

        @test isapprox(cutoff_normalized(ws, 10)[2], cut/tot, atol=1e-10)
        @test isapprox(cutoff_while(ws, 10)[2], cut/tot, atol=1e-10)

        ws = sort(exp.(randn(20)), rev=true)
        keep, cut, tot = cutoff(ws, 10)
        @test isapprox(cutoff_normalized(ws, 10)[2], cut/tot, atol=1e-10)
        @test isapprox(cutoff_while(ws, 10)[2], cut/tot, atol=1e-10)

        # when weights all equal, resample all with weight 1/N
        ws = [1. for _ in 1:20]
        keep, cut, tot = cutoff(ws, 10)
        @test isapprox(cutoff_normalized(ws, 10)[2], cut/tot, atol=1e-10)
        @test isapprox(cutoff_while(ws, 10)[2], cut/tot, atol=1e-10)

        @test keep == 1         # first particle NOT kept
        @test cut/tot ≈ 1/10

        # when weights are all equal or zero, also resample all with weight 1/N
        # ...actually, in THIS case you effectively only HAVE N particles withn
        # non-zero weight so you should keep them all, which is what
        # cutoff-ascending gives you (unlike the old solution).
        ws = [i > 10 ? 0. : 1. for i in 1:20]
        keep, cut, tot = cutoff(ws, 10)
        @test keep == 11

        ws = [i > 10 ? 0. : rand() for i in 1:20]
        keep, cut, tot = cutoff(ws, 10)
        @test keep == 11

    end

    @testset "stratified sampling" begin

        Random.seed!(1999)
        w = exp.(2*rand(100))
        w ./= sum(w)
        x = collect(1:100)

        for n in [1, 8, 64, 90, 99]
            for _ in 1:100
                x_samp = Particles.sample_stratified(x, n, w)
                @test length(x_samp) ≤ n
            end
        end

    end

end


text:
using DSP
export Amplitude, Phase, Hilbert

abstract type InstantaneousSignalProperty end

"""
    Amplitude <: InstantaneousSignalProperty

Indicates that the instantaneous amplitudes of a signal should be used. """
struct Amplitude <: InstantaneousSignalProperty end

"""
    Phase <: InstantaneousSignalProperty

Indicates that the instantaneous phases of a signal should be used. """
struct Phase <: InstantaneousSignalProperty end

"""
    Hilbert(est;
        source::InstantaneousSignalProperty = Phase(),
        target::InstantaneousSignalProperty = Phase(),
        cond::InstantaneousSignalProperty = Phase())
    ) <: TransferEntropyEstimator

Compute transfer entropy on instantaneous phases/amplitudes of relevant signals, which are
obtained by first applying the Hilbert transform to each signal, then extracting the
phases/amplitudes of the resulting complex numbers[^Palus2014]. Original time series are
thus transformed to instantaneous phase/amplitude time series. Transfer
entropy is then estimated using the provided `est` on those phases/amplitudes (use e.g.
[`VisitationFrequency`](@ref), or [`SymbolicPermutation`](@ref)).

!!! info
    Details on estimation of the transfer entropy (conditional mutual information)
    following the phase/amplitude extraction step is not given in Palus (2014). Here,
    after instantaneous phases/amplitudes have been obtained, these are treated as regular
    time series, from which transfer entropy is then computed as usual.

See also: [`Phase`](@ref), [`Amplitude`](@ref).

[^Palus2014]: Paluš, M. (2014). Cross-scale interactions and information transfer. Entropy, 16(10), 5263-5289.
"""
struct Hilbert <: TransferEntropyEstimator
    source::InstantaneousSignalProperty
    target::InstantaneousSignalProperty
    cond::InstantaneousSignalProperty
    est

    function Hilbert(est::VisitationFrequency;
            source::InstantaneousSignalProperty = Phase(),
            target::InstantaneousSignalProperty = Phase(),
            cond::InstantaneousSignalProperty = Phase())
        new(source, target, cond, est)
    end
end

function transferentropy(source, target, est::Hilbert; base = 2, q = 1,
        τT = -1, τS = -1, η𝒯 = 1, dT = 1, dS = 1, d𝒯 = 1)

    hil_s = DSP.hilbert(source)
    hil_t = DSP.hilbert(target)

    if est.source isa Phase
        s = angle.(hil_s)
    elseif est.source isa Amplitude
        s = abs.(hil_s)
    else
        error("est.source must be either Phase or Amplitude instance")
    end

    if est.target isa Phase
        t = angle.(hil_t)
    elseif est.target isa Amplitude
        t = abs.(hil_t)
    else
        error("est.target must be either Phase or Amplitude instance")
    end

    # Now, estimate transfer entropy on the phases/amplitudes with the given estimator.
    transferentropy(s, t, est.est, τT = τT, τS = τS, η𝒯 = η𝒯, dT = dT, dS = dS, d𝒯 = d𝒯)
end

function transferentropy(source, target, cond, est::Hilbert; base = 2, q = 1,
        τT = -1, τS = -1, τC = -1, η𝒯 = 1, dT = 1, dS = 1, dC = 1, d𝒯 = 1)

    hil_s = DSP.hilbert(source)
    hil_t = DSP.hilbert(target)
    hil_c = DSP.hilbert(cond)

    if est.source isa Phase
        s = angle.(hil_s)
    elseif est.source isa Amplitude
        s = abs.(hil_s)
    else
        error("est.source must be either Phase or Amplitude instance")
    end

    if est.target isa Phase
        t = angle.(hil_t)
    elseif est.target isa Amplitude
        t = abs.(hil_t)
    else
        error("est.target must be either Phase or Amplitude instance")
    end

    if est.cond isa Phase
        c = angle.(hil_c)
    elseif est.cond isa Amplitude
        c = abs.(hil_c)
    else
        error("est.cond must be either Phase or Amplitude instance")
    end

    transferentropy(s, t, c, est.est,
        τT = τT, τS = τS, τC = τC, η𝒯 = η𝒯, dT = dT, dS = dS, dC = dC, d𝒯 = d𝒯)
end


text:
"""

 █████╗  ██████╗██████╗ ██╗
██╔══██╗██╔════╝██╔══██╗██║
███████║██║     ██████╔╝██║
██╔══██║██║     ██╔══██╗██║
██║  ██║╚██████╗██║  ██║███████╗
╚═╝  ╚═╝ ╚═════╝╚═╝  ╚═╝╚══════╝

File:       rendezvous_simple_case.jl
Author:     Gabriel Barsi Haberfeld, 2020. gbh2@illinois.edu
Function:   This program simulates all results in the paper "Geometry-Informed
            Minimum Risk Rendezvous Algorithm for Heterogeneous Agents in Urban
            Environments"

Instructions:   Run this file in juno with Julia 1.2.0 or later.
Requirements:   JuMP, Ipopt, Plots, LinearAlgebra, BenchmarkTools.

"""

using JuMP, Ipopt
using Plots, LinearAlgebra
using BenchmarkTools
using Random
using RecipesBase
using SparseArrays
using Statistics
using Printf
using LaTeXStrings
using Measures
import Distributions: MvNormal
import Random.seed!
include("bayeslin.jl")
default(dpi=100)

function solveRDV(x0,y0,t0,Lx,Ly,Ax,Ay,vmax,tmax,rem_power,μ,Σ,θ,N,xp,yp,tt=ones(4),vp=0.0)

    RDV = Model(with_optimizer(Ipopt.Optimizer,print_level=0,max_iter=500))
    @variable(RDV, x[i=1:5]) #states
    @variable(RDV, y[i=1:5]) #states

    @variable(RDV, -vmax <= vx[i=1:4] <= vmax) #controls
    @variable(RDV, -vmax <= vy[i=1:4] <= vmax) #controls
    @variable(RDV, 0.1 <= t[i=1:4]) #controls

    T_R = @expression(RDV, sum(t[i] for i=1:2))
    r = length(μ)
    t1 = t0
    t2 = @expression(RDV, t[2])
    θ_R = @NLexpression(RDV, θ + μ[1]*((t[1] + t[2]) - t1) +
        μ[2]*((t[1] + t[2])/100 - t1/100 - (sin((2*t1*pi)/5) - sin((2*(t[1]
        + t[2])*pi)/5))/(40*pi)) +
        μ[3]*((3*(t[1] + t[2]))/20000 - (3*t1)/20000 - (sin((2*t1*pi)/5)/2000
        + sin((4*t1*pi)/5)/16000)/pi + (sin((2*(t[1] + t[2])*pi)/5)/2000
        + sin((4*(t[1] + t[2])*pi)/5)/16000)/pi))
    P1 = @NLexpression(RDV, (t[1] + t[2]) - t1)
    P2 = @NLexpression(RDV, ((t[1] + t[2])/100 - t1/100 - (sin((2*t1*pi)/5) - sin((2*(t[1]
    + t[2])*pi)/5))/(40*pi)))
    P3 = @NLexpression(RDV, ((3*(t[1] + t[2]))/20000 - (3*t1)/20000 - (sin((2*t1*pi)/5)/2000
    + sin((4*t1*pi)/5)/16000)/pi + (sin((2*(t[1] + t[2])*pi)/5)/2000
    + sin((4*(t[1] + t[2])*pi)/5)/16000)/pi))

    Σv = @NLexpression(RDV, P1*(P1*Σ[1,1] + P2*Σ[2,1] + P3*Σ[3,1]) +
                            P2*(P1*Σ[1,2] + P2*Σ[2,2] + P3*Σ[3,2]) +
                            P3*(P1*Σ[1,3] + P2*Σ[2,3] + P3*Σ[3,3]))

    @NLobjective(RDV, Min,
                  1.0*sum(vx[i]^2*t[i] + vy[i]^2*t[i] for i=[1 2 4]) #delivery
                + 0.3*(vx[3]^2*t[3] + vy[3]^2*t[3]) #cost fcn after delivery
                + 0.0*sum(t[i] for i=2:4)   #cost fcn min time
                + 1.0*(N>=0)*Σv # activate risk min when samples are enough
                - 1.0*(N<=100)*t[1] #cost fcn max decision time
                - 0.0*t[1])
    @NLconstraint(RDV, abs(t[1] - tt[1]) <= 0.1)
    if xp != 0.0 || yp != 0.0
        @NLconstraint(RDV, abs(x[2] - xp) <= 0.01)
        @NLconstraint(RDV, abs(y[2] - yp) <= 0.01)
    end
    #@constraint(RDV, 2.0 .<= tt[2:end])
    #@NLconstraint(RDV, abs(vp[1] - vx[1]) <= 1.3)
    #@NLconstraint(RDV, abs(vp[2] - vy[1]) <= 1.3)
    @constraint(RDV, x[1] == x0) #initial conditions
    @constraint(RDV, y[1] == y0)
    for i = 2:4
        @constraint(RDV, x[i] == x[i-1] + vx[i-1]*t[i-1]) #x Dynamics
        @constraint(RDV, y[i] == y[i-1] + vy[i-1]*t[i-1]) #y Dynamics
    end

    @constraint(RDV, x[5] == x[2] + vx[4]*t[4]) #Abort Dynamics Constraints
    @constraint(RDV, y[5] == y[2] + vy[4]*t[4])
    @constraint(RDV, x[5] == Ax)
    @constraint(RDV, y[5] == Ay)

    @NLconstraint(RDV, sum((vx[i]^2 + vy[i]^2)*t[i] for i=1:3) <= rem_power)
    @NLconstraint(RDV, sum((vx[i]^2 + vy[i]^2)*t[i] for i=[1 4]) <= rem_power)

    if N >= 50 #activate risk bounds when enough samples
        @NLconstraint(RDV, Σv <= 2.5)
    else
        @NLconstraint(RDV, Σv <= 5.0)
    end
    @NLconstraint(RDV, θ_R <= 0.8)
    @NLconstraint(RDV, 0.2 + θ <= θ_R)
    @constraint(RDV, x[4] == Lx)
    @constraint(RDV, y[4] == Ly)
    #@NLconstraint(RDV, 0.01 <= vx[1]^2 + vy[1]^2)
    #@constraint(RDV, t[1] <= 10.0)
    @constraint(RDV, sum(t[i] for i=1:3) <= tmax)
    @constraint(RDV, sum(t[i] for i=[1 4]) <= tmax)
    @NLconstraint(RDV, x[3] == 5 - 4.5 * sin(2*pi*θ_R))
    @NLconstraint(RDV, y[3] == ( 10 * θ_R - 5 ))
    optimize!(RDV)

    t2 = sum(value.(t)[i] for i=1:2)
    t1 = t0

    θ_R = θ + μ[1]*(t2 - t1) +
        μ[2]*(t2/100 - t1/100 - (sin((2*t1*pi)/5) - sin((2*t2*pi)/5))/(40*pi)) +
        μ[3]*((3*t2)/20000 - (3*t1)/20000 - (sin((2*t1*pi)/5)/2000 +
        sin((4*t1*pi)/5)/16000)/pi + (sin((2*t2*pi)/5)/2000 +
        sin((4*t2*pi)/5)/16000)/pi)
    #@show θ_R

    return value.(x), value.(y), value.(vx), value.(vy), value.(t), θ_R
end

function path(θ)
    x = 5 .- 4.5 .* sin.(2*pi*θ)
    y = 10 .* θ .- 5
    return x, y
end

function fit_behavior(N, α=0.005, β=1/(0.3^2), r=0:2; seeded=false)
    #D(v,β=Inf) = 0.0 + 1.1*v + 0.1*sin(1*pi*v) + 1/β*randn()
    #deviation function, from θ̇.
    if seeded
        seed!(1729)
    end
    Xo = 0 .+ rand(N)./1.0 #random samples
    Yo = D.(Xo, β) #observed deviation
    Xt = collect(-0.0:0.005:1.2)
    Yt = D.(Xt) #actual deviation
    regress(Xo, Yo, Xt, Yt, polynomial, α, β, r)
end

function fit_weights(N, α=0.005, β=1/(0.3^2), r=0:2)
    D(v,β=Inf) = 2/(1+exp(-5*v)) - 1 + 1/β*randn()
    Xo = rand(N) #random samples
    Yo = D.(Xo, β) #observed deviation
    μ, Σ = posterior(Yo, polynomial(Xo, r), α, β)
end

function fit_weights_filtered(N, μ0, α=0.005, β=1/(0.3^2), r=0:2)
    Xo = rand(N) #random samples
    Yo = D.(Xo, β) #observed deviation
    μ, Σ = posterior(Yo, polynomial(Xo, r), α, β)
    μ = 0.2.*μ + 0.8.*μ0
    return μ, Σ
end

function dynamics(x, y, vx, vy, t, dt, rem_power)
    x           = x + vx*dt
    y           = y + vy*dt
    t           = t + dt
    rem_power   = rem_power - vx^2*dt - vy^2*dt #TODO check if this is correct
    return x, y, rem_power, t
end

function run_fit(N)
    model = fit_behavior(N, 0.005, 1/(0.3^2), 0:2)
    plot(model, xlabel="Historic Speed", ylabel="Driver's Speed",background_color="white",xlims=(0,1),ylims=(0,1.5))
end

function find_t_end(μf,tmax,tbound,θ0=0.0,t0=0.0)
    for c=t0:0.001:tbound
        if θ0 + μf(t0,c) >= 1.0
            return c - t0
        end#end if
    end#end for
    println("Couldn't find end of trajectory, refitting")
    return tmax
end#end fun

function plot_sol(N=100,bg="black",t0=0.0,θ0=0.0,x0=0.0,y0=0.0)

    μ, Σ = fit_weights(N)
    θ̇(t) = (0.1 .+ 0.1.*cos.(4*pi.*t/10))

    ϕf(t1,t2) = [t2 - t1,
        (t2/100 - t1/100 - (sin((2*t1*pi)/5) - sin((2*t2*pi)/5))/(40*pi)),
        ((3*t2)/20000 - (3*t1)/20000 - (sin((2*t1*pi)/5)/2000 + sin((4*t1*pi)/5)/16000)/pi + (sin((2*t2*pi)/5)/2000 + sin((4*t2*pi)/5)/16000)/pi)]
    Σf(t1,t2)=ϕf(t1,t2)'*Σ*ϕf(t1,t2)
    μf(t1,t2) = μ[1]*(t2 - t1) +
        μ[2]*(t2/100 - t1/100 - (sin((2*t1*pi)/5) - sin((2*t2*pi)/5))/(40*pi)) +
        μ[3]*((3*t2)/20000 - (3*t1)/20000 - (sin((2*t1*pi)/5)/2000 +
        sin((4*t1*pi)/5)/16000)/pi + (sin((2*t2*pi)/5)/2000 +
        sin((4*t2*pi)/5)/16000)/pi)
    D(v,β=Inf) = 2/(1+exp(-5*v)) - 1 + 1/β*randn()
    P1(t1,t2) = (t2 - t1)
    P2(t1,t2) = (t2/100 - t1/100 - (sin((2*t1*pi)/5) - sin((2*t2*pi)/5))/(40*pi))
    P3(t1,t2) = ((3*t2)/20000 - (3*t1)/20000 - (sin((2*t1*pi)/5)/2000 +
    sin((4*t1*pi)/5)/16000)/pi + (sin((2*t2*pi)/5)/2000 +
    sin((4*t2*pi)/5)/16000)/pi)

    Σv(t1,t2) = P1(t1,t2)*(P1(t1,t2)*Σ[1,1] + P2(t1,t2)*Σ[2,1] + P3(t1,t2)*Σ[3,1]) +
                P2(t1,t2)*(P1(t1,t2)*Σ[1,2] + P2(t1,t2)*Σ[2,2] + P3(t1,t2)*Σ[3,2]) +
                P3(t1,t2)*(P1(t1,t2)*Σ[1,3] + P2(t1,t2)*Σ[2,3] + P3(t1,t2)*Σ[3,3])
    tbound = 50.0
    tmax = 50.0
    tmax = find_t_end(μf,tmax,tbound,θ0,t0)
    #@show tmax
    x, y, vx, vy, t, θ_R = solveRDV(x0,y0,t0,Lx,Ly,Ax,Ay,vmax,tmax,rem_power,μ,Σ,θ0,N,x0,y0)
    T_R = sum(t[1:2])
    risk = Σv(t0,T_R)
    #@show θ_R T_R risk
    Σe = vx'.^2*t + vy'.^2*t
    Σs = sum(sqrt.(x.^2+y.^2))
    Σt = sum(t)
    Δt = Σt - tmax
    p = path(θ_R)
    Δx = abs(x[3] - p[1])
    Δy = abs(y[3] - p[2])
    @show x y vx vy t
    rR = Σv(t0,sum(t[1:2]))*(abs.(vx[1:2])'*t[1:2] + abs.(vy[1:2])'*t[1:2])/rem_power
    rA = Σv(t0,sum(t[1:2]))
    t1 = Float64(t[1])
    s = @sprintf("rho_R = %-15.4f\n rho_A = %-15.4f\n t1 = %-15.4f",rR,rA,t1)
    plot()
    plot!(path(collect(0:0.01:1)),color="gray",width=2.0)
    plot_path(1000,Σ,t0,θ0,tmax,bg,true)
    plot!(x[1:3],y[1:3],background_color=bg,width=3.0,color=:steelblue)
    plot!([x[2] ;x[5]],[y[2] ;y[5]],background_color=bg,width=1.0,color="gray",linestyle=:dash)
    plot!([x[3] ;x[4]],[y[3] ;y[4]],background_color=bg,width=1.0,color="gray",linestyle=:dash)
    scatter!([x[2]],[y[2]],background_color=bg,markersize=3.0,color=:grey)
    scatter!([x[1]],[y[1]],background_color=bg,markersize=7.0,markershape=:pentagon,color=:blue)
    scatter!([x[4]],[y[4]],background_color=bg,markersize=7.0,markershape=:utriangle,color=:orange)
    scatter!([x[5]],[y[5]],background_color=bg,markersize=7.0,markershape=:utriangle,color=:red)
    scatter!(path(θ0),background_color=bg,markersize=10.0,markershape=:square,color=:green,annotation=(2.5,3.0,s))
    p = scatter!(path(θ_R),legend=false,background_color=bg,markersize=7.0,xlims = (-1,11),ylims = (-6,6),color=:cyan)

    display(p)
    return vx[1], vy[1]
end

function plot_sol_filtered(μ0,N=100,bg="black")

    μ, Σ = fit_weights_filtered(N,μ0)
    θ̇(t) = (0.1 + 0.05*cos(4*pi*t/10))
    #=ϕf(t1,t2) = [t2 - t1,
        t2/10 - t1/10 - (sin((2*t1*pi)/5) - sin((2*t2*pi)/5))/(8*pi),
        (9*t2)/800 - (9*t1)/800 - (sin((2*t1*pi)/5)/40 + sin((4*t1*pi)/5)/640)/pi + (sin((2*t2*pi)/5)/40 + sin((4*t2*pi)/5)/640)/pi]
    Σf(t1,t2)=ϕf(t1,t2)'*Σ*ϕf(t1,t2)
    μf(t1,t2) = μ[1]*(t2 - t1) +
        μ[2]*(t2/10 - t1/10 - (sin((2*t1*pi)/5) - sin((2*t2*pi)/5))/(8*pi)) +
        μ[3]*((9*t2)/800 - (9*t1)/800 - (sin((2*t1*pi)/5)/40 + sin((4*t1*pi)/5)/640)/pi + (sin((2*t2*pi)/5)/40 + sin((4*t2*pi)/5)/640)/pi)
    D(v,β=Inf) = 2/(1+exp(-5*v)) - 1 + 1/β*randn()
    P1(t1,t2) = (t2 - t1)
    P2(t1,t2) = (t2)/10 - t1/10 - (sin((2*t1*pi)/5) -
        sin((2*(t2)*pi)/5))/(8*pi)
    P3(t1,t2) = (9*(t2))/800 - (9*t1)/800 -
        (sin((2*t1*pi)/5)/40 + sin((4*t1*pi)/5)/640)/pi +
        (sin((2*(t2)*pi)/5)/40 + sin((4*(t2)*pi)/5)/640)/pi

    =#

    ϕf(t1,t2) = [t2 - t1,
        (t2/100 - t1/100 - (sin((2*t1*pi)/5) - sin((2*t2*pi)/5))/(40*pi)),
        ((3*t2)/20000 - (3*t1)/20000 - (sin((2*t1*pi)/5)/2000 + sin((4*t1*pi)/5)/16000)/pi + (sin((2*t2*pi)/5)/2000 + sin((4*t2*pi)/5)/16000)/pi)]
    Σf(t1,t2)=ϕf(t1,t2)'*Σ*ϕf(t1,t2)
    μf(t1,t2) = μ[1]*(t2 - t1) +
        μ[2]*(t2/100 - t1/100 - (sin((2*t1*pi)/5) - sin((2*t2*pi)/5))/(40*pi)) +
        μ[3]*((3*t2)/20000 - (3*t1)/20000 - (sin((2*t1*pi)/5)/2000 +
        sin((4*t1*pi)/5)/16000)/pi + (sin((2*t2*pi)/5)/2000 +
        sin((4*t2*pi)/5)/16000)/pi)
    D(v,β=Inf) = 2/(1+exp(-5*v)) - 1 + 1/β*randn()
    P1(t1,t2) = (t2 - t1)
    P2(t1,t2) = (t2/100 - t1/100 - (sin((2*t1*pi)/5) - sin((2*t2*pi)/5))/(40*pi))
    P3(t1,t2) = ((3*t2)/20000 - (3*t1)/20000 - (sin((2*t1*pi)/5)/2000 +
    sin((4*t1*pi)/5)/16000)/pi + (sin((2*t2*pi)/5)/2000 +
    sin((4*t2*pi)/5)/16000)/pi)

    Σv(t1,t2) = P1(t1,t2)*(P1(t1,t2)*Σ[1,1] + P2(t1,t2)*Σ[2,1] + P3(t1,t2)*Σ[3,1]) +
                P2(t1,t2)*(P1(t1,t2)*Σ[1,2] + P2(t1,t2)*Σ[2,2] + P3(t1,t2)*Σ[3,2]) +
                P3(t1,t2)*(P1(t1,t2)*Σ[1,3] + P2(t1,t2)*Σ[2,3] + P3(t1,t2)*Σ[3,3])
    tbound = 1000.0
    tmax = 1000.0
    tmax = find_t_end(μf,tmax,tbound)
    @show tmax
    x, y, vx, vy, t, θ_R = @time solveRDV(x0,y0,t0,Lx,Ly,vmax,tmax,rem_power,μ,θ0,N)
    T_R = sum(t[1:2])
    risk = Σv(t0,T_R)
    @show θ_R T_R risk
    Σe = vx'.^2*t + vy'.^2*t
    Σs = sum(sqrt.(x.^2+y.^2))
    Σt = sum(t)
    Δt = Σt - tmax
    p = path(θ_R)
    Δx = abs(x[3] - p[1])
    Δy = abs(y[3] - p[2])

    println("Displaying resulting trajectory:")
    @show x y vx vy t
    println("Checking Rendezvous condition")
    @show Δx Δy
    println("Checking constraints:")
    @show Σe Σs Σt Δt tmax

    plot(x[1:4],y[1:4],background_color=bg,width=3.0)
    plot!([x[2];x[5]],[y[2];y[5]],background_color=bg,width=3.0,color="red")
    plot_path(1000,bg)
    scatter!(x[1:4],y[1:4],background_color=bg,markersize=10.0)
    p = scatter!(p,legend=false,background_color=bg,markersize=5.0)
    display(p)
    return μ
end

function plot_path(n,Σ,t0,θ0,tmax,bg="black",cgr=true)
    θ = Array(θ0:1.0/n:1)
    N = length(θ)
    x, y = path(θ)
    #z = θ̇.(collect(range(0,length=N,step=dt)))
    #z = z + D.(z)
    t0 = 0.0.*ones(N)
    t = collect(range(0,length=N,stop=tmax))
    P1(t1,t2) = (t2 - t1)
    P2(t1,t2) = (t2/100 - t1/100 - (sin((2*t1*pi)/5) - sin((2*t2*pi)/5))/(40*pi))
    P3(t1,t2) = ((3*t2)/20000 - (3*t1)/20000 - (sin((2*t1*pi)/5)/2000 +
    sin((4*t1*pi)/5)/16000)/pi + (sin((2*t2*pi)/5)/2000 +
    sin((4*t2*pi)/5)/16000)/pi)
    Σv(t1,t2) = P1(t1,t2)*(P1(t1,t2)*Σ[1,1] + P2(t1,t2)*Σ[2,1] + P3(t1,t2)*Σ[3,1]) +
                P2(t1,t2)*(P1(t1,t2)*Σ[1,2] + P2(t1,t2)*Σ[2,2] + P3(t1,t2)*Σ[3,2]) +
                P3(t1,t2)*(P1(t1,t2)*Σ[1,3] + P2(t1,t2)*Σ[2,3] + P3(t1,t2)*Σ[3,3])
    z = Σv.(t0,t)
    #cgrad = cgrad([:red, :yellow, :blue])

    if cgr
        mcgrad = cgrad([:blue, :yellow, :red])
        plot!(x,y,background_color=bg,lc=mcgrad,line_z=z,width=3.0)
    else
        plot!(x,y,background_color=bg,width=3.0)
    end
end

function plot_var(tf,N=100,bg="black")
    μ, Σ = fit_weights(N)
    ϕf(t1,t2) = [t2 - t1,
        (t2/100 - t1/100 - (sin((2*t1*pi)/5) - sin((2*t2*pi)/5))/(40*pi)),
        ((3*t2)/20000 - (3*t1)/20000 - (sin((2*t1*pi)/5)/2000 + sin((4*t1*pi)/5)/16000)/pi + (sin((2*t2*pi)/5)/2000 + sin((4*t2*pi)/5)/16000)/pi)]
    Σf(t1,t2)=ϕf(t1,t2)'*Σ*ϕf(t1,t2)
    μf(t1,t2) = μ[1]*(t2 - t1) +
        μ[2]*(t2/100 - t1/100 - (sin((2*t1*pi)/5) - sin((2*t2*pi)/5))/(40*pi)) +
        μ[3]*((3*t2)/20000 - (3*t1)/20000 - (sin((2*t1*pi)/5)/2000 +
        sin((4*t1*pi)/5)/16000)/pi + (sin((2*t2*pi)/5)/2000 +
        sin((4*t2*pi)/5)/16000)/pi)
    D(v,β=Inf) = 2/(1+exp(-5*v)) - 1 + 1/β*randn()
    P1(t1,t2) = (t2 - t1)
    P2(t1,t2) = (t2/100 - t1/100 - (sin((2*t1*pi)/5) - sin((2*t2*pi)/5))/(40*pi))
    P3(t1,t2) = ((3*t2)/20000 - (3*t1)/20000 - (sin((2*t1*pi)/5)/2000 +
    sin((4*t1*pi)/5)/16000)/pi + (sin((2*t2*pi)/5)/2000 +
    sin((4*t2*pi)/5)/16000)/pi)

    Σv(t1,t2) = P1(t1,t2)*(P1(t1,t2)*Σ[1,1] + P2(t1,t2)*Σ[2,1] + P3(t1,t2)*Σ[3,1]) +
                P2(t1,t2)*(P1(t1,t2)*Σ[1,2] + P2(t1,t2)*Σ[2,2] + P3(t1,t2)*Σ[3,2]) +
                P3(t1,t2)*(P1(t1,t2)*Σ[1,3] + P2(t1,t2)*Σ[2,3] + P3(t1,t2)*Σ[3,3])
    tbound = 3000.0
    tmax = tf
    tmax = find_t_end(μf,tmax,tbound)
    t = collect(range(0.0, length=N, stop=tmax))

    plot(t,Σv.(0.0,t),width=3.0,background_color=bg)
end

function MPCfy(x0,y0,θ0,Lx,Ly,Ax,Ay,vmax,tmax,dt,Ni,H,rem_power,ρ=0.2,β = 1/(0.10^2))
    D(v,β=Inf) = 2/(1+exp(-5*v)) - 1 + 1/β*randn()
    θ̇(t) = (0.01 + 0.01*cos(4*pi*t/10))
    α = 0.001
    β = 1/(0.10^2)
    r = 0:2
    @assert 0<=ρ<=1 "FIR param error"
    tvec = zeros(H)
    μv = zeros(H)
    ρv = zeros(H)
    ρRv = zeros(H)
    x = zeros(H)
    y = zeros(H)
    td = zeros(H)
    x[1] = x0
    y[1] = y0
    xp = x0
    yp = y0
    t = 0
    Xo = rand(Ni) #random samples
    Yo = D.(Xo, β) #observed deviation
    μ, Σ = posterior(Yo, polynomial(Xo, r), α, β)
    μv[1] = μ[1]
    tt = 1*ones(4)
    vp = zeros(2)
    P1(t1,t2) = (t2 - t1)
    P2(t1,t2) = (t2/100 - t1/100 - (sin((2*t1*pi)/5) - sin((2*t2*pi)/5))/(40*pi))
    P3(t1,t2) = ((3*t2)/20000 - (3*t1)/20000 - (sin((2*t1*pi)/5)/2000 +
    sin((4*t1*pi)/5)/16000)/pi + (sin((2*t2*pi)/5)/2000 +
    sin((4*t2*pi)/5)/16000)/pi)
    epsk = 0
    lx = 0.0
    ly = 4.0
    lvec = collect(range(ly, length=4, step=-0.8))
    k = 1
        anim = @animate for i = 2:H
            Xo = [Xo ; θ̇(θ0)]
            Yo = [Yo ; D(Xo[end], β)]
            μ0 = μ
            μ, Σ = posterior(Yo, polynomial(Xo, r), α, β)
            μ =  (1-ρ).*μ0 + ρ*μ
            μv[i] = μ[1]
            # ϕf(t1,t2) = [t2 - t1,
            #     (t2/100 - t1/100 - (sin((2*t1*pi)/5) - sin((2*t2*pi)/5))/(40*pi)),
            #     ((3*t2)/20000 - (3*t1)/20000 - (sin((2*t1*pi)/5)/2000 + sin((4*t1*pi)/5)/16000)/pi + (sin((2*t2*pi)/5)/2000 + sin((4*t2*pi)/5)/16000)/pi)]
            # Σf(t1,t2)=ϕf(t1,t2)'*Σ*ϕf(t1,t2)
            μf(t1,t2) = μ[1]*(t2 - t1) +
                μ[2]*(t2/100 - t1/100 - (sin((2*t1*pi)/5) - sin((2*t2*pi)/5))/(40*pi)) +
                μ[3]*((3*t2)/20000 - (3*t1)/20000 - (sin((2*t1*pi)/5)/2000 +
                sin((4*t1*pi)/5)/16000)/pi + (sin((2*t2*pi)/5)/2000 +
                sin((4*t2*pi)/5)/16000)/pi)

            Σv(t1,t2) = P1(t1,t2)*(P1(t1,t2)*Σ[1,1] + P2(t1,t2)*Σ[2,1] + P3(t1,t2)*Σ[3,1]) +
                        P2(t1,t2)*(P1(t1,t2)*Σ[1,2] + P2(t1,t2)*Σ[2,2] + P3(t1,t2)*Σ[3,2]) +
                        P3(t1,t2)*(P1(t1,t2)*Σ[1,3] + P2(t1,t2)*Σ[2,3] + P3(t1,t2)*Σ[3,3])
            tbound = 100.0
            tmax = 50.0
            tmax = find_t_end(μf,tmax,tbound,θ0,t)
            tvec[i-1] = tmax
            xt, yt, vx, vy, tt, θ_R = solveRDV(x[i-1],y[i-1],t,Lx,Ly,Ax,Ay,vmax,tmax,rem_power,μ,Σ,θ0,length(Xo),xp,yp,tt,vp)
            vp = [vx[1] vy[1]]
            xp = xt[2]
            yp = yt[2]
            Σe = vx'.^2*t + vy'.^2*t
            bg = "white"
            rA = Σv(t,sum(tt[1:2]))*(abs.(vx[1:2])'*tt[1:2] + abs.(vy[1:2])'*tt[1:2])/rem_power
            rR = Σv(t,sum(tt[1:2]))
            t1 = Float64(tt[1])
            if rR <= 0.01
                s1 = @sprintf("\$\\rho_R = %-15.4f\$",rR)
                s2 = @sprintf("\$\\rho_A = %-15.4f\$",rA)
                s3 = @sprintf("\$t_1= %-15.4f\$",t1)
                s4 = "Rendezvous is safe"
            else
                s1 = @sprintf("\$\\rho_R = %-15.4f\$",rR)
                s2 = @sprintf("\$\\rho_A = %-15.4f\$",rA)
                s3 = @sprintf("\$t_1= %-15.4f\$",t1)
                s4 = "Rendezvous is risky"
            end
            plot(path(collect(0:0.01:1)),color="gray",width=2.0)
            annotate!(lx,lvec[1], text(string(s1), 20, :left))
            annotate!(lx,lvec[2], text(string(s2), 20, :left))
            annotate!(lx,lvec[3], text(string(s3), 20, :left))
            annotate!(lx,lvec[4], text(string(s4), 20, (rR <= 0.01) ? :green : :red, :left))
            plot_path(1000,Σ,t0,θ0,tmax,bg,true)
            plot!(xt[1:3],yt[1:3],background_color=bg,width=3.0,color=:steelblue)
            plot!([xt[2] ;xt[5]],[yt[2] ;yt[5]],background_color=bg,width=1.0,color="gray",linestyle=:dash)
            plot!([xt[3] ;xt[4]],[yt[3] ;yt[4]],background_color=bg,width=1.0,color="gray",linestyle=:dash)
            scatter!([xt[2]],[yt[2]],background_color=bg,markersize=3.0,color=:grey)
            scatter!([xt[1]],[yt[1]],background_color=bg,markersize=7.0,markershape=:pentagon,color=:blue)
            scatter!([xt[4]],[yt[4]],background_color=bg,markersize=7.0,markershape=:utriangle,color=:orange)
            scatter!([xt[5]],[yt[5]],background_color=bg,markersize=7.0,markershape=:utriangle,color=:red)
            scatter!(path(θ0),background_color=bg,markersize=10.0,markershape=:square,color=:green)
            p = scatter!(path(θ_R),legend=false,background_color=bg,markersize=7.0,xlims = (-1,11),ylims = (-6,6),color=:cyan)
            p = plot!(tickfont = Plots.font("serif", pointsize=round(22.0)))
            if i in [3 127 233 310]
                println("PRINTING IMAGE")
                display(p)
                s = @sprintf("/Users/gabrielbarsi/Documents/GitHub/Safe-Optimal-Rendezvous/Safe-Optimal-Rendezvous/plot_%d.png",k)
                savefig(s)
                k = k + 1
            end
            xt, yt, rem_power, t = dynamics(xt[1], yt[1], vx[1], vy[1], t, dt, rem_power)
            x[i] = xt[1]
            y[i] = yt[1]
            ρ = Σv(t,sum(tt[1:2]))
            ρv[i-1] = ρ
            td[i-1] = tt[1]
            θ0 = θ0 + θ̇(θ0)*dt
            t_power = (abs.(vx[1:3])'*tt[1:3] + abs.(vy[1:3])'*tt[1:3])
            @show i
            # @show tt[1]
            # @show rem_power t_power tmax
            # @show θ0 θ_R vx vy ρ
            ρ_R = Σv(t,sum(tt[1:2]))*(abs.(vx[1:2])'*tt[1:2] + abs.(vy[1:2])'*tt[1:2])/rem_power
            # @show ρ_R
            ρRv[i-1] = ρ_R
            if ((tt[1] <= 1.0+1e-3) && (i>=10)) || (ρ_R >= 0.4)
                println("End Condition Met")
                epsk = i
                break
            end
        end#end for
        μ, Σ = posterior(Yo, polynomial(Xo, r), α, β)
        Σv(t1,t2) = P1(t1,t2)*(P1(t1,t2)*Σ[1,1] + P2(t1,t2)*Σ[2,1] + P3(t1,t2)*Σ[3,1]) +
                    P2(t1,t2)*(P1(t1,t2)*Σ[1,2] + P2(t1,t2)*Σ[2,2] + P3(t1,t2)*Σ[3,2]) +
                    P3(t1,t2)*(P1(t1,t2)*Σ[1,3] + P2(t1,t2)*Σ[2,3] + P3(t1,t2)*Σ[3,3])
        xt, yt, vx, vy, tt, θ_R = solveRDV(x[end],y[end],t,Lx,Ly,Ax,Ay,vmax,tmax,rem_power,μ,Σ,θ0,length(Xo),xp,yp)

        println("Assessing Risk:")
        ρ = Σv(t,sum(tt[1:2]))
        @show ρ
        if ρ<=0.01
            println("Mission is a go")
        else
            println("Start abort route")
        end
        tvec[end] = tvec[end-1]
        ρv[end] = ρv[end-1]
        ρRv[end] = ρRv[end-1]
        td[end] = td[end-1]
        gif(anim, "/Users/gabrielbarsi/Documents/GitHub/Safe-Optimal-Rendezvous/Safe-Optimal-Rendezvous/anim_fps30.gif", fps = 30)
        mp4(anim, "/Users/gabrielbarsi/Documents/GitHub/Safe-Optimal-Rendezvous/Safe-Optimal-Rendezvous/anim_fps30.mp4", fps = 30)
        return μv, tvec, ρv, td, ρRv, epsk
end

function genfigs(N,x0,y0,t0,θ0)
    D(v,β=Inf) = 2/(1+exp(-5*v)) - 1 + 1/β*randn()
    bg = "white"
    seed!(1729)
    yh = y0
    xh = x0
    vxh = 0.0
    vyh = 0.0
    dt = 0.1
    θ̇(t) = (0.1 .+ 0.1.*cos.(4*pi.*t/10))
    vx, vy = plot_sol(N,bg,t0,θ0,x0,y0)
    s = @sprintf("plot_%d.pdf",0)
    savefig(s)
    for i=1:20
        seed!(1729)
        s = @sprintf("plot_%d.pdf",i)
        t0+dt*i
        θ0 = θ0 + θ̇(t0)/1*dt
        x0 = x0+vx*dt*5
        y0 = y0+vy*dt*5
        vx, vy = plot_sol(N+10*i,bg,t0,θ0,x0,y0)
        xh = [xh x0]
        yh = [yh y0]
        vxh = [vxh vx]
        vyh = [vyh vy]
        savefig(s)
    end
    #plot(vyh')
    #p = plot!(yh')
    #display(p)
end

x0          = 10.0
y0          = -3.0
t0          = 0.0
θ0          = 0.0
# Lx          = 10.0
# Ly          = -y0 + 2
# Ax          = 7.5
# Ay          = -y0 + 2
Lx          = 5.0
Ly          = y0
Ax          = 7.5
Ay          = y0
vmax        = 2.5
tmax        = 10.0
dt          = 0.05
rem_power   = 20.0
N           = 1000
Ni          = 5
H           = Int(ceil(16/dt))

clearconsole()
#@benchmark solveRDV($x0,$y0,$t0,$Lx,$Ly,$Ax,$Ay,$vmax,$tmax,$rem_power,$μ,$Σ,$θ0,$N)

upscale = 1.0 #8x upscaling in resolution
fntsm = Plots.font("serif", pointsize=round(22.0*upscale))
fntlg = Plots.font("serif", pointsize=round(22.0*upscale))
default(bottom_margin=1mm)
default(top_margin=1mm)
default(right_margin=15mm)
default(left_margin=1mm)
default(titlefont=fntlg, guidefont=fntlg, tickfont=fntsm, legendfont=fntsm)
default(size=(800*(upscale),600*(upscale))) #Plot canvas size
default(width = 3.0*upscale)

sn = 2
seed!(sn)
μfilt, tf, ρvf, tdf, ρRvf, epsk = MPCfy(x0,y0,θ0,Lx,Ly,Ax,Ay,vmax,tmax,dt,Ni,H,rem_power,0.2,1/(0.10^2))

upscale = 2.0 #8x upscaling in resolution
fntsm = Plots.font("serif", pointsize=round(14.0*upscale))
fntlg = Plots.font("serif", pointsize=round(14.0*upscale))
default(bottom_margin=15mm)
default(top_margin=5mm)
default(right_margin=5mm)
default(left_margin=20mm)
default(titlefont=fntlg, guidefont=fntlg, tickfont=fntsm, legendfont=fntsm)
default(size=(800*(upscale),600*(upscale))) #Plot canvas size
default(width = 3.0*upscale)
plot()
plot!(ρvf,label=L"\rho_R")
plot!(ρRvf,label=L"\rho_A")
hline!([0.4],label=L"\gamma_A")
ylabel!("Risk")
p1 = hline!([0.01],label=L"\gamma_R")
p2 = plot(tdf,label=L"t_1")
xlabel!(L"k")
ylabel!(L"t_1[s]")
if epsk > 0
    p2 = vline!([epsk],label=L"t_1=\epsilon")
end
p = plot(p1,p2,layout=(2,1))
display(p)
savefig("risktimefail.pdf")

# t0 = t0+0.1
# θ0 = θ0+0.1
# plot_sol(N,"white",t0,θ0,x0,y0)
# genfigs(N,x0,y0,t0,θ0)

# seed!(sn)
# μnfilt, tu, ρvu, tdu, ρRvu = MPCfy(x0,y0,θ0,Lx,Ly,Ax,Ay,vmax,tmax,dt,Ni,H,rem_power,0.2,1/(0.30^2))
# p = plot(ρRvf,width=2.0)
# p = plot!(ρRvu,width=2.0)
# display(p)
# plot(ρvf,width=2.0)
# plot!(ρvu,width=2.0)
# p1 = hline!([0.01],width=2.0,ylims=(0.0,0.1))
# plot(tdf,width=2.0)
# plot!(tdu,width=2.0)
# p2 = hline!([0.1],width=2.0)
# plot(p1,p2,layout=(2,1))

# b = @benchmarkable solveRDV($x0,$y0,$t0,$Lx,$Ly,$Ax,$Ay,$vmax,$tmax,$rem_power,$μ,$Σ,$θ0,$N)
# tune!(b)
# run(b)
# plot(μfilt,label="filtered")
# plot!(μunfilt,label="raw")
#
# plot(tf,label="Filtered",width=2.0)
# plot!(tu,label="Raw",width=2.0)
# xlabel!("Control Iteration")
# ylabel!("Horizon Prediction")
# seed!(1729)
# plot_sol(N,"white",t0,θ0,x0,y0)

# seed!(1729)
# run_fit(50)

max_stars_repo_name: gbarsih/Safe-Optimal-Rendezvous

max_issues_repo_name: gbarsih/Safe-Optimal-Rendezvous

max_forks_repo_name: gbarsih/Safe-Optimal-Rendezvous


text:
"""The Float32 + stochastic rounding type."""
primitive type Float32sr <: AbstractFloat 32 end

# basic properties
Base.sign_mask(::Type{Float32sr}) = 0x8000_0000
Base.exponent_mask(::Type{Float32sr}) = 0x7f80_0000
Base.significand_mask(::Type{Float32sr}) = 0x007f_ffff
Base.precision(::Type{Float32sr}) = 24

"""Mask for both sign and exponent bits. Equiv to ~significand_mask(Float64)."""
signexp_mask(::Type{Float64}) = 0xfff0_0000_0000_0000

Base.one(::Type{Float32sr}) = reinterpret(Float32sr,one(Float32))
Base.zero(::Type{Float32sr}) = reinterpret(Float32sr,0x0000_0000)
Base.one(::Float32sr) = one(Float32sr)
Base.zero(::Float32sr) = zero(Float32sr)

Base.typemin(::Type{Float32sr}) = Float32sr(typemin(Float32))
Base.typemax(::Type{Float32sr}) = Float32sr(typemax(Float32))
Base.floatmin(::Type{Float32sr}) = Float32sr(floatmin(Float32))
Base.floatmax(::Type{Float32sr}) = Float32sr(floatmax(Float32))

Base.typemin(::Float32sr) = typemin(Float32sr)
Base.typemax(::Float32sr) = typemax(Float32sr)
Base.floatmin(::Float32sr) = floatmin(Float32sr)
Base.floatmax(::Float32sr) = floatmax(Float32sr)

Base.eps(::Type{Float32sr}) = Float32sr(eps(Float32))
Base.eps(x::Float32sr) = Float32sr(eps(Float32(x)))

const Inf32sr = reinterpret(Float32sr, Inf32)
const NaN32sr = reinterpret(Float32sr, NaN32)

# basic operations
Base.abs(x::Float32sr) = reinterpret(Float32sr, abs(reinterpret(Float32,x)))
Base.isnan(x::Float32sr) = isnan(reinterpret(Float32,x))
Base.isfinite(x::Float32sr) = isfinite(reinterpret(Float32,x))

Base.uinttype(::Type{Float32sr}) = UInt32
Base.nextfloat(x::Float32sr) = Float32sr(nextfloat(Float32(x)))
Base.prevfloat(x::Float32sr) = Float32sr(prevfloat(Float32(x)))

Base.:(-)(x::Float32sr) = reinterpret(Float32sr, reinterpret(UInt32, x) ⊻ Base.sign_mask(Float32sr))

# conversions
Base.Float32(x::Float32sr) = reinterpret(Float32,x)
Float32sr(x::Float32) = reinterpret(Float32sr,x)
Float32sr(x::Float16) = Float32sr(Float32(x))
Float32sr(x::Float64) = Float32sr(Float32(x))
Base.Float16(x::Float32sr) = Float16(Float32(x))
Base.Float64(x::Float32sr) = Float64(Float32(x))

Float32sr(x::Integer) = Float32sr(Float32(x))
(::Type{T})(x::Float32sr) where {T<:Integer} = T(Float32(x))

const eps_F32 = prevfloat(Float64(nextfloat(zero(Float32))))
const floatmin_F32 = Float64(floatmin(Float32))
const oneF64 = reinterpret(Int64,one(Float64))

"""Stochastically round x::Float64 to Float32 with distance-proportional probabilities."""
function Float32_stochastic_round(x::Float64)
    rbits = rand(Xor128[],UInt64)               # create random bits

    # subnormals are rounded with float-arithmetic for uniform stoch perturbation
    abs(x) < floatmin_F32 && return Float32sr(x+eps_F32*(reinterpret(Float64,oneF64 | (rbits >> 12))-1.5))

    ui = reinterpret(UInt64,x)
	ui += (rbits & 0x0000_0000_1fff_ffff)       # add stochastic perturbation in [0,u)
    ui &= 0xffff_ffff_e000_0000                 # round to zero
	return reinterpret(Float32sr,Float32(reinterpret(Float64,ui)))
end

"""Chance that x::Float64 is round up when converted to Float32sr."""
function Float32_chance_roundup(x::Float64)
	xround = Float64(Float32(x))
	xround == x && return zero(Float64)
	xround_down, xround_up = xround < x ? (xround,Float64(nextfloat(Float32(xround)))) :
		(Float64(prevfloat(Float32(xround))),xround)

	return (x-xround_down)/(xround_up-xround_down)
end

# Promotion
Base.promote_rule(::Type{Float16}, ::Type{Float32sr}) = Float32
Base.promote_rule(::Type{Float64}, ::Type{Float32sr}) = Float64

for t in (Int8, Int16, Int32, Int64, Int128, UInt8, UInt16, UInt32, UInt64, UInt128)
    @eval Base.promote_rule(::Type{Float32sr}, ::Type{$t}) = Float32sr
end

# Rounding
Base.round(x::Float32sr, r::RoundingMode{:ToZero}) = Float32sr(round(Float32(x), r))
Base.round(x::Float32sr, r::RoundingMode{:Down}) = Float32sr(round(Float32(x), r))
Base.round(x::Float32sr, r::RoundingMode{:Up}) = Float32sr(round(Float32(x), r))
Base.round(x::Float32sr, r::RoundingMode{:Nearest}) = Float32sr(round(Float32(x), r))

# Comparison
import Base.:(==)
function ==(x::Float32sr, y::Float32sr)
	return Float32(x) == Float32(y)
end

for op in (:<, :<=, :isless)
    @eval Base.$op(a::Float32sr, b::Float32sr) = ($op)(Float32(a), Float32(b))
end

# Arithmetic
for f in (:+, :-, :*, :/, :^)
	@eval Base.$f(x::Float32sr, y::Float32sr) = Float32_stochastic_round($(f)(Float64(x), Float64(y)))
end

for func in (:sin,:cos,:tan,:asin,:acos,:atan,:sinh,:cosh,:tanh,:asinh,:acosh,
             :atanh,:exp,:exp2,:exp10,:expm1,:log,:log2,:log10,:sqrt,:cbrt,:log1p)
    @eval begin
        Base.$func(a::Float32sr) = Float32_stochastic_round($func(Float64(a)))
    end
end

for func in (:atan,:hypot)
    @eval begin
        Base.$func(a::Float32sr,b::Float32sr) = Float32_stochastic_round($func(Float64(a),Float64(b)))
    end
end

# Showing
function Base.show(io::IO, x::Float32sr)
    if isinf(x)
        print(io, x < 0 ? "-Inf32sr" : "Inf32sr")
    elseif isnan(x)
        print(io, "NaN32sr")
    else
		io2 = IOBuffer()
        print(io2,Float32(x))
        f = String(take!(io2))
        print(io,"Float32sr("*f*")")
    end
end

Base.bitstring(x::Float32sr) = bitstring(reinterpret(UInt32,x))

function Base.bitstring(x::Float32sr,mode::Symbol)
    if mode == :split	# split into sign, exponent, signficand
        s = bitstring(x)
		return "$(s[1]) $(s[2:9]) $(s[10:end])"
    else
        return bitstring(x)
    end
end


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

using Markdown
using InteractiveUtils

# ╔═╡ 413bc522-7d27-11eb-0c68-e9b6228728c3
begin
	using LinearAlgebra;
	using Random;
	using Plots;
end

# ╔═╡ 046e441e-7d39-11eb-128d-9965981464f1
begin
using PyCall
np = pyimport("numpy")
end

# ╔═╡ 919e2ef6-7d24-11eb-3ebd-8f770adc87c9
md"""
# Volatility is rough (2014)

[https://arxiv.org/pdf/1410.3394.pdf](https://arxiv.org/pdf/1410.3394.pdf)

Log-prices $Y_{t}$ often modelled as a semi-martingale

$dY_{t} = \mu_{t} dt + \sigma dW_{t}$

+ Dupire Local Volatility - $\sigma(Y_{t}, t)$ is a deterministic function of state $Y_{t}$ and time
+ Stochastic Volatility - Hull and White, Heston, SABR
+ Neither model the implied volatility surface well

Volatility as a *long memory* process

This makes Fractional Brownian Motion possibly better suited to capture this, given long-memory given the Hurst Exponent h < 1/2

$\mathbb{E}[|W_{t + \Delta}^{H} - W_{t}^{H}|^{q}] = \mathbb{E}[|Z|^{q}] \Delta^{q H}$

$\psi(\tau) := |\frac{\partial}{\partial k} \sigma_{BS}(k , \tau)|_{k = 0} , k = log(S/K)$

"""

# ╔═╡ 222d672a-7d25-11eb-00ab-3f2572366f63
begin

function cov_at(s, t, H)
	(abs(s) ^ (2 * H) + abs(t) ^ (2 * H) - abs(s - t) ^ (2 * H)) / 2
end

N = 100
H = (1:9) ./ 10 #hurst parameter h < 1/2 negatively corrleated;  h > 1/2 positively correlated
dt = 0.1
rng = MersenneTwister(1234);
p = plot()
for h = H
	cov_matrix = [cov_at(i, j, h) for i = (1:N) .* dt for  j = (1:N) .* dt]
	cov_matrix = reshape(cov_matrix, N, N)
	C = cholesky(cov_matrix)
	Ws = randn(rng, Float32, N)
	Bs = C.L * Ws
	print(Bs)
	plot!(p, (1:N) .* dt, Bs, label="H=$h");
end
p
end

# ╔═╡ 2f512d8c-7d2b-11eb-2f62-4fa0a8e0e102
md"""
ON  VIX  FUTURES  IN  THE  ROUGH  BERGOMI  MODEL

https://arxiv.org/pdf/1701.04260.pdf

Functional central limit theorems for rough volatility

https://arxiv.org/abs/1711.03078

+ https://github.com/search?p=2&q=rough+volatility&type=Repositories

Deep Learning Volatility

ON  VIX  FUTURES  IN  THE  ROUGH  BERGOMI  MODEL

The quadratic rough heston model and the joint s&p 500/vixsmile calibration problem

Turbo-Charged Rough Bergomi

Rough Volatility with Python: https://tpq.io/p/rough_volatility_with_python.html

Perfect hedging in rough Heston models

https://arxiv.org/pdf/1703.05049.pdf

Modelling Volatility with FractionalBrownian Motion and the RFSV Model, Bachelor Thesis

http://www.peterhinz.net/pdf/Bachelor_Thesis.pdf

Turbocharging Monte Carlo pricingfor the rough Bergomi model

https://arxiv.org/pdf/1708.02563.pdf

Christian Bayer, Peter Friz, Jim Gatheral, Pricing Under Rough Volatility (2015) https://papers.ssrn.com/sol3/papers.cfm?abstract_id=2554754

Portfolio Optimization under Fast Mean-Reverting and Rough Fractional Stochastic Environment https://www.tandfonline.com/doi/abs/10.1080/1350486X.2019.1584532

Martin Forde, Hongzhong Zhang, Asymptotics for rough stochasticvolatility models (2016) https://arxiv.org/abs/1610.08878

Rough Volatility Modelling (MSc Thesis), https://www.duo.uio.no/bitstream/handle/10852/64079/ThesisXiaoyanZhang.pdf?sequence=1&isAllowed=y

https://sites.google.com/site/roughvol/home/risks-1

Rough stochastic volatility and applications of the rough Bergomi model (MSc Thesis) https://www.imperial.ac.uk/media/imperial-college/faculty-of-natural-sciences/department-of-mathematics/math-finance/TEMEL_TURKER_01421814.pdf

"""

# ╔═╡ Cell order:
# ╠═919e2ef6-7d24-11eb-3ebd-8f770adc87c9
# ╠═413bc522-7d27-11eb-0c68-e9b6228728c3
# ╠═222d672a-7d25-11eb-00ab-3f2572366f63
# ╠═046e441e-7d39-11eb-128d-9965981464f1
# ╠═2f512d8c-7d2b-11eb-2f62-4fa0a8e0e102


text:
# This file is a part of BAT.jl, licensed under the MIT License (MIT).

@recipe function f(
    prior::NamedTupleDist;
    vsel=collect(1:5),
    bins = 200,
    diagonal = Dict(),
    upper = Dict(),
    lower = Dict(),
    vsel_label = []
)
    vsel = collect(vsel)
    if isa(vsel, Vector{<:Integer}) == false
        vsel = asindex.(Ref(BAT.varshape(prior)), vsel)
        vsel = reduce(vcat, vsel)
    end
    vsel = vsel[vsel .<=  totalndof(BAT.varshape(prior))]

    xlabel = [getstring(prior, i) for i in vsel]
    ylabel = ["p($l)" for l in xlabel]

    if length(vsel_label) > 0
        xlabel = [vsel_label[i] for i in 1:length(vsel_label)]
        ylabel = ["p("*vsel_label[i]*")" for i in 1:length(vsel_label)]
    end

    nparams = length(vsel)
    layout --> nparams^2
    size --> (1000, 600)

    bins = if isa(bins, Integer)
        fill(bins, length(vsel))
    elseif isa(bins, Tuple{Vararg{Union{Integer, StepRangeLen}}})
        bins
    elseif isa(bins, NamedTuple)
        tmp_bins::Vector{Any} = fill(200, length(vsel))
        for k in keys(bins)
            idx = findfirst(isequal(string(k)), getstring.(Ref(prior), vsel))
            tmp_bins[idx] = bins[idx]
        end
        tmp_bins
    end

    for i in 1:nparams
        # diagonal
        @series begin
            subplot := i + (i-1)*nparams

            seriestype --> get(diagonal, "seriestype", :stephist)
            bins := bins[i]
            colors --> get(diagonal, "colors", default_colors)
            intervals --> get(diagonal, "intervals", default_credibilities)
            legend --> get(diagonal, "legend", false)
            xlims --> get(diagonal, "xlims", :auto)
            ylims --> get(diagonal, "ylims", :auto)
            linecolor --> get(diagonal, "linecolor", :black)
            xguide --> xlabel[i]
            yguide --> ylabel[i]

            prior, (vsel[i])
        end

        # upper right plots
        for j in i+1:nparams

            @series begin
                subplot := j + (i-1)*nparams

                seriestype --> get(upper, "seriestype", :smallest_intervals_contour)
                bins := (bins[i], bins[j])
                more_colors = []
                colors --> get(upper, "colors", colormap("Blues", 10))
                more_intervals = [0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 0.99]
                intervals --> get(upper, "intervals", more_intervals)
                interval_labels --> get(upper, "interval_labels", [])
                legend --> get(upper, "legend", false)
                xlims --> get(upper, "xlims", :auto)
                ylims --> get(upper, "ylims", :auto)
                xguide --> xlabel[i]
                yguide --> xlabel[j]

                prior, (vsel[i], vsel[j])
            end

            # lower left plots
            @series begin
                subplot := i + (j-1)*nparams

                seriestype --> get(lower, "seriestype", :smallest_intervals_contour)
                bins := (bins[i], bins[j])
                colors --> get(lower, "colors", default_colors)
                intervals --> get(lower, "intervals", default_credibilities)
                interval_labels --> get(lower, "interval_labels", [])
                legend --> get(lower, "legend", false)
                xlims --> get(lower, "xlims", :auto)
                ylims --> get(lower, "ylims", :auto)
                xguide --> xlabel[i]
                yguide --> xlabel[j]

                prior, (vsel[i], vsel[j])
            end

        end
    end

end


text:
using HEP
using Base.Test

v = HEP.FourVector(4.0, 1.0, 2.0, 3.0)
@test_approx_eq perp(v) 2.23606797749979
@test_approx_eq eta(v) 1.1035868415601455
@test_approx_eq phi(v) 1.1071487177940904
@test_approx_eq rho(v) 3.7416573867739413
@test_approx_eq theta(v) 0.6405223126794246
@test_approx_eq perp(v) 2.23606797749979
@test_approx_eq l(v) 1.4142135623730951

v2 = HEP.FourVectorSph(10.0, -2.0, 3.14/2.0, 100.0)
@test_approx_eq perp(v2) 10.0
@test_approx_eq eta(v2) -2.0000000000000004
@test_approx_eq phi(v2) 1.57
@test_approx_eq rho(v2) 37.62195691083632
@test_approx_eq theta(v2) 2.872556662840912
@test_approx_eq perp(v2) 10.0
@test_approx_eq l(v2) 99.99999999999999

@test_approx_eq deltar(v, v2) deltar(v2, v)
@test_approx_eq deltar(v, v) 0
@test_approx_eq deltar(v, v2) 3.137910545656924

v2_c = convert(v2, FourVector)

@test_approx_eq l(v2_c) l(v2)
@test_approx_eq x(v2_c) x(v2)
@test_approx_eq y(v2_c) y(v2)
@test_approx_eq z(v2_c) z(v2)

vt = v + v2_c

@test_approx_eq x(vt) 1.0079632671073326
@test_approx_eq y(vt) 11.999996829318347
@test_approx_eq z(vt) -33.26860407847019
@test_approx_eq t(vt) 110.84292976983


text:
using DeDisp
using Test

@testset "DeDisp.jl" begin
    # Write your tests here.
end


text:
@testset "cont" begin
    for (m,s,b) in ((DynamicAxis(5), static(OneTo(5)), OneTo(5)),
                   )
        @testset "Type: $(typeof(b))" begin
            for i1 in (m[1] - step(m), m[1], m[4], m[5] + 2step(m))
                for i2 in (m[2], m[3], m[5], m[5] + step(m))
                    for f in (<, >, <=, >=, ==)
                        @testset "Comparison: $f" begin
                            @testset "count($f($i1), $b)" begin
                                @test count(f(i1), m) == count(f(i1), b)
                                @test count(f(i1), s) == count(f(i1), b)
                            end
                            @testset "count($f($i2), $b)" begin
                                @test count(f(i2), m) == count(f(i2), b)
                                @test count(f(i2), s) == count(f(i2), b)
                            end
                        end
                   end
                end
            end
        end
    end

    @testset "Find with empty range" begin
        for i in (0, 1)
            @testset "Number: $i" begin
                for f in (<, >, <=, >=, ==)
                    m, s, b = MutableRange(LinRange(1, 1, 0)), static(LinRange(1, 1, 0)), LinRange(1, 1, 0)
                    @testset "count" begin
                        @test count(f(i), m) == count(f(i), b)
                        @test count(f(i), s) == count(f(i), b)
                    end
                end
            end
        end
    end
end


text:
# test numericRootGenericRandomized
using IncrementalInference
using Test

mutable struct LineResidual <: FunctorPairwise
  m::Float64
  c::Float64
end

# y = mx + c
# res = y      -      m*x  -  c
#       meas          variables and fixed values
function (lr::LineResidual)(res::Vector{Float64},
                            userdata,
                            idx::Int,
                            z::Tuple,
                            x::Array{Float64,2},
                            y::Array{Float64,2}  )
  #
  res[1] = z[1][idx] - (y[1,idx] - (lr.m*x[1,idx] + lr.c))
  nothing
end

@testset "test CommonConvWrapper{T}" begin

function assembleConvType(functor::T, xDim::Int, zDim::Int, nvars::Int) where {T <: FunctorPairwise}
  # @info "assembleConvType -- development testing function only, not intended for production."
  N = 3

  vars = Array{Array{Float64,2},1}()
  for i in 1:nvars
    push!(vars, zeros(xDim, N))
  end

  CommonConvWrapper(functor,vars[1],zDim,vars, measurement=(zeros(zDim,N),))
end

# TODO -- expand testing to include subcomponent tests from numericRootGenericRandomizedFnc
lr1 = LineResidual(2.0, 3.0)
ccw = assembleConvType(lr1, 1, 1, 2)

res = zeros(1)
ccw(res, zeros(1))

# gwp(x, res)
ccw.cpt[1].particleidx = 1
numericRootGenericRandomizedFnc!( ccw )

@test abs(ccw.cpt[1].Y[1] + 1.50) < 1e-5

end

# println("Test shuffling function")
#
# function testshuffle!(res, x)
#   # println("testshuffle!(x,res) gets x=$(x), and length(res)=$(length(res))")
#   @show res
#   @show x
#   nval = sum(abs.(x-[11.0;12.0;13.0]))
#   # trick the solver so that f/f' = 0/1e10, and therefore < tol
#   # resulting in termination of solver
#   if nval < 1e-8
#     res[1:2] = [0.0;0.0] # fake at root evaluation
#   else
#     res[1:2] = 1e10*randn(2) # fake large gradient
#   end
#   nothing
# end
#
# # test shuffling function
# ALLTESTS = Bool[]
# @show x0 = collect(11:13)+0.0
# for i in 1:10
#   y = numericRootGenericRandomizedFnc(
#           testshuffle!,
#           2, 3, x0,
#           perturb=0.0    )
#   @show y
#   @show y.%10
#   @show yy = y.%10.0
#   push!(ALLTESTS, norm(yy-collect(1:3)) < 0.1)
# end
# @test sum(ALLTESTS) > 9
#
#
# println("Test if shuffling results in correct mapping for solving")
#
#
# function testshuffle2!(res, x)
#   # println("testshuffle2!(x,res) gets x=$(x), and length(res)=$(length(res))")
#   res[1:2] = x - [1.0;2.0] # fake at root evaluation
#   nothing
# end
#
# # test shuffling function
# x0 = collect(11:12)+0.0
# for i in 1:10
#   println("starting with x0=$(x0)")
#   y = numericRootGenericRandomizedFnc(
#           testshuffle2!,
#           2, 2, x0,
#           perturb=0.0,
#           testshuffle=true    )
#   println("result y=$(y)")
#   @test norm(y-collect(1:2)) < 0.1
# end
#
#
#
# # x is dimension 3, z dimension is 2
# function residualrandtest!(res, x)
#   val = norm(x[1:2])
#   res[1] = 10.0 - val
#   nothing
# end
#
# for i in 1:10
#   x0 = ones(2)
#   y = numericRootGenericRandomizedFnc(
#           residualrandtest!,
#           1, 2, x0   )
#   #
#   @test abs(norm(y) - 10.0) < 1e-4
# end