function minsort(A,b)
    A1 = (eltype(A))[]
    A2 = (eltype(A))[]
    A3 = (eltype(A))[]
    for i in firstindex(A):lastindex(A)
        if A[i]<b
            push!(A1,A[i])
        elseif A[i]==b
            push!(A2,A[i])
        else
            push!(A3,A[i])
        end
    end
    A=vcat(A1,A2,A3)
    return A
end

function minsort2(A,b)
    A1 = Array{eltype(A)}(undef,length(A))
    A2 = Array{eltype(A)}(undef,length(A))
    A3 = Array{eltype(A)}(undef,length(A))
    j  = 1
    k  = 1
    l  = 1
    for i in firstindex(A):lastindex(A)
        if A[i]<b
            A1[j]=A[i]
            j=j+1
        elseif A[i]==b
            A2[k]=A[i]
            k=k+1
        else
            A3[l]=A[i]
            l=l+1
        end
    end
    A[1:j-1]=A1[1:j-1]
    A[j:j+k-2]=A2[1:k-1]
    A[j+k-1:j+k+l-3]=A3[1:l-1]
    return A
end

function minsort3(A,b)
    i = 1
    j = 1
    l = lastindex(A)
    k = 2
    while k<l
        while A[i]>=b && k<l
            if A[i]==b
                A[k],A[i]=A[i],A[k]
                k=k+1
            end
            if A[i]>b
                A[l],A[i]=A[i],A[l]
                l=l-1
            end
        end
        if j>k
            k=j+1
        end
        j=j+1
        i=i+1
    end
    return A
end


"""
    prior_probs(model::Model, parm_grid)

Computes a grid of prior probabilities over model parameters.

- `model::Model`: a model object
- `parm_grid`: a grid of parameter values
"""
function prior_probs(model::Model, parm_grid)
    return prior_probs(model.prior, parm_grid)
end

"""
    prior_probs(prior, parm_grid)

Computes a grid of prior probabilities over model parameters.

- `prior`: a vector of `Distribution` objects, one for each parameter
- `parm_grid`: a grid of parameter values
"""
function prior_probs(prior, parm_grid)
    dens = [mapreduce((θ,d)->pdf(d, θ), *, g, prior) for g in parm_grid]
    return dens/sum(dens)
end

"""
    prior_probs(prior, parm_grid)

Computes a grid of uniform prior probabilities over model parameters.

- `prior::Nothing`: a vector of `Distribution` objects, one for each parameter
- `parm_grid`: a grid of parameter values
"""
function prior_probs(prior::Nothing, parm_grid)
    return fill(1/length(parm_grid), size(parm_grid))
end

"""
    loglikelihood(model::Model, design_grid, parm_grid, data_grid, model_type, model_state)

Computes a three dimensional grid of log likelihoods where the first dimension is the model parameters, the second
dimension is the design parameters, and the third dimension is the data values.

- `model::Model`: a model object containing a log likelihood function and prior distributions
- `parm_grid`: a grid of model parameters
- `design_grid`: a grid of design parameters
- `data_grid`: a grid of data values
- `model_type`: model type can be `Static` or `Dynamic`
- `model_state`: model state variables that are updated for `Dynamic` models
"""
function loglikelihood(model::Model, design_grid, parm_grid, data_grid, model_type, model_state)
    return loglikelihood(model.loglike, design_grid, parm_grid, data_grid)
end

"""
    loglikelihood(model::Model, design_grid, parm_grid, data_grid, model_type, model_state)

Computes a three dimensional grid of log likelihoods where the first dimension is the model parameters, the second
dimension is the design parameters, and the third dimension is the data values.

- `model::Model`: a model object containing a log likelihood function and prior distributions
- `parm_grid`: a grid of model parameters
- `design_grid`: a grid of design parameters
- `data_grid`: a grid of data values
- `model_type`: model type can be `Static` or `Dynamic`
- `model_state`: model state variables that are updated for `Dynamic` models
"""
function loglikelihood(model::Model, design_grid, parm_grid, data_grid, model_type::Dyn, model_state)
    return loglikelihood(model.loglike, design_grid, parm_grid, data_grid, model_state)
end

"""
    loglikelihood(model::Model, design_grid, parm_grid, data_grid, model_type, model_state)

Computes a three dimensional grid of log likelihoods where the first dimension is the model parameters, the second
dimension is the design parameters, and the third dimension is the data values.

- `loglike`: a model object containing a log likelihood function and prior distributions
- `parm_grid`: a grid of model parameters
- `design_grid`: a grid of design parameters
- `data_grid`: a grid of data values
- `model_type`: model type can be `Static` or `Dynamic`
- `model_state`: model state variables that are updated for `Dynamic` models
"""
function loglikelihood(loglike, design_grid, parm_grid, data_grid)
    LLs = zeros(length(parm_grid), length(design_grid), length(data_grid))
    for (d, data) in enumerate(data_grid)
        for (k,design) in enumerate(design_grid)
            for (p,parms) in enumerate(parm_grid)
                LLs[p,k,d] = loglike(parms..., design..., data...)
            end
        end
    end
    return LLs
end

"""
    loglikelihood(model::Model, design_grid, parm_grid, data_grid, model_type, model_state)

Computes a three dimensional grid of log likelihoods for a dynamic model where the first dimension is the model parameters,
the second dimension is the design parameters, and the third dimension is the data values.

- `loglike`: a model object containing a log likelihood function and prior distributions
- `parm_grid`: a grid of model parameters
- `design_grid`: a grid of design parameters
- `data_grid`: a grid of data values
- `model_type`: model type can be `Static` or `Dynamic`
- `model_state`: model state variables that are updated for `Dynamic` models
"""
function loglikelihood(loglike, design_grid, parm_grid, data_grid, model_state)
    LLs = zeros(length(parm_grid), length(design_grid), length(data_grid))
    i = 0
    for (d, data) in enumerate(data_grid)
        for (k,design) in enumerate(design_grid)
            for (p,parms) in enumerate(parm_grid)
                i += 1
                state = deepcopy(model_state[i])
                LLs[p,k,d] = loglike(parms..., design..., data..., state)
            end
        end
    end
    return LLs
end

"""
    loglikelihood(optimizer::Optimizer)

Computes a three dimensional grid of log likelihoods for a dynamic model where the first dimension is the model parameters,
the second dimension is the design parameters, and the third dimension is the data values.

- `optimizer`: an optimizer object

"""
function loglikelihood!(optimizer)
    @unpack model, log_like, design_grid, parm_grid, data_grid, model_state = optimizer
    return loglikelihood!(model.loglike, log_like, design_grid, parm_grid, data_grid, model_state)
end

"""
    loglikelihood(optimizer::Optimizer)

Computes a three dimensional grid of log likelihoods where the first dimension is the model parameters, the second
dimension is the design parameters, and the third dimension is the data values.

- `loglike`: a model object containing a log likelihood function and prior distributions
- `parm_grid`: a grid of model parameters
- `design_grid`: a grid of design parameters
- `data_grid`: a grid of data values
- `model_type`: model type can be `Static` or `Dynamic`
- `model_state`: model state variables that are updated for `Dynamic` models
"""
function loglikelihood!(loglike, log_like, design_grid, parm_grid, data_grid, model_state)
    i = 0
    for (d, data) in enumerate(data_grid)
        for (k,design) in enumerate(design_grid)
            for (p,parms) in enumerate(parm_grid)
                i += 1
                state = deepcopy(model_state[i])
                log_like[p,k,d] = loglike(parms..., design..., data..., state)
            end
        end
    end
    return nothing
end

"""
    marginal_log_like!(optimizer)

Marginalizes the log likelihoods over model parameters, resulting in a three dimensional array where the first dimension is length 1,
the second dimension is the design parameters, and the third dimension is the data values.

- `optimizer`: an optimizer object
"""
function marginal_log_like!(optimizer)
    @unpack marg_log_like,log_like,log_post = optimizer
    marg_log_like .= marginal_log_like(log_post, log_like)
end

"""
    marginal_log_like(optimizer)

Marginalizes the log likelihoods over model parameters, resulting in a three dimensional array where the first dimension is length 1,
the second dimension is the design parameters, and the third dimension is the data values.

- `log_post`: a vector of posterior log likelihood values for the model parameters
- `log_like`: a three dimensional vector of log likelihood values where the first dimension corresponds to the model parameters,
the second dimension corresponds to the design parameters, and the third dimension corresponds to the data values
"""
function marginal_log_like(log_post, log_like)
    return logsumexp(log_post .+ log_like, dims=1)
end

# """
#     marginal_posterior(optimizer)

# Computes a three dimensional grid of log likelihoods where the first dimension is the model parameters, the second
# dimension is the design parameters, and the third dimension is the data values.

# - `optimizer`: an optimizer object
# """
# function marginal_posterior(optimizer)
#     @unpack posteriors = optimizer
#     return map(d->sum(posterior, dims=d), ndims(posterior):-1:1)
# end

function compute_entropy(log_like)
    return -1*sum(exp.(log_like) .* log_like, dims=3)[:,:]
end

function conditional_entropy(entropy, post)
    return entropy'*post
end

function conditional_entropy!(optimizer)
    @unpack cond_entropy, entropy,log_post = optimizer
    post = exp.(log_post)
    cond_entropy .= conditional_entropy(entropy, post)
end

function marginal_entropy!(optimizer::Optimizer)
    @unpack marg_entropy,marg_log_like = optimizer
    marg_entropy .= marginal_entropy(marg_log_like)
end

function marginal_entropy(marg_log_like)
    return -sum(exp.(marg_log_like).*marg_log_like, dims=3)[:]
end

"""
    mutual_information(optimizer)

Computes a vector of the mutual information values where each element corresponds to a design parameter.

- `marg_entropy`: a vector of marginal entropy values where each element corresponds to a design parameter
- `cond_entropy`: a vector of conditional entropy values where each element corresponds to a design parameter
"""
function mutual_information(marg_entropy, cond_entropy)
    return marg_entropy .- cond_entropy
end

"""
    mutual_information!(optimizer)

Computes a vector of the mutual information values where each element corresponds to a design parameter.

- `optimizer`: an optimizer object
"""
function mutual_information!(optimizer)
    @unpack mutual_info,marg_entropy,cond_entropy = optimizer
    mutual_info .= mutual_information(marg_entropy, cond_entropy)
    return nothing
end

function get_best_design(optimizer)
    @unpack best_design,design_names = optimizer
    return NamedTuple{design_names}(best_design)
end

function find_best_design!(optimizer)
    @unpack design_names,mutual_info,design_grid = optimizer
    best_design = find_best_design(mutual_info, design_grid, design_names)
    optimizer.best_design = best_design
    return best_design
end

function find_best_design(mutual_info, design_grid, design_names)
    _,best = findmax(mutual_info)
    best_design = design_grid[best]
    return best_design
end

function find_best_design!(optimizer::Optimizer{A}) where {A<:Rand}
    @unpack design_grid,design_names = optimizer
    best_design = rand(design_grid)
    optimizer.best_design = best_design
    return best_design
end

function update_posterior!(optimizer, data)
    @unpack log_post,design_grid,data_grid,log_like,best_design = optimizer
    dn = find_index(design_grid, best_design)
    for datum in data
        da = find_index(data_grid, (datum,))
        log_post .+= log_like[:,dn,da]
    end
    log_post .-= logsumexp(log_post)
    return nothing
end

function update!(optimizer, data)
    update_posterior!(optimizer, data)
    marginal_log_like!(optimizer)
    marginal_entropy!(optimizer)
    conditional_entropy!(optimizer)
    mutual_information!(optimizer)
    best_design = find_best_design!(optimizer)
    return best_design
end

function update!(optimizer::Optimizer{A,MT}, data, args...; kwargs...) where {A,MT<:Dyn}
    update_posterior!(optimizer, data)
    update_states!(optimizer, data, args...; kwargs...)
    loglikelihood!(optimizer)
    marginal_log_like!(optimizer)
    marginal_entropy!(optimizer)
    conditional_entropy!(optimizer)
    mutual_information!(optimizer)
    best_design = find_best_design!(optimizer)
    return best_design
end

function update!(optimizer::Optimizer{A,MT}, data, args...; kwargs...) where {A<:Rand,MT<:Dyn}
    update_posterior!(optimizer, data)
    update_states!(optimizer, data, args...; kwargs...)
    loglikelihood!(optimizer)
    best_design = find_best_design!(optimizer)
    return best_design
end

function update!(optimizer::Optimizer{A}, data) where {A<:Rand}
    update_posterior!(optimizer, data)
    best_design = find_best_design!(optimizer)
    return best_design
end

function update_states!(optimizer, obs_data, args...; kwargs...)
    @unpack model_state, data_grid, design_grid, parm_grid, update_state! = optimizer
    i = 0
    for data in data_grid
        for design in design_grid
            for parms in parm_grid
                i += 1
                update_state!(model_state[i], parms, design, data, obs_data, args...; kwargs...)
            end
        end
    end
    return nothing
end

function to_grid(vals::NamedTuple)
    k = keys(vals)
    v = product(vals...) |> collect
    return k,v
end

function to_grid(vals)
    k = [Symbol(string("v",i)) for i in 1:length(vals[1])]
    return (k...,),vals
end

to_grid(vals::Tuple) = vals

function find_index(grid, val)
    i = 0
    for g in grid
        i += 1
        if g == val
            return i
        end
    end
    return i
end

function mean_post(optimizer)
    @unpack log_post,parm_grid = optimizer
    post = exp.(log_post)
    return mean_post(post, parm_grid)
end

function mean_post(post, parm_grid)
    return mapreduce((p,v)->p.*v, .+, post, parm_grid)
end

function std_post(post, parm_grid)
    mu = mean_post(post, parm_grid)
    return mapreduce((p,v)->p.*(v .- mu).^2, .+, post, parm_grid) .|> sqrt
end

function std_post(optimizer)
    @unpack log_post,parm_grid = optimizer
    post = exp.(log_post)
    return std_post(post, parm_grid)
end

function create_state(model_type::Dyn, T, dims, args...; kwargs...)
    state = fill(T(args...; kwargs...), dims)
    return state .= deepcopy.(state)
end

function create_state(model_type::Stat, T, dims, args...; kwargs...)
    return nothing
end

# function update(θ1, θ2, p, data)
#     #like = pdf.(Binomial.(n, x), k)
#     like = pdf.(Bernoulli.(x), d)
#     for d in data
#         θ1 .*= like
#         θ2 .*= like
#     end

#     # e1 = θ1'*like
#     # e2 = θ2'*like

#     # θ1 .= (θ1 .* like)/e1
#     # θ2 .= (θ2 .* like)/e2
#     e1 = sum(θ1)
#     e2 = sum(θ2)
#     θ1 ./= e1
#     θ2 ./= e2

#     post_odds = (p * e1) / ((1 - p) * e2)
#     p = post_odds/(1 + post_odds)
#     return p, θ1, θ2
# end

# # function update_model_posterior(ps, es)
# #     map(i->ps[i]./(sum(ps.*(es./es[i]))),1:length(ps))
# # end

# using Distributions
# p = .5
# x = .001:.001:.999
# θ1 = pdf.(Beta(10,1), x)
# θ1 /= sum(θ1)

# θ2 = pdf.(Beta(1,1), x)
# θ2 /= sum(θ2)

# p, θ1, θ2 = update(θ1, θ2, p, [1])


using VertexEliminationOrder
using Test
using Graphs

G = smallgraph("house")
@info "Testing minwidth with house graph"
@test minwidth!(G) == ([1, 5, 4, 3, 2], 2)

G = smallgraph("house")
@info "Testing minwidth with house graph"
@test minfill!(G) == ([5, 1, 4, 3, 2], 2)


"""
    int2binary(n::Int64, array=zeros(Int64,0))

Converts any integer to a binary representation as initiger array with 0, 1.

# Arguments
- `n::Int64`: Integer value which has to be converted
- `array=zeros(Int64,0)`: Empty array for return

# Examples
```julia-repl
julia> import ClassicAlgorithmsCollections
julia> ClassicAlgorithmsCollections.int2binary(131)
([1,0,0,0,0,0,1,1],1)
```
"""
function int2binary(n::Int64, array = zeros(Int64, 0))
    if n > 1
        int2binary(n >> 1, array)
    end
    n &= 1
    append!(array, n)
    return array, n
end


module Matrix2Tabular

export float2sci, matrix2tabular

function float2sci(x::Float64, figs::Int64=2, dollars::Bool=false)
    power = convert(Int,round(log10(x),RoundDown))
    if 0 <= power <= 1
        figs = max(figs-power,2)
        x = string(round(x,digits=figs))
    else
        x = round(x/(10.0^power),digits=figs)
        x = string(x,"\\times10^{",power,"}")
    end

    if dollars
        return string(" \$ ",x," \$ ")
    else
        return x
    end
end

function matrix2tabular(A::Matrix{Float64}, figs::Int64=2, dollars::Bool=false)
    M,N = size(A)

    outstring = " "

    for m = 1:M
        for n = 1:N
            outstring *= float2sci(A[m,n], figs, dollars)
            if n == N
                outstring *= "\\\\ \n "
            end
        end
    end

    return outstring
end
end


using BenchmarkTools

day16data = """E0525D9802FA00B80021B13E2D4260004321DC648D729DD67B2412009966D76C0159ED274F6921402E9FD4AC1B0F652CD339D7B82240083C9A54E819802B369DC0082CF90CF9280081727DAF41E6A5C1B9B8E41A4F31A4EF67E2009834015986F9ABE41E7D6080213931CB004270DE5DD4C010E00D50401B8A708E3F80021F0BE0A43D9E460007E62ACEE7F9FB4491BC2260090A573A876B1BC4D679BA7A642401434937C911CD984910490CCFC27CC7EE686009CFC57EC0149CEFE4D135A0C200C0F401298BCF265377F79C279F540279ACCE5A820CB044B62299291C0198025401AA00021D1822BC5C100763A4698FB350E6184C00A9820200FAF00244998F67D59998F67D5A93ECB0D6E0164D709A47F5AEB6612D1B1AC788846008780252555097F51F263A1CA00C4D0946B92669EE47315060081206C96208B0B2610E7B389737F3E2006D66C1A1D4ABEC3E1003A3B0805D337C2F4FA5CD83CE7DA67A304E9BEEF32DCEF08A400020B1967FC2660084BC77BAC3F847B004E6CA26CA140095003900BAA3002140087003D40080022E8C00870039400E1002D400F10038C00D100218038F400B6100229500226699FEB9F9B098021A00800021507627C321006E24C5784B160C014A0054A64E64BB5459DE821803324093AEB3254600B4BF75C50D0046562F72B1793004667B6E78EFC0139FD534733409232D7742E402850803F1FA3143D00042226C4A8B800084C528FD1527E98D5EB45C6003FE7F7FCBA000A1E600FC5A8311F08010983F0BA0890021F1B61CC4620140EC010100762DC4C8720008641E89F0866259AF460C015D00564F71ED2935993A539C0F9AA6B0786008D80233514594F43CDD31F585005A25C3430047401194EA649E87E0CA801D320D2971C95CAA380393AF131F94F9E0499A775460"""

function bint(bits, offset, len)
    return parse(Int, bits[(begin + offset):(begin + offset + len - 1)]; base=2)
end
const part = [0, 0]

function parsepacket(bits::String, idx=0)
    results, result = Int[], 0
    version = bint(bits, idx, 3)
    part[1] += version
    idx += 3
    typeid = bint(bits, idx, 3)
    idx += 3
    if typeid == 4 # literal value
        numbits, firstwas1 = "", true
        while firstwas1
            numbits *= bits[(begin + idx + 1):(begin + idx + 4)]
            firstwas1 = (bits[begin + idx] == '1')
            idx += 5
        end
        result = bint(numbits, 0, length(numbits))
    else  # or else typeid is one of the operators
        lengthtypeid = bits[begin + idx]
        idx += 1
        if lengthtypeid == '1'  # vector of nresults child packets follows
            nresults = bint(bits, idx, 11)
            idx += 11
            for _ in 1:nresults
                child, idx = parsepacket(bits, idx)
                push!(results, child)
            end
        else  # length of subresults format when length type id is 0
            subpacketlength = bint(bits, idx, 15)
            idx += 15
            subpacketend = idx + subpacketlength
            while idx < subpacketend
                child, idx = parsepacket(bits, idx)
                push!(results, child)
            end
        end
    end
    # part 2 computations on packets which depend on operator (the typeid)
    if typeid == 0
        result = sum(results)
    elseif typeid == 1
        result = prod(results)
    elseif typeid == 2
        result = minimum(results)
    elseif typeid == 3
        result = maximum(results)
    elseif typeid == 5
        result = first(results) > last(results)
    elseif typeid == 6
        result = first(results) < last(results)
    elseif typeid == 7
        result = first(results) == last(results)
    end
    return result, idx
end

function day16(s)
    part[1] = 0
    part[2], _ = parsepacket(string(parse(BigInt, s; base=16); base=2))
    return part
end

par = day16(day16data)
println("Part 1: ", par[1])
println("Part 2: ", par[2])

@btime day16(day16data)


import Test: @inferred

@testset "Substitution" begin
    Mod.@polyvar x[1:3]

    @test subs(2, x[1]=>3) == 2
    @test iszero((x[1]-x[1])(x[1]=>x[2]))
    @test subs(CustomPoly(x[1]+x[2]), x[2]=>x[1]) == 2x[1]

    a = (x[1])(x[1]=>x[2])
    b = x[2]
    @test (x[1])(x[1]=>x[2]) == x[2]

    p = x[1]*x[2]*x[3]
    @test convert(Int, p(x => (1, 2, 3))) == 6

    p = x[1]^2 + x[1]*x[3] - 3
    @test convert(Int, p(x => (5, x[1]*x[2], 4))) == 42

    p = x[1]^2 + x[2]^2
    q = p(x[1:2] => [1 -1; 1 1] * vec(x[1:2]))
    @test q == 2p

    t = 2.0 * x[1] * x[2]^2
    @test t(x => [1, 2, 3]) == 8.0
    @test t((x[2], x[1]) => [1, 3]) == 6.0

    p = x[1] + x[2] + 2*x[1]^2 + 3*x[1]*x[2]^2
    #@inferred p((x[1], x[2]) => (1.0, 2.0))

    @inferred subs(p, x[2] => 2.0)
    @test subs(p, x[2] => 2.0) == 13x[1] + 2 + 2x[1]^2
    @inferred subs(p, x[2] => x[1])
    @inferred subs(p, x[2] => x[1]) == 2x[1] + 2x[1]^2 + 3x[1]^3

    p = x[1] - x[1]
    @test iszero(p)
    @inferred p(x[1] => 1)
    @test iszero(p(x[1] => 1))
    @inferred subs(p, x[1] => 1)
    @test iszero(subs(p, x[1] => 1))

    q = (x[1] + 1) / (x[1] + 2)
    @test isapproxzero(q(x[1] => -1))
    @test !isapproxzero(q(x[1] => 1))
    @test q(x[1] => 1) ≈ 2/3

    q = (x[1] + x[3]) / (x[2] - 1)
    @test subs(q, x[1:2] => (x[2], x[1])) == (x[2] + x[3]) / (x[1] - 1)

    # Taken from perf/runbenchmark.jl
    p = x[1] + x[2] + 2x[1]^2 + x[2]^3

    varst = (x[1], x[2])
    varsv = [x[1], x[2]]
    valst = (1.0, 2.0)
    valsv = [1.0, 2.0]

    @test variable.(Tuple(varsv)) == varst
    @test variable.(Tuple(varsv)) isa Tuple{<:AbstractVariable, <:AbstractVariable}

    @test p(variables(p) => valst) == 13
    @test p(variables(p) => valsv) == 13
    @test p(varst => valst) == 13
    @test p(varsv => valsv) == 13

    Mod.@polyvar x y
    @test subs([x^2 + y, x + y], x => y) == [y^2+y, 2y]
end


using MomentClosure
using MomentClosure: define_μ, define_M
using Test

@testset "creating symbolic moments" begin

    μ = define_μ(3, 3)
    @test μ[(0,0,0)] == 1
    @test string(μ[(2,1,0)]) == "μ₂₁₀(t)"
    @test string(μ[(0,0,3)]) == "μ₀₀₃(t)"
    M = define_M(3, 3)
    @test M[(1,0,0)] == 0
    @test string(M[1,0,2]) == "M₁₀₂(t)"

end


@testset "Testing estspec" begin

    # set up
    x_20 = rand(20)
    x_21 = rand(21)

    @testset "testing output sizes of periodogram and ar_periodogram" begin
    # test shapes of periodogram and ar_periodogram functions
        for x in Any[x_20, x_21]
            w, I_w = periodogram(x)
            n_w, n_Iw, n_x = length(w), length(I_w), length(x)

            @test n_w == floor(Int, n_x / 2 + 1)
            @test n_Iw == floor(Int, n_x / 2 + 1)

            w, I_w = ar_periodogram(x)
            n_w, n_Iw, n_x = length(w), length(I_w), length(x)

            # when x is even we get 10 elements back
            @test n_w == (iseven(n_x) ? floor(Int, n_x / 2) : floor(Int, n_x / 2 + 1))
            @test n_Iw == (iseven(n_x) ? floor(Int, n_x / 2) : floor(Int, n_x / 2 + 1))
        end
    end  # teset set

    @testset "testing `smooth` function options" begin
        # window length must be between 3 and length(x)
        @test_throws ArgumentError smooth(x_20, window_len=25)
        @test_throws ArgumentError smooth(x_20, window_len=2)

        # test hanning is default
        out = smooth(x_20, window="foobar")
        @test out == smooth(x_20, window="hanning")

        # test window_len must be odd
        @test length(smooth(x_20, window_len=6)) == length(smooth(x_20, window_len=7))

    end  # @testset

    @testset "issue from http://discourse.quantecon.org/t/question-about-estspec-jl/61" begin
        T = 150
        rho = -0.9
        e = randn(T)
        x = [e[1]]
        tmp = e[1]
        for t = 2:T
            tmp = rho*tmp+e[t]
            push!(x,tmp)
        end

        for i in 3:2:69
            ar_periodogram(x, "hamming", i)
            # just count that the above didn't throw an error
            @test true
        end
    end

end  # @testset


using Test
using PolynomialBasis
using ImplicitDomainQuadrature
# using Revise
using CutCell
include("useful_routines.jl")

polyorder = 1
numqp = 2

basis = TensorProductBasis(2, polyorder)
levelset = InterpolatingPolynomial(1, basis)
nf = CutCell.number_of_basis_functions(basis)
mesh = CutCell.Mesh([0.0, 0.0], [3.0, 1.0], [3, 1], nf)
nodalcoordinates = CutCell.nodal_coordinates(mesh)
nodalconnectivity = CutCell.nodal_connectivity(mesh)
cellmaps = CutCell.cell_maps(mesh)

normal = [1.0, 0.0]
x0 = [0.5, 0.0]
tol = 1e-3
perturbation = 0.0
levelsetcoeffs = plane_distance_function(nodalcoordinates, normal, x0)

cellsign = CutCell.cell_sign!(levelset, levelsetcoeffs, nodalconnectivity, tol, perturbation)
@test allequal(cellsign, [0, 1, 1])

normal = [1.0, 0.0]
x0 = [1.0, 0.0]
tol = 1e-3
perturbation = 1e-3
levelsetcoeffs = plane_distance_function(nodalcoordinates, normal, x0)
cellsign = CutCell.cell_sign!(levelset,levelsetcoeffs,nodalconnectivity,tol,perturbation)

@test allapprox(levelsetcoeffs[1:4],[-1.0+perturbation,-1.0+perturbation,perturbation, perturbation])
@test allapprox(levelsetcoeffs[[3,4,5,6]],[perturbation,perturbation,1.0,1.0])
@test allapprox(levelsetcoeffs[[5,6,7,8]],[1.,1.,2.,2.])

posactivenodeids = CutCell.active_node_ids(+1, cellsign, nodalconnectivity)
@test allequal(posactivenodeids, 1:8)
negactivenodeids = CutCell.active_node_ids(-1, cellsign, nodalconnectivity)
@test allequal(negactivenodeids, 1:4)

totalnumnodes = CutCell.number_of_nodes(mesh)
cutmeshnodeids =
    CutCell.cut_mesh_nodeids(posactivenodeids, negactivenodeids, totalnumnodes)
testcutmeshnodeids = [
    1 2 3 4 5 6 7 8
    9 10 11 12 0 0 0 0
]
@test allequal(cutmeshnodeids, testcutmeshnodeids)

cutmesh = CutCell.CutMesh(levelset, levelsetcoeffs, mesh)
@test allequal(CutCell.nodal_connectivity(cutmesh, +1, 1), [1, 2, 3, 4])
@test allequal(CutCell.nodal_connectivity(cutmesh, -1, 1), [9, 10, 11, 12])


using Peacock
using PyPlot
#using Peacock.FDFD

# Permittivity
mu1 = [14 12.4im 0; -12.4im 14 0; 0 0 1]
function epf(x,y)
    d = 1/2.8
    r0 = 0.12
    # equation of a circle with radius 0.2a
    if (x - d)^2 + (y - d)^2 < r0^2 || (x - d)^2 + (y + d)^2 < r0^2 || (x + d)^2 + (y - d)^2 < r0^2 || (x + d)^2 + (y + d)^2 < r0^2
        #    if x^2+y^2 < r0^2
        # dielectric inside the circle
        return 15
    else
        # air outside the circle
        return 1
    end
end

# Permeability is unity everywhere
function muf(x,y)
    d = 1/2.8
    r0 = 0.12
    # equation of a circle with radius 0.2a
    if (x - d)^2 + (y - d)^2 < r0^2 || (x - d)^2 + (y + d)^2 < r0^2 || (x + d)^2 + (y - d)^2 < r0^2 || (x + d)^2 + (y + d)^2 < r0^2
        # dielectric inside the circle
        return mu1
    else
        # air outside the circle
        return 1
    end
end

a1 = [1, 0]  # first lattice vector
a2 = [0, 1]  # second lattice vector
#a2 = 0.5*[sqrt(3), 1]

d1 = 0.005  # resolution along first lattice vector
d2 = 0.005  # resolution along second lattice vector

G = BrillouinZoneCoordinate(  0,   0, "Γ")
X = BrillouinZoneCoordinate(1/2,   0, "X")
M = BrillouinZoneCoordinate(1/2, 1/2, "M")
ks = [G,X,M,G]

figure(figsize=(4,3))
geometry_FDFD = Geometry(epf, muf, a1, a2, d1, d2, TE)
solver_FDFD = Peacock.FDFD.Solver(geometry_FDFD, [2*d1, 2*d2])
plot_band_diagram(solver_FDFD, ks, TE, color="red",
            bands=1:4, dk=0.4, frequency_scale=1/2pi)

geometry_FDFD = Geometry(epf, muf, a1, a2, d1, d2, TM)
solver_FDFD = Peacock.FDFD.Solver(geometry_FDFD, [2*d1, 2*d2])
plot_band_diagram(solver_FDFD, ks, TM, color="blue",
            bands=1:4, dk=0.4, frequency_scale=1/2pi)
ylim(0,0.8)

geometry_PWE = Geometry(epf, muf, a1, a2, d1, d2)
solver_PWE =  Solver(geometry_PWE, 7)
figure(figsize=(4,3))
plot_band_diagram(solver_PWE, ks, TE, color="red",
            bands=1:4, dk=0.1, frequency_scale=1/2pi)
plot_band_diagram(solver_PWE, ks, TM, color="blue";
            bands=1:4, dk=0.1, frequency_scale=1/2pi)
ylim(0,0.8)


function kpp_psim(xi)

if xi>=0
  y=1+5*xi;
elseif (xi<0) & (xi>=-0.2)
  y=(1-16*xi)^(-1/4);
else
  y=(1.26-8.38*xi)^(-1/3);
end
return y
end


export Rect,
    Translation,
    Scaling,
    RefFrame

#----------------------------------------------------------------------
#
# Coordinate transformations
#

"""
Rectangle at (x, y) with width w and height h
"""
mutable struct Rect
    x::Float64
    y::Float64
    w::Float64
    h::Float64
end

"""
    union(r1::Rect, r2::Rect)

Compute smallest `Rect` covering `r1` and `r2`
"""
function Base.union(r1::Rect, r2::Rect)
    xmin = min(r1.x, r2.x, r1.x + r1.w, r2.x + r2.w)
    xmax = max(r1.x, r2.x, r1.x + r1.w, r2.x + r2.w)
    ymin = min(r1.y, r2.y, r1.y + r1.h, r2.y + r2.h)
    ymax = max(r1.y, r2.y, r1.y + r1.h, r2.y + r2.h)
    Rect(xmin, ymin, xmax - xmin, ymax - ymin)
end

abstract type AbstractTransform end

"""
translation by `(x, y)`
"""
# translate child by (x, y) from parent
struct Translation <: AbstractTransform
    x::Float64 # origin of user in device coords
    y::Float64
end

"""
scaling from `from` to `to`
"""
struct Scaling <: AbstractTransform
    from::Rect # device (parent)
    to::Rect   # user   (child)
end

# compute parent coords from child coords
function user2dev(t::Translation, x, y)
    x .+ t.x, y .+ t.y
end

# compute child coords from parent coords
function dev2user(t::Translation, x, y)
    x .- t.x, y .- t.y
end

function user2dev(s::Scaling, x, y)
    fromx = s.from.x .+ (x .- s.to.x) ./ s.to.w .* s.from.w
    fromy = s.from.y .+ (y .- s.to.y) ./ s.to.h .* s.from.h
    fromx, fromy
end

function dev2user(s::Scaling, x, y)
    tox = s.to.x .+ (x .- s.from.x) ./ s.from.w .* s.to.w
    toy = s.to.y .+ (x .- s.from.y) ./ s.from.h .* s.to.h
    tox, toy
end

#-------------------------------------------------------------
#
# RefFrame
#
# contains transformation into coordinate system
# and environment in `params`
#

abstract type AbstractRefFrame end

struct RefFrame <: AbstractRefFrame
    transform::AbstractTransform
    children::Vector{AbstractRefFrame}
    params::Dict

    function RefFrame(; transform = Translation(0.0, 0.0),
                      children = AbstractRefFrame[],
                      params...)
        new(transform, children, Dict(params))
    end
end

transform(ref::AbstractRefFrame) = ref.transform
params(ref::AbstractRefFrame) = ref.params

# overload coordinate transformations

user2dev(ref::AbstractRefFrame, x, y) = user2dev(transform(ref), x, y)

function user2dev(refs::Vector{T}, x, y) where T <: AbstractRefFrame
    for ref::T in reverse(refs)
        x, y = user2dev(ref, x, y)
    end
    x, y
end

function user2dev(ref::AbstractRefFrame, r::Rect)
    x, y = user2dev(transform(ref), [r.x, r.x + r.w], [r.y, r.y + r.h])
    Rect(x[1], y[1], x[2] - x[1], y[2] - y[1])
end

function user2dev(refs::Vector{T}, r::Rect) where T <: AbstractRefFrame
    for ref::T in reverse(refs)
        r = user2dev(ref, r)
    end
    r
end

dev2user(ref::AbstractRefFrame, x, y) = dev2user(transform(ref), x, y)

function dev2user(refs::Vector{T}, x, y) where T <: AbstractRefFrame
    for ref::T in refs
        x, y = dev2user(ref, x, y)
    end
    x, y
end

# resolve parameters along reference frame chain
function getparam(refs::Vector{T}, param, default = nothing; fail = false) where T <: AbstractRefFrame
    for ref::T in reverse(refs)
        if haskey(ref.params, param)
            return ref.params[param]
        end
    end
    fail && error("No such parameter: $param")
    default
end

getparam(ref::AbstractRefFrame, param, default = nothing; fail = false) = getparam([ref], param, default; fail = fail)


f(x) = x ^4
f2(x) = 4.0 * x ^ 3.0

x0 = 1

for i = 1:40
	@printf("%.4e\n",x0)
	x0 = x0 - f(x0) / f2(x0)
end


using PQEcolaPoint

# build a binary tree with three levels -
model = build(CRRLatticeModel; number_of_levels=84, σ=0.35, T=(28.0 / 365), μ=0.00174)

# compute -
a = price(model, 84; weights=true)


using JuMP
using LightGraphs, SimpleWeightedGraphs

function single_commodity_flow!(model, graph :: SimpleWeightedGraph, k :: Int)
    n::Int = nv(graph)
    es::Vector{SimpleWeightedEdge} = collect(edges(graph))
    sort!(es, by=dst)
    sort!(es, by=src, alg=MergeSort)

    @variables(model, begin
        f[i=1:n, j=1:n; has_edge(graph, i, j)], (lower_bound=0)
        z[i=2:n], (binary=true, lower_bound = 0, upper_bound = 1)
    end)

    alpha::Int = 1

    for i in 2:n
        if has_edge(graph, alpha, i)
            fix(variable_by_name(model, "y[$i,$alpha]"), 0, force=true) # Constraint (14)
            fix(f[i, alpha], 0, force=true) # Constraint (15)
            set_upper_bound(f[alpha,i], k)
        end
        for j in 2:n
            if has_edge(graph, i, j)
                set_upper_bound(f[i,j], k-1) # Constraint (16)
                set_upper_bound(f[j,i], k-1) # Constraint (16)
            end
        end
        @constraint(model, sum(variable_by_name(model, "y[$j,$i]") for j in 1:n if has_edge(graph, j, i)) == z[i]) # Constraint (17)
        #@constraint(model, sum(variable_by_name(model, "y[$i,$j]") for j in 2:n if has_edge(graph, j, i)) <= length(neighbors(graph, i)) - 1)
    end

    for i in 2:n
        @constraint(model,
        (sum(f[dst(e),i] for e in es if src(e) == i) + sum(f[src(e),i] for e in es if dst(e) == i)
        - (sum(f[i,dst(e)] for e in es if src(e) == i && dst(e) != alpha) + sum(f[i,src(e)] for e in es if dst(e) == i && src(e) != alpha)) == z[i])) # Constraint (18)
    end
    @constraint(model, source_flow, sum(f[alpha,j] for j = 2:n if has_edge(graph, alpha, j)) == k) # Constraint (19)
    @constraint(model, one_real_root, sum(variable_by_name(model, "y[$alpha,$j]") for j in 2:n if has_edge(graph, alpha, j)) == 1) # Constraint (20)

    for i in 1:n
        for j in 1:n
            if has_edge(graph, i, j)
                y_ij = variable_by_name(model, "y[$i,$j]")
                if i != 1 && j != 1
                    @constraint(model,
                        f[i,j] <= (k-1) * y_ij # Constraint (21)
                    )
                elseif i == 1
                    @constraint(model,
                        f[i,j] <= k * y_ij # Constraint (22)
                    )
                end
            end
        end
    end

    @constraint(model, sum(z[i] for i = 2:n) == k) # Constraint (23)

end


sig = [cos(2*pi*0.15*i) for i=1:2048];
@test_approx_eq_eps Estimation.quinn_fernandes(sig,2*pi*0.1)/(2*pi) 0.15 1e-5


using PowerAnalyses
using Test: @testset, @test

@testset "t-test" begin
    es = 0.5
    alpha = 0.05
    power = 0.95
    n = 50
    N = [n, n]

    # Values are obtained via `pwr` version 1.3 and `G*Power` version 3.1.9.7.
    @test get_alpha(OneSampleTTest(two_tails); es, power, n) ≈ 0.067 atol=0.001
    @test get_alpha(OneSampleTTest(one_tail); es, power, n) ≈ 0.033 atol=0.001
    @test get_power(OneSampleTTest(two_tails); es, alpha, n) ≈ 0.933 atol=0.001
    @test get_power(OneSampleTTest(one_tail); es, alpha, n) ≈ 0.967 atol=0.001

    # For methods which require `find_zero`, compare the outcome with pwr; G*Power is far off.
    @test get_es(OneSampleTTest(two_tails); alpha, power, n) ≈ 0.520 atol=0.001
    @test get_es(OneSampleTTest(one_tail); alpha, power, n) ≈ 0.471 atol=0.001
    @test get_n(OneSampleTTest(two_tails); alpha, power, es) ≈ 53.941 atol=0.001
    @test get_n(OneSampleTTest(one_tail); alpha, power, es) ≈ 44.679 atol=0.001

    # G*Power gives 0.77; they seem to calculate the noncentrality_parameter differently.
    # pwr.t.test(n=50, d=0.5, sig.level=NULL, power=0.95, type="two.sample", alternative="two.sided")
    @test get_alpha(IndependentSamplesTTest(two_tails); es, power, n) ≈ 0.392 atol=0.01
    # Same as the one sample t-test.
    @test get_alpha(DependentSamplesTTest(two_tails); es, power, n) ≈ 0.067 atol=0.01

    n_predictors = 8
    # Beware that G*Power assumes input to be f² and not f.
    @test get_alpha(DeviationFromZeroMultipleRegression(n_predictors); es=sqrt(es), power, n) ≈ 0.072 atol=0.1
    n_tested_predictors = 6
    @test get_alpha(IncreaseMultipleRegression(n_predictors, n_tested_predictors); es=sqrt(es), power, n) ≈ 0.043 atol=0.1
    n_groups = 2
    @test get_alpha(OneWayANOVA(n_groups); es, power, n) ≈ 0.067 atol=0.001
    df = 10
    @test get_alpha(MultifactorFixedEffectsANOVA(n_groups, df); es, power, n) ≈ 0.461 atol=0.01

    n_variables = 2
    @test get_alpha(ConstantVectorHotellingTsqTest(n_variables); es, power, n) ≈ 0.132 atol=0.001
    @test get_alpha(TwoVectorsHotellingTsqTest(n_variables); es, power, n=N) ≈ 0.511 atol=0.001

    df = 5
    @test get_power(GoodnessOfFitChisqTest(df); es, alpha, n) ≈ 0.787 atol=0.001
    @test get_alpha(GoodnessOfFitChisqTest(df); es, power, n) ≈ 0.253 atol=0.001
    @test get_es(GoodnessOfFitChisqTest(df); alpha, power, n) ≈ 0.629 atol=0.001
    @test get_n(GoodnessOfFitChisqTest(df); alpha, power, es) ≈ 79.12 atol=0.001

    @test get_alpha(ConstantVarianceChisqTest(one_tail); es, power, n) ≈ 0.038 atol=0.02

    # TODO: Add test for PointBiseralTTest
end


using OptiMo
using CUTEst

# problempus
problems = CUTEst.select( min_var=1, max_var=100, min_con=1, max_con=100 );
problem = problems[4]
nlp = CUTEstModel( problem )
prob = NLPOptiModel( nlp );
x0 = copy( prob.meta.x0 )

xk = x0 .+ 0.1
xp = x0 .+ 0.2
dp = min.( 1.0, 1.0 ./ abs.(xp) )
wp = 1.0

# pure feasibility
fprob_1 = FeasOptiModel( prob )

# feasibility with diff starting point
fprob_2 = FeasOptiModel( prob, x0=xk )

# feasibility with indicator
fprob_3 = FeasOptiModel( prob, with_indicator=true )

# feasibility with proximal regularization
fprob_4 = FeasOptiModel( prob, x0=xk, xprox=xp, wprox=wp )
fprob_5 = FeasOptiModel( prob, x0=xk, xprox=xp, wprox=wp, dprox=dp )

# feasibility with indicator and proximal regularization
fprob_5 = FeasOptiModel( prob, x0=xk, xprox=xp, wprox=wp, dprox=dp, with_indicator=true )

# feasibility with Huber loss
fprob_6 = FeasOptiModel( prob, x0=xk, xprox=xp, wprox=wp, with_indicator=true, loss=:huber )
fprob_7 = FeasOptiModel( prob, x0=xk, xprox=xp, wprox=wp, dprox=dp, loss=:huber, huber_rho=1e-4 )

# feasibility problem with name
fprob_8 = FeasOptiModel( prob, with_indicator=true, loss=:huber, name="myfprob" )

infea = infeasibility( fprob_8, x0 )
println( infea )

finalize( nlp )


#Fichier qui permet de créer les différents PLNE pour le problème du VRP

using JuMP
using CPLEX
#using GLPK

function createVRP_MTZ(params, nodes, demands, costs, t)

    model = Model(CPLEX.Optimizer)
    #model = Model(GLPK.Optimizer)

    #=

    m = nb de véhicule
    Q = capacité d'un véhicule

    =#

    m = params["k"]
    n = params["n"]
    Q = params["Q"]

    ####################VARIABLES#######################
    @variable(model, x[i=0:n, j=0:n], Bin) #Variable binaire x_(i,j) pour chaque arête
    for i in 0:n
        delete(model, x[i, i]) #on enlève les variables qui correspondent aux arêtes en trop (les (i, i))
    end

    @variable(model, 0 <= w[i=1:n] <= Q) #Variables w_i

    ####################CONTRAINTES####################
    @constraint(model, sum(x[0, j] for j in 1:n) <= m)#contrainte 6
    @constraint(model, sum(x[i, 0] for i in 1:n) <= m)#contrainte 7

    for i in 1:n

        nodesIndexWithoutI = filter(e -> e != i, 0:n)

        @constraint(model, sum(x[i,j] for j in nodesIndexWithoutI) == 1)#contraintes 8
        @constraint(model, sum(x[j,i] for j in nodesIndexWithoutI) == 1)#contraintes 9

    end

    for i in 1:n

        nodesIndexWithoutI = filter(e -> e != i, 1:n)
        for j in nodesIndexWithoutI
            @constraint(model, w[i] - w[j] >= demands[i, t] - (Q+demands[i, t]) * (1 - x[i, j]))#contrainte 10
        end
    end

    ####################FONCTION OBJECTIF####################
    @objective(model, Min, sum(edgeCost * x[edge[1], edge[2]] for (edge, edgeCost) in costs))

    return model

end


function dq_ri(δ::T) where {T <: Number}
    ## Uses the referenceframe of the Kundur page 852 of dq to RI
    return T[
        sin(δ) cos(δ)
        -cos(δ) sin(δ)
    ]
end

function ri_dq(δ::T) where {T <: Number}
    #Uses the reference frame of the Kundur page 852 of RI to dq
    return T[
        sin(δ) -cos(δ)
        cos(δ) sin(δ)
    ]
end

function dq_ri(δ::Float64)
    ## Uses the referenceframe of the Kundur page 852 of dq to RI
    return Float64[
        sin(δ) cos(δ)
        -cos(δ) sin(δ)
    ]
end

function ri_dq(δ::Float64)
    #Uses the reference frame of the Kundur page 852 of RI to dq
    return Float64[
        sin(δ) -cos(δ)
        cos(δ) sin(δ)
    ]
end


# constructors
export mirror_symmetric_qubit_3povm, asymmetric_qubit_3povm
export trine_qubit_povm, sic_qubit_povm
export planar_symmetric_qubit_povm

export sqrt_povm
export naimark_dilation

"""
    mirror_symmetric_qubit_3povm( θ :: Real ) :: POVM{Float64}

Constructs a `POVM` aligned with the three mirror symmetric qubit states.
The first measurement is aligned with the ``|0\\rangle`` state and the remaining
two are symmetric across the z-axis.

A `DomainError` is thrown if argument `θ` ∉ [π/4,π/2].
"""
function mirror_symmetric_qubit_3povm(θ::Real) :: POVM{Float64}
    if !( π/4 <= θ <= π/2)
        throw(DomainError(θ, "angle must be in range [π/4,π/2]"))
    end

    # POVM weights
    γ1 = 1 - cot(θ)^2
    γ2 = 0.5*(1 + cot(θ)^2)

    mirror_symmetric_qubits = mirror_symmetric_qubit_states(θ)

    π1 = γ1*mirror_symmetric_qubits[1]
    π2 = γ2*mirror_symmetric_qubits[2]
    π3 = γ2*mirror_symmetric_qubits[3]

    POVM([π1, π2, π3])
end

"""
    asymmetric_qubit_3povm( θ1::Real, θ2::Real ) :: POVM

Constructs a general non-orthogonal 3-element `POVM`.

Inputs:
* `θ1 ∈ (0,π/2] or [-π/2,0)`, the angle element 2 makes with ``|0\\rangle`` state.
* `θ2 ∈ [-π/2,0) or (0,π/2]`, the angle element 3 makes with ``|0\\rangle`` state.

A `DomainError` is thrown if `θ1` and `θ2` are not in the valid ranges. Note that
one angle must be positive and the other negative.
"""
function asymmetric_qubit_3povm(θ1::Real,θ2::Real) :: POVM{Float64}

    if !(((0 < θ1 <= π/2) && (-π/2 <= θ2 < 0)) || ((0 < θ2 <= π/2) && (-π/2 <= θ1 < 0)))
        throw(DomainError((θ1,θ2), "angles (θ1, θ2) are not in the valid range"))
    end

    # povm scaling terms derived analytically
    γ2 = 1/(sin(θ1)^2 - sin(θ1)*cos(θ1)*tan(θ2))
    γ3 = -γ2*(sin(θ1)*cos(θ1))/(sin(θ2)*cos(θ2))
    γ1 = 1 - γ2*cos(θ1)^2 - γ3*cos(θ2)^2

    π1 = γ1*[1 0;0 0]
    π2 = γ2*[cos(θ1)^2 cos(θ1)*sin(θ1); cos(θ1)*sin(θ1) sin(θ1)^2]
    π3 = γ3*[cos(θ2)^2 cos(θ2)*sin(θ2); cos(θ2)*sin(θ2) sin(θ2)^2]

    POVM([π1, π2, π3])
end

"""
    planar_symmetric_qubit_povm( n :: Int64 ) :: POVM{Float64}

Constructs an `n`-element `POVM{Float64}` from the [`planar_symmetric_qubit_states`](@ref).
Each state is multipled by a factor of `2/n` to satisfy the completeness relation.

A `DomainError` is thrown if `n ≥ 2` is not satisfied.
"""
function planar_symmetric_qubit_povm(n :: Int64) :: POVM{Float64}
    POVM(2/n*planar_symmetric_qubit_states(n))
end

"""
    trine_qubit_povm() :: POVM{Float64}

The `POVM` with elements parallel to the trine qubit states (see [`trine_qubit_kets`](@ref)).
"""
trine_qubit_povm() :: POVM{Float64} = POVM(2/3 * trine_qubit_states())

"""
    sic_qubit_povm() :: POVM{Complex{Float64}}

The `POVM` with elements parallel to the symmetric informationally complete (SIC) qubit states
(see [`sic_qubit_kets`](@ref)).
"""
sic_qubit_povm() :: POVM{Complex{Float64}} = POVM(0.5 * sic_qubit_states())

"""
    sqrt_povm(priors :: Probabilities, states :: Vector{<:State}) :: POVM

Returns the "pretty good" square-root povm for the given density operator
states and priors.
"""
function sqrt_povm(priors :: Probabilities, states :: Vector{<:State}) :: POVM
    ρ_mix = sum(priors .* states)
    ρ_sqrt = sqrt(inv(ρ_mix))

    Π = map(
        (ρ) -> ρ_sqrt * ρ * ρ_sqrt,
        (priors .* states)
    )

    POVM(Π)
end

"""
    _naimark_kraus_operators(Π::POVM) :: Array{Array{Complex{Float64},2},1}

Returns the Kraus operators for the provided POVM. In general, the Kraus operators
form a continuum and are non-unique. This method applies the construction

```math
k_i = \\sqrt{\\Pi_i}\\otimes |i\\rangle
```
"""
function _naimark_kraus_operators(Π::POVM) :: Array{Array{Complex{Float64},2},1}
    num_el = length(Π)

    map(i -> kron(sqrt(Π[i]), Matrix(I,num_el,num_el)[:,i]), 1:num_el)
end

"""
    naimark_dilation( Π::POVM )

Returns the dilated projectors which are equivalent to the provided POVM. During
measurement, the measured state must be tensored with the ancilla.
"""
function naimark_dilation(Π::POVM)
    n = length(Π)
    d = size(Π[1])[1]

    ancilla = zeros(n,n)
    ancilla[1,1] = 1

    k = _naimark_kraus_operators(Π)
    V = sum(k)

    orth_comp = nullspace(V')

    U_cols = []
    null_id = 1
    for i in 1:n*d
        if floor((i-1)/n) == (i-1)/n
            push!(U_cols,V[:,convert(Int,(i-1)/n + 1)])
        else
            push!(U_cols, orth_comp[:,null_id])
            null_id = null_id + 1
        end

    end

    U = hcat(U_cols...)

    p_ancilla = map( i -> begin
        p = zeros(n,n)
        p[i,i] = 1

        return p
    end, 1:n )

    projectors = map(i -> U'*kron(Matrix(1I,d,d),p_ancilla[i])*U, 1:n)

    Dict(
        "projectors" => projectors,
        "ancilla" => ancilla
    )
end


import StateSpaceReconstruction

@testset "Triangulation estimators" begin

	pts = invariantize([rand(3) for i = 1:20])

	TOa = transferoperator_triangulation_approx(pts, n_sample_pts = 50)
	TOe = transferoperator_triangulation_exact(pts)

	@test TOa isa TransferOperatorTriangulationApprox
	@test TOe isa TransferOperatorTriangulationExact
    @test minimum(sum(TOa.transfermatrix, dims = 2)) > 0.99
    @test minimum(sum(TOe.transfermatrix, dims = 2)) > 0.99

    # @testset "Triangulation approximate estimator" begin
    #     E = StateSpaceReconstruction.cembed([diff(rand(15)) for i = 1:3])
    #     E_invariant = invariantize(E)
    #
    #     # Triangulations
    #     triang = triangulate(E)
    #     triang_inv = triangulate(E_invariant)
    #
    #     # Transfer operators from *invariant* triangulations
    #     TO = transferoperator_triang(triang_inv)
    #     TO_approx = transferoperator_triang(triang_inv, exact = false, parallel = false)
    #     TO_approx_rand = transferoperator_triang(triang_inv, exact = false, parallel = false, sample_randomly = true)
    #     invm_approx = invariantmeasure(TO_approx)
    #     invm_approx_rand = invariantmeasure(TO_approx_rand)
    #
    #     @test all(invm_approx.dist .>=  -tol)
    #     @test all(invm_approx_rand.dist .>= -tol)
    #
    #     @test sum(invm_approx.dist) ≈ 1
    #     @test sum(invm_approx_rand.dist) ≈ 1
    # end

end


# Type
"""
	(type) Arc{T<:AnyComplex} in the complex plane

Each `Arc` type is parameterized according to the common type of its complex input arguments.
"""
struct Arc{T<:AnyComplex} <: AbstractCurve
	circle::Circle{T}
	start::Float64  # specified as positive fraction of 1 ccw rotation from positive real
	delta::Float64
end

# Untyped constructors
"""
	Arc(C,start,delta)

Consruct an arc that is the part of the Circle `C` starting at parameter value `start` and ending at `start+delta`. The values are expressed in terms of fractions of a complete circle. The `start` value should be in [0,1), and `delta` should be in [-1,1].

	Arc(a,b,c)

Construct the arc starting at point `a`, passing through `b`, and ending at `c`. If the three points are collinear, a `Segment` is returned instead.
"""
function Arc(C::Circle{T},start::Real,delta::Real) where T<:AnyComplex
	if delta < 0
		Arc{T}(reverse(C),float(-start),float(-delta))
	else
		Arc{T}(C,float(start),float(delta))
	end
end

# Construct from 3 points
function Arc(a::Number,m::Number,b::Number)
	a,m,b = promote(complex(float(a)),m,b)
	C = Circle(a,m,b)
	if isa(C,Line)  # collinear
		Segment(a,b)
	else
		α,β = a-C.center,b-C.center
		ti = mod(angle(α)/(2π),1)
		delta = mod(angle(β/α)/(2π),1) # force into (0,1)
		# which of the two circle pieces do we use?
		if !C.ccw
			ti = 1-ti
			delta = 1-delta
		end
		Arc(C,ti,delta)
	end
end

# Complex type converters
for ctype in [:Spherical,:Polar,:Complex]
	@eval begin
		function $ctype(A::Arc{T}) where T<:AnyComplex
			Arc($ctype(A.circle),A.start,A.delta)
		end
	end
end

# Required methods
function point(A::Arc,t::Real)
	s = scaleto(A.start,A.start+A.delta,t)
	point(A.circle,s)
end
arclength(A::Arc) = arclength(A.circle)*A.delta
(C::Arc)(t::Real) = point(C,t)

"""
	arg(A::Arc,z)

Find the parameter argument `t` such that `A(t)==z` is true.

This gives undefined results if `z` is not actually on the arc.
"""
function arg(A::Arc,z::Number)
	tc = arg(A.circle,z)
	t = mod(tc-A.start+DEFAULT[:tol],1)-DEFAULT[:tol]
	t/A.delta
end

unittangent(A::Arc,t::Real) = unittangent(A.circle,A.start+t*A.delta)
function tangent(A::Arc{T},t::Real) where T <: AnyComplex
	T( tangent(A.circle,A.start+t*A.delta)*A.delta )
end

+(A::Arc,z::Number) = Arc(A.circle+z,A.start,A.delta)
function -(A::Arc)
	ti = mod(A.start+0.5,1)
	Arc(-A.circle,ti,A.delta)
end

function *(A::Arc,z::Number)
	phi = angle(z)/(2*pi)
	ti = A.circle.ccw ? mod(A.start+phi,1) : mod(A.start-phi,1)
	Arc(z*A.circle,ti,A.delta)
end

"""
	inv(A)

Invert the arc `A` through the origin. In general the inverse is an `Arc`, though the result is a `Segment` if the arc's circle passes through the origin.
"""
function inv(A::Arc)
	w = 1 ./ point(A,[0,0.5,1])
	Arc(w...)
end

# Other methods
isfinite(::Arc) = true
conj(A::Arc) = Arc(conj(A(0)),conj(A(0.5)),conj(A(1)))
reverse(A::Arc) = Arc(A(1),A(0.5),A(0))

"""
	isapprox(A1::Arc,A2::Arc; tol=<default>)
	A1 ≈ A2

Determine if `A1` and `A2` represent the same arc, irrespective of the type or values of its parameters. Identity is determined by agreement within `tol`, which is interpreted as the weaker of absolute and relative differences.
"""
function isapprox(A1::Arc,A2::Arc;tol=DEFAULT[:tol])
	if isapprox(A1.circle,A2.circle,tol=tol)
		z1 = point(A1,[0,1])
		z2 = point(A2,[0,1])
		appx(u,v) = isapprox(u,v,atol=tol,rtol=tol)
		return appx(z1,z2) || appx(z1,reverse(z2))
	else
		return false
	end
end

"""
	dist(z,A::Arc)

Compute the distance from number `z` to the arc `A`.
"""
function dist(z::Number,A::Arc)
	C = A.circle
	# result depends on whether the radius to z intersects the arc
	ti,del = A.start,A.delta
	if mod(arg(C,z)-ti,1) ≤ del
		return dist(z,C)
	else
		return min( abs(z-point(A,0)), abs(z-point(A,1)) )
	end
end

"""
	closest(z,A::Arc)

Find the point on arc `A` that lies closest to `z`.
"""
function closest(z::Number,A::Arc)
	ζ = z - A.circle.center
	d = A.delta/2
	# rotate arc to position symmetric about positive Re axis
	ϕ = angle( ζ/exp(2im*pi*(A.start+d)) ) / (2π)
	if ϕ > d
		point(A,1)
	elseif ϕ < -d
		point(A,0)
	else
		A.circle.center + A.circle.radius*sign(ζ)
	end
end

function show(io::IO,A::Arc{T}) where {T}
	print(IOContext(io,:compact=>true),"Arc(",A(0.0),"...",A(1.0),")")
end
function show(io::IO,::MIME"text/plain",A::Arc{T}) where {T}
	print(io,"Arc{$T} in the complex plane:\n   fraction ",A.delta," of (",A.circle,") starting at ",A.start)
end


using MPI
using Test
using LinearAlgebra
using Random
using KernelAbstractions, StaticArrays

using ClimateMachine
using ClimateMachine.SystemSolvers
using ClimateMachine.SystemSolvers:
    band_lu_kernel!,
    band_forward_kernel!,
    band_back_kernel!,
    DGColumnBandedMatrix

import ClimateMachine.MPIStateArrays: array_device

ClimateMachine.init()
const ArrayType = ClimateMachine.array_type()

function band_to_full(B, p, q)
    _, n = size(B)

    A = similar(B, n, n) # assume square
    fill!(A, 0)
    for j in 1:n, i in max(1, j - q):min(j + p, n)
        A[i, j] = B[q + i - j + 1, j]
    end
    A
end

function run_columnwiselu_test(FT, N)

    Nq = N .+ 1
    Nq1 = Nq[1]
    Nq2 = Nq[2]
    Nqv = Nq[3]
    nstate = 3
    nvertelem = 5
    nhorzelem = 4
    eband = 2

    m = n = Nqv * nstate * nvertelem
    p = q = Nqv * nstate * eband - 1

    Random.seed!(1234)
    AB = rand(FT, Nq1, Nq2, p + q + 1, n, nhorzelem)

    AB[:, :, q + 1, :, :] .+= 10 # Make A's diagonally dominate

    Random.seed!(5678)
    b = rand(FT, Nq1, Nq2, Nqv, nstate, nvertelem, nhorzelem)
    x = similar(b)

    perm = (4, 3, 5, 1, 2, 6)
    bp = reshape(PermutedDimsArray(b, perm), n, Nq1, Nq2, nhorzelem)
    xp = reshape(PermutedDimsArray(x, perm), n, Nq1, Nq2, nhorzelem)

    d_F = ArrayType(AB)
    d_F = DGColumnBandedMatrix{
        3,
        N,
        nstate,
        nhorzelem,
        nvertelem,
        eband,
        false,
        typeof(d_F),
    }(
        d_F,
    )

    groupsize = (Nq1, Nq2)
    ndrange = (Nq1, Nq2, nhorzelem)

    event = Event(array_device(d_F.data))
    event = band_lu_kernel!(array_device(d_F.data), groupsize, ndrange)(
        d_F,
        dependencies = (event,),
    )
    wait(array_device(d_F.data), event)

    F = Array(d_F.data)

    for h in 1:nhorzelem, j in 1:Nq2, i in 1:Nq1
        B = AB[i, j, :, :, h]
        G = band_to_full(B, p, q)
        GLU = lu!(G, Val(false))

        H = band_to_full(F[i, j, :, :, h], p, q)

        @assert H ≈ G

        xp[:, i, j, h] .= GLU \ bp[:, i, j, h]
    end

    b = reshape(b, Nq1 * Nq2 * Nqv, nstate, nvertelem * nhorzelem)
    x = reshape(x, Nq1 * Nq2 * Nqv, nstate, nvertelem * nhorzelem)

    d_x = ArrayType(b)

    event = Event(array_device(d_x))
    event = band_forward_kernel!(array_device(d_x), groupsize, ndrange)(
        d_x,
        d_F,
        dependencies = (event,),
    )

    event = band_back_kernel!(array_device(d_x), groupsize, ndrange)(
        d_x,
        d_F,
        dependencies = (event,),
    )
    wait(array_device(d_x), event)

    result = x ≈ Array(d_x)
    return result
end

@testset "Columnwise LU test" begin
    for FT in (Float64, Float32)
        for N in ((1, 1, 1), (1, 1, 2), (2, 2, 1))
            result = run_columnwiselu_test(FT, N)
            @test result
        end
    end
end


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

function _fit_curve(
    pts::AbstractMatrix{Float64}, # 2×n
    desired_distance_between_samples::Real;
    max_iterations::Int=50,
    epsilon::Float64=1e-4,
    n_intervals_in_arclen::Int=100,
    )

    @assert(size(pts, 1) == 2)

    spline_coeffs = fit_cubic_spline(pts)

    L = calc_curve_length(spline_coeffs[1], spline_coeffs[2], n_intervals_per_segment=n_intervals_in_arclen)
    n = round(Int, L/desired_distance_between_samples, RoundNearestTiesUp)+1

    s_arr = collect(linspace(0.0,L,n))
    t_arr = calc_curve_param_given_arclen(spline_coeffs[1], spline_coeffs[2], s_arr,
        curve_length=L, max_iterations=max_iterations, epsilon=epsilon, n_intervals_in_arclen=n_intervals_in_arclen)

    x_arr = sample_spline(spline_coeffs[1], t_arr)
    y_arr = sample_spline(spline_coeffs[2], t_arr)
    θ_arr = sample_spline_theta(spline_coeffs[1], spline_coeffs[2], t_arr)

    κ_arr = sample_spline_curvature(spline_coeffs[1], spline_coeffs[2], t_arr)
    κd_arr = sample_spline_derivative_of_curvature(spline_coeffs[1], spline_coeffs[2], t_arr)

    @assert(!any(s->isnan(s), s_arr))
    @assert(!any(s->isnan(s), x_arr))
    @assert(!any(s->isnan(s), y_arr))
    @assert(!any(s->isnan(s), θ_arr))

    curve = Array{CurvePt}(undef, n)
    for i in 1 : n
        pos = VecSE2(x_arr[i], y_arr[i], θ_arr[i])
        curve[i] = CurvePt(pos, s_arr[i], κ_arr[i], κd_arr[i])
    end
    curve
end

"""
    read_dxf(io::IO, ::Type{Roadway})
Return a Roadway generated from a DXF file

    Layers with names such as seg001 will contain LWPOLYLINEs.
    Each LWPOLYLINE corresponds to a lane centerline, which together
    are all neighbored.
"""
function read_dxf(io::IO, ::Type{Roadway};
    dist_threshold_lane_connect::Float64 = 0.25, # [m]
    desired_distance_between_curve_samples::Float64 = 1.0 # [m]
    )

    lines = readlines(io)

    i = findfirst(isequal("ENTITIES\n"), lines)
    i != nothing || error("ENTITIES section not found")

    ###################################################
    # Pull pts for each lane
    lane_pts_dict = Dict{LaneTag, Vector{VecE2}}()

    i = findnext(lines, "LWPOLYLINE\n", i)
    while i != 0
        i = findnext(lines, "  8\n", i)
        if i != 0 # segment identifier found in LWPOLYLINE

            if ismatch(r"(?<=seg)(\d*)", lines[i+1])
                segid = parse(Int, match(r"(?<=seg)(\d*)", lines[i+1]).match)

                    i = findnext(lines, "AcDbPolyline\n", i)
                i != 0 || error("AcDbPolyline not found in LWPOLYLINE!")
                i = findnext(lines, " 90\n", i)
                i != 0 || error("Number of vertices not found in AcDbPolyline!")

                N = parse(Int, lines[i+1])
                N > 0 || error("Empty line segment!")

                pts = Array{VecE2}(undef, N)

                i = findnext(lines, " 10\n", i)
                i != 0 || error("First point not found in AcDbPolyline!")

                for j in 1 : N
                    x = parse(Float64, lines[i+1])
                    y = parse(Float64, lines[i+3])
                    i += 4
                    pts[j] = VecE2(x,y)
                end

                laneid = 1
                for tag in keys(lane_pts_dict)
                    if tag.segment == segid
                        laneid += 1
                    end
                end
                lane_pts_dict[LaneTag(segid, laneid)] = pts
            end

            i = findnext(lines, "LWPOLYLINE\n", i)
        end
    end

    ###################################################
    # Shift pts to connect to previous / next pts
    lane_next_dict = Dict{LaneTag, Tuple{VecE2, LaneTag}}()
    lane_prev_dict = Dict{LaneTag, Tuple{VecE2, LaneTag}}()

    for (tag, pts) in lane_pts_dict
        # see if can connect to next
        best_tag = NULL_LANETAG
        best_ind = -1
        best_sq_dist = dist_threshold_lane_connect
        for (tag2, pts2) in lane_pts_dict
            if tag2.segment != tag.segment
                for (ind,pt) in enumerate(pts2)
                    sq_dist = normsquared(VecE2(pt - pts[end]))
                    if sq_dist < best_sq_dist
                        best_sq_dist = sq_dist
                        best_ind = ind
                        best_tag = tag2
                    end
                end
            end
        end
        if best_tag != NULL_LANETAG
            # remove our last pt and set next to pt to their pt
            pop!(pts)
            lane_next_dict[tag] = (lane_pts_dict[best_tag][best_ind], best_tag)
            if best_ind == 1 # set connect prev as well
                lane_prev_dict[best_tag] = (pts[end], tag)
            end
        end
    end
    for (tag, pts) in lane_pts_dict
        # see if can connect to prev
        if !haskey(lane_prev_dict, tag)
            best_tag = NULL_LANETAG
            best_ind = -1
            best_sq_dist = dist_threshold_lane_connect
            for (tag2, pts2) in lane_pts_dict
                if tag2.segment != tag.segment
                    for (ind,pt) in enumerate(pts2)
                        sq_dist = normsquared(VecE2(pt - pts[1]))
                        if sq_dist < best_sq_dist
                            best_sq_dist = sq_dist
                            best_ind = ind
                            best_tag = tag2
                        end
                    end
                end
            end
            if best_tag != NULL_LANETAG
                # connect 'em
                shift!(pts)
                lane_prev_dict[tag] = (lane_pts_dict[best_tag][best_ind], best_tag)
            end
        end
    end

    ###################################################
    # Build the roadway
    retval = Roadway()
    for (tag, pts) in lane_pts_dict
        if !has_segment(retval, tag.segment)
            push!(retval.segments, RoadSegment(tag.segment))
        end
    end

    lane_new_dict = Dict{LaneTag, LaneTag}() # old -> new tag
    for seg in retval.segments

        # pull lanetags for this seg
        lanetags = LaneTag[]
        for tag in keys(lane_pts_dict)
            if tag.segment == seg.id
                push!(lanetags, tag)
            end
        end

        # sort the lanes such that the rightmost lane is lane 1
        # do this by taking the first lane,
        # then project each lane's midpoint to the perpendicular at the midpoint

        @assert(!isempty(lanetags))
        proj_positions = Array{Float64}(undef, length(lanetags))

        first_lane_pts = lane_pts_dict[lanetags[1]]
        n = length(first_lane_pts)
        lo = first_lane_pts[div(n,2)]
        hi = first_lane_pts[div(n,2)+1]
        midpt_orig = (lo + hi)/2
        dir = polar(1.0, atan(hi - lo) + π/2) # direction perpendicular (left) of lane

        for (i,tag) in enumerate(lanetags)
            pts = lane_pts_dict[tag]
            n = length(pts)
            midpt = (pts[div(n,2)] + pts[div(n,2)+1])/2
            proj_positions[i] = proj(midpt - midpt_orig, dir, Float64)
        end

        for (i,j) in enumerate(sortperm(proj_positions))

            tag = lanetags[j]
            boundary_left = i == length(proj_positions) ? LaneBoundary(:solid, :white) : LaneBoundary(:broken, :white)
            boundary_right = i == 1 ? LaneBoundary(:solid, :white) : LaneBoundary(:broken, :white)

            pts = lane_pts_dict[tag]
            pt_matrix = Array{Float64}(undef, 2, length(pts))
            for (k,P) in enumerate(pts)
                pt_matrix[1,k] = P.x
                pt_matrix[2,k] = P.y
            end

            println("fitting curve ", length(pts), "  "); tic()
            curve = _fit_curve(pt_matrix, desired_distance_between_curve_samples)
            toc()

            tag_new = LaneTag(seg.id, length(seg.lanes)+1)
            lane = Lane(tag_new, curve,
                        boundary_left = boundary_left,
                        boundary_right = boundary_right)
            push!(seg.lanes, lane)
            lane_new_dict[tag] = tag_new
        end
    end

    ###################################################
    # Connect the lanes
    for (tag_old, tup) in lane_next_dict
        next_pt, next_tag_old = tup
        lane = retval[lane_new_dict[tag_old]]
        next_tag_new = lane_new_dict[next_tag_old]
        dest = retval[next_tag_new]
        roadproj = proj(VecSE2(next_pt, 0.0), dest, retval)

        # println("connecting $(lane.tag) to $(dest.tag)")

        cindS = curveindex_end(lane.curve)
        cindD = roadproj.curveproj.ind

        if cindD == CURVEINDEX_START # a standard connection
            connect!(lane, dest)
            # remove any similar connection from lane_prev_dict
            if haskey(lane_prev_dict, next_tag_old) && lane_prev_dict[next_tag_old][2] == tag_old
                delete!(lane_prev_dict, next_tag_old)
            end
        else
            # otherwise connect as before
            pushfirst!(lane.exits,  LaneConnection(true,  cindS, RoadIndex(cindD, dest.tag)))
            push!(dest.entrances, LaneConnection(false, cindD, RoadIndex(cindS, lane.tag)))
        end
    end
    for (tag_old, tup) in lane_prev_dict
        prev_pt, prev_tag_old = tup
        lane = retval[lane_new_dict[tag_old]]
        prev_tag_new = lane_new_dict[prev_tag_old]
        prev = retval[prev_tag_new]
        roadproj = proj(VecSE2(prev_pt, 0.0), prev, retval)

        # println("connecting $(lane.tag) from $(prev.tag)")

        cindS = roadproj.curveproj.ind
        cindD = CURVEINDEX_START

        if cindS == curveindex_end(prev) # a standard connection
            @assert(!has_prev(prev))
            connect!(prev, lane)
        else
            # a standard connection
            push!(prev.exits,  LaneConnection(true,  cindS, RoadIndex(cindD, lane.tag)))
            pushfirst!(lane.entrances, LaneConnection(false, cindD, RoadIndex(cindS, prev.tag)))
        end
    end

    retval
end


using Random
Random.seed!(172)
using LinearAlgebra: Matrix
using CompressingSolvers
using SparseArrays
using LinearAlgebra
using MKLSparse

# Setting up the test domains
ρ = 4.0
# ρ = Inf
q = 4
# A, coarse_domains, scales, basis_functions, multicolor_ordering, fine_domains, tree_function = CompressingSolvers.FD_Laplacian_subdivision_2d(q, ρ)
α(x) = rand()
A, coarse_domains, scales, basis_functions, multicolor_ordering, fine_domains, tree_function = CompressingSolvers.FD_periodic_Laplacian_subdivision_2d(q, ρ, α)

measurement_matrix = CompressingSolvers.form_measurement_matrix(multicolor_ordering)
@show size(measurement_matrix)
@time measurement_results = CompressingSolvers.measure(cholesky(A), measurement_matrix)

@time L = CompressingSolvers.reconstruct(multicolor_ordering, CompressingSolvers.center.(fine_domains), measurement_matrix, measurement_results, tree_function)

# @show opnorm(inv(Matrix(A)) - L * L') / opnorm(inv(Matrix(A)))
@show CompressingSolvers.compute_relative_error(L, cholesky(A), 200, 200)
# 𝐅 = CompressingSolvers.SupernodalFactorization(multicolor_ordering, domain_supernodes);
#
# 𝐌 = CompressingSolvers.create_measurement_matrix(multicolor_ordering, 𝐅.row_supernodes);
#
# # 𝐎 = CompressingSolvers.measure(inv(Matrix(A)), 𝐌, 𝐅.row_supernodes);
# @time 𝐎 = CompressingSolvers.measure(cholesky(A), 𝐌, 𝐅.row_supernodes);
#
# 𝐇 = hcat(SparseMatrixCSC.(vcat(𝐅.column_supernodes...))...);
#
# @time CompressingSolvers.reconstruct!(𝐅, 𝐎, 𝐌, multicolor_ordering);
# L = SparseMatrixCSC(𝐅)
#
# # @show norm(L * L' - inv(Matrix(A))) / norm(inv(Matrix(A)))
# @show length(multicolor_ordering)
# @show length(reduce(vcat, multicolor_ordering))
# @show CompressingSolvers.compute_relative_error(L, cholesky(A))


# This file is part of IntegerSequences.
# Copyright Peter Luschny. License is MIT.

(@__DIR__) ∉ LOAD_PATH && push!(LOAD_PATH, (@__DIR__))

module SelfConvolutive

using Nemo

export ModuleSelfConvolutive, SelfConvRec
export L000698, L001710, L003319, L005411, L005412, L006012, L006318
export L047891, L062980, L082298, L082301, L082302, L105523, L107716
export L111529, L111530, L111531, L111532, L111533, L146559, L167872

"""

Some exactly solvable self-convolutive recurrences.

* SelfConvRec, L000698, L001710, L003319, L005411, L005412, L006012, L006318, L047891, L062980, L082298, L082301, L082302, L105523, L107716, L111529, L111530, L111531, L111532, L111533, L146559, L167872
"""
const ModuleSelfConvolutive = ""

# R. J. Martin and M. J. Kearney,
# An exactly solvable self-convolutive recurrence
# http://arXiv.org/abs/1103.4936
# Integral representation of certain combinatorial recurrences
# see also http://oeis.org/A172455

Convolution(A, n) = sum(A[j+1] * A[n-j+1] for j ∈ 0:n)

"""

An exactly solvable self-convolutive recurrence studied by R. J. Martin and M. J. Kearney.
"""
function SelfConvRec(len, a, b, c)
    A = fill(fmpz(0), len)
    len == 0 && return A
    A[1] = 1

    for n ∈ 1:len-1
        A[n+1] = (a * (n + 1) + b) * A[n] + c * Convolution(A, n - 1)
    end
    A
end

# """
# Return the powers of 2.
# """
# L000079(len::Int) = SelfConvRec(len, 1, 1, -1)
# """
# Return the Catalan numbers.
# """
# L000108(len::Int) = SelfConvRec(len, 0, 0, 1)
# """
# Return the factorial numbers.
# """
# L000142(len::Int) = SelfConvRec(len, 1, -1, 0)
# """
# Return the powers of 3.
# """
# L000244(len::Int) = SelfConvRec(len, 2, 1, -2)
# """
# Return the powers of 5.
# """
# L000351(len::Int) = SelfConvRec(len, 4, 1, -4)
# """
# Return the powers of 6.
# """
# L000400(len::Int) = SelfConvRec(len, 5, 1, -5)
# """
# Return the powers of 7.
# """
# L000420(len::Int) = SelfConvRec(len, 6, 1, -6)

"""

Return the number of indecomposable perfect matchings on ``[2n]``.
"""
L000698(len::Int) = SelfConvRec(len, 2, -3, 1)
"""

Return the order of alternating group ``A_n``, or number of even permutations of ``n`` letters.
"""
L001710(len::Int) = SelfConvRec(len, 1, 1, 0)

# """
# Return n!/3!.
# """
# L001715(len::Int) = SelfConvRec(len, 1, 2, 0)
# """
# Return n!/4!.
# """
# L001720(len::Int) = SelfConvRec(len, 1, 3, 0)
# """
# Return n!/5!.
# """
# L001725(len::Int) = SelfConvRec(len, 1, 4, 0)
# """
# Return n!/6!.
# """
# L001730(len::Int) = SelfConvRec(len, 1, 5, 0)

"""

Return the number of connected permutations of ``[1..n]``. Also called indecomposable permutations.
"""
L003319(len::Int) = SelfConvRec(len, 1, -2, 1)
"""

Return the number of non-vanishing Feynman diagrams of order ``2n`` for the electron or the photon propagators in quantum electrodynamics.
"""
L005411(len::Int) = SelfConvRec(len, 2, -4, 1)
"""

Return the number of non-vanishing Feynman diagrams of order ``2n`` for the vacuum polarization in quantum electrodynamics.
"""
L005412(len::Int) = SelfConvRec(len, 2, -2, 1)
"""

Counting some sets of permutations.
"""
L006012(len::Int) = SelfConvRec(len, -1, 2, 2)
"""

Return the large Schröder numbers.
"""
L006318(len::Int) = SelfConvRec(len, 0, 1, 1)
"""

Return the number of planar rooted trees with ``n`` nodes and tricolored end nodes.
"""
L047891(len::Int) = SelfConvRec(len, 0, 2, 1)
# """
# Return (n+7)!/7!.
# """
# L049388(len::Int) = SelfConvRec(len, 1, 6, 0)
# """
# Return (3*n+4)!!!/4!!!.
# """
# L051604(len::Int) = SelfConvRec(len, 3, 1, 0)
# """
# Return (3*n+5)!!!/5!!!.
# """
# L051605(len::Int) = SelfConvRec(len, 3, 2, 0)
# """
# Return (3*n+6)!!!/6!!!.
# """
# L051606(len::Int) = SelfConvRec(len, 3, 3, 0)
# """
# Return (3*n+7)!!!/7!!!.
# """
# L051607(len::Int) = SelfConvRec(len, 3, 4, 0)
# """
# Return (3*n+8)!!!/8!!!.
# """
# L051608(len::Int) = SelfConvRec(len, 3, 5, 0)
# """
# Return (3*n+9)!!!/9!!!.
# """
# L051609(len::Int) = SelfConvRec(len, 3, 6, 0)
# """
# Return (4*n+5)(!^4)/5(!^4).
# """
# L051617(len::Int) = SelfConvRec(len, 4, 1, 0)
# """
# Return  (4*n+6)(!^4)/6(!^4).
# """
# L051618(len::Int) = SelfConvRec(len, 4, 2, 0)
# """
# Return (4*n+7)(!^4)/7(!^4).
# """
# L051619(len::Int) = SelfConvRec(len, 4, 3, 0)
# """
# Return (4*n+8)(!^4)/8(!^4).
# """
# L051620(len::Int) = SelfConvRec(len, 4, 4, 0)
# """
# Return (4*n+9)(!^4)/9(!^4).
# """
# L051621(len::Int) = SelfConvRec(len, 4, 5, 0)
# """
# Return (4*n+10)(!^4)/10(!^4).
# """
# L051622(len::Int) = SelfConvRec(len, 4, 6, 0)
# """
# Return (5*n+6)(!^5)/6.
# """
# L051687(len::Int) = SelfConvRec(len, 5, 1, 0)
# """
# Return (5*n+7)(!^5)/7(!^5).
# """
# L051688(len::Int) = SelfConvRec(len, 5, 2, 0)
# """
# Return (5*n+8)(!^5)/8(!^5).
# """
# L051689(len::Int) = SelfConvRec(len, 5, 3, 0)
# """
# Return (5*n+9)(!^5)/9(!^5).
# """
# L051690(len::Int) = SelfConvRec(len, 5, 4, 0)
# """
# Return (5*n+10)(!^5)/10(!^5).
# """
# L051691(len::Int) = SelfConvRec(len, 5, 5, 0)
# """
# Return ((6*n+7)(!^6))/7.
# """
# L053100(len::Int) = SelfConvRec(len, 6, 1, 0)
# """
# Return ((6*n+8)(!^6))/8(!^6).
# """
# L053101(len::Int) = SelfConvRec(len, 6, 2, 0)
# """
# Return ((6*n+9)(!^6))/9(!^6).
# """
# L053102(len::Int) = SelfConvRec(len, 6, 3, 0)
# """
# Return ((6*n+10)(!^6))/10(!^6).
# """
# L053103(len::Int) = SelfConvRec(len, 6, 4, 0)
# """
# Return ((7*n+8)(!^7))/8.
# """
# L053104(len::Int) = SelfConvRec(len, 7, 1, 0)
# """
# Return ((7*n+9)(!^7))/9(!^7).
# """
# L053105(len::Int) = SelfConvRec(len, 7, 2, 0)
# """
# Return ((7*n+10)(!^7))/10(1^7).
# """
# L053106(len::Int) = SelfConvRec(len, 7, 3, 0)

"""

Return the number of rooted unlabeled connected triangular maps on a compact closed oriented surface with ``2n`` faces.
"""
L062980(len::Int) = SelfConvRec(len, 6, -8, 1)
"""

Return the number of lattice paths from ``(0,0)`` to ``(n+1,n+1)`` that consist of steps ``(i,0)`` and ``(0,j)`` with ``i,j≥1`` and stay strictly below the diagonal line ``y=x`` except at the endpoints.
"""
L082298(len::Int) = SelfConvRec(len, 0, 3, 1)
"""

Return the number of Schröder paths of semilength n in which the (2,0)-steps come in 4 colors.
"""
L082301(len::Int) = SelfConvRec(len, 0, 4, 1)
"""

Return the coefficients in the expansion of ``(1-5x-√(25x^2-14x+1))/(2x)``.
"""
L082302(len::Int) = SelfConvRec(len, 0, 5, 1)

# """
# Return the coefficients in the expansion of ``(1-6x-√(36x^2-16x+1))/(2x)``.
# """
# L082305(len::Int) = SelfConvRec(len, 0, 6, 1)
# """
# Return the coefficients in the expansion of ``(1-7x-√(49x^2-18x+1))/(2x)``.
# """
# L082366(len::Int) = SelfConvRec(len, 0, 7, 1)
# """
# Return the coefficients in the expansion of ``(1-8x-√(64x^2-20x+1))/(2*x)``.
# """
# L082367(len::Int) = SelfConvRec(len, 0, 8, 1)

"""

Return the coefficients in the expansion of ``(1+2x-√(1+4x^2))/(2x)``.
"""
L105523(len::Int) = SelfConvRec(len, 0, -2, 1)
"""

A transform the of triple factorial numbers.
"""
L107716(len::Int) = SelfConvRec(len, 3, -4, 1)
"""

A convolutory inverse of the factorial sequence.
"""
L111529(len::Int) = SelfConvRec(len, 1, -3, 2)
"""

A convolutory inverse of the factorial sequence.
"""
L111530(len::Int) = SelfConvRec(len, 1, -4, 3)
"""

A convolutory inverse of the factorial sequence.
"""
L111531(len::Int) = SelfConvRec(len, 1, -5, 4)
"""

A convolutory inverse of the factorial sequence.
"""
L111532(len::Int) = SelfConvRec(len, 1, -6, 5)
"""

A convolutory inverse of the factorial sequence.
"""
L111533(len::Int) = SelfConvRec(len, 1, -7, 6)

#"""
#"""
#L111546(len::Int) = SelfConvRec(len, 1, 0, 1)
#"""
#"""
#L111556(len::Int) = SelfConvRec(len, 1, 1, 1)

"""

Return the expansion of ``(1-x)/(1 - 2x + 2x^2)``.
"""
L146559(len::Int) = SelfConvRec(len, 1, 1, -2)
"""

Return half the number of Feynman diagrams of order ``2(n+1)``, for the electron self-energy in quantum electrodynamics.
"""
L167872(len::Int) = SelfConvRec(len, 2, -3, 2)

# """
# """
# L172450(len::Int) = SelfConvRec(len, 2, 0, -1)
# """
# """
# L172455(len::Int) = SelfConvRec(len, 6, -4, -1)
# """
# """
# L172485(len::Int) = SelfConvRec(len, -1, -2, 3)
# """
# """
# L177354(len::Int) = SelfConvRec(len, 1, 2, 1)

#START-TEST-########################################################
using Test, SeqTests

function test()
    @testset "SelfConvRec" begin
        if data_installed()

            L = [
                L005411,
                L005412,
                L006012,
                L006318,
                L047891,
                L062980,
                L082298,
                L082301,
                L082302,
                L105523,
                L107716,
                L111529,
                L111530,
                L111531,
                L111532,
                L111533,
                L146559,
                L167872
            ]
            SeqTest(L, 'L')
        end
    end
end

function demo()

    for n ∈ 0:6
        println(n, " ↦ ", L005411(n))
    end

    f(len) = SelfConvRec(len, 2, -1, -1)
    println(f(11))
end

"""

L005411(500) :: 0.238653 seconds (252.99 k allocations: 3.887 MB)
"""
function perf()
    GC.gc()
    @time L005411(500)
end

function main()
    test()
    demo()
    perf()
end

main()

end # module

#=
L --> A000079
fmpz[1, 2, 4, 8, 16, 32]
fmpz[1, 2, 4, 8, 16, 32]
L --> A000108
fmpz[1, 1, 2, 5, 14, 42]
fmpz[1, 1, 2, 5, 14, 42]
L --> A000142
fmpz[1, 1, 2, 6, 24, 120]
fmpz[1, 1, 2, 6, 24, 120]
L --> A000244
fmpz[1, 3, 9, 27, 81, 243]
fmpz[1, 3, 9, 27, 81, 243]
L --> A000351
fmpz[1, 5, 25, 125, 625, 3125]
fmpz[1, 5, 25, 125, 625, 3125]
L --> A000400
fmpz[1, 6, 36, 216, 1296, 7776]
fmpz[1, 6, 36, 216, 1296, 7776]
L --> A000420
fmpz[1, 7, 49, 343, 2401, 16807]
fmpz[1, 7, 49, 343, 2401, 16807]
#############################
L --> A000698
fmpz[1, 1, 2, 10, 74, 706]
fmpz[1, 2, 10, 74, 706, 8162]
#####################
seq((n+2)!/2, n=0..9);
L --> A001710
fmpz[1, 1, 1, 3, 12, 60]
fmpz[1, 3, 12, 60, 360, 2520]
######################
L --> A001715
fmpz[1, 4, 20, 120, 840, 6720]
fmpz[1, 4, 20, 120, 840, 6720]
L --> A001720
fmpz[1, 5, 30, 210, 1680, 15120]
fmpz[1, 5, 30, 210, 1680, 15120]
L --> A001725
fmpz[1, 6, 42, 336, 3024, 30240]
fmpz[1, 6, 42, 336, 3024, 30240]
L --> A001730
fmpz[1, 7, 56, 504, 5040, 55440]
fmpz[1, 7, 56, 504, 5040, 55440]
##################
L --> A003319
fmpz[0, 1, 1, 3, 13, 71]
fmpz[1, 1, 3, 13, 71, 461]
##################
L --> A005411
fmpz[1, 1, 4, 25, 208, 2146]
fmpz[1, 1, 4, 25, 208, 2146]
L --> A005412
fmpz[1, 3, 18, 153, 1638, 20898]
fmpz[1, 3, 18, 153, 1638, 20898]
L --> A006012
fmpz[1, 2, 6, 20, 68, 232]
fmpz[1, 2, 6, 20, 68, 232]
L --> A006318
fmpz[1, 2, 6, 22, 90, 394]
fmpz[1, 2, 6, 22, 90, 394]
L --> A047891
fmpz[1, 3, 12, 57, 300, 1686]
fmpz[1, 3, 12, 57, 300, 1686]
L --> A049388
fmpz[1, 8, 72, 720, 7920, 95040]
fmpz[1, 8, 72, 720, 7920, 95040]
L --> A051604
fmpz[1, 7, 70, 910, 14560, 276640]
fmpz[1, 7, 70, 910, 14560, 276640]
L --> A051605
fmpz[1, 8, 88, 1232, 20944, 418880]
fmpz[1, 8, 88, 1232, 20944, 418880]
L --> A051606
fmpz[1, 9, 108, 1620, 29160, 612360]
fmpz[1, 9, 108, 1620, 29160, 612360]
L --> A051607
fmpz[1, 10, 130, 2080, 39520, 869440]
fmpz[1, 10, 130, 2080, 39520, 869440]
L --> A051608
fmpz[1, 11, 154, 2618, 52360, 1204280]
fmpz[1, 11, 154, 2618, 52360, 1204280]
L --> A051609
fmpz[1, 12, 180, 3240, 68040, 1632960]
fmpz[1, 12, 180, 3240, 68040, 1632960]
L --> A051617
fmpz[1, 9, 117, 1989, 41769, 1044225]
fmpz[1, 9, 117, 1989, 41769, 1044225]
L --> A051618
fmpz[1, 10, 140, 2520, 55440, 1441440]
fmpz[1, 10, 140, 2520, 55440, 1441440]
L --> A051619
fmpz[1, 11, 165, 3135, 72105, 1946835]
fmpz[1, 11, 165, 3135, 72105, 1946835]
L --> A051620
fmpz[1, 12, 192, 3840, 92160, 2580480]
fmpz[1, 12, 192, 3840, 92160, 2580480]
L --> A051621
fmpz[1, 13, 221, 4641, 116025, 3364725]
fmpz[1, 13, 221, 4641, 116025, 3364725]
L --> A051622
fmpz[1, 14, 252, 5544, 144144, 4324320]
fmpz[1, 14, 252, 5544, 144144, 4324320]
L --> A051687
fmpz[1, 11, 176, 3696, 96096, 2978976]
fmpz[1, 11, 176, 3696, 96096, 2978976]
L --> A051688
fmpz[1, 12, 204, 4488, 121176, 3877632]
fmpz[1, 12, 204, 4488, 121176, 3877632]
L --> A051689
fmpz[1, 13, 234, 5382, 150696, 4972968]
fmpz[1, 13, 234, 5382, 150696, 4972968]
L --> A051690
fmpz[1, 14, 266, 6384, 185136, 6294624]
fmpz[1, 14, 266, 6384, 185136, 6294624]
L --> A051691
fmpz[1, 15, 300, 7500, 225000, 7875000]
fmpz[1, 15, 300, 7500, 225000, 7875000]
L --> A053100
fmpz[1, 13, 247, 6175, 191425, 7082725]
fmpz[1, 13, 247, 6175, 191425, 7082725]
L --> A053101
fmpz[1, 14, 280, 7280, 232960, 8852480]
fmpz[1, 14, 280, 7280, 232960, 8852480]
L --> A053102
fmpz[1, 15, 315, 8505, 280665, 10945935]
fmpz[1, 15, 315, 8505, 280665, 10945935]
L --> A053103
fmpz[1, 16, 352, 9856, 335104, 13404160]
fmpz[1, 16, 352, 9856, 335104, 13404160]
L --> A053104
fmpz[1, 15, 330, 9570, 344520, 14814360]
fmpz[1, 15, 330, 9570, 344520, 14814360]
L --> A053105
fmpz[1, 16, 368, 11040, 408480, 17973120]
fmpz[1, 16, 368, 11040, 408480, 17973120]
L --> A053106
fmpz[1, 17, 408, 12648, 480624, 21628080]
fmpz[1, 17, 408, 12648, 480624, 21628080]
L --> A062980
fmpz[1, 5, 60, 1105, 27120, 828250]
fmpz[1, 5, 60, 1105, 27120, 828250]
L --> A082298
fmpz[1, 4, 20, 116, 740, 5028]
fmpz[1, 4, 20, 116, 740, 5028]
L --> A082301
fmpz[1, 5, 30, 205, 1530, 12130]
fmpz[1, 5, 30, 205, 1530, 12130]
L --> A082302
fmpz[1, 6, 42, 330, 2814, 25422]
fmpz[1, 6, 42, 330, 2814, 25422]
L --> A082305
fmpz[1, 7, 56, 497, 4760, 48174]
fmpz[1, 7, 56, 497, 4760, 48174]
L --> A082366
fmpz[1, 8, 72, 712, 7560, 84616]
fmpz[1, 8, 72, 712, 7560, 84616]
L --> A082367
fmpz[1, 9, 90, 981, 11430, 140058]
fmpz[1, 9, 90, 981, 11430, 140058]
L --> A105523
fmpz[1, -1, 0, 1, 0, -2]
fmpz[1, -1, 0, 1, 0, -2]
L --> A107716
fmpz[1, 3, 21, 219, 2973, 49323]
fmpz[1, 3, 21, 219, 2973, 49323]
L --> A111529
fmpz[1, 1, 4, 22, 148, 1156]
fmpz[1, 1, 4, 22, 148, 1156]
L --> A111530
fmpz[1, 1, 5, 33, 261, 2361]
fmpz[1, 1, 5, 33, 261, 2361]
L --> A111531
fmpz[1, 1, 6, 46, 416, 4256]
fmpz[1, 1, 6, 46, 416, 4256]
L --> A111532
fmpz[1, 1, 7, 61, 619, 7045]
fmpz[1, 1, 7, 61, 619, 7045]
L --> A111533
fmpz[1, 1, 8, 78, 876, 10956]
fmpz[1, 1, 8, 78, 876, 10956]
L --> A111546
fmpz[1, 3, 15, 99, 783, 7083]
fmpz[1, 3, 15, 99, 783, 7083]
L --> A111556
fmpz[1, 4, 24, 184, 1664, 17024]
fmpz[1, 4, 24, 184, 1664, 17024]
L --> A146559
fmpz[1, 1, 0, -2, -4, -4]
fmpz[1, 1, 0, -2, -4, -4]
L --> A167872
fmpz[1, 3, 21, 207, 2529, 36243]
fmpz[1, 3, 21, 207, 2529, 36243]
L --> A172450
fmpz[1, 3, 12, 63, 432, 3798]
fmpz[1, 3, 12, 63, 432, 3798]
L --> A172485
fmpz[1, -1, -1, 3, 3, -21]
fmpz[1, -1, -1, 3, 3, -21]
#################################
L --> A177354
fmpz[5, 35, 305, 3095, 35225, 439775]
fmpz[1, 5, 35, 305, 3095, 35225]
###########################

L = [ L000079, L000108, L000142, L000244, L000351, L000400,
 L000420, L001715, L001720, L001725, L001730,
 L005411, L005412, L006012, L006318, L047891, L049388, L051604,
 L051605, L051606, L051607, L051608, L051609, L051617, L051618,
 L051619, L051620, L051621, L051622, L051687, L051688, L051689,
 L051690, L051691, L053100, L053101, L053102, L053103, L053104,
 L053105, L053106, L062980, L082298, L082301, L082302, L082305,
 L082366, L082367, L105523, L107716, L111529, L111530, L111531,
 L111532, L111533, L111546, L111556, L146559, L167872,
 L172450, L172485 ]
# L000698, L001710, L003319, L177354
SeqTest(L, 'L')

=#


using Base.Test
using RigidBodyDynamics
using RigidBodyDynamics: Bounds
using StaticArrays: SVector
using Cbc: CbcSolver
using LCPSim
using MeshCat
using MeshCatMechanisms

#=
"""
`care(A, B, Q, R)`

Compute 'X', the solution to the continuous-time algebraic Riccati equation,
defined as A'X + XA - (XB)R^-1(B'X) + Q = 0, where R is non-singular.

Algorithm taken from:
Laub, "A Schur Method for Solving Algebraic Riccati Equations."
http://dspace.mit.edu/bitstream/handle/1721.1/1301/R-0859-05666488.pdf
"""
function care(A, B, Q, R)
    G = try
        B*inv(R)*B'
    catch
        error("R must be non-singular.")
    end

    Z = [A  -G;
        -Q  -A']

    S = schurfact(Z)
    S = ordschur(S, real(S.values).<0)
    U = S.Z

    (m, n) = size(U)
    U11 = U[1:div(m, 2), 1:div(n,2)]
    U21 = U[div(m,2)+1:m, 1:div(n,2)]
    return U21/U11
end

"""
`lqr(A, B, Q, R)`

Calculate the optimal gain matrix `K` for the state-feedback law `u = K*x` that
minimizes the cost function:

J = integral(x'Qx + u'Ru, 0, inf).

For the continuous time model `dx = Ax + Bu`.

`lqr(sys, Q, R)`

Solve the LQR problem for state-space system `sys`. Works for both discrete
and continuous time systems.

See also `LQG`

Usage example:
```julia
A = [0 1; 0 0]
B = [0;1]
C = [1 0]
sys = ss(A,B,C,0)
Q = eye(2)
R = eye(1)
L = lqr(sys,Q,R)

u(t,x) = -L*x # Form control law,
t=0:0.1:5
x0 = [1,0]
y, t, x, uout = lsim(sys,u,t,x0)
plot(t,x, lab=["Position", "Velocity"]', xlabel="Time [s]")
```
"""
function lqr(A, B, Q, R)
    S = care(A, B, Q, R)
    K = R\B'*S
    return K
end

function dynamics(mechanism, x, u)
    state = MechanismState{eltype(x)}(mechanism)
    result = DynamicsResult{eltype(x)}(mechanism)
    ẋ = zeros(eltype(x), 4)
    dynamics!(ẋ, result, state, x, u)
    ẋ
end

A = ForwardDiff.jacobian(x -> dynamics(mechanism, x, zeros(eltype(x), 2)), [π, 0, 0, 0])
B = ForwardDiff.jacobian(u -> dynamics(mechanism, eltype(u)[π, 0, 0, 0], [0, u[2]]), [0., 0])
Q = diagm([10, 10, 1, 1])
R = diagm([1, 1])
K = lqr(A, B, Q, R)
=#

@testset "acrobot" begin
    K = [0.0 0.0 0.0 0.0; -266.087 -107.349 -114.95 -54.3175]
    urdf = joinpath(@__DIR__, "..", "examples", "urdf", "Acrobot.urdf")
    mechanism = parse_urdf(Float64, urdf)
    world = root_body(mechanism)
    env = Environment{Float64}([])

    x0 = MechanismState{Float64}(mechanism)
    set_velocity!(x0, zeros(num_velocities(x0)))
    set_configuration!(x0, findjoint(mechanism, "shoulder"), [π])
    set_configuration!(x0, findjoint(mechanism, "elbow"), [-0.1])
    q0 = copy(configuration(x0))
    v0 = copy(velocity(x0))

    controller! = let x0 = vcat(q0, v0), K = K
        (τ, t, x) -> begin
            x̂ = vcat(configuration(x), velocity(x)) - x0
            τ .= -K * x̂
        end
    end

    Δt = 0.05
    N = 100
    results = LCPSim.simulate(x0, controller!, env, Δt, N, CbcSolver());
    @test length(results) == N
    @test norm(velocity(results[end].state)) < 0.03
    @test norm(configuration(results[end].state) - q0) < 1.2

    vis = Visualizer()
    if !haskey(ENV, "CI")
        open(vis)
        wait(vis)
    end

    mvis = MechanismVisualizer(mechanism, URDFVisuals(urdf), vis)
    for r in results
        sleep(Δt)
        set_configuration!(mvis, configuration(r.state))
    end
end


using Trapz

export RHS_Spiral, get_g_ft, extend_symmetric, conj

import Base:conj
conj(S::State) = State(gp=S.gp, u=conj.(S.u))

function coupling_kernel(x::Real, y::Real, σ::Real)
    x = if (x > π)  x - 2π else x end
    y = if (y > π)  y - 2π else y end
    if (x^2 + y^2 ≤ (π * σ)^2)  1 / (π^3 * σ^2) else 0.0 end
end

function get_g_ft(gp::GenericParams)
    xs = getXs(gp)
    fft([coupling_kernel(x, y, gp.p[:σ]) for y in xs, x in xs])
end

get_g_ft(S::State) = get_g_ft(S.gp)

function extend_symmetric(A::Matrix; dim::Int=1)
    cat(A, reverse(A, dims=dim), dims=dim)
end

function RHS_Spiral(S::State, S₀_conj::State, g_ft::Matrix)
    @unpack α, γ, Ω, vx, vy = S.gp.p

    Gz = ifft(g_ft .* fft(S.u)) * (exp(-im * α) * (2π / S.gp.N) ^ 2)
    z2_G_conjz = S.u .^ 2 .* ifft(g_ft .* fft(conj.(S.u))) * (exp(im * α) * (2π / S.gp.N) ^ 2)

    V = vx * ∂ₓ(S) .+ vy * ∂ₓ(S, axis='y') .- (γ + im * Ω) * S.u .+ 0.5 * Gz .- 0.5 * z2_G_conjz

    V1 = - (γ + im * Ω) * S.u
    V2 = vx * ∂ₓ(S) .+ vy * ∂ₓ(S, axis='y')
    V3 = 0.5 * Gz .- 0.5 * z2_G_conjz

    xs = range(-π, π, length=S.gp.N)
    g = real(trapz((xs, xs), S.u .* ∂ₓ(S₀_conj)))
    h = real(trapz((xs, xs), S.u .* ∂ₓ(S₀_conj, axis='y')))
    i = real(trapz((xs, xs), im * S.u .* S₀_conj.u))

    V, V1, V2, V3, [g, h, i]
end

function RHS_Spiral(V::Array, V₀_conj::Array, g_ft::Array, gp::GenericParams)
    S = State(gp=gp, u=reshape(V, (gp.N, gp.N)))
    S₀_conj = State(gp=gp, u=reshape(V₀_conj, (gp.N, gp.N)))
    V, pinning = RHS_Spiral(S, S₀_conj, g_ft)
    append!(vec(V), pinning)
end

function RHS_Spiral_Sym()
    # a
end


include("../src/util.jl")
include("../src/LUMND.jl")

using .LUMND
using Test
using Random
using SHA
using MatrixDepot

Random.seed!(0)

@testset "Partition" begin
    for i = 1:50
        d = rand(2:3)
        n = d == 2 ? rand(5:20) : rand(5:10)
        A = Ad(n,d)
        N = size(A,1)
        maxLvl = rand(1:10)
        (subseps, hrch, dofs) = LUMND.mnd(A, maxLvl)
        # Check sizes
        @test length(dofs) == maxLvl
        @test length(hrch) == maxLvl
        @test all([length(dofs[i]) == 2^(maxLvl-i) for i = 1:maxLvl])
        @test all([length(hrch[i]) == 2^(maxLvl-i) for i = 1:maxLvl])
        @test length(subseps) == N
        # Check ND consistency
		for (l1, s1) in LUMND.BinTreeIt(maxLvl)
			for (l2, s2) in LUMND.BinTreeIt(maxLvl)
                d1, d2 = dofs[l1][s1], dofs[l2][s2]
                if ! are_connected(l1, s1, l2, s2, maxLvl)
                    @test norm(A[d1,d2]) == 0.0
                    @test norm(A[d2,d1]) == 0.0
                end
            end
		end
        # Check hierarchy consistency
		for (l, s) in LUMND.BinTreeIt(maxLvl)
            h = hrch[l][s]
            # One root
            @test length(h.nit[1]) == 1
            # Right depth
            @test length(h.nit) == l
            ss = subseps[dofs[l][s]]
            @test all(sort(unique(ss)) .== 1:length(h.nit[end])) # Will pass if ss = []
            for l_ = 1:length(h.nit)
            	# At least one node per level, all numbered 1 ... n
                l__ = length(h.nit[l_])
                @assert l__ > 0
                @test all(1:l__ .== [n.i for n in h.nit[l_]])
            	# At least one children, except at leaves
                for n in h.nit[l_]
                    if l_ < length(h.nit)
                        @assert length(n.c) > 0
                    else
                        @test length(n.c) == 0
                    end
                end
            end
        end
    end
end

function checkAccuracy(A, lvl)
    b = rand(size(A, 1))
    tree = LUMND.factorize(A, lvl)
    x = LUMND.solve(tree, b)
    xref = A\b
    if issymmetric(A)
        k = cond(A, 1)
    else # Crazy slow otherwise
        k = 100
    end
    resref = norm(A*xref-b)/norm(b)
    @test(norm(A*x-b)/norm(b)     < 10 * resref)
    @test(norm(x-xref)/norm(xref) < k  * 1e-12 )
    return b, k, x, xref
end

@testset "Solve-Smallworld-Gen" begin
    for n in 2:20:500
        A = matrixdepot("smallworld", n)
        leaf = rand([1,2,5])
        N = size(A,1)
        A = A + sparse(UniformScaling(10.0), N, N) # Make it non-singular
        (I,J,V) = findnz(A)
        sig = rand([1e-1, 1e-2, 1e-3, 1e-4]) # Make it non-symmetric
        V += sig .* randn(size(V)) .* V
        A = sparse(I,J,V,N,N)
        lvl = Int64(round(log(N / (leaf)) / log(2) + 1))
        (b, k, x, xref) = checkAccuracy(A, lvl)
        @printf("Smallworld | %3d -> %4.2e %4.2e\n", N, norm(A*x-b)/norm(b), norm(x-xref)/norm(xref))
    end
end

@testset "Solve-Wathen-SPD" begin
    for n in 1:5:30
        A = matrixdepot("wathen", n)
        leaf = rand([1,2,5])
        N = size(A,1)
        lvl = Int64(round(log(N / (leaf)) / log(2) + 1))
        (b, k, x, xref) = checkAccuracy(A, lvl)
        @printf("Wathen | %5d -> %4.2e %4.2e\n", N, norm(A*x-b)/norm(b), norm(x-xref)/norm(xref))
    end
end

@testset "Solve-Lapl-SPD" begin
    @printf("  # |     N lvl         k ->   |Ax-b|   |x-x*| hash\n")
    for i = 1:100
        d = rand(2:3)
        n = d == 2 ? rand(5:30) : rand(5:10)
        leaf = rand([1,2,5,10,15,20])
        lvl = Int64(round(log(n^d / (leaf)) / log(2) + 1))
        A = Ad(n,d)
        (b, k, x, xref) = checkAccuracy(A, lvl)
        @printf("%3d | %5d  %2d  %2.2e -> %4.2e %4.2e %d\n", i, size(A,1), lvl, k, norm(A*x-b)/norm(b), norm(xref - x)/norm(xref), hash(x))
    end
end

@testset "Solve-Lapl-Gen" begin
    @printf("  # |     N lvl         k ->   |Ax-b|   |x-x*| hash\n")
    for i = 1:100
        d = rand(2:3)
        n = d == 2 ? rand(5:30) : rand(5:10)
        leaf = rand([1,2,5,10,15,20])
        lvl = Int64(ceil(log(n^d / (leaf)) / log(2)))
        A = Ad(n,d)
        N = size(A,1)
        (I,J,V) = findnz(A)
        sig = rand([1e-1, 1e-2, 1e-3, 1e-4]) # Make it non-symmetric
        V += sig .* randn(size(V)) .* V
        A = sparse(I,J,V,N,N)
        (b, k, x, xref) = checkAccuracy(A, lvl)
        @printf("%3d | %5d  %2d  %2.2e -> %4.2e %4.2e %d\n", i, size(A,1), lvl, k, norm(A*x-b)/norm(b), norm(xref - x)/norm(xref), hash(x))
    end
end

@testset "Solve-Blur-SPD" begin
    for n in 3:5:53
        A = matrixdepot("blur", n)
        leaf = rand([1,2,5])
        N = size(A,1)
        lvl = Int64(round(log(N / (leaf)) / log(2)))
        (b, k, x, xref) = checkAccuracy(A, lvl)
        @printf("Blur | %2d -> %4.2e %4.2e\n", n, norm(A*x-b)/norm(b), norm(x-xref)/norm(xref))
    end
end


using SphericalHarmonics
using Test, LinearAlgebra, StaticArrays, BenchmarkTools

@testset "SphericalHarmonics" begin
   include("test_ylm.jl")
   include("test_cg.jl")
end


# functions related to noncentral T distribution

# R implementations
using .RFunctions:
    ntdistpdf,
    ntdistlogpdf,
    ntdistcdf,
    ntdistccdf,
    ntdistlogcdf,
    ntdistlogccdf,
    ntdistinvcdf,
    ntdistinvccdf,
    ntdistinvlogcdf,
    ntdistinvlogccdf


"""
    Contiguity(dimension::Int)

Ensures that, in the binary variables `x` constrained to be in this set,
all the 1s are contiguous. The vector must correspond to the regular expression
`0*1*0*`.
"""
struct Contiguity <: MOI.AbstractVectorSet
    dimension::Int
end

# isbits types, nothing to copy
function copy(set::Contiguity)
    return set
end


using Dates, Statistics

##
function agg_mean(df::DataFrame, d::Vector{Symbol}, y::String)
	if y=="day"
		df[:,:dateTime] = floor.(df[:,:dateTime], Day(1))
	elseif y=="week"
		df[:,:dateTime] = floor.(df[:,:dateTime], Week(1))
	elseif y=="month"
		df[:,:dateTime] = floor.(df[:,:dateTime], Month(1))
	elseif y=="year"
		df[:,:dateTime] = floor.(df[:,:dateTime], Year(1))
	else
		println("Pick year,month or day")
	end
combine(groupby(df, [:dateTime]), d .=> mean)
end
###

##
function agg_sum(df::DataFrame, d::Vector{Symbol}, y::String)
	if y=="day"
		df[:,:dateTime] = floor.(df[:,:dateTime], Day(1))
	elseif y=="week"
		df[:,:dateTime] = floor.(df[:,:dateTime], Week(1))
	elseif y=="month"
		df[:,:dateTime] = floor.(df[:,:dateTime], Month(1))
	elseif y=="year"
		df[:,:dateTime] = floor.(df[:,:dateTime], Year(1))
	else
		println("Pick year,month or day")
	end
combine(groupby(df, [:dateTime]), d .=> sum)
end
###

##
function agg_max(df::DataFrame, d::Vector{Symbol}, y::String)
	if y=="day"
		df[:,:dateTime] = floor.(df[:,:dateTime], Day(1))
	elseif y=="week"
		df[:,:dateTime] = floor.(df[:,:dateTime], Week(1))
	elseif y=="month"
		df[:,:dateTime] = floor.(df[:,:dateTime], Month(1))
	elseif y=="year"
		df[:,:dateTime] = floor.(df[:,:dateTime], Year(1))
	else
		println("Pick year,month or day")
	end
combine(groupby(df, [:dateTime]), d .=> maximum)
end
############################################################
function agg_min(df::DataFrame, d::Vector{Symbol}, y::String)
	if y=="day"
		df[:,:dateTime] = floor.(df[:,:dateTime], Day(1))
	elseif y=="week"
		df[:,:dateTime] = floor.(df[:,:dateTime], Week(1))
	elseif y=="month"
		df[:,:dateTime] = floor.(df[:,:dateTime], Month(1))
	elseif y=="year"
		df[:,:dateTime] = floor.(df[:,:dateTime], Year(1))
	else
		println("Pick year,month or day")
	end
combine(groupby(df, [:dateTime]), d .=> minimum)
end

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


#---------------------------------------------#
# Geometrical transformations for the segment #
#---------------------------------------------#

function getSegTransform(A::Array{Float64, 1}, B::Array{Float64,1})
    #= Returns (u0, u) the transform data from [-1, 1] to a 2d segment [A, B] =#
    return (A + B)/2, (B-A)/2
end

 @inline function segRefToSeg2d(xref::Float64, A::Array{Float64,1},
    B::Array{Float64, 1})
    #= Transformation from the 1d [-1, 1] reference segment to a 2-dimensional
        segment [A, B] where A and B are the nodes =#
    return A + (xref + 1)/2*(B-A)
end

@inline function segRefToSeg2d(xref::Float64, u0::Array{FLoat64, 1},
    u::Array{Float64})
    #= Affine transformation ans = u0 + xref*u where u0 and u are obtained from
        getSegTransform() =#
    return u0 + xref*u
end

#----------------------------------------------#
# Geometrical transformations for the triangle #
#----------------------------------------------#
function getTriTransform(T::Array{Float64,2})
    #= Returns the matrices required for the computation of the transformation
        from the triangle {(0,0),(1,0),(1,0)} to {A, B, C} where A, B and C are
        stored columnwise in T =#
    return T[:, 1], [T[1,2]-T[1,1] T[1,3]-T[1,1];T[2,2]-T[2,1] T[2,3]-T[2,1]]
end

@inline function triRefToAny(xref::Array{Float64,1}, T::Array{Float64, 2})
    #= Transformation from the reference triangle {(0,0),(1,0),(1,0)} to
        T={A, B, C} =#
    return T[:, 1] + [T[1,2]-T[1,1] T[1,3]-T[1,1];T[2,2]-T[2,1] T[2,3]-T[2,1]]*xref
end

@inline function triRefToAny(xref::Array{Float64}, u0::Array{Float64,1},
    u::Array{Float64})
    #= Transformation from the reference triangle {(0,0),(1,0),(1,0)} to
        T={A, B, C} using the matrices returned by getTriTransform() =#
    return u0 + u*xref
end


cube = map(c -> c == '#', vcat(permutedims.(collect.(readlines("day17/input.txt")))...))
# cube = map(c -> c == '#', vcat(permutedims.(collect.(readlines("day17/small.txt")))...))
cube = reshape(cube, size(cube)..., 1, 1)

function step_cube(cube)
    # pad cube
    cube_new = zeros(Bool, size(cube) .+ 2)
    # copy data into new cube
    for r in 1:size(cube, 1), c in 1:size(cube, 2), p in 1:size(cube, 3), w in 1:size(cube, 4)
        cube_new[r+1, c+1, p+1, w+1] = cube[r, c, p, w]
    end
    # retain cube for study
    cube_study = copy(cube_new)
    # run thru dims
    mr, mc, mp, mw = size(cube_new)
    for r in 1:size(cube_new, 1), c in 1:size(cube_new, 2), p in 1:size(cube_new, 3), w in 1:size(cube_new, 4)
        neighbors = cube_study[max(1, r-1):min(r+1, mr), max(1, c-1):min(c+1, mc), max(1, p-1):min(p+1, mp), max(1, w-1):min(w+1, mw)]
        if cube_study[r, c, p, w] && !(count(neighbors) in [3, 4])
            cube_new[r, c, p, w] = false
        elseif !cube_study[r, c, p, w] && count(neighbors) == 3
            cube_new[r, c, p, w] = true
        end
    end
    return cube_new
end

for i in 1:6
    global cube
    cube = step_cube(cube)
end

println(count(cube))


"""
    smooth(z, sdim)

Smoothen field `z` with a circular kernel of radius `sdim` using convolution
"""
function smooth(zi, sdim)

    println("Smoothin ....")
    iinan = findall(isnan.(zi))
    iinotnan = findall(.~isnan.(zi))
    zi[iinan] .= 0.

    (nsx,nsy) = size(zi)
    #sdim=div(div(ns,nas),2)

    nsx2 = div(nsx, 2)
    nsy2 = div(nsy, 2)
    mask = zeros(nsx, nsy)

    for i = 1:nsx
        for j = 1:nsy
            kx = i-1
            ky = j-1
            if (i>nsx2+1)
                kx = i-nsx-1
            end
            if (j>nsy2+1)
                ky = j-nsy-1
            end
            r2 = kx*kx+ky*ky
            if (r2<=sdim*sdim)
                mask[i,j] = 1.0
            end
        end
    end
    fm = fft(mask)
    zf = real(ifft(fm.*fft(zi)))/sum(mask)
    if length(iinan)>0
        zi1 = deepcopy(zi)
        zi1[iinotnan] .= 1.0
        zf = zf./(real(ifft(fm.*fft(zi1)))/sum(mask))
    end
    zf[iinan] .= NaN

    return zf
end