module TestGrid

using Test
import LinearAlgebra: norm, norm2
import GraphIdx
import GraphIdx.Grid: Pixel, line_D, incmat
import GraphIdx.Grid: GridGraph, iter_edges, iter_edges_pixel
import GraphIdx: num_edges, num_nodes

@testset "Pixel                          " begin
    p0 = Pixel(0, 0)
    @test norm(p0) == 0.0
    @test norm2(p0) == 0.0

    p1 = Pixel(1, 1)
    @test norm2(p1) == 2.0
    @test norm(p1) ≈ √2

    p3 = Pixel(1, 3)
    @test norm2(p3) == 10
    @test norm(p3)  ≈ √10
end

@testset "line_D                         " begin
    D = line_D(3)
    @test D ≈ [1  -1   0
               0  +1  -1]
    D = line_D(5)
    @test D ≈ [1  -1   0   0   0
               0  +1  -1   0   0
               0   0  +1  -1   0
               0   0   0  +1  -1]

    for n in [11, 13, 42]
        @test line_D(n) == incmat(n, 1)
        @test line_D(n) == incmat(1, n)
    end
end

@testset "GridGraph: 3x2                 " begin
    g = GridGraph(3, 2)
    @test (g |> collect).edges == [(1, 2), (4, 5), (2, 3), (5, 6), (1, 4), (2, 5), (3, 6)]
end

@testset "GridGraph: 2x3                 " begin
    g = GridGraph(2, 3)
    m = 2*2 + 3
    n = 6
    @test num_nodes(g) == n
    @test num_edges(g) == m
    @test (g |> collect).edges == [
        (1, 2), (3, 4), (5, 6), (1, 3), (3, 5), (2, 4), (4, 6)
    ]
    @testset "iter edge pixels" begin
        local k = 0
        iter_edges_pixel(g) do i1, j1, i2, j2, lam::Float64
            k += 1
            @test lam ≈ 1
            if k == 1
                @test (i1, j1) == (1, 1)
                @test (i2, j2) == (2, 1)
            elseif k == 4
                @test (i1, j1) == (1, 1)
                @test (i2, j2) == (1, 2)
            elseif k == 5
                @test (i1, j1) == (1, 2)
                @test (i2, j2) == (1, 3)
            end
        end
        @test k == m
    end

    @testset "iter edges" begin
        local k = 0
        iter_edges(g) do u, v, lam::Float64
            k += 1
            if k == 1
                @test u == 1
                @test v == 2
            elseif k == 4
                @test u == 1
                @test v == 3
            elseif k == 5
                @test u == 3
                @test v == 5
            else
                @test u < v
            end
            @test lam ≈ 1
        end
        @test k == m
    end
end

@testset "GridGraph: 3x7                 " begin
    """
     1  4  7  10  13  16  19
     2  5  8  11  14  17  20
     3  6  9  12  15  18  21
    """
    g = GridGraph(3, 7)
    edge_graph = g |> collect
    @test edge_graph.edges == [
        (1, 2),
        (4, 5),
        (7, 8),
        (10, 11),
        (13, 14),
        (16, 17),
        (19, 20),
        (2, 3),
        (5, 6),
        (8, 9),
        (11, 12),
        (14, 15),
        (17, 18),
        (20, 21),
        (1, 4),
        (4, 7),
        (7, 10),
        (10, 13),
        (13, 16),
        (16, 19),
        (2, 5),
        (5, 8),
        (8, 11),
        (11, 14),
        (14, 17),
        (17, 20),
        (3, 6),
        (6, 9),
        (9, 12),
        (12, 15),
        (15, 18),
        (18, 21)
    ]
    idx = edge_graph |> e -> GraphIdx.IncidenceIndex(GraphIdx.num_nodes(g), e.edges)
    @test idx[1] == [(2, 1), (4, 15)]
    @test idx[5] == [(4, 2), (6, 9), (2, 21), (8, 22)]
    @testset "compare to enumerate(GridGraph)" begin
        edges = Set{NTuple{2,Int}}()
        GraphIdx.enumerate_edges(g) do e::Int, u::Int, v::Int
            push!(edges, (u, v))
        end
        @test edges == Set(edge_graph.edges)
    end
end

end


using GibbsTypePriors
using Test, Nemo

include("test_common_functions.jl")
include("test_Cnk.jl")
include("test_Cnk_variable_prec.jl")
include("test_Vnk.jl")
include("test_Vnk_variable_prec.jl")
include("test_Pkn.jl")
include("test_Pkn_variable_prec.jl")
include("test_Expect_Kn.jl")


using LinearAlgebraicRepresentation
Lar = LinearAlgebraicRepresentation
using Plasm

function extendlines(V::Lar.Points, EV::Lar.Cells, s=1.001)
	@assert size(V,1)==2 "extendlines() is for 2D models!"
	segments = [hcat(V[:,v1], V[:,v2]) for (v1,v2) in EV]
	n = length(EV)

	for segi in segments
		# renaming of segment vertices
		x1 = segi[1,1]; x2 = segi[1,2];
		y1 = segi[2,1]; y2 = segi[2,2];
		# middle segment point
		tx = sum(x1+x2)/2
		ty = sum(y1+y2)/2
		# composite translation and scaling transformation
		segi[1,1] = s*(x1-tx)+tx; segi[1,2] = s*(x2-tx)+tx;
		segi[2,1] = s*(y1-ty)+ty; segi[2,2] = s*(y2-ty)+ty;
	end
	V = zeros(Float64, 2, 2*length(EV))
	# new vertices for expanded line segments
	for (i,segi) in enumerate(segments)
		V[:,i] = segi[:,1]
		V[:,n+i] = segi[:,2]
	end
	EV = [[k,n+k] for k=1:length(segments)]
	return V, EV
end

n=100; t=0.6
V = zeros(Float64,2,2*n)
EV = [zeros(Int64,2) for k=1:n]

for k=1:n
	v1 = rand(Float64,2)
	v2 = rand(Float64,2)
	vm = (v1+v2)/2
	transl = rand(Float64,2)
	V[:,k] = (v1-vm)*t + transl
	V[:,n+k] = (v2-vm)*t + transl
	EV[k] = [k,n+k]
end

V = Plasm.normalize(V)
model = (V,EV)
Sigma = Lar.spaceindex(model)

Plasm.view(Plasm.numbering(.1)((V,[[[k] for k=1:size(V,2)], EV])))
Z,EZ = extendlines(V,EV,1.1)
Plasm.view(Plasm.numbering(.1)((Z,[[[k] for k=1:size(Z,2)], EZ])))

Plasm.view(V,EV)
model = V,EV;
model2 = Z,EZ;
W,EW = Lar.fragmentlines(model);
Z,EZ = Lar.fragmentlines(model2);

Plasm.viewexploded(W,EW)(1.2,1.2,1.2)
Plasm.viewexploded(Z,EZ)(1.2,1.2,1.2)
W,EW = Lar.fragmentlines(model);
Z,EZ = Lar.fragmentlines(model2);
U,EVs = Lar.biconnectedComponent((W,EW::Lar.Cells));
T,ETs = Lar.biconnectedComponent((Z,EZ::Lar.Cells));
# 2-connected components (H & T)

W = convert(Lar.Points, U')
T = convert(Lar.Points, T')
cop_EV = Lar.coboundary_0(EW::Lar.Cells)
cop_EW = convert(Lar.ChainOp, cop_EV)
cop_EZ = Lar.coboundary_0(EZ::Lar.Cells)
cop_EZ = convert(Lar.ChainOp, cop_EZ)
V, copEV, copFE = Lar.Arrangement.planar_arrangement(W::Lar.Points, cop_EW::Lar.ChainOp)
Z, copEZ, copFE2 = Lar.Arrangement.planar_arrangement(T::Lar.Points, cop_EZ::Lar.ChainOp)

triangulated_faces = Lar.triangulate2D(V, [copEV, copFE])
triangulated_faces2 = Lar.triangulate2D(Z, [copEZ, copFE2])
FVs = convert(Array{Lar.Cells}, triangulated_faces)
FZs = convert(Array{Lar.Cells}, triangulated_faces2)
W = convert(Lar.Points, V')
Z = convert(Lar.Points, Z')
Plasm.viewcolor(W::Lar.Points, FVs::Array{Lar.Cells})
Plasm.viewcolor(Z::Lar.Points, FZs::Array{Lar.Cells})

model = U,EVs;
model2 = T',ETs;
Plasm.view(Plasm.lar_exploded(model)(1.2,1.2,1.2))
Plasm.view(Plasm.lar_exploded(model2)(1.2,1.2,1.2))


include("../utils.jl")

@testset "Multinomial doubly stochastic Matrices" begin
    M = MultinomialDoubleStochastic(3)
    @test manifold_dimension(M) == 4
    @test repr(M) == "MultinomialDoubleStochastic(3)"
    p = ones(3, 3) ./ 3
    X = zeros(3, 3)
    @test is_point(M, p)
    @test is_vector(M, p, X)
    pf1 = [0.1 0.9 0.1; 0.1 0.9 0.1; 0.1 0.1 0.9] #not sum 1
    @test_throws CompositeManifoldError is_point(M, pf1, true)
    pf2r = [0.1 0.9 0.1; 0.8 0.05 0.15; 0.1 0.05 0.75]
    @test_throws DomainError is_point(M, pf2r, true)
    @test_throws CompositeManifoldError is_point(M, pf2r', true)
    pf3 = [1.0 0.0 0.0; 0.0 1.0 0.0; 0.0 0.0 1.0] # contains nonpositive entries
    @test_throws CompositeManifoldError is_point(M, pf3, true)
    Xf2c = [-0.1 0.0 0.1; -0.2 0.1 0.1; 0.2 -0.1 -0.1] #nonzero columns
    @test_throws CompositeManifoldError is_vector(M, p, Xf2c, true)
    @test_throws DomainError is_vector(M, p, Xf2c', true)
    @test representation_size(M) == (3, 3)
    pE = similar(p)
    embed!(M, pE, p)
    pE2 = embed(M, p)
    @test pE == p
    @test pE2 == p
    @test_throws DomainError project(M, -ones(3, 3))
    @test project(M, p) == p
    p2 = [0.1 0.2 0.7; 0.2 0.7 0.1; 0.7 0.1 0.2]
    p3 = [0.1 0.4 0.5; 0.4 0.5 0.1; 0.5 0.1 0.4]

    X2 = [-0.1 0.0 0.1; 0.0 0.2 -0.2; 0.1 -0.2 0.1]
    @test is_vector(
        M,
        p2,
        vector_transport_to(M, p, X2, p2, ProjectionTransport());
        atol=10^-15,
    )
    X3 = [1.0 1.0 1.0; 0.0 0.0 0.0; 0.0 0.0 0.0]
    @test inner(M, p, X3, X3) == 9.0

    types = [Matrix{Float64}]
    TEST_FLOAT32 && push!(types, Matrix{Float32})
    TEST_STATIC_SIZED && push!(types, MMatrix{4,2,Float64,8})

    for T in types
        pts = [convert(T, p), convert(T, p2), convert(T, p3)]
        @testset "Type $T" begin
            test_manifold(
                M,
                pts,
                test_injectivity_radius=false,
                test_reverse_diff=false,
                test_forward_diff=false,
                test_project_tangent=true,
                test_exp_log=false,
                test_default_vector_transport=true,
                vector_transport_methods=[ProjectionTransport()],
                default_inverse_retraction_method=nothing,
                default_retraction_method=ProjectionRetraction(),
                is_tangent_atol_multiplier=20,
                is_point_atol_multiplier=20,
                test_inplace=true,
            )
        end
    end
end


#    Copyright (c) 2021 J.A. Duffek
#    Copyright (c) 2000 D.M. Spink
#
#    This program is free software; you can redistribute it and/or modify
#    it under the terms of the GNU General Public License as published by
#    the Free Software Foundation; either version 2 of the License, or
#    (at your option) any later version.
#
#    This program is distributed in the hope that it will be useful,
#    but WITHOUT ANY WARRANTY; without even the implied warranty of
#    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
#    GNU General Public License for more details.
#
#    You should have received a copy of the GNU General Public License
#    along with this program; if not, see <http://www.gnu.org/licenses/>.

using Plots;

"""
    demogeom()

Demonstration of how to construct a 2D geometric shape from a piece-wise set of
NURBSs.

# Examples:
```julia
julia> demogeom()
```
"""
function demogeom()
coefs = [0.0 7.5 15.0 25.0 35.0 30.0 27.5 30.0;
         0.0 2.5  0.0 -5.0  5.0 15.0 22.5 30.0];
knots = vec([0.0 0.0 0.0 1/6 1/3 1/2 2/3 5/6 1.0 1.0 1.0]);

# Geometric boundary curve
geom = [
  nrbmak(coefs,knots);
  nrbline([30.0;30.0],[20.0;30.0]);
  nrbline([20.0;30.0],[20.0;20.0]);
  nrbcirc(10.0,[10.0;20.0],1.5*pi,0.0);
  nrbline([10.0;10.0],[0.0;10.0]);
  nrbline([0.0;10.0],[0.0;0.0]);
  nrbcirc(5.0,[22.5;7.5])
];

plot(title = "2D Geometry formed by a series of NURBS curves.",
     framestyle=:box,
     legend = false);
for i in geom
  nrbplot!(i,50);
end # for i
current();
end # demogeom


module MaximumLikelihood

using Optim
using PDMats
using ValidatedNumerics
using StatsFuns
using AutoAligns
using ForwardDiff

export estimate_ML, estimate_ML_parameters

"""
Maximum likelihood estimator.

θ  ML estiamte
Σ  covariance matrix (estimated at the mode, using Fisher information)
varnames  variable names
optimization_result  contains convergence information from Optim.optimize
"""
immutable ML_estimator{T, ΣT <: AbstractPDMat, oT}
    θ::Vector{T}
    Σ::ΣT
    varnames::Vector{String}
    optimization_result::oT
end

"""
Default variable names, when not provided.
"""
default_varnames(n) = [string("θ", i) for i in 1:n]

"Calculate a default scaling for the log likelihood."
function calculate_log_ℓ_scale(log_ℓ_scale, log_ℓ, initial_θ)
    if log_ℓ_scale == :default
        Float64(1 / (1+abs(log_ℓ(initial_θ))))
    elseif log_ℓ_scale == :none
        1.0
    elseif isa(log_ℓ_scale, Real)
        convert(Float64, log_ℓ_scale)
    else
        error("Invalid log_ℓ_scale $(log_ℓ_scale)")
    end
end

"""
Internal function for estimating the maximum likelihood parameters of
the log likelihood function `log_ℓ`, with initial value
`initial_θ`. Return the result of `optimize`.

The log likelihood is scaled with `log_ℓ_scale`, for which `:default`
uses information estimates from the function at `initial_θ`, while
`:none` uses a unit scaling.

`method` and `options` are passed to `Optim.optimize`.
"""
function _estimate_ML(log_ℓ, initial_θ;
                     log_ℓ_scale = :default,
                     method = BFGS(),
                     options = Optim.Options(autodiff = true))
    γ = -calculate_log_ℓ_scale(log_ℓ_scale, log_ℓ, initial_θ)
    o = optimize(θ -> γ * log_ℓ(θ), initial_θ,
                 method, Optim.Options(autodiff = true))
    if !Optim.converged(o)
        error("Maximum likelihood did not converge. Check concavity and initial value.")
    end
    o
end

"""
Maximum likelihood estimator. Returns just the parameters as a vector.
"""
function estimate_ML_parameters(log_ℓ, initial_θ,
                                log_ℓ_scale = :default,
                                method = BFGS(),
                                options = Optim.Options(autodiff = true))
    Optim.minimizer(_estimate_ML(log_ℓ, initial_θ,
                                 log_ℓ_scale = log_ℓ_scale,
                                 method = method,
                                 options = options))
end

"""
Estimate parameters using maximum likelihood, with the log likelihood
function `log_ℓ` and initial guess `initial_θ`.

Return an `ML_estimator`, with `varnames` optimally
specified. `method` and `options` are passed to
`Optim.optimize`. `log_ℓ` needs to work with ForwardDiff.
"""
function estimate_ML(log_ℓ, initial_θ;
                     log_ℓ_scale = :default,
                     θ_scale = :default,
                     method = BFGS(),
                     options = Optim.Options(autodiff = true),
                     varnames = default_varnames(length(initial_θ)))
    o = _estimate_ML(log_ℓ, initial_θ;
                     log_ℓ_scale = log_ℓ_scale,
                     method = method,
                     options = options)
    θ = Optim.minimizer(o)
    Σ = inv(PDMat(Symmetric(-ForwardDiff.hessian(log_ℓ, θ))))
    ML_estimator(θ, Σ, varnames, o)
end

"""
Univariate confidence intervals.
"""
function confidence_intervals(ml::ML_estimator, p=0.025)
    @assert p < 0.5
    s = norminvcdf(1-p)
    [θ ± σ*s for (θ, σ) in zip(ml.θ, diag(ml.Σ))]
end

function Base.show(io::IO, ml::ML_estimator; p = 0.025)
    @assert p < 0.5
    aa = AutoAlign(align = Dict(1 => left, :default => right))
    p_string(p) = string(round(100*p, 1), "%")
    v_string(v) = @sprintf("%.5g", v) # significant digits hardcoded
    println(aa, "name", "  ", "est", "  ", p_string(p), "  ", p_string(1-p))
    for (varname, θ, ci) in zip(ml.varnames, ml.θ, confidence_intervals(ml, 0.025))
        println(aa, varname, "", v_string(θ), "", v_string(ci.lo), "", v_string(ci.hi))
    end
    print(io, aa)
end

end # module


export LogisticBinomial, HalfCauchy, HalfNormal, EqualMix

struct HalfCauchy{T<:Real} <: Distribution{Univariate,Continuous}
    σ::T
end

HalfCauchy(σ::Integer = 1) = HalfCauchy(float(σ))

Dists.params(d::HalfCauchy) = (d.σ,)

Dists.logpdf(d::HalfCauchy, x::Real) = log(2) + logdensity(Cauchy(0, d.σ), x)

Dists.pdf(d::HalfCauchy, x) = 2 * pdf(Cauchy(0, d.σ), x)

Base.rand(rng::AbstractRNG, d::HalfCauchy) = abs(rand(rng, Cauchy(0, d.σ)))

Dists.quantile(d::HalfCauchy, p) = quantile(Cauchy(0, d.σ), (p + 1) / 2)

Dists.support(::HalfCauchy) = RealInterval(0.0, Inf)

struct HalfNormal{T<:Real} <: Distribution{Univariate,Continuous}
    σ::T
end

HalfNormal(σ::Integer = 1) = HalfNormal(float(σ))

Dists.params(d::HalfNormal) = (d.σ,)

Dists.logpdf(d::HalfNormal, x::Real) = log(2) + logdensity(Normal(0, d.σ), x)

Dists.pdf(d::HalfNormal, x) = 2 * pdf(Normal(0, d.σ), x)

Base.rand(rng::AbstractRNG, d::HalfNormal) = abs(rand(rng, Normal(0, d.σ)))

Dists.quantile(d::HalfNormal, p) = quantile(Normal(0, d.σ), (p + 1) / 2)

Dists.support(::HalfNormal) = RealInterval(0.0, Inf)

# Binomial distribution, parameterized by logit(p)
LogisticBinomial(n, x) = Binomial(n, logistic(x))

struct EqualMix{T}
    components::Vector{T}
end

Dists.logpdf(m::EqualMix, x) = logsumexp(map(d -> logdensity(d, x), m.components))

rand(m::EqualMix) = rand(GLOBAL_RNG, m)

Base.rand(rng::AbstractRNG, m::EqualMix) = rand(rng, rand(rng, m.components))

xform(d::EqualMix, _data) = xform(d.components[1], _data)

# StudentT(ν, μ = 0.0, σ = 1.0) = LocationScale(μ, σ, TDist(ν))

xform(d::Dists.Dirichlet, _data) = UnitSimplex(length(d.alpha))


# PREAMBLE

# PKG_SETUP

# ## Setup

using IterativeLQR
using LinearAlgebra
using Plots

# ## horizon
T = 101

# ## acrobot
num_state = 4
num_action = 1
num_parameter = 0

function acrobot(x, u, w)
    mass1 = 1.0
    inertia1 = 0.33
    length1 = 1.0
    lengthcom1 = 0.5

    mass2 = 1.0
    inertia2 = 0.33
    length2 = 1.0
    lengthcom2 = 0.5

    gravity = 9.81
    friction1 = 0.1
    friction2 = 0.1

    function M(x, w)
        a = (inertia1 + inertia2 + mass2 * length1 * length1
            + 2.0 * mass2 * length1 * lengthcom2 * cos(x[2]))

        b = inertia2 + mass2 * length1 * lengthcom2 * cos(x[2])

        c = inertia2

       return [a b; b c]
    end

    function Minv(x, w)
        m = M(x, w)
        a = m[1, 1]
        b = m[1, 2]
        c = m[2, 1]
        d = m[2, 2]
        1.0 / (a * d - b * c) * [d -b;-c a]
    end

    function τ(x, w)
        a = (-1.0 * mass1 * gravity * lengthcom1 * sin(x[1])
            - mass2 * gravity * (length1 * sin(x[1])
            + lengthcom2 * sin(x[1] + x[2])))

        b = -1.0 * mass2 * gravity * lengthcom2 * sin(x[1] + x[2])

        return [a; b]
    end

    function C(x, w)
        a = -2.0 * mass2 * length1 * lengthcom2 * sin(x[2]) * x[4]
        b = -1.0 * mass2 * length1 * lengthcom2 * sin(x[2]) * x[4]
        c = mass2 * length1 * lengthcom2 * sin(x[2]) * x[3]
        d = 0.0

        return [a b; c d]
    end

    function B(x, w)
        [0.0; 1.0]
    end

    q = view(x, 1:2)
    v = view(x, 3:4)

    qdd = Minv(q, w) * (-1.0 * C(x, w) * v
            + τ(q, w) + B(q, w) * u[1] - [friction1; friction2] .* v)

    return [x[3]; x[4]; qdd[1]; qdd[2]]
end

function midpoint_explicit(x, u, w)
    h = 0.1 # timestep
    x + h * acrobot(x + 0.5 * h * acrobot(x, u, w), u, w)
end

# ## model
dynamics = Dynamics(midpoint_explicit, num_state, num_action, num_parameter)
model = [dynamics for t = 1:T-1]

# ## initialization
x1 = [0.0; 0.0; 0.0; 0.0]
xT = [0.0; π; 0.0; 0.0]
ū = [1.0 * randn(num_action) for t = 1:T-1]
w = [zeros(num_parameter) for t = 1:T]
x̄ = rollout(model, x1, ū, w)

# ## objective
ot = (x, u, w) -> 0.1 * dot(x[3:4], x[3:4]) + 0.1 * dot(u, u)
oT = (x, u, w) -> 0.1 * dot(x[3:4], x[3:4])
ct = Cost(ot, num_state, num_action, num_parameter)
cT = Cost(oT, num_state, 0, num_parameter)
objective = [[ct for t = 1:T-1]..., cT]

# ## constraints
goal(x, u, w) = x - xT

cont = Constraint()
conT = Constraint(goal, num_state, 0)
constraints = [[cont for t = 1:T-1]..., conT]

# ## problem
prob = Solver(model, objective, constraints)
initialize_controls!(prob, ū)
initialize_states!(prob, x̄)

# ## solve
solve!(prob)

# ## solution
x_sol, u_sol = get_trajectory(prob)

# ## visuals
plot(hcat(x_sol...)')
plot(hcat(u_sol...)', linetype=:steppost)

# ## benchmark allocations + timing
info = @benchmark solve!($prob, x̄, ū) setup=(x̄=deepcopy(x̄), ū=deepcopy(ū))


using OffsetArrays
using ImageFiltering
using Distances
using PaddedViews

"""Apply the opening operation to `A` with window size `ω`."""
function opening(A::Array{T,2}, ω::Integer) where {T<:Real}
    A = mapwindow(minimum, A, (ω, ω))  # erosion
    A = mapwindow(maximum, A, (ω, ω))  # dilation
    A
end

"""Apply the opening operation to `A` with window size `ω`."""
function opening!(A::Array{T,2}, ω::Integer, out::Array{T,2}) where {T<:Real}
    mapwindow_sep!(minimum, A, ω, out, Inf)  # erosion
    mapwindow_sep!(maximum, out, ω, A, -Inf)  # dilation
    A
end

"""Apply the opening operation to `A` with window size `ω`."""
function opening_circ!(A::Array{T,2}, ω::Integer, out::Array{T,2}) where {T<:Real}
    mapwindowcirc!(minimum_mask, A, ω, out, Inf)  # erosion
    mapwindowcirc!(maximum_mask, out, ω, A, -Inf)  # dilation
    A
end

function circmask(n::Integer)
    # TODO This could be precomputed for first N integers
    kern = falses(-n:n, -n:n)
    for I in CartesianIndices(kern)
        i, j = Tuple(I)
        kern[I] = i^2 + j^2 <= n^2
    end
    return kern.parent
end

function opening_circ(A::Array{T,2}, ω::Integer) where {T<:Real}
    m = circmask(ω >> 1)
    A = mapwindow(x -> minimum(x[m]), A, (ω, ω))  # erosion
    A = mapwindow(x -> maximum(x[m]), A, (ω, ω))  # dilation
    A
end

# First discussed here https://github.com/JuliaImages/ImageFiltering.jl/issues/179
function mapwindow!(f, img, window, out)
    R = CartesianIndices(img)
    I_first, I_last = first(R), last(R)
    Δ = CartesianIndex(ntuple(x -> window ÷ 2, ndims(img)))
    @inbounds for I in R
        patch = max(I_first, I - Δ):min(I_last, I + Δ)
        out[I] = f(view(img, patch))
    end
    out
end

function mapwindow_stack!(f, img, window, out)
    R = CartesianIndices(img)
    I_first, I_last = first(R), last(R)
    Δ = CartesianIndex(1, 1)
    out2 = copy(img)
    iterations = window:-2:3
    @inbounds for _ in iterations  # repeat 3x3 window
        for I in R
            patch = max(I_first, I - Δ):min(I_last, I + Δ)
            out[I] = f(view(out2, patch))
        end
        out2 .= out
    end
    out
end

function mapwindow_sep!(f, img, window, out, fill = Inf)
    Δ = window ÷ 2

    w, h = size(img)
    A = PaddedView(fill, img, (-Δ+1:w+Δ, -Δ+1:h+Δ))
    out2 = copy(out)

    # Maximum/minimum is seperable into 1d
    @inbounds for i in 1:h, j in 1:w
        out2[j, i] = f(@view A[j-Δ:j+Δ, i])
    end
    A = PaddedView(fill, out2, (-Δ+1:w+Δ, -Δ+1:h+Δ))
    @inbounds for j in 1:w, i in 1:h
        out[j, i] = f(@view A[j, i-Δ:i+Δ])
    end
    out
end

function mapwindowcirc!(f, img, window, out, fill = Inf)
    R = CartesianIndices(img)
    Δ = CartesianIndex(ntuple(x -> window ÷ 2, ndims(img)))

    w, h = size(img)
    A = PaddedView(fill, img, (-Δ[1]+1:w+Δ[1], -Δ[2]+1:h+Δ[2]))

    m = euclidean.(Tuple.(-Δ:Δ), Ref((0, 0))) .<= Δ[1]
    @inbounds for I in R
        patch = (I-Δ):(I+Δ)
        out[I] = f(view(A, patch), m)
    end
    out
end

function mapwindowcirc_approx!(f, img, window, out, fill = Inf)
    R = CartesianIndices(img)
    Δ = CartesianIndex(1, 1)

    w, h = size(img)

    iterations = window:-2:3
    A = PaddedView(fill, img, (-Δ[1]+1:w+Δ[1], -Δ[2]+1:h+Δ[2]))

    m = euclidean.(Tuple.(-Δ:Δ), Ref((0, 0))) .<= Δ[1]
    @inbounds for _ in iterations  # repeat 3x3 window
        for I in R
            patch = (I-Δ):(I+Δ)
            out[I] = f(view(A, patch), m)
        end
        img .= out
    end
    out
end

@inline function maximum_mask(x, m)
    o = -Inf
    @inbounds for I in eachindex(x)
        o = ifelse(m[I], max(o, x[I]), o)
    end
    o
end

@inline function minimum_mask(x, m)
    o = Inf
    @inbounds for I in eachindex(x)
        o = ifelse(m[I], min(o, x[I]), o)
    end
    o
end


export pai

"""
    pai(x, y, d, τ; w = 0, correspondence_measure = Statistics.cor) → Float64
    pai(x, y, d, τ, bootstrap_method::Symbol; w = 0, correspondence_measure = Statistics.cor,
        method = :segment, L = ceil(Int, (length(x)-d*τ)*0.2), nreps = 100) → Vector{Float64}

Compute the pairwise asymmetric inference (PAI; McCracken, 2014)[^McCracken2014] between `x` and `y`.
Returns the correspondence between original and cross mapped values (the default is
`ρ = correspondence_measure(y(t), ỹ(t) | M_xy)`).

PAI is a modification to Sugihara et al. (2012)'s CCM algorithm[^Sugihara2012], where instead of
using completely out-of-sample prediction when trying to predict ``y(t)``, values about *both* variables
are included in the embedding used to make predictions. Specifically, PAI computes the
correspondence between the values ``y(t)`` and the cross-map estimated values ``ỹ(t) | M_xy``,
where the ``\\tilde{y}(t)`` are the values estimated using the embedding ``M_{xy} = \\{ ( x_t, x_{t-\\tau}, x_{t-2\\tau}, \\ldots, x_{t-(d - 1)\\tau} ) \\}``.
*Note: a `d+1`-dimensional embedding is used, rather than the `d`-dimensional embedding used for CCM.

Like for the CCM algorithm, the Theiler window `r` indicates how many temporal neighbors of the predictee is to be excluded
during the nearest neighbors search (the default `r = 0` excludes only the predictee itself, while
`r = 2` excludes the point itself plus its two nearest neighbors in time).

If `bootstrap_method` is specified, then `nreps` different bootstrapped estimates of
`correspondence_measure(y(t), ỹ(t) | M_x)` are returned. The following bootstrap methods are available:

- `bootstrap_method = :random` selects training sets of length `L` consisting of randomly selected
    points from the embedding ``M_x``  (time ordering does not matter). This is method 3 from Luo
    et al. (2015)[^Luo2015], which critiqued the original Sugihara et al. methodology.
- `bootstrap_method = :segment` selects training sets consisting of time-contiguous segments
    (each of lenght `L`) of embedding vectors in ``M_x`` (time ordering matters). This is
    method 2 from Luo et al. (2015)[^Luo2015].

[^McCracken2014]: McCracken, James M., and Robert S. Weigel. "Convergent cross-mapping and pairwise asymmetric inference." Physical Review E 90.6 (2014): 062903.
[^Sugihara2012]: Sugihara, George, et al. "Detecting causality in complex ecosystems." Science (2012): 1227079.[http://science.sciencemag.org/content/early/2012/09/19/science.1227079](http://science.sciencemag.org/content/early/2012/09/19/science.1227079)
[^Luo2015]: "Questionable causality: Cosmic rays to temperature." Proceedings of the National Academy of Sciences Aug 2015, 112 (34) E4638-E4639; DOI: 10.1073/pnas.1510571112 Ming Luo, Holger Kantz, Ngar-Cheung Lau, Wenwen Huang, Yu Zhou
"""
function pai(x, y, d, τ; correspondence_measure = Statistics.cor, r = 0)
    # Embed `x` and `y`, using only current values for `y` and current+ historical values for `x`.
    Mₓy = crossmapembed(x, y, d, τ, PAIEmbedding())

    # Compute `ỹ | Mₓy` and return `correspondence_measure(ỹ | Mₓy, y).
    crossmap_basic(Mₓy, y, d, τ; correspondence_measure = correspondence_measure, r = r)
end

function pai(x, y, d, τ, bootstrap_method::Symbol;
        L = ceil(Int, (length(x) - d * τ) * 0.2), nreps = 100,
        r = 0, correspondence_measure = Statistics.cor)
    # Embed `x` and `y`, using only current values for `y` and current+ historical values for `x`.
    @assert length(x) == length(y)
    @assert length(x) - d * τ > 0
    Mₓy = crossmapembed(x, y, d, τ, PAIEmbedding())

    # Compute `ỹ | Mₓy` and return `nreps` values for `correspondence_measure(ỹ | Mₓy, y),
    # each value being computed on a random library (sequential or not) consisting of `L` points.
    return crossmap_bootstrap(Mₓy, y, d, τ, bootstrap_method;
        correspondence_measure = correspondence_measure, r = r)
end


"""
Julia expression wrapper
"""
struct Expression
    expr::Expr
    syms::Vector{Symbol}
end
Expression(ex::Expr) = Expression(ex, sort!(symbols(ex)))
show(io::IO, e::Expression) = infix(io, e.expr)
(e::Expression)(val) = evaluate(val, e.expr, e.syms)

function symbols(ex::Expr)
    syms = Symbol[]
    for e in ex.args
        isa(e, Symbol) && push!(syms, e)
        isa(e, Expr) && append!(syms, symbols(e))
    end
    unique!(syms)
end

height(ex) = isa(ex, Expr) ? maximum( height(e) for e in ex.args )+1 : 0
nodes(ex) = !isa(ex, Expr) ? 1 : length(ex.args) > 0 ? sum( nodes(e) for e in ex.args ) : 0
length(ex) = nodes(ex)

function depth(root, ex; d=0)
    return if root == ex
        d
    elseif isa(root, Expr)
        maximum( depth(e, ex, d=d+1) for e in root.args )
    else
        -1
    end
end

function copyto!(ex1::Expr, ex2::Expr)
    ex1.head = ex2.head
    ex1.args = deepcopy(ex2.args)
    ex1
end

function randsubexpr(ex)
    !isa(ex, Expr) && return ex
    csize = map(nodes, ex.args[2:end])
    cidx = rand(1:sum(csize)+1)-1
    if cidx == 0
        ex
    else
        nidx = findfirst(i->cidx<=i, cumsum(csize))
        randsubexpr(ex.args[nidx+1])
    end
end

function getindex(ex::Expr, idx::Int)
    if idx == 0
        ex
    else
        csize = cumsum(map(nodes, ex.args[2:end]))
        nidx = findfirst(i->i>=idx, csize)
        nex = ex.args[nidx+1]
        !isa(nex, Expr) ? nex : nex[csize[nidx]-idx]
    end
end

function setindex!(ex::Expr, subex, idx::Int)
    csize = cumsum(map(nodes, ex.args[2:end]))
    nidx = findfirst(i->i>=idx, csize)
    nex = ex.args[nidx+1]
    if !isa(nex, Expr) || csize[nidx]-idx == 0
        ex.args[nidx+1] = subex
    else
        nex[csize[nidx]-idx] = subex
    end
end

function evaluate(val, ex::Expr, psyms::Vector{Symbol})
    exprm = ex.args
    exvals = (isa(nex, Expr) || isa(nex, Symbol) ? evaluate(val, nex, psyms) : nex for nex in exprm[2:end])
    exprm[1](exvals...)
end

function evaluate(val, ex::Symbol, psyms::Vector{Symbol})
    pidx = findfirst(isequal(ex), psyms)
    val[pidx]
end

iszeronum(root) = isa(root, Number) && iszero(root)
isonenum(root) = isa(root, Number) && isone(root)

"""
    simplify!(expr)

Simplify an algebraic expression
"""
function simplify!(root)
    # return if not an expression
    !isa(root, Expr) && return root
    # return if not a function
    root.head != :call && return root

    # recursively simplify arguments
    for i in 2:length(root.args)
        !isa(root.args[i], Expr) && continue
        root.args[i] = simplify!(root.args[i])
    end

    # some elementary simplification rules
    if root.args[1] == (-) && root.args[2] == root.args[3] # additive inverse: x-x = 0
        root = 0
    elseif root.args[1] == (/) && (root.args[2] == root.args[3] || iszeronum(root.args[3])) # look for multiplicative inverse x/x = 1
        root = 1
    elseif (root.args[1] == (*) || root.args[1] == (/)) &&  (iszeronum(root.args[2]) || iszeronum(root.args[3])) # look for 0: x*0 = 0*x = 0
        root = 0
    elseif (root.args[1] == (+) || root.args[1] == (-)) && iszeronum(root.args[3]) # look for 0: x ± 0 = 0
        root = root.args[2]
    elseif root.args[1] == (+) && iszeronum(root.args[2]) # look for 0: 0 ± x = 0
        root = root.args[3]
    elseif (root.args[1] == (*) || root.args[1] == (/)) && isonenum(root.args[3]) # x*1 = x || x/1 = x
        root = root.args[2]
    elseif root.args[1] == (*) && isonenum(root.args[2]) # 1*x = x
        root = root.args[3]
    else
        # x+x = 2x
        if root.args[1] == (+) && root.args[2] == root.args[3]
            root.args[1] = (*)
            root.args[2] = 2
        end
        # evaluate numerical arithmetic
        if all(e->!(isa(e, Symbol) || isa(e, Expr)), root.args[2:end])
            root = root.args[1](root.args[2:end]...)
        end
    end
    return root
end

function infix(io::IO, root; digits=3)
    if isa(root, Number)
        print(io, round(root, digits=digits))
    elseif isa(root, Expr) && root.head == :call
        if root.args[1] ∈ [+, -, *, /, ^]
            print(io, "(")
            infix(io, root.args[2])
            infix(io, root.args[1])
            infix(io, root.args[3])
            print(io, ")")
        else
            infix(io, root.args[1])
            print(io, "(")
            for ex in root.args[2:end]
                infix(io, ex)
            end
            print(io, ")")
        end
    else
        print(io, root)
    end
end
show(io::IO, ::MIME"text/html", e::Expression) = infix(io, e.expr)

latex(io::IO, root::Number; digits=3) = print(io, round(root, digits=digits))
latex(io::IO, root::Symbol; kwargs...) = print(io, root)
function latex(io::IO, root::Expr; digits=3)
    root.head != :call && print(io, expr)
    if root.args[1] == (/)
        print(io, "\\frac{")
        latex(io, root.args[2], digits=digits)
        print(io, "}{")
        latex(io, root.args[3], digits=digits)
        print(io, "}")
    elseif root.args[1] ∈ [+, -, *]
        print(io, "\\left(")
        latex(io, root.args[2], digits=digits)
        print(io, root.args[1])
        latex(io, root.args[3], digits=digits)
        print(io, "\\right)")
    elseif root.args[1] == (^)
        print(io, "{")
        latex(io, root.args[2], digits=digits)
        print(io, "}^{")
        latex(io, root.args[3], digits=digits)
        print(io, "}")
    else
        print(io, "\\")
        print(io, root.args[1])
        print(io, "\\left(")
        for ex in root.args[2:end]
            latex(io, ex, digits=digits)
        end
        print(io, "\\right)")
    end
end
show(io::IO, ::MIME"text/latex", e::Expression) = latex(io, e.expr)


module TestMGT

include("square.jl")

using Test
import TreeLas.MGT
import GraphIdx
import GraphIdx: EdgeGraph
import GraphIdx.Io: prettyarr

Base.sort(i::T, j::T) where {T} =
    i < j ? (i, j) : (j, i)

@testset "MGT     : Square               " begin
    edges, n = square_edges()
    m = length(edges)
    lambda = GraphIdx.Const(0.5)
    y = Float64[0 1; 0 0.5]
    x = MGT.gaplas(y, edges, lambda)
    @test size(x) == size(y)
    @test sum(x) ≈ sum(y)
    @test x ≈ mean(y) * ones(size(y)...)
end

@testset "MGT     : demo 3x7             " begin
    include("demo3x7.jl")
    n1, n2 = 3, 7
    n = n1 * n2
    m = 32
    root = 1
    s = collect(1:n) |> x -> reshape(x, n1, n2)
    @test prettyarr(s) == """
    3×7 Array{Int64,2}:
     1  4  7  10  13  16  19
     2  5  8  11  14  17  20
     3  6  9  12  15  18  21
    """ |> chomp
    @test size(y) == (n1,n2)

    edges = GraphIdx.Edge{Int}[
        (1,  2),  (4,  5),  (7, 8), (10, 11), (13, 14), (16, 17),
        (19, 20), (2,  3),  (5, 6), (8,   9), (11, 12), (14, 15),
        (17, 18), (20, 21), (1, 4), (4,   7), (7,  10), (10, 13),
        (13, 16), (16, 19), (2, 5), (5,   8), (8,  11), (11, 14),
        (14, 17), (17, 20), (3, 6), (6,   9), (9,  12), (12, 15),
        (15, 18), (18, 21)
    ]

    @test 7*2 + 6*3 == m
    @test length(edges) == m

    γ = [-0.0,  -1.0,  -0.24, -0.04, -0.43, -0.73,
         -0.7,  -0.28, -1.41, -1.43, -0.51, -0.52,
         -1.14, -0.02, -0.11, -0.02, -0.79, -0.33,
         -0.74, -0.65, -1.11, -0.78, -0.51, -0.14,
         -0.44, -0.78, -0.58, -0.80, -1.43, -0.13,
         -1.06, -0.38]

    @testset "compute γ manually" begin
        gam = [-abs(y[u] - y[v]) for (u, v) in edges]
        @test γ ≈ gam
    end

    tree_mask = GraphIdx.kruskal_mst(n, edges, γ)
    tree_cost = γ[tree_mask]
    tree_edges = edges[tree_mask]
    idx = GraphIdx.IncidenceIndex(n, edges)
    local parent::Vector{Int}, selected =
        GraphIdx.prim_mst_edges(n, edges, γ, root)

    @test GraphIdx.Tree.find_root(parent) == root
    @test parent[root] == root
    @test parent ==
        [1, 5, 6, 1, 4, 5, 8, 9, 6, 7, 8, 9, 10, 15, 18, 13, 16, 17, 20, 17, 18]
    @test -sum(tree_cost) ≈ 15.79

    prim_edges = [sort(i, j) for (i, j) in enumerate(parent) if i != j]
    @test length(prim_edges) == n-1
    s_prim, s_kruskal = Set(prim_edges), Set(tree_edges)
    prim_mask = [e ∈ s_prim for e in edges]
    prim_cost = γ[prim_mask]
    @test length(s_prim) == n-1
    @test length(s_kruskal) == n-1
    @test -sum(prim_cost) ≈ 15.79

    sdiff = symdiff(s_prim, s_kruskal)
    sdiff_mask = [e ∈ sdiff for e in edges]
    sdiff_cost = Set(γ[sdiff_mask])
    @test length(sdiff_cost) == 1
    @test sdiff_cost == Set([-0.51])

    @test length(selected) == n - 1
    @test Set(edges[selected]) == s_prim

    @testset "extract non-tree" begin
        z = copy(y)
        lambda = GraphIdx.Ones{Float64}()
        tlam = fill(NaN, length(y))
        alpha = zeros(length(edges))
        MGT.extract_non_tree!(EdgeGraph(n, edges), parent, z, alpha, tlam, lambda)
        @test z ≈ y
        @test all(tlam[1:end .!= root] .≈ 1.0)
    end

    @testset "call gaplas" begin
        local lam = GraphIdx.Ones{Float64}()
        x = MGT.gaplas(y, edges, lam, max_iter=1, verbose=false)
        @test rationalize.(x) == [
            639//800  639//800  547//650  547//650  547//650  547//650  547//650
            639//800  639//800  547//650  547//650  547//650  547//650  547//650
            639//800  639//800  639//800  639//800  547//650  547//650  547//650
        ]
        x = MGT.gaplas(y, edges, lam, max_iter=5, verbose=true)
        @test mean(x) ≈ mean(y)
    end
end

end


abstract StochasticApproximationOptimizer <: Optimizer

SPSADefaultParameters = {
  :Alpha => 0.602,  # The optimal value is 1.0 but values down to 0.602 often can give faster convergence
  :Gamma => 0.101,  # The optimal value is 1/6 but values down to 0.101 often can give faster convergence
  :a     => 0.0017,
  :c     => 1.9, # Recommendation is value 1.9 but that assumes noisy function, otherwise should be low
  :A     => 10
}

type SimultaneousPerturbationSA2 <: StochasticApproximationOptimizer
  search_space::SearchSpace
  parameters::Parameters
  k::Int64
  n::Int64
  theta::Array{Float64, 2}
  delta_ck::Array{Float64, 2}

  SimultaneousPerturbationSA2(parameters) = begin
    ss = parameters[:SearchSpace]
    n = numdims(ss)
    new(ss, Parameters(parameters, SPSADefaultParameters),
      0, n, rand_individual(ss), zeros(Float64, n, 2))
  end
end

name(spsa::SimultaneousPerturbationSA2) = "SPSA2 (Simultaneous Perturbation Stochastic Approximation, 1st order, 2 samples)"

sample_bernoulli_vector(n::Int64) = 2.0 * round(rand(n,1)) - 1.0

function ask(spsa::SimultaneousPerturbationSA2)

  delta = sample_bernoulli_vector(spsa.n)
  ck = spsa.parameters[:c]/(spsa.k + 1)^spsa.parameters[:Gamma]
  spsa.delta_ck = ck * delta

  theta_plus = spsa.theta + spsa.delta_ck
  theta_minus = spsa.theta - spsa.delta_ck

  [(theta_plus, 1), (theta_minus, 2)]

end

function tell!(spsa::SimultaneousPerturbationSA2, rankedCandidates)

  # Use index of rank to get right values for yplus and yminus, respectively.
  if rankedCandidates[1][2] == 1
    yplus = rankedCandidates[1][3]
    yminus = rankedCandidates[2][3]
  else
    yplus = rankedCandidates[2][3]
    yminus = rankedCandidates[1][3]
  end

  # Estimate gradient.
  ghat = (yplus - yminus) ./ (2.0 * spsa.delta_ck)

  # Calc new estimate of theta based on estimate of gradient, ghat.
  ak = spsa.parameters[:a]/(spsa.k + 1 + spsa.parameters[:A])^spsa.parameters[:Alpha]
  theta_new = spsa.theta - ak * ghat
  rand_bound_from_target!(theta_new, spsa.theta, spsa.search_space)
  spsa.theta = theta_new
  spsa.k += 1

  return 0
end


module FLOWMath

include("quadrature.jl")
export trapz

include("roots.jl")
export brent

include("smooth.jl")
export abs_smooth

include("interplate.jl")
export akima_setup
export akima_interp
export akima
export interp2d
export interp3d
export interp4d

end # module


#! /usr/bin/julia

# Rosetta Code, Non-decimal radices/Output

hi = 50
println("Primes \u2264 ", hi, " written in common bases.")
println("     bin   oct  dec  hex")
for i in primes(hi)
    println(@sprintf("%8s %4s   %2s   %2s", bin(i), oct(i), dec(i), hex(i)))
end


"""
    HalfWing(foils :: Vector{Foil},
             chords,
             twists,
             spans,
             dihedrals,
             sweeps,
             position = zeros(3),
             angle    = 0.
             axis     = [0.,1.,0.])

Definition for a `HalfWing` consisting of ``N+1`` `Foil`s, their associated chord lengths ``c`` and twist angles ``ι``, for ``N`` sections with span lengths ``b``, dihedrals ``δ`` and leading-edge sweep angles ``Λ_{LE}``, with all angles in degrees.
"""
struct HalfWing{T <: Real, N <: AbstractAffineMap} <: AbstractWing
    foils      :: Vector{<: AbstractFoil}
    chords     :: Vector{T}
    twists     :: Vector{T}
    spans      :: Vector{T}
    dihedrals  :: Vector{T}
    sweeps     :: Vector{T}
    affine     :: N
end

function HalfWing(foils, chords, twists, spans, dihedrals, sweeps, w_sweep = 0., position = zeros(3), angle = 0., axis = [0.,1.,0.], affine = AffineMap(AngleAxis(deg2rad(angle), axis...), position))
    # Error handling
    check_wing(foils, chords, twists, spans, dihedrals, sweeps)

    # Convert sweep angles to leading-edge
    sweeps = @. sweep_angle(
                    chords[2:end] / chords[1:end-1], # Section tapers
                    aspect_ratio(spans, chords[2:end], chords[1:end-1]), # Section aspect ratios
                    deg2rad(sweeps), # Sweep angles at desired normalized location
                    -w_sweep # Normalized sweep angle location ∈ [0,1]
                )

    # TODO: Perform automatic cosine interpolation of foils with minimum number of points for surface construction?
    # foils = cosine_interpolation.(foils, 60)

    T = promote_type(eltype(chords), eltype(twists), eltype(spans), eltype(dihedrals), eltype(sweeps), typeof(w_sweep))
    N = typeof(affine)

    # Convert angles to radians, adjust twists to leading edge, and generate HalfWing
    HalfWing{T,N}(foils, chords, -deg2rad.(twists), spans, deg2rad.(dihedrals), sweeps, affine)
end

function check_wing(foils, chords, twists, spans, dihedrals, sweeps)
    # Check if number of sections match up with number of edges (NEEDS WORK)
    @assert (length ∘ zip)(foils, chords, twists) == (length ∘ zip)(spans, dihedrals, sweeps) + 1 "N+1 foils, chords and twists are required for N section(s)."
    # Check if lengths are positive
    @assert any(x -> x >= 0., chords) || any(x -> x >= 0., spans) "Chord and span lengths must be positive."
    # Check if dihedrals and sweeps are within bounds
    @assert any(x -> x >= -90. || x <= 90., dihedrals) || any(x -> x >= -90. || x <= 90., sweeps) "Dihedrals and sweep angles must not exceed ±90ᵒ."
end

# Named arguments version for ease, with default NACA-4 0012 airfoil shape
function HalfWing(;
        chords,
        twists    = zero(chords),
        spans     = ones(length(chords) - 1) / (length(chords) - 1),
        dihedrals = zero(spans),
        sweeps    = zero(spans),
        foils     = fill(naca4(0,0,1,2), length(chords)),
        w_sweep   = 0.0,
        position  = zeros(3),
        angle     = 0.,
        axis      = [0., 1., 0.]
    )

    HalfWing(foils, chords, twists, spans, dihedrals, sweeps, w_sweep, position, angle, axis)
end

# Getters
foils(wing     :: HalfWing) = wing.foils
chords(wing    :: HalfWing) = wing.chords
twists(wing    :: HalfWing) = wing.twists
spans(wing     :: HalfWing) = wing.spans
dihedrals(wing :: HalfWing) = wing.dihedrals

"""
    sweeps(wing :: AbstractWing, w = 0.)

Obtain the sweep angles (in radians) at the corresponding normalized chord length ratio ``w ∈ [0,1]``.
"""
sweeps(wing :: HalfWing, w = 0.) =
    @views @. sweep_angle(
                    wing.chords[2:end] / wing.chords[1:end-1], # Section tapers
                    aspect_ratio(wing.spans, wing.chords[2:end], wing.chords[1:end-1]), # Section aspect ratios
                    wing.sweeps, # Section leading-edge sweep angles
                    w # Normalized sweep angle location ∈ [0,1]
                )

aspect_ratio(b, c1, c2) = 2b / (c1 + c2)
sweep_angle(λ, AR, Λ_LE, w) = Λ_LE - atan(2w * (1 - λ), AR * (1 + λ))

# Affine transformation
Base.position(wing :: HalfWing) = wing.affine.transformation
orientation(wing :: HalfWing) = wing.affine.linear
affine_transformation(wing :: HalfWing) = wing.affine

span(wing :: HalfWing) = sum(wing.spans)

taper_ratio(wing :: HalfWing) = last(wing.chords) / first(wing.chords)

function projected_area(wing :: HalfWing)
    mean_chords = forward_sum(wing.chords) / 2 # Mean chord lengths of sections.
    mean_twists = forward_sum(wing.twists) / 2 # Mean twist angles of sections
    sum(@. wing.spans * mean_chords * cos(mean_twists))
end

section_macs(wing :: HalfWing) = @views mean_aerodynamic_chord.(wing.chords[1:end-1], forward_division(wing.chords))

section_projected_areas(wing :: HalfWing) = wing.spans .* forward_sum(wing.chords) / 2

function mean_aerodynamic_chord(wing :: HalfWing)
    areas = section_projected_areas(wing)
    macs  = section_macs(wing)
    sum(macs .* areas) / sum(areas)
end

function mean_aerodynamic_center(wing :: HalfWing, factor = 0.25)
    # Compute mean aerodynamic chords and projected areas
    macs        = section_macs(wing)
    areas       = section_projected_areas(wing)

    # Computing leading edge coordinates
    wing_LE     = leading_edge(wing)
    x_LEs       = getindex.(wing_LE, 1)
    y_LEs       = getindex.(wing_LE, 2)

    # Compute x-y locations of MACs
    x_mac_LEs   = @views @. y_mac(x_LEs[1:end-1], 2 * x_LEs[2:end], wing.chords[2:end] / wing.chords[1:end-1])
    y_macs      = @views @. y_mac(y_LEs[1:end-1], wing.spans, wing.chords[2:end] / wing.chords[1:end-1])

    mac_coords  = @. SVector(x_mac_LEs + factor * macs, y_macs, 0.)

    affine_transformation(wing)(sum(mac_coords .* areas) / sum(areas))
end

camber_thickness(wing :: HalfWing, num :: Integer) = camber_thickness.(wing.foils, num)

maximum_thickness_to_chord(wing :: HalfWing, num :: Integer) = maximum_thickness_to_chord.(camber_thickness(wing, num))

function max_thickness_to_chord_ratio_sweeps(wing :: HalfWing, num)
    # Compute (t/c)_max locations and values
    xs_max_tbyc = maximum_thickness_to_chord(wing, num)
    max_tbyc    = getindex.(xs_max_tbyc, 2)
    xs_temp     = getindex.(xs_max_tbyc, 1)

    # Get leading edge
    le          = leading_edge(wing)

    # Determine x-coordinates in Cartesian frame
    xs          = @. getindex(le, 1) + wing.chords * getindex(xs_temp, 1)

    # Compute sectional x-coordinates
    ds          = forward_difference(xs)

    # Compute leading-edge sweep angles accounting for dihedral
    widths      = @. wing.spans / cos(wing.dihedrals)
    sweeps      = @. atan(ds, widths)

    # Averaging for sections
    xs          = forward_sum(xs_temp) / 2
    tbycs       = forward_sum(max_tbyc) / 2

    xs, tbycs, sweeps
end

"""
    leading_edge(wing :: HalfWing, flip = false)

Compute the leading edge coordinates of a `HalfWing`, with an option to flip the signs of the ``y``-coordinates.
"""
function leading_edge(wing :: HalfWing, flip = false)
    spans, dihedrals, sweeps = wing.spans, wing.dihedrals, wing.sweeps

    sweeped_spans    = [ 0; cumsum(@. spans * tan(sweeps)) ]
    dihedraled_spans = [ 0; cumsum(@. spans * tan(dihedrals)) ]
    cum_spans        = [ 0; cumsum(spans) ]

    leading          = SVector.(sweeped_spans, ifelse(flip, -cum_spans, cum_spans), dihedraled_spans)

    ifelse(flip, leading[end:-1:1], leading)
end

"""
    wing_bounds(wing :: HalfWing, flip = false)

Compute the leading and trailing edge coordinates of a `HalfWing`, with an option to flip the signs of the ``y``-coordinates.
"""
function wing_bounds(wing :: HalfWing, flip = false)
    chords          = wing.chords
    twisted_chords  = @. chords * sin(wing.twists)

    leading         = leading_edge(wing, flip)
    trailing        = SVector.(chords, (zeros ∘ length)(chords), twisted_chords)

    shifted_trailing = ifelse(flip, trailing[end:-1:1], trailing) .+ leading

    leading, shifted_trailing
end

"""
    trailing_edge(wing :: HalfWing, flip = false)

Compute the trailing edge coordinates of a `HalfWing`, with an option to flip the signs of the ``y``-coordinates.
"""
trailing_edge(wing :: HalfWing, flip = false) = wing_bounds(wing, flip)[2]


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], [1 0.3], [0, 0], (0, 10))
sol = solve(prob, 0.02, ClosedFormHankelM())
plot(sol)


num = 600851475143
sqr = sqrt(num)
c = 3
max = num
while c <= sqr
    while num % c == 0
        num /= c
        max = c
    end
    c += 2
end
println(max)

003 - Largest Prime Factor/003 - Largest_Prime_Factor.jl


using LinearAlgebra, Statistics, Roots, Optim, Plots, Random
Random.seed!(1234)
n = 1000 # number of observations to draw

# the true regression function
r(x) = 2 + 1/50*x*(30-x)
# the true density function
σy = 3/2
f(x,y) = 3/4000 * 1/√(2π*σy^2) * x*(20-x)*exp(-1/(2σy^2)*(y-r(x))^2)

# x values and y values for a grid
xs = 0:1/2^3:20
ys = 0:1/2^3:10

# F(t) is the CDF of X
F(t) = -t^3/4000 + 3t^2/400

"Sample from the distribution of X (inverse CDF trick)"
function sampleX()
    U = rand()
    find_zero(t->F(t)-U,(0,20),Bisection())
end

"Sample from joint distribution of X and Y"
function sampleXY(r,σ)
    X = sampleX()
    Y = r(X) + σ*randn()
    (X,Y)
end

# Sample n observations
observations = [sampleXY(r,σy) for i=1:n]


module RelPerm

#TODO add capillary pressure here -- see equations 8.11 and 8.12 in "introduction to reservoir simulation using matlab/gnu octave" for the brooks-corey and van genuchten forms of capillary pressure

module Corey
	function S_wn(S_w, S_wir, S_nir)
		return (S_w - S_wir) / (1 - S_wir - S_nir)
	end
	function kr_w(S_w, S_wir, S_nir, N_w, k0_w)
		if S_w < S_wir
			return 0.0
		elseif S_w > 1 - S_nir
			return k0_w
		else
			return k0_w * (S_wn(S_w, S_wir, S_nir)) ^ N_w
		end
		#return k0_w * (S_wn(S_w, S_wir, S_nir)) ^ N_w
	end
	function kr_n(S_n, S_wir, S_nir, N_n, k0_n)
		if S_n < S_nir
			return 0.0
		elseif S_n > 1 - S_wir
			return k0_n
		else
			return k0_n * (1 - S_wn(1 - S_n, S_wir, S_nir)) ^ N_n
		end
		#return k0_n * (1 - S_wn(1 - S_n, S_wir, S_nir)) ^ N_n
	end
end

end


using SIAN

println("Setting up the problem")

ode = @ODEmodel(
  x1'(t) = -x1(t) * x5(t) / (k2 + x1(t)),
  x2'(t) = 2 * x1(t) * x5(t) / ((k2 + x1(t)) * 3) - k4 * x2(t),
  x3'(t) = k4 * (x2(t)) / 2 - k4 * x3(t),
  x4'(t) = x1(t) * x5(t) / (3 * (k2 + x1(t))) + k4 * (x2(t)) / 2 + k4 * x3(t),
  x5'(t) = -k3 * x5(t),
  y1(t) = x1(t),
  y2(t) = x2(t) + x3(t),
  y3(t) = x4(t)
)

res = identifiability_ode(ode, get_parameters(ode); p=0.99, p_mod=0, nthrds=1)

println(res)


#Density of MtFA
function logfY_MtFA(model, Mquantities, Squantities, index)
    Mquantities.sigma = real(model.B[:,:,index]*model.B[:,:,index]') + model.D[:,:,index]
    Mquantities.Fji[:,index] = logpdf(MvTDist(model.Ψ[1,index],Vector(model.mu[:,index]), Matrix(Mquantities.sigma)),Squantities.Y')
    #return logpdf(MvTDist(model.Ψ[1,index],Vector(model.mu[:,index]), Matrix(Mquantities.sigma)),Squantities.Y')
  #return(mvtnorm::dmvt(data, delta = mu, sigma = Re(B%*%t(B)+D), df = w_params))
end

#Density of MSLFA
function logfY_MSLFA(model, Mquantities, Squantities, index)
    #n = size(data)[1]
    #Mquantities.mq.int_container = zeros(Squantities.n,1)
    Mquantities.sigma = real(model.B[:,:,index]*model.B[:,:,index]') + model.D[:,:,index]
    for j in 1:Squantities.n
        Mquantities.mq.integrand = function integrand(w)
            w^(model.Ψ[index] - 1) * pdf(MvNormal(Vector(model.mu[:,index]), Matrix(Mquantities.sigma)/w ), Squantities.Y[j,:] )
        end
        Mquantities.mq.int_res = log(model.Ψ[index]*quadgk( Mquantities.mq.integrand,0,1,rtol=1e-10)[1])
        if Mquantities.mq.int_res == -Inf
            Mquantities.Fji[j,index] = -500
        else
            Mquantities.Fji[j,index] = Mquantities.mq.int_res
        end
    end
end

#Density of MCNFA
function logfY_MCNFA(model, Mquantities, Squantities, index)
    Mquantities.sigma = real(model.B[:,:,index]*model.B[:,:,index]') + model.D[:,:,index]
    Mquantities.Fji[:,index] =  log.(
      model.Ψ[1,index] .* pdf(MvNormal(Vector(model.mu[:,index]), Matrix(Mquantities.sigma)/model.Ψ[2,index] ), Squantities.Y')  .+
      (1-model.Ψ[1,index]).*pdf(MvNormal(Vector(model.mu[:,index]), Matrix(Mquantities.sigma) ), Squantities.Y' )
      )
end

function logd_GH(data,chi,psi,lambda,B, mu, D, beta)
#  #Density of Generalised Hyperbolic distribution.
    n,p = size(data)
    sig_i = real(B*B')+D
    delta = Distances.colwise(SqMahalanobis(inv(sig_i)), Array(data'), mu)
    #delta = mahalanobis(data, mu, inv(sig_i), inverted = FALSE)
    delta_b = beta'*inv(sig_i)*beta
    if chi != 0
        return 0.5*(lambda - 0.5*p) .* (log.(chi .+ delta) .- log(psi + delta_b)) .+
           0.5*lambda*(log(psi)-log(chi)) .+ log.( besselk.(lambda-0.5*p , sqrt.( (psi+delta_b) .* (chi .+ delta) ))) .-
           0.5*p*log(2*pi) .- 0.5 * log(det(sig_i)) .- log(besselk(lambda, sqrt(psi*chi)))  .+
           (data - (reshape(repeat(mu,inner=1,outer=n), p, n) |> permutedims) )*inv(sig_i)*beta
    else
    #Check this to make sure I've calculated the limiting case density correctly...!
        return 0.5*(lambda - 0.5*p) .* (log.(chi .+ delta) .- log.(psi .+ delta_b)) .+
        lambda*(log(psi)) .+ log.(besselk.(lambda-0.5*p, sqrt.((psi.+delta_b) .* (chi .+ delta) ))) .-
        0.5*p*log(2*pi) .- 0.5 * log(det(sig_i)) .- (lambda-1)*log(2) .- lgamma(lambda) .+
        (data - (reshape(repeat(mu,inner=1,outer=n), p, n) |> permutedims) )*inv(sig_i)*beta
        #if(any(is.nan(temp))){browser()}
        # temp
    end
end

#Density of GHFA
function logfY_MGHFA(model, Mquantities, Squantities, index)
    Mquantities.Fji[:,index] = logd_GH(Squantities.Y,model.Ψ[1,index],model.Ψ[1,index],model.Ψ[2,index],model.B[:,:,index], model.mu[:,index], model.D[:,:,index], model.beta[:,index])
end

function logfY_MBSFA(model, Mquantities, Squantities, index)
    Mquantities.mq.a_is = 1/(model.Ψ[index]^2)
    Mquantities.Fji[:,index] =log.(0.5*exp.(logd_GH(Squantities.Y,Mquantities.mq.a_is,Mquantities.mq.a_is,0.5,model.B[:,:,index], model.mu[:,index], model.D[:,:,index], model.beta[:,index])) .+
               0.5*exp.(logd_GH(Squantities.Y,Mquantities.mq.a_is,Mquantities.mq.a_is,-0.5,model.B[:,:,index], model.mu[:,index], model.D[:,:,index], model.beta[:,index])))
end

function logfY_MLFA(model, Mquantities, Squantities, index)
    Mquantities.mq.a = model.Ψ[index]
    Mquantities.Fji[:,index] = log.(Mquantities.mq.a/(1+Mquantities.mq.a)*exp.(logd_GH(Squantities.Y,0,2*Mquantities.mq.a,1,model.B[:,:,index], model.mu[:,index], model.D[:,:,index], model.beta[:,index])) .+
               1/(1+Mquantities.mq.a)*exp.(logd_GH(Squantities.Y,0,2*Mquantities.mq.a,2,model.B[:,:,index], model.mu[:,index], model.D[:,:,index], model.beta[:,index])))
end


module Interpolation

    using DocStringExtensions
    using StaticArrays
    using LinearAlgebra

    using WaveProp.Utils
    using WaveProp.Geometry

    export
        barycentric_lagrange_matrix,
        barycentric_lagrange_weights,
        MonomialBasis,
        vandermond,
        lagrange_basis,
        PolynomialBasis,
        LagrangeBasis, gradLagrangeBasis,
        trace, grad

    include("lagrange.jl")
    include("monomial.jl")
    include("polynomialbasis.jl")

end


# Power flow module. The implementation is a modification of
# MATPOWER's code. We attach the original MATPOWER's license in makeYbus.m:
#
# MATPOWER
# Copyright (c) 1996-2016, Power Systems Engineering Research Center (PSERC)
# by Ray Zimmerman, PSERC Cornell
#
# Covered by the 3-clause BSD License.
#
# Expression of Jacobians and Hessians from MATPOWER

#
function matpower_residual_jacobian(V, Ybus)
    n = size(V, 1)
    Ibus = Ybus*V
    diagV       = sparse(1:n, 1:n, V, n, n)
    diagIbus    = sparse(1:n, 1:n, Ibus, n, n)
    diagVnorm   = sparse(1:n, 1:n, V./abs.(V), n, n)

    dSbus_dVm = diagV * conj(Ybus * diagVnorm) + conj(diagIbus) * diagVnorm
    dSbus_dVa = 1im * diagV * conj(diagIbus - Ybus * diagV)
    return (dSbus_dVm, dSbus_dVa)
end

function _matpower_lineflow_jacobian(V, branches::Branches)
    nb = size(V, 1)
    f = branches.from_buses
    t = branches.to_buses
    nl = length(f)

    diagV     = sparse(1:nb, 1:nb, V, nb, nb)
    diagVnorm = sparse(1:nb, 1:nb, V./abs.(V), nb, nb)

    # Connection matrices
    Cf = sparse(1:nl, f, ones(nl), nl, nb)
    Ct = sparse(1:nl, t, ones(nl), nl, nb)

    i = [1:nl; 1:nl]
    Yf = sparse(i, [f; t], [branches.Yff; branches.Yft], nl, nb)
    Yt = sparse(i, [f; t], [branches.Ytf; branches.Ytt], nl, nb)

    If = Yf * V
    It = Yt * V

    diagCfV = sparse(1:nl, 1:nl, Cf * V, nl, nl)
    diagCtV = sparse(1:nl, 1:nl, Ct * V, nl, nl)

    Sf = diagCfV * conj(If)
    St = diagCtV * conj(It)

    diagIf = sparse(1:nl, 1:nl, If, nl, nl)
    diagIt = sparse(1:nl, 1:nl, It, nl, nl)

    dSf_dVm = conj(diagIf) * Cf * diagVnorm + diagCfV * conj(Yf * diagVnorm)
    dSf_dVa = 1im * (conj(diagIf) * Cf * diagV - diagCfV * conj(Yf * diagV))

    dSt_dVm = conj(diagIt) * Ct * diagVnorm + diagCtV * conj(Yt * diagVnorm)
    dSt_dVa = 1im * (conj(diagIt) * Ct * diagV - diagCtV * conj(Yt * diagV))

    return (Sf, St, dSf_dVm, dSf_dVa, dSt_dVm, dSt_dVa)
end

function matpower_lineflow_power_jacobian(V, branches::Branches)
    nl = length(branches.from_buses)
    (Sf, St, dSf_dVm, dSf_dVa, dSt_dVm, dSt_dVa) = _matpower_lineflow_jacobian(V, branches)

    dSf = sparse(1:nl, 1:nl, Sf, nl, nl)
    dSt = sparse(1:nl, 1:nl, St, nl, nl)

    dHf_dVm = 2 * (real(dSf) * real(dSf_dVm) + imag(dSf) * imag(dSf_dVm))
    dHf_dVa = 2 * (real(dSf) * real(dSf_dVa) + imag(dSf) * imag(dSf_dVa))

    dHt_dVm = 2 * (real(dSt) * real(dSt_dVm) + imag(dSt) * imag(dSt_dVm))
    dHt_dVa = 2 * (real(dSt) * real(dSt_dVa) + imag(dSt) * imag(dSt_dVa))

    dH_dVm = [dHf_dVm; dHt_dVm]
    dH_dVa = [dHf_dVa; dHt_dVa]
    return dH_dVm, dH_dVa
end

# Hessian vector-product H*λ, H = Jₓₓ (from Matpower)
# Suppose ordering is correct
function _matpower_hessian(V, Ybus, λ)
    n = size(V, 1)
    # build up auxiliary matrices
    Ibus = Ybus * V
    diagV       = sparse(1:n, 1:n, V, n, n)
    diagIbus    = sparse(1:n, 1:n, Ibus, n, n)
    d_invV      = sparse(1:n, 1:n, 1 ./ abs.(V), n, n)
    d_lambda    = sparse(1:n, 1:n, λ, n, n)

    A = sparse(1:n, 1:n, λ .* V, n, n)
    B = Ybus * diagV
    C = A * conj(B)
    D = Ybus' * diagV
    Dl = sparse(1:n, 1:n, D * λ, n, n)
    E = conj(diagV)  * (D * d_lambda - Dl)
    F = C - A * sparse(1:n, 1:n, conj(Ibus), n, n)
    G = d_invV

    # θθ
    G11 = E + F
    # vθ
    G21 = 1im .* G * (E - F)
    # vv
    G22 = G * (C + transpose(C)) * G
    return (G11, transpose(G21), G22)
end

function residual_hessian(V, Ybus, λ, pv, pq, ref)
    # decompose vector
    n = length(V)
    λp = zeros(n) ; λq = zeros(n)
    npv = length(pv)
    npq = length(pq)
    nref = length(ref)
    λp[pv] = λ[1:npv]
    λp[pq] = λ[npv+1:npv+npq]
    λq[pq] = λ[npv+npq+1:end]

    Gp11, Gp12, Gp22 = _matpower_hessian(V, Ybus, λp)
    Pθθ = real.(Gp11)
    Pvθ = real.(Gp12)
    Pvv = real.(Gp22)

    Gq11, Gq12, Gq22 = _matpower_hessian(V, Ybus, λq)
    Qθθ = imag.(Gq11)
    Qvθ = imag.(Gq12)
    Qvv = imag.(Gq22)

    # w.r.t. xx
    H11 = Pθθ[pv, pv] + Qθθ[pv, pv]
    H12 = Pθθ[pv, pq] + Qθθ[pv, pq]
    H13 = Pvθ[pv, pq] + Qvθ[pv, pq]
    H22 = Pθθ[pq, pq] + Qθθ[pq, pq]
    H23 = Pvθ[pq, pq] + Qvθ[pq, pq]
    H33 = Pvv[pq, pq] + Qvv[pq, pq]

    Hxx = [
        H11  H12  H13
        H12' H22  H23
        H13' H23' H33
    ]::SparseMatrixCSC{Float64, Int}

    # w.r.t. uu
    H11 = Pvv[ref, ref] + Qvv[ref, ref]
    H12 = Pvv[ref,  pv] + Qvv[ref,  pv]
    H22 = Pvv[pv,   pv] + Qvv[pv,   pv]

    Huu = [
         H11  H12 spzeros(nref, npv)
         H12' H22 spzeros(npv, npv)
         spzeros(npv, nref + 2 * npv)
    ]::SparseMatrixCSC{Float64, Int}

    # w.r.t. xu
    Pvθ = real.(transpose(Gp12))
    Qvθ = imag.(transpose(Gq12))
    H11 = Pvθ[ref, pv] + Qvθ[ref, pv]
    H12 = Pvθ[ref, pq] + Qvθ[ref, pq]
    H13 = Pvv[ref, pq] + Qvv[ref, pq]
    H21 = Pvθ[pv,  pv] + Qvθ[pv,  pv]
    H22 = Pvθ[pv,  pq] + Qvθ[pv,  pq]
    H23 = Pvv[pv,  pq] + Qvv[pv,  pq]

    Hxu = [
        H11  H12  H13
        H21  H22  H23
        spzeros(npv, npv + 2 * npq)
    ]::SparseMatrixCSC{Float64, Int}

    return (Hxx, Hxu, Huu)
end

# ∂²pg_ref / ∂²x
function active_power_hessian(V, Ybus, pv, pq, ref)
    # decompose vector
    n = length(V)
    npv = length(pv)
    npq = length(pq)
    nref = length(ref)

    λp = zeros(n)
    # Pick only components wrt ref nodes
    λp[ref] .= 1.0

    G11, G12, G22 = _matpower_hessian(V, Ybus, λp)
    Pθθ = real.(G11)
    Pvθ = real.(G12)
    Pvv = real.(G22)

    H11 = Pθθ[pv, pv]
    H12 = Pθθ[pv, pq]
    H13 = Pvθ[pv, pq]
    H22 = Pθθ[pq, pq]
    H23 = Pvθ[pq, pq]
    H33 = Pvv[pq, pq]

    # w.r.t. xx
    Hxx = [
        H11  H12  H13
        H12' H22  H23
        H13' H23' H33
    ]::SparseMatrixCSC{Float64, Int}

    # w.r.t. uu
    H11 = Pvv[ref, ref]
    H12 = Pvv[ref, pv]
    H22 = Pvv[pv, pv]

    Huu = [
         H11  H12 spzeros(nref, npv)
         H12' H22 spzeros(npv, npv)
         spzeros(npv, nref + 2 * npv)
    ]::SparseMatrixCSC{Float64, Int}

    # w.r.t. xu
    Pvθ = real.(transpose(G12))
    Qvθ = imag.(transpose(G12))
    H11 = Pvθ[ref, pv]
    H12 = Pvθ[ref, pq]
    H13 = Pvv[ref, pq]
    H21 = Pvθ[pv, pv]
    H22 = Pvθ[pv, pq]
    H23 = Pvv[pv, pq]

    Hxu = [
        H11  H12  H13
        H21  H22  H23
        spzeros(npv, npv + 2 * npq)
    ]::SparseMatrixCSC{Float64, Int}

    return (Hxx, Hxu, Huu)
end

# ∂²qg / ∂²x * λ
function reactive_power_hessian(V, Ybus, λ, pv, pq, ref)
    n = length(V)
    npv = length(pv)
    npq = length(pq)
    nref = length(ref)

    G11, G12, G22 = _matpower_hessian(V, Ybus, λ)
    Qθθ = imag.(G11)
    Qvθ = imag.(G12)
    Qvv = imag.(G22)

    H11 = Qθθ[pv, pv]
    H12 = Qθθ[pv, pq]
    H13 = Qvθ[pv, pq]
    H22 = Qθθ[pq, pq]
    H23 = Qvθ[pq, pq]
    H33 = Qvv[pq, pq]

    Hxx = [
        H11  H12  H13
        H12' H22  H23
        H13' H23' H33
    ]::SparseMatrixCSC{Float64, Int}

    H11 = Qvv[ref, ref]
    H12 = Qvv[ref, pv]
    H22 = Qvv[pv, pv]

    Huu = [
         H11  H12 spzeros(nref, npv)
         H12' H22 spzeros(npv, npv)
         spzeros(npv, nref + 2* npv)
    ]::SparseMatrixCSC{Float64, Int}

    Pvθ = real.(transpose(G12))
    Qvθ = imag.(transpose(G12))
    H11 = Qvθ[ref, pv]
    H12 = Qvθ[ref, pq]
    H13 = Qvv[ref, pq]
    H21 = Qvθ[pv, pv]
    H22 = Qvθ[pv, pq]
    H23 = Qvv[pv, pq]

    Hxu = [
        H11  H12  H13
        H21  H22  H23
        spzeros(npv, 2*npq+npv)
    ]::SparseMatrixCSC{Float64, Int}
    return (Hxx, Hxu, Huu)
end


#Matrices related to stochastic processes

import Base: start, next, done
export AiryProcess, BrownianProcess, WhiteNoiseProcess, next!

abstract StochasticProcess{T<:Real}

immutable WhiteNoiseProcess{T<:Real} <: StochasticProcess{T}
    dt::T
end

immutable BrownianProcess{T<:Real} <: StochasticProcess{T}
    dt::T
end

immutable AiryProcess{S<:Real, T<:Real} <: StochasticProcess{T}
    dt::T
    beta::S
end

done{T}(p::StochasticProcess{T}, x...) = false #Processes can always go on forever

#######################
# White noise process #
#######################

start{T}(p::WhiteNoiseProcess{T}) = nothing
next{T}(p::WhiteNoiseProcess{T}, x::Void=nothing) = (randn()*sqrt(p.dt), nothing)

####################
# Brownian process #
####################

start{T}(p::BrownianProcess{T}) = zero(T)
function next{T}(p::BrownianProcess{T}, x::T)
    newx = x + randn()*sqrt(p.dt)
    newx, newx
end

################
# Airy process #
################
start{T}(p::AiryProcess{T}) = SymTridiagonal(T[-(2/p.dt^2)], T[])

"""
Like next, but update only the state of the AiryProcess

Skip the eigenvalue computation, which gets expensive
"""
function next!{T}(p::AiryProcess{T}, S::SymTridiagonal{T})
    t = (size(S, 1)-1)*p.dt

    #Discretized Airy operator plus diagonal noise
    x = inv(p.dt^2)
    push!(S.dv, -2x - t + 2/sqrt(p.dt*p.beta)*randn())
    push!(S.ev, x)

    S
end
function next{T}(p::AiryProcess{T}, S::SymTridiagonal{T})
    t = (size(S, 1)-1)*p.dt

    #Discretized Airy operator plus diagonal noise
    x = inv(p.dt^2)
    push!(S.dv, -2x - t + 2/sqrt(p.dt*p.beta)*randn())
    push!(S.ev, x)

    (eigmax(S), S)
end


#=
   Basic model as an template for other models...
=#

using ConstraintSolver, JuMP
using Cbc, GLPK, Ipopt
const CS = ConstraintSolver
include("constraints_utils.jl")

function alphametic(problem_str="SEND+MORE=MONEY", base=10, print_solutions=true,all_solutions=false,timeout=6)

    cbc_optimizer = optimizer_with_attributes(Cbc.Optimizer, "logLevel" => 0)
    glpk_optimizer = optimizer_with_attributes(GLPK.Optimizer)
    ipopt_optimizer = optimizer_with_attributes(Ipopt.Optimizer)

    model = Model(optimizer_with_attributes(CS.Optimizer,   "all_solutions"=> all_solutions,
                                                            # "all_optimal_solutions"=>all_solutions,
                                                            "logging"=>[], # :true,
                                                            # "logging"=> :true,

                                                            "traverse_strategy"=>:BFS,
                                                            # "traverse_strategy"=>:DFS,
                                                            # "traverse_strategy"=>:DBFS,

                                                            # "branch_split"=>:Smallest,
                                                            # "branch_split"=>:Biggest,
                                                            "branch_split"=>:InHalf,

                                                            # https://wikunia.github.io/ConstraintSolver.jl/stable/options/#branch_strategy-(:Auto)
                                                            "branch_strategy" => :IMPS, # default
                                                            # "branch_strategy" => :ABS, # Activity Based Search
                                                            # "activity.decay" => 0.999, # default 0.999
                                                            # "activity.max_probes" => 10, # default, 10
                                                            # "activity.max_confidence_deviation" => 20, # default 20

                                                            # "simplify"=>false,
                                                            # "simplify"=>true, # default

                                                            "time_limit"=>timeout,

                                                            # "backtrack" => false, # default true
                                                            # "backtrack_sorting" => false, # default true

                                                            # "lp_optimizer" => cbc_optimizer,
                                                            # "lp_optimizer" => glpk_optimizer,
                                                            # "lp_optimizer" => ipopt_optimizer,
                                        ))

    problem = split(problem_str,r"[+=-]").|>string
    println("problem: $problem base:$base")
    p_len = length(problem)

    # create the lookup table of (digit -> ix)
    a = join(problem).|>unique
    n = length(a)
    lookup = Dict(zip(a,1:n))
    # println("lookup:",lookup)

    # length of each number
    lens = problem.|>length

    @variable(model, 0 <= x[1:n] <= base-1, Int)
    sums = [@variable(model, integer=true,lower_bound=base^(lens[i]-1),upper_bound=(base^lens[i])-1) for i in 1:length(lens)]
    # @variable(model, 1 <= sums[1:length(lens)] <= base^(maximum(lens))-1, Int)

    @constraint(model, x in CS.AllDifferent())

    ix = 1
    for prob in problem

      # sum all the digits with proper exponents to a number
      this_len = length(prob)
      ss = [(base^i) * x[lookup[prob[this_len - i]]] for i in (this_len-1):-1:0]
      @constraint(model,sums[ix] == sum(ss))

      # leading digits must be > 0
      @constraint(model,x[lookup[prob[1]]] > 0)

      ix += 1

    end

    # the last number (RHS) is the sum of the previous numbers (LHS)
    @constraint(model, sum([sums[i] for i in 1:p_len-1]) == sums[end])

    # Solve the problem
    optimize!(model)

    status = JuMP.termination_status(model)
    # println("status:$status")
    num_sols = 0
    if status == MOI.OPTIMAL
        num_sols = MOI.get(model, MOI.ResultCount())
        println("num_sols:$num_sols\n")
        if print_solutions
            for sol in 1:num_sols
                println("solution #$sol")
                x_val = convert.(Integer,JuMP.value.(x; result=sol))
                println("x:$x_val")
                for c in a
                  println("$c: $(x_val[lookup[c]])")
                end
            end
        end
        println()
    else
        println("status:$status")
    end

    return status, num_sols
end

problems = [
  "SEND+MORE=MONEY",
  "SEND+MOST=MONEY", "VINGT+CINQ+CINQ=TRENTE",
  "EIN+EIN+EIN+EIN=VIER",
  "DONALD+GERALD=ROBERT",
  "SATURN+URANUS+NEPTUNE+PLUTO+PLANETS",  # this is hard
  "WRONG+WRONG=RIGHT"
]

function run_problems(base=10)
  for problem in problems
    # @time alphametic(problem,10,true,true,6)
    @time alphametic(problem,base,true,false,116)
    println()
  end

end

#=
if length(ARGS) > 0
  problem = ARGS[1]
  base = 10
  if length(ARGS) > 1
    base = parse(Int,ARGS[2])
  end
  if problem == "TEST" || problem == "test"
    run_problems(base)
  else
    alphametic(problem)
  end
else
  run_problems()
end
=#

run_problems()


struct TwoDLinearSolverSplineMass
    nx1::Int
    nx2::Int
    factor::Float64
    eigvals1::Vector{Float64}
    eigvals2::Vector{Float64}
    wk::Array{ComplexF64,2}

    function TwoDLinearSolverSplineMass(nx1, nx2, eigvals1, eigvals2)
        factor = 1.0
        wk = zeros(ComplexF64, nx1, nx2)
        return new(nx1, nx2, factor, eigvals1, eigvals2, wk)
    end
end

function solve(solver::TwoDLinearSolverSplineMass, rhs::Vector{Float64})::Vector{Float64}
    nx1, nx2 = solver.nx1, solver.nx2

    solver.wk .= reshape(rhs, nx1, nx2)

    fft!(solver.wk)

    for j in 1:nx2, i in 1:nx1
        @inbounds solver.wk[i, j] /= (
            solver.eigvals1[i] * solver.eigvals2[j] * solver.factor
        )
    end

    ifft!(solver.wk)

    return vec(real(solver.wk))
end


###############
# H-OBCA: Hierarchical Optimization-based Collision Avoidance - a path planner for autonomous parking
# Copyright (C) 2018
# Alexander LINIGER [liniger@control.ee.ethz.ch; Automatic Control Lab, ETH Zurich]
# Xiaojing ZHANG [xiaojing.zhang@berkeley.edu; MPC Lab, UC Berkeley]
# Atsushi SAKAI [atsushisakai@global.komatsu; Komatsu Ltd / MPC Lab]
#
# This program is free software: you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with this program.  If not, see <http://www.gnu.org/licenses/>.
###############
# The paper describing the theory can be found here:
# 	X. Zhang, A. Liniger and F. Borrelli; "Optimization-Based Collision Avoidance"; Technical Report, 2017, [https://arxiv.org/abs/1711.03449]
#   X. Zhang, A. Liniger, A. Sakai and F. Borrelli; "Autonomous  Parking  using  Optimization-Based  Collision  Avoidance"; Technical Report, 2018 [add URL]
###############

###############
# Function computes H-representation for obstacles given their vertices
# it is assumed that the vertices are given in CLOCK-WISE, and that the first vertex is repeated at the end of the vertex list
###############

function obstHrep(nOb, vOb, lOb)

	# do simple checks
	if nOb != length(lOb)
		println("ERROR in number of obstacles")
	end

	# these matrices contain the H-rep
	A_all = zeros(sum(vOb)-nOb,2)
	b_all = zeros(sum(vOb)-nOb,1)

	# counter for lazy people
	lazyCounter = 1;

	for i = 1 : nOb	# building H-rep

		A_i = zeros(vOb[i]-1,2)
		b_i = zeros(vOb[i]-1,1)

		# take two subsequent vertices, and compute hyperplane
		for j = 1 : vOb[i]-1

			# extract two vertices
			v1 = lOb[i][j]		# vertex 1
			v2 = lOb[i][j+1]	# vertex 2

			# find hyperplane passing through v1 and v2
			if v1[1] == v2[1]	# perpendicular hyperplane, not captured by general formula
				if v2[2] < v1[2]   # line goes "down"
					A_tmp = [1 0]
					b_tmp = v1[1]
				else
					A_tmp = [-1 0]
					b_tmp = -v1[1]
				end
			elseif v1[2] == v2[2] # horizontal hyperplane, captured by general formula but included for numerical stability
				if v1[1] < v2[1]
					A_tmp = [0 1]
					b_tmp = v1[2]
				else
					A_tmp = [0 -1]
					b_tmp = -v1[2]
				end
			else   # general formula for non-horizontal and non-vertical hyperplanes
				ab = [v1[1] 1 ; v2[1] 1] \ [v1[2] ; v2[2]]
				a = ab[1]
				b = ab[2]

				if v1[1] < v2[1]  # v1 --> v2 (line moves right)
					A_tmp = [-a 1]
					b_tmp = b
				else  # v2 <-- v1 (line moves left)
					A_tmp = [a -1]
					b_tmp = -b

				end
			end
			# store vertices
			A_i[j,:] = A_tmp
			b_i[j] = b_tmp
		end

		# store everything
		A_all[lazyCounter : lazyCounter+vOb[i]-2,:] = A_i
		b_all[lazyCounter : lazyCounter+vOb[i]-2] = b_i

		# update counter
		lazyCounter = lazyCounter + vOb[i]-1
	end

	return A_all, b_all

end


# Copyright (c) 2015 Aki Vehtari, Tuomas Sivula
# Original Matlab version by Aki Vehtari. Translation to Python
# by Tuomas Sivula.

# This software is distributed under the GNU General Public
# License (version 3 or later); please refer to the file
# License.txt, included with the software, for details.

"""
    psisloo(log_lik, [wcpp, wtrunc])

PSIS leave-one-out log predictive densities.

Computes the log predictive densities given posterior samples of the log
likelihood terms p(y_i|\theta^s) in input parameter `log_lik`. Returns a
sum of the leave-one-out log predictive densities `loo`, individual
leave-one-out log predictive density terms `loos` and an estimate of Pareto
tail indeces `ks`. If tail index k>0.5, variance of the raw estimate does
not exist and if tail index k>1 the mean of the raw estimate does not exist
and the PSIS estimate is likely to have large variation and some bias.

# Arguments
* `log_lik::AbstractArray`: Array of size n x m containing n posterior samples of the log likelihood terms p(y_i|\theta^s).
* `wcpp::Real`: Percentage of samples used for GPD fit estimate (default is 20).
* `wtrunc::Float64`: Positive parameter for truncating very large weights to n^wtrunc. Providing False or 0 disables truncation. Default values is 3/4.

# Returns
* `loo::Real`: sum of the leave-one-out log predictive densities.
* `loos::AbstractArray`: Individual leave-one-out log predictive density terms.* `ks::AbstractArray`: Estimated Pareto tail indeces.
"""
# Compute LOO and standard error
function psisloo(log_lik::AbstractArray, wcpp::Int64=20, wtrunc::Float64=3/4)

    # log raw weights from log_lik
    lw = copy(log_lik)

    # compute Pareto smoothed log weights given raw log weights
    lwp, ks = psislw(-lw, wcpp, wtrunc)

    lwp += lw
    loos = reshape(logsumexp(lwp; dims=1), size(lwp, 2))
    loo = sum(loos)

    return loo, loos, ks
end

export
    psisloo


st=Structure()
lcset=LoadCaseSet()
add_uniaxial_metal!(st,"steel",2e11,0.2,7849.0474)
add_general_section!(st,"frame",4.26e-3,3.301e-6,6.572e-5,9.651e-8,1e-3,1e-3,0,0)

N=600
for i in 0:N
    add_node!(st,i,3i/100,2i/100,2i/100)
end

for i in 0:N-1
    add_beam!(st,i,i,i+1,"steel","frame")
end

set_nodal_restraint!(st,0,true,true,true,true,true,true)

add_static_case!(lcset,"DL",1.1)
@time begin
assembly=assemble!(st,lcset,path=PATH)
end

@time begin
solve(assembly)
end

r=result_nodal_reaction(assembly,"DL",0)
@test r≈[-2.83076e-10,-9.04954e-10,8926.15,53556.9,-80335.3,-3.29473e-8] rtol=1e-2
r=result_nodal_displacement(assembly,"DL",N)
@test r≈[0.454025, 0.302683, -0.98385, -0.0336002, 0.0504003, -5.77725e-13] rtol=1e-2


using Qutilities
using Test

using LinearAlgebra: diagm, I, tr

@testset "Qutilities" begin

@testset "sigma_x, sigma_y, sigma_z" begin
    @test sigma_x^2 == I
    @test sigma_y^2 == I
    @test sigma_z^2 == I
    @test -im*sigma_x*sigma_y*sigma_z == I
end

@testset "ptrace, ptranspose" begin
    let A = I(2),
        B = reshape(1:16, 4, 4),
        C = [[0, 0, 1] [0, 1, 0] [1, 0, 0]],
        ABC = kron(A, B, C),
        dims = (2, 4, 3)

        @test ptrace(ABC, dims, 1) == tr(A) * kron(B, C)
        @test ptranspose(ABC, dims, 1) == kron(A', B, C)
        @test ptrace(ABC, dims, 2) == tr(B) * kron(A, C)
        @test ptranspose(ABC, dims, 2) == kron(A, B', C)
        @test ptrace(ABC, dims, 3) == tr(C) * kron(A, B)
        @test ptranspose(ABC, dims, 3) == kron(A, B, C')
    end

    let M = reshape(1.0:16.0, 4, 4),
        MT1 = [[1.0, 2.0, 9.0, 10.0] [5.0, 6.0, 13.0, 14.0] [3.0, 4.0, 11.0, 12.0] [7.0, 8.0, 15.0, 16.0]],
        MT2 = [[1.0, 5.0, 3.0, 7.0] [2.0, 6.0, 4.0, 8.0] [9.0, 13.0, 11.0, 15.0] [10.0, 14.0, 12.0, 16.0]]

        @test ptrace(M, (1, 4), 1) == M
        @test ptranspose(M, (1, 4), 1) == M
        @test ptrace(M, (1, 4), 2) == fill(tr(M), 1, 1)
        @test ptranspose(M, (1, 4), 2) == transpose(M)
        @test ptrace(M, (2, 2), 1) == [[12.0, 14.0] [20.0, 22.0]]
        @test ptranspose(M, (2, 2), 1) == MT1
        @test ptrace(M, (2, 2), 2) == [[7.0, 11.0] [23.0, 27.0]]
        @test ptranspose(M, (2, 2), 2) == MT2
        @test ptrace(M, (4, 1), 1) == fill(tr(M), 1, 1)
        @test ptranspose(M, (4, 1), 1) == transpose(M)
        @test ptrace(M, (4, 1), 2) == M
        @test ptranspose(M, (4, 1), 2) == M

        @test ptrace(M, 1) == ptrace(M, (2, 2), 1)
        @test ptrace(M, 2) == ptrace(M, (2, 2), 2)
        @test ptranspose(M, 1) == ptranspose(M, (2, 2), 1)
        @test ptranspose(M, 2) == ptranspose(M, (2, 2), 2)

        @test ptrace(M) == ptrace(M, (2, 2), 2)
        @test ptranspose(M) == ptranspose(M, (2, 2), 2)
    end

    let M = [[1.0, im] [im, 1.0]]

        @test ptrace(M, (1, 2), 1) == M
        @test ptranspose(M, (1, 2), 1) == M
        @test ptrace(M, (1, 2), 2) == fill(tr(M), 1, 1)
        @test ptranspose(M, (1, 2), 2) == transpose(M)
    end
end

@testset "binent" begin
    @test binent(0.0) == 0.0
    @test binent(0.5) == 1.0
    @test binent(1.0) == 0.0
end

@testset "purity, S_vn, S_renyi" begin
    let rho1 = I(4) / 4.0,
        rho2 = [[2.0, im] [-im, 2.0]] / 2.0,
        eigs2 = [1.0, 3.0] / 2.0

        @test purity(rho1) == 0.25
        @test purity(rho2) == sum(eigs2.^2)

        @test S_renyi(rho1, 0) == 2.0
        @test S_renyi(rho2, 0) == 1.0

        @test S_vn(rho1) == 2.0
        @test S_vn(rho2) == -sum(eigs2 .* log2.(eigs2))

        @test S_renyi(rho1) == 2.0
        @test S_renyi(rho2) == -log2(sum(eigs2.^2))

        @test S_renyi(rho1, Inf) == 2.0
        @test S_renyi(rho2, Inf) == -log2(maximum(eigs2))
    end
end

@testset "mutinf" begin
    let rho = diagm([3, 2, 1, 2]) / 8.0

        @test isapprox(mutinf(rho), (1.5 - 5.0 * log2(5.0) / 8.0))
        @test isapprox(mutinf(rho, S_renyi), (1.0 + 2.0 * log2(3.0) - log2(17.0)))
    end
end

@testset "spinflip, concurrence, concurrence_lb, formation, negativity" begin
    let rho = reshape(1.0:16.0, 4, 4),
        rho_f = [[16.0, -15.0, -14.0, 13.0] [-12.0, 11.0, 10.0, -9.0] [-8.0, 7.0, 6.0, -5.0] [4.0, -3.0, -2.0, 1.0]]

        @test spinflip(rho) == rho_f
    end

    let rho = zeros(4, 4)

        rho[1, 1] = 0.5
        rho[4, 4] = 0.5

        let C = concurrence(rho)

            @test spinflip(rho) == rho
            @test C == 0.0
            @test concurrence_lb(rho) == 0.0
            @test formation(C) == 0.0
            @test negativity(rho) == 0.0
        end
    end

    let rho = zeros(4, 4)

        rho[1, 1] = 0.125
        for i in 2:3, j in 2:3
            rho[i, j] = 0.375
        end
        rho[4, 4] = 0.125

        let C = concurrence(rho),
            a = (2 + sqrt(3.0)) / 4.0,
            b = (2 - sqrt(3.0)) / 4.0

            @test spinflip(rho) == rho
            @test C == 0.5
            @test concurrence_lb(rho) == sqrt(3.0)/4.0
            @test formation(C) == -a * log2(a) - b * log2(b)
            @test negativity(rho) == log2(1.5)
        end
    end

    let rho = zeros(4, 4)

        for i in 2:3, j in 2:3
            rho[i, j] = 0.5
        end

        let C = concurrence(rho)

            @test spinflip(rho) == rho
            @test C == 1.0
            @test concurrence_lb(rho) == 1.0
            @test formation(C) == 1.0
            @test negativity(rho) == 1.0
        end
    end

    let rho = Matrix{ComplexF64}(undef, 4, 4)

        for i in 1:4
            for j in 1:4
                if i < j
                    rho[i, j] = 0.0625im
                elseif i == j
                    rho[i, j] = 0.25
                else
                    rho[i, j] = -0.0625im
                end
            end
        end

        let C = concurrence(rho),
            rho_f = copy(transpose(rho))

            rho_f[1, 4] *= -1
            rho_f[2, 3] *= -1
            rho_f[3, 2] *= -1
            rho_f[4, 1] *= -1

            @test spinflip(rho) == rho_f
            @test C == 0.0
            @test concurrence_lb(rho) == 0.0
            @test formation(C) == 0.0
            @test isapprox(negativity(rho), 0.0, atol=1e-15)
        end
    end
end

end


"""
"""
function autodiff_cell_residual_from_energy(uh_to_cell_energy::Function,uh,cell_ids=_default_cell_ids(uh))
  @assert is_a_fe_function(uh)
  U = get_fe_space(uh)
  cell_u_to_cell_energy = _change_argument_to_cell_u(uh_to_cell_energy,U)
  cell_u = get_cell_values(uh)
  cell_r = autodiff_array_gradient(cell_u_to_cell_energy,cell_u,cell_ids)
  cell_r
end

"""
"""
function autodiff_cell_jacobian_from_energy(uh_to_cell_energy::Function,uh,cell_ids=_default_cell_ids(uh))
  @assert is_a_fe_function(uh)
  U = get_fe_space(uh)
  cell_u_to_cell_energy = _change_argument_to_cell_u(uh_to_cell_energy,U)
  cell_u = get_cell_values(uh)
  cell_j = autodiff_array_hessian(cell_u_to_cell_energy,cell_u,cell_ids)
  cell_j
end

"""
"""
function autodiff_cell_jacobian_from_residual(uh_to_cell_residual::Function,uh,cell_ids=_default_cell_ids(uh))
  @assert is_a_fe_function(uh)
  U = get_fe_space(uh)
  cell_u_to_cell_residual = _change_argument_to_cell_u(uh_to_cell_residual,U)
  cell_u = get_cell_values(uh)
  cell_j = autodiff_array_jacobian(cell_u_to_cell_residual,cell_u,cell_ids)
  cell_j
end

function _default_cell_ids(uh)
  cell_u = get_cell_values(uh)
  cell_ids = IdentityVector(length(cell_u))
end

function _change_argument_to_cell_u(uh_to_cell_energy,U)
  function f(cell_u)
    uh = CellField(U,cell_u)
    cell_e = uh_to_cell_energy(uh)
    cell_e
  end
  f
end


import StaticArrays.SVector
import StaticArrays.norm

abstract type AbstractDOF end

struct Point{d, T}<:AbstractDOF
    pos::SVector{d,T}
    id::Int
end

function distance(p1::Point{d,T}, p2::Point{d,T}) where {d, T}
    # return norm(p1.pos - p2.pos)
    out = zero(T)
    @fastmath @inbounds @simd for k = 1 : d
        out += (p1.pos[k] - p2.pos[k])^2
    end
    return sqrt(out)
end

# function that takes in a matrix and returns a n array of points corresponding to dim-slices of the matrix.
function mat2points(x::AbstractMatrix; dims::Int=1)
    @assert dims ∈ [1,2]
    if dims == 2
        x = x'
    end

    points = Vector{Point{size(x, 1), eltype(x)}}(undef, size(x, 2))
    for k = 1 : size(x, 2)
        points[k] = Point{size(x, 1), eltype(x)}(SVector{size(x, 1), eltype(x)}(x[:, k]), k)
    end

    return points
end


mutable struct ADMM{U} <: AbstractOptimizer
    λ::U
    ρ::U
end

"""
    ADMM()
    ADMM(λ, ρ)

`ADMM` is an implementation of Lasso using the alternating direction methods of multipliers and loosely based on [this implementation](https://web.stanford.edu/~boyd/papers/admm/lasso/lasso.html).

`λ` is the sparsification parameter, `ρ` the augmented Lagrangian parameter.

# Example
```julia
opt = ADMM()
opt = ADMM(1e-1, 2.0)
```
"""
ADMM() = ADMM(0.1, 1.0)

function set_threshold!(opt::ADMM, threshold)
    opt.λ = threshold*opt.ρ
end

get_threshold(opt::ADMM) = opt.λ/opt.ρ

init(o::ADMM, A::AbstractArray, Y::AbstractArray) =  A \ Y
init!(X::AbstractArray, o::ADMM, A::AbstractArray, Y::AbstractArray) =  ldiv!(X, qr(A, Val(true)), Y)

#soft_thresholding(x::AbstractArray, t::T) where T <: Real = sign.(x) .* max.(abs.(x) .- t, zero(eltype(x)))

function fit!(X::AbstractArray, A::AbstractArray, Y::AbstractArray, opt::ADMM; maxiter::Int64 = 1, convergence_error::T = eps()) where T <: Real
    n, m = size(A)

    g = NormL1(get_threshold(opt))

    x̂ = deepcopy(X)
    ŷ = zero(X)

    P = I(m)/opt.ρ - (A' * pinv(opt.ρ*I(n) + A*A') *A)/opt.ρ
    c = P*(A'*Y)

    x_i = similar(X)
    x_i .= X

    iters = 0

    @inbounds for i in 1:maxiter
        iters += 1

        x̂ .= P*(opt.ρ*X - ŷ) + c
        prox!(X, g, x̂ + ŷ/opt.ρ)
        ŷ .= ŷ + opt.ρ*(x̂ - X)

        if norm(x_i - X, 2) < convergence_error
            break
        else
            x_i .= X
        end

    end

    X[abs.(X) .< get_threshold(opt)] .= zero(eltype(X))
    return iters
end


using LinearAlgebra
export sample_urn_without_replacement, adj_mat_to_vec, vec_to_adj_mat
export rand_perturb, get_num_vertices

function sample_urn_without_replacement(
    n::Int, bins::Vector{Int}
    )
    var_bins = copy(bins)
    n_tot = sum(bins)
    @assert n_tot ≥ n "Sum of bins must be ≥ n."
    out = Int[]
    for i in 1:n
        counter = 0
        z = rand(1:n_tot)
        for (obs,bin_size) in enumerate(var_bins)
            counter += bin_size
            if counter ≥ z
                var_bins[obs] -= 1
                n_tot -= 1
                push!(out, obs)
                break
            end
        end
    end
    return out
end

function adj_mat_to_vec(
        A::Matrix;
        directed::Bool=true,
        self_loops::Bool=true
    )

    V = size(A,1)
    if directed & self_loops
        return A[:]
    elseif directed & !self_loops
        return A[[i!=j ? true : false for i=1:V,j=1:V]]
    elseif !directed & self_loops
        return A[[i ≤ j ? true : false for i=1:V,j=1:V]]
    elseif !directed & !self_loops
        return A[[i < j ? true : false for i=1:V,j=1:V]]
    end
end

function vec_to_adj_mat(
    x::Vector;
    directed::Bool=true,
    self_loops::Bool=true
    )
    M = length(x)
    if directed & self_loops
        V = round(Int, √(M))
        A = zeros(eltype(x), V, V)
        A[:] = copy(x)
        return A
    elseif directed & !self_loops
        V = round(Int, √(M+1/4) + 1/2)
        A = zeros(eltype(x), V, V)
        A[[i!=j ? true : false for i=1:V,j=1:V]] = copy(x)
        return A
    elseif !directed & self_loops
        V = round(Int, √(2M+1/4) - 1/2)
        A = zeros(eltype(x), V, V)
        A[[i ≤ j ? true : false for i=1:V,j=1:V]] = copy(x)
        A[[i > j ? true : false for i=1:V,j=1:V]] = A'[[i > j ? true : false for i=1:V,j=1:V]]
        @assert issymmetric(A) "Output must be symmetric for un-directed case."
        return A
    elseif !directed & !self_loops
        V = round(Int, √(2M+1/4) + 1/2)
        A = zeros(eltype(x), V, V)
        A[[i < j ? true : false for i=1:V,j=1:V]] = copy(x)
        A += A'
        @assert issymmetric(A) "Output must be symmetric for un-directed case."
        return A
    end

end

rand_perturb(A::Matrix{Bool}, τ::Float64) = [rand() < τ ? !x : x for x in A]

function get_num_vertices(
    M::Int, # Number of edges
    directed::Bool, self_loops::Bool
    )
    if directed & self_loops
        return round(Int, √(M))
    elseif directed & !self_loops
        return round(Int, √(M+1/4) + 1/2)
    elseif !directed & self_loops
        return round(Int, √(2M+1/4) - 1/2)
    elseif !directed & !self_loops
        return round(Int, √(2M+1/4) + 1/2)
    end
end


using Base.Filesystem

include("../src/Triangulation.jl")

# create_nodes
nodes = Triangulation.create_nodes(10, [0., 1.], [0., 1.])

# in_circumcircle
Triangulation.in_circumcircle([1, 2, 3], 4, nodes)

# is_clockwise
Triangulation.is_clockwise([0., 0.], [1.,0.], [0., 1.])

# create_supertriangle
Triangulation.create_supertriangle(nodes)

# require_flip
AB = Triangulation.Edge([1, 2], [])
BC = Triangulation.Edge([2, 3], [])
CD = Triangulation.Edge([3, 4], [])
BD = Triangulation.Edge([2, 4], [])
AD = Triangulation.Edge([1, 4], [])
edges = [AB, BC, CD, BD, AD]
triangles = [Triangulation.Triangle([1, 4, 5]), Triangulation.Triangle([2, 3, 4])]
width = 1.
height = width/tan(pi/3)
nodes = [[0., -height], [width, 0.],  [0., height], [-width, 0.]]
Triangulation.require_flip(1, 2, triangles, edges, nodes) == true

# compute_angle
nodes_2 = [[0., 0.], [1., 0.], [0., tan(pi/3)]]
edge1 = Triangulation.Edge([1, 2], [])
edge2 = Triangulation.Edge([1, 3], [])
Triangulation.compute_angle(edge1, edge2, nodes_2)

# get_tri_nodes
edges = [
    Triangulation.Edge([1, 2], []),
    Triangulation.Edge([2, 3], []),
    Triangulation.Edge([3, 1], [])
]
triangles = [Triangulation.Triangle([1, 2, 3])]
Triangulation.get_tri_node_ids(1, triangles, edges)

# inside_triangle
Triangulation.inside_triangle([1, 2, 3], 4, nodes)

# get_triangulation_nodes
Triangulation.get_triangulation_nodes(edges)

# flip_triangulation!
AB = Triangulation.Edge([1, 2], [1, 0])
BC = Triangulation.Edge([2, 3], [2, 0])
CD = Triangulation.Edge([3, 4], [2, 0])
BD = Triangulation.Edge([2, 4], [1, 2])
AD = Triangulation.Edge([1, 4], [1, 0])
edges = Dict{Int, Triangulation.Edge}(1=>AB, 2=>BC, 3=>CD, 4=>BD, 5=>AD)
triangles = Dict{Int, Triangulation.Triangle}(
    1 => Triangulation.Triangle([1, 4, 5]),
    2 => Triangulation.Triangle([2, 3, 4])
)
width = 1.
height = width/tan(pi/3)
nodes = [[0., -height], [width, 0.],  [0., height], [-width, 0.]]
Triangulation.flip_triangulation!(triangles, edges, nodes, true, "./_tmp_.txt", init_flip_step=0)

# plot_step
Triangulation.plot_step("./_tmp_flip_1.txt", false)

# triangulate
nodes = Triangulation.create_nodes(5, [0., 1.], [0., 1.])
Triangulation.triangulate(nodes, false, "./_tmp_.txt")

# parse_args
Triangulation.parse_args("-i 5 --long=3 --long2 3")

# print_help
Triangulation.print_help()

# get_arg_value
Triangulation.get_arg_value(["c", "d"], Dict("c" => "ok", "d" => "ok"))

# get_main_parameters
Triangulation.get_main_parameters("-c -o myOutput -h -p -s")
Triangulation.get_main_parameters("--create-nodes --output=myOutput --help --plot --save-steps")
Filesystem.touch("./_tmp_test.txt")
Triangulation.get_main_parameters("-f ./_tmp_test.txt")
Triangulation.get_main_parameters("-l ./_tmp_test*.txt")

# main
Triangulation.main(["--help"])
Triangulation.main(["-l ./results*.txt"])

# julia_main
Triangulation.julia_main()

# remove tmp files
Filesystem.rm("./_tmp_test.txt")
Filesystem.rm("./_tmp_flip_1.txt")
Filesystem.rm("./_tmp_flip_1.png")


"""
    g = gaussianize(z)

Gaussianize field `z` using a sorted random sequence.
"""
function gaussianize(z)
    (m,n) = size(z)
    nn = m*n
    zr = z[:]
    zs = sort(zr)
    i = sortperm(zr)
    rrk = zeros(Int16,nn,1)
    rrk[i] = collect(1:nn)
    gg = randn(nn)
    w = sort(gg)
    gr = w[rrk]
    gs = Statistics.std(gr)
    if gs==0
        gs=1
    end
    gr = gr/gs
    g = reshape(gr,m,n)
end


############################ 2D methods ###############################

"""
shearingLayer(inputSize::Union{Int,NTuple{N, T}};
                       scale = -1, shearLevel = scale,
                       dType = Float32, σ = abs, trainable = false,
                       plan = true, boundary=Pad(-1,-1),
                       averagingLayer = false) where {N,T}

create a ConvFFT layer that uses shearlets from [Shearlab.jl](https://arsenal9971.github.io/Shearlab.jl/). By default it isn't trainable.
# New Arguments
- `scale::Integer=-1`: gives the number of scales to compute. If this is `-1`,
                       it is computed automatically based on the image size,
                       with 2 scales if the image is smaller than 24, and 4
                       otherwise -

- `shearLevel::Integer=scale`: controls the number of shearing levels to use at
                               each scale. Should roughly correspond to how far
                               to shear in one direction at the finest scale.
- `averagingLayer:Bool=false`: sets whether or not to only apply just the
                               averaging filter.
"""
function shearingLayer(inputSize::Union{Int,NTuple{N,T}}; scale=-1,
                       shearLevel=scale, bias=false,
                       init=Flux.glorot_normal, dType=Float32, σ=abs,
                       trainable=false, plan=true, boundary=Pad(-1, -1),
                       averagingLayer=false) where {N,T}

    scale = defaultShearletScale(inputSize, scale)
    if shearLevel < 0
        shearLevel = scale
    end
    effectiveShears = ceil.(Int, (1:shearLevel) / 2)
    shears = Shearlab.getshearletsystem2D(inputSize[1:2]..., scale,
                                          effectiveShears,
                                          typeBecomes=dType,
                                          padded=typeof(boundary) <: Pad)
    if averagingLayer
        shearlets = shears.shearlets[:,:, end:end]
    else
        shearlets = shears.shearlets
    end
    if dType <: Real
        shearlets = Complex{dType}.(shearlets)# correct the element type
    else
        shearlets = dType.(shearlets)
    end
    nShears = shears.nShearlets
    if typeof(boundary) <: Pad
        boundary = Pad{2}(shears.padBy)
    end
    bias = nothing

    return ConvFFT(shearlets, bias, inputSize, σ, plan=plan,
                   boundary=boundary, dType=dType,
                   trainable=trainable, OT=dType)
end

function defaultShearletScale(inputSize, scale)
    if minimum(inputSize[1:2]) >= 24 && scale > 0
        return scale
    elseif minimum(inputSize[1:2]) >= 24
        return 4
    elseif minimum(inputSize[1:2]) >= 12
        return 2
    else
        error("can't meaningfully make shearlets at size $(inputSize[1:2])")
    end
end

#######################################################################
#######################################################################
############################ 1D methods ###############################
#######################################################################
#######################################################################

"""
    waveletLayer(inputSize::Union{Int,NTuple{N, T}};
                 dType = Float32, σ = identity, trainable = false,
                 plan = true, init = Flux.glorot_normal, bias=false,
                 convBoundary=Sym(), cw = Morlet(), averagingLayer = false,
                 varargs...) where {N,T}

Create a ConvFFT layer that uses wavelets from [ContinuousWavelets.jl](https://github.com/dsweber2/ContinuousWavelets.jl). By default it isn't trainable. varargs are any of the settings that can be passed on to creating a `CFW` type.
# New Arguments
- `convBoundary::ConvBoundary=Sym()`: the type of symmetry to use in computing
                                      the transform. Note that convBoundary and
                                      boundary are different, with boundary
                                      needing to be set using types from
                                      ContinuousWavelets and convBoundary needs
                                      to be set using the FourierFilterFlux
                                      boundary types.
- `cw::ContWaveClass=Morlet()`: the type of wavelet to use, e.g. `dog2`,
                                `Morlet()`. See ContinuousWavelets.jl for more.

- `averagingLayer::Bool=false`: the same idea as for shearingLayer, if this is
                                true, only return the results from the
                                averaging filter.
arguments from wavelet constructor
Q=8, boundary::T=SymBoundary(),
        averagingType::A = Father(),
        averagingLength::Int = 4, frameBound=1, p::N=Inf,
        β=4
"""
function waveletLayer(inputSize::Union{T,NTuple{N,T}};
                      dType=Float32, σ=identity, trainable=false,
                      plan=true, init=Flux.glorot_normal, bias=false,
                      convBoundary=Sym(), cw=Morlet(), averagingLayer=false,
                      varargs...) where {N,T <: Int}
    waveletType = wavelet(cw; varargs...)
    wavelets, ω = computeWavelets(inputSize[1], waveletType; T=T)

    if averagingLayer
        wavelets = wavelets[:, 1:1]
    elseif typeof(waveletType.averagingType) <: Union{ContinuousWavelets.Dirac,Father}
        wavelets = cat(wavelets[:, 2:end], wavelets[:,1], dims=2)
    end

    if dType <: Real
        wavelets = Complex{dType}.(wavelets) # correct the element type
    else
        wavelets = dType.(wavelets)
    end

    if typeof(cw) <: Dog || typeof(cw) <: ContOrtho
        OT = dType
        An = nothing
    else
        if typeof(waveletType.averagingType) <: NoAve
            An = (-1,)
        else
            An = (size(wavelets, 2),)
        end
        if dType <: Real
            OT = Complex{dType}
            averagingStyle = RealWaveletRealSignal
        else
            OT = dType
            averagingStyle = RealWaveletComplexSignal
        end
        An = map(ii -> ((ii in An) ? averagingStyle() :
                      AnalyticWavelet()), (1:size(wavelets, 2)[end]...,))
    end
    if bias
        bias = dType.(init(inputSize[2:end - 1]..., size(wavelets, 2)))
    else
        bias = nothing
    end

    return ConvFFT(wavelets, bias, inputSize, σ, plan=plan, dType=dType,
                   trainable=trainable, boundary=convBoundary, OT=OT, An=An)
end