type CompositeMap{T}<:AbstractLinearMap{T}
#    t::Type{T} # just stores T, trick to make CompositeMap{T} actually depend on T
    maps::Vector{AbstractLinearMap} # stored in order of application to vector
    function CompositeMap(maps::Vector{AbstractLinearMap})
        N=length(maps)
        for n=2:N
            size(maps[n],2)==size(maps[n-1],1) || throw(DimensionMismatch("CompositeMap"))
        end
        for n=1:N
            promote_type(T,eltype(maps[n]))==T || throw(InexactError())
        end
        new(maps)
    end
end

# basic methods
Base.size(A::CompositeMap,n)=(n==1 ? size(A.maps[end],1) : (n==2 ? size(A.maps[1],2) : error("AbstractLinearMap objects have only 2 dimensions")))
Base.size(A::CompositeMap)=(size(A,1),size(A,2))
Base.isreal(A::CompositeMap)=all(isreal,A.maps) # sufficient but not necessary
# the following rules are sufficient but not necessary
function Base.issym(A::CompositeMap)
    N=length(A.maps)
    if isodd(N)
        issym(A.maps[div(N+1,2)]) || return false
    end
    for n=1:div(N,2)
        A.maps[n]==transpose(A.maps[N-n+1]) || return false
    end
    return true
end
function Base.ishermitian(A::CompositeMap)
    N=length(A.maps)
    if isodd(N)
        ishermitian(A.maps[div(N+1,2)]) || return false
    end
    for n=1:div(N,2)
        A.maps[n]==ctranspose(A.maps[N-n+1]) || return false
    end
    return true
end
function Base.isposdef(A::CompositeMap)
    N=length(A.maps)
    if isodd(N)
        isposdef(A.maps[div(N+1,2)]) || return false
    end
    for n=1:div(N,2)
        A.maps[n]==ctranspose(A.maps[N-n+1]) || return false
    end
    return true
end

# composition of linear maps
function *(A1::CompositeMap,A2::CompositeMap)
    size(A1,2)==size(A2,1) || throw(DimensionMismatch("*"))
    T=promote_type(eltype(A1),eltype(A2))
    return CompositeMap{T}(AbstractLinearMap[A2.maps...,A1.maps...])
end
function *(A1::AbstractLinearMap,A2::CompositeMap)
    size(A1,2)==size(A2,1) || throw(DimensionMismatch("*"))
    T=promote_type(eltype(A1),eltype(A2))
    return CompositeMap{T}(AbstractLinearMap[A2.maps...,A1])
end
function *(A1::CompositeMap,A2::AbstractLinearMap)
    size(A1,2)==size(A2,1) || throw(DimensionMismatch("*"))
    T=promote_type(eltype(A1),eltype(A2))
    return CompositeMap{T}(AbstractLinearMap[A2,A1.maps...])
end
function *(A1::AbstractLinearMap,A2::AbstractLinearMap)
    size(A1,2)==size(A2,1) || throw(DimensionMismatch("*"))
    T=promote_type(eltype(A1),eltype(A2))
    return CompositeMap{T}(AbstractLinearMap[A2,A1])
end

# comparison of CompositeMap objects
==(A::CompositeMap,B::CompositeMap)=(eltype(A)==eltype(B) && A.maps==B.maps)

# special transposition behavior
Base.transpose(A::CompositeMap)=CompositeMap{eltype(A)}(AbstractLinearMap[transpose(M) for M in reverse(A.maps)])
Base.ctranspose(A::CompositeMap)=CompositeMap{eltype(A)}(AbstractLinearMap[ctranspose(M) for M in reverse(A.maps)])

# multiplication with vectors
function Base.A_mul_B!(y::AbstractVector,A::CompositeMap,x::AbstractVector)
    # no size checking, will be done by individual maps
    N=length(A.maps)
    if N==1
        Base.A_mul_B!(y,A.maps[1],x)
    else
        T=promote_type(eltype(A),eltype(x))
        dest=Array(T,size(A.maps[1],1))
        Base.A_mul_B!(dest,A.maps[1],x)
        source=dest
        if N>2
            dest=Array(T,size(A.maps[2],1))
        end
        for n=2:N-1
            resize!(dest,size(A.maps[n],1))
            Base.A_mul_B!(dest,A.maps[n],source)
            dest,source=source,dest # alternate dest and source
        end
        Base.A_mul_B!(y,A.maps[N],source)
    end
    return y
end
function Base.At_mul_B!(y::AbstractVector,A::CompositeMap,x::AbstractVector)
    # no size checking, will be done by individual maps
    N=length(A.maps)
    if N==1
        Base.At_mul_B!(y,A.maps[1],x)
    else
        T=promote_type(eltype(A),eltype(x))
        dest=Array(T,size(A.maps[N],2))
        Base.At_mul_B!(dest,A.maps[N],x)
        source=dest
        if N>2
            dest=Array(T,size(A.maps[N-1],2))
        end
        for n=N-1:-1:2
            resize!(dest,size(A.maps[n],2))
            Base.At_mul_B!(dest,A.maps[n],source)
            dest,source=source,dest # alternate dest and source
        end
        Base.At_mul_B!(y,A.maps[1],source)
    end
    return y
end
function Base.Ac_mul_B!(y::AbstractVector,A::CompositeMap,x::AbstractVector)
    # no size checking, will be done by individual maps
    N=length(A.maps)
    if N==1
        Base.Ac_mul_B!(y,A.maps[1],x)
    else
        T=promote_type(eltype(A),eltype(x))
        dest=Array(T,size(A.maps[N],2))
        Base.Ac_mul_B!(dest,A.maps[N],x)
        source=dest
        if N>2
            dest=Array(T,size(A.maps[N-1],2))
        end
        for n=N-1:-1:2
            resize!(dest,size(A.maps[n],2))
            Base.Ac_mul_B!(dest,A.maps[n],source)
            dest,source=source,dest # alternate dest and source
        end
        Base.Ac_mul_B!(y,A.maps[1],source)
    end
    return y
end


include("Opcodes.jl")
using .OpCodes
using StaticArrays

function run1(data, startwhite = false)
    right =  @SMatrix [0  1;
                     -1  0]
    left = @SMatrix [0 -1;
                      1  0]
    dir =   @SVector [-1, 0]
    pos =   @SVector [0, 0]
    field = Dict{typeof(pos), Int}()
    inp = Channel{Int}(1)
    outp = Channel{Int}(1)
    ic = IntComputer(data, inp, outp)
    @async compute!(ic)
    startwhite && (field[pos] = 1)
    while !ic.finished[]
        put!(inp, get(field, pos, 0))
        field[pos] = take!(outp)
        dir = take!(outp) == 1 ? right * dir : left * dir
        pos = pos + dir
    end
    field
end

# question 1
f = run1(readline("data/Dec11_data.txt"))
length(keys(f))

# question 2 (plus an added arg to the function)
f = run1(readline("data/Dec11_data.txt"), true)

b = fill(NaN, 100, 100)
for (k,v) in f
    b[(k .+ 50)...] = v
end
heatmap(b, ylim = (40,60), xlim = (40, 100), yflip = true)


module TestBautin
include("preamble_plots.jl")

using Bifurcations: resolved_points
using Bifurcations.Codim2LimitCycle: FoldLimitCycleProblem
using Bifurcations.Examples.Bautin

prob = Bautin.make_prob()
solver1 = init(prob)
solve!(solver1)

codim1_points = resolved_points(solver1)
@test length(codim1_points) == 1
@test codim1_points[1].point_type === Codim1.PointTypes.hopf

hopf_prob = BifurcationProblem(
    codim1_points[1],
    solver1,
    (@lens _.β₂),
    (-2.0, 2.0),
)
hopf_solver = init(
    hopf_prob;
)
solve!(hopf_solver)

hopf_β₁ = [u[end-1] for sweep in hopf_solver.super.sol.sweeps for u in sweep.u]
hopf_β₂ = [u[end]   for sweep in hopf_solver.super.sol.sweeps for u in sweep.u]
@test all(@. abs(hopf_β₁) < 1e-6)
@test maximum(hopf_β₂) > 2
@test minimum(hopf_β₂) < -2

codim2_points = resolved_points(hopf_solver)
@test length(codim2_points) == 1
@test codim2_points[1].point_type === Codim2.PointTypes.bautin
β_bautin = codim2_points[1].u[end-1:end]
@test all(@. abs(β_bautin) < 1e-6)

plt = plot(hopf_solver)
@test_nothrow nullshow(plt)

flc_prob = FoldLimitCycleProblem(
    # codim2_points[1],  # SpecialPointInterval is required ATM
    special_intervals(hopf_solver, Codim2.PointTypes.bautin)[1],
    hopf_solver;
    num_mesh = 20,
    degree = 3,
)
@test flc_prob.t_domain == ([-2.0, -2.0], [2.0, 2.0])
flc_solver = init(
    flc_prob;
    start_from_nearest_root = true,
    max_branches = 0,  # TODO: stop manually doing this
)
solve!(flc_solver)

flc_β₁ = [u[end-1] for sweep in flc_solver.super.sol.sweeps for u in sweep.u]
flc_β₂ = [u[end]   for sweep in flc_solver.super.sol.sweeps for u in sweep.u]
@test all(@. abs(4 * flc_β₁ + flc_β₂^2) < 5e-3)
@test maximum(flc_β₂) > 2
@test minimum(flc_β₂) > -1e-6

plt_lcs = plot_state_space(flc_solver)
@test_nothrow nullshow(plt_lcs)

end  # module


# definition of samplers and random generation

## Uniform

Sampler(rng::AbstractRNG, d::Union{UniformWrap,UniformType}, n::Repetition) =
    Sampler(rng, d[], n)

## floats

### override def from Random

Sampler(rng::AbstractRNG, ::Type{T}, n::Repetition) where {T<:AbstractFloat} =
    Sampler(rng, CloseOpen01(T), n)

### fall-back on Random definitions
rand(r::AbstractRNG, ::SamplerTrivial{CloseOpen01{T}}) where {T} =
    rand(r, SamplerTrivial(Random.CloseOpen01{T}()))

rand(r::AbstractRNG, ::SamplerTrivial{CloseOpen12{T}}) where {T} =
    rand(r, SamplerTrivial(Random.CloseOpen12{T}()))

### CloseOpenAB

Sampler(rng::AbstractRNG, d::CloseOpenAB{T}, n::Repetition) where {T} =
    SamplerTag{CloseOpenAB{T}}((a=d.a, d=d.b - d.a, sp=Sampler(rng, CloseOpen01{T}(), n)))

rand(rng::AbstractRNG, sp::SamplerTag{CloseOpenAB{T}}) where {T} =
    sp.data.a + sp.data.d  * rand(rng, sp.data.sp)

## sampler for pairs and complex numbers

function Sampler(rng::AbstractRNG, u::Combine2{T}, n::Repetition) where T <: Union{Pair,Complex}
    sp1 = Sampler(rng, u.x, n)
    sp2 = u.x == u.y ? sp1 : Sampler(rng, u.y, n)
    SamplerTag{Cont{T}}((sp1, sp2))
end

rand(rng::AbstractRNG, sp::SamplerTag{Cont{T}}) where {T<:Union{Pair,Complex}} =
    T(rand(rng, sp.data[1]), rand(rng, sp.data[2]))

### additional methods for complex numbers

Sampler(rng::AbstractRNG, u::Combine1{Complex}, n::Repetition) =
    Sampler(rng, Combine(Complex, u.x, u.x), n)

Sampler(rng::AbstractRNG, ::Type{Complex{T}}, n::Repetition) where {T<:Real} =
    Sampler(rng, Combine(Complex, T, T), n)

## Normal & Exponential

rand(rng::AbstractRNG, ::SamplerTrivial{Normal01{T}}) where {T<:Union{AbstractFloat,Complex{<:AbstractFloat}}} =
    randn(rng, T)

Sampler(rng::AbstractRNG, d::Normalμσ{T}, n::Repetition) where {T} =
    SamplerSimple(d, Sampler(rng, Normal(T), n))

rand(rng::AbstractRNG, sp::SamplerSimple{Normalμσ{T},<:Sampler}) where {T} =
    sp[].μ + sp[].σ  * rand(rng, sp.data)

rand(rng::AbstractRNG, ::SamplerTrivial{Exponential1{T}}) where {T<:AbstractFloat} =
    randexp(rng, T)

Sampler(rng::AbstractRNG, d::Exponentialθ{T}, n::Repetition) where {T} =
    SamplerSimple(d, Sampler(rng, Exponential(T), n))

rand(rng::AbstractRNG, sp::SamplerSimple{Exponentialθ{T},<:Sampler}) where {T} =
    sp[].θ * rand(rng, sp.data)


struct KO_Centered{N} end

#= Kreiss-Oliger dissipation

These are operators of the form

  σ (-1)^(ord+3)/2 / 2^(ord+1) h^(ord+1) ∂^(ord+1) / ∂x^(ord+1)

note that, unlike the reference below, we add these directly to the state vector
(and *not* to its time-derivative)

cf: https://einsteintoolkit.org/thornguide/CactusNumerical/Dissipation/documentation.html
=#
function KO_Centered{N}(order::Int, sigma_diss::T, dx::T,
                        len::Int) where {T<:AbstractFloat,N}
    @assert (order + 1) % 2 == 0 "Only implemented for odd order."

    derivative_order    = order + 1
    approximation_order = 2
    stencil_length      = order + 2
    stencil_offset      = div(stencil_length-1,2)

    weights = calculate_weights(order + 1, order + 1, 0)

    a   = div(order + 3, 2)
    s   = (-1)^a

    stencil_coefs = s * sigma_diss / 2^(order + 1) .* weights

    PeriodicFD{T,N,typeof(stencil_coefs)}(derivative_order, approximation_order,
                                          dx, len, stencil_length,
                                          stencil_offset, stencil_coefs)
end

abstract type Filter{T,N} end

#= Kreiss-Oliger dissipation

These are operators of the form

  σ (-1)^(ord+3)/2 / 2^(ord+1) h^(ord+1) ∂^(ord+1) / ∂x^(ord+1)

note that, unlike the reference below, we add these directly to the state vector
(and *not* to its time-derivative)

cf: https://einsteintoolkit.org/thornguide/CactusNumerical/Dissipation/documentation.html
=#
function KO_kernel(order::Int, sigma_diss::T) where {T<:Real}
    @assert (order + 1) % 2 == 0 "Only implemented for odd order."

    kernel = calculate_weights(order + 1, order + 1, 0)

    a   = div(order + 3, 2)
    s   = (-1)^a
    fac = s * sigma_diss / 2^(order + 1)

    rmul!(kernel, fac)

    # adding one to the middle entry so that it becomes a low-pass filter
    mid = div(length(kernel) + 1, 2)
    kernel[mid] += 1

    kernel
end

function exp_kernel!(f::Vector{T}, γ::T, α::T) where {T<:Real}
    M  = length(f) - 1
    f .= [ exp( -α * ( (i-1)/M )^(γ*M) ) for i in eachindex(f) ]
    nothing
end
function exp_kernel(dim::Int, γ::T, α::T) where {T<:Real}
    f  = Array{T}(undef, dim)
    exp_kernel!(f, γ, α)
    f
end

struct ConvFilter{T<:Real,N,A} <: Filter{T,N}
    kernel :: Vector{T}
    _cache :: A
end

struct FftFilter{T<:Real,N,A,FT<:FFTW.r2rFFTWPlan} <: Filter{T,N}
    kernel   :: Vector{T}
    fft_plan :: FT
    _cache   :: A
end

struct KO_Filter{N} end

function KO_Filter{N}(order::Int, sigma_diss::T, Nxx...) where {T<:Real,N}
    kernel = KO_kernel(order, sigma_diss)
    nt = Threads.nthreads()
    _cache = [Array{T}(undef, Nxx...) for _ in 1:nt]
    ConvFilter{T,N,typeof(_cache)}(kernel, _cache)
end

KO_Filter(args...) = KO_Filter{1}(args...)

struct Exp_Filter{N} end

# α = -log(ϵ) where ϵ is the machine epsilon.
# for the standard choice of ϵ = 2^-52, α = 36.0437
function Exp_Filter{N}(γ::T, α::T, Nxx...) where {T<:AbstractFloat,N}
    kernel = exp_kernel(Nxx[N], γ, α)
    nt = Threads.nthreads()
    _cache = [Array{T}(undef, Nxx...) for _ in 1:nt]

    # REDFT00 is the DCT-I, see http://www.fftw.org/fftw3_doc/Real-even_002fodd-DFTs-_0028cosine_002fsine-transforms_0029.html#Real-even_002fodd-DFTs-_0028cosine_002fsine-transforms_0029

    # let's use only one thread per FFTW so that they don't trample over each other
    FFTW.set_num_threads(1)

    # we want to use the DCT-I since this basis is precisely the one we have by
    # using the Gauss-Lobatto grid points
    fft_plan = FFTW.plan_r2r(_cache[1], FFTW.REDFT00, N)

    FftFilter{T,N,typeof(_cache),typeof(fft_plan)}(kernel, fft_plan, _cache)
end
function Exp_Filter{N}(γ::T, Nxx...) where {T<:AbstractFloat,N}
    α = T(36.0437)
    Exp_Filter{N}(γ, α, Nxx...)
end

Exp_Filter(args...) = Exp_Filter{1}(args...)

function convolution!(fout::AbstractVector{T}, f::AbstractVector{T},
                      g::AbstractVector{T}) where {T}
    f_len = length(f)
    g_len = length(g)
    mid   = div(g_len + 1, 2)

    @fastmath @inbounds for i in eachindex(f)
        sum_i = zero(T)

        if mid <= i <= (f_len - mid + 1)
            @inbounds for aa in 1:g_len
                i_circ = i - (mid - aa)
                sum_i += g[aa] * f[i_circ]
            end
        else
            @inbounds for aa in 1:g_len
                # imposing periodicity
                i_circ = 1 + mod(i - (mid-aa) - 1, f_len)
                sum_i += g[aa] * f[i_circ]
            end
        end
        fout[i] = sum_i
    end

    fout
end

# convolution along N axis
function convolution!(fout::AbstractArray{T,M}, f::AbstractArray{T,M},
                      g::AbstractVector{T}, N::Integer) where {T,M}
    # make sure axis of convolution is contained in the dimensions of f
    @assert N <= M

    f_len = size(f,N)
    g_len = length(g)
    mid   = div(g_len + 1, 2)

    @fastmath @inbounds for idx in CartesianIndices(f)
        i     = idx.I[N] # convolution to be done along this direction
        sum_i = zero(T)

        if mid <= i <= (f_len - mid + 1)
            @inbounds for aa in 1:g_len
                i_circ = i - (mid - aa)
                I = Base.setindex(idx.I, i_circ, N)
                sum_i += g[aa] * f[I...]
            end
        else
            @inbounds for aa in 1:g_len
                # imposing periodicity
                i_circ = 1 + mod(i - (mid-aa) - 1, f_len)
                I = Base.setindex(idx.I, i_circ, N)
                sum_i += g[aa] * f[I...]
            end
        end
        fout[idx] = sum_i
    end

    fout
end

function (filter::ConvFilter)(f::AbstractVector)
    id = Threads.threadid()
    cache = filter._cache[id]
    @assert length(cache) == length(f)
    convolution!(cache, f, filter.kernel)
    copyto!(f, cache)
end

function (filter::ConvFilter{T,M})(f::AbstractArray{T}) where {T,M}
    id = Threads.threadid()
    cache = filter._cache[id]
    @assert length(cache) == length(f)
    convolution!(cache, f, filter.kernel, M)
    copyto!(f, cache)
end

# see, eg, D. Gottlieb, J.S. Hesthaven, "Spectral methods for hyperbolic
# problems", Journal of Computational and Applied Mathematics 128 (2001) 83–131,
# or "Idempotent filtering in spectral and spectral element methods", Journal of
# Computational Physics 220 (2006) 41-58
function (filter::FftFilter)(f::AbstractVector)
    M  = length(f) - 1
    id = Threads.threadid()
    cache = filter._cache[id]

    copyto!(cache, f)

    # compute the DCT-I of f
    mul!(f, filter.fft_plan, cache)

    # in momentum space, act with the filter kernel [eq (2.5) of "Idempotent filtering..."]
    # filter.kernel .* f
    @inbounds @simd for i in eachindex(f)
        # the division by 2*(N-1) is to invert the FFT, see below
        cache[i] = 0.5/M * filter.kernel[i] * f[i]
    end

    # now go back to position space using the DCT-I. the division by 2*(N-1)
    # above comes from the normalization used. DCT-I is its own inverse up to a
    # constant, and FFTW defines it as such, cf:
    # http://www.fftw.org/fftw3_doc/1d-Real_002deven-DFTs-_0028DCTs_0029.html#g_t1d-Real_002deven-DFTs-_0028DCTs_0029
    mul!(f, filter.fft_plan, cache)
end

# filter along N axis
function (filter::FftFilter{T,N})(f::AbstractArray{T,M}) where {T,N,M}
    # make sure axis of filtering is contained in the dimensions of f
    @assert N <= M
    id = Threads.threadid()
    cache = filter._cache[id]
    copyto!(cache, f)

    Rpre  = CartesianIndices(size(f)[1:N-1])
    Rpost = CartesianIndices(size(f)[N+1:end])

    # use a function barrier here to separate the type-unstable portion
    _fftfilter!(f, cache, filter.kernel, filter.fft_plan, Rpre,
                size(f, N), Rpost)
end

# Multidimensional algorithm adapted from
# http://julialang.org/blog/2016/02/iteration
function _fftfilter!(fout, f, kernel, fft_plan::FFTW.Plan, Rpre, n, Rpost)
    # compute the DCT-I of f
    mul!(fout, fft_plan, f)

    @fastmath @inbounds for Ipost in Rpost
        @inbounds for i in 1:n
            @inbounds @simd for Ipre in Rpre
                f[Ipre,i,Ipost] = 0.5/(n-1) * kernel[i] * fout[Ipre,i,Ipost]
            end
        end
    end

    # go back to position space using the DCT-I
    mul!(fout, fft_plan, f)
end


function solve(digits, actual, easy)
    if easy
        return sum((x -> x in [2, 3, 4, 7]).((length).(actual)))
    else
        # Idea:
        #   find '1', '4', '7' and '8' through number of segments
        #
        #   find '3' by seeing which number with 5 segments has 2 in common with '1'
        #   find '2' and '5' by taking remaining 5 segment numbers, '5' has 3 segments in common with '4'
        #
        #   find '6' by seeing which number with 6 segments has 1 in common with '1'
        #   find '0' and '9' by taking remaining 6 segment numbers, '9' has 4 segments in common with '4'
        #
        sort_internal(x) = join(sort(collect(x)))
        ds = (sort_internal).(digits)
        as = (sort_internal).(actual)

        sort!(ds, by=length)
        zero, one, two, three, four, five, six, seven, eight, nine = "abcdefghij"
        one, seven, four, eight = ds[1], ds[2], ds[3], ds[end]
        for i in 4:6
            if length(intersect(ds[i], one)) == 2
                three = ds[i]
                two = ds[(i - 3) % 3 + 4]
                five = ds[(i - 2) % 3 + 4]
            end
        end
        if length(intersect(two, four)) == 3
            two, five = five, two
        end
        for i in 7:9
            if length(intersect(ds[i], one)) == 1
                six = ds[i]
                zero = ds[(i - 6) % 3 + 7]
                nine = ds[(i - 5) % 3 + 7]
            end
        end
        if length(intersect(zero, four)) == 4
            zero, nine = nine, zero
        end
        lookup = [zero, one, two, three, four, five, six, seven, eight, nine]

        ans = 0
        for a in as
            ans = 10ans + (findfirst(isequal(a), lookup) - 1)
        end
        return ans
    end
end

# 2 -> 1
# 3 -> 7
# 4 -> 4
# 5 -> 2, 3, 5
# 6 -> 6, 9, 0
# 7 -> 8

function main()
    s_easy = 0
    s_hard = 0
    for line in readlines()
        digits, actual = split(strip(line), " | ")
        digits = split(digits)
        actual = split(actual)
        s_easy += solve(digits, actual, true)
        s_hard += solve(digits, actual, false)
    end
    println(s_easy)
    println(s_hard)
end

main()


push!(LOAD_PATH, joinpath(@__DIR__, "..", ".."))

using Test
using StaticArrays, IntervalSets, LinearAlgebra, UnPack

import ClimaCore:
    ClimaCore,
    slab,
    Spaces,
    Domains,
    Meshes,
    Geometry,
    Topologies,
    Spaces,
    Fields,
    Operators
import ClimaCore.Domains.Geometry: Cartesian2DPoint
using ClimaCore.Geometry

using Logging: global_logger
using TerminalLoggers: TerminalLogger
global_logger(TerminalLogger())

# set up function space
function hvspace_2D(
    xlim = (-π, π),
    zlim = (0, 4π),
    helem = 10,
    velem = 50,
    npoly = 4,
)
    FT = Float64
    vertdomain = Domains.IntervalDomain(
        Geometry.ZPoint{FT}(zlim[1]),
        Geometry.ZPoint{FT}(zlim[2]);
        boundary_tags = (:bottom, :top),
    )
    vertmesh = Meshes.IntervalMesh(vertdomain, nelems = velem)
    vert_center_space = Spaces.CenterFiniteDifferenceSpace(vertmesh)
    horzdomain = Domains.RectangleDomain(
        Geometry.XPoint{FT}(xlim[1])..Geometry.XPoint{FT}(xlim[2]),
        Geometry.YPoint{FT}(-0)..Geometry.YPoint{FT}(0),
        x1periodic = true,
        x2boundary = (:a, :b),
    )
    horzmesh = Meshes.EquispacedRectangleMesh(horzdomain, helem, 1)
    horztopology = Topologies.GridTopology(horzmesh)

    quad = Spaces.Quadratures.GLL{npoly + 1}()
    horzspace = Spaces.SpectralElementSpace1D(horztopology, quad)

    hv_center_space =
        Spaces.ExtrudedFiniteDifferenceSpace(horzspace, vert_center_space)
    hv_face_space = Spaces.FaceExtrudedFiniteDifferenceSpace(hv_center_space)
    return (hv_center_space, hv_face_space)
end

# set up rhs!
hv_center_space, hv_face_space = hvspace_2D((-500, 500), (0, 1000))
#hv_center_space, hv_face_space = hvspace_2D((-500,500),(0,30000), 5, 30)

const MSLP = 1e5 # mean sea level pressure
const grav = 9.8 # gravitational constant
const R_d = 287.058 # R dry (gas constant / mol mass dry air)
const γ = 1.4 # heat capacity ratio
const C_p = R_d * γ / (γ - 1) # heat capacity at constant pressure
const C_v = R_d / (γ - 1) # heat capacity at constant volume
const R_m = R_d # moist R, assumed to be dry

function pressure(ρθ)
    if ρθ >= 0
        return MSLP * (R_d * ρθ / MSLP)^γ
    else
        return NaN
    end
end

Φ(z) = grav * z

# Reference: https://journals.ametsoc.org/view/journals/mwre/140/4/mwr-d-10-05073.1.xml, Section 5a
function init_dry_rising_bubble_2d(x, z)
    x_c = 0.0
    z_c = 350.0
    r_c = 250.0
    θ_b = 300.0
    θ_c = 0.5
    cp_d = C_p
    cv_d = C_v
    p_0 = MSLP
    g = grav

    # auxiliary quantities
    r = sqrt((x - x_c)^2 + (z - z_c)^2)
    θ_p = r < r_c ? 0.5 * θ_c * (1.0 + cospi(r / r_c)) : 0.0 # potential temperature perturbation

    θ = θ_b + θ_p # potential temperature
    π_exn = 1.0 - g * z / cp_d / θ # exner function
    T = π_exn * θ # temperature
    p = p_0 * π_exn^(cp_d / R_d) # pressure
    ρ = p / R_d / T # density
    ρθ = ρ * θ # potential temperature density

    return (ρ = ρ, ρθ = ρθ, ρuₕ = ρ * Geometry.Cartesian1Vector(0.0))
end

# initial conditions
coords = Fields.coordinate_field(hv_center_space);
face_coords = Fields.coordinate_field(hv_face_space);

Yc = map(coords) do coord
    bubble = init_dry_rising_bubble_2d(coord.x, coord.z)
    bubble
end;

ρw = map(face_coords) do coord
    Geometry.Cartesian3Vector(0.0)
end;

Y = Fields.FieldVector(Yc = Yc, ρw = ρw);

function rhs!(dY, Y, _, t)
    ρw = Y.ρw
    Yc = Y.Yc
    dYc = dY.Yc
    dρw = dY.ρw

    # spectral horizontal operators
    hdiv = Operators.Divergence()
    hgrad = Operators.Gradient()

    # vertical FD operators with BC's
    vdivf2c = Operators.DivergenceF2C(
        bottom = Operators.SetValue(Geometry.Cartesian3Vector(0.0)),
        top = Operators.SetValue(Geometry.Cartesian3Vector(0.0)),
    )
    vvdivc2f = Operators.DivergenceC2F(
        bottom = Operators.SetDivergence(Geometry.Cartesian3Vector(0.0)),
        top = Operators.SetDivergence(Geometry.Cartesian3Vector(0.0)),
    )
    uvdivf2c = Operators.DivergenceF2C(
        bottom = Operators.SetValue(
            Geometry.Cartesian3Vector(0.0) ⊗ Geometry.Cartesian1Vector(0.0),
        ),
        top = Operators.SetValue(
            Geometry.Cartesian3Vector(0.0) ⊗ Geometry.Cartesian1Vector(0.0),
        ),
    )
    If_bc = Operators.InterpolateC2F(
        bottom = Operators.SetValue(Geometry.Cartesian1Vector(0.0)),
        top = Operators.SetValue(Geometry.Cartesian1Vector(0.0)),
    )
    If = Operators.InterpolateC2F(
        bottom = Operators.Extrapolate(),
        top = Operators.Extrapolate(),
    )
    Ic = Operators.InterpolateF2C()
    ∂ = Operators.DivergenceF2C(
        bottom = Operators.SetValue(Geometry.Cartesian3Vector(0.0)),
        top = Operators.SetValue(Geometry.Cartesian3Vector(0.0)),
    )
    ∂f = Operators.GradientC2F()
    ∂c = Operators.GradientF2C()
    B = Operators.SetBoundaryOperator(
        bottom = Operators.SetValue(Geometry.Cartesian3Vector(0.0)),
        top = Operators.SetValue(Geometry.Cartesian3Vector(0.0)),
    )

    uₕ = @. Yc.ρuₕ / Yc.ρ
    w = @. ρw / If(Yc.ρ)
    p = @. pressure(Yc.ρθ)

    # density
    @. dYc.ρ = -∂(ρw)
    @. dYc.ρ -= hdiv(Yc.ρuₕ)

    # potential temperature
    @. dYc.ρθ = -(∂(ρw * If(Yc.ρθ / Yc.ρ)))
    @. dYc.ρθ -= hdiv(uₕ * Yc.ρθ)

    # horizontal momentum
    Ih = Ref(
        Geometry.Axis2Tensor(
            (Geometry.Cartesian1Axis(), Geometry.Cartesian1Axis()),
            @SMatrix [1.0]
        ),
    )
    @. dYc.ρuₕ = -hdiv(Yc.ρuₕ ⊗ uₕ + p * Ih)
    @. dYc.ρuₕ -= uvdivf2c(ρw ⊗ If_bc(uₕ))

    # vertical momentum
    @. dρw = B(
        Geometry.transform(
            Geometry.Cartesian3Axis(),
            -(∂f(p)) - If(Yc.ρ) * ∂f(Φ(coords.z)),
        ) - vvdivc2f(Ic(ρw ⊗ w)),
    )
    uₕf = @. If_bc(Yc.ρuₕ / Yc.ρ) # requires boundary conditions
    @. dρw -= hdiv(uₕf ⊗ ρw)

    # 3) diffusion
    κ = 10.0 # m^2/s

    Yfρ = @. If(Yc.ρ)

    #  1a) horizontal div of horizontal grad of horiz momentun
    @. dYc.ρuₕ += hdiv(κ * (Yc.ρ * hgrad(Yc.ρuₕ / Yc.ρ)))

    #  1b) vertical div of vertical grad of horiz momentun
    @. dYc.ρuₕ += uvdivf2c(κ * (Yfρ * ∂f(Yc.ρuₕ / Yc.ρ)))

    #  1c) horizontal div of horizontal grad of vert momentum
    @. dρw += hdiv(κ * (Yfρ * hgrad(ρw / Yfρ)))

    #  1d) vertical div of vertical grad of vert momentun
    @. dρw += vvdivc2f(κ * (Yc.ρ * ∂c(ρw / Yfρ)))

    # 2a) horizontal div of horizontal grad of potential temperature
    @. dYc.ρθ += hdiv(κ * (Yc.ρ * hgrad(Yc.ρθ / Yc.ρ)))

    # 2b) vertical div of vertial grad of potential temperature
    @. dYc.ρθ += ∂(κ * (Yfρ * ∂f(Yc.ρθ / Yc.ρ)))

    Spaces.weighted_dss!(dYc)
    Spaces.weighted_dss!(dρw)
    return dY
end

dYdt = similar(Y);
rhs!(dYdt, Y, nothing, 0.0);

# run!
using OrdinaryDiffEq
Δt = 0.03
prob = ODEProblem(rhs!, Y, (0.0, 200.0))
sol = solve(
    prob,
    SSPRK33(),
    dt = Δt,
    saveat = 1.0,
    progress = true,
    progress_message = (dt, u, p, t) -> t,
);

ENV["GKSwstype"] = "nul"
import Plots
Plots.GRBackend()

dirname = "bubble"
path = joinpath(@__DIR__, "output", dirname)
mkpath(path)

# post-processing
import Plots
anim = Plots.@animate for u in sol.u
    Plots.plot(u.Yc.ρθ ./ u.Yc.ρ, clim = (300.0, 300.8))
end
Plots.mp4(anim, joinpath(path, "bubble.mp4"), fps = 20)


"""
    box
        -fully actuated
"""
struct Box{T} <: Model{T}
	# dimensions
	nq::Int # generalized coordinates
	nu::Int # controls
	nw::Int # parameters
	nc::Int # contact points

    mass::T
    inertia::T
    gravity::T

    friction_body_world::Vector{T}
    contact_corner_offset::Vector{Vector{T}}
    friction_joint::Vector{T}
end

# Methods
function lagrangian(model::Box, q, q̇)
    L = 0.0
    L += 0.5 * model.mass * dot(q̇[1:2], q̇[1:2])
    L -= model.mass * model.gravity * q[2]

    L += 0.5 * model.inertia * q̇[3]^2

    return L
end

function kinematics(model::Box, q)
    pos = q[1:2]
    θ = q[3]
    R = rotation_matrix(θ)
    [
        pos + R * model.contact_corner_offset[1];
        pos + R * model.contact_corner_offset[2];
        pos + R * model.contact_corner_offset[3];
        pos + R * model.contact_corner_offset[4];
    ]
end

function signed_distance(model::Box, q)
    kinematics(model, q)[collect([2, 4, 6, 8])]
end

function input_jacobian(model::Box, q)
	[
	 1.0 0.0 0.0;
	 0.0 1.0 0.0;
     0.0 0.0 1.0;
    ]
end

function contact_jacobian(model::Box, q)
	j = kinematics(model, q)
	Symbolics.jacobian(j, q)
end

# nominal configuration
function nominal_configuration(model::Box)
	[0.0; 0.1; 0.0]
end

# friction coefficients
friction_coefficients(model::Box) = model.friction_body_world

# Dimensions
nq = 3 # configuration dimension
nu = 3 # control dimension
nw = 0 # parameters
nc = 4 # number of contact points

# Parameters
r_dim = 0.5
gravity = 10.0#9.81
mass = 1.0 # mass
inertia = 1.0 / 12.0 * mass * ((2.0 * r_dim)^2 + (2.0 * r_dim)^2)

# Contact corner
cc1 = [r_dim, r_dim]
cc2 = [-r_dim, r_dim]
cc3 = [r_dim, -r_dim]
cc4 = [-r_dim, -r_dim]

contact_corner_offset = [cc1, cc2, cc3, cc4]
friction_body_world = [1.0, 1.0, 1.0, 1.0]  # coefficient of friction

# Model
box = Box(nq, nu, nw, nc,
		  mass, inertia, gravity,
		  friction_body_world, contact_corner_offset, zeros(0))

box_contact_kinematics = [q -> kinematics(box, q)[1:2],
                          q -> kinematics(box, q)[3:4],
                          q -> kinematics(box, q)[5:6],
                          q -> kinematics(box, q)[7:8]]

box_contact_kinematics_jacobians = [q -> contact_jacobian(box, q)[1:2, :],
                                    q -> contact_jacobian(box, q)[3:4, :],
                                    q -> contact_jacobian(box, q)[5:6, :],
                                    q -> contact_jacobian(box, q)[7:8, :]]

name(::Box) = :box


using Yota

include("primitives.jl")
include("coupling.jl")

# function g_func(c::Coupling, z)
    # mask, s, t = c.mask, c.s, c.t
    # zp, zn = mask .* z, (1 .- mask) .* z  # z positive, z negative
    # return zp .+ (zn - t(zp)) .* exp.(-s(zp))
# end

function logprob(c::Coupling, x)
end

# fun(c, x) = sum(g_func(c, f_func(c, x)))

mutable struct RealVNP
    c1::Coupling
    c2::Coupling
end

function RealNVP(xz_len, u_len)
    mlps = [MLP3(xz_len, u_len, u_len, xz_len) for i=1:4]
    mask = vcat([1 for i=1:Int(xz_len / 2)], [0 for i=1:Int(xz_len / 2)])
    RealNVP(Coupling(mlps[1], mlps[2]),
            Coupling(mlps[3], mlps[4]))
end

function main2()
    mask = [0, 0, 0, 0, 0, 1, 1, 1, 1, 1]
    s = MLP3{Float64}(10, 5, 5, 10)
    t = MLP3{Float64}(10, 5, 5, 10)
    c = Coupling(mask, s, t)

    x = rand(10, 20)
    z = f_func(c, x)
    g_func(c, z) .- x
end


# ------------------------------------------------------------------
# Licensed under the MIT License. See LICENSE in the project root.
# ------------------------------------------------------------------

# auxiliary functions and variables
uniform(h; λ)      = (h ≤ λ)
triangular(h; λ)   = (h ≤ λ) * (λ - h)
epanechnikov(h; λ) = (h ≤ λ) * (λ^2 - h^2)
kernfun = Dict(
  :uniform => uniform,
  :triangular => triangular,
  :epanechnikov => epanechnikov
)

"""
    GHC(k, λ; kern=:epanechnikov, link=:ward)

A method for partitioning spatial data into `k` clusters
according to a range `λ` using Geostatistical Hierarchical
Clustering (GHC). The larger the range the more connected
are nearby samples.

## Parameters

* `k`    - Approximate number of clusters
* `λ`    - Approximate range of kernel function
* `kern` - Kernel function (`:uniform`, `:triangular` or `:epanechnikov`)
* `link` - Linkage function (`:single`, `:average`, `:complete`, `:ward` or `:ward_presquared`)

## References

* Fouedjio, F. 2016. [A hierarchical clustering method for multivariate geostatistical data]
  (https://www.sciencedirect.com/science/article/abs/pii/S2211675316300367)

### Notes

- The range parameter controls the sparsity pattern of the pairwise
  distances, which can greatly affect the computational performance
  of the GHC algorithm. We recommend choosing a range that is small
  enough to connect nearby samples. For example, clustering data over
  a 100x100 Cartesian grid with unit spacing is possible with `λ=1.0`
  or `λ=2.0` but the problem starts to become computationally unfeasible
  around `λ=10.0` due to the density of points.
"""
struct GHC <: ClusteringMethod
  k::Int
  λ::Float64
  kern::Symbol
  link::Symbol
end

function GHC(k, λ; kern=:epanechnikov, link=:ward)
  # sanity checks
  @assert k > 0 "invalid number of clusters"
  @assert λ > 0 "invalid kernel range"
  @assert kern ∈ [:uniform,:triangular,:epanechnikov] "invalid kernel function"
  @assert link ∈ [:single,:average,:complete,:ward,:ward_presquared] "invalid linkage function"
  GHC(k, λ, kern, link)
end

function partition(data, method::GHC)
  # GHC parameters
  k    = method.k
  λ    = method.λ
  kern = method.kern
  link = method.link

  # dissimilarity matrix
  D = ghc_dissimilarity_matrix(data, kern, λ)

  # classical hierarchical clustering
  tree = hclust(D, linkage=link)

  # cut tree to produce clusters
  labels = cutree(tree, k=k)

  # convert labels to subsets
  maxlabel = maximum(labels)
  subsets  = [Int[] for i in 1:maxlabel]
  for (i, l) in enumerate(labels)
    push!(subsets[l], i)
  end

  # return partition
  Partition(data, subsets)
end

function ghc_dissimilarity_matrix(data, kern, λ)
  # retrive domain/table
  𝒟 = domain(data)
  𝒯 = values(data)

  # kernel matrix
  K = ghc_kernel_matrix(kern, λ, 𝒟)

  # difference matrices
  Δ = ghc_diff_matrices(𝒯)

  # sum of cross-variograms
  Γ = ghc_variogram_sum(K, Δ)
end

function ghc_kernel_matrix(kern, λ, 𝒟)
  # kernel function
  fn    = kernfun[kern]
  Kλ(h) = fn(h, λ=λ)

  # collect coordinates
  coords = [coordinates(centroid(𝒟, i)) for i in 1:nelements(𝒟)]

  # lag matrix
  H = pairwise(Euclidean(), coords)

  # kernel matrix
  K = Kλ.(H)

  # return sparse version
  sparse(K)
end

function ghc_diff_matrices(𝒯)
  # covariates as columns
  covars = Tables.columntable(𝒯)

  # number of covariates
  p = length(covars)

  # one matrix per covariate pair
  Δ = Matrix{Matrix{Float64}}(undef, p, p)
  @inbounds for j in 1:p
    Zj = ghc_normalize(covars[j])
    Δj = pairwise(Euclidean(), Zj)
    for i in j+1:p
      Zi = ghc_normalize(covars[i])
      Δi = pairwise(Euclidean(), Zi)
      Δ[i,j] = Δi .* Δj
    end
    Δ[j,j] = Δj .* Δj
    for i in 1:j-1
      Δ[i,j] = Δ[j,i] # leverage the symmetry
    end
  end

  Δ
end

function ghc_variogram_sum(K, Δ)
  n = size(K, 1)
  Γ = zeros(n, n)
  for Δₒ in Δ # for each covariate pair
    # update lower triangular matrix
    @inbounds for j in 1:n
      kj = K[:,j]
      for i in j+1:n
        ki = K[:,i]
        Kij = kron(ki, kj)
        I, W = findnz(Kij)
        num = sum(W .* Δₒ[I], init=0.0)
        den = sum(W, init=0.0)
        iszero(den) || (Γ[i,j] += (1/2) * (num/den))
      end
    end
  end

  # mirror upper triangular matrix
  @inbounds for j in 1:n
    Γ[j,j] = 0.0
    for i in 1:j-1
      Γ[i,j] = Γ[j,i] # leverage the symmetry
    end
  end

  Γ
end

function ghc_normalize(x)
  μ = mean(x)
  σ = std(x, mean=μ)
  (x .- μ) ./ σ
end


# SPDX-License-Identifier: MIT

struct NormalWishart{T1<:AbstractVector{<:AbstractFloat},T2<:AbstractFloat,
                     T3<:AbstractMatrix{<:AbstractFloat},T4<:AbstractFloat} <: ExponentialFamilyDistribution
	μ::T1
	β::T2
	W::T3
	ν::T4
end

function η(p::NormalWishart)
	D = length(p.μ)
    μ, β, W, ν = p.μ, p.β, p.W, p.ν

	M =
	η₁ = β * μ
	η₂ = -β/2
	η₃ = -(1/2)*vec(inv(W) + β * μ*μ')
	η₄ =  (ν - D) / 2
	η = vcat(η₁, η₂, η₃, η₄)
end

function ξ(p::NormalWishart, η)
	D = length(p.μ)
	β = -2*η[D+1]
	μ = (1/β)*η[1:D]
	ν = 2*η[end] + D
    W = inv(Symmetric(reshape(-2*η[D+2:end-1], D, D) - β*(μ*μ')))
	vcat(μ, β, vec(W), ν)
end

function unpack(p::NormalWishart, μ)
	D = length(p.μ)
	Yx = μ[1:D]
	xᵀYx = μ[D+1]
    Y = reshape(μ[D+2:end-1], D, D)
	logdetY = μ[end]
	(Yx, xᵀYx, Y, logdetY)
end

function A(p::NormalWishart, η)
	D = length(p.μ)

	# We don't use `p.W` or `p.ν` to be able to calculate the gradient w.r.t. η.
    β = -2*η[D+1]
	μ = (1/β)*η[1:D]
	ν = 2*η[end] + D
    W⁻¹ = reshape(-2*η[D+2:end-1], D, D) - β*μ*μ'

	(
		- (D/2)*log(β)
		+ (ν/2)*(-logdet(W⁻¹) + D*log(2))
		+ sum(loggamma.((ν + 1 .- (1:D))/2))
	)
end

function sample(p::NormalWishart)
    μ, β, W, ν = p.μ, p.β, p.W, p.ν
    Λ = rand(Distributions.Wishart(ν, W))
    m = rand(Distributions.MultivariateNormal(m, inv(β*Λ)))
	m, Λ
end


import Base.length

using Distributions:Categorical
using StatsFuns: logsumexp
using Scruff.SFuncs: Cat

export
    Particles,
    effective_sample_size,
    normalize_weights,
    log_prob_evidence,
    resample,
    probability,
    marginal

"Sample = Dict{Symbol, Any}"
Sample = Dict{Symbol, Any}

"""
    struct Particles

A structure of particles containing a vector of `Samples` and of log_weights.
"""
struct Particles
    samples::Vector{Sample}
    log_weights::Vector{Float64}
end

"""
    probability(parts::Particles, predicate::Sample -> Bool)::Float64

    Returns the probability that the predicate is satisfied
"""
function probability(parts::Particles, predicate::Function)::Float64
    sum = 0.0
    tot = 0.0
    for i in 1:length(parts.samples)
        w = exp(parts.log_weights[i])
        s = parts.samples[i]
        if predicate(s)
            sum += w
        end
        tot += w
    end
    return sum / tot
end

"""
    marginal(parts::Particles, x::Symbol)::Cat

Returns a Cat representing the marginal distribution over the given symbol according to parts
"""
function marginal(parts::Particles, x::Symbol)::Cat
    d = Dict{Any, Float64}()
    xs = [s[x] for s in parts.samples]
    lws = normalize_weights(parts.log_weights)
    for i in 1:length(parts.samples)
        d[xs[i]] = get(d, xs[i], 0.0) + exp(lws[i])
    end
    ks = Vector{Any}()
    ps = Vector{Float64}()
    for (k,p) in d
        push!(ks, k)
        push!(ps, p)
    end
    return Cat(ks, ps)
end

"""
    expected_sample_size(log_weights::Vector{Float64})

Effective sample size
"""
function effective_sample_size(lws::Vector{Float64})
    log_ess = (2 * logsumexp(lws)) - logsumexp(2 .* lws)
    return exp(log_ess)
end

"""
    normalize_weights(log_weights::Vector{Float64})

Normalize weights
"""
function normalize_weights(lws::Vector{Float64})
    return lws .- logsumexp(lws)
end

function log_prob_evidence(lws::Vector{Float64})
    return logsumexp(lws) - log(length(lws))
end

function resample(ps::Particles, target_num_particles::Int = length(ps.samples))
    lnws = normalize_weights(ps.log_weights)
    weights = exp.(lnws)
    selections = rand(Categorical(weights/sum(weights)), target_num_particles)
    samples = map(selections) do ind
        ps.samples[ind]
    end
    return Particles(samples, zeros(target_num_particles))
end


using OLS
using Base.Test

# write your own tests here

data = readcsv("/home/rudfaden/Dropbox/Archive/Advanced microeconometrics/Class/Week2/wagepan.txt")

# CREATE VARIABLES
nr   = data[:,1]                                                           # Individual identifier
y    = data[:,9]
x    = [data[:,3] data[:,5] data[:,7] data[:,4] data[:,10] data[:,6] data[:,8]]

lbly = "LWAGE"
lblx = ["CONSTANT", "BLACK", "HISP", "EDUC", "EXPER", "EXPERSQ", "MARRIED", "UNION"]

b, se, t, R2, R2adj, F, s2, p=ols(y,x)

olstable("Wage Estimation",lbly,lblx,b,se,t,R2,R2adj,F,s2,p)


# Testing continuous univariate distributions

using Distributions, Random
using Test

using Calculus: derivative

n_tsamples = 100

# additional distributions that have no direct counterparts in R references
@testset "Testing $(distr)" for distr in [Biweight(),
                                          Biweight(1,3),
                                          Epanechnikov(),
                                          Epanechnikov(1,3),
                                          Triweight(),
                                          Triweight(2),
                                          Triweight(1, 3),
                                          Triweight(1)]

    test_distr(distr, n_tsamples; testquan=false)
end

# Test for non-Float64 input
using ForwardDiff
@test string(logpdf(Normal(0,1),big(1))) == "-1.418938533204672741780329736405617639861397473637783412817151540482765695927251"
@test derivative(t -> logpdf(Normal(1.0, 0.15), t), 2.5) ≈ -66.66666666666667
@test derivative(t -> pdf(Normal(t, 1.0), 0.0), 0.0) == 0.0

# Test for numerical problems
@test pdf(Logistic(6,0.01),-2) == 0

@testset "Normal with std=0" begin
    d = Normal(0.5,0.0)
    @test pdf(d, 0.49) == 0.0
    @test pdf(d, 0.5) == Inf
    @test pdf(d, 0.51) == 0.0

    @test cdf(d, 0.49) == 0.0
    @test cdf(d, 0.5) == 1.0
    @test cdf(d, 0.51) == 1.0

    @test ccdf(d, 0.49) == 1.0
    @test ccdf(d, 0.5) == 0.0
    @test ccdf(d, 0.51) == 0.0

    @test quantile(d, 0.0) == -Inf
    @test quantile(d, 0.49) == 0.5
    @test quantile(d, 0.5) == 0.5
    @test quantile(d, 0.51) == 0.5
    @test quantile(d, 1.0) == +Inf

    @test rand(d) == 0.5
    @test rand(MersenneTwister(123), d) == 0.5
end

# Test for parameters beyond those supported in R references
@testset "VonMises with large kappa" begin
    d = VonMises(1.1, 1000)
    @test var(d) ≈ 0.0005001251251957198
    @test entropy(d) ≈ -2.034688918525470
    @test cf(d, 2.5) ≈ -0.921417 + 0.38047im atol=1e-6
    @test pdf(d, 0.5) ≈ 1.758235814051e-75
    @test logpdf(d, 0.5) ≈ -172.1295710466005
    @test cdf(d, 1.0) ≈ 0.000787319 atol=1e-9
end


module DataPreprocessing

using CSV, DataFrames, Plots
using RollingFunctions, StatsBase, ShiftedArrays
using Downloads: download

url = "https://archive.ics.uci.edu/ml/machine-learning-databases/housing/"
dataset = "housing.data"
datadict = "housing.names"

download("$(url)$(dataset)", dataset)
download("$(url)$(datadict)", datadict)

# The row data is split into lines and delimited by varying amounts of whitespace
rows = readlines(open(dataset))
rowdata = map(row -> join(split(row), ","), rows)
csvdata = write(open("housing.csv", "w"), join(rowdata, "\n"))

# According to the data dictionary, the column names are as follows
columns = [:CRIM, :ZN, :INDUS, :CHAS, :NOX, :RM, :AGE, :DIS, :RAD, :TAX, :PTRATIO, :B, :LSTAT, :MEDV]

# Read the data and add the column header
df = CSV.read("housing.csv", DataFrame; header=columns)

# Descriptive statistics
size(df)
describe(df)

# Identify nominal and continuous variables
nominal = [:CHAS, :RAD]
continuous = [:CRIM, :ZN, :INDUS, :NOX, :RM, :AGE, :DIS, :TAX, :PTRATIO, :B, :LSTAT]

# Examine the distributions of the nominal variables
# Make a table of the counts of the level

histogram(df[!,:RAD], bins=0:25)
histogram(df[!, :CHAS])

radcount = counts(df[!, :RAD])
chascount = counts(df[!, :CHAS])

# Make histograms for all of the nominal variables

plot(
    histogram(df[:, :CRIM]),
    histogram(df[:, :ZN]),
    histogram(df[:, :INDUS]),
    histogram(df[:, :NOX]),
    histogram(df[:, :RM]),
    histogram(df[:, :AGE]),
    histogram(df[:, :DIS]),
    histogram(df[:, :TAX]),
    histogram(df[:, :PTRATIO]),
    histogram(df[:, :B]),
    histogram(df[:, :LSTAT]),
    histogram(df[:, :MEDV])
)

# :MEDV has a strange spike at the value 50. Try removing any exactly equal to 50.

df = df[df[:, :MEDV] .!== 50.0, :]

# Calculate the correlation every variable has with :MEDV

correlation_matrix = mapreduce(permutedims, vcat, [[corkendall(df[:, column], df[:, variable]) for variable in columns] for column in columns])

# Display it as a heat map

heatmap(correlation_matrix, xlabel=columns, ylabel=columns)

# Make a dataframe
df1 = DataFrame(correlation_matrix, columns)
df1[!, :ROWLABEL] = columns

# Sort
df2 = sort(df1, :MEDV, rev=true)

# Linear regression
cols = [:CRIM, :ZN, :INDUS, :NOX, :RM, :AGE, :DIS, :TAX, :PTRATIO, :LSTAT, :MEDV]
y = df[!, :MEDV]
X_n = Dict(col => df[!, col] for col in cols)

models = Dict(
    :CRIM => lm(@formula(MEDV ~ 1 + CRIM), df),
    :ZN => lm(@formula(MEDV ~ 1 + ZN), df),
    :INDUS => lm(@formula(MEDV ~ 1 + INDUS), df),
    :NOX => lm(@formula(MEDV ~ 1 + NOX), df),
    :RM => lm(@formula(MEDV ~ 1 + RM), df),
    :AGE => lm(@formula(MEDV ~ 1 + AGE), df),
    :DIS => lm(@formula(MEDV ~ 1 + DIS), df),
    :TAX => lm(@formula(MEDV ~ 1 + TAX), df),
    :PTRATIO => lm(@formula(MEDV ~ 1 + PTRATIO), df),
    :LSTAT => lm(@formula(MEDV ~ 1 + LSTAT), df),
    :MEDV => lm(@formula(MEDV ~ 1 + MEDV), df),
)

plots = Dict(i => scatter(X_n[i], y, smooth=true, xlabel=i, legend=false)
            for (i, model) in models)
n
plot(values(plots)..., size=(2000, 1000))

# Feature Selection! Remove B, Convert ZN to categorical, Transform CRIM
# B removed

end # module


function check_dist_numerical(dist, chn; mean_tol = 0.1, var_atol = 1.0, var_tol = 0.5)
    @testset "numerical" begin
        # Extract values.
        chn_xs = chn[1:2:end, :x, :].value

        # Check means.
        dist_mean = mean(dist)
        mean_shape = size(dist_mean)
        if !all(isnan, dist_mean) && !all(isinf, dist_mean)
            chn_mean = Array(mean(chn_xs, dims=1))
            chn_mean = length(chn_mean) == 1 ?
                chn_mean[1] :
                reshape(chn_mean, mean_shape)
            atol_m = length(chn_mean) > 1 ?
                mean_tol * length(chn_mean) :
                max(mean_tol, mean_tol * chn_mean)
            @test chn_mean ≈ dist_mean atol=atol_m
        end

        # Check variances.
        # var() for Distributions.MatrixDistribution is not defined
        if !(dist isa Distributions.MatrixDistribution)
            # Variance
            dist_var = var(dist)
            var_shape = size(dist_var)
            if !all(isnan, dist_var) && !all(isinf, dist_var)
                chn_var = Array(var(chn_xs, dims=1))
                chn_var = length(chn_var) == 1 ?
                    chn_var[1] :
                    reshape(chn_var, var_shape)
                atol_v = length(chn_mean) > 1 ?
                    mean_tol * length(chn_mean) :
                    max(mean_tol, mean_tol * chn_mean)
                @test chn_mean ≈ dist_mean atol=atol_v
            end
        end
    end
end

# Helper function for numerical tests
function check_numerical(chain,
                        symbols::Vector,
                        exact_vals::Vector;
                        atol=0.2,
                        rtol=0.0)
    for (sym, val) in zip(symbols, exact_vals)
        E = val isa Real ?
            mean(chain[sym].value) :
            vec(mean(chain[sym].value, dims=[1]))
        @info (symbol=sym, exact=val, evaluated=E)
        @test E ≈ val atol=atol rtol=rtol
    end
end

# Wrapper function to quickly check gdemo accuracy.
function check_gdemo(chain; atol=0.2, rtol=0.0)
    check_numerical(chain, [:s, :m], [49/24, 7/6], atol=atol, rtol=rtol)
end

# Wrapper function to check MoGtest.
function check_MoGtest_default(chain; atol=0.2, rtol=0.0)
    check_numerical(chain,
        [:z1, :z2, :z3, :z4, :mu1, :mu2],
        [1.0, 1.0, 2.0, 2.0, 1.0, 4.0],
        atol=atol, rtol=rtol)
end


using Test, Limbdark, LinearAlgebra, Statistics, ForwardDiff, DiffResults, Random

include("loglinspace.jl")

include("test_cel_vec.jl")
include("test_s2_gradient.jl")
include("test_transit_poly.jl")
include("test_Mn.jl")

# Skip plots for CI tests
skip_plots = true
include("test_transit_poly_gradient2.jl")
include("test_integrate_transit_simpson_vec.jl")


"""
    GetIndexWithKMP(string::String, sub_string::String, ignore_case::Bool)::Int

Find if a string contain a given sub string with KMP Search, as an explanation might be too lengthy
As such, a detailed explanation can be found at https://towardsdatascience.com/pattern-search-with-the-knuth-morris-pratt-kmp-algorithm-8562407dba5b

Questions and answers
1. Why KMP instead of naive search?
- Given n = length of string , m = length of string P
- Then, the naive search will have time complexity O(mn) to find all occurrences of pattern P in S
- However, knuth-morris-pratt (KMP) algorithm will have tim complexity of O(m+n) to find all occurrences of pattern P in S
- Hence, KMP is more efficient than the naive approach

Example

```julia
GetIndexWithKMP("Hello I am Gervin", "Hello I am Gervin", false) # returns 1
GetIndexWithKMP("ABABDABACDABABCABAB", "ABABCABAB", false) # returns 11
GetIndexWithKMP("SeEms Like IgNOrE CaSe Work", "seems like ignore case work", true) # returns 1
```

Contributed By:- [Gervin Fung](https://github.com/GervinFung)
Note: This function will also allow ignoring cases also

"""

# by convention -1 will be returned if the string does not contain the sub-string given
# in this case, 0 will be returned for Julia with start index of 1
# reason is that -1 is for language with start index of 0
# Should be convenient for others as most programming language use -1 if the string does not contain the sub-string given
const NO_SUBSTRING_INDEX = 0
# 1 is the first index, unlike others, where 0 is the first index
# Should be convenient for others as most programming language use 0 as first index
const JULIA_FIRST_INDEX = 1

function create_suffic_array(
    pattern_length::Int,
    sub_string::String,
)::Vector{Int}

    # Longest Proper Prefix which is Suffix array
    lps::Vector{Int} = ones(Int, pattern_length)

    index::Int = JULIA_FIRST_INDEX
    i::Int = 1 + JULIA_FIRST_INDEX

    while (i < pattern_length)
        if sub_string[i] == sub_string[index]
            lps[i] = index + 1
            index += 1
            i += 1
        else
            if index != 1
                index = lps[index-1]
            else
                lps[i] = 1
                i += 1
            end
        end
    end

    return lps
end

# this function will be used to obtain the index which the substring was found
function get_index_with_kmp(
    string::String,
    sub_string::String,
    ignore_case::Bool,
)::Int
    string = ignore_case ? lowercase(string) : string
    sub_string = ignore_case ? lowercase(sub_string) : sub_string

    string_length::Int = length(string)
    substring_length::Int = length(sub_string)

    lps::Vector{Int} = create_suffic_array(substring_length, sub_string)

    # likewise, 1 is the first index, unlike others, where 0 is the first index
    i::Int = JULIA_FIRST_INDEX
    j::Int = JULIA_FIRST_INDEX

    while i < string_length + 1 && j < substring_length + 1
        if string[i] == sub_string[j]
            i += 1
            j += 1
        else
            if j != 1
                j = lps[j-1]
            else
                i += 1
            end
        end
    end

    return j == substring_length + 1 ? i - j + 1 : NO_SUBSTRING_INDEX
end

# optional function that returns boolean if string does contain the sub-string given
function contain_substring_with_kmp(
    string::String,
    sub_string::String,
    ignore_case::Bool,
)::Bool
    return get_index_with_kmp(string, sub_string, ignore_case) !=
           NO_SUBSTRING_INDEX
end


### Abstract metaheuristic
abstract type AbstractMetaheuristic end

### Clearing
mutable struct Clearing <: AbstractMetaheuristic
	# Parameters
	nind::Int64
	pm::Float64
	pXgene::Float64
	dec::Float64
	η::Float64
	nm::Float64
	crossover::String # crossover type
	kapa::Float64
	sigma::Float64
	norm::Union{Float64, Int64}
	# Population
	population::Array{Float64,2}

	Clearing(;nind = 50,
		pm = 0.05,
		pXgene = 0.05,
		dec = 0.5,
		eta1 = 1.,
		nm = 50.,
		crossover = "auto",
		kapa = 1.,
		sigma = 0.05,
		norm = Inf) =
		new(nind, pm, pXgene, dec, eta1, nm, crossover, kapa, sigma,norm)
end

mutable struct Options
	log::Bool
	reltol::Float64
	itmax_unchanged::Int64
	time_limit::Int64 # in hours
	iterations::Int64
	multithreads::Bool
	verbose::Bool

	Options(;log = true,
		reltol = 1e-10,
		itmax_unchanged = 100,
		time_limit = 168,
		iterations = 100,
		multithreads = true,
		verbose = true) =
		new(log, reltol, itmax_unchanged, time_limit, iterations, multithreads, verbose)
end

### Results
mutable struct MetaheuristicResults
	method::AbstractMetaheuristic
	status::String
	minimizer::AbstractArray
	minimum::Float64
	iterations::Int64
	options::Options
end


# Author: Ashwani Rathee
# Program to Calculate Sum of Natural Numbers

println("Enter n: ")

n=readline()
n=parse(Int64,n)
sum=0
i=1
#for i in 1:n
#    sum=sum+i
#end
while i<=n
      global sum=sum+i
      global i=i+1
end
println("The sum of n-natural terms is ",sum)


function DataSplit(X,Y;p=[0.6,0.2,0.2])
   m=size(X,1)
   TrNumber=round(Int,m*p[1])
   CvNumber=round(Int,m*p[2])
   TsNumber=round(Int,m*p[3])

    #Ramdomly sampled the X inputs
    r = sample(1:m, m, replace = false)
    Xr=X[r,:]
    Yr=Y[r,:]

    #Get X and Y train
    Xtr=Xr[1:TrNumber,:]
    Ytr=Yr[1:TrNumber]

    #Get X and Y CrossValidation
    Xcv=Xr[TrNumber+1:TrNumber+CvNumber,:]
    Ycv=Yr[TrNumber+1:TrNumber+CvNumber]

    #Get X and Y Test
    Xts=Xr[TrNumber+CvNumber+1:end,:]
    Yts=Yr[TrNumber+CvNumber+1:end]

    return Xtr, Ytr, Xcv, Ycv, Xts, Yts

end


using Base.Test
using Snopt
using ForwardDiff
include("dircol.jl")

function gen_usrfun(model, obj, dt, pack, method; grads=:none)
    n,m,N = pack

    # Weights (Trapezoidal)
    weights = get_weights(method,N)

    # Jacobian for Dynamics
    f_aug! = f_augmented!(model!.f, model.n, model.m)
    zdot = zeros(n)
    F(z) = ForwardDiff.jacobian(f_aug!,zdot,z)

    function eval_f(Z)
        X,U = unpackZ(Z,pack)
        cost(X,U,weights,obj)
    end

    function eval_g(Z)
        X,U = unpackZ(Z,pack)
        calc_constraints(X,U,method,dt,model.f)
    end

    function usrfun(Z)
        # Objective Function (Cost)
        J = eval_f(Z)

        # Constraints (dynamics only for now)
        c = Float64[] # No inequality constraints for now
        ceq = eval_g(Z)

        fail = false

        if grads == :none
            return J, c, ceq, fail
        else
            X,U = unpackZ(Z,pack)

            # Gradient of Objective
            if grads == :auto
                grad_f = ForwardDiff.gradient(eval_f,Z)
            else
                grad_f = cost_gradient(X,U,weights,obj)
            end

            # Constraint Jacobian
            jacob_c = Float64[]
            if grads == :auto
                jacob_ceq = ForwardDiff.jacobian(eval_g,Z)
            else
                jacob_ceq = constraint_jacobian(X,U,dt,method,F)
            end

            return J, c, ceq, grad_f, jacob_c, jacob_ceq, fail
        end
    end

    return usrfun, eval_f, eval_g
end

function dircol(model,obj,dt,grads)

    method = :trapezoid
    method = :hermite_simpson_separated

    # Constants
    nSeg = Int(floor(obj.tf/dt)); # Number of segments
    dt = obj.tf/nSeg
    if method == :trapezoid
        N = nSeg + 1
    elseif method == :hermite_simpson_separated
        N = 2*nSeg + 1
    end
    n,m = model.n, model.m
    pack = (n,m,N)

    usrfun, eval_f, eval_g = gen_usrfun(model, obj, dt, pack, method, grads=grads)

    # Set up the problem
    Z0 = get_initial_state(obj,N)
    lb,ub = get_bounds(obj,N)

    # Set options
    options = Dict{String, Any}()
    options["Derivative option"] = 0
    options["Verify level"] = 1

    # Solve the problem
    @time z_opt, fopt, info = snopt(usrfun, Z0, lb, ub, options)
    @time snopt(usrfun, Z0, lb, ub, options)
    # xopt, fopt, info = Z0, Inf, "Nothing"
    x_opt,u_opt = unpackZ(z_opt,pack)

    println(info)
    return x_opt, u_opt, fopt
end

model = TrajectoryOptimization.Dynamics.pendulum[1]
model! = TrajectoryOptimization.Dynamics.pendulum![1]
obj = TrajectoryOptimization.Dynamics.pendulum[2]
obj = TrajectoryOptimization.ConstrainedObjective(obj, u_min=-2, u_max=2)
dt = 0.1

using Juno
# @profiler dircol(model, obj, dt, :other)
x_opt, u_opt, fopt = dircol(model, obj, dt, :none)
plot(x_opt')

weights = get_weights(:hermite_simpson_separated,N)
N = size(x_opt,2)
pack = (model.n,model.m,N)
X,U = x_opt,u_opt
Z = packZ(X,U)
f_aug! = f_augmented!(model!.f, model.n, model.m)
zdot = zeros(model.n)
F(z) = ForwardDiff.jacobian(f_aug!,zdot,z)
usrfun, eval_f, eval_g = gen_usrfun(model,obj,dt, pack, :hermite_simpson_separated, grads=:auto)

# Constraints
jacob_g = constraint_jacobian(X,U,dt,:hermite_simpson_separated,F)
jacob_g_auto = ForwardDiff.jacobian(eval_g,Z)
errors = jacob_g_auto == jacob_g

# Objective
grad_f = cost_gradient(X,U,weights,obj)
grad_f_auto = ForwardDiff.gradient(eval_f,Z)
errors = grad_f - grad_f_auto
errors[end-10:end]
grad_f ≈ grad_f_auto

#
# Z0 = get_initial_state(obj,N)
# options = Dict{String, Any}()
# lb,ub = get_bounds(obj,N)
# options["Derivative option"] = 0
# options["Verify level"] = 1
#
# @time xopt, fopt, info = snopt(gradfree, Z0, lb, ub, options)
# x_soln,u_soln = unpackZ(xopt)
# plot(x_soln')
# plot(u_soln')
# eval_g(xopt)
#
# # Tests
# X,U = unpackZ(Z0)
# X = rand(size(X))
# U = rand(size(U))
# Z = packZ(X,U)
#
# X2,U2 = unpackZ(Z)
# @test X2 == X
# @test U2 == U
#
# # Test cost
# J1 = eval_f(Z)
# J2 = cost(X,U)
# @test J1 == J2
#
# # Test Constraints
# g = eval_g(Z)
#
# # Test Gradient of Objective
# grad_f = zeros((n+m)*N)
# eval_grad_f(Z,grad_f)
# grad_f
# grad_f_auto = ForwardDiff.gradient(eval_f,Z)
# @test grad_f == grad_f_auto
#
# # Gradient of Constraints
# jacob_g = eval_jac_g(Z)
# jacob_g_auto = ForwardDiff.jacobian(eval_g,Z)
# @test jacob_g_auto == jacob_g
#
# # Test usrfun
# f_val,g2,df,dg,fail = usrfun(Z)
# @test f_val == J1
# @test g2 == g
# @test df == grad_f
# @test dg == jacob_g
# @test ~fail


using Test
using NeXLUncertainties

@testset "UncertainValues" begin
    @testset "Label" begin
        l1a = label(123)
        l1b = label(123)
        l2a = nl"X"
        l2b = nl"X"
        l3a = label(123.0)
        l3b = label(123.0)
        l3c = label(123.1)
        @test isequal(l1a, l1b)
        @test isequal(l2a, l2b)
        @test isequal(l3a, l3b)
        @test !isequal(l1a, l2a)
        @test !isequal(l1a, l3a)
        @test !isequal(l2a, l3a)
        @test !isequal(l3a, l3c)
    end

    @testset "Covariances" begin

        cov = [
            0.1^2 -0.3*0.1*0.2 0.0
            -0.3*0.1*0.2 0.2^2 0.2*0.2*0.3
            0.0 0.2*0.2*0.3 0.3^2
        ]
        @test checkcovariance!(cov)

        bad = [
            0.1^2 -0.29*0.1*0.2 0.0
            -0.3*0.1*0.2 0.2^2 0.2*0.2*0.3
            0.0 0.2*0.2*0.3 0.3^2
        ]

        @test_throws ErrorException checkcovariance!(bad)

        lbls = [label("X0"), label("X1"), label("Z")]
        vals = [2.0, 8.0, 12.0]

        uvs1 = uvs(lbls, vals, cov)

        @test σ(uvs1, label("X0")) == 0.100
        @test σ(uvs1, label("X1")) == 0.200
        @test σ(uvs1, label("Z")) == 0.3
        @test σ(uvs1, label("Z")) == 0.3

        @test filter(uvs1, [label("Z"), label("X1")]) ==
              [cov[3, 3] cov[3, 2]; cov[2, 3] cov[2, 2]]

        @test uvs1[label("Z")] == uv(12.0, 0.3)
        @test uvs1[label("X1")] == uv(8.0, 0.2)
        @test get(uvs1, label("X1"), uv(8.1, 0.0)) == uv(8.0, 0.2)
        @test get(uvs1, label("Z"), uv(8.1, 0.0)) ≠ uv(8.0, 0.2)
        @test get(uvs1, label("Zz"), uv(8.1, 0.0)) == uv(8.1, 0.0)

        @test length(uvs1) == 3

        @test value(uvs1, label("Z")) == 12.0
        @test value(uvs1, label("X1")) == 8.0

        @test covariance(uvs1, label("X1"), label("Z")) == 0.2 * 0.2 * 0.3
        @test covariance(uvs1, label("Z"), label("X1")) == 0.2 * 0.2 * 0.3

        @test variance(uvs1, label("Z")) == 0.3^2
        @test variance(uvs1, label("X1")) == 0.2^2

        @test uncertainty(uvs1, label("Z")) == 0.3
        @test uncertainty(uvs1, label("X1")) == 0.2
        @test uncertainty(uvs1, label("Z"), 2.0) == 2.0 * 0.3
        @test uncertainty(uvs1, label("X1"), 3.0) == 3.0 * 0.2
    end

    @testset "cat" begin
        uvs1 = uvs(
            [nl"A", nl"B", nl"C"],
            [1.0, 2.0, 3.0],
            [0.1 0.05 -0.03; 0.05 0.2 0.06; -0.03 0.06 0.3],
        )
        uvs2 = uvs(
            [nl"D", nl"E", nl"F"],
            [4.0, 5.0, 6.0],
            [0.2 0.1 -0.13; 0.1 0.3 -0.12; -0.13 -0.12 0.1],
        )
        uvs3 = cat([uvs1, uvs2])
        @test value(uvs3, nl"A") == 1.0
        @test value(uvs3, nl"C") == 3.0
        @test value(uvs3, nl"A") == value(uvs1, nl"A")
        @test value(uvs3, nl"C") == value(uvs1, nl"C")
        @test value(uvs3, nl"E") == value(uvs2, nl"E")
        @test value(uvs3, nl"F") == value(uvs2, nl"F")
        @test covariance(uvs3, nl"A", nl"B") == 0.05
        @test covariance(uvs3, nl"E", nl"E") == variance(uvs3, nl"E")
        @test covariance(uvs3, nl"E", nl"E") == 0.3
        @test covariance(uvs3, nl"E", nl"F") == -0.12
        @test covariance(uvs3, nl"B", nl"D") == 0.0
        @test covariance(uvs3, nl"E", nl"E") == variance(uvs3, nl"E")
        @test σ(uvs3, nl"E") == sqrt(0.3)
        @test σ(uvs2, nl"E") == σ(uvs3, nl"E")

        ex = filter(uvs3, [nl"A", nl"C", nl"E"])
        @test ex[1, 1] == covariance(uvs3, nl"A", nl"A")
        @test ex[1, 2] == covariance(uvs3, nl"A", nl"C")
        @test ex[1, 2] == ex[2, 1]
        @test ex[3, 1] == covariance(uvs3, nl"E", nl"A")
        @test ex[1, 3] == ex[3, 1]

        @test uvs3[nl"E"] == UncertainValue(5.0, sqrt(0.3))
        @test uvs3[nl"C"] == UncertainValue(3.0, sqrt(0.3))
        @test get(uvs3, nl"C", missing) == UncertainValue(3.0, sqrt(0.3))
        @test ismissing(get(uvs3, nl"XX", missing))
        @test get(uvs3, nl"C", missing) == uvs3[nl"C"]
        @test get(uvs3, nl"C", UncertainValue(3.2, sqrt(0.99))) == uvs3[nl"C"]
        @test get(uvs3, nl"zZ", UncertainValue(3.2, sqrt(0.99))) ==
              UncertainValue(3.2, sqrt(0.99))

        @test length(uvs3) == 6
        @test length(uvs1) == 3

        vals = values(uvs3)
        lbls = labels(uvs3)
        @test value(uvs3[lbls[1]]) == vals[1]
        @test value(uvs3[lbls[3]]) == vals[3]

        @test uncertainty(uvs3, nl"A", 2.0) == 2.0 * sqrt(0.1)
    end
end


mutable struct Rotational{T,Nc} <: Joint{T,Nc}
    V12::SMatrix{2,3,T,6}
    V3::Adjoint{T,SVector{3,T}}
    qoff::Quaternion{T}
    cid::Int64

    function Rotational{T,Nc}(body1::AbstractBody{T}, body2::AbstractBody{T}; axis::AbstractVector{T} = zeros(3), offset::Quaternion{T} = Quaternion{T}()) where {T,Nc}
        axis = vrotate(SVector(axis...), inv(offset))
        if norm(axis) != 0
            axis = axis / norm(axis)
        end
        A = svd(skew(axis)).Vt # in frame of body1
        V12 = A[1:2,:]
        V3 = axis' # instead of A[3,:] for correct sign: abs(axis) = abs(A[3,:])
        cid = body2.id

        new{T,Nc}(V12, V3, offset, cid), body1.id, body2.id
    end
end

Rotational0 = Rotational{T,0} where T
Rotational1 = Rotational{T,1} where T
Rotational2 = Rotational{T,2} where T
Rotational3 = Rotational{T,3} where T


immutable EnvironmentRegion{N, T}
    position::SimpleHRepresentation{N, T}
    force::SimpleHRepresentation{N, T}
end
dimension{N, T}(::Type{EnvironmentRegion{N, T}}) = N

function isfree(region::EnvironmentRegion)
    vr = vrep(polyhedron(region.force))
    !hasrays(vr) && all(all(p .== 0) for p in points(vr))
end

function lphrep_with_bounds(vr::VRepresentation)
    hr = hrep(polyhedron(vr, CDDLibrary()))
    lphr = LPHRepresentation(hr)
    l, u = axis_aligned_bounding_box(vr)
    LPHRepresentation(lphr.A, l, u, lphr.lb, lphr.ub)
end

# Forces
function friction_cone_constraints(n, μ, fnmax)
    n⟂ = perp(n)
    β1 = normalize(n + μ * n⟂) * fnmax
    β2 = normalize(n - μ * n⟂) * fnmax
    vr = SimpleVRepresentation([zeros(1, length(n⟂)); β1'; β2'])
    SimpleHRepresentation(hrep(polyhedron(vr, CDDLibrary())))
end

function no_force_constraints(n)
    SimpleHRepresentation(eye(2), zeros(2), IntSet([1, 2]))
end

# Positions
function line_segment_constraints(p1, p2)
    vr = SimpleVRepresentation([p1'; p2'])
    SimpleHRepresentation(hrep(polyhedron(vr, CDDLibrary())))
end

function axis_aligned_free_box(p1, p2)
    l = min.(p1, p2)
    u = max.(p1, p2)
    n = length(l)
    SimpleHRepresentation([eye(n); -eye(n)], [u; -l])
end

# Position × Force environment regions
function contact_region(p1, p2, μ, fnmax)
    n = normalize(perp(p2 - p1))
    position = line_segment_constraints(p1, p2)
    force = friction_cone_constraints(n, μ, fnmax)
    EnvironmentRegion(position, force)
end

function axis_aligned_free_box_region(p1, p2)
    position = axis_aligned_free_box(p1, p2)
    force = no_force_constraints(length(p1))
    EnvironmentRegion(position, force)
end

function force_bounds{T<:EnvironmentRegion}(environment::AbstractVector{T})
    # TODO: could do this on a contact-by-contact basis if certain contact-region assignments are not allowed
    n = dimension(T)
    fmin = zeros(n)
    fmax = zeros(n)
    for region in environment
        fl, fu = axis_aligned_bounding_box(region.force)
        fmin .= min.(fmin, fl)
        fmax .= max.(fmax, fu)
    end
    fmin, fmax
end


using Gen
import LinearAlgebra: eigvals
import DifferentialEquations: solve, ODEProblem, OrdinaryDiffEq

# --------------------------------------------------------------------------------------------
# We create a type for the parameter associated to the predator-prey differential equation
# --------------------------------------------------------------------------------------------
mutable struct PredatorPreyParameter
  P0::Float64
  Q0::Float64
  r::Float64
  K::Float64
  s::Float64
  a::Float64
  u::Float64
  v::Float64
  start_time::Float64
  stop_time::Float64
  solution::Union{Nothing, OrdinaryDiffEq.ODECompositeSolution}
end
Base.Array(p::PredatorPreyParameter) = [
  p.P0, p.Q0, p.r, p.K, p.s, p.a, p.u, p.v, p.start_time, p.stop_time, p.solution
]
init(p::PredatorPreyParameter) = [p.P0, p.Q0]
ode_params(p::PredatorPreyParameter) = [p.r, p.K, p.s, p.a, p.u, p.v]
times(p::PredatorPreyParameter) = (p.start_time, p.stop_time)
Base.display(p::PredatorPreyParameter) = print(
"""
PredatorPreyParameter ($(isnothing(p.solution) ? "without" : "with") solution):
  P0: $(p.P0)
  Q0: $(p.Q0)
  r: $(p.r)
  K: $(p.K)
  s: $(p.s)
  a: $(p.a)
  u: $(p.u)
  v: $(p.v)
  start_time: $(p.start_time)
  stop_time: $(p.stop_time)
""")

struct PredatorPreyPrior <: Gen.Distribution{PredatorPreyParameter} end
const predator_prey_prior = PredatorPreyPrior()

# --------------------------------------------------------------------------------------------
# Use DifferentialEquations.jl to simulate solutions
# --------------------------------------------------------------------------------------------
function predator_prey_system(dy, y, p, t)
  r, K, s, a, u, v = p
  dy[1] = r * y[1] * (1 - y[1] / K) - s * y[1] * y[2] / (a + y[1])
  dy[2] = u * y[1] * y[2] / (a + y[1]) - v * y[2]
end

function predator_prey_problem(parameter::PredatorPreyParameter)
  ODEProblem(predator_prey_system, init(parameter), times(parameter), ode_params(parameter))
end

# --------------------------------------------------------------------------------------------
# Prior distribution imposes domain knowledge: parameters must produce stable populations
# --------------------------------------------------------------------------------------------
function stable!(parameter::PredatorPreyParameter)::Bool
  # if we have a solution already, we are good
  if !isnothing(parameter.solution)
    return true
  end

  # unpack the parameter
  P0, Q0 = init(parameter)
  r, K, s, a, u, v = ode_params(parameter)

  # if P0 > K, population is already above carrying capacity
  if P0 > K
    return false
  end

  # stable point must be positive and under carrying capacity
  Pf = a * v / (u - v)
  Qf = a * r * (1 + v / (u - v)) * (K - a * v / (u - v)) / (K * s)
  if (Pf <= 0 || Pf > K || Qf <= 0)
    return false
  end

  # Jacobian at stable point must have eigenvalues with (almost) positive real components
  jacobian = [
    (r - 2r*Pf/K - s*((a+Pf)*Qf - Pf*Qf)/(a+Pf)^2) (-s*Pf/(a+Pf)) ;
    (u*((a+Pf)*Qf - Pf*Qf)/(a+Pf)^2) (u*Pf/(a+Pf) - v)
  ]
  lam, = eigvals(jacobian)
  if real(lam) < -1e-14
    return false
  end

  # we passed cyclic condition; check if numerically stable
  solution = solve(predator_prey_problem(parameter), verbose=false)
  if solution.retcode === :Success
    parameter.solution = solution
    return true
  end
  return false
end

function Gen.random(
  dist::PredatorPreyPrior,
  lower::Float64,
  upper::Float64,
  start_time::Float64,
  stop_time::Float64
)
  # parse arguments
  if lower <= 0
    throw(ArgumentError("PredatorPreyPrior must assume positive parameters"))
  end

  # rejection sample uniform
  while true
    Q0, r, K, s, a, u, v = [rand() * (upper-lower) + lower for _=1:7]
    P0 = rand() * (K - lower) + lower
    p = PredatorPreyParameter(P0, Q0, r, K, s, a, u, v, start_time, stop_time, nothing)

    if stable!(p)
      return p
    end
  end
end

function Gen.logpdf(
  dist::PredatorPreyPrior,
  param::PredatorPreyParameter,
  lower::Float64,
  upper::Float64,
  start_time::Float64,
  stop_time::Float64
)
  is_stable = stable!(param)
  same_times = (start_time, stop_time) == times(param)
  possible = same_times && all(x -> x >= lower && x <= upper, Array(param)[1:8]) && is_stable
  possible ? -Inf : 0.0
end
Gen.has_output_grad(dist::PredatorPreyPrior) = true
Gen.has_argument_grads(dist::PredatorPreyPrior) = (true, true)
Gen.logpdf_grad(dist::PredatorPreyPrior, value::PredatorPreyParameter, lower::Float64, upper::Float64) = (0.0, 1.0, -1.0)

@gen function measurement(mu::Vector{Float64}, var::Float64)
  @trace(mvnormal(mu, [var 0.0 ; 0.0 var]), :z)
end

@gen function predator_prey_model(
  lower_prior::Float64,
  upper_prior::Float64,
  start_time::Float64,
  stop_time::Float64,
  timesteps::Int64,
  measurement_noise::Float64
)
  # sample our custom prior
  parameter = @trace(
    predator_prey_prior(lower_prior, upper_prior, start_time, stop_time),
    :predator_prey_parameter
  )

  # simulate measurements
  times = LinRange(start_time, stop_time, timesteps+1)[2:end]
  means = map(t -> parameter.solution(t), times)
  noises = fill(measurement_noise, timesteps)
  @trace(Map(measurement)(means, noises), :measurements)
end


# Zinc Blend - test

A="Ga"
C="As" # Old friend

const hbarsquaredbym = 7.62 # eV.Å^2

V(eta,d)=eta*hbarsquaredbym/d^2
ixp(f)=exp(im*f) # imaginary exponential shorthand... I think as used in Wannier's solid state book?

# See page 76 - simple tetrahedral
# ηssσ, ηspσ, ηppσ, ηppπ

# Covalent, apparently, with GaAs
d=SST[AtoZ[A]].rc + SST[AtoZ[C]].rc
println("Distance: $d")

H=HarrisonIME # using traditional Harrison site params

# p 76, left hand side
Ess=V(H.ηssσ,d)
Esp=-V(H.ηspσ,d)/3^(1/2)
Exx=1/3*V(H.ηppσ,d) + 2/3*V(H.ηppπ,d)
Exy=1/3*V(H.ηppσ,d) - 1/3*V(H.ηppπ,d)

const ϵsc = -SST[AtoZ[C]].ϵs
const ϵsa = -SST[AtoZ[A]].ϵs
const ϵpc = -SST[AtoZ[C]].ϵp
const ϵpa = -SST[AtoZ[A]].ϵp

# Gamma point for now; e^0 = 1
a=d

d1=[ 1  1  1]'*a/4
d2=[ 1 -1 -1]'*a/4
d3=[-1  1 -1]'*a/4
d4=[-1 -1  1]'*a/4

# This is a terrible function currently; not very hygenic at all
function bandsfork(k)
    g0=ixp(k⋅d1) + ixp(k⋅d2) + ixp(k⋅d3) + ixp(k⋅d4)
    g1=ixp(k⋅d1) + ixp(k⋅d2) - ixp(k⋅d3) - ixp(k⋅d4)
    g2=ixp(k⋅d1) - ixp(k⋅d2) + ixp(k⋅d3) - ixp(k⋅d4)
    g3=ixp(k⋅d1) - ixp(k⋅d2) - ixp(k⋅d3) + ixp(k⋅d4)

    # Gamma point only...
    #g0=1.0
    #g1=1.0
    #g2=1.0
    #g3=1.0

    g0s=conj(g0)
    g1s=conj(g1)
    g2s=conj(g2)
    g3s=conj(g3)

    # Table 3--1, p. 77
    LCAOH= [
    ϵsc       Ess*g0  0        0        0        Esp*g1  Esp*g2  Esp*g3 ;
    Ess*g0    ϵsa    -Esp*g1s -Esp*g2s -Esp*g3s  0       0       0      ;
    0        -Esp*g1  ϵpc      0        0        Exx*g0  Exy*g3  Exy*g2 ;
    0        -Esp*g2  0        ϵpc      0        Exy*g3  Exx*g0  Exy*g1 ;
    0        -Esp*g3  0        0        ϵpc      Exy*g2  Exy*g1  Exx*g0 ;
    Esp*g1s  0        Exx*g0s  Exy*g3s  Exy*g2s  ϵpa     0       0      ;
    Esp*g2s  0        Exy*g3s  Exx*g0s  Exy*g1s  0       ϵpa     0      ;
    Esp*g3s  0        Exy*g2s  Exy*g1s  Exx*g0s  0       0       ϵpa   ]

    println("\nLCAOH matrix @ k=$k: ")
    display(LCAOH)

    println("\nEigendecomposition: ")
    display(eigvals(LCAOH)) # Do ya thing Liner Algebra!

    return eigvals(LCAOH)
end

# Point in k-space
k=[0.0 0.0 0.0]'

bands=[]

for r in 0:1.0:1.0
    k=[r r 0]' # Gamma -> X?
    append!(bands,bandsfork(k))
end

#display(bands)
#print(bands)


"""
    Double integrator
"""

struct DoubleIntegrator{I, T} <: Model{I, T}
    n::Int
    m::Int
    d::Int
end

function fd(model::DoubleIntegrator{Discrete, FixedTime}, x⁺, x, u, w, h, t)
    x⁺ - [x[1] + x[2] + w[1]; x[2] + u[1] + w[2]]
end

function fd(model::DoubleIntegrator{Discrete, FixedTime}, x, u, w, h, t)
    [x[1] + x[2] + w[1]; x[2] + u[1] + w[2]]
end

function get_dynamics(model::DoubleIntegrator)
    A = @SMatrix [1.0 1.0; 0.0 1.0]
    B = @SMatrix [0.0; 1.0]

    return A, B
end

n, m, d = 2, 1, 2
model = DoubleIntegrator{Discrete, FixedTime}(n, m, d)

"""
    continuous-time double integrator
"""
struct DoubleIntegratorContinuous{I, T} <: Model{I, T}
    n::Int
    m::Int
    d::Int
end

function f(model::DoubleIntegratorContinuous, x, u, w)
    [x[2] + w[1]; u[1] + w[2]]
end

state_output(model::DoubleIntegratorContinuous, z) = z

model_con = DoubleIntegratorContinuous{Midpoint, FixedTime}(n, m, d)

# visualization
function visualize!(vis, model::DoubleIntegrator, x;
        color = RGBA(0.0, 0.0, 0.0, 1.0),
        r = 0.1, Δt = 0.1)

    default_background!(vis)

    setobject!(vis["particle"], Sphere(Point3f0(0.0),
        convert(Float32, r)),
        MeshPhongMaterial(color = color))

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

    T = length(x)
    for t = 1:T
        pos = [x[t][1]; 0.0; 0.0]

        MeshCat.atframe(anim,t) do
            settransform!(vis["particle"], Translation(pos))
        end
    end

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

    MeshCat.setanimation!(vis, anim)
end


using Test
using Random
using Distributions
using SparseArrays

include("../src/LogisticRegression.jl")
lr = LogisticRegression
@testset "basic_functionality" begin
    @test lr.sigmoid(3) == 0.9525741268224334
    @test lr.sigmoid(.5) == 0.6224593312018546
    buncha_vals = [2,3,.5,1]
    res = lr.par_sigmoid(buncha_vals)
    @test res ==  [ 0.8807970779778823,
                    0.9525741268224334,
                    0.6224593312018546,
                    0.7310585786300049 ]
end
@testset "logistic regression" begin
    num_instances = 5000
    # generate dense training data
    x1_v1 = rand(Normal(0, 2), num_instances)
    x1_v2 = rand(Normal(0, .3), num_instances)
    x2_v1 = rand(Normal(4, 1), num_instances)
    x2_v2 = rand(Normal(2, .3), num_instances)

    x1 = [x1_v1 x1_v2]
    x2 = [x2_v1 x2_v2]
    data = [x1 ; x2]
    labels = [zeros(num_instances) ; ones(num_instances)]

    # generate dense test data
    x3_v1 = rand(Normal(0, 2), num_instances)
    x3_v2 = rand(Normal(0, .3), num_instances)
    x4_v1 = rand(Normal(4, 1), num_instances)
    x4_v2 = rand(Normal(2, .3), num_instances)
    x3 = [x3_v1 x3_v2]
    x4 = [x4_v1 x4_v2]
    test_data = [x3 ; x4]
    test_labels = [zeros(num_instances) ; ones(num_instances)]

    model = lr.LogisticRegressor(add_intercept=true)
    lr.fit!(model, data, labels)
    test_preds = lr.predict(model, test_data)
    acc = sum(test_preds .== test_labels)/length(test_labels)
    @test acc > .95 # this should be very easy to do well
    # since the distributions have exactly the same properties for
    # test and train and there's minimal overlap in classes
end
@testset "sparse logistic regression" begin
    num_instances = 5000
    # generate sparse training data
    x1_v1 = sparse(rand(Binomial(4, .2), num_instances))
    x1_v2 = sparse(rand(Binomial(8, .1),
    num_instances))
    x2_v1 = sparse(rand(Binomial(4, .8), num_instances))
    x2_v2 = sparse(rand(Binomial(8, .7), num_instances))

    x1 = [x1_v1 x1_v2]
    x2 = [x2_v1 x2_v2]
    data = [x1 ; x2]
    labels = [zeros(num_instances) ; ones(num_instances)]

    # generate sparse test data
    x3_v1 = sparse(rand(Binomial(4, .2), num_instances))
    x3_v2 = sparse(rand(Binomial(8, .1),
    num_instances))
    x4_v1 = sparse(rand(Binomial(4, .8), num_instances))
    x4_v2 = sparse(rand(Binomial(8, .7), num_instances))
    x3 = [x3_v1 x3_v2]
    x4 = [x4_v1 x4_v2]
    test_data = [x3 ; x4]
    test_labels = [zeros(num_instances) ; ones(num_instances)]

    model = lr.LogisticRegressor(add_intercept=true)
    lr.fit!(model, data, labels)
    test_preds = lr.predict(model, test_data)
    acc = sum(test_preds .== test_labels)/length(test_labels)
    @test acc > .95 # this should be very easy to do well
    # since the distributions have exactly the same properties for
    # test and train and there's minimal overlap in classes
end


#=
Shows

@author : Spencer Lyon <spencer.lyon@nyu.edu>
          Victoria Gregory <victoria.gregory@nyu.edu>

@date: 2014-08-05

References
----------

http://quant-econ.net/career.html
=#

srand(41)  # reproducible results
wp = CareerWorkerProblem()
v_init = fill(100.0, wp.N, wp.N)
func(x) = update_bellman(wp, x)
v = compute_fixed_point(func, v_init, max_iter=500, verbose=false)

surface(wp.theta, wp.epsilon, v', alpha=0.5,
        linewidth=0.25, zlims=(150, 200),
        xlabel="theta", ylabel="epsilon", cbar=false)


module Variogram
using Distances
using SmoothingSplines
using Plots

export variogram
export getdecorrelationlength

"""
    Utility type to collect values for a semivariogram bin
"""
type BuildBin
    sumofsquares::Float64
    entries::Int

    BuildBin() = new(0.0, 0)
end

"""
   Add a value to a mean
"""
function add(x::BuildBin, value::Float64)
    x.sumofsquares += value^2
    x.entries += 1
end

"""
   Calculate the final value of a mean
"""
function mean(x::BuildBin)
    result::Float64 = NaN

    if x.entries > 0
        result = x.sumofsquares / (2 * x.entries)

    end

    return result
end

"""
    Computes a variogram for a set of geographical points.

    The passed in `pointdata` must be an array in which is row
    contains `lon`, `lat`, `value`.

    `binsize` defines the size of the bins in the variogram,
    in kilometres.

"""
function variogram(pointdata::Array{Float64,2}, binsize::Int64)

    @assert length(pointdata[1, :]) == 3 "Point data must contain 3 columns"
    @assert binsize > 0 "Bin size must be greater than zero"

    # Create the array of bins.
    # The Earth is 40,075km in diameter, so the maximum
    # distance between points is 20,037.5km.
    # So the number of bins is ceil(20,037.5 / binsize).
    bins::Array{BuildBin} = [BuildBin() for i = 1:ceil(20037.5 / binsize)]
    bin_distances::Array{Int64} = collect(0:binsize:20037.5)

    # Go through every pair of points, and
    # add the difference in value between each pair
    # to the appropriate bin
    npoints::Int64 = size(pointdata)[1]

    largest_bin::Int64 = 0

    for i in 1:npoints - 1

        point1::Array{Float64} = pointdata[i, 1:2]
        value1::Float64 = pointdata[i, 3]

        for j in i:npoints

            point2::Array{Float64} = pointdata[j, 1:2]

            distance::Float64 = evaluate(Distances.Haversine(6371), point1, point2)
            bin::Int64 = Int64(floor(distance / binsize)) + 1
            if bin > largest_bin
                largest_bin = bin
            end
            add(bins[bin], abs(pointdata[j, 3] - value1))
        end
    end

    bin_means::Array{Float64} = [mean(i) for i in bins]

    return [bin_distances[1:largest_bin] bin_means[1:largest_bin]]
end

"""
    Get the decorrelation length of a variogram.

    Returns -1 if no decorrelation length can be Calculated
"""
function getdecorrelationlength(variogram::Array{Float64, 2}, plot::Bool)

    local decorrelationlength::Int64 = -1
    fillgaps!(variogram)

    if size(variogram)[1] > 2
        local spline = SmoothingSplines.fit(SmoothingSplines.SmoothingSpline, variogram[:,1], variogram[:,2], 1e6)
        local smoothed = SmoothingSplines.predict(spline)

        if plot
            Plots.plot!(variogram[:,1], smoothed, linewidth=2)
        end

        limitindex::Int64 = getdecorrelationlimit(smoothed)
        if limitindex != -1
            decorrelationlength = variogram[limitindex, 1]
        end
    end

    return decorrelationlength
end

"""
    Fill gaps in a variogram using linear interpolation
"""
function fillgaps!(variogram::Array{Float64,2})

    local lastlag::Int64 = 0
    local currentlag::Int64 = 1

    while currentlag < size(variogram)[1]

        if !isnan(variogram[currentlag, 2])
            lastlag = currentlag
            currentlag +=  1
        else
            local nextgoodlag::Int64 = currentlag + 1
            while isnan(variogram[nextgoodlag, 2])
                nextgoodlag = nextgoodlag + 1
            end

            local gaplength::Int64 = nextgoodlag - lastlag
            local gaprange::Float64 = variogram[nextgoodlag, 2] - variogram[lastlag, 2]
            local stepsize::Float64 = gaprange / gaplength

            local fillstep::Int64 = 0
            for filllag in currentlag:nextgoodlag - 1
                fillstep += 1
                variogram[filllag, 2] = variogram[lastlag, 2] + (stepsize * fillstep)
            end

            currentlag = nextgoodlag + 1
            lastlag = currentlag - 1
        end
    end
end

"""
    Calculate the decorrelation limit for a variogram
    as the first step whose delta is less than 5%
    of the first positive delta of the varioagram.
"""
function getdecorrelationlimit(spline::Array{Float64, 1})

    local deltas::Array{Float64} = [(spline[i + 1] - spline[i]) for i in 1:length(spline) - 1]

    local position::Int64 = 1
    local firstpositivedelta::Float64 = 0

    # Find the first positive delta
    while firstpositivedelta == 0 && position <= length(deltas)
        if deltas[position] > 0
            firstpositivedelta = deltas[position]
        end
        position += 1
    end

    # Find the first delta that's smaller than 5% of the first positive delta
    local decorrelationposition::Int64 = -1

    while decorrelationposition == -1 && position < length(deltas)
        if deltas[position] <= 0 || deltas[position] / firstpositivedelta <= 0.05
            decorrelationposition = position
        else
            position += 1
        end
    end

    return decorrelationposition
end

end #Module


"""
This functions implements the algorithm from the paper
"Six DOF eye-to-hand calibration from 2D measurements using planar constraints"
which solves the problem `Prb = N'A(X*Ps)`
If result is bad, check if you send data in correct form\n
`POSES` is always the position of the tool frame from the robot FK\n
`points_S` are measurements from a line laser scanner. Given
in the sensor frame. This script assumes that the laser plane is the
sensor XY plane with y-axis pointing outwards from the sensor.\n
`lines_S` are vectors corresponding to the direction of the measured laser line
planes is a vector of indices corresponding to which plane a mesurement
comes from. It must be same length as `points_S`.\n
`N_calibs` determines the number of iterations\n
@inproceedings{carlson2015six,
  title={Six DOF eye-to-hand calibration from 2D measurements using planar constraints},
  author={Carlson, Fredrik Bagge and Johansson, Rolf and Robertsson, Anders},
  booktitle={Intelligent Robots and Systems (IROS), 2015 IEEE/RSJ International Conference on},
  pages={3628--3632},
  year={2015},
  organization={IEEE}
}
"""
function calibNAXP(points_S, lines_S, POSES, T_TF_S, planes,  N_calibs; doplot=false)
    N_planes = maximum(planes)
    RMScalibs = zeros(N_calibs)
    ALLcalibs = zeros(N_calibs,N_planes)
    for c = 1:N_calibs
        N_poses = size(POSES,3)
        # Convert points to RB cordinate system
        points = zeros(4,N_poses)
        lines = zeros(3,N_poses)
        for i = 1:N_poses
            T_RB_S = POSES[:,:,i] * T_TF_S
            points[:,i] = T_RB_S*[points_S[:,i]; 1]
            lines[:,i] = T2R(T_RB_S)*lines_S[:,i]
        end

        # Find plane centers, normals and distances
        mu_RB = zeros(3,N_poses)
        N_RB = mu_RB
        normals = zeros(N_planes,3)
        for j = 1:N_planes
            ind = planes .== j
            mu_t = mean(points[1:3,ind],2)
            mu_RB[:,ind] = repmat(mu_t,1,sum(ind))
            D,V = eig(cov(points[1:3,ind]'))
            N_RBt = V[:,1]
            if vecdot(mu_t,N_RBt) < 0
                N_RBt = -1*N_RBt
            end
            N_RBt = N_RBt*(N_RBt'mu_t)
            N_RB[:,ind] = repmat(N_RBt,1,sum(ind))
            normals[j,:] = N_RBt
        end

        # Estimation
        A = zeros(2*N_poses,9)
        y = zeros(2*N_poses,1)
        for i = 1:N_poses
            Nt = N_RB[:,i]
            Ra = POSES[1:3,1:3,i]
            Ta = POSES[1:3,4,i]
            Pt = points_S[:,i]
            y[i] = norm(N_RB[:,i])^2-vecdot(Nt,Ta)
            A[i,:] = (reshape(repmat(Nt'Ra,3,1)',9,1).*[reshape(repmat(Pt[1:2],1,3)',6,1);1;1;1])' #TODO: rewrite
            if true
                Pt = points_S[:,i] + 0.1*1.01^c*randn()*lines_S[:,i]
                y[i+N_poses] = norm(N_RB[:,i])^2-vecdot(Nt,Ta)
                A[i+N_poses,:] = (reshape(repmat(Nt'Ra,3,1)',9,1) .*[reshape(repmat(Pt[1:2],1,3)',6,1);1;1;1])'
            end

        end
        w = A\y
        er = y-A*w

        H = reshape(w,3,3)
        Rx = H[1:3,1]
        Ry = H[1:3,2]
        Rz = cross(Rx,Ry) # These lines must be changed if laser plane is changed
        Rnn = [Rx Ry Rz]
        @show Rnn
        @edb R = toOrthoNormal(Rnn)
        t = H[1:3,3]

        w2 = R[1:3,1:2]
        w2 = w2[:]
        y2 = y - A[:,1:6]*w2
        t2 = A[:,7:9]\y2
        if c < 0 #N_calibs
            T_TF_S = Rt2T(R,t)
        else
            T_TF_S = Rt2T(R,t2)
        end
        RMSi = zeros(N_planes)
        for j = 1:N_planes
            ind = planes .== j
            ind = find(ind)
            RMSi[j] = sqrt(pointDiff(T_TF_S,POSES[:,:,ind],points_S[1:3,ind])[1])
        end
        RMScalibs[c] = mean(RMSi)
        ALLcalibs[c,:] = RMSi'
        #  any(abs(RMSi) > 1e-1) && warn("Points does not seem to lie on a plane")
    end
    if doplot
        plotPlanes(normals)
        plotLines(points,lines)
    end
    norms = 0 # TODO: not implemented
    T_TF_S, RMScalibs,ALLcalibs, norms
end

function plotLines(points,lines)
    P = size(points,2)
    for i = 1:P
        p = points[1:3,i]
        l = lines[1:3,i]
        plot3smart([p'+0.05*l'; p'-0.05*l'],c=:red)
    end
end

function plotPlanes(normals)
    P = size(normals,1)
    for i = 1:P
        n = normals[i,:]'
        xdir = n+[1, 0, 0]
        ydir = normalize(cross(n,xdir))
        xdir = normalize(cross(ydir,n))

        p[:,1] = n + xdir + ydir
        p[:,2] = n + xdir - ydir
        p[:,3] = n - xdir - ydir
        p[:,4] = n - xdir + ydir

        fill3(p[1,:],p[2,:],p[3,:])
    end
    plot!(alpha=0.5, zlims=(-0.3, 1))
end

function pointDiff(T_TF_S, T_RB_TF,points_S)
    loops = 1
    if size(T_TF_S,1) != 4
        loops = size(T_TF_S,2)
    end
    d = zeros(loops)
    for p = 1:loops
        if size(T_TF_S,1) != 4
            T_TF_S = Rt2T(rpy2R(T_TF_S[4:6,p]'),T_TF_S[1:3,p])
        end
        points = zeros(4,size(points_S,2))
        for i = 1:size(T_RB_TF,3)
            T_RB_S = T_RB_TF[:,:,i]*T_TF_S
            points[:,i] = T_RB_S*[points_S[:,i];1]
        end
        points = points[1:3,:]
        U,S,V = svd(points')
        d[p] = S[3]
        # d = max(SCOREp(:,3))^2;
        # d = mean(abs(SCOREp(:,3)))^2;
    end
    d
end


# Interactive plot arrays: random walks as examples

using PlotlyJS
using Distributions

# Simulate some random walks
labs = ["J","U","L","I","A"];
N = 10000;
x = 1:N;
y = cumsum(rand(Normal(0,1), N, length(labs)),dims=1);
randomwalks = [scatter(;x=x,y=y[:,i],mode="lines",name=labs[i]) for i in 1:length(labs)];

layout = Layout(
  title = "Intracellular population dynamics",
  xaxis_title="time (d)",
  yaxis_title="Change in # molecules",
  font_family = "Arial",
  font_size = 14
)

# Plot all walks together
plot(randomwalks,layout)

# Plot walks in an array
layout["title"]=""
plots = [plot(randomwalks[i],layout) for i in 1:length(labs)];
[plots[1] plots[2]]
[plots[1] plots[2]  plots[3]
plots[4] plots[5] plot()]


### routiens that exploit matrix structure for computational advantages.

"""
Assumes diagonal entries of L are all positive.
logdettriangular(L::AbstractMatrix{T})::T
"""
function logdettriangular(L::AbstractMatrix{T})::T where T <: Real
    return sum(log(L[i,i]) for i = 1:size(L)[1])
end

### routines for matrix transforms and operations.

"""
evalinverse!( Y::Matrix{T}.
                inv_E::Matrix{T},
                f::Vector{T},
                h::T)

Computes Y := inv(M), where M := [E f; f' h], where E is DxD, f is Dx1, h is 1x1.
Uses the Matrix inversion lemma; see Theorem 4.3.2 from Murphy's book.

Test:
M = Utilities.generaterandomposdefmat(3)
E = M[1:2,1:2]
f = vec(M[1:2,3])
h = M[3,3]
inv_E = inv(E)

Y = zeros(3,3)
evalinverse!( Y,
                inv_E,
                f,
                h)
println("inv(M)-Y = ", inv(M)-Y)
"""
function evalinverse!(  Y::Matrix{T},
                        inv_E::Matrix{T},
                        f::Vector{T},
                        h::T) where T <: Real
    #
    D = length(f)
    @assert size(inv_E) == (D,D)
    @assert size(Y) == (D+1,D+1)

    # compute the Schur complement of M w.r.t. E.
    schur = h - dot(f,inv_E*f)

    b = inv_E*f

    Y[1:D,1:D] = inv_E + (b*b')./schur
    Y[1:D, D+1] = -b ./ schur
    Y[D+1, 1:D] = -b ./ schur
    Y[D+1, D+1] = one(T)/schur

    return nothing
end

function evalinverse(   inv_E::Matrix{T},
                        f::Vector{T},
                        h::T)::Matrix{T} where T <: Real
    #
    Y = Matrix{T}(undef,size(inv_E))
    evalinverse!(Y, inv_E, f, h)

    return Y
end

function forcesymmetric(A::Matrix{T})::Matrix{T} where T <: Real
    return (A+A')./2
end

# replace the upper triangular entires with the lower ones.
function replaceupperbylowertriangular!(A::Matrix{T})::Nothing where T <: Real
    N = size(A,1)
    @assert size(A,2) == size(A,1)
    for j = 1:N
        for i = j+1:N
            A[j,i] = A[i,j]
        end
    end

    return nothing
end

function naivesqrtpsdmatrix(A)
    @assert LinearAlgebra.isposdef(A)
    s,Q = LinearAlgebra.eigen(A)

    s2 = sqrt.(s)

    B = Q'*LinearAlgebra.Diagonal(s2)*Q
    B = forcesymmetric(B)

    return B
end

function array2matrix(X::Array{Vector{T},L})::Matrix{T} where {T,L}

    N = length(X)
    D = length(X[1])

    out = Matrix{T}(undef,D,N)
    for n = 1:N
        out[:,n] = X[n]
    end

    return out
end

"""
    makeblockdiagonalmatrix(A::Vector{Matrix{T}})::Matrix{T}
"""
function makeblockdiagonalmatrix(A::Vector{Matrix{T}})::Matrix{T} where T <: Real
    K = length(A)

    N_rows = 0
    N_cols = 0

    for i = 1:K
        N_rows += size(A[i], 1)
        N_cols += size(A[i], 2)
    end

    out = zeros(T, N_rows, N_cols)
    st_rows = 1
    st_cols = 1

    for k = 1:K
        M,N = size(A[k])

        out[st_rows:st_rows+M-1,st_cols:st_cols+N-1] = A[k]

        st_rows += M
        st_cols += N
    end
    return out
end

"""
    invertblockmatrix!( out::Vector{Matrix{T}},
                    src::Vector{Matrix{T}})::Nothing
"""
function invertblockmatrix!(out::Vector{Matrix{T}},
                            src::Vector{Matrix{T}})::Nothing where T <: Real
    @assert length(out) == length(src)
    for i = 1:length(src)
        out[i] = inv(src[i])
    end
    return nothing
end

"""
    blockmatrixmatrixproduct(   A::Matrix{T},
                                B_set::Vector{Matrix{T}})::Matrix{T}

does A*makeblockdiagonalmatrix(B_set).

# Example:
```
D = 30
p = 30
B2_set = collect(randn(D,D) for i=1:p)
B2 = makeblockdiagonalmatrix( B2_set )
B = randn(D*p,D*p)
norm(B*B2 - blockmatrixmatrixproduct(B,B2_set))
```
"""
function blockmatrixmatrixproduct(  A::Matrix{T},
                                    B_set::Vector{Matrix{T}})::Matrix{T} where T <: Real
    K = length(B_set)

    M,N = size(B_set[1])
    @assert K*N == size(A,2)

    st = 1
    out = Matrix{T}(undef, size(A,1), K*M)
    for i = 1:length(B_set)
        # parse.
        st = (i-1)*size(B_set[i],1) + 1
        W = view(A, :, st:st+N-1)

        # matrix-vector product.
        out[:,st:st+N-1] = W*B_set[i]
    end

    return out
end

"""
    blockmatrixvectorproduct(   B_set::Vector{Matrix{T}},
                                v::Vector{T})::Vector{T}

does makeblockdiagonalmatrix(B_set)*v.

# Example:
```
D = 30
p = 30
B2_set = collect(randn(D,D) for i=1:p)
B2 = makeblockdiagonalmatrix( B2_set )
b = randn(D*p)
norm(B2*b - blockmatrixvectorproduct(B2_set,b))
```
"""
function blockmatrixvectorproduct(  B_set::Vector{Matrix{T}},
                                    v::Vector{T})::Vector{T} where T <: Real
    K = length(B_set)

    M,N = size(B_set[1])
    @assert K*N == length(v)

    st = 1
    out = Array{T}(K*M)
    for i = 1:length(B_set)
        # parse.
        st = (i-1)*size(B_set[i],2) + 1
        w = view(v, st:st+N-1)

        # matrix-vector product.
        out[st:st+N-1] = B_set[i]*w
    end

    return out
end

#### symmetric matrix efficient querying.

"""
    readuppertriangular(i::Int, j::Int, N::Int, v::Vector{T})::T

Query the (i-j)-th entry of an upper triangular matrix that is stored in v.

```
A = randn(7,7)

v = packageuppertriangle(A)

function packagemat(v, N)
    B = zeros(Float64, N, N)

    for j = 1:N
        for i = 1:j
            B[i,j] = readuppertriangular(i, j, N, v)
        end
    end

    return B
end

C = packagemat(v, size(A,1))
display(A-C)
```
"""
function readuppertriangular(i::Int, j::Int, N::Int, v::Vector{T})::T where T

    linear_ind = div(j*(j-1), 2) + i

    #Printf.@printf("(i,j) = (%d,%d), linear index = %d, offset = %d\n", i, j, linear_ind, offset)
    return v[linear_ind]
end

"""
    readsymmetric(i::Int, j::Int, N::Int, v::Vector{T})::T

Queries a symmetric matrix stored in a vector format.
"""
function readsymmetric(i::Int, j::Int, N::Int, v::Vector{T})::T where T
    if i < j
        return readuppertriangular(i, j, N, v)
    end

    return readuppertriangular(j, i, N, v)
end

"""
    writeuppertriangular!(v::Vector{T}, i::Int, j::Int, N::Int, val::T)::Nothing

Writes a value to an upper triangular matrix storage.
"""
function writeuppertriangular!(v::Vector{T}, i::Int, j::Int, N::Int, val::T)::Nothing where T

    linear_ind = div(j*(j-1), 2) + i
    v[linear_ind] = val

    return nothing
end

"""

```
D = 3
v = Vector{Float64}(undef, 6)
@time Utilities.writesymmetric!(v, 2, 3, 3, 1.23)

function fillv!(v, D)
    c = 0.0
    for j = 1:D
        for i = 1:j
            Utilities.writesymmetric!(v, i, j, D, c)
            c += 1
        end
    end

end

fillv!(v, D)
display(v)
```
"""
function writesymmetric!(v::Vector{T}, i::Int, j::Int, N::Int, val::T)::Nothing where T
    if i < j
        writeuppertriangular!(v, i, j, N, val)

        return nothing
    end

    writeuppertriangular!(v, j, i, N, val)

    return nothing
end

"""
    packageuppertriangle(L::Matrix{T})::Vector{T}

Extracts the upper triangular portion of a matrix. Includes diagonal entries.
"""
function packageuppertriangle(L::Matrix{T})::Vector{T} where T
    N = size(L,1)

    a = Vector{T}(undef, div(N*(N+1),2) )

    p = 1
    for j = 1:N
        for i = 1:j
            a[p] = L[i,j]
            p += 1
        end
    end

    return a
end