function mats(p, nx, ny, nu)
	A         = zeros(eltype(p), nx, nx)
	for i = ny:nx-1
		A[i-ny+1,i+1] = 1
	end
	s,e       = 1,nx*ny
	A[:,1:ny] = reshape(p[s:e], nx, ny)
	s,e       = e+1,e+nu*nx
	B         = reshape(p[s:e], nx, nu)
	s,e       = e+1,e+ny*nx
	K         = reshape(p[s:e], nx, ny)
	A,B,K
end
function model_from_params(p, nx, ny, nu)
	A,B,K = mats(p, nx, ny, nu)
	x0    = copy(p[end-nx+1:end])
	sys   = StateSpaceNoise(A,B,K,1.)
	sysf  = SysFilter(sys,x0,similar(x0,ny))
end

function pem_costfun(p,y,u,nx,metric::F) where F # To ensure specialization on metric
	nu,ny = obslength(u),obslength(y)
	model = model_from_params(p, nx, ny, nu)
	return mean(metric(e) for e in prediction_errors(model,y,u))
end
function sem_costfun(p,y,u,nx,metric::F) where F # To ensure specialization on metric
	nu,ny = obslength(u),obslength(y)
	model = model_from_params(p, nx, ny, nu)
	return mean(metric(e) for e in simulation_errors(model,y,u))
end

"""
sys, x0, opt = pem(y, u; nx, kwargs...)

System identification using the prediction error method.

# Arguments:
- `y`: Measurements, either a matrix with time along dim 2, or a vector of vectors
- `u`: Control signals, same structure as `y`
- `nx`: Number of poles in the estimated system. Thus number should be chosen as number of system poles plus number of poles in noise models for measurement noise and load disturbances.
- `focus`: Either `:prediction` or `:simulation`. If `:simulation` is chosen, a two stage problem is solved with prediction focus first, followed by a refinement for simulation focus.
- `metric`: A Function determining how the size of the residuals is measured, default `sse` (e'e), but any Function such as `norm`, `e->sum(abs,e)` or `e -> e'Q*e` could be used.
- `regularizer(p)=0`: function for regularization. The structure of `p` is detailed below
- `solver` Defaults to `Optim.BFGS()`
- `stabilize_predictor=true`: Modifies the estimated Kalman gain `K` in case `A-KC` is not stable by moving all unstable eigenvalues to the unit circle.
- `difficult=false`: If the identification problem appears to be difficult and ends up in a local minimum, set this flag to true to solve an initial global optimization problem to supply a good initial guess. This is expected to take some time.
- `kwargs`: additional keyword arguments are sent to `Optim.Options`.

# Return values
- `sys::StateSpaceNoise`: identified system. Can be converted to `StateSpace` by `convert(StateSpace, sys)`, but this will discard the Kalman gain matrix.
- `x0`: Estimated initial state
- `opt`: Optimization problem structure. Contains info of the result of the optimization problem

## Structure of parameter vecotr `p`
```julia
A = size(nx,ny)
B = size(nx,nu)
K = size(nx,ny)
x0 = size(nx)
p = [A[:];B[:];K[:];x0]
```
"""
function pem(y, u; nx, solver = BFGS(), focus=:prediction, metric=sse, regularizer=p->0, iterations=100, stabilize_predictor=true, difficult=false, kwargs...)
	nu,ny = obslength(u),obslength(y)

	A       = 0.0001randn(nx,ny)
	B       = 0.001randn(nx,nu)
	K       = 0.001randn(nx,ny)
	x0      = 0.001randn(nx)
	p       = [A[:];B[:];K[:];x0]
	options = Options(;iterations=iterations, kwargs...)
	cfp     = p->pem_costfun(p,y,u,nx,metric) + regularizer(p)
	if difficult
		stabilizer = p-> 10000*!stabfun(nx,nu,ny)(p)
		options0 = Options(;iterations=iterations, kwargs...)
		opt = optimize(p->cfp(p)+stabilizer(p), 1000p, ParticleSwarm(n_particles=100length(p)), options0)
		p = minimizer(opt)
	end
	opt     = optimize(cfp, p, solver, options; autodiff = :forward)
	println(opt)
	if focus == :simulation
		@info "Focusing on simulation"
		cf = p->sem_costfun(p,y,u,nx,metric) + regularizer(p)
		opt = optimize(cf, minimizer(opt), NewtonTrustRegion(), Options(;iterations=iterations÷2, kwargs...); autodiff = :forward)
		println(opt)
	end
	model = model_from_params(minimizer(opt), nx, ny, nu)
	if !isstable(model.sys)
		@warn("Estimated system does not have a stable prediction filter (A-KC)")
		if stabilize_predictor
			@info("Stabilizing predictor")
			# model = stabilize(model)
			model = stabilize(model, solver, options, cfp)
		end
	end
	isstable(ss(model.sys)) || @warn("Estimated system is not stable")

	model.sys, copy(model.state), opt
end

function stabilize(model)
	s           = model.sys
	@unpack A,K = s
	C           = s.C
	poles       = eigvals(A-K*C)
	newpoles = map(poles) do p
		ap = abs(p)
		ap <= 1 && (return p)
		p / (ap + sqrt(eps()))
	end
	K2   = ControlSystems.acker(A',C', newpoles)' .|> real
	all(abs(p) <= 1 for p in eigvals(A-K*C)) || @warn("Failed to stabilize predictor")
	s.K .= K2
	model
end

function stabilize(model, solver, options, cfi)
	s = model.sys
	cost = function(p)
		maximum(abs.(eigvals(s.A-p*s.K*s.C)))-0.9999999
	end
	p = fzero(cost,1e-9,1-1e-9)
	s.K .*= p
	model
end

function stabfun(nx,ny,nu)
	function (p)
		model = model_from_params(p,nx,nu,ny)
		isstable(model.sys)
	end
end


# Experimental threads example. WIP!

using StanSample
using Statistics
using Distributions

ProjDir = @__DIR__
cd(ProjDir) #do

bernoullimodel = "
data {
  int<lower=1> N;
  int<lower=0,upper=1> y[N];
}
parameters {
  real<lower=0,upper=1> theta;
}
model {
  theta ~ beta(1,1);
  y ~ bernoulli(theta);
}
";

#isdir(ProjDir * "/tmp") && rm(ProjDir * "/tmp", recursive=true)
tmpdir = ProjDir * "/tmp"

p1 = 12 # p1 is the number of models to fit
p = range(0, stop=1, length=p1)

n = 100;
observeddata = [Dict("N" => n, "y" => rand(Bernoulli(p[i]), n)) for i in 1:p1]

sm = [SampleModel("bernoulli_m$i", bernoullimodel; tmpdir=tmpdir) for i in 1:p1];

println("\nThreads loop\n")

estimates = Vector(undef, p1)
Threads.@threads for i in 1:p1
    rc = stan_sample(sm[i]; data=observeddata[i]);
    if success(rc)
      samples = read_samples(sm[i]; output_format=:namedtuple)

      estimates[i] = [mean(reshape(samples.theta, 4000)), std(reshape(samples.theta, 4000))]
    end
end

estimates |> display

#end


function inex(digits)
#
#  test polynomial suggested by Goedecker
#
    p = poly([(10/11)*[1;1;1;1;1]; (20/11)*[1;1;1]; (30/11)*[1;1]])
    p = round.(10^digits*reverse(p.a)) ./ 10^digits
    z = reverse([10/11 5; 20/11 3; 30/11 2]; dims=1)
    p, PolyZeros(z)
end


# Double Integrator
T = Float64
model = TrajectoryOptimization.Dynamics.doubleintegrator
model_d = rk3(model)
n = model.n; m = model.m

# costs
Q = 1.0*Diagonal(I,n)
Qf = 1.0*Diagonal(I,n)
R = 1.0e-1*Diagonal(I,m)
x0 = [0; 0.]
xf = [1.; 0] # (ie, swing up)

N = 21
dt = 0.1
U0 = [0.001*rand(m) for k = 1:N-1]
obj = TrajectoryOptimization.LQRObjective(Q,R,Qf,xf,N)

u_max = 1.5
u_min = -1.5

bnd = BoundConstraint(n,m,u_max=u_max, u_min=u_min,trim=true)
goal = goal_constraint(xf)
constraints = Constraints(N)
for k = 1:N-1
    constraints[k] += bnd
end
constraints[N] += goal

doubleintegrator = TrajectoryOptimization.Problem(model_d, obj, constraints=constraints, x0=x0, xf=xf, N=N, dt=dt)
initial_controls!(doubleintegrator, U0)


using ConstrainedDynamics
using ConstrainedDynamics: params, pure_quaternion, Lmat, Lᵀmat, Rmat, Rᵀmat, Tmat, Tᵀmat, Vmat, Vᵀmat, VLmat, VLᵀmat, VRmat, VRᵀmat, LVᵀmat, LᵀVᵀmat, RVᵀmat, RᵀVᵀmat, slerp
using StaticArrays
using LinearAlgebra

w1 = rand()
v1 = rand(3)
w2 = rand()
v2 = rand(3)
qvref1 = pure_quaternion(v1)
qref1 = UnitQuaternion(w1,v1...)
qref2 = UnitQuaternion(w2,v2...)
q1 = UnitQuaternion(w1,SA[v1...])
@test q1 == qref1
q1 = UnitQuaternion(w1,v1)
@test q1 == qref1
qv1 = UnitQuaternion(SA[v1...])
@test qv1 == qvref1
qv1 = UnitQuaternion(v1)
@test qv1 == qvref1

@test isapprox(norm(params(qref1*qref2) - Lmat(q1)*params(qref2)), 0.0; atol = 1e-10)
@test isapprox(norm(params(qref1\qref2) - Lᵀmat(q1)*params(qref2)), 0.0; atol = 1e-10)
@test isapprox(norm(params(qref2*qref1) - Rmat(q1)*params(qref2)), 0.0; atol = 1e-10)
@test isapprox(norm(params(qref2/qref1) - Rᵀmat(q1)*params(qref2)), 0.0; atol = 1e-10)

@test isapprox(norm(params(inv(qref1)) - Tmat()*params(q1)), 0.0; atol = 1e-10)
@test isapprox(norm(params(inv(qref1)) - Tᵀmat()*params(q1)), 0.0; atol = 1e-10)
@test isapprox(norm(params(qvref1)[2:4] - Vmat()*params(qv1)), 0.0; atol = 1e-10)
@test isapprox(norm(params(qvref1) - Vᵀmat()*v1), 0.0; atol = 1e-10)

@test isapprox(norm(params(qref1*qref2)[2:4] - VLmat(q1)*params(qref2)), 0.0; atol = 1e-10)
@test isapprox(norm(params(qref1\qref2)[2:4] - VLᵀmat(q1)*params(qref2)), 0.0; atol = 1e-10)
@test isapprox(norm(params(qref2*qref1)[2:4] - VRmat(q1)*params(qref2)), 0.0; atol = 1e-10)
@test isapprox(norm(params(qref2/qref1)[2:4] - VRᵀmat(q1)*params(qref2)), 0.0; atol = 1e-10)

@test isapprox(norm(params(qref1*qvref1) - LVᵀmat(q1)*v1), 0.0; atol = 1e-10)
@test isapprox(norm(params(qref1\qvref1) - LᵀVᵀmat(q1)*v1), 0.0; atol = 1e-10)
@test isapprox(norm(params(qvref1*qref1) - RVᵀmat(q1)*v1), 0.0; atol = 1e-10)
@test isapprox(norm(params(qvref1/qref1) - RᵀVᵀmat(q1)*v1), 0.0; atol = 1e-10)

@test isapprox(norm(params(qref1) - params(slerp(qref1,qref2,0.0))), 0.0; atol = 1e-10)
@test isapprox(norm(params(qref2) - params(slerp(qref1,qref2,1.0))), 0.0; atol = 1e-10)


"""Betina Robust Sampled based Formulation"""
struct SampleBased <: AbstractSampleBased
    sampled_returns::Array{Float64,2}
    number_of_assets::Int
    number_of_samples::Int
end

function SampleBased(;
    sampled_returns::Array{Float64,2}
)
    number_of_samples, number_of_assets = size(sampled_returns)

    return SampleBased(
        sampled_returns, number_of_assets, number_of_samples
    )
end

function predicted_portfolio_return!(::JuMP.Model, w, formulation::AbstractSampleBased;
    predicted_mean = mean(formulation.sampled_returns, dims=1)'[:,1]
)
    return sum(w'predicted_mean)
end

function portfolio_return!(model::JuMP.Model, w, formulation::AbstractSampleBased; kwargs...)
    return predicted_portfolio_return!(model, w, formulation; kwargs...)
end

function predicted_portfolio_variance!(::JuMP.Model, w, formulation::AbstractSampleBased;
    predicted_covariance = cov(formulation.sampled_returns, dims=1)
)
    return sum(w'predicted_covariance * w)
end

function portfolio_variance!(model::JuMP.Model, w, formulation::AbstractSampleBased; kwargs...)
    return predicted_portfolio_variance!(model, w, formulation)
end

function po_max_predicted_return_limit_return(formulation::AbstractPortfolioFormulation, minimal_return;
    rf::Real = 0.0, current_wealth::Real = 1.0,
    model::JuMP.Model = base_model(formulation.number_of_assets; current_wealth=current_wealth),
    w = model[:w],
    kwargs...
)
    # auxilary variables
    @variable(model, R)
    sum_invested = create_sum_invested_variable(model, w)

    # model
    @constraint(model, R == portfolio_return!(model, w, formulation) + rf * (current_wealth - sum_invested))
    @constraint(model, R >= minimal_return * current_wealth)

    # objective function
    @objective(model, Max, predicted_portfolio_return!(model, w, formulation; kwargs...) + rf * (current_wealth - sum_invested))

    return model
end


using ManifoldMeasures, Manifolds, MeasureTheory, Random
using Test
using ManifoldMeasures: mass, logmass
using Manifolds: ℝ

@testset "Hausdorff" begin
    @testset "basic" begin
        M = Sphere(2)
        d = Hausdorff(M)
        @test d isa Hausdorff{Sphere{2,ℝ},Nothing}
        @test base_manifold(d) === M
        @test d.atlas isa Nothing
        @test sprint(show, "text/plain", d) === "Hausdorff($M, nothing)"
        p = normalize(randn(3))
        @test iszero(logdensity(d, p))
    end

    @testset "manifolds" begin
        @testset "Circle" begin
            @test mass(Hausdorff(Circle(ℝ))) ≈ 2π
            @test mass(Hausdorff(Circle(ℂ))) ≈ 2π
        end

        @testset "Sphere" begin
            @test mass(Hausdorff(Sphere(0))) ≈ 2
            @test mass(Hausdorff(Sphere(1))) ≈ 2π
            @test mass(Hausdorff(Sphere(2))) ≈ 4π
            @test mass(Hausdorff(Sphere(3))) ≈ 2π^2
            for n in 0:10
                @test mass(Hausdorff(Sphere(n, ℂ))) ≈ mass(Hausdorff(Sphere(2n + 1)))
                @test mass(Hausdorff(Sphere(n, ℍ))) ≈ mass(Hausdorff(Sphere(4n + 3)))
            end
        end

        @testset "ProjectiveSpace" begin
            @testset "n=$n" for n in 0:10
                @test mass(Hausdorff(ProjectiveSpace(n))) ≈ mass(Hausdorff(Sphere(n))) / 2
                @test mass(Hausdorff(ProjectiveSpace(n, ℂ))) ≈
                      mass(Hausdorff(Sphere(n, ℂ))) / mass(Hausdorff(Sphere(0, ℂ)))
                @test mass(Hausdorff(ProjectiveSpace(n, ℍ))) ≈
                      mass(Hausdorff(Sphere(n, ℍ))) / mass(Hausdorff(Sphere(0, ℍ)))
            end
        end

        @testset "Stiefel" begin
            function mass_stiefel_recursive(::Stiefel{n,1,𝔽}) where {n,𝔽}
                return mass(Hausdorff(Sphere(n - 1, 𝔽)))
            end
            function mass_stiefel_recursive(::Stiefel{n,k,𝔽}) where {n,k,𝔽}
                return mass(Hausdorff(Sphere(n - 1, 𝔽))) *
                       mass_stiefel_recursive(Stiefel(n - 1, k - 1, 𝔽))
            end
            @testset "Stiefel($n, $k, $𝔽)" for n in 1:10, k in 1:2:n, 𝔽 in (ℝ, ℂ, ℍ)
                k == 1 && @test mass(Hausdorff(Stiefel(n, k, 𝔽))) ≈
                      mass(Hausdorff(Sphere(n - 1, 𝔽)))
                @test mass(Hausdorff(Stiefel(n, k, 𝔽))) ≈
                      mass_stiefel_recursive(Stiefel(n, k, 𝔽))
            end
        end

        @testset "Grassmann" begin
            @testset "Grassmann($n, $k, $𝔽)" for n in 1:10, k in 1:2:n, 𝔽 in (ℝ, ℂ, ℍ)
                k == 1 && @test mass(Hausdorff(Grassmann(n, k, 𝔽))) ≈
                      mass(Hausdorff(ProjectiveSpace(n - 1, 𝔽)))
                @test mass(Hausdorff(Grassmann(n, k, 𝔽))) ≈
                      mass(Hausdorff(Stiefel(n, k, 𝔽))) / mass(Hausdorff(Stiefel(k, k, 𝔽)))
            end
        end

        @testset "$MT" for MT in (Rotations, SpecialOrthogonal)
            @test mass(Hausdorff(MT(1))) ≈ 1
            @test mass(Hausdorff(MT(2))) ≈ 2π
            @test mass(Hausdorff(MT(3))) ≈ 8π^2
        end
    end
end


# SPDX-License-Identifier: MIT

#======================================================================
This file wrapped a few basic linear algebra operations in order to
have more fine-grained control between CPU/GPU code and different
semifield algebra operations.
======================================================================#

# Element-wise division.
eldiv!(out, x::AbstractArray, y::AbstractArray) = broadcast!(/, out, x, y)

# Element-wise multiplication.
elmul!(out, x::AbstractArray, y::AbstractArray) =  broadcast!(*, out, x, y)
function elmul!(out, x::AbstractSparseArray{T}, y::AbstractArray{T}) where T
    fill!(out, zero(T))
    broadcast!(*, out, x, y)
end
elmul!(out, x::CuSparseVector, y::AbstractVector) = elmul_svdv!(out, x, y)
elmul!(out, x::CuSparseVector, Y::AbstractMatrix) = elmul_svdm!(out, x, Y)

# Matrix multiplication.
matmul!(out, X::AbstractMatrix{T}, y::AbstractArray{T}) where T =
    mul!(out, X, y, one(T), zero(T))
function matmul!(out, X::AbstractSparseMatrix{T}, y::AbstractArray{T}) where T
    fill!(out, zero(T))
    mul!(out, X, y, one(T), zero(T))
end
matmul!(out, X::CuMatrix{T}, Y::CuMatrix{T}) where T =
    mul_dmdm!(out, X, Y)
matmul!(out, X::CuSparseMatrixCSR{T}, y::AbstractVector{T}) where T =
    mul_smdv!(out, X, y)
matmul!(out, X::CuSparseMatrixCSR{T}, Y::AbstractMatrix{T}) where T =
    mul_smdm!(out, X, Y)


using Plots, HyperbolicPlane, SimpleDrawing

function tritest(X::HPolygon)
    set_thickness(X,3)
    set_color(X,:blue)
    TT = triangulate(X)
    plot()
    draw(TT)
    draw(X)
    for p in X.plist
        a = HPoint(p)
        set_color(a,:red)
        set_radius(a,1.5)
        draw(a)
    end
    finish()
end


"""
    min_radius(n::Integer, idx::Integer, data::Array{<:Real,2}, target::Array{<:Integer,1}, dist_func::Any)::Float64

Compute minimum raidus of hypersphere centered around each data sample so that each hypersphere contains
at least n samples from same class as the corresponding data sample as well as n samples from a different class.

Author: Jernej Vivod
"""
function min_radius(n::Integer, e_idx::Integer, data::Array{<:Real,2}, target::Array{<:Integer,1}, dist_func::Any)::Float64

    # Get masks for samples with same and different class.
    hit_neigh_mask = target .== target[e_idx]
    miss_neigh_mask = .!hit_neigh_mask

    # Get distances to samples from same and different classes.
    dists_same = dist_func(data[e_idx:e_idx, :], data[hit_neigh_mask, :])
    dists_other = dist_func(data[e_idx:e_idx, :], data[miss_neigh_mask, :])

    # Return computed radius.
    return max(sort(dists_same, dims=1)[n+1], sort(dists_other, dims=1)[n])
end

"""
    iterative_relief(data::Array{<:Real,2}, target::Array{<:Integer,1}, m::Integer=-1, min_incl::Integer=3,
                          max_iter::Integer=100, dist_func::Any=(e1, e2, w) -> sum(abs.(w.*(e1 .- e2)), dims=2);
                          f_type::String="continuous")::Array{Float64,1}

Compute feature weights using Iterative Relief algorithm.

---
# Reference:
- Bruce Draper, Carol Kaito, and Jose Bins. Iterative Relief. Proceedings
CVPR, IEEE Computer Society Conference on Computer Vision and
Pattern Recognition., 6:62 – 62, 2003.
"""
function iterative_relief(data::Array{<:Real,2}, target::Array{<:Integer,1}, m::Integer=-1, min_incl::Integer=3,
                          max_iter::Integer=100, dist_func::Any=(e1, e2, w) -> sum(abs.(w.*(e1 .- e2)), dims=2);
                          f_type::String="continuous")::Array{Float64,1}

    # Initialize distance weights.
    dist_weights = ones(Float64, 1, size(data, 2))

    # Initialize iteration counter, convergence indicator and
    # Array for storing feature weights from previous iteration.
    iter_count = 0
    convergence = false
    feature_weights_prev = zeros(Float64, size(data, 2))

    # Iterate until reached maximum iterations or convergence.
    @inbounds while iter_count < max_iter && !convergence

        # Increment iteration counter.
        iter_count += 1

        # Reset feature weights to zero and sample samples.
        feature_weights = zeros(Float64, size(data, 2))
        sample_idxs = StatsBase.sample(1:size(data, 1), if (m==-1) size(data,1) else m end, replace=false)

        # Set m if currently set to signal value -1.
        if (m == -1) m = size(data, 1) end

        # Go over sampled indices.
        @inbounds for idx in sample_idxs

            # Get next sampled sample.
            e = data[idx:idx, :]

            # Get minimum radius needed to include n samples from same class and n samples from different class.
            min_r = min_radius(min_incl, idx, data, target, (e1, e2) -> dist_func(e1, e2, dist_weights))

            # Compute hypersphere inclusions and distances to examples within the hypersphere.
            # Distances to examples from same class.

            dist_same_all = dist_func(data[(target .== target[idx]) .& (1:length(target) .!= idx), :], e, dist_weights)
            sel = dist_same_all .<= min_r
            dist_same = dist_same_all[sel]
            data_same = data[(target .== target[idx]) .& (1:length(target) .!= idx), :][vec(sel), :]

            # Distances to examples with different class.
            dist_other_all = dist_func(data[target .!= target[idx], :], e, dist_weights)
            sel = dist_other_all .<= min_r
            dist_other = dist_other_all[sel]
            data_other = (data[target .!= target[idx], :])[vec(sel), :]

            ### Weights Update ###

            w_miss = max.(0, 1 .- (dist_other.^2/min_r.^2))
            w_hit = max.(0, 1 .- (dist_same.^2/min_r.^2))

            if f_type == "continuous"
                # If features continuous.

                numerator1 = sum(abs.(e .- data_other) .* w_miss, dims=1)
                denominator1 = sum(w_miss) + eps(Float64)

                numerator2 = sum(abs.(e .- data_same) .* w_hit, dims=1)
                denominator2 = sum(w_hit) + eps(Float64)

                feature_weights .+= vec(numerator1 ./ denominator1 .- numerator2 ./ denominator2)

            elseif f_type == "discrete"
                # If features discrete.

                numerator1 = sum(Int64.(e .!= data_other) .* w_miss, dims=1)
                denominator1 = sum(w_miss) + eps(Float64)

                numerator2 = sum(Int64.(e .!= data_same) .* w_hit, dims=1)
                denominator2 = sum(w_hit) + eps(Float64)

                feature_weights .+= vec(numerator1 ./ denominator1 .- numerator2 ./ denominator2)

            else
                throw(DomainError(f_type, "f_type can only be equal to \"continuous\" or \"discrete\"."))
            end

            ######################

        end

        # Update distance weights by feature weights - use algorithm's own feature evaluations
        # to weight features when computing distances.
        dist_weights += permutedims(feature_weights)

        # Check convergence.
        if sum(abs.(feature_weights .- feature_weights_prev)) < 1.0e-3
            convergence = true
        end

        # Set current feature weights as previous feature weights.
        feature_weights_prev = feature_weights
    end

    # Return computed feature weights.
    return vec(dist_weights)

end


"""
    TimeVaryingCLF

Time varying Control Lyapunov function V for a control affine system
# Fields
- `V`: function V(x, t) that represents the CLF
- `∇V`: function ∇V(x, t) defining the spatial gradient of the CLF
- `∂ₜV`: function ∂ₜV(x, t) defining the time derivative of the CLF
- `α`: class K function α(V(x, t))
"""
struct TimeVaryingCLF <: LyapunovFunction
    V
    ∇V
    ∂ₜV
    α
end

"""
    TimeVaryingCLF(V, α)

Construct a CLF given V and class K function α.
"""
function TimeVaryingCLF(V, α)
    ∇V(x, t) = gradient((r, s) -> V(r, s), x, t)[1]'
    ∂ₜV(x, t) = gradient((r, s) -> V(r, s), x, t)[2]
    return TimeVaryingCLF(V, ∇V, ∂ₜV, α)
end

"""
	(V::TimeVaryingCLF)(x, t)

Evaluate CLF at state x and time t.
"""
function (V::TimeVaryingCLF)(x, t)
    return V.V(x, t)
end


""" [Tan2018](@cite) """
LifeCycleTan2018_θ_liq_ice(::Type{FT}) where {FT} =
    z -> if z <= 520.0
        FT(298.7)
    elseif z > 520.0 && z <= 1480.0
        FT(298.7) + (z - 520) * (FT(302.4) - FT(298.7)) / (1480 - 520)
    elseif z > 1480.0 && z <= 2000
        FT(302.4) + (z - 1480) * (FT(308.2) - FT(302.4)) / (2000 - 1480)
    elseif z > 2000.0
        FT(308.2) + (z - 2000) * (FT(311.85) - FT(308.2)) / (3000 - 2000)
    else
        FT(0)
    end

""" [Tan2018](@cite) """
LifeCycleTan2018_q_tot(::Type{FT}) where {FT} =
    z -> if z <= 520
        (FT(17) + z * (FT(16.3) - FT(17.0)) / 520) / 1000
    elseif z > 520.0 && z <= 1480.0
        (FT(16.3) + (z - 520) * (FT(10.7) - FT(16.3)) / (1480 - 520)) / 1000
    elseif z > 1480.0 && z <= 2000.0
        (FT(10.7) + (z - 1480) * (FT(4.2) - FT(10.7)) / (2000 - 1480)) / 1000
    elseif z > 2000.0
        (FT(4.2) + (z - 2000) * (3 - FT(4.2)) / (3000 - 2000)) / 1000
    else
        FT(0)
    end

""" [Tan2018](@cite) """
LifeCycleTan2018_u(::Type{FT}) where {FT} =
    z -> if z <= 700.0
        FT(-8.75)
    else
        FT(-8.75) + (z - 700) * (FT(-4.61) - FT(-8.75)) / (3000 - 700)
    end
""" [Tan2018](@cite) """
LifeCycleTan2018_tke(::Type{FT}) where {FT} =
    z -> if z <= 2500.0
        FT(1) - z / 3000
    else
        FT(0)
    end

# Large-scale cooling
""" [Tan2018](@cite) """
LifeCycleTan2018_dTdt(::Type{FT}) where {FT} =
    (Π, z) -> if z <= 1500.0
        FT(-2 / (3600 * 24)) * Π
    else
        FT(-2 / (3600 * 24) + (z - 1500) * (0 - -2 / (3600 * 24)) / (3000 - 1500)) * Π
    end

# geostrophic velocity profiles
""" [Tan2018](@cite) """
LifeCycleTan2018_geostrophic_u(::Type{FT}) where {FT} =
    z -> -10 + FT(1.8e-3) * z

""" [Tan2018](@cite) """
LifeCycleTan2018_geostrophic_v(::Type{FT}) where {FT} =
    z -> FT(0)

# Large-scale drying
""" [Tan2018](@cite) """
LifeCycleTan2018_dqtdt(::Type{FT}) where {FT} =
    z -> if z <= 300.0
        FT(-1.2e-8)   #kg/(kg * s)
    elseif z > 300.0 && z <= 500.0
        FT(-1.2e-8) + (z - 300) * (0 - FT(-1.2e-8)) / (500 - 300) #kg/(kg * s)
    else
        FT(0)
    end

#Large scale subsidence
""" [Tan2018](@cite) """
LifeCycleTan2018_subsidence(::Type{FT}) where {FT} =
    z -> if z <= 1500.0
        FT(0) + z * (FT(-0.65) / 100 - 0) / (1500 - 0)
    elseif z > 1500.0 && z <= 2100.0
        FT(-0.65) / 100 + (z - 1500) * (0 - FT(-0.65) / 100) / (2100 - 1500)
    else
        FT(0)
    end


# # TODO -- find a better home for Domains (or delete them...)
# abstract Domain

# immutable RealDomain <: Domain end
# Base.in(x,::Type{RealDomain}) = typeof(x) <: Real

# immutable NonNegRealDomain <: Domain end
# Base.in(x,::Type{NonNegRealDomain}) = (typeof(x) <: Real && x >= 0)

# domain(::GaussianTransformation) = Real
# domain(::PoissonTransformation) = NonNegRealDomain

## GAUSSIAN ##
"""
    tfm = GaussianTransformation(σ)

Adds zero-mean gaussian noise to an input variable x with standard deviation σ.

X -> ξ,  ξ ∼ N(X,σ)
"""
immutable GaussianTransformation{T<:Real} <: StochasticTransformation
    σ::T
end

# constructors
"""
Adds zero-mean gaussian noise to an input variable x with standard deviation σ.

X -> ξ,  ξ ∼ N(X,σ)
"""
GaussianTransformation(d::Normal) = GaussianTransformation(d.σ)
GaussianTransformation() = GaussianTransformation(1.0)
Transformation(d::Normal) = GaussianTransformation(d.σ)
StochasticTransformation(d::Normal) = GaussianTransformation(d.σ)

# functions
Base.rand(tfm::GaussianTransformation, x) = x + randn()*tfm.σ
transform(tfm::GaussianTransformation, x) = Normal(x,tfm.σ)

## POISSON ##
"""
Transform a non-negative variable, x, into poisson random variable with mean x.

x -> ξ,  ξ ∼ Poisson(x)
"""
immutable PoissonTransformation <: StochasticTransformation end

# constructors
Transformation(d::Poisson) = PoissonTransformation()
StochasticTransformation(d::Poisson) = PoissonTransformation()

# functions
Base.rand(::PoissonTransformation, x) = rand(Poisson(x))
transform(::PoissonTransformation, x) = Poisson(x)

## BERNOULLI ##
"""
Transform an input 0 ≤ x ≤ 1 into a Bernoulli random variable with mean x.

x -> ξ,  ξ ∼ Bernoulli(x)
"""
immutable BernoulliTransformation <: StochasticTransformation end

# constructors
Transformation(d::Bernoulli) = BernoulliTransformation()
StochasticTransformation(d::Bernoulli) = BernoulliTransformation()

# functions
Base.rand(::BernoulliTransformation, x) = rand(Bernoulli(x))
transform(::BernoulliTransformation, x) = Bernoulli(x)

## NEGATIVE BINOMIAL ##
"""
Transform a non-negative variable, x, into a negative binomial random variable
with mean x and (fixed) variance σ²

x -> ξ,  ξ ∼ NegativeBinomial,  E{ξ} = x
"""
immutable NegativeBinomialTransformation{T<:Real} <: StochasticTransformation
    σ²::T
end

# constructors
Transformation(d::NegativeBinomial) = NegativeBinomialTransformation(var(d))
StochasticTransformation(d::NegativeBinomial) = NegativeBinomialTransformation(var(d))
NegativeBinomialTransformation(d::NegativeBinomial) = NegativeBinomialTransformation(var(d))
NegativeBinomialTransformation() = NegativeBinomialTransformation(1.0)

# functions
function specify_moments(::Type{NegativeBinomial},μ,σ²)
    p = μ / σ²
    r = μ*p / (1.0-p)
    return NegativeBinomial(r,p)
end
Base.rand(tfm::NegativeBinomialTransformation, x) = rand(specify_moments(NegBinomial,x,tfm.σ²))
transform(tfm::NegativeBinomialTransformation, x) = specify_moments(NegBinomial,x,tfm.σ²)


function bulk_modulus(l, m)
    return l + 2m / 3
end

function lame_lambda(k, m)
    return k - 2m / 3
end

function stress_field(x)
    return [1.0, 1.0, 0.0]
end

function onleftboundary(x, L, W)
    return x[1] ≈ 0.0
end

function onbottomboundary(x, L, W)
    return x[2] ≈ 0.0
end

function onrightboundary(x, L, W)
    return x[1] ≈ L
end

function ontopboundary(x, L, W)
    return x[2] ≈ W
end

function assemble_linear_system(
    mesh,
    basis,
    cellquads,
    facequads,
    interfacequads,
    stiffness,
    penalty,
    eta,
    displacementscale,
)

    L, W = CutCellDG.widths(mesh)
    lambda1, mu1 = CutCellDG.lame_coefficients(stiffness, +1)

    sysmatrix = CutCellDG.SystemMatrix()
    sysrhs = CutCellDG.SystemRHS()

    CutCellDG.assemble_displacement_bilinear_forms!(
        sysmatrix,
        basis,
        cellquads,
        stiffness,
        mesh,
    )
    CutCellDG.assemble_interelement_condition!(
        sysmatrix,
        basis,
        facequads,
        stiffness,
        mesh,
        penalty,
        eta,
    )
    CutCellDG.assemble_coherent_interface_condition!(
        sysmatrix,
        basis,
        interfacequads,
        stiffness,
        mesh,
        penalty,
        eta,
    )
    CutCellDG.assemble_penalty_displacement_component_bc!(
        sysmatrix,
        sysrhs,
        x -> 0.0,
        basis,
        facequads,
        stiffness,
        mesh,
        x -> onleftboundary(x, L, W),
        [1.0, 0.0],
        penalty,
    )
    CutCellDG.assemble_penalty_displacement_component_bc!(
        sysmatrix,
        sysrhs,
        x -> 0.0,
        basis,
        facequads,
        stiffness,
        mesh,
        x -> onbottomboundary(x, L, W),
        [0.0, 1.0],
        penalty,
    )

    CutCellDG.assemble_traction_force_linear_form!(
        sysrhs,
        x -> stress_field(x)[[1, 3]],
        basis,
        facequads,
        mesh,
        x -> onrightboundary(x, L, W),
    )
    CutCellDG.assemble_traction_force_linear_form!(
        sysrhs,
        x -> stress_field(x)[[3, 2]],
        basis,
        facequads,
        mesh,
        x -> ontopboundary(x, L, W),
    )

    matrix = CutCellDG.sparse_displacement_operator(sysmatrix, mesh)
    rhs = CutCellDG.displacement_rhs_vector(sysrhs, mesh)

    return matrix, rhs
end

function nodal_displacement(
    distancefunc,
    stiffness,
    nelmts,
    basis,
    numqp,
    penaltyfactor,
    eta,
)
    L, W = 1.0, 1.0
    lambda1, mu1 = CutCellDG.lame_coefficients(stiffness, +1)
    lambda2, mu2 = CutCellDG.lame_coefficients(stiffness, -1)

    dx = 1.0 / nelmts
    penalty = penaltyfactor / dx * 0.5 * (lambda1 + mu1 + lambda2 + mu2)
    displacementscale = 0.1

    mesh = CutCellDG.DGMesh([0.0, 0.0], [L, W], [nelmts, nelmts], basis)
    levelset = InterpolatingPolynomial(1, basis)
    levelsetcoeffs = CutCellDG.levelset_coefficients(distancefunc, mesh)

    cutmesh = CutCellDG.CutMesh(mesh, levelset, levelsetcoeffs)
    cellquads =
        CutCellDG.CellQuadratures(cutmesh, levelset, levelsetcoeffs, numqp)
    interfacequads =
        CutCellDG.InterfaceQuadratures(cutmesh, levelset, levelsetcoeffs, numqp)
    facequads =
        CutCellDG.FaceQuadratures(cutmesh, levelset, levelsetcoeffs, numqp)

    mergedwithcell, hasmergedcells = CutCellDG.merge_tiny_cells_in_mesh!(
        cutmesh,
        cellquads,
        facequads,
        interfacequads,
        tinyratio = 0.2,
    )
    mergedmesh = CutCellDG.MergedMesh(cutmesh, mergedwithcell)

    matrix, rhs = assemble_linear_system(
        mergedmesh,
        basis,
        cellquads,
        facequads,
        interfacequads,
        stiffness,
        penalty,
        eta,
        displacementscale,
    )

    nodaldisplacement = matrix \ rhs

    return nodaldisplacement, mergedmesh, cellquads, facequads, interfacequads
end


# Very simple implementation of some adaptive MCMC algorithms.
module AdaptiveMCMC

import Base.get

export adaptive_rwm, AdaptiveMetropolis, RobustAdaptiveMetropolis,
AdaptiveScalingMetropolis, AdaptiveScalingWithinAdaptiveMetropolis,
AdaptState, RWMState,
draw!, accept!, adapt!, adapt_rb!,
StepSize, PolynomialStepSize, RAMStepSize, get

using Random, LinearAlgebra, StaticArrays

# Abstract type of the state of an adaptive random walk Metropolis MCMC.
abstract type AdaptState end

include("linearalgebra_helpers.jl")
include("rwm.jl")       # Random walk Metropolis functionality
include("stepsize.jl")  # Step size sequences
include("ram.jl")       # RobustAdaptiveMetropolis
include("am.jl")        # AdaptiveMetropolis
include("asm.jl")       # AdaptiveScalingMetropolis and AdaptiveScalingWithinAdaptiveMetropolis
include("adaptive_rwm.jl")      # Adaptive random walk Metropolis interface `arwm`

end # module


module AbstractSDRsFMReceiver

using AbstractSDRs;
using DSP;
using FFTW;

include("Audio.jl");
using .Audio

include("GetSpectrum.jl")
using .GetSpectrum

using Blink;        # For GUI in electron
using Interact;      # For Widget and interactions
using PlotlyJS;

FLAG = false;

"""
Apply FM demodulation to the input signal
It applies Arg(sig[n+1] sig*[n]) where * stands for complex conjugate
# --- Syntax
out = fmDemod(sig)
# --- Input parameters
- sig : Input signal (expected to be complex Vector)
# --- Output parameters
- out : Demodulated signal (Float64 Vector)
"""
function fmDemod(sig)
    out = zeros(Float64,length(sig));
    @inbounds @simd for n ∈ (1:length(sig)-1)
        out[n+1] = angle(sig[n+1]*conj(sig[n]));
    end
    return out;
end

"""
Init a Low pass filter (based on FIR filter synthesis) that cut @bandPass
# --- Syntax
h = genFilter(sizeL,bandPass,samplingRate)
# --- Input parameters
- sizeL : Desired filter size
- bandPass : Max frequency in bandpass
- samplingRate : sampling frequency
# --- Output parameters
- h : filter (Vector{Float64}(sizeL,1))
"""
function genFilter(sizeL,bandPass,samplingRate)
    # --- Init frequency response
    cuttOff = bandPass / samplingRate;
    fRep  = zeros(Complex{Float64},sizeL);
    # --- Set passband value
    fRep[1:Int(floor(sizeL*cuttOff))] .= 1;
    # --- Adding linear phase term
    pulsation   = 2*pi*(0:sizeL-1) ./sizeL;# --- Pulsation \Omega
    groupDelay  = - (sizeL-1)/2;# --- Group delay
    fRep        .= fRep .* exp.(1im * groupDelay .* pulsation);
    # --- Get to time domain
    # With a force to cast in real domain
    hRep  = real(ifft(fRep));
    # --- Generates window
    wind  = hamming(sizeL);
    # --- Apply windowing (apodisation)
    h  = hRep .* wind;
end

"""
Apply the filter by direct convoltuion and downsample
# --- Syntax
 out = applyFilter(sig,h,decim)
# --- Input parameters
- sig : Signal to be filtered and downsampled (of size N)
- h : Filter to apply
- decim : Desired decimation factor (should be Int)
# --- Output parameters
- out : filtered and downsampled data (size N÷decim)
"""
function applyFilter(x,h,decim)
    l = length(h)÷2;
    return conv(x,h)[1+l:decim:end-l];
end

"""
Compute Power Spectral Density (PSD) of signal and returns a tuple (frequency,power) ready to be computed
# --- Syntax
(fs,pow) = getSpectrum(f,sig,N)
# --- Input parameters
- f : sampling frequency (in Hz)
- sig : Signal to be analyzed
# --- Output parameters
- fs : Vector of frequencies
- pow: Vector of power
"""
function getSpectrum(fs,sig;N=nothing)
    if isnothing(N)
        N = length(sig);
    end
    freqAx = collect(((0:N-1)./N .- 0.5)*fs);
    ss	   = @view sig[1:N];
    y	   = 10*log10.(abs2.(fftshift(fft(ss))));
    return (freqAx,y);
end
getSpectrum(sig) = getSpectrum(1,sig);

function main(sdr,carrierFreq,gain=20,p=nothing;kwargs...)
    # --- Simulation parameters
    global FLAG = true;
    global newCarrierFreq	  = carrierFreq;		# --- Starting frequency
    samplingRate  			  = 48e3*4;
    # --- Duration and buffer size
    duration                  = 0.5;
    nbSamples                 = Int( duration * samplingRate);
    # --- Update radio configuration
    global radio			= openSDR(sdr,newCarrierFreq,samplingRate,gain;kwargs...);
    # --- Create FIR filter
    audioRendering = 48e3;
    decim          = Int(samplingRate ÷ audioRendering);
    h              = genFilter(128,48e3,samplingRate);
    # --- Audio rendering
    stream = Audio.createAudioBuffer();
    try
        while(true)
            # --- We get buffer
            sig = recv(radio,nbSamples);
            # --- FM demodulator
            out = fmDemod(sig);
            # --- Audio decimation
            audio = applyFilter(out,h,decim);
            # --- Audio rendering
            Audio.play(stream,audio);
            # --- Plot
            deletetraces!(p,0);
            (x,y) = getSpectrum(samplingRate,sig[1:128]);
            pl1	  = scatter(; x,y, name=" ");
            addtraces!(p,pl1);
            # Exit
            if FLAG == false;
                print("BREAK");
                break;
            end
        end
    catch exception;
        close(stream);
        close(radio);
        rethrow(exception);
    end
    close(stream);
    close(radio);
end

"""
Main call to the Graphical User Interace (GUI)
# --- Syntax
gui()
# --- Input parameters
-
# --- Output parameters
-
"""
function gui()
    global FLAG = false;
    # ----------------------------------------------------
    # --- Define widgets
    # ----------------------------------------------------
    # --- Creating widget for FM frequencies
    frequencies = widget(88:0.1:107, label="FM frequencies (MHz)");
    widgetGain        = widget(0:50, label="Radio Gain");
    # --- Creating some stuff related to args
    widgetArgs = textbox(hint=""; value="addr=192.168.10.13");
    # --- Widget for radio configuration
    sdrList = AbstractSDRs.getSupportedSDRs();
    options = Observable(sdrList);
    wdg = dropdown(options);
    # --- Start button
    startButton = button("Start !"; value=0)
    # ----------------------------------------------------
    # ---
    w = Window(Blink.@d(:width=>600, :height=>600));
    title(w,"FM receiver in Julia");
    # --- Horizontal axis
    layout = hbox(pad(1em,wdg),pad(1em,widgetArgs));
    layout = vbox(layout,frequencies);
    layout = vbox(layout,widgetGain);
    layout = hbox(layout,pad(3em,startButton));
    #
    plotLayout = Layout(;
        width = 600,
        height = 400,
        title=" Spectrum ",
        xaxis_title=" Frequency  ",
        yaxis_title=" Power ",
        xaxis_showgrid=true, yaxis_showgrid=true,
        yaxis_range =[-80,0]
        )
    N = 100;
    x = 0:N-1;
    y = zeros(N);
    pl1	  = scatter(; x,y, name=" ");
    p = plot(pl1,plotLayout)
    layout = vbox(layout,p);

    body!(w,layout);
    # ----------------------------------------------------
    # ---
    # ----------------------------------------------------
    # --- Update Frequency
    on(updateC,frequencies);
    # --- Update Gain
    on(updateG,widgetGain);
    # --- Starting routine
    h = on(startButton) do click
        startButton["is-loading"][]=true
        #@show click
        #if click == 1
        #    map!(fromStartToStop,startButton,12,16)
        #end
        sdr =  wdg[];
        carrierFreq = frequencies[]*1e6
        gain        = widgetGain[];
        kwargs = (;args=widgetArgs[]);
        @async main(sdr,carrierFreq,gain,p;kwargs...);
        # @async main(:e310,carrierFreq;args="addr=192.168.10.13");
    end
    success(w.shell.proc)
    global FLAG=false;
    close(radio);
    # We get the hand back and can close the radio
    return nothing
end

function updateC(val)
    if FLAG == true
        carrierFreq = val*1e6;
        updateCarrierFreq!(radio,carrierFreq);
    end
end
function updateG(val)
    if FLAG == true
        gain = val;
        updateGain!(radio,gain);
    end
end

    end # module


using YAActL

function fib(D::Dict{Int,BigInt}, n::Int)
    get!(D, n) do
        n == 0 && return big(0)
        n == 1 && return big(1)
        return fib(D, n-1) + fib(D, n-2)
    end
end

myfib = Actor(fib, Dict{Int,BigInt}())

call!(myfib, 1000)


using JuMP, EAGO

                     m = Model()

                     EAGO.register_eago_operators!(m)

                     @variable(m, -1 <= x[i=1:2] <= 1)

                     @variable(m, -6.103907844236108 <= q <= 8.378675476959124)

                     add_NL_constraint(m, :(swish(-0.8297607126794757 + -0.5064858067335005*swish(-0.6188655598844357 + -0.38627203797486676*$(x[1]) + 0.4878933200630442*$(x[2])) + -0.8619658571743893*swish(-0.7914291135758744 + 0.8682104207735972*$(x[1]) + 0.3684602934879275*$(x[2])) + 0.8415580445654567*swish(-0.26405078449033637 + -0.6452849815457378*$(x[1]) + 0.9586530591287103*$(x[2]))) + swish(-0.0809505831099564 + 0.6185835236357913*swish(-0.6188655598844357 + -0.38627203797486676*$(x[1]) + 0.4878933200630442*$(x[2])) + -0.5288114683363174*swish(-0.7914291135758744 + 0.8682104207735972*$(x[1]) + 0.3684602934879275*$(x[2])) + -0.42070009030061817*swish(-0.26405078449033637 + -0.6452849815457378*$(x[1]) + 0.9586530591287103*$(x[2]))) + swish(0.16011867292674786 + -0.14898607305663658*swish(-0.6188655598844357 + -0.38627203797486676*$(x[1]) + 0.4878933200630442*$(x[2])) + -0.8220978033005912*swish(-0.7914291135758744 + 0.8682104207735972*$(x[1]) + 0.3684602934879275*$(x[2])) + -0.8518875556726564*swish(-0.26405078449033637 + -0.6452849815457378*$(x[1]) + 0.9586530591287103*$(x[2]))) - $q <= 0.0))

                     @objective(m, Min, q)

                     return m


@rule BIFM(:znext, Marginalisation) (m_out::MultivariateNormalDistributionsFamily, m_in::MultivariateNormalDistributionsFamily, m_zprev::ProdFinal{<:MultivariateNormalDistributionsFamily}, meta::BIFMMeta) = begin

    # fetch information from meta data
    A       = getA(meta)
    B       = getB(meta)
    H       = getH(meta)
    ξztilde = getξztilde(meta)
    BHBt    = getBHBt(meta)
    Λz      = getΛz(meta)

    # fetch statistics of incoming messages
    μ_in, Σ_in          = mean_cov(m_in)
    μ_zprev, Σ_zprev    = mean_cov(m_zprev)

    # calculate intermediate quantities
    F = I - Λz * BHBt

    m_ztilde = A * μ_zprev
    V_ztilde = A * Σ_zprev * A'

    # save required intermediate variables
    setΣu!(meta, Σ_in)
    setμu!(meta, μ_in)

    # calculate statistics of outgoing marginal
    μ_znext = F' * m_ztilde + B * ((Σ_in * (B' * ξztilde)) + μ_in)
    Σ_znext = F' * V_ztilde * F + BHBt

    # Actual return type depends on meta object as well, so we explicitly cast the result here
    # Should be noop if type matches
    T = promote_type(eltype(m_out), eltype(m_in), eltype(m_zprev))

    # return outgoing marginal
    return ProdFinal(convert(MvNormalMeanCovariance{T}, MvNormalMeanCovariance(μ_znext, Σ_znext)))

end


using FractionalDiffEq#, Plots

rightfun(x)=sin(x^2)
prob = MultiTermsFODEProblem([1 8 26 73 90], [3.5 3.1 2.3 1.2 0.5], rightfun, [30 90 3], [1 0.3 0], [0, 0], (0, 5))
sol = solve(prob, 0.5, ClosedForm())
plot(sol)


using Oceananigans

# Definition of the "Bickley jet": a sech(y)^2 jet with sinusoidal tracer
Ψ(y) = - tanh(y)
U(y) = sech(y)^2

# A sinusoidal tracer
C(y, L) = sin(2π * y / L)

# Slightly off-center vortical perturbations
ψ̃(x, y, ℓ, k) = exp(-(y + ℓ/10)^2 / 2ℓ^2) * cos(k * x) * cos(k * y)

# Vortical velocity fields (ũ, ṽ) = (-∂_y, +∂_x) ψ̃
ũ(x, y, ℓ, k) = + ψ̃(x, y, ℓ, k) * (k * tan(k * y) + y / ℓ^2)
ṽ(x, y, ℓ, k) = - ψ̃(x, y, ℓ, k) * k * tan(k * x)

"""
    u, v: Large-scale jet + vortical perturbations
       c: Sinusoid
"""
function set_bickley_jet!(model;
                          ϵ = 0.1, # perturbation magnitude
                          ℓ = 0.5, # gaussian width
                          k = 0.5) # sinusoidal wavenumber

    # total initial conditions
    uᵢ(x, y, z) = U(y) + ϵ * ũ(x, y, ℓ, k)
    vᵢ(x, y, z) = ϵ * ṽ(x, y, ℓ, k)
    cᵢ(x, y, z) = C(y, model.grid.Ly)

    # Note that u, v are only horizontally-divergence-free as resolution -> ∞.
    set!(model, u=uᵢ, v=vᵢ, c=cᵢ)

    return nothing
end

bickley_grid(; Nh, arch=CPU(), halo=(3, 3, 3)) = RectilinearGrid(arch; size=(Nh, Nh, 1), halo,
                                                                 x = (-2π, 2π), y=(-2π, 2π), z=(0, 1),
                                                                 topology = (Periodic, Periodic, Bounded))


module Gates

using QSpice.BitOps
using QSpice.State

export identity, hadamard, not, cnot, ccnot,
       swap, cswap, sqrtswap, phaseshift, cphase,
       paulix, pauliy, pauliz,
       unitary, choose1, superposition, join, split, qft,
       measure, partialmeasure, probe

function identity(state::QuantumState)
    return copy(state)
end

function superposition(state1::QuantumState, state2::QuantumState)
    return fromstates(state1, state2)
end

function join(state1::QuantumState, state2::QuantumState)
    return QuantumState(vcat(state1.vector, state2.vector), state1.bits + state2.bits)
end

function split(state::QuantumState, lastbit::Int)
    lpos = 2^lastbit
    return QuantumState(state[1:lpos], lastbit), QuantumState(state[lpos:end], state.bits - lastbit)
end

# Performs the following mapping:
# |0> to (|0> + |1>) / sqrt(2)
# |1> to (|0> - |1>) / sqrt(2)
function hadamard(state::QuantumState, bit::Int)
    newstate = Vector{Complex{Float64}}(length(state))
    bit = state.bits - bit
    invsqrt2 = 1.0 / sqrt(2)

    for i = 1:length(state)
        basis  = i - 1
        mapped0 = clearbit(basis, bit)
        mapped1 = setbit(basis, bit)

        if iszero(basis, bit)
            newstate[i] = invsqrt2 * (state[mapped0 + 1] + state[mapped1 + 1])
        else
            newstate[i] = invsqrt2 * (state[mapped0 + 1] - state[mapped1 + 1])
        end
    end

    return QuantumState(newstate, state.bits)
end

# Also called the Pauli-X gate. Performs the following mapping:
# |0> to |1>
# |1> to |0>
function not(state::QuantumState, bit::Int)
    newstate = Vector{Complex{Float64}}(length(state))
    bit = state.bits - bit

    for i = 1:length(state)
        basis = i - 1
        mapped = flipbit(basis, bit)
        newstate[i] = state[mapped + 1]
    end

    return QuantumState(newstate, state.bits)
end

# Performs the following mapping: |x, y> to |x, x + y mod2>
function cnot(state::QuantumState, ctrl::Int, flip::Int)
    newstate = Vector{Complex{Float64}}(length(state))

    ctrl = state.bits - ctrl
    flip = state.bits - flip

    for i = 1:length(state)
        basis = i - 1
        if iszero(basis, ctrl)
            newstate[i] = state[i]
        else
            mapped = flipbit(basis, flip)
            newstate[i] = state[mapped + 1]
        end
    end

    return QuantumState(newstate, state.bits)
end

# Performs the following mapping: |x, y, z> to |x, y, xy + z mod2>
function ccnot(state::QuantumState, ctrl1::Int, ctrl2::Int, flip::Int)
    newstate = Vector{Complex{Float64}}(length(state))

    ctrl1 = state.bits - ctrl1
    ctrl2 = state.bits - ctrl2
    flip  = state.bits - flip

    for i = 1:length(state)
        basis = i - 1
        if !iszero(basis, ctrl1) && !iszero(basis, ctrl2)
            mapped = flipbit(basis, flip)
            newstate[i] = state[mapped + 1]
        else
            newstate[i] = state[i]
        end
    end

    return QuantumState(newstate, state.bits)
end

# Performs the following mapping: |x, y> to |y, x>
function swap(state::QuantumState, x::Int, y::Int)
    newstate = Vector{Complex{Float64}}(length(state))

    x = state.bits - x
    y = state.bits - y

    for i = 1:length(state)
        basis = i - 1
        mapped = swapbit(basis, x, y)
        newstate[i] = state[mapped + 1]
    end

    return QuantumState(newstate, state.bits)
end

# Given the state |x, y, z> performs the following mapping:
# If x equals to 0
#     |x, y, z> to |x, y, z>
# If x equals to 1
#     |x, y, z> to |x, z, y>
function cswap(state::QuantumState, ctrl::Int, x::Int, y::Int)
    newstate = Vector{Complex{Float64}}(length(state))

    ctrl = state.bits - ctrl
    x = state.bits - x
    y = state.bits - y

    for i = 1:length(state)
        basis = i - 1
        if iszero(basis, ctrl)
            newstate[i] = state[i]
        else
            mapped = swapbit(basis, x, y)
            newstate[i] = state[mapped + 1]
        end
    end

    return QuantumState(newstate, state.bits)
end

# Performs the following mapping:
# |00> to |00>
# |01> to (0.5 + 0.5i) * |01> + (0.5 - 0.5i) * |10>
# |10> to (0.5 - 0.5i) * |01> + (0.5 + 0.5i) * |10>
# |11> to |11>
# This more generally translates to the following, less efficient
# implementation:
# |x, y> to (0.5 + 0.5i) * |x, y> + (0.5 - 0.5i) * |y, x>
function sqrtswap(state::QuantumState, x::Int, y::Int)
    newstate = Vector{Complex{Float64}}(length(state))

    x = state.bits - x
    y = state.bits - y

    mulorig = 0.5 + 0.5im
    mulswap = 0.5 - 0.5im

    for i = 1:length(state)
        basis = i - 1
        mapped = swapbit(basis, x, y)
        if basis == mapped
            newstate[i] = state[i]
        else
            newstate[i] = mulorig * state[i] + mulswap * state[mapped + 1]
        end
    end

    return QuantumState(newstate, state.bits)
end

# Given theta, it performs the following mapping:
# |0> to |0>
# |1> to e^(i * theta) * |1>
function phaseshift(state::QuantumState, bit::Int, theta::Float64)
    newstate = Vector{Complex{Float64}}(length(state))
    bit = state.bits - bit
    mul = exp(im * theta)

    for i = 1:length(state)
        basis = i - 1
        if iszero(basis, bit)
            newstate[i] = state[i]
        else
            newstate[i] = mul * state[i]
        end
    end

    return QuantumState(newstate, state.bits)
end

function cphase(state::QuantumState, control::Int, bit::Int, theta::Float64)
    newstate = Vector{Complex{Float64}}(length(state))
    control = state.bits - bit
    bit = state.bits - bit
    mul = exp(im * theta)

    for i = 1:length(state)
        basis = i - 1
        if iszero(basis, bit) || iszero(basis, control)
            newstate[i] = state[i]
        else
            newstate[i] = mul * state[i]
        end
    end

    return QuantumState(newstate, state.bits)
end

paulix(state::QuantumState, bit::Int) = not(state, bit)

# Performs the following mapping:
# |0> to i|1>
# |1> to -i|0>
function pauliy(state::QuantumState, bit::Int)
    newstate = Vector{Complex{Float64}}(length(state))
    bit = state.bits - bit

    for i = 1:length(state)
        basis = i - 1
        if iszero(basis, bit)
            mapped = setbit(basis, bit)
            newstate[i] =  im * state[mapped + 1]
        else
            mapped = clearbit(basis, bit)
            newstate[i] = -im * state[mapped + 1]
        end
    end

    return QuantumState(newstate, state.bits)
end

# A special case of the phaseshift gate. Performs the following mapping:
# |0> to |0>
# |1> to -|1>
function pauliz(state::QuantumState, bit::Int)
    newstate = Vector{Complex{Float64}}(length(state))
    bit = state.bits - bit

    for i = 1:length(state)
        basis = i - 1
        if iszero(basis, bit)
            newstate[i] =  state[i]
        else
            newstate[i] = -state[i]
        end
    end

    return QuantumState(newstate, state.bits)
end

function unitary(state::QuantumState, matrix::Array)
    if length(state.vector) != size(matrix, 1) || length(state.vector) != size(matrix, 2)
        error("The unitary matrix dimensions don't match the quantum state vector's");
    end

    return QuantumState(matrix * state.vector, state.bits)
end

function unitary(state::QuantumState, matrix::Array, x0::Int, xs::Int...)
    bits = [x0, xs...]

    if 2^length(bits) != size(matrix, 1) || 2^length(bits) != size(matrix, 2)
        error("The unitary matrix does not map enough bits")
    end

    newstate = state

    # First swap the bits to the first N positions
    for i = 1:length(bits)
        newstate = swap(newstate, i, bits[i])

        # Track swaps that would effect later qubits. An example:
        # Given a 3 bit state with an operation defined on bits 3 and 1 in that order,
        # let the bits be the x, y and x, their indices 1 2 and 3. We want to achieve
        # the following operation: x, y, z -> z, x, y
        # Step 0: Initial state: x, y, z [remaining bits: 3, 1]
        # Step 1: Swap the bit at position 1 with the first bit of the operation:
        #         x, y, z -> z, y, x [remaining bits: 1]
        # Step 2: Update the remaing bits. Replace every reference to position 1
        #         with the position it's being swapped to: [remaining bits: 3]
        # Step 3: Proceed to Step 1 with the updated remaining list
        for j = i + 1:length(bits)
            if bits[j] == i
                bits[j] = bits[i]
            end
        end
    end

    # Perform the unitary operation
    if length(state) == size(matrix, 1)
        newstate.vector = matrix * newstate.vector
    else
        gatematrix = kron(matrix, speye(2^(state.bits - length(bits))))
        newstate.vector = gatematrix * newstate.vector
    end

    # The bits variable stores the actual operations that were performed to achieve
    # the current bit order. Performing these in reverse order restores the correct
    # qubit order
    sb = length(bits) + 1
    reverse!(bits)
    for i = 1:length(bits)
        newstate = swap(newstate, sb - i, bits[i])
    end

    return newstate
end

function qft(state::QuantumState)
    omega = exp(2 * pi * im / state.bits)
    matrix = [omega^((i - 1) * (j - 1)) for i = 1:length(state), j = 1:length(state)] .* (1.0 / sqrt(length(state)))
    newstate = matrix * state.vector
    return QuantumState(newstate, state.bits)
end

function qft(state::QuantumState, bit::Int, bits::Int...)
    partialsize = 2^length([bit, bits...])
    omega = exp(2 * pi * im / state.bits)
    matrix = [omega^((i - 1) * (j - 1)) for i = 1:partialsize, j = 1:partialsize] .* (1.0 / sqrt(partialsize))
    return unitary(state, matrix, bit, bits...)
end

# Chooses one of the given gates, based on the decimal representation of the classical bit state given in
# the bits argument. It performs the following mapping:
# |x> to gates[decimal(bits) + 1](|x>)
# Where the +1 adjusts the result to Julia's 1-based indexing system
function choose1(state::QuantumState, bits::Vector{Int}, gates::Vector{Vector{Tuple{Function, Vector{Any}}}})
    index = todecimal(bits) + 1
    chain = gates[index]

    newstate::QuantumState = chain[1][1](state, chain[1][2]...)
    for i = 2:length(chain)
        newstate = chain[i][1](newstate, chain[i][2]...)
    end
    return newstate
end

# Selects a random basis vector based on the probabilities given in the argument.
# It uses the same algorithm as the one used in Fitness Proportionate Selection.
# TODO(gustorn): See if the stochastic acceptance variant performs better for large
#                quantum states
function randbasis{T<:Real}(probabilities::Vector{T})
    psum = sum(probabilities)

    target = rand() * psum
    for (i, v) in enumerate(probabilities)
        target -= v
        if (target <= 0)
            return i - 1
        end
    end

    # In case of floating point rounding error, return the element
    # with the otherwise highest probability
    return indmax(probabilities) - 1
end

# Measures the quantum state, collapsing it and returning both the resulting
# posterior state (with the appropriate basis vector having a probability of 1.0)
# and the result of the measurement.
function measure(state::QuantumState)
    probabilities = map(abs2, state.vector)::Vector{Float64}
    basis = randbasis(probabilities)

    # Measure the quantum state
    measurement = reverse(digits(basis, 2, state.bits))

    # Collapse the state
    newstate = fill(0.0 + 0im, length(state))
    newstate[basis + 1] = 1.0 + 0im
    posterior = QuantumState(newstate, state.bits)

    return posterior, measurement
end

# Performs partial measurement on the given bit of the quantum state
# in the computational basis
function partialmeasure(state::QuantumState, bit::Int)
    if state.bits == 1
        return measure(state)
    end

    posterior::Vector{Complex{Float64}} = fill(0.0 + 0im, 2^(state.bits - 1))
    bit = state.bits - bit

    # First collect the coefficients for the probabilities of the qubit being |0>
    prob0 = 0.0
    for i = 1:length(state)
        basis = i - 1
        if iszero(basis, bit)
            prob0 += abs2(state[i])
        end
    end

    # And measure the bit.
    measurement = rand() <= prob0 ? 0 : 1

    # Next collect the coefficients for the basis vectors of the
    # posterior states
    for i = 1:length(state)
        basis = i - 1
        if getbit(basis, bit) == measurement
            postbasis = removebit(basis, bit)
            posterior[postbasis + 1] += state[i]
        end
    end

    # Now normalize the new state
    # TODO(gustorn): see if the error from FP precision has any visible
    # effect on the posterior states
    n = sqrt(sumabs2(posterior))
    posterior = posterior .* (1.0 / n)

    return QuantumState(posterior, state.bits - 1), [measurement]
end

function partialmeasure(state::QuantumState, x0::Int, x1::Int, xs::Int...)
    # First construct the array of bits we want to measure.
    bits = [x0, x1, xs...]
    bits = unique(bits)
    nbits  = length(bits)

    if nbits == state.bits
        return measure(state)
    end

    map!(b -> state.bits - b, bits)
    sort!(bits)

    nposterior = state.bits - nbits
    probabilities::Vector{Float64} = fill(0.0, 2^nbits)
    posterior::Vector{Complex{Float64}} = fill(0.0 + 0im, 2^nposterior)

    # First build up the probabilities for the bits we're going to measure
    for i = 1:length(state)
        basis = i - 1
        measuredbasis = 0
        shiftby = 0

        for bit in bits
            measuredbasis = measuredbasis | (getbit(basis, bit) << shiftby)
            shiftby += 1
        end

        probabilities[measuredbasis + 1] += abs2(state[i])
    end

    # Measure it.
    measurement = digits(randbasis(probabilities), 2, nbits)

    # Now build up the posterior state
    for basis = 0:length(state) - 1
        # Check if the current basis vector matches the measurement at the specified bits
        basismatches = true
        for (bit, result) in zip(bits, measurement)
            if getbit(basis, bit) != result
                basismatches = false
                break
            end
        end

        # We only need to bother with the coefficients if the measurement matches the
        # partial basis vector
        if basismatches
            nremoved = 0
            postindex = basis
            for bit in bits
                postindex = removebit(postindex, bit - nremoved)
                nremoved += 1
            end

            # Add the coefficients from the old state to the new one
            posterior[postindex + 1] += state[basis + 1]
        end
    end

    # Normalize the new state
    n = sqrt(sumabs2(posterior))
    posterior = posterior .* (1 / n)

    # Match the bit order of the regular `measure` function
    reverse!(measurement)
    return QuantumState(posterior, state.bits - length(bits)), measurement
end

# Prints the current state of the qubit in terms of basis vectors, coefficients
# and probabilities. Useful for debugging and visualization. Guaranteed not to disturb
# the quantum state
function probe(state::QuantumState, name::AbstractString)
    println(name, "\n", state)
    return state
end

# The same as probe, but without the need for an explicit name
function probe(state::Vector{Int}, name::AbstractString)
    println(name, "\n", state, "\n")
    return state
end

end


export runTTC, ttc, solve_ttc, readImage, uniformBlur, rgb2gray

##############################################
# Generic convn function using direct method for computing convolutions:
# Accelerated Convolutions for Efficient Multi-Scale Time to Contact Computation in Julia
# Alexander Amini, Alan Edelman, Berthold Horn
##############################################

function readImage(f)
    img1 = restrict(read(f,Image))
end
function rgb2gray(img)
    img = convert(Array{Float64,3}, separate(img))
    (mean(img,3)[:,:,1])
end
function uniformBlur(img, n)
    I = convn(img, ones(n,n)./(n^2))
    I[n:size(I,1)-n+1,n:size(I,2)-n+1]
end

function krestrict(k,img)
    for i=k:-1:1
        img=restrict(img);
    end
    img
end

function solve_ttc( img1, img2 )
    #SOLVE_TTC Computes the time to contact and focus of expansion
    # using two images as input
    ## Inputs:
    #   img1 = mxn matrix double image
    #   img2 = mxn matrix doulbe image (after img1)
    ## Outputs:
    #   out  = 1x3 vector with the following components:
    #          - T  = time to contact
    #          - x0 = x coordinate of FOE
    #          - y0 = y coordinate of FOE

    ## #####################################

    stack = cat(3,img1,img2);
    (m,n) = size(img1);

    ########################################
    ####  DEFINE Kernel Scaling Factors ####
    DIVIDE_EX = 16/(n); ####################
    DIVIDE_EY = 16/(m); ####################
    DIVIDE_ET = 4/(min(n,m)); ##############
    DIVIDE_GG = 1; #########################
    ########################################

    # ######################################

    ## setup

    xv = linspace(-n/2,n/2,n-4); yv=linspace(-m/2,m/2,m-4);
    x = [j for i in yv, j in xv];
    y = [i for i in yv, j in xv];

    ## create kernels that average both images
    sobel = [1;2;1]*[-1 0 1]; #sobel operator is 3x3
    sobel3_x = (1/DIVIDE_EX) * repeat(sobel,outer=[1,1,2]);
    sobel3_y = (1/DIVIDE_EY) * repeat(sobel',outer=[1,1,2]);
    prewitt2_t = (1/DIVIDE_ET) * reshape([1;1]*[-1 -1 1 1],2,2,2); #prewitt operator is 2x2

    EX = convn(stack,sobel3_x); #x dir grad
    EY = convn(stack,sobel3_y); #y dir grad
    ET = convn(stack,prewitt2_t); #t dir grad

    EX = EX[3:m-2,3:n-2,2];
    EY = EY[3:m-2,3:n-2,2];
    ET = ET[3:m-2,3:n-2,2];
    GG = EX.*x./(DIVIDE_GG) + EY.*y./(DIVIDE_GG); #radial grad

    ## solve
    E = [EX[:] EY[:] GG[:]];
    v = E\(-ET[:]); #v=3x1 vector

    ## compile results
    x0 = -v[1]/v[3]; #foe x
    y0 = -v[2]/v[3]; #foe y
    T = 1/v[3];      #ttc
    return T,x0,y0,EX,EY,ET,v; #all results

end

function ttc(img1,img2)

    (T_n,x0_n,y0_n,EX_n,EY_n,ET_n,v_n)=solve_ttc(img1,img2)
    (T_p,x0_p,y0_p,EX_p,EY_p,ET_p,v_p) = (T_n,x0_n,y0_n,EX_n,EY_n,ET_n,v_n)

    T_p = T_n+1

    k=0
    while T_p > T_n && T_n>0
        (T_p,x0_p,y0_p,EX_p,EY_p,ET_p,v_p) = (T_n,x0_n,y0_n,EX_n,EY_n,ET_n,v_n)

        img1_ = restrict(uniformBlur(img1,2));
        img2_ = restrict(uniformBlur(img2,2));

        if minimum(size(img1_))<=6
            break
        end

        (T_n,x0_n,y0_n,EX_n,EY_n,ET_n,v_n)=solve_ttc(img1_,img2_)

        img1=img1_; img2=img2_
        k+=1
    end
    #println(k)
    return T_p, x0_p*k, y0_p*k, EX_p, EY_p, ET_p, v_p*k, k

end

runTTC(f) = runTTC(f, "search");
function runTTC(f, method)
    #f = VideoIO.openvideo("./test_vids/Newman4.avi")
    #f = VideoIO.openvideo("./test_vids/Tipper3.avi")
    #f = VideoIO.openvideo("./test_vids/ttc_car_clip.mp4")
    #f = VideoIO.openvideo("./test_vids/toyota.mp4")

    img1_rgb = readImage(f)
    img1 = rgb2gray(img1_rgb)

    if method=="search"
        img1 = uniformBlur(img1,5);
    elseif isa(method,Int)
        img1 = uniformBlur(img1,method)
    end

    (T,x0,y0,EX,EY,ET,v)=solve_ttc(img1,img1)

    (m,n) = size(img1)

    c = canvasgrid(2,2)
    ops = [:pixelspacing => [1,1]]

    (canvas,cEX,cEY,cET) = (c[1,1],c[1,2],c[2,2],c[2,1])

    canvas, ptnr = ImageView.view(canvas, img1_rgb; ops...)
    cEX, ptnrX = ImageView.view(cEX, EX;ops...)
    cEY, ptnrY = ImageView.view(cEY, EY;ops...)
    cET, ptnrT = ImageView.view(cET, ET;ops...)

    FOE = ImageView.annotate!(canvas, ptnr, ImageView.AnnotationText(0,0, "X"))
    TTC = ImageView.annotate!(canvas, ptnr, ImageView.AnnotationText(0,0, "X"))

    A = ImageView.annotate!(canvas, ptnr, ImageView.AnnotationText(0,0, "A"))
    B = ImageView.annotate!(canvas, ptnr, ImageView.AnnotationText(0,0, "B"))
    C = ImageView.annotate!(canvas, ptnr, ImageView.AnnotationText(0,0, "C"))

    ImageView.annotate!(cEX, ptnrX, ImageView.AnnotationText(n/2,30, "EX", color=RGB(1,0,0), fontsize=35))
    ImageView.annotate!(cEY, ptnrY, ImageView.AnnotationText(n/2,30, "EY", color=RGB(1,0,0), fontsize=35))
    ImageView.annotate!(cET, ptnrT, ImageView.AnnotationText(n/2,30, "ET", color=RGB(1,0,0), fontsize=35))

    records = Matrix(0,4);
    iter = 0;
    while !eof(f)
        try
            #read and view new image
            img2_rgb = readImage(f)
            img2 = rgb2gray(img2_rgb)

            if method=="search"
                img2 = uniformBlur(img2,5);
                #solve the least squares time to contact
                (T,x0,y0,EX,EY,ET,v,k)=ttc(img1,img2)
            elseif method=="none"
                (T,x0,y0,EX,EY,ET,v)=solve_ttc(img1,img2)
            elseif isa(method,Int)
                img2 = uniformBlur(img2,method)
                (T,x0,y0,EX,EY,ET,v)=solve_ttc(img1,img2)
            end

            iter += 1

            if T>10^5 || T<0
                continue
            end

            records = [records; [iter T x0 y0]];

            _, ptnr = ImageView.view(canvas, img2_rgb;ops...)
            _, ptnrX = ImageView.view(cEX, EX;ops...)
            _, ptnrY = ImageView.view(cEY, EY;ops...)
            _, ptnrT = ImageView.view(cET, ET;ops...)

            #remove old annotations
            ImageView.delete!(canvas, FOE)
            ImageView.delete!(canvas, TTC)
            ImageView.delete!(canvas, A)
            ImageView.delete!(canvas, B)
            ImageView.delete!(canvas, C)

            #add new ones
            FOE = ImageView.annotate!(canvas, ptnr, ImageView.AnnotationText(x0+(n)/2, y0+(m)/2, "X", color=RGB(0,1,0), fontsize=20))
            TTC = ImageView.annotate!(canvas, ptnr, ImageView.AnnotationText(n/2,30, string(round(T*10)/10), color=RGB(0,0.5,0.5), fontsize=35))
            A = ImageView.annotate!(canvas, ptnr, ImageView.AnnotationText(n-60,m-60, string("A:    ",round(v[1]*100)/100), color=RGB(0,0.5,0.5), fontsize=15))
            B = ImageView.annotate!(canvas, ptnr, ImageView.AnnotationText(n-60,m-40, string("B:    ",round(v[2]*100)/100), color=RGB(0,0.5,0.5), fontsize=15))
            C = ImageView.annotate!(canvas, ptnr, ImageView.AnnotationText(n-60,m-20, string("C: ",round(v[3]*10000)/10000), color=RGB(0,0.5,0.5), fontsize=15))

            #increment moving image window
            img1_rgb = img2_rgb;
            img1 = img2;

            #sleep to slow down
            #sleep(0)

        catch e
            if isa(e, InterruptException) || (isa(e,ErrorException) && e.msg=="invalid window")
                return  records
            else
                throw(e)
            end
        end
    end
    return records
end


export Arity, Image, Signature, Closure
export Associative, Commutative

"""
    Arity <: Property

Representation of function `f` having the arity `n`, `Arity(f, n)`.
"""
struct Arity <: Property
    f
    n::Int
end

"""
    Image <: Property

Representation of expression `ex` having the mathematical image `S::AbstractSet`,
`Image(ex, S)`.
"""
struct Image{T} <: Property
    ex::T
    S::AbstractSet
end
isvalid(i::Image{<:Union{Symbolic,Variable,Expr}}, ctx::Context) =
    invoke(isvalid, Tuple{Property,Context}, i, ctx)
isvalid(i::Image, ctx::Context) =
    i.ex ∈ i.S || invoke(isvalid, Tuple{Property,Context}, i, ctx)
implies(i::Image, i′::Image, ::Context) = i.ex == i′.ex && i.S ⊆ i′.S

"""
    Signature <: Property

Representation of function `f` having signature `(x₁, x₂, …, xₙ) -> y` given `xᵢ` and `y` as
image sets, `Signature(f, [x₁, x₂, …, xₙ], y)`.
"""
struct Signature <: Property
    f
    args::Vector{AbstractSet}
    S::AbstractSet
end
implies(sig::Signature, a::Arity, ::Context) = a.f == sig.f && a.n == length(sig.args)
implies(sig::Signature, i::Image{Expr}, ctx::Context) =
    sig.S ⊆ i.S && i.ex.head === :call && length(i.ex.args) == 1 + length(sig.args) &&
        sig.f == i.ex.args[1] && all(isvalid(ctx), Image.(i.ex.args[2:end], sig.args))

"""
    Closure <: Property

Representation of function `f` being closed under set `S`, `Closure(f, S)`.
"""
struct Closure <: Property
    f
    S::AbstractSet
end
implies(p::Closure, q::Closure, ::Context) = p.f == q.f && q.S ⊆ p.S
implies(p::Closure, sig::Signature, ::Context) =
    p.f == sig.f && q.S ⊆ p.S && all(sig.args .⊆ Ref(p.S))
implies(p::Closure, i::Image{Expr}, ctx::Context) =
    p.S ⊆ i.S && i.ex.head === :call && !isempty(i.ex.args) &&
        p.f == i.ex.args[1] && all(isvalid(ctx), Image.(i.ex.args[2:end], Ref(p.S)))

"""
    Associative <: Property

Representation of function `f` having the associative property, `Associative(f)`.
"""
struct Associative <: Property
    f
end
implies(p::Associative, a::Arity, ::Context) = a.n == 2 && a.f == p.f

"""
    Commutative <: Property

Representation of function `f` having the commutative property, `Commutative(f)`.
"""
struct Commutative <: Property
    f
end


using MLDataUtils

distance(a::Array{Float64,1}, b::Array{Float64,2}) = sqrt.(vec(sum((a' .- b).^2, 2)))

function get_feat(data::Array{Float64, 2}, X_train::Array{Float64, 2}, y_train, k_index::Int64, class)
    dists = Float64[]
    inclass_X = X_train[y_train .== class, :]

    for i in 1:size(data, 1)
        datum = data[i, :]
        distances = distance(datum, inclass_X)
        sorted_distances_index = sortperm(distances)
        nearest_index = sorted_distances_index[1:k_index]
        dist = sum(distances[nearest_index])
        append!(dists, dist)
    end

    return dists
end

function knnExtract(X, y, k::Int64 = 1, holds::Int64 = 5)
    classes = sort(unique(y))
    CLASS_NUM = length(classes)
    result = zeros(size(X, 1), CLASS_NUM * k)
    train_index, test_index = kfolds(size(X, 1), holds)

    for hold in 1:holds
        X_train, X_test = X[train_index[hold], :], X[test_index[hold], :]
        y_train, y_test = y[train_index[hold]], y[test_index[hold]]

        features = zeros(0, size(X_test, 1))

        for class_index in 1:CLASS_NUM
            for k_index in 1:k
                feat =  get_feat(X_test, X_train, y_train, k_index, classes[class_index])
                features = vcat(features, feat')
            end
        end

        result[test_index[hold], :] = features'
    end

    return result
end


function test_NfOrdCls()
  Qx, x = PolynomialRing(FlintQQ, "x")

  K1, a1 = NumberField(x^3 - 2, "a")
  O1 = EquationOrder(K1)

  K2, a2 = NumberField(x - 2, "a")
  O2 = EquationOrder(K2)

  f3 = x^64 - 64*x^62 + 1952*x^60 - 37760*x^58 + 520144*x^56 - 5430656*x^54 + 44662464*x^52 - 296854272*x^50 + 1623421800*x^48 - 7398867840*x^46 + 28362326720*x^44 - 92043777280*x^42 + 254005423840*x^40 - 597659820800*x^38 + 1200442440064*x^36 - 2057901325824*x^34 + 3006465218196*x^32 - 3732682723968*x^30 + 3922021702720*x^28 - 3467892873984*x^26 + 2561511781920*x^24 - 1565841089280*x^22 + 782920544640*x^20 - 315492902400*x^18 + 100563362640*x^16 - 24754058496*x^14 + 4559958144*x^12 - 602516992*x^10 + 53796160*x^8 - 2968064*x^6 + 87296*x^4 - 1024*x^2 + 2

  K3, a3 = NumberField(f3, "a")
  O3 = EquationOrder(K3)

  @test_and_infer(signature, (O1, ), (1, 1))
  @test_and_infer(signature, O2, (1, 0))
  @test_and_infer(signature, (O3, ), (64, 0))

end


export stieltjes, lanczos, mcdiscretization

function removeZeroWeights(n::Vector{Float64},w::Vector{Float64})
    nw = [n w]
    inds = findall(w->abs(w)>=eps(), nw[:,2])
    # nw = sortrows(nw, by=x->x[1])
    nw = nw[inds,:]
    nw = sortslices(nw, dims=1, by=x->x[1])
    nw[:,1], nw[:,2]
end

"""
    stieltjes(N::Int64,nodes_::Vector{Float64},weights_::Vector{Float64};removezeroweights::Bool=true)
Stieltjes procedure---Given the nodes and weights the function
generates the first`N` recurrence coefficients of the
corresponding discrete orthogonal polynomials.

Set the Boolean `removezeroweights` to `true` if zero weights should be removed.
"""
function stieltjes(N::Int64,nodes_::Vector{Float64},weights_::Vector{Float64};removezeroweights::Bool=true)
    tiny = 10*floatmin()
    huge = 0.1*floatmax()
    α, β = zeros(Float64,N), zeros(Float64,N)
    nodes, weights = removezeroweights ? removeZeroWeights(nodes_,weights_) : (nodes_, weights_)
    Ncap = length(nodes)
    @assert N > 0 && N <= Ncap "N is out of range."
    s0::Float64 = sum(weights)
    @inbounds α[1] = dot(nodes,weights)/s0
    @inbounds β[1] = s0
    N == 1 && return α, β
    p0, p1, p2 = zeros(Float64,Ncap), zeros(Float64,Ncap), ones(Float64,Ncap)
    for k=1:N-1
        p0 = p1
        p1 = p2
        @inbounds p2 = (nodes .- α[k]) .* p1 - β[k]*p0
        s1 = dot(weights,p2.^2)
        s2 = dot(nodes, weights.*p2.^2)
        # s1 = sum( weights[ν]*p2[ν]^2 for ν=1:Ncap )
        # s2 = sum( weights[ν]*nodes[ν]*p2[ν]^2 for ν=1:Ncap )
        @assert abs(s1)>=tiny "Underflow in stieltjes() for k=$k; try using `removeZeroWeights`"
        @assert maximum(abs.(p2))<=huge && abs(s2)<=huge "Overflow in stieltjes for k=$k"
        @inbounds α[k+1] = s2/s1
        @inbounds β[k+1] = s1/s0
        s0 = s1
    end
    α, β
end

"""
    lanczos(N::Int64,nodes::Vector{Float64},weights::Vector{Float64};removezeroweights::Bool=true)
Lanczos procedure---given the nodes and weights the function
generates the first `N` recurrence coefficients of the
corresponding discrete orthogonal polynomials.

Set the Boolean `removezeroweights` to `true` if zero weights should be removed.

The script is adapted from the routine RKPW in
W.B. Gragg and W.J. Harrod, *The numerically stable
reconstruction of Jacobi matrices from spectral data*,
Numer. Math. 44 (1984), 317-335.
"""
function lanczos(N::Int64,nodes::Vector{Float64},weights::Vector{Float64};removezeroweights::Bool=true)
    @assert length(nodes) == length(weights) > 0 "inconsistent number of nodes and weights"
    nodes, weights = removezeroweights ? removeZeroWeights(nodes,weights) : (nodes, weights)
    Ncap = length(nodes)
    (N<=0 || N>Ncap) && error("$N out of range")
    p0 = copy(nodes)
    p1 = zeros(Float64,Ncap);
    @inbounds p1[1] = first(weights)
    for n = 1:Ncap-1
      @inbounds pn = weights[n+1]
      gam = 1.; sig = 0.; t = 0.;
      @inbounds xlam = nodes[n+1]
      for k = 1:n+1
        @inbounds ρ = p1[k] + pn
        tmp = gam*ρ
        tsig = sig
        gam, sig = ρ > 0 ? (p1[k]/ρ, pn/ρ) : (1.,0.)
        @inbounds tk = sig * (p0[k] - xlam) - gam * t
        @inbounds p0[k] -= (tk - t);
        t = tk
        @inbounds pn = sig <= 0 ? tsig*p1[k] : (t^2)/sig
        tsig = sig
        @inbounds p1[k] = tmp;
      end
    end
    @inbounds p0[Base.OneTo(N)], p1[Base.OneTo(N)]
end

"""
    mcdiscretization(N::Int64,quads::Vector{},discretemeasure::Matrix{Float64}=zeros(0,2);discretization::Function=stieltjes,Nmax::Integer=300,ε::Float64=1e-8,gaussquad::Bool=false)
This routine returns ``N`` recurrence coefficients of the polynomials that are
orthogonal relative to a weight function ``w`` that
is decomposed as a sum of ``m`` weights ``w_i`` with domains ``[a_i,b_i]`` for ``i=1,\\dots,m``,
```math
w(t) = \\sum_{i}^{m} w_i(t) \\quad \\text{with } \\operatorname{dom}(w_i) = [a_i, b_i].
```
For each weight ``w_i`` and its domain ``[a_i, b_i]`` the function `mcdiscretization()`
expects a quadrature rule of the form
    nodes::Vector{Float64}, weights::Vector{Float64} = my_quad_i(N::Int64)
all of which are stacked in the parameter `quad`
    quad = [ my_quad_1, ..., my_quad_m ]
If the weight function has a discrete part (specified by `discretemeasure`)
it is added on to the discretized continuous weight function.

The function `mcdiscretization()` performs a sequence of discretizations of the
given weight ``w(t)``, each discretization being
followed by an application of the Stieltjes or Lanczos procedure
(keyword `discretization in [stieltjes, lanczos]`) to produce approximations to the desired recurrence
coefficients.
The function applies to each subinterval ``i`` an `N`-point quadrature rule (the ``i``th entry of `quad`)
to discretize the weight function ``w_i`` on that subinterval.
If the procedure converges to within a prescribed accuracy
`ε` before `N` reaches its maximum allowed value `Nmax`.
If the function does not converge, the function prompts an error message.

The keyword `gaussquad` should be set to `true` if Gauss quadrature rules
are available *for all* ``m`` weights ``w_i(t)`` with ``i = 1, \\dots, m``.

For further information, please see W. Gautschi "Orthogonal Polynomials: Approximation
and Computation", Section 2.2.4.
"""
function mcdiscretization(N::Int64,quads::Vector{},discretemeasure::Matrix{Float64}=zeros(0,2);discretization::Function=stieltjes,Nmax::Integer=300,ε::Float64=1e-8,gaussquad::Bool=false,removezeroweights::Bool=false)
    @assert Nmax>0 && Nmax>N "invalid choice of Nmax=$Nmax."
    @assert ε>0 "invalid choice of ε = $ε"
    @assert discretization in [stieltjes, lanczos] "unknown discretization $discretization"
    @assert length(quads)>0 "no quadrature rule specified"
    δ = !gaussquad ? 1 : 2
    # δ::Int64=1
    # gaussquad ? δ=2 : ()
    mc, mp = size(quads,1), size(discretemeasure,1)
    Δ::Int64=1; kount::Int64=-1;
    α, β, b = zeros(Float64,N), zeros(Float64,N), ones(Float64,N)
    Mi = floor(Int64,(2*N-1)/δ);
    while any(abs.(β-b).>ε*abs.(β))
        b = copy(β);
        kount += 1
        if kount > 1 Δ=2^(floor(Int64,kount/5))*N end
        # kount>1 ? Δ=2^(floor(Int64,kount/5))*N : ()
        Mi+=Δ;
        Mi > Nmax && error("Mi=$Mi exceeds Nmax=$Nmax")
        Ntot = mc*Mi;
        xx, ww = zeros(Float64,Ntot), zeros(Float64,Ntot)
        for i=1:mc
            nn=(i-1)*Mi;
            @inbounds x, w = quads[i](Mi)
            @inbounds xx[nn+1:nn+Mi], ww[nn+1:nn+Mi] = x, w
        end
        if mp>0
            @inbounds xx[Ntot+1:Ntot+mp], ww[Ntot+1:Ntot+mp] = discretemeasure[:,1], discretemeasure[:,2]
        end
        α, β = discretization(N,xx,ww;removezeroweights=removezeroweights)
        end
    # printstyled("\nSuccess: ",color=:green)
    # print("converged after $kount iteration(s).\n\n")
    return α, β
end


"""
Compute velocity `u` and angular velocity `ω` from position.

Assumes θ in radians.
Assumes values at equally spaced time intervals.
"""
function kinematics_from_position(
    x::Vector{Float64},
    y::Vector{Float64},
    θ::Vector{Float64};
    fps::Int=60,
    smooth::Bool=false,
    smooth_wnd::Float64=0.1,  # smoothing window size in seconds
)
    # compute speed
    speed = sqrt.(Δ(x) .^ 2 .+ Δ(y) .^ 2) .* fps

    # compute longitudinal and lateral velocities
    β = atan.(Δ(y), (Δ(x) .+ eps())) .- θ
    u = @. speed * cos(β)
    v = @. speed * sin(β)

    # compute angular velocity
    time = (collect(0:length(speed)) ./ fps)[2:end]
    ω = ∂(time, unwrap(θ))

    # smooth
    if smooth
        wnd = int(fps * smooth_wnd)
        speed = movingaverage(speed, wnd)
        u = movingaverage(u, wnd)
        v = movingaverage(v, wnd)
        ω = movingaverage(ω, wnd)
    end

    return speed, u, v, ω
end


using LinearAlgebra
using JuMP
using Hypatia
using MinimaxAdaptiveControl
using Test
##
@testset "Recreate Anders' results" begin
    A = [2 -1 1.0; 1 0 0; 0 0 0]
    B1 = Matrix{Float64}(undef, 3, 1)
    B1[:] = [0; 0; 1.0]
    B2 = -B1
    Q = I(3)
    γ = 19
    R = I(1)
    As = [A, A]
    Bs = [B1, B2]

    mac = MinimaxAdaptiveControl.MAController(As, Bs, Q, R, γ, [0.; 0; 0])
    PAnders = [
        20.628 -11.101 11.101
        -11.101 7.842 -6.842
        11.101 -6.842 7.842
    ]
    KAnders = [1.786 -1.288 1.288]

    TAnders = [155 -84.4 84.4
                -84.4 89.0 -87.5
                84.4 -87.5 89.0]
    @test norm(mac.candidates[1].P - mac.candidates[2].P) ≈ 0
    @test norm(mac.candidates[1].K + mac.candidates[2].K) ≈ 0
    @test norm(mac.candidates[1].P - PAnders) ≈ 0 atol = 1e-1
    @test norm(mac.candidates[1].K - KAnders) ≈ 0 atol = 1e-3
end

@testset "Validate Tsyn" begin
    A = [2 -1 1.0; 1 0 0; 0 0 0]
    B1 = Matrix{Float64}(undef, 3, 1)
    B1[:] = [0; 0; 1.0]
    B2 = -B1
    Q = I(3)
    γ = 20
    R = I(1)
    As = [A, A]
    Bs = [B1, B2]

    mac = MAController(As, Bs, Q, R, γ, [0.; 0; 0])
    model = Model(Hypatia.Optimizer)
    unset_silent(model) # For now broken...
    Tval, stat = Tsyn(mac, model);

    P = mac.candidates[1].P
    B = B1
    K = mac.candidates[1].K
    # Tval = Tval + 1e-4*I(3) # Might be needed
    ineq13 = P - (Q + K' * R * K + ((A - B * K)' / (inv(P) - γ^(-2) * I(3))) * (A - B * K))
    ineq14 = Tval - (Q + K' * R * K + ((A + B * K)' / (inv(P) - γ^(-2) * I(3))) * (A + B * K))
    ineq15 = Tval - (Q + K' * (R - γ^2 * B' * B) * K + (A' / (inv(Tval) - γ^(-2) * I(3)) * A))

    @test minimum(eigvals(ineq13)) > 0
    @test minimum(eigvals(ineq14)) > 0
    @test minimum(eigvals(ineq15)) > 0
    @test maximum(eigvals(Tval)) < γ^2
end

@testset "Testing X and Z" begin
    # Simple. one-dim
    A1, A2, B1, B2, Q, R = [fill(i * 1., 1, 1) for i in [1,2,3,5,7,11]]
    γ = 13
    As = [A1, A2]
    Bs = [B1, B2]
    mac = MinimaxAdaptiveControl.MAController(As, Bs, Q, R, γ, [0.; 0; 0])
    K1 = mac.candidates[1].K
    P1 = mac.candidates[1].P
    K2 = mac.candidates[2].K
    P2 = mac.candidates[2].P

    model = Model(Hypatia.Optimizer)
    @variable(model, T[1:1,1:1] in PSDCone())

    X12_ref = [(T - Q - K2' * R * K2) (A1 - B1 * K2)
            (A1 - B1 * K2)' inv(P1) - I(1) / γ^2]
    @test X12_ref == MinimaxAdaptiveControl.X(mac, T, 1, 2, 0)

    A11 = A1 - B1 * K1
    A21 = A2 - B2 * K1
    Z121_ref11 = T - Q - K1' * R * K1 + γ^2 / 2 * (A11' * A11 + A21' * A21)
    Z121_ref21 = (A11 + A21)
    Z121_ref12 = (A11 + A21)'
    Z121_ref22 = 4 / γ^4 * (γ^2 * I(1) - T)
    Z121_ref = [Z121_ref11 Z121_ref12
                Z121_ref21 Z121_ref22]

    @test Z121_ref == MinimaxAdaptiveControl.Z(mac, T, 1, 2, 1, 0)
end

@testset "test valuefunction" begin
    A = [3. -1;1 0]
    B1 = Matrix{Float64}(undef, 2, 1)
    B1[:,:] = [1. 0]
    B2 = Matrix{Float64}(undef, 2, 1)
    B2[:,:] = [5 0]

    As = [A,A]
    Bs = [B1,B2]
    Q = I(2) * 1.
    gamma = 22
    R = I(1) * 1.

    mac = MAController(As, Bs, Q, R, gamma, [0.; 0])
    model = Model(Hypatia.Optimizer)
    unset_silent(model)
    Tval, stat = Tsyn(mac, model, 0.01)

    @testset "V is monotonic" begin
        N = 100
        x = [3;-5.]
        u = [0.]
        Vs = zeros(N)
        for k = 1:N
            update!(mac, x, u)
            u = -K(mac)*x
            x = A*x + B1*u
            Vs[k] = Vbar(mac,Tval, x)
        end
        Vdiffs = Vs[2:N] - Vs[1:N-1]
        @test maximum(Vdiffs) <= 0
    end

    @testset "Reduction to ±B" begin
        A = [2 -1 1.0; 1 0 0; 0 0 0]
        B1 = Matrix{Float64}(undef, 3, 1)
        B1[:] = [0; 0; 1.0]
        B2 = -B1
        Q = I(3)
        γ = 19
        R = I(1)*1.
        As = [A, A]
        Bs = [B1, B2]

        x0 = [0.; 0; 0]
        mac = MinimaxAdaptiveControl.MAController(As, Bs, Q, R, γ,x0 )
        model = Model(Hypatia.Optimizer)
        unset_silent(model)
        Tval, stat = Tsyn(mac, model, 1e-4)

        x = [3;-5.;7]
        u = [2.]
        function Vval(mac, x,x0, u)
            P = mac.candidates[1].P
            c1 = x'*P*x - γ^2*(-x + A*x0 +B1*u)'*(-x + A*x0 +B1*u)
            c2 = x'*P*x - γ^2*(-x + A*x0 -B1*u)'*(-x + A*x0 -B1*u)
            c3 = x'*Tval*x - γ^2*((-x + A*x0)'*(-x + A*x0) + u'*B1'*B1*u)
            return maximum([c1, c2, c3])
        end

        update!(mac, x,u)
        @test Vval(mac,x,x0,u) == Vbar(mac,Tval, x)
    end
end


using BSplines

function gen_traj(t, via_points; order=3, basis=nothing)
    @assert length(t) == size(points)[1]
    x = via_points[:,1]
    y = via_points[:,2]
    z = via_points[:,3]

    if basis === nothing
        basis = averagebasis(order, t)
    end
    xspline = BSplines.interpolate(basis, t, x)
    yspline = BSplines.interpolate(basis, t, y)
    zspline = BSplines.interpolate(basis, t, z)

    xspline, yspline, zspline
end


"""
    struct GadgetSphere{T} <: AbstractGadgetGeometry
        center::Vector{T}
        radius::T
    end

Defines a sphere by `center` and `radius`.
To be used for [`read_particles_in_geometry`](@ref)
"""
struct GadgetSphere{T} <: AbstractGadgetGeometry
    center::Vector{T}
    radius::T
end

"""
    function get_geometry_center(sphere::GadgetSphere)

Returns the center of `sphere`.
"""
get_geometry_center(sphere::GadgetSphere) = sphere.center

"""
    get_geometry_box_corners(sphere::GadgetSphere)

Returns a tuple with the lower left and upper right corner of a box which contains the `sphere`.
"""
function get_geometry_box_corners(sphere::GadgetSphere)
    sphere.center .- sphere.radius, sphere.center .+ sphere.radius
end

"""
    get_geometry_mask(sphere::GadgetSphere, pos::Matrix{T}) where T

Returns the indices of all particles contained in the `sphere`.
"""
function get_geometry_mask(sphere::GadgetSphere, pos::Matrix{T}) where T
    r² = sphere.radius^2
    mask = @views @. (pos[1,:] - sphere.center[1])^2 + (pos[2,:] - sphere.center[2])^2 + (pos[3,:] - sphere.center[3])^2 ≤ r²

    return findall(mask)
end


#### Biomass dynamics ODE
# three species dynamical DifferentialEquations

# BEFW model
function BEFW(du, u, p, t)

    ### extinction below threshold value
    for i in 1:length(u)
        u[i] = u[i] <= 10e-12 ? 0.0 : u[i]
    end

    ### Growth (gain)
    producers = [1.0, 0.0, 0.0]
    G = (1 .- (u ./ p.K)) .* producers # logistic growth term
    growth = p.r .* G .* u
    @assert growth[2] == 0
    @assert growth[3] == 0

    ### Consumption
    # functional response
    Fij = functional_response(B = u, parameters = p)

    # gains and losses
    consumption_gain, consumption_loss = consumption(B = u, fr = Fij)
    @assert consumption_gain[1] == 0
    @assert consumption_loss[3] == 0

    ### metabolism loss
    metabolism_loss = p.x .* u
    @assert metabolism_loss[1] == 0

    # calculate changes in biomass
    dbdt = growth .+ consumption_gain .- consumption_loss .- metabolism_loss

    for i in eachindex(dbdt)
        du[i] = dbdt[i] #can't return du directly, have to have 2 different objects dbdt and du for some reason...
    end

    nothing
end

# G = ifelse.(G .== Inf, 0, G)

# findall(G.=-Inf)

# growth[2]
# typeof(growth[2])
function BEFW_CG(du, u, p, t)

    ### extinction below threshold value
    for i in 1:length(u)
        u[i] = u[i] <= 10e-12 ? 0.0 : u[i]
    end

    ### Growth (gain)
    producers = [1.0, 0.0, 0.0]
    G = (1 .- (u ./ p.K)) .* producers # logistic growth term
    growth = p.r .* G .* u
    @assert growth[2] == 0
    @assert growth[3] == 0

    ### Consumption
    # functional response
    Fij = functional_response(B = u, parameters = p)

    # gains and losses
    consumption_gain, consumption_loss = p.response .* consumption(B = u, fr = Fij)
    @assert consumption_gain[1] == 0
    @assert consumption_loss[3] == 0

    ### metabolism loss
    metabolism_loss = p.x .* u
    @assert metabolism_loss[1] == 0

    # calculate changes in biomass
    dbdt = growth .+ consumption_gain .- consumption_loss .- metabolism_loss

    for i in eachindex(dbdt)
        du[i] = dbdt[i] #can't return du directly, have to have 2 different objects dbdt and du for some reason...
    end

    nothing
end
p.response
u = [1/2, 1/2, 1/2]


using LinearAlgebra, LuxurySparse

LinearAlgebra.tr(x::Value) = register(LinearAlgebra.tr, x)
gradient(::typeof(tr), grad, output, x) = (grad * IMatrix{size(x, 1), eltype(grad)}(), )

gradient(::typeof(transpose), grad, output, x::AbstractMatrix) = (transpose(grad), )
function gradient(::typeof(*), grad, output, lhs::AbstractArray{<:Real}, rhs::AbstractArray{<:Real})
    grad * transpose(rhs), transpose(lhs) * grad
end


import CLBLAS
using OpenCL: cl

const clblas = CLBLAS
clblas.setup()

alpha = cl.cl_float(5)
beta = cl.cl_float(2)
A = float32([1.0 2.0 3.0; 4.0 5.0 6.0])  #rand(Float32, 2, 3)
B = float32([1.0 2.0; 3.0 4.0; 5.0 6.0]) #rand(Float32, 3, 2)
C = float32([1.0 2.0; 3.0 4.0])#rand(Float32, 2, 2)

data = (5.0*A*B) + (2.0*C)

future = clblas.clblasSgemm(UInt32(0), UInt32(0), alpha, A, B, beta, C)

result = fetch(future)
println(result)
println("------------")
println(data)
@assert(result == data)
info("success!")

clblas.teardown()


# This file contains functions for the Revenue Efficiency DEA model
"""
    RevenueDEAModel
An data structure representing a revenue DEA model.
"""
struct RevenueDEAModel <: AbstractRevenueDEAModel
    n::Int64
    m::Int64
    s::Int64
    rts::Symbol
    disposX::Symbol
    dmunames::Union{Vector{String},Nothing}
    eff::Vector
    lambda::SparseMatrixCSC{Float64, Int64}
    techeff::Vector
    alloceff::Vector
    Xtarget::Matrix
    Ytarget::Matrix
end

"""
    dearevenue(X, Y, P)
Compute revenue efficiency using data envelopment analysis for
inputs `X`, outputs `Y` and price of outputs `P`.

# Optional Arguments
- `rts=:VRS`: chooses variable returns to scale. For constant returns to scale choose `:CRS`.
- `dispos=:Strong`: chooses strong disposability of inputs. For weak disposability choose `:Weak`.
- `names`: a vector of strings with the names of the decision making units.

# Examples
```jldoctest
julia> X = [5 3; 2 4; 4 2; 4 8; 7 9.0];

julia> Y = [7 4; 10 8; 8 10; 5 4; 3 6.0];

julia> P = [3 2; 3 2; 3 2; 3 2; 3 2.0];

julia> dearevenue(X, Y, P)
Revenue DEA Model
DMUs = 5; Inputs = 2; Outputs = 2
Orientation = Output; Returns to Scale = VRS
──────────────────────────────────
    Revenue  Technical  Allocative
──────────────────────────────────
1  0.644444   0.777778    0.828571
2  1.0        1.0         1.0
3  1.0        1.0         1.0
4  0.5        0.5         1.0
5  0.456522   0.6         0.76087
──────────────────────────────────
```
"""
function dearevenue(X::Union{Matrix,Vector}, Y::Union{Matrix,Vector},
    P::Union{Matrix,Vector}; rts::Symbol = :VRS, dispos::Symbol = :Strong,
    names::Union{Vector{String},Nothing} = nothing,
    optimizer::Union{DEAOptimizer,Nothing} = nothing)::RevenueDEAModel

    # Check parameters
    nx, m = size(X, 1), size(X, 2)
    ny, s = size(Y, 1), size(Y, 2)

    np, sp = size(P, 1), size(P, 2)

    if nx != ny
        throw(DimensionMismatch("number of rows in X and Y ($nx, $ny) are not equal"));
    end
    if np != ny
        throw(DimensionMismatch("number of rows in P and Y ($np, $ny) are not equal"));
    end
    if sp != s
        throw(DimensionMismatch("number of columns in P and Y ($sp, $s) are not equal"));
    end

    if dispos != :Strong && dispos != :Weak
        throw(ArgumentError("`disposY` must be :Strong or :Weak"));
    end

    # Default optimizer
    if optimizer === nothing
        optimizer = DEAOptimizer(:LP)
    end

    # Get maximum revenue targets and lambdas
    n = nx

    Xtarget = X[:,:]
    Ytarget, rlambdaeff = deamaxrevenue(X, Y, P, rts = rts, dispos = dispos, optimizer = optimizer)

    # Revenue, technical and allocative efficiency
    refficiency  = vec( sum(P .* Y, dims = 2) ./ sum(P .* Ytarget, dims = 2) )
    techefficiency = 1 ./ efficiency(dea(X, Y, orient = :Output, rts = rts, slack = false, disposX = dispos, optimizer = optimizer))
    allocefficiency = refficiency ./ techefficiency
    return RevenueDEAModel(n, m, s, rts, dispos, names, refficiency, rlambdaeff, techefficiency, allocefficiency, Xtarget, Ytarget)

end

ismonetary(model::RevenueDEAModel)::Bool = false;

function Base.show(io::IO, x::RevenueDEAModel)
    compact = get(io, :compact, false)

    n = nobs(x)
    m = ninputs(x)
    s = noutputs(x)
    disposX = x.disposX
    dmunames = names(x)

    eff = efficiency(x)
    techeff = efficiency(x, :Technical)
    alloceff = efficiency(x, :Allocative)

    if !compact
        print(io, "Revenue DEA Model \n")
        print(io, "DMUs = ", n)
        print(io, "; Inputs = ", m)
        print(io, "; Outputs = ", s)
        print(io, "\n")
        print(io, "Orientation = Output")
        print(io, "; Returns to Scale = ", string(x.rts))
        print(io, "\n")
        if disposX == :Weak print(io, "Weak disposability of inputs \n") end

        show(io, CoefTable(hcat(eff, techeff, alloceff), ["Revenue", "Technical", "Allocative"], dmunames))
    end

end