const MPSTensor = Array{Float64,3}
const MPS = Vector{MPSTensor}

# Split a single multi-site tensor into single-site MPS tensors in the dumb way
function splitMPStensor(T::Array{Float64})
    v = MPS(undef,ndims(T)-2);
    (l,r) = (2,ndims(T)-1)
    s = collect(size(T))
    while r>l
        # Calculate the bond dimensions of sweeping tensor either way and minimise
        L = s[l-1]*s[l]
        R = s[r]*s[r+1]
        if L <= R
            v[l-1] = reshape(Matrix{Float64}(LinearAlgebra.I,L,L),(s[l-1],s[l],L));
            s[l] *= s[l-1]
            l += 1
        else
            v[r-1] = reshape(Matrix{Float64}(LinearAlgebra.I,R,R),(R,s[r],s[r+1]));
            s[r] *= s[r+1]
            r -= 1
        end
    end
    v[l-1] = reshape(T,(s[l-1],s[l],s[l+1]))
    return v
end

# Truncates down the bond dimension of an MPS, performing a (lossy) compression
function truncMPS!(M::MPS, χ::Int64)
    # Put the MPS in canonical form using the QR decomposition, sweeping left-to-right
    for i ∈ 1:length(M)-1
        X = reshape(M[i],(size(M[i],1)*size(M[i],2),size(M[i],3)))
        q,r = LinearAlgebra.qr(X)
        if size(r,1)==size(r,2)
            M[i] = reshape(Matrix(q),size(M[i]))
            LinearAlgebra.lmul!(LinearAlgebra.UpperTriangular(r),
                reshape(M[i+1],(size(M[i+1],1),size(M[i+1],2)*size(M[i+1],3))))
        else
            M[i] = reshape(Matrix(q),(size(M[i],1),size(M[i],2),size(r,1)))
            M[i+1] = reshape(r*reshape(M[i+1], (size(M[i+1],1),
                size(M[i+1],2)*size(M[i+1],3))), (size(r,1),size(M[i+1],2),size(M[i+1],3)))
        end
    end
    # Perform the bond truncation using the SVD decomposition, sweeping right-to-left
    for i ∈ length(M):-1:2
        X = reshape(M[i],(size(M[i],1),size(M[i],2)*size(M[i],3)))
        # In some rare cases the default svd can fail to converge
        try
            F = LinearAlgebra.svd!(X);
            (u,s,v) = (F.U,F.S,F.V)
        catch _
            F = LinearAlgebra.svd!(X; alg=LinearAlgebra.QRIteration())
            (u,s,v) = (F.U,F.S,F.V)
        end
        b = min(length(s),χ)
        u = u[:,1:b]
        s = s[1:b]
        v = v[:,1:b]'
        M[i] = reshape(v,(b,size(M[i],2),size(M[i],3)));
        X = reshape(M[i-1],(size(M[i-1],1)*size(M[i-1],2),size(M[i-1],3)))*u
        LinearAlgebra.rmul!(X,LinearAlgebra.Diagonal(s));
        M[i-1] = reshape(X,(size(M[i-1],1),size(M[i-1],2),b));
    end
    return M
end

# Find the permutation transformation between two vectors
function permutebetween(from, to)
    σf = sortperm(from)
    σt = sortperm(to)
    arr = Vector{Int}(undef,length(from))
    for i ∈ eachindex(from)
        arr[σt[i]]=σf[i]
    end
    return arr
end

"""
    sweep_contract(LTN::LabelledTensorNetwork, χ, τ;
        fast=false, valid=false, planar=false, connected=false, report=false)
    sweep_contract(TN::TensorNetwork, χ, τ;
        fast=false, valid=false, planar=false, connected=false, report=false)

Returns the contraction of the `TensorNetwork TN`, or the `LabelledTensorNetwork LTN` using
the sweepline contraction algorithm of `arXiv:2101.04125`. The MPS is truncated down to a
bond dimension of `χ` whenever any bond dimension exceeds `τ`.

By default the network is checked for validity, planarised, and sweep-connected, where
necessary. The keyword flags `valid`, `planar`, and `connected` can be used to skip these,
or the flag `fast` can be used to skip them all. If these flags are enabled then contraction
may fail on poorly formed networks.

To avoid underflow/overflow issues the contraction value of the network is returned as a
tuple `(f::Float64, i::Int64)` where `1≦f<2` or `f` is `0`, representing a value of `f*2^i`.
The function `ldexp` can be used to convert this back to a Float64.

`sweep_contract` is non-mutating and acts upon a deep copy of the network, where possible
use the more efficient mutating version `sweep_contract!`.
"""
sweep_contract(LTN::LabelledTensorNetwork, χ::Int, τ::Int;
    fast=false, valid=false, planar=false, connected=false, report=false) =
sweep_contract!(deepcopy(LTN), χ, τ;
    fast=fast, planar=planar, connected=connected, report=report)

sweep_contract(TN::TensorNetwork, χ::Int, τ::Int;
    fast=false, valid=false, planar=false, connected=false, report=false) =
sweep_contract!(deepcopy(TN), χ, τ;
    fast=fast, planar=planar, connected=connected, report=report)

"""
    sweep_contract!(LTN::LabelledTensorNetwork, χ, τ;
        fast=false, valid=false, planar=false, connected=false, report=false)
    sweep_contract!(TN::TensorNetwork, χ, τ;
        fast=false, valid=false, planar=false, connected=false, report=false)

The mutating form of `sweep_contract`.
"""
sweep_contract!(LTN::LabelledTensorNetwork, χ::Int, τ::Int;
    fast=false, valid=false, planar=false, connected=false, report=false) =
sweep_contract!(delabel(LTN), χ, τ;
        fast=fast, planar=planar, connected=connected, report=report)

function sweep_contract!(TN::TensorNetwork, χ::Int, τ::Int;
    fast=false,valid=false,planar=false,connected=false,report=false)::Tuple{Float64,Int}
    if !fast
        valid || checkvalid(TN)
        planar || planarise!(TN)
        connected || connect!(hull!(TN))
    end

    sort!(TN)

    resexp = 0
    count = 0

    MPS_t = [ones(1,1,1)]
    MPS_i = Int[]

    for (i,t) ∈ enumerate(TN)
        ind_up = Int[]
        ind_do = Int[]
        for n ∈ t.adj
            if TN[n]>t
                push!(ind_up, n)
            elseif TN[n]<t
                push!(ind_do, n)
            else
                throw(InvalidTNError("Overlapping tensors"))
            end
        end
        sort!(ind_up, by=λ->atan(TN[λ].x-t.x,TN[λ].y-t.y))
        sort!(ind_do, by=λ->atan(TN[λ].x-t.x,t.y-TN[λ].y))
        σ = permutebetween(t.adj, [ind_do; ind_up])
        t.arr = permutedims(t.arr, σ)
        s = size(t.arr)
        t.arr = reshape(t.arr,(prod(s[1:length(ind_do)]),s[length(ind_do)+1:end]...))

        if isempty(MPS_i)
            MPS_t = splitMPStensor(MPS_t[1][1]*reshape(t.arr,(size(t.arr)...,1)))
            MPS_i = ind_up
        else
            lo = findfirst(isequal(i), MPS_i)
            hi = findlast(isequal(i), MPS_i)

            isnothing(lo) && throw(InvalidTNError("Disconnected TN"))

            X::Array{Float64} = MPS_t[lo]
            for j ∈ lo+1:hi
                finalsize = (size(X,1),size(X,2)*size(MPS_t[j],2),size(MPS_t[j],3))
                X = reshape(X,(size(X,1)*size(X,2),size(X,3)))*
                    reshape(MPS_t[j],(size(MPS_t[j],1),size(MPS_t[j],2)*size(MPS_t[j],3)))
                X = reshape(X,finalsize)
            end
            X = permutedims(X,[1,3,2])
            M = reshape(t.arr,(size(t.arr,1),prod(size(t.arr)[2:end])))
            X = reshape(
                reshape(X,(size(X,1)*size(X,2),size(X,3)))*M,
                (size(X,1),size(X,2),size(M,2))
            )
            X = permutedims(X,[1,3,2])
            X = reshape(X,(size(X,1),size(t.arr)[2:end]...,size(X,3)))

            MPS_i = [MPS_i[1:lo-1]; ind_up; MPS_i[hi+1:end]]
            if ndims(X)!=2
                MPS_t = [MPS_t[1:lo-1]; splitMPStensor(X); MPS_t[hi+1:end]]
            elseif isempty(MPS_i)
                MPS_t=[reshape([X[1]],(1,1,1))]
            elseif lo>1
                s = size(MPS_t[lo-1])
                MPS_t[lo-1] = reshape(
                    reshape(MPS_t[lo-1],(s[1]*s[2],s[3]))*X,
                    (s[1],s[2],size(X,2))
                )
                MPS_t = [MPS_t[1:lo-1]; MPS_t[hi+1:end]]
            else
                s = size(MPS_t[hi+1])
                MPS_t[hi+1] = reshape(
                    X*reshape(MPS_t[hi+1],(s[1],s[2]*s[3])),
                    (size(X,1),s[2],s[3])
                )
                MPS_t = [MPS_t[1:lo-1]; MPS_t[hi+1:end]]
            end

            if any(size.(MPS_t,3).>τ)
                count += 1
                truncMPS!(MPS_t, χ)
                if LinearAlgebra.norm(MPS_t[1])==0
                    return (0.0,typemin(Int))
                end
                h = Int(floor(log2(LinearAlgebra.norm(MPS_t[1]))))
                resexp += h
                MPS_t[1] /= exp2(h)
            end
        end
    end

    report && println("Number of truncations: $count")

    res = MPS_t[1][1];
    if res == 0.0
        return (0.0, typemin(Int64));
    end
    h = Int(floor(log2(abs(res))));
    return (res/exp2(h),resexp+h);
end


using Test, SpecialFunctions, Statistics
import TruncatedNormal as TruncNorm

@test isnan(TruncNorm.tnvar(1, 0))
@test isnan(TruncNorm.tnvar(+Inf, 0))
@test isnan(TruncNorm.tnvar(0, -Inf))
@test isnan(TruncNorm.tnvar(NaN, NaN))
@test isnan(TruncNorm.tnvar(0, NaN))
@test isnan(TruncNorm.tnvar(NaN, 0))

@test TruncNorm.tnvar(-Inf, +Inf) == 1
@test TruncNorm.tnvar(+Inf, +Inf) == 0
@test TruncNorm.tnvar(-Inf, -Inf) == 0
@test TruncNorm.tnvar(0, +Inf) ≈ 1 - 2/π
@test TruncNorm.tnvar(-Inf, 0) ≈ 1 - 2/π

for x = -10:10
    @test TruncNorm.tnvar(x, +Inf) ≈ 1 + √(2/π) * x / erfcx(+x / √2) - 2/π / erfcx(+x / √2)^2
    @test TruncNorm.tnvar(-Inf, x) ≈ 1 - √(2/π) * x / erfcx(-x / √2) - 2/π / erfcx(-x / √2)^2
end

@test TruncNorm.tnvar(50, 70) ≈ 0.0003990431868038995479099272265360593305365

@test TruncNorm.tnvar(-Inf, Inf, 0, 1) == 1

for x = -10:10
    @test TruncNorm.tnvar(x, +Inf, 0, 1) ≈ TruncNorm.tnvar(x, +Inf)
    @test TruncNorm.tnvar(-Inf, x, 0, 1) ≈ TruncNorm.tnvar(-Inf, x)
end

@test TruncNorm.tnvar(50, 70, -2, 3) ≈ 0.029373438107168350377591231295634273607812172191712
@test TruncNorm.tnvar(-100.0, 0.0, 0.0, 2.0986317998643735) ≈ 1.6004193412141677189841357987638847137391508803335
@test TruncNorm.tnvar(0.0, 0.9, 0.0, 0.07132755843183151) ≈ 0.0018487407287725028827020557707636415445504260892486
@test TruncNorm.tnvar(-100.0, 100.0, 0.0, 17.185261847875548) ≈ 295.333163899557735486302841237124507431445
@test TruncNorm.tnvar(-100.0, 0.5, 0.0, 0.47383322897860064) ≈ 0.145041095812679283837328561547251019229612
@test TruncNorm.tnvar(-100.0, 100.0, 0.0, 17.185261847875548) ≈ 295.333163899557735486302841237124507431445
@test TruncNorm.tnvar(-10000, 10000, 0, 1) ≈ 1

# https://github.com/JuliaStats/Distributions.jl/issues/827
@test TruncNorm.tnvar(999000, 1e6) ≥ 0
@test TruncNorm.tnvar(0, 1000, 1000000, 1) ≥ 0

@test_broken TruncNorm.tnvar(1e6, Inf) ≈ 9.99999999994000000000050000000e-13
@test_broken TruncNorm.tnvar(999000, 1e6) ≈ 1.00200300399898194688784897455e-12
@test_broken TruncNorm.tnvar(0, 1000, 1000000, 1) ≈ 1.00200300399898194688784897455909681215272616880064850605775930845141e-12


using LinearAlgebra

function readInput(filename::String)
	x::Vector{Int} = []
    y::Vector{Int} = []
    folds::Vector{Tuple{String, Int}} = []
    open(filename) do file
        for line in readlines(file)
            if isempty(line)
                continue
            end

            if occursin(",", line)
                x_line, y_line = parse.(Int, split(line, ","))
                push!(x, x_line)
                push!(y, y_line)
            else
                if occursin("x", line)
                    x_ = parse.(Int, line[length("fold along x=") + 1:end])
                    push!(folds, ("x", x_))
                else
                    y_ = parse.(Int, line[length("fold along y=") + 1:end])
                    push!(folds, ("y", y_))
                end
            end
        end
    end
    max_x = maximum(x) + 1
    max_y = maximum(y) + 1
    m = falses(max_x, max_y)

    for (xᵢ, yᵢ) in zip(x, y)
        m[xᵢ + 1, yᵢ + 1] = true
    end

    return BitMatrix(transpose(m)), folds
end

function print_matrix(mat)
    show(stdout, "text/plain", mat)
    println()
end

function fold_y(mat, y::Int)
    r = size(mat, 1)
    top_half = mat[1:y, :]
    bot_half = mat[y+2:end, :]
    if y + 1 > r / 2
        top_half[end-size(bot_half, 1)+1:end, :] .|= reverse(bot_half, dims=1)
        return top_half
    else
        bot_half[end-size(top_half, 1)+1:end, :] .|= reverse(top_half, dims=1)
        return reverse(bot_half, dims=1)
    end
end

function fold_x(mat, x::Int)
    c = size(mat, 2)
    left_half = mat[:, 1:x]
    rigt_half = mat[:, x+2:end]
    if x + 1 > c / 2
        left_half[:, end-size(rigt_half, 2)+1:end] .|= reverse(rigt_half, dims=2)
        return left_half
    else
        rigt_half[:, end-size(left_half, 2)+1:end] .|= reverse(left_half, dims=2)
        return reverse(rigt_half, dims=2)
    end
end

function part1(mat, folds)::Int
    dir, n = folds[1]
    if dir == "x"
        return sum(fold_x(mat, n))
    else
        return sum(fold_y(mat, n))
    end
end

function part2(mat, folds)
    for (dir, n) in folds
        if dir == "x"
            mat = fold_x(mat, n)
        else
            mat = fold_y(mat, n)
        end
    end

    return mat
end

mat, folds = readInput("input.txt")

println(part1(mat, folds))
print_matrix(part2(mat, folds))


# Following the algorithm by William and Seeger, 2001
# Cs is equivalent to X_mm and C to X_mn

function sampleindex(X::AbstractMatrix, r::Real; obsdim::Integer=defaultobs)
    0 < r <= 1 || throw(ArgumentError("Sample rate `r` must be in range (0,1]"))
    n = size(X, obsdim)
    m = ceil(Int, n*r)
    S = StatsBase.sample(1:n, m; replace=false, ordered=true)
    return S
end

function nystrom_sample(k::Kernel, X::AbstractMatrix, S::Vector{<:Integer}; obsdim::Integer=defaultobs)
    obsdim ∈ [1, 2] || throw(ArgumentError("`obsdim` should be 1 or 2 (see docs of kernelmatrix))"))
    Xₘ = obsdim == 1 ? X[S, :] : X[:, S]
    C = kernelmatrix(k, Xₘ, X; obsdim=obsdim)
    Cs = C[:, S]
    return (C, Cs)
end

function nystrom_pinv!(Cs::Matrix{T}, tol::T=eps(T)*size(Cs,1)) where {T<:Real}
    # Compute eigendecomposition of sampled component of K
    QΛQᵀ = LinearAlgebra.eigen!(LinearAlgebra.Symmetric(Cs))

    # Solve for D = Λ^(-1/2) (pseudo inverse - use tolerance from before factorization)
    D = QΛQᵀ.values
    λ_tol = maximum(D)*tol

    for i in eachindex(D)
        @inbounds D[i] = abs(D[i]) <= λ_tol ? zero(T) : one(T)/sqrt(D[i])
    end

    # Scale eigenvectors by D
    Q = QΛQᵀ.vectors
    QD = LinearAlgebra.rmul!(Q, LinearAlgebra.Diagonal(D))  # Scales column i of Q by D[i]

    # W := (QD)(QD)ᵀ = (QΛQᵀ)^(-1)  (pseudo inverse)
    W = QD*QD'

    # Symmetrize W
    return LinearAlgebra.copytri!(W, 'U')
end

@doc raw"""
    NystromFact

Type for storing a Nystrom factorization. The factorization contains two fields: `W` and
`C`, two matrices satisfying:
```math
\mathbf{K} \approx \mathbf{C}^{\intercal}\mathbf{W}\mathbf{C}
```
"""
struct NystromFact{T<:Real}
    W::Matrix{T}
    C::Matrix{T}
end

function NystromFact(W::Matrix{<:Real}, C::Matrix{<:Real})
    T = Base.promote_eltypeof(W, C)
    return NystromFact(convert(Matrix{T}, W), convert(Matrix{T}, C))
end

@doc raw"""
    nystrom(k::Kernel, X::Matrix, S::Vector; obsdim::Int=defaultobs)

Computes a factorization of Nystrom approximation of the square kernel matrix of data
matrix `X` with respect to kernel `k`. Returns a `NystromFact` struct which stores a
Nystrom factorization satisfying:
```math
\mathbf{K} \approx \mathbf{C}^{\intercal}\mathbf{W}\mathbf{C}
```
"""
function nystrom(k::Kernel, X::AbstractMatrix, S::Vector{<:Integer}; obsdim::Int=defaultobs)
    C, Cs = nystrom_sample(k, X, S; obsdim=obsdim)
    W = nystrom_pinv!(Cs)
    return NystromFact(W, C)
end

@doc raw"""
    nystrom(k::Kernel, X::Matrix, r::Real; obsdim::Int=defaultobs)

Computes a factorization of Nystrom approximation of the square kernel matrix of data
matrix `X` with respect to kernel `k` using a sample ratio of `r`.
Returns a `NystromFact` struct which stores a Nystrom factorization satisfying:
```math
\mathbf{K} \approx \mathbf{C}^{\intercal}\mathbf{W}\mathbf{C}
```
"""
function nystrom(k::Kernel, X::AbstractMatrix, r::Real; obsdim::Int=defaultobs)
    S = sampleindex(X, r; obsdim=obsdim)
    return nystrom(k, X, S; obsdim=obsdim)
end

"""
    nystrom(CᵀWC::NystromFact)

Compute the approximate kernel matrix based on the Nystrom factorization.
"""
function kernelmatrix(CᵀWC::NystromFact{<:Real})
    W = CᵀWC.W
    C = CᵀWC.C
    return C'*W*C
end


#=Copyright (c) 2017 Gabriel Goh

Permission is hereby granted, free of charge, to any person obtaining a copy of
this software and associated documentation files (the "Software"), to deal in
the Software without restriction, including without limitation the rights to
use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
the Software, and to permit persons to whom the Software is furnished to do so,
subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.=#

# f(x) = ((1 + ((x[1]+x[2]+1)^2)*(19 - 14*x[1]+3*x[1]^2-14*x[2]+6*x[1]*x[2]+3*x[2]^2))*
#         (30 + ((2*x[1] - 3*x[2])^2)*(18 - 32*x[1] +
#             12*x[1]^2 + 48*x[2] - 36*x[1]*x[2] + 27*x[2]^2))  )
# ∇f(x) = ForwardDiff.gradient(f,x)

# Beale
# f(x) = ( (1.5 - x[1] + x[1]*x[2])^2
#      + (2.25 - x[1] + x[1]*x[2]^2)^2
#      + (2.625 - x[1] + x[1]*x[2]^3)^2 )
# ∇f(x) = ForwardDiff.gradient(f,x)

# Rosenbrock
# f(x) = 100*(x[2] - x[1]^2)^2 + (x[1] - 1)^2
# ∇f(x) = ForwardDiff.gradient(f,x)

# Monkey Saddle
# f(x) = x[1]^3 - 3*x[1]*x[2]^2
# ∇f(x) = [3*x[1] - 3*x[2]^2; -6*x[1]*x[2]]

A = diagm([1; 1])
f(x) = 0.5*vecdot(x,A*x)
∇f(x) = A*x

α = 1.5; β = 0.51

w = [1;1]; z = zeros(2)

N = 2000
fW = zeros(N)
wp = w
W = zeros(2,N)
for i = 1:N
    if i > 2
        #if vecdot(∇f(w), w - wp) > 0; println(i); z = zeros(2); end
    end
    W[:,i] = vecdot(∇f(w), w - wp)
    wp = w
    z = β*z + ∇f(w)
    w = w - α*(∇f(w) + β*z)
    println(f(w))
    if abs(f(w)) < 100; fW[i] = f(w); else; fW[i] = NaN; end
    #W[:,i] = w
end

plot(fW)


module Dec8Part2
function get_new_index_compute_accumulator(op, value, current_index, accumulator)
    if op == "jmp"
        current_index += value
    elseif op == "acc"
        accumulator += value
        current_index += 1
    else
        current_index += 1
    end
    return current_index, accumulator
end

function parse_operation(x::String)::Array{Union{String,Int64},1}
    op, value = split(x, ' ')
    return [String(op), parse(Int64, value)]
end

function get_accumulator_run_operations(operations::Array, index::Int64, accumulator::Int64)::Union{Nothing,Int64}
    visited_index = Int64[]
    push!(visited_index, index)
    n_ops = length(operations)
    while index <= n_ops
        op, value = operations[index]
        index, accumulator = get_new_index_compute_accumulator(op, value, index, accumulator)
        if index ∉ visited_index
            push!(visited_index, index)
        else
            return nothing
        end
    end
    if n_ops ∈ visited_index
        return accumulator
    else
        return nothing
    end
end

function correct_get_accumulator(operations::Array, index::Int64, accumulator::Int64)
    for (index_op, operation) in enumerate(operations)
        ops = deepcopy(operations)
        op = operation[1]
        value = operation[2]
        if op == "jmp"
            ops[index_op][1] = "nop"
            acc = get_accumulator_run_operations(ops, index, accumulator)
            if acc === nothing
                continue
            else
                return acc
            end
        elseif op == "nop"
            ops[index_op][1] = "jmp"
            acc = get_accumulator_run_operations(ops, index, accumulator)
            if acc === nothing
                continue
            else
                return acc
            end
        end
    end
end

function main(file_path::String)::Int64
    operations = readlines(file_path)
    current_index = 1
    accumulator = 0
    operations = parse_operation.(operations)
    return correct_get_accumulator(operations, current_index, accumulator)
end
end


function calculate_curvature(xs, fs, i)
    Δf_iph = fs[i+1] - fs[i]    # i plus a half
    Δf_imh = fs[i] - fs[i-1]    # i minus a half
    Δx_iph = xs[i+1] - xs[i]
    Δx_imh = xs[i] - xs[i-1]
    Δx_ic = xs[i+1] - xs[i-1]   # i central

    dfdx_iph = Δf_iph / Δx_iph
    dfdx_imh = Δf_imh / Δx_imh

    2 * (dfdx_iph - dfdx_imh) * (Δx_ic)
end

function calculate_curvature_integral(xs, fs, p, interval, n_intervals, f_range)
    tot_w = 0.0
    result = 0.0
    # Do a small convolution
    for (q,w) in ((-1, 0.25), (0, 0.5), (1, 0.25))
        interval == 1 && q == -1 && continue
        interval == n_intervals && q == 1 && continue
        tot_w += w
        i = p + q
        # Estimate integral of second derivative over interval, use that as a refinement indicator
        # https://mathformeremortals.wordpress.com/2013/01/12/a-numerical-second-derivative-from-three-points/
        result += abs(calculate_curvature(xs, fs, i)) / f_range * w
    end
    result /= tot_w
end

function calculate_refinement(f, xs, fs, n_intervals, intervals_to_refine, n_tot_refinements)
    @assert length(xs) == length(fs)
    n_points = length(xs)

    n_intervals_to_refine = length(intervals_to_refine)
    n_new_points = 2*n_intervals_to_refine
    # Do division of the intervals
    new_xs = zeros(eltype(xs), n_points + n_new_points)
    new_fs = zeros(eltype(fs), n_points + n_new_points)
    new_tot_refinements = zeros(Int, n_intervals + n_intervals_to_refine)
    k = 0
    kk = 0
    for i in 1:n_points
        if iseven(i) # This is a point in an interval
            interval = i ÷ 2
            if interval in intervals_to_refine
                kk += 1
                new_tot_refinements[interval - 1 + kk] = n_tot_refinements[interval] + 1
                new_tot_refinements[interval + kk] = n_tot_refinements[interval] + 1

                k += 1
                new_xs[i - 1 + k] = (xs[i] + xs[i-1]) / 2
                new_fs[i - 1 + k] = f(new_xs[i-1 + k])

                new_xs[i + k] = xs[i]
                new_fs[i + k] = fs[i]

                new_xs[i + 1 + k] = (xs[i+1] + xs[i]) / 2
                new_fs[i + 1 + k] = f(new_xs[i + 1 + k])
                k += 1
            else
                new_tot_refinements[interval + kk] = n_tot_refinements[interval]
                new_xs[i + k] = xs[i]
                new_fs[i + k] = fs[i]
            end
        else
            new_xs[i + k] = xs[i]
            new_fs[i + k] = fs[i]
        end
    end

    return (new_xs, new_fs, new_tot_refinements)
end

"""
    adapted_grid(f, minmax::Tuple{Number, Number}; max_recursions = 7)

Computes a grid `x` on the interval [minmax[1], minmax[2]] so that
`plot(f, x)` gives a smooth "nice" plot.
The method used is to create an initial uniform grid (21 points) and refine intervals
where the second derivative is approximated to be large. When an interval
becomes "straight enough" it is no longer divided. Functions are never evaluated
exactly at the end points of the intervals.

The parameter `max_recusions` computes how many times each interval is allowed to
be refined.
"""
function adapted_grid(f, minmax::Tuple{Real, Real}; max_recursions = 7)
    if minmax[1] >= minmax[2]
        throw(ArgumentError("interval must be given as (min, max)"))
    end

    # When an interval has curvature smaller than this, stop refining it.
    max_curvature = 0.05

    # Initial number of points
    n_points = 21
    n_intervals = n_points ÷ 2
    @assert isodd(n_points)

    xs = collect(range(minmax[1]; stop=minmax[2], length=n_points))
    # Move the first and last interior points a bit closer to the end points
    xs[2] = xs[1] + (xs[2] - xs[1]) * 0.25
    xs[end-1] = xs[end] - (xs[end] - xs[end-1]) * 0.25

    # Wiggle interior points a bit to prevent aliasing and other degenerate cases
    rng = MersenneTwister(1337)
    rand_factor = 0.05
    for i in 2:length(xs)-1
        xs[i] += rand_factor * 2 * (rand(rng) - 0.5) * (xs[i+1] - xs[i-1])
    end

    n_tot_refinements = zeros(Int, n_intervals)

    # We only evaluate the function on interior points
    fs = [NaN; [f(x) for x in xs[2:end-1]]; NaN]
    while true
        curvatures = zeros(n_intervals)
        active = falses(n_intervals)
        isfinite_f = isfinite.(fs)
        min_f, max_f = any(isfinite_f) ? extrema(fs[isfinite_f]) : (0.0, 0.0)
        f_range = max_f - min_f
        # Guard against division by zero later
        if f_range == 0 || !isfinite(f_range)
            f_range = one(f_range)
        end
        # Skip first and last interval
        for interval in 2:n_intervals-1
            p = 2 * interval
            if n_tot_refinements[interval] >= max_recursions
                # Skip intervals that have been refined too much
                active[interval] = false
            elseif !all(isfinite.(fs[[p-1,p,p+1]]))
                active[interval] = true
            else
                curvatures[interval] = calculate_curvature_integral(
                    xs, fs, p, interval, n_intervals, f_range)
                # Only consider intervals with a high enough curvature
                active[interval] = curvatures[interval] > max_curvature
            end
        end
        # Approximate end intervals as being the same curvature as those next to it.
        # This avoids computing the function in the end points
        curvatures[1] = curvatures[2]
        active[1] = active[2]
        curvatures[end] = curvatures[end-1]
        active[end] = active[end-1]

        if all(x -> x >= max_recursions, n_tot_refinements[active])
            break
        end

        n_target_refinements = n_intervals ÷ 2
        interval_candidates = collect(1:n_intervals)[active]
        n_refinements = min(n_target_refinements, length(interval_candidates))
        perm = sortperm(curvatures[active])
        intervals_to_refine = sort(interval_candidates[perm[length(perm) - n_refinements + 1:end]])

        (new_xs, new_fs, new_tot_refinements) = calculate_refinement(
            f, xs, fs, n_intervals, intervals_to_refine, n_tot_refinements)

        xs = new_xs
        fs = new_fs
        n_tot_refinements = new_tot_refinements
        n_points = length(xs)
        n_intervals = n_points ÷ 2
    end

    return xs[2:end-1]
end


using SparseDiffTools
using SparseArrays
using VertexSafeGraphs
using LightGraphs
using Random

const LG = LightGraphs

n = rand(10:100)
matrices = Array{SparseMatrixCSC, 1}(undef, 0)

for _ in 1:20
    n = rand(10:100)
    matrix = sprand(Int64, n, n, 0.5)
    push!(matrices, copy(matrix))
end

for i in 1:20
    matrix = matrices[i]
    g = matrix2graph(matrix, false)
    for e in edges(g)
        src = LG.src(e)
        dst = LG.dst(e)
        col1 = abs.(matrix[:, src])
        col2 = abs.(matrix[:, dst])
        pr = col1' * col2
        @test pr != 0
    end
end

for i in 1:20
    matrix = matrices[i]
    g = matrix2graph(matrix, true)
    for e in edges(g)
        src = LG.src(e)
        dst = LG.dst(e)
        row1 = abs.(matrix[src, :])
        row2 = abs.(matrix[dst, :])
        pr = row1' * row2
        @test pr != 0
    end
end

@info "stored zeros"
for i in 1:20
    matrix = matrices[i]
    g = matrix2graph(matrix)
    # recalculate graph with stored zeros
    matrix_sz = copy(matrix)
    fill!(matrix_sz, 0.0)
    g_sz = matrix2graph(matrix_sz)
    # check that graphs are the same
    @test nv(g) == nv(g_sz)
    @test ne(g) == ne(g_sz)
    for e in edges(g)
        @test has_edge(g_sz, e)
    end
    for e in edges(g_sz)
        @test has_edge(g, e)
    end
end


using MambaModels

## Data
line = Dict{Symbol, Any}()

df = DataFrame(CSV.read(joinpath(@__DIR__, "..", "..", "data", "Howell1.csv"),
  delim=';'));

# Use only adults

df2 = filter(row -> row[:age] >= 18, df);
mean_weight = mean(df2[:, :weight])
df2[!, :weight_c] = convert(Vector{Float64}, df2[:, :weight]) .- mean_weight ;
line[:x] = df2[:, :weight_c];
line[:y] = df2[:, :height];
line[:xmat] = convert(Array{Float64, 2}, [ones(length(line[:x])) line[:x]])

# Model Specification

model = Model(
  y = Stochastic(1,
    (xmat, beta, s2) -> MvNormal(xmat * beta, sqrt(s2)),
    false
  ),
  beta = Stochastic(1, () -> MvNormal([178, 0], [sqrt(10000), sqrt(100)])),
  s2 = Stochastic(() -> Uniform(0, 50))
)

# Initial Values

inits = [
  Dict{Symbol, Any}(
    :y => line[:y],
    :beta => [rand(Normal(178, 100)), rand(Normal(0, 10))],
    :s2 => rand(Uniform(0, 50))
  )
  for i in 1:3
]

# Tuning Parameters

scale1 = [0.5, 0.25]
summary1 = identity
eps1 = 0.5

scale2 = 0.5
summary2 = x -> [mean(x); sqrt(var(x))]
eps2 = 0.1

# Define sampling scheme

scheme = [
  Mamba.NUTS([:beta]),
  Mamba.Slice([:s2], 10)
]

setsamplers!(model, scheme)

# MCMC Simulation

chn = mcmc(model, line, inits, 10000, burnin=1000, chains=3)

# Show draws summary

describe(chn)

# Convert to MCMCChains.Chains object

chn2 = MCMCChains.Chains(chn.value, String.(chn.names))

# Describe the MCMCChains

MCMCChains.describe(chn2)

# End of `04/m4.1m.jl`


using DiffEqIO
using Base.Test

# Generate Data

using DiffEqBase, OrdinaryDiffEq

f_2dlinear = (t,u,du) -> du.=1.01u
prob = ODEProblem(f_2dlinear,rand(2,2),(0.0,1.0))
sol1 =solve(prob,Euler();dt=1//2^(4))

using DataFrames
df = DataFrame(sol1)

using ParameterizedFunctions

f = @ode_def LotkaVolterra begin
  dx = a*x - b*x*y
  dy = -c*y + d*x*y
end a=>1.5 b=>1 c=3 d=1

prob = ODEProblem(f,[1.0,1.0],(0.0,1.0))
sol2 =solve(prob,Tsit5())

df = DataFrame(sol2)

using CSV
CSV.write("out.csv",df)

# Use JLD

#=

using JLD
JLD.save("out.jld","sol",sol2)
=#


"""
    mergesorted(x::Vector{T}, y::T)

"""
function mergesorted(x::Vector{T}, y::T) where T
    z, indexes = mergesorted(x, [y])
    return z, indexes[1]
end

"""
    mergesorted(x::Vector{T}, y::Vector{S})

"""
function mergesorted(x::Vector{T}, y::Vector{S}) where {T,S}
    nx = length(x)
    ny = length(y)
    nz = nx + ny
    z  = Array{promote_type(T,S)}(undef, nz)

    ind_x   = 1
    ind_y   = 1
    indexes = ones(Int64, ny)

    @inbounds for ind_z in 1:nz
        val_x = x[ind_x]
        val_y = y[ind_y]
        if val_x <= val_y
            z[ind_z]  = val_x
            ind_x    += 1
            if ind_x > nx
                indexes[ind_y:end] .= (ind_z + 1):nz
                z[(ind_z + 1):end] .= y[ind_y:end]
                break
            end
        else
            z[ind_z]        = val_y
            indexes[ind_y]  = ind_z
            ind_y          += 1
            if ind_y > ny
                z[(ind_z+1):end] .= x[ind_x:end]
                break
            end
        end
    end
    return z, indexes
end

auc_trapezoidal((x,y)::Tuple{RealVector, RealVector}) = auc_trapezoidal(x, y)

"""
    auc(x::RealVector, y::RealVector)

Computes the area under curve `(x,y)` using trapezoidal rule.
"""
function auc_trapezoidal(x::RealVector, y::RealVector)
    n = length(x)
    n == length(y) || throw(DimensionMismatch("Inconsistent lengths of `x` and `y`."))

    if issorted(x)
        prm = 1:n
    elseif issorted(x, rev = true)
        prm = n:-1:1
    else
        throw(ArgumentError("x must be sorted."))
    end

    val = zero(promote_type(eltype(x), eltype(y)))
    for i in 2:n
        Δx   = x[prm[i]]  - x[prm[i-1]]
        Δy   = (y[prm[i]] + y[prm[i-1]])/2
        if !(isnan(Δx) || isnan(Δy) || Δx == 0)
            val += Δx*Δy
        end
    end
    return val
end


type SmolyakInterp
    grid::SmolyakGrid
    f_on_grid::Vector{Float64}
    coefs::Vector{Float64}
end

SmolyakInterp(sg::SmolyakGrid, fgrid::AbstractVector) =
    SmolyakInterp(sg, fgrid, find_coefs(sg, fgrid))

# Use pre-computed LU fact of B to get coefficients
find_coefs(sg::SmolyakGrid, f_on_grid::AbstractVector) = sg.B_fact \ f_on_grid

# Convenience method to update coefficients.
update_coefs!(si::SmolyakInterp, f_on_grid) =
    (si.coefs = find_coefs(si.grid, f_on_grid))

# Interpolation
function interpolate!(out::AbstractVector, new_B::AbstractMatrix,
                      si::SmolyakInterp, pts::AbstractMatrix)
    d = size(pts, 2)

    d != si.grid.d && error("pts should have $d columns")

    cube_pts = dom2cube(pts, si.grid.lb, si.grid.ub)
    build_B!(new_B, d, si.grid.mu, cube_pts, si.grid.pinds)
    A_mul_B!(out, new_B, si.coefs)
end

function interpolate!(out::AbstractVector, si::SmolyakInterp, pts::AbstractMatrix)
    new_B = ones(size(pts, 1), size(si.grid.pinds, 1))
    interpolate!(out, new_B, si, pts)
end

interpolate(si::SmolyakInterp, pts::AbstractMatrix) =
    interpolate!(Array(Float64, size(pts, 1)), si, pts)

Base.call(si::SmolyakInterp, pts::AbstractMatrix) = interpolate(si, pts)


# Unit Test for `SolveQuadraticProgram()`

using LinearAlgebra;
using SparseArrays;
using Gurobi; #<! Solves problems which SCS struggles with
using OSQP;
using Random;
using StableRNGs;
using Test;

seedNumber = 1234;
Random.seed!(seedNumber);
# Random.seed!(StableRNG(seedNumber), seedNumber);

include("GenerateQuadraticProgram.jl");
include("SolveQuadraticProgram.jl");
include("LinearSystemSolvers.jl");
include("SolveQuadraticProgramJump.jl");

# @enum ProblemClass randomQp = 1 inequalityConstrainedQp equalityConstrainedQp optimalControl portfolioOptimization lassoOptimization huberFitting supportVectorMachine isotonicRegression
@enum RefSolver gurobiSolver = 1 osqpSolver

## Parameters

# Simulaion

refSolver = osqpSolver;

numSimulations  = 10;
mNumElements = [    0010 0100;
0010 0100;
0010 0100;
0010 0100;
0010 0100;
0010 0100;
0010 0100;
0010 0100;
0010 0100;];
mNumConstraints = [ 0000 0000;
0000 0000;
0005 0050;
0000 0000;
0000 0000;
0000 0000;
0000 0000;
0000 0000;
0000 0000;]; #<! Set to 0 for OSQP Paper dimensions

# Solver
numIterations   = 50000;
ϵAbs            = 1e-7;
ϵRel            = 1e-7;
ρ               = 0.1;
adptΡ           = true;
hLinSolInit     = FacLdlInit;
hLinSol         = FacLdl!;

absDevThr = 1e-5;

hOptFun = optimizer_with_attributes(Gurobi.Optimizer, "OptimalityTol" => 1e-8, "FeasibilityTol" => 1e-8);

@testset verbose = true "Unit Test vs. Gurobi" begin
for problemClass ∈ instances(ProblemClass)
    @testset "Unit Test of $(problemClass) Problem Class" begin
    for iSim ∈ 1:numSimulations
        for iDim ∈ 1:size(mNumElements, 2)
            mP, vQ, mA, vL, vU = GenerateRandomQP(problemClass, mNumElements[Int(problemClass), iDim]; numConstraints = mNumConstraints[Int(problemClass), iDim]);
            # Some problems might change the actual data
            numElements     = size(mP, 1);
            numConstraints  = size(mA, 1);

            vX = zeros(numElements);

            if (refSolver == gurobiSolver)
                runTimeRefSolver = @elapsed vT = SolveQpJump(mP, vQ, mA, vL, vU; hOptFun = hOptFun);
            elseif (refSolver == osqpSolver)
                runTimeRefSolver = @elapsed begin
                    osqPModel = OSQP.Model();
                    OSQP.setup!(osqPModel; P = mP, q = vQ, A = mA, l = vL, u = vU, rho = ρ, max_iter = numIterations, eps_abs = 1e-7, eps_rel = 1e-7, scaling = 0);
                    osqpRes = OSQP.solve!(osqPModel);
                    vT = osqpRes.x;
                end
            end

            runTime     = @elapsed convFlag = SolveQuadraticProgram!(vX, mP, vQ, mA, vL, vU, hLinSolInit, hLinSol; numIterations = numIterations, ϵAbs = ϵAbs, ϵRel = ϵRel, ρ = ρ, adptΡ = adptΡ);

            maxAbsDev = norm(vT - vX, Inf);

            println("The max absolute error is: $(maxAbsDev)");
            println("The run time is: $(runTime) [Sec]");
            println("The output flag is: $(convFlag)");
            println("The reference solver ($(refSolver)) run time is: $(runTimeRefSolver) [Sec]");
            @test maxAbsDev <= absDevThr;
        end
    end
end #<! End Test Set Problem Class
end

end #<! Test Set Unit Test


"""
	parratio(eigvec::AbstractMatrix{<:Number})

Compute participation ratio of an array of eigenvectors, supplied as a Matrix
"""
function parratio(eigvec::AbstractMatrix{<:Number})
	denom = sum(abs2, eigvec, dims=2).^2
	numer = sum(x->abs2(x)^2, eigvec,dims=2)

	return numer./denom
end


# See The Visualization Toolkit: An Object-Oriented Approach to 3D Graphics,
# 4th Edition, Chapter 8, Advanced Data Representation

# Function interpolation
# length(w) == length(poly) == N, hence @inbounds is safe
function (poly::Polytope{K,P,N,T})(coords...) where {K,P,N,T}
    w = interpolation_weights(typeof(poly), coords...)
    p = zero(T)
    @inbounds @simd for i in eachindex(w)
        p += w[i]*poly[i]
    end
    return p
end

# Shape interpolation
# length(w) == length(poly) == N, hence @inbounds is safe
function (poly::Polytope{K,P,N,T})(coords...) where {K,P,N,T<:Point}
    w = interpolation_weights(typeof(poly), coords...)
    p = zero(typeof(coordinates(poly[1])))
    @inbounds @simd for i in eachindex(w)
        p += w[i]*coordinates(poly[i])
    end
    return Point(p)
end

# 1-polytope
interpolation_weights(::Type{<:LineSegment},      r) = Vec(1-r, r)
interpolation_weights(::Type{<:QuadraticSegment}, r) = Vec((2r - 1)*( r - 1),
                                                           ( r    )*(2r - 1),
                                                           (4r    )*( 1 - r))
# 2-polytope
interpolation_weights(::Type{<:Triangle},      r, s) = Vec((1 - r - s), r, s)
interpolation_weights(::Type{<:Quadrilateral}, r, s) = Vec((1 - r)*(1 - s),
                                                           (    r)*(1 - s),
                                                           (    r)*(    s),
                                                           (1 - r)*(    s))
interpolation_weights(::Type{<:QuadraticTriangle}, r, s) = Vec((2(1 - r - s) - 1)*(1 - r - s),
                                                               (      r         )*(2r - 1   ),
                                                               (      s         )*(2s - 1   ),
                                                               (     4r         )*(1 - r - s),
                                                               (     4r         )*(        s),
                                                               (     4s         )*(1 - r - s))
function interpolation_weights(::Type{<:QuadraticQuadrilateral}, r, s)
    ξ = 2r - 1; η = 2s - 1
    return Vec((1 - ξ)*(1 - η)*(-ξ - η - 1)/4,
               (1 + ξ)*(1 - η)*( ξ - η - 1)/4,
               (1 + ξ)*(1 + η)*( ξ + η - 1)/4,
               (1 - ξ)*(1 + η)*(-ξ + η - 1)/4,
                          (1 - ξ^2)*(1 - η)/2,
                          (1 - η^2)*(1 + ξ)/2,
                          (1 - ξ^2)*(1 + η)/2,
                          (1 - η^2)*(1 - ξ)/2)
end

# 3-polytope
interpolation_weights(::Type{<:Tetrahedron}, r, s, t) = Vec((1 - r - s - t), r, s, t)
interpolation_weights(::Type{<:Hexahedron},  r, s, t) = Vec((1 - r)*(1 - s)*(1 - t),
                                                            (    r)*(1 - s)*(1 - t),
                                                            (    r)*(    s)*(1 - t),
                                                            (1 - r)*(    s)*(1 - t),
                                                            (1 - r)*(1 - s)*(    t),
                                                            (    r)*(1 - s)*(    t),
                                                            (    r)*(    s)*(    t),
                                                            (1 - r)*(    s)*(    t))
function interpolation_weights(::Type{<:QuadraticTetrahedron}, r, s, t)
    u = 1 - r - s - t
    return Vec((2u-1)u,
               (2r-1)r,
               (2s-1)s,
               (2t-1)t,
                  4u*r,
                  4r*s,
                  4s*u,
                  4u*t,
                  4r*t,
                  4s*t)
end
function interpolation_weights(::Type{<:QuadraticHexahedron}, r, s, t)
    ξ = 2r - 1; η = 2s - 1; ζ = 2t - 1
    return Vec((1 - ξ)*(1 - η)*(1 - ζ)*(-2 - ξ - η - ζ)/8,
               (1 + ξ)*(1 - η)*(1 - ζ)*(-2 + ξ - η - ζ)/8,
               (1 + ξ)*(1 + η)*(1 - ζ)*(-2 + ξ + η - ζ)/8,
               (1 - ξ)*(1 + η)*(1 - ζ)*(-2 - ξ + η - ζ)/8,
               (1 - ξ)*(1 - η)*(1 + ζ)*(-2 - ξ - η + ζ)/8,
               (1 + ξ)*(1 - η)*(1 + ζ)*(-2 + ξ - η + ζ)/8,
               (1 + ξ)*(1 + η)*(1 + ζ)*(-2 + ξ + η + ζ)/8,
               (1 - ξ)*(1 + η)*(1 + ζ)*(-2 - ξ + η + ζ)/8,
                          (1 - ξ^2)*(1 - η  )*(1 - ζ  )/4,
                          (1 + ξ  )*(1 - η^2)*(1 - ζ  )/4,
                          (1 - ξ^2)*(1 + η  )*(1 - ζ  )/4,
                          (1 - ξ  )*(1 - η^2)*(1 - ζ  )/4,
                          (1 - ξ^2)*(1 - η  )*(1 + ζ  )/4,
                          (1 + ξ  )*(1 - η^2)*(1 + ζ  )/4,
                          (1 - ξ^2)*(1 + η  )*(1 + ζ  )/4,
                          (1 - ξ  )*(1 - η^2)*(1 + ζ  )/4,
                          (1 - ξ  )*(1 - η  )*(1 - ζ^2)/4,
                          (1 + ξ  )*(1 - η  )*(1 - ζ^2)/4,
                          (1 + ξ  )*(1 + η  )*(1 - ζ^2)/4,
                          (1 - ξ  )*(1 + η  )*(1 - ζ^2)/4)
end


using FlightSims
const FS = FlightSims
using DifferentialEquations
using LinearAlgebra
using Plots
using Test
using Transducers

function main()
    # linear system
    A = [0 1;
         0 0]  # 2 x 2
    B = [0 1]'  # 2 x 1
    n, m = 2, 1
    env = LinearSystemEnv(A, B)  # exported from FlightSims
    x0 = State(env)([1.0, 2.0])
    p0 = zero.(x0)  # auxiliary parameter
    # optimal control
    Q = Matrix(I, n, n)
    R = Matrix(I, m, m)
    lqr = LQR(A, B, Q, R)  # exported from FlightSims
    u_lqr = Command(lqr)  # (x, p, t) -> -K*x; minimise J = ∫ (x' Q x + u' R u) from 0 to ∞

    # simulation
    tf = 10.0
    Δt = 0.01
    affect!(integrator) = integrator.p = copy(integrator.u)  # auxiliary callback funciton
    cb = PeriodicCallback(affect!, Δt; initial_affect=true)  # auxiliary callback
    @Loggable function dynamics!(dx, x, p, t)
        @onlylog p  # activate this line only when logging data
        u = u_lqr(x)
        @log x, u
        @nested_log Dynamics!(env)(dx, x, p, t; u=u)  # exported `state` and `input` from `Dynamics!(env)`
    end
    prob, df = sim(
                   x0,  # initial condition
                   dynamics!,  # dynamics with input of LQR
                   p0;
                   tf=tf,  # final time
                   callback=cb,
                   savestep=Δt,
                  )
    ts = df.time
    xs = df.sol |> Map(datum -> datum.x) |> collect
    us = df.sol |> Map(datum -> datum.u) |> collect
    ps = df.sol |> Map(datum -> datum.p) |> collect
    states = df.sol |> Map(datum -> datum.state) |> collect
    inputs = df.sol |> Map(datum -> datum.input) |> collect
    @test xs == states
    @test us == inputs
    p_x = plot(ts, hcat(states...)';
               title="state variable", label=["x1" "x2"], color=[:black :black], lw=1.5,
              )  # Plots
    plot!(p_x, ts, hcat(ps...)';
          ls=:dash, label="param", color=[:red :orange], lw=1.5
         )
    savefig("figures/x_lqr.png")
    plot(ts, hcat(inputs...)'; title="control input", label="u")  # Plots
    savefig("figures/u_lqr.png")
    df
end


# get all neighbors at distance l
function l_hop_neighbors!(g, u, l, bfsqueue, is_visited, k)
    startIt, endIt = 1, 2
	bfsqueue[1] = u
	is_visited[u] = true

	for i=1:l
		lastEndIt = endIt
		while startIt != lastEndIt
			for v in neighbors(g, bfsqueue[startIt])
				if k[v] > 0 && !is_visited[v]
					bfsqueue[endIt] = v
					endIt += 1
					is_visited[v] = true
				end
			end
			startIt += 1
		end
	end

	for i=1:endIt-1
		is_visited[bfsqueue[i]] = false
	end
	return startIt, endIt-1
end

function weak_neighbor_decore(g,k=2)
	n = nv(g)
	attack_nodes = Int[]
	deg = degree(g)
	degmax = maximum(deg)
	score = zeros(Int,n)
	bfsqueue = zeros(Int,n)
	is_visited = fill(false, n)
	H = [SortedDict{Int,Set{Int}}() for i=1:degmax]

	for i in vertices(g)
		if deg[i] > 0
			deg_i = max(deg[i], k-1)
			scr_i = 0
			for j in neighbors(g,i)
				scr_i += deg[j]
			end
			score[i] = scr_i
			if haskey(H[deg_i], scr_i)
				push!(H[deg_i][scr_i],i)
			else
				H[deg_i][scr_i] = Set{Int}(i)
			end
		end
	end

	d = isempty(H[k-1]) ? degmax : k-1
	cnt = 0
	done = false
	while cnt < n && done == false
		cnt += 1
		# get the min score of degree d nodes
		min_scr_d, list = first(H[d])

		# remove one of them at random
		i = rand(list)

		# if d >= k add them to the seed set
		d >= k && push!(attack_nodes, i)

		# remove element from H
		delete!(list, i)
		deg[i] = 0
		isempty(list) && delete!(H[d],min_scr_d)

		# update scores of 1-hop neighbors
		startIt, endIt = l_hop_neighbors!(g, i, 1, bfsqueue, is_visited, deg)
		for index in startIt:endIt
			j = bfsqueue[index]
			deg_j = max(deg[j],k-1)
			list = H[deg_j][score[j]]
			delete!(list,j)
			isempty(list) && delete!(H[deg_j], score[j])
			deg[j] -= 1
		end
		for index in startIt:endIt
			j = bfsqueue[index]
			deg_j = max(deg[j],k-1)
			scr_j = 0
			for nb in neighbors(g,j)
				if deg[nb] > 0
					scr_j += deg[nb]
				end
			end
			score[j] = scr_j
			if haskey(H[deg_j],scr_j)
				push!(H[deg_j][scr_j],j)
			else
				H[deg_j][scr_j] = Set{Int}(j)
			end
		end

		# update scores of 2-hop neighbors
		startIt, endIt = l_hop_neighbors!(g, i, 2, bfsqueue, is_visited, deg)
		for index in startIt:endIt
			j = bfsqueue[index]
			deg_j = max(deg[j],k-1)
			list = H[deg_j][score[j]]
			delete!(list,j)
			isempty(list) && delete!(H[deg_j], score[j])
			scr_j = 0
			for nb in neighbors(g,j)
				if deg[nb] > 0
					scr_j += deg[nb]
				end
			end
			score[j] = scr_j
			if haskey(H[deg_j],scr_j)
				push!(H[deg_j][scr_j],j)
			else
				H[deg_j][scr_j] = Set{Int}(j)
			end
		end

		while isempty(H[degmax])
			degmax -= 1
			done = degmax <= k-1
		end

		d = isempty(H[k-1]) ? degmax : k-1
	end
	attack_nodes
end


module StateTest
using Base.Test
using Quantum

xb = basis(:X,[1:5]);
yb = basis(:Y,[1:5]);
xyb = kron(xb,yb);

@test 0*xb[1]==dvec([0], basis(xb[1]))
@test 1*xb[1]==dvec([1], basis(xb[1]))
@test xb[1]'*xb[1]==1
@test isdual(xb[1]',xb[1])
@test sum(xb[:])==DiracVector([1,1,1,1,1], xb)
@test xb[1]'*xyb[1]==1*yb[1]
@test separate(xyb[1])==xyb[1][:]
@test reduce(+,[xb[i]*yb[j]' for i=1:5, j=1:5]) == DiracMatrix(ones(5,5), xb, yb')
@test inner(xyb[1]', xyb[1], 1)==1
@test inner(xyb[1]', xyb[1], 2)==xb[1]'*yb[1]*yb[1]'*xb[1]
@test inner(xyb[1]', xyb[2], 1)==0
@test inner(xyb[1]', xyb[2], 2)==xb[1]'*yb[2]*yb[1]'*xb[1]

k = ket(:K,1)
s = ket(:S,"a")
v = [tensor(s,s),tensor(s,k),tensor(s',s'), tensor(s',k')]

@test typeof(v[1]) == Tensor{Ket{:S,ASCIIString}}
@test typeof(v[2]) == Tensor{Ket}
@test typeof(v[3]) == Tensor{Bra{:S,ASCIIString}}
@test typeof(v[4]) == Tensor{Bra}
@test typeof(tensor(v[1], s)) == Tensor{Ket{:S,ASCIIString}}
@test typeof(tensor(s, v[1])) == Tensor{Ket{:S,ASCIIString}}
@test typeof(tensor(v[2], s)) == Tensor{Ket}
@test typeof(tensor(s, v[2])) == Tensor{Ket}
@test typeof(tensor(v[3], s')) == Tensor{Bra{:S,ASCIIString}}
@test typeof(tensor(s', v[3])) == Tensor{Bra{:S,ASCIIString}}
@test typeof(tensor(v[4], s')) == Tensor{Bra}
@test typeof(tensor(s', v[4])) == Tensor{Bra}

@test typeof(tensor(v[1], v[1])) == Tensor{Ket{:S,ASCIIString}}
@test typeof(tensor(v[2], v[1])) == Tensor{Ket}
@test typeof(tensor(v[3], v[3])) == Tensor{Bra{:S,ASCIIString}}
@test typeof(tensor(v[4], v[3])) == Tensor{Bra}

end


## Julia 1.1.0

# Initialize
using Plots
plotlyjs() # Backend PlotlyJS

angles = 200 * 2π
Θ = range(0, stop = angles, length = 10000)

x(n) = cos.(Θ) + (1.0/n) .* cos.(n .* Θ)
y(n) = sin.(Θ) - (1.0/n) .* sin.(n .* Θ)

v = exp(0.0625)
plot(x(v), y(v), size=(960, 960))
savefig("./png/round-spiral.png")


# Computes μ(mean) and σ(standard deviation) for each kind of component

function getStepData(IT,f)
    getStepData(IT)
end
function getStepData(IT)
    N = IT.obs
    Nel = IT.nelements[STEP]
    μ = Vector{Float64}(Nel)
    σ = Vector{Float64}(Nel)

    for i = 1:Nel
        μ[i] = (N-i)/N
        σ[i] = sqrt(( i*μ[i]^2+(N-i)*(1-μ[i])^2 )/N)
    end
    return σ,μ
end

function getSlopeData(IT,f)
    getSlopeData(IT)
end
function getSlopeData(IT)
    N = IT.obs
    Nel = IT.nelements[SLOPE]
    μ = Vector{Float64}(Nel)
    σ = Vector{Float64}(Nel)

    for i = 1:Nel
        μ[i] = (
        0.5*(N+1)^2 - 0.5*N - i*(N+1) - 0.5*(i+1)^2 + 0.5*i + i*(i+1)
        )/N
        aux =
        (i*μ[i]^2+(N+1)*i^2+(2*(N+1))*i*μ[i]+(N+1)*μ[i]^2
        -i*(N+1)^2+i*(N+1)-μ[i]*(N+1)^2+μ[i]*(N+1)+(1/3)*(N+1)^3
        -(1/2)*(N+1)^2+(1/6)*N-(i+1)*i^2-(2*(i+1))*i*μ[i]
        -(i+1)*μ[i]^2+i*(i+1)^2-i*(i+1)+μ[i]*(i+1)^2
        -μ[i]*(i+1)-(1/3)*(i+1)^3+(1/2)*(i+1)^2-(1/6)*i)/N
        σ[i] = sqrt(aux)
    end
    return σ,μ
end

function getSpikeData(IT,f)
    getSpikeData(IT)
end
function getSpikeData(IT)
    N = IT.obs
    Nel = IT.nelements[SPIKE]
    μ = Vector{Float64}(Nel)
    σ = Vector{Float64}(Nel)

    for i = 1:Nel
        μ[i] = 1/N
        σ[i] = sqrt( ((N-1)*μ[i]^2 + (1-μ[i])^2)/N )
    end
    return σ,μ
end

function getSineData(IT, ω)
    N = IT.obs
    N_frequencies = IT.nelements[4]
    μ = Vector{Float64}(N_frequencies)
    σ = Vector{Float64}(N_frequencies)

    for i = 1:N_frequencies
        sin_ω = sin(ω[i])
        cos_ω = cos(ω[i])
        sin_N1ω = sin((N+1)*ω[i])
        cos_N1ω = cos((N+1)*ω[i])

        if abs(sin_ω)<1e-8
            println("sin ωs")
            println("$(ω[i]) $i")
        end
        if abs(cos_ω)<1e-8
            println("cos ωs")
            println("$(ω[i]) $i")
        end

        μ[i] =  (
        (1/2)*sin_ω*cos_N1ω/(cos_ω-1)-(1/2)*sin_N1ω
        -(1/2)*sin_ω*cos_ω/(cos_ω-1)+(1/2)*sin_ω
        )/N

        σ[i] = sqrt( (

        (1/2+μ[i]^2)*(N+1)+(1/2)*cos_N1ω^2
        -(1/2)*cos_ω*sin_N1ω*cos_N1ω/sin_ω
        -μ[i]*sin_ω*cos_N1ω/(cos_ω-1)+sin_N1ω*μ[i]
        -1/2-μ[i]^2+μ[i]*sin_ω*cos_ω/(cos_ω-1)-μ[i]*sin_ω

      )/N )
    end
    return σ,μ
end

function getCosData(IT, ω)
    N = IT.obs
    N_frequencies = IT.nelements[5]
    μ = Vector{Float64}(N_frequencies)
    σ = Vector{Float64}(N_frequencies)

    for i = 1:N_frequencies
        sin_ω = sin(ω[i])
        cos_ω = cos(ω[i])
        sin_N1ω = sin((N+1)*ω[i])
        cos_N1ω = cos((N+1)*ω[i])

        if abs(sin_ω)<1e-8
            println("sin ωs")
            println("$(ω[i]) $i")
        end
        if abs(cos_ω)<1e-8
            println("cos ωs")
            println("$(ω[i]) $i")
        end

        μ[i] =  (
                 -(1/2)*cos_N1ω-(1/2)*sin_ω*sin_N1ω/(cos_ω-1)
                 +(1/2)*cos_ω+(1/2)*sin_ω^2/(cos_ω-1)
                )/N

                (1/2+μ[i]^2)*(N+1)-(1/2)*cos_N1ω^2
                +(1/2)*cos_ω*sin_N1ω*cos_N1ω/sin_ω
                +cos_N1ω*μ[i]+μ[i]*sin_ω*sin_N1ω/(cos_ω-1)
                -1/2-μ[i]^2-cos_ω*μ[i]-μ[i]*sin_ω^2/(cos_ω-1)

        σ[i] = sqrt( (
                (1/2+μ[i]^2)*(N+1)-(1/2)*cos_N1ω^2
                +(1/2)*cos_ω*sin_N1ω*cos_N1ω/sin_ω
                +cos_N1ω*μ[i]+μ[i]*sin_ω*sin_N1ω/(cos_ω-1)
                -1/2-μ[i]^2-cos_ω*μ[i]-μ[i]*sin_ω^2/(cos_ω-1)
                )/N )
    end
    return σ,μ
end

getData = Vector{Function}(5)
getData[STEP] = getStepData
getData[SPIKE] = getSpikeData
getData[SLOPE] = getSlopeData
getData[SIN] = getSineData
getData[COS] = getCosData


# Bernoulli distributions

function dBernoulli(p,q)
  res=0
  if (p!=q)
     if (p<=0) p = eps() end
     if (p>=1) p = 1-eps() end
     res=(p*log(p/q) + (1-p)*log((1-p)/(1-q)))
  end
  return(res)
end

function dupBernoulli(p,level)
# KL upper confidence bound:
# return qM>p such that d(p,qM)=level
lM = p
uM = min(min(1,p+sqrt(level/2)),1)
for j = 1:16
   qM = (uM+lM)/2
   if dBernoulli(p,qM) > level
      uM= qM
   else
      lM=qM
   end
end
return(uM)
end

function dlowBernoulli(p,level)
# KL lower confidence bound:
# return lM<p such that d(p,lM)=level
lM = max(min(1,p-sqrt(level/2)),0)
uM = p
for j = 1:16
   qM = (uM+lM)/2;
   if dBernoulli(p,qM) > level
      lM= qM;
   else
      uM=qM;
   end
end
return(lM)
end

# Poisson distributions

function dPoisson(p,q)
  if (p==0)
     res=q
  else
     res=q-p + p*log(p/q)
  end
  return(res)
end

function dupPoisson(p,level)
# KL upper confidence bound: generic way
# return qM>p such that d(p,qM)=level
lM = p
# finding an upper bound
uM = max(2*p,1)
while (dPoisson(p,uM)<level)
     uM=2*uM
end
for j = 1:16
   qM = (uM+lM)/2
   if dPoisson(p,qM) > level
      uM= qM
   else
      lM=qM
   end
end
return(uM)
end

function dlowPoisson(p,level)
# KL lower confidence bound: generic way
# return lM<p such that d(p,lM)=level
# finding a lower bound
lM=p/2
if p!=0
   while (dPoisson(p,lM)<level)
      lM=lM/2
   end
end
uM = p
for j = 1:16
   qM = (uM+lM)/2;
   if dPoisson(p,qM) > level
      lM= qM;
   else
      uM=qM;
   end
end
return(lM)
end

# Exponential distribution

function dExpo(p,q)
  res=0
  if (p!=q)
     if (p<=0)|(q<=0)
        res=Inf
     else
        res=p/q - 1 - log(p/q)
     end
  end
  return(res)
end

function dupExpo(p,level)
# KL upper confidence bound: generic way
# return qM>p such that d(p,qM)=level
lM = p
# finding an upper bound
uM = max(2*p,1)
while (dExpo(p,uM)<level)
     uM=2*uM
end
for j = 1:16
   qM = (uM+lM)/2
   if dExpo(p,qM) > level
      uM= qM
   else
      lM=qM
   end
end
return(uM)
end

function dlowExpo(p,level)
# KL lower confidence bound: generic way
# return lM<p such that d(p,lM)=level
# finding a lower bound
lM=p/2
if p!=0
   while (dExpo(p,lM)<level)
      lM=lM/2
   end
end
uM = p
for j = 1:16
   qM = (uM+lM)/2;
   if dExpo(p,qM) > level
      lM= qM;
   else
      uM=qM;
   end
end
return(lM)
end

# Gaussian distribution

function dGaussian(p,q)
  (p-q)^2/(2*sigma^2)
end

function dupGaussian(p,level)
   p+sigma*sqrt(2*level)
end

function dlowGaussian(p,level)
   p-sigma*sqrt(2*level)
end


# Section 5.3.1 in Ericson, C. (2004). Real-time collision detection
function intersect(l::LineSegment{Dim, T}, plane::Hyperplane{Dim, T}) where {Dim, T}
    r = (plane.d - (plane.𝗻 ⋅ l.𝘅₁.coord))/(plane.𝗻 ⋅l.𝘂)
    (r ≥ 0 && r ≤ 1) && return true, l(r)
    return false, nan(Point{Dim,T})
end


"""

```
move!(x::AbstractVector{T}, newcm::AbstractVector,beta, gamma, theta) where T
```

Translates and rotates a molecule according
to the desired input center of coordinates and Euler rotations
modifyies the vector x.

"""
function move!(x::AbstractVector{T}, newcm::AbstractVector,
               beta, gamma, theta) where T

  # Compute center of coordinates of x
  cm = centerofcoordinates(x)

  # Obtain rotation matrix
  A = eulermat(beta, gamma, theta)

  for i in eachindex(x)
    x[i] = A*(x[i]-cm) + newcm
  end

  nothing
end


function ContinuedFraction(α,β,γ,n::Int,N::Int; tol = 1e-10, Nmin = 20, rN = zero(α(1)))
    num = β(0)
    for i = 1:n
        num = β(i) - α(i-1)*γ(i)/num
    end
    lhs = num

    num = β(N)+α(N)*rN
    for i = (N):-1:(n+1)
       num = β(i-1) - α(i-1)*γ(i)/num
    end
    rhs = num - β(n)

    lhs + rhs
end

function rNCoeffs(D0,D1,D2,D3,D4)
    fp = sqrt(-D0 - D1 - D2)
    u1 = -fp
    u2 = (1/2)*(-4 - 2*D0 - D1)
    u3 = (fp)*(8 + 16*D0 + 8*(D0^2) + 12*D1 + 8*D0*D1 + (D1^2) + 8*D2 +
    4*D0*D2 - 4*D3 - 4*D4)/(8*(D0 + D1 + D2))
    u4 = (1/2)*(4 + 4*D0 + 2*D0^2 + D1 + D0*D1 - D3)
    u1,u2,u3,u4
end

function CF(P)
    _,_,(D₀,D₁,D₂,D₃,D₄) = ParameterTransformations(P)
    n = P.n; N = P.Nmax;
    αₙ(n) = n^2 + (D₀+1)*n + D₀
    βₙ(n) = -2*n^2 + (D₁+2)*n +D₃
    γₙ(n) = n^2 + (D₂-3)*n  + D₄ - D₂ + 2
    u1,u2,u3,u4 = rNCoeffs(D₀,D₁,D₂,D₃,D₄)
    rN = 1 + u1*N^(-0.5) + u2*N^(-1) + u3*N^(1.5) + u4*N^(-2)
    ContinuedFraction(αₙ,βₙ,γₙ,n,N; rN = rN)
end


### A Pluto.jl notebook ###
# v0.19.0

using Markdown
using InteractiveUtils

# This Pluto notebook uses @bind for interactivity. When running this notebook outside of Pluto, the following 'mock version' of @bind gives bound variables a default value (instead of an error).
macro bind(def, element)
    quote
        local iv = try Base.loaded_modules[Base.PkgId(Base.UUID("6e696c72-6542-2067-7265-42206c756150"), "AbstractPlutoDingetjes")].Bonds.initial_value catch; b -> missing; end
        local el = $(esc(element))
        global $(esc(def)) = Core.applicable(Base.get, el) ? Base.get(el) : iv(el)
        el
    end
end

# ╔═╡ 1954018c-4a07-4ca5-9011-d340ccd9996f
begin
    import Pkg
    Pkg.activate() # disable Pluto's package management
end

# ╔═╡ 1b8554d8-fd89-11ea-3760-d960e6f55ebd
begin
    using SpecialFunctions
    x = 0:0.1:15
    j0, j1, j2, j3 = [besselj.(order, x) for order in 0:3]
end

# ╔═╡ 05ec2818-fd89-11ea-3da8-23ea406ba0da
begin
    ENV["GRDISPLAY"] = "pluto"
    using GR
end

# ╔═╡ 6a7c464e-fd8b-11ea-042f-cfbcaddf2d0e
begin
    using PlutoUI
    @bind xmax Slider(5:15)
end

# ╔═╡ 25f44cb2-fd89-11ea-1bfb-ad13a07d717e
plot(x, j0, x, j1, x, j2, x, j3,
     labels=["J_0", "J_1", "J_2", "J_3"], location=11, xlim=(0, xmax))

# ╔═╡ Cell order:
# ╠═1954018c-4a07-4ca5-9011-d340ccd9996f
# ╠═1b8554d8-fd89-11ea-3760-d960e6f55ebd
# ╠═05ec2818-fd89-11ea-3da8-23ea406ba0da
# ╠═6a7c464e-fd8b-11ea-042f-cfbcaddf2d0e
# ╠═25f44cb2-fd89-11ea-1bfb-ad13a07d717e


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

using Markdown
using InteractiveUtils

# ╔═╡ 8585ad90-d99f-11eb-0782-8f1b33d43b4d
begin
    import Pkg
    # activate a clean environment
    Pkg.activate(mktempdir())

    Pkg.add(name="Images")
    Pkg.add(name="FileIO")

	using Images
	using FileIO
end

# ╔═╡ 0c8998d9-d8c1-40c3-b147-f44003fef025
begin
	url = "https://i.imgur.com/VGPeJ6s.jpg"
	download(url, "philip.jpg")
	philip = load("philip.jpg");
end

# ╔═╡ 03a8f21d-c985-49b2-9a5e-9bd4355b2d72
decimate(img, k) = img[1:k:end, 1:k:end]

# ╔═╡ 5e7d5bf3-147e-44d1-9c25-8e5a830087e2
poor_phil = decimate(philip, 8)

# ╔═╡ 64857813-e2f0-45f3-992c-e3cb55a1bfaf
function clamp_at_boundary(M, i, j)
	return M[
		clamp(i, 1, size(M, 1)),
		clamp(j, 1, size(M, 2)),
	]
end

# ╔═╡ 568094ea-255d-4100-9c60-2641b78afddd
function convolve(M, kernel, M_index_func=clamp_at_boundary)
	size(M)
    height = size(kernel, 1)
    width = size(kernel, 2)

    half_height = height ÷ 2
    half_width = width ÷ 2
    new_image = similar(M)

	# (i, j) loop over the original image
    @inbounds for i in 1:size(M, 1)
        for j in 1:size(M, 2)
            # (k, l) loop over the neighbouring pixels
			new_image[i, j] = sum([
				kernel[k, l] * M_index_func(M, i - k, j - l)
				for k in -half_height:-half_height + height - 1
				for l in -half_width:-half_width + width - 1
			])
        end
    end

    return new_image
end

# ╔═╡ ece09f3d-9c33-486e-95b2-3a723ef27dc5
box_blur(n) = centered(ones(n, n) ./ (n^2))

# ╔═╡ f9bcaeea-6aaa-4ca2-9a33-f92dea365984
# convolve(poor_phil, box_blur(2))
size(poor_phil)

# ╔═╡ bae786fe-1bba-46e1-8728-778ad0bc349d
kernel = box_blur(5) # Kernel.gaussian((1, 0.1))

# ╔═╡ cb5a3e07-db6b-4751-9480-4f1c07ac7e95
sum(kernel)

# ╔═╡ cf71d105-e775-424f-9da8-9c552ede6fe7
convolve(poor_phil, kernel)

# ╔═╡ 2f573afa-0e25-453c-89ef-7e332252fe42

# ╔═╡ Cell order:
# ╠═8585ad90-d99f-11eb-0782-8f1b33d43b4d
# ╠═0c8998d9-d8c1-40c3-b147-f44003fef025
# ╠═5e7d5bf3-147e-44d1-9c25-8e5a830087e2
# ╟─03a8f21d-c985-49b2-9a5e-9bd4355b2d72
# ╟─64857813-e2f0-45f3-992c-e3cb55a1bfaf
# ╠═568094ea-255d-4100-9c60-2641b78afddd
# ╠═ece09f3d-9c33-486e-95b2-3a723ef27dc5
# ╠═f9bcaeea-6aaa-4ca2-9a33-f92dea365984
# ╠═bae786fe-1bba-46e1-8728-778ad0bc349d
# ╠═cb5a3e07-db6b-4751-9480-4f1c07ac7e95
# ╠═cf71d105-e775-424f-9da8-9c552ede6fe7
# ╠═2f573afa-0e25-453c-89ef-7e332252fe42


import PyPlot

function plot1D(gp::GP; clim::Tuple{Float64, Float64}=(minimum(gp.X), maximum(gp.X)), CI::Float64=1.96, res::Int=1000, fname::AbstractString="")

    sx = (clim[2]-clim[1])/(res-1)
    x=collect(clim[1]:sx:clim[2])
    mu, Sigma = predict(gp, x)
    conf = CI*sqrt(Sigma)
    u = mu + conf
    l = mu - conf

    fig = PyPlot.figure("Gaussian Process Regression")
    ax = fig[:add_subplot](1,1,1)
    ax[:set_xlim](xmin=clim[1], xmax=clim[2])
    ax[:set_ylim](ymin=minimum(l), ymax=maximum(u))

    ax[:plot](x,mu,label="Mean function")       #plot the mean function
    ax[:scatter](gp.X,gp.y,marker="o",label="Observations",color="black")  #plot the observations
    ax[:plot](x,l,label="Confidence Region",color="red")            # plot the confidence region
    ax[:plot](x,u,color="red")
    ax[:fill_between](x,u,l, facecolor="blue", alpha=0.25)
    PyPlot.legend(loc = "best", fontsize=15);
    if fname != ""
        PyPlot.savefig(fname)
    end
end

function plot2D(gp::GP; clim::Tuple{Float64, Float64, Float64, Float64} = (minimum(gp.X[1,:]), maximum(gp.X[1,:]), minimum(gp.X[2,:]), maximum(gp.X[2,:])), res::Int=50, fname::AbstractString="")

    x = linspace(clim[1], clim[2], res)
    y = linspace(clim[3], clim[4], res)
    xgrid = repmat(x', res, 1 )
    ygrid = repmat(y, 1, res )

    mu = predict(gp,[vec(xgrid)';vec(ygrid)'])[1]
    zgrid  = reshape(mu,res,res)

    fig = PyPlot.figure("Contour Plot of Gaussian Process Regression")
    ax = fig[:add_subplot](1,1,1)
    ax[:set_xlim](xmin=clim[1], xmax=clim[2])
    ax[:set_ylim](ymin=clim[3], ymax=clim[4])
    ax[:contour](xgrid,ygrid,zgrid)
    ax[:scatter](gp.X[1,:],gp.X[2,:],marker="o",label="Observations",color="black")  #plot the observations
    if fname != ""
        PyPlot.savefig(fname)
    end
end

function PyPlot.plot(gp::GP; kwargs...)
    d, n = size(gp.X)
    PyPlot.close()
    if d>2
        error("Only 1D and 2D plots are permitted")
    elseif d==1
        plot1D(gp; kwargs...)
    elseif d==2
        plot2D(gp; kwargs...)
    end
end


using LinearAlgebra, Test
using Combinatorics

using MultivariatePolynomials
const MP = MultivariatePolynomials

function div_number_test()
    Mod.@polyvar x
    @test div(2x, 2) == x
    @test div(6x + 9x^2, 3) == 2x + 3x^2
    @test div(6x + 9x^2, 4) == x + 2x^2
    if VERSION >= v"1.6"
        @test div(10x^3, 4, RoundUp) == 3x^3
        @test div(6x + 9x^2, 4, RoundUp) == 2x + 3x^2
    end
end

function gcd_lcm_test()
    Mod.@polyvar x y z
    @test gcd(x*y, y) == y
    @test lcm(x*y, y) == x*y
    @test gcd(x*y^2*z, y*z^3) == y*z
    @test lcm(x*y^2*z, y*z^3) == x*y^2*z^3
    @test gcd(x^2*y^7*z^3, x^4*y^5*z^2) == x^2*y^5*z^2
    @test lcm(x^2*y^7*z^3, x^4*y^5*z^2) == x^4*y^7*z^3
end

# Taken from
# Ideals, Varieties, and Algorithms
# Cox, Little and O'Shea, Fourth edition
# They have been adapted to the grlex ordering
function divides_test()
    Mod.@polyvar x y z
    @test divides(leadingmonomial(x+y), x) # leadingmonomial(x+y) will be x^1*y^0 -> tricky test !
    @test !divides(leadingmonomial(x^2+y), x)
    @test divides(x*y, x^2*y)
    @test divides(x*y, x*y^2)
    @test divides(y*z, x*y*z)
    @test !divides(y*z, x*z)
    @test !divides(x^2*y, x*y^2)
end

function leading_term_test()
    Mod.@polyvar x y z
    @test @inferred(monic(2x^2 + 4y + 2)) == x^2 + 2y + 1
    # See page 60
    p = 4x*y^2*z + 4z^2 + 7x^2*z^2 - 5x^3
    @test leadingcoefficient(p) == 7
    @test leadingmonomial(p) == x^2*z^2
    @test leadingterm(p) == 7x^2*z^2
end

function divrem_test()
    Mod.@polyvar x y
    @test (@inferred div(x*y^2 + 1, x*y + 1)) == y
    @test (@inferred rem(x*y^2 + 1, x*y + 1)) == -y + 1
    @test (@inferred div(x*y^2 + x, y)) == x*y
    @test (@inferred rem(x*y^2 + x, y)) == x
    @test (@inferred rem(x^4 + x^3 + (1+1e-10)*x^2 + 1, x^2 + x + 1)) == 1
    @test (@inferred rem(x^4 + x^3 + (1+1e-10)*x^2 + 1, x^2 + x + 1; ztol=1e-11)) ≈ -((1+1e-10)-1)x + 1
end

function testdiv(p, ps)
    q, r = @inferred divrem(p, ps)
    @test p == dot(q, ps) + r
    q, r
end

function multi_div_test()
    Mod.@polyvar x y
    # Example 1
    q, r = testdiv(x*y^2 + 1, [x*y + 1, y + 1])
    @test q == [y, -1]
    @test r == 2
    # Example 2
    q, r = testdiv(x^2*y + x*y^2 + y^2, [x*y - 1, y^2 - 1])
    @test q == [x + y, 1]
    @test r == x + y + 1
    # Example 4
    q, r = testdiv(x^2*y + x*y^2 + y^2, [y^2 - 1, x*y - 1])
    @test q == [x + 1, x]
    @test r == 2x + 1
    # Example 5
    q, r = testdiv(x*y^2 - x, [x*y - 1, y^2 - 1])
    @test q == [y, 0]
    @test r == -x + y
    q, r = testdiv(x*y^2 - x, [y^2 - 1, x*y - 1])
    @test q == [x, 0]
    @test r == 0

    @testset "Issue DynamicPolynomials#62" begin
        p = x^2 + x + 1
        q = rem(p, [x^2-y])
        @test q == x + y + 1
    end

    @test (@inferred rem(x^2*y + (1+1e-10)*x*y + 1, [x^2 + x, y + 1])) == 1
    @test (@inferred rem(x^2*y + (1+1e-10)*x*y + 1, [x^2 + x, y + 1]; ztol=1e-11)) == -((1+1e-10)-1)x + 1
end

function test_gcd_unit(expected, p1, p2, algo)
    g = @inferred gcd(p1, p2, algo)
    # it does not make sense, in general, to speak of "the" greatest common
    # divisor of u and v; there is a set of greatest common divisors, each
    # one being a unit multiple of the others [Knu14, p. 424] so `expected` and
    # `-expected` are both accepted.
    @test g == expected || g == -expected
end

function test_gcdx_unit(expected, p1, p2, algo)
    test_gcd_unit(expected, p1, p2, algo)
    a, b, g = @inferred gcdx(p1, p2, algo)
    # it does not make sense, in general, to speak of "the" greatest common
    # divisor of u and v; there is a set of greatest common divisors, each
    # one being a unit multiple of the others [Knu14, p. 424] so `expected` and
    # `-expected` are both accepted.
    @test iszero(MP.pseudo_rem(g, expected, algo)[2])
    @test a * p1 + b * p2 == g
end
function _test_gcdx_unit(expected, p1, p2, algo)
    test_gcdx_unit(expected, p1, p2, algo)
    test_gcdx_unit(expected, p2, p1, algo)
end

function univariate_gcd_test(algo=GeneralizedEuclideanAlgorithm())
    Mod.@polyvar x
    test_gcdx_unit(x + 1, x^2 - 1, x^2 + 2x + 1, algo)
    test_gcdx_unit(x + 1, x^2 + 2x + 1, x^2 - 1, algo)
    test_gcdx_unit(x + 1, x^2 - 1, x + 1, algo)
    test_gcdx_unit(x + 1, x + 1, x^2 - 1, algo)
    test_gcdx_unit(x + 1, x - x, x + 1, algo)
    test_gcdx_unit(x + 1, x + 1, x - x, algo)
    test_gcdx_unit(x - x + 1, x + 1, x + 2, algo)
    test_gcdx_unit(x - x + 1, x + 1, 2x + 1, algo)
    test_gcdx_unit(x - x + 1, x - 1, 2x^2 - 2x - 2, algo)
    @test       0  == @inferred gcd(x - x, x^2 - x^2, algo)
    @test       0  == @inferred gcd(x^2 - x^2, x - x, algo)
end

function _mult_test(a::Number, b)
    @test iszero(maxdegree(b))
end
function _mult_test(a::Number, b::Number)
    @test iszero(rem(a, b))
    @test iszero(rem(b, a))
end
function _mult_test(a, b)
    @test iszero(rem(a, b))
    @test iszero(rem(b, a))
end
function mult_test(expected, a, b, algo)
    g = @inferred MP._simplifier(a, b, algo)
    @test g isa promote_type(polynomialtype(a), polynomialtype(b))
    _mult_test(expected, g)
end
function mult_test(expected, a::Number, b, algo)
    g = @inferred MP._simplifier(a, b, algo)
    @test g isa promote_type(typeof(a), MP.coefficienttype(b))
    _mult_test(expected, g)
end
function mult_test(expected, a, b::Number, algo)
    g = @inferred MP._simplifier(a, b, algo)
    @test g isa promote_type(MP.coefficienttype(a), typeof(b))
    _mult_test(expected, g)
end
function sym_test(a, b, g, algo)
    mult_test(g, a, b, algo)
    mult_test(g, b, a, algo)
end
function triple_test(a, b, c, algo)
    sym_test(a * c, b * c, gcd(a, b, algo) * c, algo)
    sym_test(b * a, c * a, gcd(b, c, algo) * a, algo)
    sym_test(c * b, a * b, gcd(c, a, algo) * b, algo)
end

function multivariate_gcd_test(::Type{T}, algo=GeneralizedEuclideanAlgorithm()) where {T}
    Mod.@polyvar x y z
    o = one(T)
    zr = zero(T)
    sym_test(o, o * x, o, algo)
    sym_test(o * x, zr * x, o * x, algo)
    sym_test(o * x + o, zr * x, o * x + o, algo)
    # Inspired from https://github.com/JuliaAlgebra/MultivariatePolynomials.jl/issues/160
    f1 = o * x * y + o * x
    f2 = o * y^2
    f3 = o * x
    sym_test(f1, f2, 1, algo)
    sym_test(f2, f3, 1, algo)
    sym_test(f3, f1, x, algo)
    triple_test(f1, f2, f3, algo)

    @testset "Issue #173" begin
        p1 = o*x*y + x
        p2 = x^2
        sym_test(p1, p2, x, algo)
    end

    p1 = o*z - z
    p2 = z
    @test gcd(p1, p2, algo) == z
    p1 = o*y - y
    p2 = z
    @test gcd(p1, p2, algo) == z
    test_relatively_prime(p1, p2, algo) = test_gcd_unit(o, p1, p2, algo)
    test_relatively_prime(2o*y*z - o, y - z, algo)
    test_relatively_prime(2o*y^2*z - y, y - z, algo)
    test_relatively_prime(2o*y^2*z - y, y^3*z - y - z, algo)
    test_relatively_prime(2o*y^3 + 2*y^2*z - y, y^3 + y^3*z - y - z, algo)
    test_relatively_prime(z^4 + 2o*y^3 + 2*y^2*z - y, y*z^4 + y^3 + y^3*z - y - z, algo)
    test_relatively_prime(
        y*z^3 + z^4 + 2o*y^3 + 2*y^2*z - y,
        y^2*z^3 + y*z^4 + y^3 + y^3*z - y - z,
        algo,
    )
    if T != Int || (algo != GeneralizedEuclideanAlgorithm(false, false) && algo != GeneralizedEuclideanAlgorithm(true, false))
        test_relatively_prime(
            -3o*y^2*z^3 - 3*y^4 + y*z^3 + z^4 + 2*y^3 + 2*y^2*z - y,
            3o*y^3*z^3 - 2*y^5 + y^2*z^3 + y*z^4 + y^3 + y^3*z - y - z,
            algo,
        )
    end
    test_relatively_prime(
        -z^6 - 3o*y^2*z^3 - 3*y^4 + y*z^3 + z^4 + 2*y^3 + 2*y^2*z - y,
        -y*z^6 - 3o*y^3*z^3 - 2*y^5 + y^2*z^3 + y*z^4 + y^3 + y^3*z - y - z,
        algo,
    )
    test_relatively_prime(
        -z^6 - 3o*y^2*z^3 - 3*y^4 + y*z^3 + z^4 + 2*y^3 + 2*y^2*z - y,
        -y^2*z^6 - 3o*y^4*z^3 - 2*y^6 + y^3*z^3 + y^2*z^4 + y^5 + y^4*z - y^2 - y*z,
        algo,
    )

    a = (o * x + o * y^2) * (o * z^3 + o * y^2 + o * x)
    b = (o * x + o * y + o * z) * (o * x^2 + o * y)
    c = (o * x + o * y + o * z) * (o * z^3 + o * y^2 + o * x)
    if T != Int || (algo != GeneralizedEuclideanAlgorithm(false, false) && algo != GeneralizedEuclideanAlgorithm(true, false))
        sym_test(a, b, 1, algo)
    end
    sym_test(b, c, x + y + z, algo)
    sym_test(c, a, z^3 + y^2 + x, algo)
    if (T != Int || (algo != GeneralizedEuclideanAlgorithm(false, false) && algo != GeneralizedEuclideanAlgorithm(true, false) && algo != GeneralizedEuclideanAlgorithm(false, true))) &&
        (T != Float64 || (algo != GeneralizedEuclideanAlgorithm(false, true) && algo != GeneralizedEuclideanAlgorithm(true, true)))
        triple_test(a, b, c, algo)
    end
end

function lcm_test()
    Mod.@polyvar x
    l = @inferred lcm(x^2 - 1, x^2 + 2x + 1)
    exp = -(x-1) * (x+1)^2
    @test l == exp || l == -exp
    l = @inferred lcm(x^2 + 2x + 1, x^2 - 1)
    @test l == exp || l == -exp
    @test x^2 - 1 == @inferred lcm(x^2 - 1, x - 1)
    @test x^2 - 1 == @inferred lcm(x - 1, x^2 - 1)
    @test       0 == @inferred lcm(x - x, x + 1)
    @test       0 == @inferred lcm(x + 1, x - x)
    @test       0 == @inferred lcm(x - x, x^2 - x^2)
    @test       0 == @inferred lcm(x^2 - x^2, x - x)
end

function deflation_test()
    Mod.@polyvar a b c
    function _test(p, eshift, edefl)
        shift, defl = MP.deflation(p)
        @test shift == eshift
        @test defl == edefl
        q = MP.deflate(p, shift, defl)
        @test p == MP.inflate(q, shift, defl)
    end
    for (x, y, z) in permutations((a, b, c))
        _test(x + x^2 + y * x, x, x * y)
        p = x^2 + x^2 * y^3 + y^6 * x^4
        _test(p, x^2, x^2 * y^3)
        @test p == MP.deflate(p, z^0, z^0)
        _test(p, x^2, x^2 * y^3)
    end
end

function extracted_variable_test()
    Mod.@polyvar x y z
    function _test(p1, p2, w1, w2)
        i1, i2, n = MP._extracted_variable(p1, p2)
        v(p, i) = iszero(i) ? nothing : variables(p)[i]
        if string(Mod) == "DynamicPolynomials"
            alloc_test(() -> MP._extracted_variable(p1, p2), 0)
        end
        @test v(p1, i1) == w1
        @test v(p2, i2) == w2
        i2, i1, n = MP._extracted_variable(p2, p1)
        if string(Mod) == "DynamicPolynomials"
            alloc_test(() -> MP._extracted_variable(p2, p1), 0)
        end
        @test v(p1, i1) == w1
        @test v(p2, i2) == w2
    end
    _test(x - x, y - y, nothing, nothing)
    _test(x + y + z, x - z, y, nothing)
    _test(x + y + z, x - y, z, nothing)
    _test(x + y + z, y - z, x, nothing)
    _test(x^4 + y^5 + z^6, x^3 + y^2 + z, z, z)
    _test(x^6 + y^5 + z^4, x + y^2 + z^3, x, x)
    _test(x^6 + y + z^4 - y, x + y + z^3 - y, x, x)
end

@testset "Division" begin
    @testset "div by number" begin
        div_number_test()
    end
    @testset "GCD and LCM" begin
        gcd_lcm_test()
    end
    @testset "Divides" begin
        divides_test()
    end
    @testset "Leading function" begin
        leading_term_test()
    end
    @testset "Division examples" begin
        divrem_test()
    end
    @testset "Division by multiple polynomials examples" begin
        multi_div_test()
    end
    @testset "Univariate gcd primitive_rem=$primitive_rem" for primitive_rem in [false, true]
        @testset "skip_last=$skip_last" for skip_last in [false, true]
            univariate_gcd_test(GeneralizedEuclideanAlgorithm(primitive_rem, skip_last))
        end
    end
    @testset "Multivariate gcd $T" for T in [Int, BigInt, Rational{BigInt}, Float64]
        if T != Rational{BigInt} || VERSION >= v"1.6"
            # `gcd` for `Rational{BigInt}` got defined at some point between v1.0 and v1.6
            @testset "primitive_rem=$primitive_rem" for primitive_rem in [false, true]
                @testset "skip_last=$skip_last" for skip_last in [false, true]
                    multivariate_gcd_test(T, GeneralizedEuclideanAlgorithm(primitive_rem, skip_last))
                end
            end
        end
    end
    @testset "lcm" begin
        lcm_test()
    end
end


@testset "Continuous identity system" begin
    for sd in 1:3
        s = ContinuousIdentitySystem(sd)
        @test statedim(s) == sd
        @test inputdim(s) == 0
        @test noisedim(s) == 0
        @test islinear(s) && isaffine(s) && !ispolynomial(s)
        @test !isnoisy(s) && !iscontrolled(s) && !isconstrained(s)
    end
end

@testset "Continuous constrained identity system" begin
    for sd in 1:3
        X = Singleton(ones(sd))
        s = ConstrainedContinuousIdentitySystem(sd, X)
        @test statedim(s) == sd
        @test inputdim(s) == 0
        @test noisedim(s) == 0
        @test stateset(s) == X
        @test islinear(s) && isaffine(s) && !ispolynomial(s)
        @test !isnoisy(s) && !iscontrolled(s) && isconstrained(s)
    end
end

@testset "Continuous linear system" begin
    for sd in 1:3
        s = LinearContinuousSystem(zeros(sd, sd))
        @test statedim(s) == sd
        @test inputdim(s) == 0
        @test noisedim(s) == 0
        @test islinear(s) && isaffine(s) && !ispolynomial(s)
        @test !isnoisy(s) && !iscontrolled(s) && !isconstrained(s)
    end
end

@testset "Continuous affine system" begin
    for sd in 1:3
        s = AffineContinuousSystem(zeros(sd, sd), zeros(sd))
        @test statedim(s) == sd
        @test inputdim(s) == 0
        @test !islinear(s) && isaffine(s) && !ispolynomial(s)
        @test !isnoisy(s) && !iscontrolled(s) && !isconstrained(s)
    end
end

@testset "Continuous linear control system" begin
    for sd in 1:3
        s = LinearControlContinuousSystem(zeros(sd, sd), ones(sd, sd))
        @test statedim(s) == sd
        @test inputdim(s) == sd
        @test noisedim(s) == 0
        @test islinear(s) && isaffine(s) && !ispolynomial(s)
        @test !isnoisy(s) && iscontrolled(s) && !isconstrained(s)
    end
end

@testset "Continuous constrained linear system" begin
    A = [1. 1; 1 -1]
    X = Line([1., -1], 0.) # line x = y
    s = ConstrainedLinearContinuousSystem(A, X)
    @test statedim(s) == 2
    @test inputdim(s) == 0
    @test noisedim(s) == 0
    @test stateset(s) == X
    @test islinear(s) && isaffine(s) && !ispolynomial(s)
    @test !isnoisy(s) && !iscontrolled(s) && isconstrained(s)
end

@testset "Continuous constrained affine system" begin
    A = [1. 1; 1 -1]
    c = [1.; 1.]
    X = Line([1., -1], 0.) # line x = y
    s = ConstrainedAffineContinuousSystem(A, c, X)
    @test statedim(s) == 2
    @test inputdim(s) == 0
    @test noisedim(s) == 0
    @test stateset(s) == X
    @test !islinear(s) && isaffine(s) && !ispolynomial(s)
    @test !isnoisy(s) && !iscontrolled(s) && isconstrained(s)
end

@testset "Continuous affine control system with state constraints" begin
    A = zeros(2, 2)
    B = Matrix([0.5 1.5]')
    c = [0.0, 1.0]
    X = Line([1., -1], 0.) # line x = y
    U = Interval(-1.0, 1.0)  # -1 <= u <= 1
    s = ConstrainedAffineControlContinuousSystem(A, B, c, X, U)
    @test statedim(s) == 2
    @test inputdim(s) == 1
    @test noisedim(s) == 0
    @test stateset(s) == X
    @test inputset(s) == U
    @test !islinear(s) && isaffine(s) && !ispolynomial(s)
    @test !isnoisy(s) && iscontrolled(s) && isconstrained(s)
end

@testset "Continuous constrained linear control system" begin
    A = [1. 1; 1 -1]
    B = Matrix([0.5 1.5]')
    X = Line([1., -1], 0.)
    U = Interval(0.9, 1.1)
    s = ConstrainedLinearControlContinuousSystem(A, B, X, U)
    @test statedim(s) == 2
    @test inputdim(s) == 1
    @test noisedim(s) == 0
    @test stateset(s) == X
    @test inputset(s) == U
    @test islinear(s) && isaffine(s) && !ispolynomial(s)
    @test !isnoisy(s) && iscontrolled(s) && isconstrained(s)

    # initial value problem composite type
    x0 = Singleton([1.5, 2.0])
    p = InitialValueProblem(s, x0)
    @test inputset(p) == U
    @test statedim(p) == 2
    @test inputdim(p) == 1

    # check that the matrices A and B need not be of the same type
    # with A dense and B a lazy adjoint
    B = [0.5 1.5]'
    s = ConstrainedLinearControlContinuousSystem(A, B, X, U)

    # check that the matrices A and B need not be of the same type
    # with A sparse and B a lazy adjoint
    A = sparse([1], [2], [1.0], 2, 2) # sparse matrix
    s = ConstrainedLinearControlContinuousSystem(A, B, X, U)
end

@testset "Continuous linear algebraic system" begin
    for sd in 1:3
        s = LinearAlgebraicContinuousSystem(zeros(sd, sd), zeros(sd, sd))
        @test statedim(s) == sd
        @test inputdim(s) == 0
        @test noisedim(s) == 0
        @test islinear(s) && isaffine(s) && !ispolynomial(s)
        @test !isnoisy(s) && !iscontrolled(s) && !isconstrained(s)
    end
end

@testset "Continuous constrained linear algebraic system" begin
    A = [1. 1; 1 -1]
    E = [0. 1; 1 0]
    X = LinearConstraint([0, -1.], 0.) # the set y ≥ 0
    s = ConstrainedLinearAlgebraicContinuousSystem(A, E, X)
    @test statedim(s) == 2
    @test inputdim(s) == 0
    @test noisedim(s) == 0
    @test stateset(s) == X
    @test islinear(s) && isaffine(s) && !ispolynomial(s)
    @test !isnoisy(s) && !iscontrolled(s) && isconstrained(s)
end

@testset "Initial value problem for a continuous constrained linear algebraic system" begin
    A = [1. 1; 1 -1]
    E = [0. 1; 1 0]
    X = LinearConstraint([0, -1.], 0.) # the set y ≥ 0
    s = ConstrainedLinearAlgebraicContinuousSystem(A, E, X)
    @test statedim(s) == 2
    @test inputdim(s) == 0
    @test noisedim(s) == 0
    @test stateset(s) == X
    @test islinear(s) && isaffine(s) && !ispolynomial(s)
    @test !isnoisy(s) && !iscontrolled(s) && isconstrained(s)

    x0 = Singleton([1.5, 2.0])
    p = IVP(s, x0)
end

@testset "Polynomial system in continuous time" begin
    @polyvar x y
    p = 2x^2 - 3x + y

    # default constructor for scalar p and
    s = PolynomialContinuousSystem(p)
    @test statedim(s) == 2
    @test inputdim(s) == 0
    @test noisedim(s) == 0
    @test !islinear(s) && !isaffine(s) && ispolynomial(s)
    @test !isnoisy(s) && !iscontrolled(s) && !isconstrained(s)

    @test TypedPolynomials.nvariables(s) == 2
    @test TypedPolynomials.variables(s) == (x, y)

    # constructor for scalar p and given number of variables
    s = PolynomialContinuousSystem([p], 2)
end

@testset "Polynomial system in continuous time with state constraints" begin
    @polyvar x y
    p = 2x^2 - 3x + y
    X = BallInf(zeros(2), 0.1)

    # default constructor for scalar p and
    s = ConstrainedPolynomialContinuousSystem(p, X)
    @test statedim(s) == 2
    @test inputdim(s) == 0
    @test noisedim(s) == 0
    @test dim(stateset(s)) == dim(X)
    @test !islinear(s) && !isaffine(s) && ispolynomial(s)
    @test !isnoisy(s) && !iscontrolled(s) && isconstrained(s)

    @test TypedPolynomials.nvariables(s) == 2
    @test TypedPolynomials.variables(s) == (x, y)
end

function vanderpol!(x, dx)
    dx[1] = x[2]
    dx[2] = x[2] * (1-x[1]^2) - x[1]
    return dx
end

@testset "Continuous system defined by a function" begin
    s = BlackBoxContinuousSystem(vanderpol!, 2)
    f! = (x, dx) -> s.f(x, dx)
    x = [1.0, 0.0]
    dx = similar(x)
    f!(x, dx)
    @test dx ≈ [0.0, -1.0]
    @test !islinear(s) && !isaffine(s) && !ispolynomial(s)
    @test !isnoisy(s) && !iscontrolled(s) && !isconstrained(s)
end

@testset "Continuous system defined by a function with state constraints" begin
    H = HalfSpace([1.0, 0.0], 0.0) # x <= 0
    s = ConstrainedBlackBoxContinuousSystem(vanderpol!, 2, H)
    f! = (x, dx) -> s.f(x, dx)
    x = [1.0, 0.0]
    dx = similar(x)
    f!(x, dx)
    @test dx ≈ [0.0, -1.0]
    @test stateset(s) == H
    @test !islinear(s) && !isaffine(s) && !ispolynomial(s)
    @test !isnoisy(s) && !iscontrolled(s) && isconstrained(s)
end

function vanderpol_controlled!(x, u, dx)
    dx[1] = x[2]
    dx[2] = x[2] * (1-x[1]^2) - x[1] + u[1]
    return dx
end

@testset "Continuous control system defined by a function with state constraints" begin
    H = HalfSpace([1.0, 0.0], 0.0) # x <= 0
    U = Interval(-0.1, 0.1)
    s = ConstrainedBlackBoxControlContinuousSystem(vanderpol_controlled!, 2, 1, H, U)
    f! = (x, u, dx) -> s.f(x, u, dx)
    x = [1.0, 0.0]
    u = an_element(U)
    dx = similar(x)
    f!(x, u, dx)
    @test dx ≈ [0.0, -1.0 + u[1]]
    @test statedim(s) == 2
    @test inputdim(s) == 1
    @test stateset(s) == H
    @test inputset(s) == U
    @test !islinear(s) && !isaffine(s) && !ispolynomial(s)
    @test !isnoisy(s) && iscontrolled(s) && isconstrained(s)
end

# Noisy
@testset "Noisy Continuous constrained linear system" begin
    A = [1. 1; 1 -1]
    D = [1. 2; 0 1]
    X = Line([1., -1], 0.) # line x = y
    W = BallInf(zeros(2), 2.0)
    s = NoisyConstrainedLinearContinuousSystem(A, D, X, W)
    @test s.A == A
    @test s.D == D
    @test statedim(s) == 2
    @test inputdim(s) == 0
    @test noisedim(s) == 2 == dim(W)
    @test stateset(s) == X
    @test noiseset(s) == W
    @test islinear(s) && isaffine(s) && !ispolynomial(s)
    @test isnoisy(s) && !iscontrolled(s) && isconstrained(s)
end

@testset "Noisy Continuous constrained control linear system" begin
    A = [1. 1; 1 -1]
    B = Matrix([0.5 1.5]')
    D = [1. 2; 0 1]
    X = Line([1., -1], 0.) # line x = y
    U = Hyperrectangle(low=[0.9], high=[1.1])
    W = BallInf(zeros(2), 2.0)
    s = NoisyConstrainedLinearControlContinuousSystem(A, B, D, X, U, W)
    @test s.A == A
    @test s.B == B
    @test s.D == D
    @test statedim(s) == 2
    @test inputdim(s) == dim(U)
    @test noisedim(s) == 2 == dim(W)
    @test stateset(s) == X
    @test inputset(s) == U
    @test noiseset(s) == W
    @test islinear(s) && isaffine(s) && !ispolynomial(s)
    @test isnoisy(s) && iscontrolled(s) && isconstrained(s)
end

@testset "Noisy Continuous constrained control affine system" begin
    A = [1. 1; 1 -1]
    B = Matrix([0.5 1.5]')
    c = [1.0, 0.5]
    D = [1. 2; 0 1]
    X = Line([1., -1], 0.) # line x = y
    U = Hyperrectangle(low=[0.9], high=[1.1])
    W = BallInf(zeros(2), 2.0)
    s = NoisyConstrainedAffineControlContinuousSystem(A, B, c, D, X, U, W)
    @test s.A == A
    @test s.B == B
    @test s.c == c
    @test s.D == D
    @test statedim(s) == 2
    @test inputdim(s) == dim(U)
    @test noisedim(s) == 2 == dim(W)
    @test stateset(s) == X
    @test inputset(s) == U
    @test noiseset(s) == W
    @test isaffine(s) && !islinear(s) && !ispolynomial(s)
    @test isnoisy(s) && iscontrolled(s) && isconstrained(s)
end

@testset "Noisy Continuous constrained control blackbox system" begin
    n = 2
    m = 1
    l = 2
    f(x,u,w) = ones(n,n)*x + ones(n,m)*u + ones(n,l)*w
    X = BallInf(zeros(n), 1.0)
    U =  BallInf(zeros(m), 1.0)
    W =  BallInf(zeros(l), 1.0)
    s = NoisyConstrainedBlackBoxControlContinuousSystem(f, n, m, l, X, U, W)
    @test s.f == f
    @test statedim(s) == n
    @test inputdim(s) == dim(U)
    @test noisedim(s) == l == dim(W)
    @test stateset(s) == X
    @test inputset(s) == U
    @test noiseset(s) == W
    @test !islinear(s) && !isaffine(s) && !ispolynomial(s)
    @test isnoisy(s) && iscontrolled(s) && isconstrained(s)
end


## Testing: Compare it GLMNet on toy problem
@testset "Compare with GLMNet" begin
    N = 100
    srand(1)
    y = collect(1:N) + randn(N)*10
    X = [1:N (1:N)+randn(N)*5 (1:N)+randn(N)*10 (1:N)+randn(N)*20]
    X = X .- mean(X, 1)
    y = y .- mean(y)
    norm_x = sqrt(N)*std(X, 1, corrected=false)
    std_y = std(y, corrected=false)
    yn = y./std_y
    Xn = X./norm_x

    # Test for a single value
    λ = 0.001
    path = GLMNet.glmnet(Xn, yn; lambda=[λ], intercept=false, standardize=false,
              tol=1e-20)
    β, _ = NarmaxLasso.pathwise_coordinate_optimization(Xn, yn, λ, tol=1e-20)
    @test vec(Matrix{Float64}(path.betas)) ≈ β atol=1e-5

    # Test for multiple value
    λ_list = collect(100:-1:1)*0.01
    path = GLMNet.glmnet(Xn, yn; lambda=λ_list, intercept=false,
              standardize=false, tol=1e-20)
    β, _ = NarmaxLasso.pathwise_coordinate_optimization(Xn, yn, λ_list, tol=1e-20)
    @test Matrix{Float64}(path.betas) ≈ β atol=1e-5
end

## Generate nonlinear System Inputs
function nonlinear_function(y, u, v)
    return ((0.8 - 0.5*exp(-y[1]^2))*y[1] - (0.3 + 0.9*exp(-y[1]^2))*y[2] + u[1]
            + 0.2*u[2] + 0.1*u[1]*u[2] + 0.1*v[1] + 0.3*v[2])
end

## Testing: Compare it GLMNet on dynamic example
@testset "Compare with GLMNet on Dynamic Data" begin
    # Generate input
    u = NarmaxLasso.generate_random_input(1000, 5);
    y = NarmaxLasso.simulate(u, nonlinear_function, [1, 2], [1, 2], [1, 2];
                             σv=0.1, σw=0.5);
    # Generate Regressor matrix
    X = NarmaxLasso.lag([u y], [collect(1:30),
                    collect(1:30)])
    N, p = size(X)
    # Normalize
    X = X .- mean(X, 1)
    y = y .- mean(y)
    norm_x = sqrt(N)*std(X, 1, corrected=false)
    std_y = std(y, corrected=false)
    y = y./std_y
    X = X./norm_x
    # Test single point
    λ = 1e-5
    path = GLMNet.glmnet(X, y, lambda=[λ], intercept=false,
                        standardize=false, tol=1e-20)
    β, λ= NarmaxLasso.pathwise_coordinate_optimization(X, y, λ, tol=1e-15)
    @test vec(Matrix{Float64}(path.betas)) ≈ β atol=1e-5
    # Test sequence of points
    λ = collect(100:-1:1)*0.00001
    path = GLMNet.glmnet(X, y, lambda=λ, intercept=false,
                         standardize=false, tol=1e-20)
    β, λ= NarmaxLasso.pathwise_coordinate_optimization(X, y, λ, tol=1e-15)
    @test Matrix{Float64}(path.betas) ≈ β atol=1e-5
end


"""

# d_separation

$(SIGNATURES)

Computes the d_separation between 2 sets of nodes conditioned on a third set.

### Required arguments
```julia
d_separation(
* `d::DAG`                             : DAG
* `f::SymbolList`                      : First set
*  s::SymbolList`                      : Second set
)
```

### Keyword arguments
```julia
* `c::SymbolListOrNothing=nothing`     : Conditioning set
* `debug=false`                        : Trace execution
```

### Returns
```julia
* `res::Bool`                          : Boolean result of test
```

# Extended help

### Example

### d_separation between mechanics and statistics, conditioning on algebra
```julia
using StructuralCausalModels, CSV

df = DataFrame!(CSV.File(scm_path("..", "data", "marks.csv"));

d = OrderedDict(
  :mechanics => [:vectors, :algebra],
  :vectors => [:algebra],
  :analysis => [:algebra],
  :statistics => [:algebra, :analysis]
);

dag = DAG("marks", d, df);
d_separation(marks, [:statistics], [:mechanics]; c=[:algebra]))
```
### Acknowledgements

Original author:                       Giovanni M. Marchetti

Translated to Julia:                   Rob J Goedman

### License

The R package ggm is licensed under License: GPL-2.

The Julia translation is licenced under: MIT.

Part of the API, exported.
"""
function d_separation(d::DAG, f::SymbolList, s::SymbolList;
  c::SymbolListOrNothing=nothing, debug=false)

  e = induced_covariance_graph(d, vcat(f, s), c; debug=debug)
  sum(e[f, s]) == 0

end

export
  d_separation


# ---
# title: 1411. Number of Ways to Paint N × 3 Grid
# id: problem1411
# author: Tian Jun
# date: 2020-10-31
# difficulty: Hard
# categories: Dynamic Programming
# link: <https://leetcode.com/problems/number-of-ways-to-paint-n-3-grid/description/>
# hidden: true
# ---
#
# You have a `grid` of size `n x 3` and you want to paint each cell of the grid
# with exactly one of the three colours: **Red** , **Yellow** or **Green**
# while making sure that no two adjacent cells have the same colour (i.e no two
# cells that share vertical or horizontal sides have the same colour).
#
# You are given `n` the number of rows of the grid.
#
# Return _the number of ways_ you can paint this `grid`. As the answer may grow
# large, the answer **must be** computed modulo `10^9 + 7`.
#
#
#
# **Example 1:**
#
#
#
#     Input: n = 1
#     Output: 12
#     Explanation: There are 12 possible way to paint the grid as shown:
#     ![](https://assets.leetcode.com/uploads/2020/03/26/e1.png)
#
#
# **Example 2:**
#
#
#
#     Input: n = 2
#     Output: 54
#
#
# **Example 3:**
#
#
#
#     Input: n = 3
#     Output: 246
#
#
# **Example 4:**
#
#
#
#     Input: n = 7
#     Output: 106494
#
#
# **Example 5:**
#
#
#
#     Input: n = 5000
#     Output: 30228214
#
#
#
#
# **Constraints:**
#
#   * `n == grid.length`
#   * `grid[i].length == 3`
#   * `1 <= n <= 5000`
#
#
## @lc code=start
using LeetCode

## add your code here:
## @lc code=end


# ---
# title: 1513. Number of Substrings With Only 1s
# id: problem1513
# author: Tian Jun
# date: 2020-10-31
# difficulty: Medium
# categories: Math, String
# link: <https://leetcode.com/problems/number-of-substrings-with-only-1s/description/>
# hidden: true
# ---
#
# Given a binary string `s` (a string consisting only of '0' and '1's).
#
# Return the number of substrings with all characters 1's.
#
# Since the answer may be too large, return it modulo 10^9 + 7.
#
#
#
# **Example 1:**
#
#
#
#     Input: s = "0110111"
#     Output: 9
#     Explanation: There are 9 substring in total with only 1's characters.
#     "1" -> 5 times.
#     "11" -> 3 times.
#     "111" -> 1 time.
#
# **Example 2:**
#
#
#
#     Input: s = "101"
#     Output: 2
#     Explanation: Substring "1" is shown 2 times in s.
#
#
# **Example 3:**
#
#
#
#     Input: s = "111111"
#     Output: 21
#     Explanation: Each substring contains only 1's characters.
#
#
# **Example 4:**
#
#
#
#     Input: s = "000"
#     Output: 0
#
#
#
#
# **Constraints:**
#
#   * `s[i] == '0'` or `s[i] == '1'`
#   * `1 <= s.length <= 10^5`
#
#
## @lc code=start
using LeetCode

## add your code here:
## @lc code=end


qs = 1.0 .+ 2.0.^(-5:2)
times = Array{Float64}(undef,length(qs),4)
means = Array{Float64}(undef,length(qs),4)

using StochasticDiffEq, DiffEqProblemLibrary, Random,
      Plots, ParallelDataTransfer, DiffEqMonteCarlo, Distributed
Random.seed!(99)

using DiffEqProblemLibrary.SDEProblemLibrary: importsdeproblems; importsdeproblems()
full_prob = DiffEqProblemLibrary.SDEProblemLibrary.oval2ModelExample(largeFluctuations=true,useBigs=false)
import DiffEqProblemLibrary.SDEProblemLibrary: prob_sde_additivesystem,
            prob_sde_additive, prob_sde_2Dlinear, prob_sde_linear, prob_sde_wave
prob = remake(full_prob,tspan=(0.0,1.0))

println("Solve once to compile.")
sol = solve(prob,EM(),dt=1/2^(18))
Int(sol.u[end][1]!=NaN)
println("Compilation complete.")
num_runs = 10000

probs = Vector{SDEProblem}(undef,3)
p1 = Vector{Any}(undef,3)
p2 = Vector{Any}(undef,3)
p3 = Vector{Any}(undef,3)
## Problem 1
probs[1] = prob_sde_linear
## Problem 2
probs[2] = prob_sde_wave
## Problem 3
probs[3] = prob_sde_additive

println("Setup Complete")

## Timing Runs

function runAdaptive(i,k)
  sol = solve(prob,SRIW1(),dt=1/2^(8),abstol=2.0^(-15),reltol=2.0^(-10),
              verbose=false,maxIters=Int(1e12),qmax=qs[k])
  Int(any(isnan,sol[end]) || sol.t[end] != 1)
end

#Compile
monte_prob = MonteCarloProblem(probs[1])
test_mc = solve(monte_prob,SRIW1(),dt=1/2^(4),adaptive=true,num_monte=1000,abstol=2.0^(-1),reltol=0)
DiffEqBase.calculate_monte_errors(test_mc);

for k in eachindex(qs)
  global times
  Random.seed!(99)
  adaptiveTime = @elapsed numFails = sum(map((i)->runAdaptive(i,k),1:num_runs))
  println("k was $k. The number of Adaptive Fails is $numFails. Elapsed time was $adaptiveTime")
  times[k,4] = adaptiveTime
end

for k in eachindex(probs)
  global probs, times, means, qs
  println("Problem $k")
  ## Setup
  prob = probs[k]

  for i in eachindex(qs)
    msim = solve(monte_prob,dt=1/2^(4),SRIW1(),adaptive=true,num_monte=num_runs,abstol=2.0^(-13),reltol=0,qmax=qs[i])
    test_msim = DiffEqBase.calculate_monte_errors(msim)
    times[i,k] = test_msim.elapsedTime
    means[i,k] = test_msim.error_means[:final]
    println("for k=$k and i=$i, we get that the error was $(means[i,k]) and it took $(times[i,k]) seconds")
  end
end

using DiffEqBenchmarks
DiffEqBenchmarks.bench_footer(WEAVE_ARGS[:folder],WEAVE_ARGS[:file])


using Images, Random
import Base.Threads: @threads, @spawn

function escapetime(z; maxiter=80)
    c = z
    for n = 1:maxiter
        if abs(z) > 2
            return n-1
        end
        z = z^2 + c
    end
    return maxiter
end

function mandel(; width=80, height=20, maxiter=80)
    out = zeros(Int, height, width)
    real = range(-2.0, 0.5, length=width)
    imag = range(-1.0, 1.0, length=height)
    for x in 1:width
        for y in 1:height
            z = real[x] + imag[y]*im
            out[y,x] = escapetime(z, maxiter=maxiter)
        end
    end
    return out
end

@time m = mandel(width=1200,height=900,maxiter=400)
Gray.(m./80)

## CSV parsing

using CSV, BenchmarkTools

run(`python3 -m timeit -s "import pandas" -p "pandas.read_csv('mixed.csv')"`)

@btime CSV.read("mixed.csv", threaded=false)

@btime CSV.read("mixed.csv", threaded=true)


"""
        SegyHeaderInfo(filename_in)

Print segy header information to screen. The input is the name of the headers file

"""
function SegyHeaderInfo(h_segy)

#requires using Printf, Statistics

println("Calculating statistics.")

#key gets all the types of the composite type SegyHeader).
key = fieldnames(SeisMain.SegyHeader)

min_h = zeros(Float64,length(key))
max_h = zeros(Float64,length(key))
mean_h = zeros(Float64,length(key))

for ikey=1:length(key)
		min_h[ikey] = convert(Float64,getfield(h_segy[1],key[ikey]))
		max_h[ikey] = convert(Float64,getfield(h_segy[1],key[ikey]))
		mean_h[ikey] += convert(Float64,getfield(h_segy[1],key[ikey]))
end

nx = size(h_segy,1)

for itrace = 2:nx
	for ikey = 1 : length(key)
		aux = convert(Float64,getfield(h_segy[itrace],ikey))
		if (aux < min_h[ikey])
			min_h[ikey] = aux
		end
		if ( aux > max_h[ikey])
			max_h[ikey] = aux
		end
		mean_h[ikey] += aux
	end
end

for ikey=1:length(key)
	mean_h[ikey] /= nx
end

	println("       Key          Minimum          Maximum             Mean");
	println("=============================================================")
	for ikey=1:length(key)
		@printf("%10s      %11.3f      %11.3f      %11.3f\n",string(key[ikey]),min_h[ikey],max_h[ikey],mean_h[ikey])
	end

end