import Pkg

if dirname(Pkg.project().path) != @__DIR__
    Pkg.activate(@__DIR__)
end

using Test
using LennardJones
using LinearAlgebra

@testset "Test" begin

@test LennardJones.lj_potential_uij(LennardJones.R_MIN) ≈ -1.0

r₁ = [0.916914, 0.111325, 0.594877]
r₂ = [0.454572, 0.979098, 0.988925]
dr = r₂ - r₁
r = norm(dr)
ϵ = 0.8
σ = 0.9
dist = (a, b) -> a - b

u = LennardJones.lj_potential_uij(r, ϵ = ϵ, σ = σ)
@test u ≈ -0.7509440391688087
@test LennardJones.lj_potential_uij(dr, ϵ = ϵ, σ = σ) ≈ u
@test LennardJones.lj_potential_uij(r₁, r₂, ϵ = ϵ, σ = σ, dist = dist) ≈ u

f = LennardJones.lj_potential_fij(dr, ϵ = ϵ, σ = σ)
@test f ≈ [0.7369705345615358, -1.3832252568186918, -0.6281102846008022]
@test LennardJones.lj_potential_fij(r₁, r₂, ϵ = ϵ, σ = σ, dist = dist) ≈ -f

cutoff = 2.5
u₂ = LennardJones.lj_potential_uij_cutoff(dr, ϵ = ϵ, σ = σ, r_cutoff = cutoff)
@test u₂ ≈ -0.7439934985138913
@test LennardJones.lj_potential_uij_cutoff(r₁, r₂, ϵ = ϵ, σ = σ, r_cutoff = cutoff, dist = dist) ≈ u₂

f₂ = LennardJones.lj_potential_fij_cutoff(dr, ϵ = ϵ, σ = σ, r_cutoff = cutoff)
@test f₂ ≈ f
@test LennardJones.lj_potential_fij_cutoff(r₁, r₂, ϵ = ϵ, σ = σ, r_cutoff = cutoff, dist = dist) ≈ -f₂

r₃ = [0.669256, 0.300412, 0.671123]
dr₂ = r₃ - r₁
u₃ = LennardJones.lj_potential_wca_uij(dr₂, ϵ = ϵ, σ = σ)
@test u₃ ≈ 759679.8217248165
@test LennardJones.lj_potential_wca_uij(r₁, r₃, ϵ = ϵ, σ = σ, dist = dist) ≈ u₃

f₃ = LennardJones.lj_potential_wca_fij(dr₂, ϵ = ϵ, σ = σ)
@test f₃ ≈ [-2.1962740885915317e7, 1.6768563042159226e7, 6.761627492701628e6]
@test LennardJones.lj_potential_wca_fij(r₁, r₃, ϵ = ϵ, σ = σ, dist = dist) ≈ -f₃

end


import RestrictedBoltzmannMachines as RBMs
using Test: @test, @testset
using LinearAlgebra: Diagonal, logdet

@testset "linalg" begin
    A = randn(2,2) + Diagonal([10, 10])
    B = randn(2,2)
    C = randn(2,2)
    D = randn(2,2) + Diagonal([10, 10])
    @test RBMs.block_matrix_logdet(A, B, C, D) ≈ logdet([A B; C D])
    @test RBMs.block_matrix_invert(A, B, C, D) ≈ inv([A B; C D])

    A = randn(3,3) + Diagonal([10, 10, 10])
    B = randn(3,2)
    C = randn(2,3)
    D = randn(2,2) + Diagonal([10, 10])
    @test RBMs.block_matrix_logdet(A, B, C, D) ≈ logdet([A B; C D])
    @test RBMs.block_matrix_invert(A, B, C, D) ≈ inv([A B; C D])

    A = randn(2,2) + Diagonal([10, 10])
    B = randn(2,3)
    C = randn(3,2)
    D = randn(3,3) + Diagonal([10, 10, 10])
    @test RBMs.block_matrix_logdet(A, B, C, D) ≈ logdet([A B; C D])
    @test RBMs.block_matrix_invert(A, B, C, D) ≈ inv([A B; C D])
end


"""
    get_haplotype_transition_matrix(r, θ, α)

Compute transition matrices for the hidden Markov chains in haplotypes.
This is 2 equations above eq8 in "Gene hunting with hidden Markov model knockoffs" by Sesia et al.

# Inputs
`r`: Length `p` vector, the "recombination rates"
`θ`: Size `p × K` matrix, `θ[j, k]` is probability that the allele is 1 for SNP `p` at `k`th haplotype motif
`α`: Size `p × K` matrix, probabilities that haplotype motifs succeed each other. Rows should sum to 1.

# Output
`Q`: A `p`-dimensional vector of `K × K` matrices. `Q[:, :, j]` is the `j`th transition matrix.
"""
function get_haplotype_transition_matrix(
    r::AbstractVecOrMat, # p × 1
    θ::AbstractMatrix,   # p × K
    α::AbstractMatrix,   # p × K
    )
    K = size(θ, 2)
    p = size(r, 1)
    Q = Array{Float64, 3}(undef, K, K, p)
    @inbounds for j in 1:p
        Qj = @view(Q[:, :, j])
        for k in 1:K, knew in 1:K
            Qj[k, knew] = (1 - exp(-r[j])) * α[j, knew] # note: Pr(j|i) = Q_{i,j} (i.e. rows of Q must sum to 1)
            if k == knew
                Qj[k, knew] += exp(-r[j])
            end
        end
    end
    return Q # Rows of Q should sum to 1
end

"""
    get_genotype_transition_matrix(r, θ, α, q, table)

Compute transition matrices for the hidden Markov chains in unphased genotypes.
This is equation 9 of "Gene hunting with hidden Markov model knockoffs" by Sesia et al.

# Inputs
`r`: Length `p` vector, the "recombination rates"
`θ`: Size `p × K` matrix, `θ[j, k]` is probability that the allele is 1 for SNP `p` at `k`th haplotype motif
`α`: Size `p × K` matrix, probabilities that haplotype motifs succeed each other. Rows should sum to 1.
`q`: Length `K` vector of initial probabilities
`table`: a `MarkovChainTable` that maps markov chain states k = 1, ..., K+(K+1)/2
    to haplotype pairs (ka, kb).
"""
function get_genotype_transition_matrix(
    r::AbstractVecOrMat, # p × 1
    θ::AbstractMatrix,   # p × K
    α::AbstractMatrix,   # p × K
    q::AbstractVector,   # p × 1
    table::MarkovChainTable
    )
    # first compute haplotype transition matrix
    H = get_haplotype_transition_matrix(r, θ, α)
    K = size(H, 2)
    p = size(H, 3)
    statespace = (K * (K + 1)) >> 1

    # now compute genotype transition matrix
    Q = Array{Float64, 3}(undef, statespace, statespace, p)
    @inbounds for l in 1:statespace
        Q[l, :, 1] .= q
    end
    # for l in 1:statespace
    #     Q[:, :, 1] .= NaN # Q1 should never be used anywhere, so we fill it with NaN
    # end
    @inbounds for j in 2:p
        Qj, Hj = @view(Q[:, :, j]), @view(H[:, :, j])
        for (row, geno) in enumerate(table)
            for (col, geno_new) in enumerate(table)
                Qj[row, col] = Hj[geno.a, geno_new.a] * Hj[geno.b, geno_new.b] # note: Pr(j|i) = Q_{i,j} (i.e. rows of Q must sum to 1)
                if geno_new.a != geno_new.b
                    Qj[row, col] += Hj[geno.a, geno_new.b] * Hj[geno.b, geno_new.a]
                end
            end
        end
    end
    return Q #Rows of Q should sum to 1
end
# helper functions from HMMBase.jl
# istransmat(A::AbstractMatrix) =
#     issquare(A) && all([isprobvec(A[i, :]) for i = 1:size(A, 1)])
# @assert all(i -> istransmat(@view(Q[:, :, i])), 1:size(Q, 3))

function get_initial_probabilities(α::AbstractMatrix, table::MarkovChainTable)
    K = size(α, 2)
    statespace = (K * (K + 1)) >> 1
    q = zeros(statespace)
    α1 = α[1, :]
    @inbounds for (i, geno) in enumerate(table)
        ka, kb = geno.a, geno.b
        q[i] = (ka == kb ? abs2(α1[ka]) : 2 * α1[ka] * α1[kb])
    end
    @assert sum(q) ≈ 1 "initial probability should sum to 1!"
    return q
end

"""
    get_haplotype_emission_probabilities(θ::AbstractMatrix, j::Int, hj::Number, zj::Int)

Computes emission probabilities for unphased HMM. This is the equation above eq8 of
"Gene hunting with hidden Markov model knockoffs" by Sesia et al.
"""
function get_haplotype_emission_probabilities(θ::AbstractMatrix, j::Int, hj::Number, zj::Int)
    if hj == 0
        return 1 - θ[j, zj]
    elseif hj == 1
        return θ[j, zj]
    else
        error("hj should be 0 or 1 but was $hj")
    end
end

"""
    get_genotype_emission_probabilities(θ::AbstractMatrix, xj::Number, ka::Int, kb::Int, j::Int)

Computes P(xj | k={ka,kb}, θ): emission probabilities for genotypes. This is eq 10 of
"Gene hunting with hidden Markov model knockoffs" by Sesia et al.
"""
function get_genotype_emission_probabilities(θ::AbstractMatrix, xj::Number, ka::Int, kb::Int, j::Int)
    if xj == 0
        return (1 - θ[j, ka]) * (1 - θ[j, kb])
    elseif xj == 1
        return θ[j, ka] * (1 - θ[j, kb]) + θ[j, kb] * (1 - θ[j, ka])
    elseif xj == 2
        return θ[j, ka] * θ[j, kb]
    else
        error("xj should be 0, 1, or 2 but was $xj")
    end
end
# for j in 1:p, (k, (ka, kb)) in enumerate(with_replacement_combinations(1:K, 2))
#     p0 = get_genotype_emission_probabilities(θ, 0, ka, kb, j)
#     p1 = get_genotype_emission_probabilities(θ, 1, ka, kb, j)
#     p2 = get_genotype_emission_probabilities(θ, 2, ka, kb, j)
#     p0 + p1 + p2 ≈ 1 || error("shouldn't happen")
# end

"""
    form_emission_prob_matrix(a, θ, xi::AbstractVector)

# Inputs
+ `a`: `p × K` matrix with values estimated from fastPHASE (i.e. they called it the α parameter)
+ `θ`: `p × K` matrix with values estimated from fastPHASE
+ `xi`: Length `p` vector with sample `i`'s genotypes (entries 0, 1 or 2)
"""
function form_emission_prob_matrix(a, θ, xi::AbstractVector, table::MarkovChainTable)
    p, K = size(a)
    statespace = (K * (K + 1)) >> 1
    f = zeros(p, statespace)
    @inbounds for j in 1:p, (k, geno) in enumerate(table)
        f[j, k] = get_genotype_emission_probabilities(θ, xi[j], geno.a, geno.b, j)
    end
    return f
end

"""
    forward_backward_sampling!(Z, X, Q, q, θ, ...)

Samples Z, the hidden states of a HMM, from observed sequence of unphased genotypes X.

# Inputs
`Z`: Length `p` vector of integers. This will store the sampled Markov states
`X`: Length `p` vector of genotypes (0, 1, or 2)
`Q`: `K × K × p` array. `Q[:, :, j]` is a `K × K` matrix of transition
    probabilities for `j`th state, i.e. Q[l, k, j] = P(X_{j} = k | X_{j - 1} = l).
    The first transition matrix is not used.
`q`: Length `K` vector of initial probabilities
`θ`: The θ parameter estimated from fastPHASE

# Preallocated storage variables
`table`: a `MarkovChainTable` that maps markov chain states to haplotype
    pairs (ka, kb)
`d`: Sampling distribution, probabilities in d.p are mutated
`α̂`: `p × K` scaled forward probability matrix, where
    `α̂[j, k] = P(x_1,...,x_k, z_k) / P(x_1,...,x_k)`
`c`: normalizing constants, `c[k] = p(x_k | x_1,...,x_{k-1})`

# Reference
Algorithm 3 of "Gene hunting with hidden Markov model knockoffs" by Sesia et al
"""
function forward_backward_sampling!(
    Z::Vector{Int},
    X::Vector,
    Q::Array{T, 3},
    q::Vector{T},
    θ::AbstractMatrix,
    table::MarkovChainTable,
    d::Categorical,
    α̂::AbstractMatrix,
    c::AbstractVector,
    ) where T
    statespace, p = size(Q, 2), size(Q, 3)
    length(X) == p || error("length(X) not equal to p")
    fill!(c, 0)

    # (scaled) forward probabilities
    @inbounds for (k, geno) in enumerate(table)
        α̂[1, k] = q[k] * get_genotype_emission_probabilities(θ, X[1], geno.a, geno.b, 1)
        c[1] += α̂[1, k]
    end
    α̂[1, :] ./= c[1]
    @inbounds for j in 2:p
        mul!(@view(α̂[j, :]), Transpose(@view(Q[:, :, j])), @view(α̂[j - 1, :])) # note: Pr(j|i) = Q_{i,j} (i.e. rows of Q must sum to 1)
        for (k, geno) in enumerate(table)
            α̂[j, k] *= get_genotype_emission_probabilities(θ, X[j], geno.a, geno.b, j)
            c[j] += α̂[j, k]
        end
        α̂[j, :] ./= c[j]
    end

    # backwards sampling
    denom = sum(@view(α̂[p, :]))
    @inbounds for k in 1:statespace
        d.p[k] = α̂[p, k] / denom
    end
    Z[end] = rand(d)
    @inbounds for j in Iterators.reverse(1:p-1)
        denom = 0.0
        for k in 1:statespace
            denom += Q[k, Z[j + 1], j + 1] * α̂[j, k]
        end
        for zj in 1:statespace
            d.p[zj] = Q[zj, Z[j + 1], j + 1] * α̂[j, zj] / denom
        end
        @assert sum(d.p) ≈ 1 "forward_backward_sampling!: probability should sum to 1 but was $(sum(d.p))"
        Z[j] = rand(d)
    end

    return Z
end
# todo: how to test correctness?

function forward_backward_sampling(
    X::Vector,
    Q::Array{T, 3},
    q::Vector{T},
    θ::AbstractMatrix
    ) where T
    p = length(X)
    K = size(Q, 1)
    Z = zeros(Int, p)
    table = MarkovChainTable(K)
    statespace = statespace(table)
    d = Categorical([1 / statespace for _ in 1:statespace])
    α̂ = zeros(p, statespace) # scaled α, where α̂[j, k] = P(x_1,...,x_k, z_k) / P(x_1,...,x_k)
    c = zeros(p) # normalizing constants, c[k] = p(x_k | x_1,...,x_{k-1})
    forward_backward_sampling!(Z, X, Q, q, θ, table, d, α̂, c)
end

"""
    hmm_knockoff(plinkname, fastphase_outfile, T=10, datadir=pwd())

Generates HMM knockoffs from binary PLINK formatted files. This is done by
first running fastPHASE, then running Algorithm 2 of "Gene hunting with hidden
Markov model knockoffs" by Sesia, Sabatti, and Candes

# Input
+ `plinkname`: Binary PLINK file names without the `.bed/.bim/.fam` suffix.

# Optional arguments
+ `T`: Number of initial starts used in fastPHASE EM algorithm (default = number
    of parallel julia threads available as measured by Threads.nthreads())
+ `datadir`: Full path to the PLINK and fastPHASE files (default = current directory)
+ `plink_outfile`: Output PLINK format name
+ `fastphase_outfile`: The output file name from fastPHASE's alpha, theta, r files
+ `args...`: Any parameter specified in `fastphase`

# Output
+ `plink_outfile.bed`: `n × p` knockoff genotypes
+ `plink_outfile.bim`: SNP mapping file. Knockoff have SNP names ending in ".k"
+ `plink_outfile.fam`: Sample mapping file, this is a copy of the original `plinkname.fam` file
+ `fastphase_outfile_rhat.txt`: averaged r hat file from fastPHASE
+ `fastphase_outfile_alphahat.txt`: averaged alpha hat file from fastPHASE
+ `fastphase_outfile_thetahat.txt`: averaged theta hat file from fastPHASE
"""
function hmm_knockoff(
    plinkname::AbstractString;
    T::Int = Threads.nthreads(),
    datadir::AbstractString = pwd(),
    plink_outfile::AbstractString = "knockoff",
    fastphase_outfile::AbstractString = "fastphase_out",
    outdir::AbstractString = datadir,
    args...
    )
    snpdata = SnpData(joinpath(datadir, plinkname))
    r, θ, α = fastphase_estim_param(snpdata; out=fastphase_outfile, args...)
    return hmm_knockoff(snpdata, r, θ, α, outdir=outdir, plink_outfile=plink_outfile)
end

"""
    hmm_knockoff(snpdata, r, θ, α)

Generates knockoff of `snpdata` with loaded r, θ, α
"""
function hmm_knockoff(
    snpdata::SnpData,
    r::AbstractVecOrMat,
    θ::AbstractMatrix,
    α::AbstractMatrix;
    outdir = pwd(),
    plink_outfile::AbstractString = "knockoff",
    )
    Xfull = snpdata.snparray
    n, p = size(Xfull)
    K = size(θ, 2)
    statespace = (K * (K + 1)) >> 1
    table = MarkovChainTable(K)

    # get initial states (marginal distribution vector) and Markov transition matrices
    q = get_initial_probabilities(α, table)
    Q = get_genotype_transition_matrix(r, θ, α, q, table)

    # preallocated arrays
    X̃full = SnpArray(joinpath(outdir, plink_outfile * ".bed"), n, p)
    X = zeros(Float64, p)
    Z = zeros(Int, p)
    Z̃ = zeros(Int, p)
    X̃ = zeros(Int, p)
    N = zeros(p, statespace)
    d_K = Categorical([1 / statespace for _ in 1:statespace]) # for sampling markov chains (length statespace)
    d_3 = Categorical([1 / statespace for _ in 1:statespace]) # for sampling genotypes (length 3)
    α̂ = zeros(p, statespace) # scaled α, where α̂[j, k] = P(x_1,...,x_k, z_k) / P(x_1,...,x_k)
    c = zeros(p) # normalizing constants, c[k] = p(x_k | x_1,...,x_{k-1})

    @showprogress for i in 1:n
        # sample hidden states (algorithm 3 in Sesia et al)
        copyto!(X, @view(Xfull[i, :]))
        forward_backward_sampling!(Z, X, Q, q, θ, table, d_K, α̂, c)

        # sample knockoff of markov chain (algorithm 2 in Sesia et al)
        markov_knockoffs!(Z̃, Z, N, d_K, Q, q)

        # sample knockoffs of genotypes (eq 6 in Sesia et al)
        sample_markov_chain!(X̃, Z̃, table, θ, d_3)

        # save knockoff
        write_plink!(X̃full, X̃, i)
    end

    # copy .bim and .fam files
    new_bim = copy(snpdata.snp_info)
    for i in 1:p
        new_bim[i, :snpid] = new_bim[i, :snpid] * ".k"
    end
    CSV.write(joinpath(outdir, plink_outfile * ".bim"), new_bim, delim='\t', header=false)
    cp(snpdata.srcfam, joinpath(outdir, plink_outfile * ".fam"), force=true)

    return X̃full
end

function hmm_knockoff(
    plinkname::AbstractString,
    fastphase_outfile::AbstractString;
    T::Int = Threads.nthreads(),
    datadir::AbstractString = pwd(),
    outfile::AbstractString = "knockoff",
    outdir::AbstractString = datadir
    )
    snpdata = SnpData(joinpath(datadir, plinkname))
    Xfull = snpdata.snparray
    n, p = size(Xfull)

    # get r, α, θ estimated by fastPHASE
    r, θ, α = process_fastphase_output(datadir, T=T, extension=fastphase_outfile)
    K = size(θ, 2)
    statespace = (K * (K + 1)) >> 1
    table = MarkovChainTable(K)

    # get initial states (marginal distribution vector) and Markov transition matrices
    q = get_initial_probabilities(α, table)
    Q = get_genotype_transition_matrix(r, θ, α, q, table)

    # preallocated arrays
    X̃full = SnpArray(joinpath(outdir, outfile * ".bed"), n, p)
    X = zeros(Float64, p)
    Z = zeros(Int, p)
    Z̃ = zeros(Int, p)
    X̃ = zeros(Int, p)
    N = zeros(p, statespace)
    d_K = Categorical([1 / statespace for _ in 1:statespace]) # for sampling markov chains (length statespace)
    d_3 = Categorical([1 / statespace for _ in 1:statespace]) # for sampling genotypes (length 3)
    α̂ = zeros(p, statespace) # scaled α, where α̂[j, k] = P(x_1,...,x_k, z_k) / P(x_1,...,x_k)
    c = zeros(p) # normalizing constants, c[k] = p(x_k | x_1,...,x_{k-1})

    @showprogress for i in 1:n
        # sample hidden states (algorithm 3 in Sesia et al)
        copyto!(X, @view(Xfull[i, :]))
        forward_backward_sampling!(Z, X, Q, q, θ, table, d_K, α̂, c)

        # sample knockoff of markov chain (algorithm 2 in Sesia et al)
        markov_knockoffs!(Z̃, Z, N, d_K, Q, q)

        # sample knockoffs of genotypes (eq 6 in Sesia et al)
        sample_markov_chain!(X̃, Z̃, table, θ, d_3)

        # save knockoff
        write_plink!(X̃full, X̃, i)
    end

    # copy .bim and .fam files
    new_bim = copy(snpdata.snp_info)
    for i in 1:p
        new_bim[i, :snpid] = new_bim[i, :snpid] * ".k"
    end
    CSV.write(joinpath(outdir, outfile * ".bim"), new_bim, delim='\t', header=false)
    cp(joinpath(datadir, plinkname * ".fam"), joinpath(outdir, outfile * ".fam"), force=true)

    return X̃full
end

function genotype_knockoffs(
    Z̃::AbstractVector,
    table::MarkovChainTable,
    θ::AbstractMatrix
    )
    p = length(Z̃)
    X̃ = zeros(eltype(Z̃), p)
    d = Categorical([1/3 for _ in 1:3])
    return sample_markov_chain!(X̃, Z̃, table, θ, d)
end

function sample_markov_chain!(
    X̃::AbstractVector,
    Z̃::AbstractVector,
    table::MarkovChainTable,
    θ::AbstractMatrix,
    d::Categorical # Categorical distribution from Distributions.jl
    )
    p = length(Z̃)
    @inbounds for j in 1:p
        a, b = index_to_pair(table, Z̃[j])
        d.p[1] = get_genotype_emission_probabilities(θ, 0, a, b, j)
        d.p[2] = get_genotype_emission_probabilities(θ, 1, a, b, j)
        d.p[3] = get_genotype_emission_probabilities(θ, 2, a, b, j)
        X̃[j] = rand(d)
    end
    X̃ .-= 1 # sampling d returns states 1~3, but genotypes are 0~2
    return X̃
end

function write_plink!(
    X̃full::SnpArray,
    X̃::AbstractVector,
    i::Int # sample index
    )
    p = length(X̃)
    @inbounds for j in 1:p
        if X̃[j] == 0
            X̃full[i, j] = 0x00
        elseif X̃[j] == 1
            X̃full[i, j] = 0x02
        elseif X̃[j] == 2
            X̃full[i, j] = 0x03
        else
            error("Genotypes should only be 0, 1, or 2 but got $(X̃[j])")
        end
    end
    return X̃full
end


#
# The types and constructors for derivative operators.
#
# The type of the operator objects is DerivativeOperator. This is a
# dumb data structure. It is initialized by the constructors for
# CenteredDifference and UpwindDifference, which call the calculate_weights
# routine in fornberg.jl to generate the stencils. The derivatives
# are computed by the mul! methods defined in
# derivative_operator_functions.jl.
#

index(i::Int, N::Int) = i + div(N, 2) + 1

struct DerivativeOperator{T<:Real,N,Wind,T2,S1,S2,S3<:SArray,T3,F} <: AbstractDerivativeOperator{T}
    derivative_order        :: Int
    approximation_order     :: Int
    dx                      :: T2
    len                     :: Int
    stencil_length          :: Int
    stencil_coefs           :: S1
    boundary_stencil_length :: Int
    boundary_point_count    :: Int
    low_boundary_coefs      :: S2
    high_boundary_coefs     :: S3
    offside                 :: Int
    coefficients            :: T3
    coeff_func              :: F
end

struct nonlinear_diffusion!{N} end
struct nonlinear_diffusion{N} end

function nonlinear_diffusion!{N}(du::AbstractVector{T}, second_differential_order::Int, first_differential_order::Int, approx_order::Int,
    p::AbstractVector{T}, q::AbstractVector{T}, dx::Union{T , AbstractVector{T} , Real},
    nknots::Int) where {T<:Real, N}
    #q is given by bc*u , u being the unknown function
    #p is given as some function of q , p being the diffusion coefficient

    @assert approx_order>1 "approximation_order must be greater than 1."
    if first_differential_order > 0
    du .= (CenteredDifference{N}(first_differential_order,approx_order,dx,nknots)*q).*(CenteredDifference{N}(second_differential_order,approx_order,dx,nknots)*p)
    else
    du .= q[2:(nknots + 1)].*(CenteredDifference{N}(second_differential_order,approx_order,dx,nknots)*p)
    end

    for l = 1:(second_differential_order - 1)
    du .= du .+ binomial(second_differential_order,l)*(CenteredDifference{N}(l + first_differential_order,approx_order,dx,nknots)*q).*(CenteredDifference{N}(second_differential_order - l,approx_order,dx,nknots)*p)
    end

    du .= du .+ (CenteredDifference{N}(first_differential_order + second_differential_order,approx_order,dx,nknots)*q).*p[2:(nknots + 1)]

end

# An out of place workaround for the mutating version
function nonlinear_diffusion{N}(second_differential_order::Int, first_differential_order::Int, approx_order::Int,
    p::AbstractVector{T}, q::AbstractVector{T}, dx::Union{T , AbstractVector{T} , Real},
    nknots::Int) where {T<:Real, N}

    du = similar(q,length(q) - 2)
    return nonlinear_diffusion!{N}(du,second_differential_order,first_differential_order,approx_order,p,q,dx,nknots)
end

struct CenteredDifference{N} end

function CenteredDifference{N}(derivative_order::Int,
                            approximation_order::Int, dx::T,
                            len::Int, coeff_func=nothing) where {T<:Real,N}
    @assert approximation_order>1 "approximation_order must be greater than 1."
    stencil_length          = derivative_order + approximation_order - 1 + (derivative_order+approximation_order)%2
    boundary_stencil_length = derivative_order + approximation_order
    dummy_x                 = -div(stencil_length,2) : div(stencil_length,2)
    left_boundary_x         = 0:(boundary_stencil_length-1)
    right_boundary_x        = reverse(-boundary_stencil_length+1:0)

    boundary_point_count    = div(stencil_length,2) - 1 # -1 due to the ghost point
    # Because it's a N x (N+2) operator, the last stencil on the sides are the [b,0,x,x,x,x] stencils, not the [0,x,x,x,x,x] stencils, since we're never solving for the derivative at the boundary point.
    deriv_spots             = (-div(stencil_length,2)+1) : -1  # unused
    L_boundary_deriv_spots  = left_boundary_x[2:div(stencil_length,2)]
    R_boundary_deriv_spots  = right_boundary_x[2:div(stencil_length,2)]

    stencil_coefs           = convert(SVector{stencil_length, T}, (1/dx^derivative_order) * calculate_weights(derivative_order, zero(T), dummy_x))
    _low_boundary_coefs     = SVector{boundary_stencil_length, T}[convert(SVector{boundary_stencil_length, T}, (1/dx^derivative_order) * calculate_weights(derivative_order, oneunit(T)*x0, left_boundary_x)) for x0 in L_boundary_deriv_spots]
    low_boundary_coefs      = convert(SVector{boundary_point_count},_low_boundary_coefs)

    # _high_boundary_coefs    = SVector{boundary_stencil_length, T}[convert(SVector{boundary_stencil_length, T}, (1/dx^derivative_order) * calculate_weights(derivative_order, oneunit(T)*x0, reverse(right_boundary_x))) for x0 in R_boundary_deriv_spots]
    # high_boundary_coefs      = convert(SVector{boundary_point_count},reverse(_high_boundary_coefs))

    high_boundary_coefs      = convert(SVector{boundary_point_count},reverse(map(reverse, _low_boundary_coefs*(-1)^derivative_order)))

    offside = 0

    coefficients            = coeff_func isa Nothing ? nothing : fill!(Vector{T}(undef,len),0)

    DerivativeOperator{T,N,false,T,typeof(stencil_coefs),
        typeof(low_boundary_coefs),typeof(high_boundary_coefs),typeof(coefficients),
        typeof(coeff_func)}(
        derivative_order, approximation_order, dx, len, stencil_length,
        stencil_coefs,
        boundary_stencil_length,
        boundary_point_count,
        low_boundary_coefs,
        high_boundary_coefs,offside,coefficients,coeff_func
        )
end

function generate_coordinates(i::Int, stencil_x, dummy_x, dx::AbstractVector{T}) where T <: Real
    len = length(stencil_x)
    stencil_x .= stencil_x.*zero(T)
    for idx in 1:div(len,2)
        shifted_idx1 = index(idx, len)
        shifted_idx2 = index(-idx, len)
        stencil_x[shifted_idx1] = stencil_x[shifted_idx1-1] + dx[i+idx-1]
        stencil_x[shifted_idx2] = stencil_x[shifted_idx2+1] - dx[i-idx]
    end
    return stencil_x
end

function CenteredDifference{N}(derivative_order::Int,
                            approximation_order::Int, dx::AbstractVector{T},
                            len::Int, coeff_func=nothing) where {T<:Real,N}

    stencil_length          = derivative_order + approximation_order - 1 + (derivative_order+approximation_order)%2
    boundary_stencil_length = derivative_order + approximation_order
    stencil_x               = zeros(T, stencil_length)
    boundary_point_count    = div(stencil_length,2) - 1# -1 due to the ghost point

    interior_x              = boundary_point_count+2:len+1-boundary_point_count
    dummy_x                 = -div(stencil_length,2) : div(stencil_length,2)-1
    low_boundary_x          = [zero(T); cumsum(dx[1:boundary_stencil_length-1])]
    high_boundary_x         = cumsum(dx[end-boundary_stencil_length+1:end])
    # Because it's a N x (N+2) operator, the last stencil on the sides are the [b,0,x,x,x,x] stencils, not the [0,x,x,x,x,x] stencils, since we're never solving for the derivative at the boundary point.
    deriv_spots             = (-div(stencil_length,2)+1) : -1

    stencil_coefs           = [convert(SVector{stencil_length, T}, calculate_weights(derivative_order, zero(T), generate_coordinates(i, stencil_x, dummy_x, dx))) for i in interior_x]
    _low_boundary_coefs     = SVector{boundary_stencil_length, T}[convert(SVector{boundary_stencil_length, T},
                                                                  calculate_weights(derivative_order, low_boundary_x[i+1], low_boundary_x)) for i in 1:boundary_point_count]
    low_boundary_coefs      = convert(SVector{boundary_point_count},_low_boundary_coefs)
    _high_boundary_coefs     = SVector{boundary_stencil_length, T}[convert(SVector{boundary_stencil_length, T},
                                                                  calculate_weights(derivative_order, high_boundary_x[end-i], high_boundary_x)) for i in boundary_point_count:-1:1]
    high_boundary_coefs      = convert(SVector{boundary_point_count},_high_boundary_coefs)

    offside = 0

    coefficients            = coeff_func isa Nothing ? nothing : zeros(T,len)

    DerivativeOperator{T,N,false,typeof(dx),typeof(stencil_coefs),
        typeof(low_boundary_coefs),typeof(high_boundary_coefs),typeof(coefficients),
        typeof(coeff_func)}(
        derivative_order, approximation_order, dx,
        len, stencil_length,
        stencil_coefs,
        boundary_stencil_length,
        boundary_point_count,
        low_boundary_coefs,
        high_boundary_coefs,offside,coefficients,coeff_func
        )
end

struct UpwindDifference{N} end

"""
```
UpwindDifference{N}(derivative_order, approximation_order, dx, len, coeff_func = nothing)
```
constructs a DerivativeOperator that automatically implements upwinding.

### Inputs
* `dx::T` or `dx::Vector{T}`: grid spacing
* `coeff_func`: function mapping index in the grid to coefficient at that grid location

### Examples
julia> drift = [1., 1., -1.]
julia> L1 = UpwindDifference(1, 1, 1., 3, i -> drift[i])
julia> L2 = UpwindDifference(1, 1, 1., 3, i -> 1.)
julia> Q = Neumann0BC(1, 1.)
julia> Array(L1 * Q)[1]
3×3 Array{Float64,2}:
 -1.0   1.0   0.0
  0.0  -1.0   1.0
  0.0   1.0  -1.0
julia> Array(L2 * Q)[1]
3×3 Array{Float64,2}:
 -1.0   1.0  0.0
  0.0  -1.0  1.0
  0.0   0.0  0.0

"""
function UpwindDifference{N}(derivative_order::Int,
                             approximation_order::Int, dx::T,
                             len::Int, coeff_func=nothing; offside::Int=0) where {T<:Real,N}

    @assert offside > -1 "Number of offside points should be non-negative"
    @assert offside <= div(derivative_order + approximation_order - 1,2) "Number of offside points should not exceed the primary wind points"

    stencil_length          = derivative_order + approximation_order
    boundary_stencil_length = derivative_order + approximation_order
    boundary_point_count    = boundary_stencil_length - 2 - offside

    # TODO: Clean up the implementation here so that it is more readable and easier to extend in the future
    dummy_x = (0.0 - offside) : stencil_length - 1.0 - offside
    stencil_coefs = convert(SVector{stencil_length, T}, (1/dx^derivative_order) * calculate_weights(derivative_order, 0.0, dummy_x))

    low_boundary_x         = 0.0:(boundary_stencil_length-1)
    L_boundary_deriv_spots = 1.0:boundary_stencil_length - 2.0 - offside
    _low_boundary_coefs     = SVector{boundary_stencil_length, T}[convert(SVector{boundary_stencil_length, T}, (1/dx^derivative_order) * calculate_weights(derivative_order, oneunit(T)*x0, low_boundary_x)) for x0 in L_boundary_deriv_spots]
    low_boundary_coefs      = convert(SVector{boundary_point_count},_low_boundary_coefs)

    high_boundary_x         = 0.0:-1.0:-(boundary_stencil_length-1.0)
    R_boundary_deriv_spots = -1.0:-1.0:-(boundary_stencil_length-2.0)
    _high_boundary_coefs     = SVector{boundary_stencil_length, T}[convert(SVector{boundary_stencil_length, T}, ((-1/dx)^derivative_order) * calculate_weights(derivative_order, oneunit(T)*x0, high_boundary_x)) for x0 in R_boundary_deriv_spots]
    high_boundary_coefs = convert(SVector{boundary_point_count + offside},_high_boundary_coefs)

    coefficients = zeros(T,len)
    if coeff_func != nothing
        compute_coeffs!(coeff_func, coefficients)
    end

    DerivativeOperator{T,N,true,T,typeof(stencil_coefs),
        typeof(low_boundary_coefs),typeof(high_boundary_coefs),Vector{T},
        typeof(coeff_func)}(
        derivative_order, approximation_order, dx, len, stencil_length,
        stencil_coefs,
        boundary_stencil_length,
        boundary_point_count,
        low_boundary_coefs,
        high_boundary_coefs,offside,coefficients,coeff_func
        )
end

# TODO implement the non-uniform grid
function UpwindDifference{N}(derivative_order::Int,
                          approximation_order::Int, dx::AbstractVector{T},
                          len::Int, coeff_func=nothing; offside::Int=0) where {T<:Real,N}

    @assert offside > -1 "Number of offside points should be non-negative"
    @assert offside <= div(derivative_order + approximation_order - 1,2) "Number of offside points should not exceed the primary wind points"

    stencil_length          = derivative_order + approximation_order
    boundary_stencil_length = derivative_order + approximation_order
    boundary_point_count    = boundary_stencil_length - 2 - offside

    # Compute Stencils
    # Compute grid from dx
    x = [0.0, cumsum(dx)...]

    # compute low_boundary_coefs: low_boundary_coefs[upwind = 1 downwind = 2, index of point]
    if offside == 0
        _upwind_coefs = SMatrix{1,boundary_point_count}([convert(SVector{boundary_stencil_length,T}, calculate_weights(derivative_order, x[i+1], x[i+1:i+boundary_stencil_length])) for i in 1:boundary_point_count])
    else
        _upwind_coefs = SMatrix{1,boundary_point_count}(append!([convert(SVector{boundary_stencil_length, T}, calculate_weights(derivative_order, x[i+1], x[1:boundary_stencil_length])) for i in 1:offside-1],[convert(SVector{boundary_stencil_length, T}, calculate_weights(derivative_order, x[i+1], x[i+1-offside:i+boundary_stencil_length-offside])) for i in offside:boundary_point_count]))
    end
    _downwind_coefs = SMatrix{1,boundary_point_count}([convert(SVector{boundary_stencil_length,T}, calculate_weights(derivative_order, x[i+1], x[1:boundary_stencil_length])) for i in 1:boundary_point_count])
    low_boundary_coefs = [_upwind_coefs ; _downwind_coefs]

    # compute stencil_coefs: low_boundary_coefs[upwind = 1 downwind = 2, index of point]
    _upwind_coefs = SMatrix{1,len - 2*boundary_point_count}([convert(SVector{stencil_length, T}, calculate_weights(derivative_order, x[i+1], x[i+1-offside:i+stencil_length-offside])) for i in boundary_point_count+1:len-boundary_point_count])
    _downwind_coefs = SMatrix{1,len - 2*boundary_point_count}([convert(SVector{stencil_length,T}, calculate_weights(derivative_order, x[i+1], x[i-stencil_length+2+offside:i+1+offside])) for i in boundary_point_count+1:len-boundary_point_count])
    stencil_coefs = [_upwind_coefs ; _downwind_coefs]

    # compute high_boundary_coefs: low_boundary_coefs[upwind = 1 downwind = 2, index of point]
    _upwind_coefs = SMatrix{1,boundary_point_count + offside}([convert(SVector{boundary_stencil_length, T}, calculate_weights(derivative_order, x[i+1], x[len-boundary_stencil_length+3:len+2])) for i in len-boundary_point_count+1-offside:len])
    if offside == 0
        _downwind_coefs = SMatrix{1,boundary_point_count}([convert(SVector{boundary_stencil_length,T}, calculate_weights(derivative_order, x[i+1], x[i-stencil_length+2:i+1])) for i in len-boundary_point_count+1:len])
    elseif offside == 1
        _downwind_coefs = SMatrix{1,boundary_point_count + offside}([convert(SVector{boundary_stencil_length,T}, calculate_weights(derivative_order, x[i+1], x[i-stencil_length+2+offside:i+1+offside])) for i in len-boundary_point_count+1-offside:len-offside+1])
    else
        _downwind_coefs = SMatrix{1,boundary_point_count + offside}(append!([convert(SVector{boundary_stencil_length,T}, calculate_weights(derivative_order, x[i+1], x[i-stencil_length+2+offside:i+1+offside])) for i in len-boundary_point_count+1-offside:len-offside+1],[convert(SVector{boundary_stencil_length,T}, calculate_weights(derivative_order, x[i+1], x[len-stencil_length+3:len+2])) for i in len-offside+2:len]))
    end
    high_boundary_coefs = [_upwind_coefs ; _downwind_coefs]

    # Compute coefficients
    coefficients = zeros(T,len)
    if coeff_func != nothing
        compute_coeffs!(coeff_func, coefficients)
    end

    DerivativeOperator{T,N,true,typeof(dx),typeof(stencil_coefs),
        typeof(low_boundary_coefs),typeof(high_boundary_coefs),Vector{T},
        typeof(coeff_func)}(
        derivative_order, approximation_order, dx, len, stencil_length,
        stencil_coefs,
        boundary_stencil_length,
        boundary_point_count,
        low_boundary_coefs,
        high_boundary_coefs,offside,coefficients,coeff_func
        )
end

CenteredDifference(args...) = CenteredDifference{1}(args...)
UpwindDifference(args...;kwargs...) = UpwindDifference{1}(args...;kwargs...)
nonlinear_diffusion(args...) = nonlinear_diffusion{1}(args...)
nonlinear_diffusion!(args...) = nonlinear_diffusion!{1}(args...)
use_winding(A::DerivativeOperator{T,N,Wind}) where {T,N,Wind} = Wind
diff_axis(A::DerivativeOperator{T,N}) where {T,N} = N
function ==(A1::DerivativeOperator, A2::DerivativeOperator)
    return all([eval(:($A1.$name == $A2.$name)) for name in fieldnames(DerivativeOperator)])
end
function Laplacian(aor::Int, dxyz::Union{NTuple{N, T}, NTuple{N,AbstractVector{T}}}, s::NTuple{N,I}, coeff_func=nothing) where {T,N,I<:Int}
    return sum(CenteredDifference{i}(2, aor, dxyz[i], s[i], coeff_func) for i in 1:N)
end


sess = Session(); init(sess)
@testset "pcl_impose_Dirichlet_boundary_conditions" begin
using Revise; using AdFem; using PyPlot; using SparseArrays
sess = Session(); init(sess)

    A = sprand(10,10,0.6)
    II, JJ, VV = findnz(A)
    nz = length(VV)
    indices = SparseTensor(A).o.indices

    IND = run(sess, indices).+1

    x = placeholder(rand(nz))
    B = RawSparseTensor(indices, x, 10, 10)
    rhs = rand(10)
    bdnode = [1;4;6]
    B1, rhs1 = impose_Dirichlet_boundary_conditions(B, rhs, bdnode, zeros(3))
    n = length(run(sess, B1.o.values))

    loss = sum(B1.o.values^2)
    g = gradients(loss, x)
    J = pcl_impose_Dirichlet_boundary_conditions(IND, bdnode, n)

    function test_f(x0)
        G = run(sess, g, x=>x0)
        H = pcl_linear_op(J, pcl_square_sum(n))
        return G, H
    end

    err1, err2 = test_hessian(test_f, rand(nz); showfig = true)
    @test all(err1.<err2)
end

@testset "pcl_compute_fem_laplace_matrix1" begin
    mmesh = Mesh(10,10,0.1)
    κ = placeholder(rand(get_ngauss(mmesh)))
    A = compute_fem_laplace_matrix1(κ, mmesh)
    loss = sum((A.o.values - rand(mmesh.elem_ndof^2 * get_ngauss(mmesh)))^2)
    g = gradients(loss, κ)

    function test_f(x0)
        G = run(sess, g, κ=>x0)
        J = pcl_compute_fem_laplace_matrix1(mmesh)
        H = pcl_linear_op(J, pcl_square_sum(mmesh.elem_ndof^2 * get_ngauss(mmesh)))
        return G, H
    end

    err1, err2 = test_hessian(test_f, rand(get_ngauss(mmesh)); showfig = true)
    @test all(err1.<err2)
end


using OVERTVerify
using LazySets
using Dates

controller_type = ARGS[1] # pass from command line, e.g. "small"
controller = "nnet_files/jmlr/single_pendulum_$(controller_type)_controller.nnet"
println("Controller: ", controller)
query = OvertQuery(
	SinglePendulum,    # problem
	controller,        # network file
	Id(),              # last layer activation layer Id()=linear, or ReLU()=relu
	"MIP",             # query solver, "MIP" or "ReluPlex"
	25,                # ntime
	0.1,               # dt
	-1,                # N_overt
	)

input_set = Hyperrectangle(low=[1., 0.], high=[1.2, 0.2])
concretization_intervals = Int.(ones(query.ntime))
t1 = Dates.time()
concrete_state_sets, symbolic_state_sets, concrete_meas_sets, symbolic_meas_sets = symbolic_reachability_with_concretization(query, input_set, concretization_intervals)
t2 = Dates.time()
dt = (t2-t1)
print("elapsed time= $(dt) seconds")

# we want to check the intersection with the avoid set: x_1 <= -.2167
avoid_set = [HalfSpace([1., 0.], -0.2167)] # 1*x_1 + 0*x_2 <= -.2167  --> x_1 <= -.2167

t1 = time()
safe, violations = check_avoid_set_intersection(symbolic_state_sets, input_set, avoid_set)
dt_check = time() - t1

using JLD2
JLD2.@save "src/examples/jmlr/data/single_pendulum_reachability_$(controller_type)_controller_data_1step.jld2" query input_set concretization_intervals concrete_state_sets concrete_meas_sets symbolic_state_sets symbolic_meas_sets dt controller avoid_set safe violations dt_check


using StructuralEstimation
# Set RNG seed
srand(124)

# State space
n = 500
o_max = 450
Xval = linspace(0, o_max, n)
p = [0.0937; 0.4475; 0.4459; 0.0127; 0.0002];

F1 = zeros(n, n)
offset = -1
for i = 1:n, j = 1:length(p)
    i+offset+j > n && continue
    F1[i,i+offset+j] = i+offset+j == n ? sum(p[j:end]) : p[j]
end

# We can handle sparse matrices
sparse_F = [sparse(F1), sparse(F1)[ones(Int64, n), :]]
F = [F1, F1[ones(Int64, n), :]]

S = State(Xval, F)
sparse_S = State(Xval, sparse_F)
bdisc = 0.9999
Z1 = [zeros(n) -0.001*Xval]
Z2 = [-ones(n) zeros(n)]

U = LinearUtility([Z1, Z2], bdisc, [11.;2.5])
V, iters = solve!(U,S)
T = 100
N = 100

D = simulate(U, S, 1, T, N)

#ESTIMATE TRANSITIONS

U.Θ*=0.
rust_nfxp = fit_nfxp(U, S, D)
rust_npl = fit_npl(U, S, D)

U = LinearUtility([Z1, Z2], 0.9999, [11., 3.]*0)
@time rust_nfxp = fit_nfxp(U, S, D; verbose = false);
U = LinearUtility([Z1, Z2], 0.9999, [11., 3.]*0)
@time rust_npl = fit_npl(U, S, D)


using Revise
using Statistics
using LaTeXStrings
using PyPlot
using LinearAlgebra
using Infiltrator
using Bem2d

"""
    plotgeometry(els, titlestring)

els structure geometry plotting for diagnostics
"""
function plotgeometry(els, titlestring)
    scale = 5e-2
    figure(figsize=(15,10))
    for i in 1:els.endidx
        plot([els.x1[i], els.x2[i]], [els.y1[i], els.y2[i]], "-g")
        quiver(els.xcenter[i], els.ycenter[i],
               els.xnormal[i], els.ynormal[i],
               width=1e-3, scale=1e2)
        text(els.xcenter[i] + scale * els.xnormal[i],
             els.ycenter[i] + scale * els.ynormal[i],
             string(i), fontsize=5)
    end
    title(titlestring)
    gca().set_aspect("equal")
end

"""
    discretized_arc(thetastart, thetaend, radius, n_pts)

Generate regularly spaced eleemnts along an curved arc.
"""
function discretized_arc(thetastart, thetaend, radius, n_pts)
    # Create geometry of discretized arc
    thetarange = collect(LinRange(thetastart, thetaend, n_pts + 1))
    x = @. radius * cos(thetarange)
    y = @. radius * sin(thetarange)
    x1 = x[1:1:end-1]
    x2 = x[2:1:end]
    y1 = y[1:1:end-1]
    y2 = y[2:1:end]
    return x1, y1, x2, y2
end

"""
    circle_subplot(nrows, ncols, plotidx, x, y, mat, npts, R, theta0, title_string)

Plot field (displacement, stress) within a circular disk and style
"""
function circle_subplot(nrows, ncols, plotidx, els, x, y, mat, npts, title_string)
    contour_levels = 100
    contour_color = "white"
    contour_linewidth = 0.5

    subplot(nrows, ncols, plotidx)
    contourf(reshape(x, npts, npts), reshape(y, npts, npts), reshape(mat, npts, npts), levels=contour_levels)
    cbar = colorbar(fraction=0.05, pad=0.05, extend = "both")
    contour(reshape(x, npts, npts), reshape(y, npts, npts), reshape(mat, npts, npts), levels=contour_levels, colors=contour_color, linewidths=contour_linewidth)
    xlabel("x (m)")
    ylabel("y (m)")
    title(title_string)
    plotelements(els)
    gca().set_aspect("equal")
end

"""
    discmaterial()

An attmept at the Crouch and Starfield level annulus solution
with varying material properties
"""
function discmaterial()
    close("all")
    DEBUG = false
    muI = 1e10
    nuI = 0.25
    muII = 0.5 * muI
    nuII = 0.25
    p = muI / 1e3 # CS example
    nels = 60
    a = 0.5
    b = 1.0
    startangle = 180
    endangle = -startangle

    # Define BEM geometry
    els = Elements(Int(1e5))
    x1, y1, x2, y2 = discretized_arc(deg2rad(startangle), deg2rad(endangle), a, nels)
    addelsez!(els, x1, y1, x2, y2, "aI")
    x1, y1, x2, y2 = discretized_arc(deg2rad(endangle), deg2rad(startangle), b, nels)
    addelsez!(els, x1, y1, x2, y2, "bI")
    # We want element K in bI to match exactly with element K in bII, so we
    # simply flip x1,y1 with x2,y2
    addelsez!(els, x2, y2, x1, y1, "bII")
    idx = getidxdict(els)
    plotgeometry(els, "BEM geometry")

    if DEBUG
        figure()
        for n in ["aI", "bI"]
            plot(els.x1[idx[n]], els.y1[idx[n]])
            quiver(els.x1[idx[n]], els.y1[idx[n]], els.xnormal[idx[n]], els.ynormal[idx[n]])
        end
        gca().set_aspect("equal")
        title("region I boundaries and normals")

        figure()
        for n in ["bII"]
            plot(els.x1[idx[n]], els.y1[idx[n]])
            quiver(els.x1[idx[n]], els.y1[idx[n]], els.xnormal[idx[n]], els.ynormal[idx[n]])
        end
        gca().set_aspect("equal")
        title("region II boundaries and normals")
    end

    # Apply normal tractions everywhere and convert from radial to Cartesian
    xtracaI = zeros(length(idx["aI"]))
    ytracaI = zeros(length(idx["aI"]))
    for i in 1:length(idx["aI"]) # Calcuate the x and y components of the tractions
        normalTractions = [0; p] # Pressure in fault normal component only.
        xtracaI[i], ytracaI[i] = els.rotmat[idx["aI"][i], :, :] * normalTractions
    end

    # Kernels and assembly
    TH = zeros(6*nels, 6*nels)

    # Region I materials
    T_aI_aI, H_aI_aI = PUTC(slip2dispstress, els, idx["aI"], idx["aI"], muI, nuI)
    T_aI_bI, H_aI_bI = PUTC(slip2dispstress, els, idx["aI"], idx["bI"], muI, nuI)
    T_bI_aI, H_bI_aI = PUTC(slip2dispstress, els, idx["bI"], idx["aI"], muI, nuI)
    T_bI_bI, H_bI_bI = PUTC(slip2dispstress, els, idx["bI"], idx["bI"], muI, nuI)

    # Region II materials
    T_bII_bII, H_bII_bII = PUTC(slip2dispstress, els, idx["bII"], idx["bII"], muII, nuII)

    # Assemble BEM operator and boundary conditions
    # This row of equations enforces displacement equality at the boundary between regions.
    TH[1:(nels*2), 1:(nels*2)] = -T_bII_bII
    TH[1:(nels*2), (nels*2+1):(nels*4)] = T_bI_bI
    TH[1:(nels*2), (nels*4+1):(nels*6)] = T_bI_aI

    # This row of equations enforces traction continuity at the boundary between regions.
    TH[(nels*2+1):(nels*4), 1:(nels*2)] = H_bII_bII
    TH[(nels*2+1):(nels*4), (nels*2+1):(nels*4)] = H_bI_bI
    TH[(nels*2+1):(nels*4), (nels*4+1):(nels*6)] = H_bI_aI

    # This row of equations enforces the radial traction BC
    TH[(nels*4+1):(nels*6), (nels*2+1):(nels*4)] = H_aI_bI
    TH[(nels*4+1):(nels*6), (nels*4+1):(nels*6)] = H_aI_aI

    matshow(TH); title(cond(TH)); colorbar()

    # Boundary conditions
    bcs = zeros(6*nels)
    bcs[(nels*4+1):(nels*6)] = interleave(xtracaI, ytracaI)

    # Simple diagonal preconditioner. Multiply every row by the inverse of its
    # diagonal entry so that the diagonal will be all ones.
    diag_entries = ones(6*nels)
    diag_entries[1:(6*nels)] = diag(TH)
    TH = TH ./ diag_entries
    bcs = bcs ./ diag_entries

    if DEBUG
        @show cond(TH)
        @show rank(TH)
        @show size(TH)
        figure()
        matshow(log10.(abs.(TH)))
        colorbar()
        figure()
        plot(els.x1[idx["aI"]], els.y1[idx["aI"]])
        quiver(els.x1[idx["aI"]], els.y1[idx["aI"]], bcs[(nels*4+1):2:end], bcs[(nels*4+2):2:end])
        title("r=a traction BCs")
    end

    # Solve BEM problem
    Ueff = TH \ bcs
    Ueffb2 = Ueff[1:1:(nels*2)]
    Ueffb1 = Ueff[(nels*2+1):1:(nels*4)]
    Ueffa1 = Ueff[(nels*4+1):1:(nels*6)]

    if DEBUG
        figure()
        plot(els.x1[idx["aI"]], els.y1[idx["aI"]])
        quiver(els.x1[idx["aI"]], els.y1[idx["aI"]], Ueffa1[1:2:end], Ueffa1[2:2:end])
        plot(els.x1[idx["bI"]], els.y1[idx["bI"]])
        quiver(els.x1[idx["bI"]], els.y1[idx["bI"]], Ueffb1[1:2:end], Ueffb1[2:2:end])
        title("region I Ueff")

        figure()
        plot(els.x1[idx["bII"]], els.y1[idx["bII"]])
        quiver(els.x1[idx["bII"]], els.y1[idx["bII"]], Ueffb2[1:2:end], Ueffb2[2:2:end])
        title("region II Ueff")

        # Effective displacements
        figure()
        plot(Ueff[1:2:end], ".r", label="ux")
        plot(Ueff[2:2:end], "+b", label="uy")
        legend()
        title("Ueff - whole vector")
    end

    # Forward line evaluation
    nprof = 30
    xprof = LinRange(0.51, 1.49, nprof)
    yprof = zeros(size(xprof))
    Uddm = zeros(nprof, 2)
    Sddm = zeros(nprof, 3)
    aidx = findall(x -> x <= b, xprof)
    bidx = findall(x -> x > b, xprof)
    UbII, SbII = constdispstress(slip2dispstress, xprof, yprof, els, idx["bII"],
                               Ueffb2[1:2:end], Ueffb2[2:2:end], muII, nuII)
    UbI, SbI = constdispstress(slip2dispstress, xprof, yprof, els, idx["bI"],
                               Ueffb1[1:2:end], Ueffb1[2:2:end], muI, nuI)
    UaI, SaI = constdispstress(slip2dispstress, xprof, yprof, els, idx["aI"],
                               Ueffa1[1:2:end], Ueffa1[2:2:end], muI, nuI)
    UI = UbI .+ UaI
    SI = SbI .+ SaI
    UII = UbII
    SII = SbII
    Uddm[aidx, :] = UI[aidx, :]
    Uddm[bidx, :] = UII[bidx, :]
    Sddm[aidx, :] = SI[aidx, :]
    Sddm[bidx, :] = SII[bidx, :]

    # Analytic solution
    nprofanalytic = 10000
    r = LinRange(0.5+1e-3, 1.50-1e-3, nprofanalytic)
    aidx = findall(x -> x <= b, r)
    bidx = findall(x -> x > b, r)
    pprime = (2*(1-nuI)*p*a^2/b^2) / (2*(1-nuI)+(muI/muII-1)*(1-a^2/b^2))
    a2b2 = a^2/b^2
    Srr1 = @. 1/(1-a2b2) * (p*a2b2-pprime - (p-pprime)*a^2/r^2)
    Stt1 = @. 1/(1-a2b2) * (p*a2b2-pprime + (p-pprime)*a^2/r^2)
    Srr2 = @. -pprime * b^2 / r^2
    Stt2 = @. pprime * b^2 / r^2
    Srr = zeros(size(Srr1))
    Stt = zeros(size(Stt1))
    Srr[aidx] = Srr1[aidx]
    Srr[bidx] = Srr2[bidx]
    Stt[aidx] = Stt1[aidx]
    Stt[bidx] = Stt2[bidx]

    # Graphically compare analytic and BEM solutions
    linewidth = 2.0
    figure(figsize=(7, 7))
    subplot(2, 1, 1)
    axvline(x=1, linewidth=1, linestyle=":", color="k")
    text([0.96], [1.1], "I", horizontalalignment="center")
    text([1.04], [1.1], "II", horizontalalignment="center")
    plot(r, Stt ./ p, "-b", linewidth=linewidth, label="analytic")
    plot(xprof, Sddm[:, 2] ./ p, "ro", markersize=6, label="DDM")
    xlabel(L"x \; / \; b")
    ylabel(L"\sigma_{yy} \; / \; p")
    xlim([0.5, 1.5])
    ylim([-0.3, 1.3])

    legend()

    subplot(2, 1, 2)
    axvline(x=1, linewidth=1, linestyle=":", color="k")
    text([0.96], [0.1], "I", horizontalalignment="center")
    text([1.04], [0.1], "II", horizontalalignment="center")
    plot(r, Srr ./ p, "-b", linewidth=linewidth, label="analytic")
    plot(xprof, Sddm[:, 1] ./ p, "ro", markersize=6, label="DDM")
    xlabel(L"x \; / \; b")
    ylabel(L"\sigma_{xx} \; / \; p")
    xlim([0.5, 1.5])
    ylim([-1.3, 0.3])
    legend()
end
discmaterial()


function measurement(z,c,μ,ξ,Ω,graph)
    n = size(graph,1)-1
    qnoise = diagm(0 => [5.0,0.02])
    nnew = maximum(c)-n

    if nnew > 0
        osize = length(ξ)
        nsize = osize+2*nnew
        Ω₁ = zeros(nsize,nsize)
        Ω₁[1:osize,1:osize] = Ω
        Ω = Ω₁
        append!(ξ,zeros(2*nnew))
        append!(μ,inverse_measurement(μ[1:3],z[:,c .> n])[:])
        padd = spzeros(Bool,nnew,n+1)
        padd1 = spzeros(Bool,n+nnew+1,nnew)
        graph = [[graph;padd] padd1]
    end

    jcb = jacobian_measurement(μ,c)
    ẑ = equation_measurement(μ,c)
    qnoiseinv = inv(qnoise)

    for (i,j) ∈ enumerate(c)
        h = jcb[:,:,i]
        dz = z[:,i]-ẑ[:,i]
        dz[2] = measure.(dz[2])
        ind = [1:3;2j+2;2j+3]

        ξ[ind] .+= h'*qnoiseinv*(dz+h*μ[ind])
        Ω[ind,ind] .+= h'*qnoiseinv*h
    end

    c = unique(c)
    rows = ones(length(c))
    cols = c .+ 1
    sg = size(graph,1)
    adjacency = sparse(rows,cols,true,sg,sg)
    graph .|= adjacency
    graph .|= adjacency'

    return Ω,graph

end


module NoncommutativeGraphsTesting

include("test_header.jl")

@testset "Classical graph" begin
    include("classical_graph.jl")
end

@testset "Simple duality" begin
    include("simple_duality.jl")
end

@testset "Block duality" begin
    include("block_duality.jl")
end

@testset "Block duality 2" begin
    include("block_duality2.jl")
end

# slow and doesn't meet accuracy tolerance
if false
@testset "Thin diag" begin
    include("thin_diag.jl")
end
end

@testset "Diag optimization" begin
    include("diag_optimization.jl")
end

@testset "Unitary transform" begin
    include("unitary_transform.jl")
end

@testset "Compatible matrices" begin
    include("compatible_matrices.jl")
end

@testset "Empty classical graph" begin
    include("empty_classical.jl")
end

@testset "Entropy splitting" begin
    include("entropy_splitting.jl")
end

end # NoncommutativeGraphsTesting


# Part 1
grid = open(f -> readlines(f), "day03_input.txt")
function treecount(grid, right, down)
    pos = [1, 1]
    hits = 0
    while pos[2] <= length(grid)
        row = grid[pos[2]]
        hits += row[(pos[1] - 1) % length(row) + 1] == '#' ? 1 : 0
        pos += [right, down]
    end
    hits
end
println("There are $(treecount(grid, 3, 1)) trees encountered")

# Part 2
slopes = [[1, 1], [3, 1], [5, 1], [7, 1], [1, 2]]
println("Tree product: $(prod([treecount(grid, slope...) for slope in slopes]))")


##
"Compute the square difference between a model spreading and an estimate."
function squarediff(model::FrequencyDirectionSpectra.FDSpectralModel, fdest::Union{FrequencyDirectionSpectra.MEM,FrequencyDirectionSpectra.MLM})
    return squarediff(model, fdest.D, fdest.Ω, fdest.Φ)
end
"Compute the square difference between a model spreading and an estimate seperated into ordinates and frequencies."
function squarediff(model::FrequencyDirectionSpectra.FDSpectralModel, D, Ω, Φ)
    return sum((FrequencyDirectionSpectra.spreading(model, Ω[i], Φ[j]) - D[i, j])^2 for i ∈ 1:size(D, 1), j ∈ 1:size(D, 2))
end
"Compute the square difference between a model sdf and an estimate."
function squarediff(model::FrequencyDirectionSpectra.FDSpectralModel,B::BartlettPeriodogram)
    return squarediff(model, B.Ω, B.ordinates)
end
"Compute the square difference between a model sdf and an estimate seperated into ordinates and frequencies."
function squarediff(model::FrequencyDirectionSpectra.FDSpectralModel, Ω, ordinates)
    return sum((FrequencyDirectionSpectra.sdf(model, ω) - I)^2 for (ω, I) ∈ zip(Ω, ordinates))
end

##
function LSfit(
    model,
    ts,
    Δ,
    θ₀;
    fdest = FrequencyDirectionSpectra.MLM,
    ωₗ = 0,
    ωᵤ = Inf,
    window = 128,
    ndirection = 100,
)
    B = BartlettPeriodogram(ts, Δ, window)[end÷2+1:end]
    Bused = B[ωₗ.<=B.Ω.<=ωᵤ]
    S = fdest(Bused.ordinates, Bused.Ω, range(-π, π, length = ndirection))
    Bz = BartlettPeriodogram(ts[:, 1], Δ, window)[end÷2+1:end][ωₗ.<=B.Ω.<=ωᵤ]
    res1 = optimize(θ -> marginalbounds(θ,ωₗ,ωᵤ) ? squarediff(model(θ), Bz) : Inf, θ₀[1:4])
    if Optim.converged(res1)
        res2 = optimize(θ -> spreadingbounds(θ) ? squarediff(model([res1.minimizer; θ]), S) : Inf, θ₀[5:end])
        if Optim.converged(res2)
            return (true, [res1.minimizer; res2.minimizer])
        end
    end
    return (false, θ₀)
end

##
function marginalbounds(θ,ωₗ,ωᵤ)
    return 4>θ[1]>0 && ωᵤ>θ[2]>ωₗ && 20>θ[3]>1 && 8>θ[4]>3
end
function spreadingbounds(θ)
    return 2π>θ[2]>0 && θ[3]>0 && θ[4]>0 && θ[5]>0
end


using OpenSpecFun
using Base.Test

const SF = OpenSpecFun

# useful test functions for relative error, which differ from isapprox
# in that errc separately looks at the real and imaginay parts
err(z, x) = z == x ? 0.0 : abs(z - x) / abs(x)
errc(z, x) = max(err(real(z),real(x)), err(imag(z),imag(x)))
≅(a,b) = errc(a,b) ≤ 1e-13

# error functions
@testset "error functions" begin
    @test SF.erf(Float16(1)) ≈ 0.84270079294971486934
    @test SF.erf(1) ≈ 0.84270079294971486934
    @test SF.erfc(1) ≈ 0.15729920705028513066
    @test SF.erfc(Float16(1)) ≈ 0.15729920705028513066
    @test SF.erfcx(1) ≈ 0.42758357615580700442
    @test SF.erfcx(Float32(1)) ≈ 0.42758357615580700442
    @test SF.erfcx(Complex64(1)) ≈ 0.42758357615580700442
    @test SF.erfi(1) ≈ 1.6504257587975428760
    @test SF.erfinv(0.84270079294971486934) ≈ 1
    @test SF.erfcinv(0.15729920705028513066) ≈ 1
    @test SF.dawson(1) ≈ 0.53807950691276841914

    @test SF.erf(1+2im) ≈ -0.53664356577856503399-5.0491437034470346695im
    @test SF.erfc(1+2im) ≈ 1.5366435657785650340+5.0491437034470346695im
    @test SF.erfcx(1+2im) ≈ 0.14023958136627794370-0.22221344017989910261im
    @test SF.erfi(1+2im) ≈ -0.011259006028815025076+1.0036063427256517509im
    @test SF.dawson(1+2im) ≈ -13.388927316482919244-11.828715103889593303im

    for elty in [Float32,Float64]
        for x in logspace(-200, -0.01)
            @test_approx_eq_eps SF.erf(SF.erfinv(x)) x 1e-12*x
            @test_approx_eq_eps SF.erf(SF.erfinv(-x)) -x 1e-12*x
            @test_approx_eq_eps SF.erfc(SF.erfcinv(2*x)) 2*x 1e-12*x
            if x > 1e-20
                xf = Float32(x)
                @test_approx_eq_eps SF.erf(SF.erfinv(xf)) xf 1e-5*xf
                @test_approx_eq_eps SF.erf(SF.erfinv(-xf)) -xf 1e-5*xf
                @test_approx_eq_eps SF.erfc(SF.erfcinv(2xf)) 2xf 1e-5*xf
            end
        end
        @test SF.erfinv(one(elty)) == Inf
        @test SF.erfinv(-one(elty)) == -Inf
        @test_throws DomainError SF.erfinv(convert(elty,2.0))

        @test SF.erfcinv(zero(elty)) == Inf
        @test_throws DomainError SF.erfcinv(-one(elty))
    end

    @test SF.erfinv(one(Int)) == SF.erfinv(1.0)
    @test SF.erfcinv(one(Int)) == SF.erfcinv(1.0)
end

# airy
@testset "airy" begin
    @test SF.airy(1.8) ≈ SF.airyai(1.8)
    @test SF.airyprime(1.8) ≈ -0.0685247801186109345638
    @test SF.airyaiprime(1.8) ≈ SF.airyprime(1.8)
    @test SF.airybi(1.8) ≈ 2.595869356743906290060
    @test SF.airybiprime(1.8) ≈ 2.98554005084659907283
    @test_throws SF.AmosException SF.airy(200im)
    @test_throws SF.AmosException SF.airybi(200)
    @test_throws ArgumentError SF.airy(5,one(Complex128))
    z = 1.8 + 1.0im
    for elty in [Complex64,Complex128]
        @test SF.airy(convert(elty,1.8)) ≈ 0.0470362168668458052247
        z = convert(elty,z)
        @test SF.airyx(z) ≈ SF.airyx(0,z)
        @test SF.airyx(0, z) ≈ SF.airy(0, z) * exp(2/3 * z * sqrt(z))
        @test SF.airyx(1, z) ≈ SF.airy(1, z) * exp(2/3 * z * sqrt(z))
        @test SF.airyx(2, z) ≈ SF.airy(2, z) * exp(-abs(real(2/3 * z * sqrt(z))))
        @test SF.airyx(3, z) ≈ SF.airy(3, z) * exp(-abs(real(2/3 * z * sqrt(z))))
        @test_throws ArgumentError SF.airyx(5,z)
    end
    @test_throws MethodError SF.airy(complex(big(1.0)))
end

# bessely0, bessely1, besselj0, besselj1
@testset "bessel" begin
    @test SF.besselj0(Float32(2.0)) ≈ SF.besselj0(Float64(2.0))
    @test SF.besselj1(Float32(2.0)) ≈ SF.besselj1(Float64(2.0))
    @test SF.bessely0(Float32(2.0)) ≈ SF.bessely0(Float64(2.0))
    @test SF.bessely1(Float32(2.0)) ≈ SF.bessely1(Float64(2.0))
    @test SF.besselj0(2) ≈ SF.besselj0(2.0)
    @test SF.besselj1(2) ≈ SF.besselj1(2.0)
    @test SF.bessely0(2) ≈ SF.bessely0(2.0)
    @test SF.bessely1(2) ≈ SF.bessely1(2.0)
    @test SF.besselj0(2.0 + im) ≈ SF.besselj(0, 2.0 + im)
    @test SF.besselj1(2.0 + im) ≈ SF.besselj(1, 2.0 + im)
    @test SF.bessely0(2.0 + im) ≈ SF.bessely(0, 2.0 + im)
    @test SF.bessely1(2.0 + im) ≈ SF.bessely(1, 2.0 + im)

    @test_throws MethodError SF.besselj(1.2,big(1.0))
    @test_throws MethodError SF.besselj(1,complex(big(1.0)))
    @test_throws MethodError SF.besseljx(1,big(1.0))
    @test_throws MethodError SF.besseljx(1,complex(big(1.0)))

    # besselh
    true_h133 = 0.30906272225525164362 - 0.53854161610503161800im
    @test SF.besselh(3,1,3) ≈ true_h133
    @test SF.besselh(-3,1,3) ≈ -true_h133
    @test SF.besselh(3,2,3) ≈ conj(true_h133)
    @test SF.besselh(-3,2,3) ≈ -conj(true_h133)
    @test_throws SF.AmosException SF.besselh(1,0)

    @test_throws MethodError SF.besselh(1,big(1.0))
    @test_throws MethodError SF.besselh(1,complex(big(1.0)))
    @test_throws MethodError SF.besselhx(1,big(1.0))
    @test_throws MethodError SF.besselhx(1,complex(big(1.0)))

    # besseli
    true_i33 = 0.95975362949600785698
    @test SF.besseli(3,3) ≈ true_i33
    @test SF.besseli(-3,3) ≈ true_i33
    @test SF.besseli(3,-3) ≈ -true_i33
    @test SF.besseli(-3,-3) ≈ -true_i33
    @test SF.besseli(Float32(-3),Complex64(-3,0)) ≈ -true_i33
    @test_throws SF.AmosException SF.besseli(1,1000)
    @test_throws DomainError SF.besseli(0.4,-1.0)

    @test_throws MethodError SF.besseli(1,big(1.0))
    @test_throws MethodError SF.besseli(1,complex(big(1.0)))
    @test_throws MethodError SF.besselix(1,big(1.0))
    @test_throws MethodError SF.besselix(1,complex(big(1.0)))

    # besselj
    @test SF.besselj(0,0) == 1
    for i = 1:5
        @test SF.besselj(i,0) == 0
        @test SF.besselj(-i,0) == 0
        @test SF.besselj(-i,Float32(0)) == 0
        @test SF.besselj(-i,Float32(0)) == 0
    end

    j33 = SF.besselj(3,3.)
    @test SF.besselj(3,3) == j33
    @test SF.besselj(-3,-3) == j33
    @test SF.besselj(-3,3) == -j33
    @test SF.besselj(3,-3) == -j33
    @test SF.besselj(3,3f0) ≈ j33
    @test SF.besselj(3,complex(3.)) ≈ j33
    @test SF.besselj(3,complex(3f0)) ≈ j33
    @test SF.besselj(3,complex(3)) ≈ j33

    j43 = SF.besselj(4,3.)
    @test SF.besselj(4,3) == j43
    @test SF.besselj(-4,-3) == j43
    @test SF.besselj(-4,3) == j43
    @test SF.besselj(4,-3) == j43
    @test SF.besselj(4,3f0) ≈ j43
    @test SF.besselj(4,complex(3.)) ≈ j43
    @test SF.besselj(4,complex(3f0)) ≈ j43
    @test SF.besselj(4,complex(3)) ≈ j43

    @test j33 ≈ 0.30906272225525164362
    @test j43 ≈ 0.13203418392461221033
    @test_throws DomainError    SF.besselj(0.1, -0.4)
    @test SF.besselj(0.1, complex(-0.4)) ≈ 0.820421842809028916 + 0.266571215948350899im
    @test SF.besselj(3.2, 1.3+0.6im) ≈ 0.01135309305831220201 + 0.03927719044393515275im
    @test SF.besselj(1, 3im) ≈ 3.953370217402609396im
    @test SF.besselj(1.0,3im) ≈ SF.besselj(1,3im)
    @test_throws SF.AmosException SF.besselj(20,1000im)
    @test_throws MethodError SF.besselj(big(1.0),3im)

    # besselk
    true_k33 = 0.12217037575718356792
    @test SF.besselk(3,3) ≈ true_k33
    @test SF.besselk(-3,3) ≈ true_k33
    true_k3m3 = -0.1221703757571835679 - 3.0151549516807985776im
    @test_throws DomainError SF.besselk(3,-3)
    @test SF.besselk(3,complex(-3)) ≈ true_k3m3
    @test SF.besselk(-3,complex(-3)) ≈ true_k3m3
    @test_throws SF.AmosException SF.besselk(200,0.01)
    # base julia issue #6564
    @test SF.besselk(1.0,0.0) == Inf

    @test_throws MethodError SF.besselk(1,big(1.0))
    @test_throws MethodError SF.besselk(1,complex(big(1.0)))
    @test_throws MethodError SF.besselkx(1,big(1.0))
    @test_throws MethodError SF.besselkx(1,complex(big(1.0)))

    # bessely
    y33 = SF.bessely(3,3.)
    @test SF.bessely(3,3) == y33
    @test SF.bessely(3.,3.) == y33
    @test SF.bessely(3,Float32(3.)) ≈ y33
    @test SF.bessely(-3,3) ≈ -y33
    @test y33 ≈ -0.53854161610503161800
    @test_throws DomainError SF.bessely(3,-3)
    @test SF.bessely(3,complex(-3)) ≈ 0.53854161610503161800 - 0.61812544451050328724im
    @test_throws SF.AmosException SF.bessely(200.5,0.1)
    @test_throws DomainError SF.bessely(0.4,-1.0)
    @test_throws DomainError SF.bessely(0.4,Float32(-1.0))
    @test_throws DomainError SF.bessely(1,Float32(-1.0))
    @test_throws DomainError SF.bessely(Cint(3),Float32(-3.))
    @test_throws DomainError SF.bessely(Cint(3),Float64(-3.))

    @test_throws MethodError SF.bessely(1.2,big(1.0))
    @test_throws MethodError SF.bessely(1,complex(big(1.0)))
    @test_throws MethodError SF.besselyx(1,big(1.0))
    @test_throws MethodError SF.besselyx(1,complex(big(1.0)))

    # besselhx
    for elty in [Complex64,Complex128]
        z = convert(elty, 1.0 + 1.9im)
        @test SF.besselhx(1.0, 1, z) ≈ convert(elty,-0.5949634147786144 - 0.18451272807835967im)
        @test SF.besselhx(Float32(1.0), 1, z) ≈ convert(elty,-0.5949634147786144 - 0.18451272807835967im)
    end

    @test_throws MethodError SF.besselh(1,1,big(1.0))
    @test_throws MethodError SF.besselh(1,1,complex(big(1.0)))
    @test_throws MethodError SF.besselhx(1,1,big(1.0))
    @test_throws MethodError SF.besselhx(1,1,complex(big(1.0)))

    # base julia issue #6653
    for f in (SF.besselj,SF.bessely,SF.besseli,SF.besselk,SF.hankelh1,SF.hankelh2)
        @test f(0,1) ≈ f(0,Complex128(1))
        @test f(0,1) ≈ f(0,Complex64(1))
    end

    # scaled bessel[ijky] and hankelh[12]
    for x in (1.0, 0.0, -1.0), y in (1.0, 0.0, -1.0), nu in (1.0, 0.0, -1.0)
        z = Complex128(x + y * im)
        z == zero(z) || @test SF.hankelh1x(nu, z) ≈ SF.hankelh1(nu, z) * exp(-z * im)
        z == zero(z) || @test SF.hankelh2x(nu, z) ≈ SF.hankelh2(nu, z) * exp(z * im)
        (nu < 0 && z == zero(z)) || @test SF.besselix(nu, z) ≈ SF.besseli(nu, z) * exp(-abs(real(z)))
        (nu < 0 && z == zero(z)) || @test SF.besseljx(nu, z) ≈ SF.besselj(nu, z) * exp(-abs(imag(z)))
        z == zero(z) || @test SF.besselkx(nu, z) ≈ SF.besselk(nu, z) * exp(z)
        z == zero(z) || @test SF.besselyx(nu, z) ≈ SF.bessely(nu, z) * exp(-abs(imag(z)))
    end
    @test_throws SF.AmosException SF.hankelh1x(1, 0)
    @test_throws SF.AmosException SF.hankelh2x(1, 0)
    @test_throws SF.AmosException SF.besselix(-1, 0)
    @test_throws SF.AmosException SF.besseljx(-1, 0)
    @test SF.besselkx(1, 0) == Inf
    @test_throws SF.AmosException SF.besselyx(1, 0)
    @test_throws DomainError SF.besselix(0.4,-1.0)
    @test_throws DomainError SF.besseljx(0.4, -1.0)
    @test_throws DomainError SF.besselkx(0.4,-1.0)
    @test_throws DomainError SF.besselyx(0.4,-1.0)
end

# SF.beta, SF.lbeta
@testset "beta, lbeta" begin
    @test SF.beta(3/2,7/2) ≈ 5π/128
    @test SF.beta(3,5) ≈ 1/105
    @test SF.lbeta(5,4) ≈ log(SF.beta(5,4))
    @test SF.beta(5,4) ≈ SF.beta(4,5)
    @test SF.beta(-1/2, 3) ≈ SF.beta(-1/2 + 0im, 3 + 0im) ≈ -16/3
    @test SF.lbeta(-1/2, 3) ≈ log(16/3)
    @test SF.beta(Float32(5),Float32(4)) == SF.beta(Float32(4),Float32(5))
    @test SF.beta(3,5) ≈ SF.beta(3+0im,5+0im)
    @test(SF.beta(3.2+0.1im,5.3+0.3im) ≈ exp(SF.lbeta(3.2+0.1im,5.3+0.3im)) ≈
          0.00634645247782269506319336871208405439180447035257028310080 -
          0.00169495384841964531409376316336552555952269360134349446910im)
end

# SF.gamma, SF.lgamma (complex argument)
@testset "gamma, lgamma" begin
    if Base.Math.libm == "libopenlibm"
        @test SF.gamma(Float64[1:25;]) == SF.gamma(1:25)
    else
        @test SF.gamma(Float64[1:25;]) ≈ SF.gamma(1:25)
    end
    for elty in (Float32, Float64)
        @test SF.gamma(convert(elty,1/2)) ≈ convert(elty,sqrt(π))
        @test SF.gamma(convert(elty,-1/2)) ≈ convert(elty,-2sqrt(π))
        @test SF.lgamma(convert(elty,-1/2)) ≈ convert(elty,log(abs(SF.gamma(-1/2))))
    end
    @test SF.lgamma(1.4+3.7im) ≈ -3.7094025330996841898 + 2.4568090502768651184im
    @test SF.lgamma(1.4+3.7im) ≈ log(SF.gamma(1.4+3.7im))
    @test SF.lgamma(-4.2+0im) ≈ SF.lgamma(-4.2)-pi*im
    @test factorial(3.0) == SF.gamma(4.0) == factorial(3)
    for x in (3.2, 2+1im, 3//2, 3.2+0.1im)
        @test factorial(x) == SF.gamma(1+x)
    end
    @test lfact(1) == 0
    @test lfact(2) == SF.lgamma(3)
end

# SF.digamma
@testset "digamma" begin
    for elty in (Float32, Float64)
        @test SF.digamma(convert(elty, 9)) ≈ convert(elty, 2.140641477955609996536345)
        @test SF.digamma(convert(elty, 2.5)) ≈ convert(elty, 0.7031566406452431872257)
        @test SF.digamma(convert(elty, 0.1)) ≈ convert(elty, -10.42375494041107679516822)
        @test SF.digamma(convert(elty, 7e-4)) ≈ convert(elty, -1429.147493371120205005198)
        @test SF.digamma(convert(elty, 7e-5)) ≈ convert(elty, -14286.29138623969227538398)
        @test SF.digamma(convert(elty, 7e-6)) ≈ convert(elty, -142857.7200612932791081972)
        @test SF.digamma(convert(elty, 2e-6)) ≈ convert(elty, -500000.5772123750382073831)
        @test SF.digamma(convert(elty, 1e-6)) ≈ convert(elty, -1000000.577214019968668068)
        @test SF.digamma(convert(elty, 7e-7)) ≈ convert(elty, -1428572.005785942019703646)
        @test SF.digamma(convert(elty, -0.5)) ≈ convert(elty, .03648997397857652055902367)
        @test SF.digamma(convert(elty, -1.1)) ≈ convert(elty,  10.15416395914385769902271)

        @test SF.digamma(convert(elty, 0.1)) ≈ convert(elty, -10.42375494041108)
        @test SF.digamma(convert(elty, 1/2)) ≈ convert(elty, -γ - log(4))
        @test SF.digamma(convert(elty, 1)) ≈ convert(elty, -γ)
        @test SF.digamma(convert(elty, 2)) ≈ convert(elty, 1 - γ)
        @test SF.digamma(convert(elty, 3)) ≈ convert(elty, 3/2 - γ)
        @test SF.digamma(convert(elty, 4)) ≈ convert(elty, 11/6 - γ)
        @test SF.digamma(convert(elty, 5)) ≈ convert(elty, 25/12 - γ)
        @test SF.digamma(convert(elty, 10)) ≈ convert(elty, 7129/2520 - γ)
    end

    for x in -10.2:0.3456:50
        @test 1e-12 > err(SF.digamma(x+0im), SF.digamma(x))
    end

    # Test against Wolfram Alpha
    @test SF.digamma(7+0im) ≅ 1.872784335098467139393487909917597568957840664060076401194232
    @test SF.digamma(7im) ≅ 1.94761433458434866917623737015561385331974500663251349960124 + 1.642224898223468048051567761191050945700191089100087841536im
    @test SF.digamma(-3.2+0.1im) ≅ 4.65022505497781398615943030397508454861261537905047116427511+2.32676364843128349629415011622322040021960602904363963042380im

    @test isa([SF.digamma(x) for x in [1.0]], Vector{Float64})
end

# SF.trigamma
@testset "trigamma" begin
    for elty in (Float32, Float64)
        @test SF.trigamma(convert(elty, 0.1)) ≈ convert(elty, 101.433299150792758817)
        @test SF.trigamma(convert(elty, 1/2)) ≈ convert(elty, π^2/2)
        @test SF.trigamma(convert(elty, 1)) ≈ convert(elty, π^2/6)
        @test SF.trigamma(convert(elty, 2)) ≈ convert(elty, π^2/6 - 1)
        @test SF.trigamma(convert(elty, 3)) ≈ convert(elty, π^2/6 - 5/4)
        @test SF.trigamma(convert(elty, 4)) ≈ convert(elty, π^2/6 - 49/36)
        @test SF.trigamma(convert(elty, 5)) ≈ convert(elty, π^2/6 - 205/144)
        @test SF.trigamma(convert(elty, 10)) ≈ convert(elty, π^2/6 - 9778141/6350400)
    end

    # Test against Wolfram Alpha
    @test SF.trigamma(8+0im) ≅ 0.133137014694031425134546685920401606452509991909746283540546
    @test SF.trigamma(8im) ≅ -0.0078125000000000000029194973110119898029284994355721719150 - 0.12467345030312762782439017882063360876391046513966063947im
    @test SF.trigamma(-3.2+0.1im) ≅ 15.2073506449733631753218003030676132587307964766963426965699+15.7081038855113567966903832015076316497656334265029416039199im

    @test isa([SF.trigamma(x) for x in [1.0]], Vector{Float64})
end

# SF.invdigamma
@testset "invdigamma" begin
    for elty in (Float32, Float64)
        for val in [0.001, 0.01, 0.1, 1.0, 10.0]
            @test abs(SF.invdigamma(SF.digamma(convert(elty, val))) - convert(elty, val)) < 1e-8
        end
    end
    @test abs(SF.invdigamma(2)) == abs(SF.invdigamma(2.))
end

@testset "polygamma" begin
    @test SF.polygamma(20, 7.) ≈ -4.644616027240543262561198814998587152547
    @test SF.polygamma(20, Float16(7.)) ≈ -4.644616027240543262561198814998587152547

    # Test against Wolfram Alpha
    @test SF.polygamma(2, 8.1+0im) ≅ -0.01723882695611191078960494454602091934457319791968308929600
    @test SF.polygamma(30, 8.1+2im) ≅ -2722.8895150799704384107961215752996280795801958784600407589+6935.8508929338093162407666304759101854270641674671634631058im
    @test SF.polygamma(3, 2.1+1im) ≅ 0.00083328137020421819513475400319288216246978855356531898998-0.27776110819632285785222411186352713789967528250214937861im
    @test 1e-11 > err(SF.polygamma(3, -4.2 + 2im),-0.0037752884324358856340054736472407163991189965406070325067-0.018937868838708874282432870292420046797798431078848805822im)
    @test SF.polygamma(13, 5.2 - 2im) ≅ 0.08087519202975913804697004241042171828113370070289754772448-0.2300264043021038366901951197725318713469156789541415899307im
    @test 1e-11 > err(SF.polygamma(123, -47.2 + 0im), 5.7111648667225422758966364116222590509254011308116701029e291)

    @test SF.polygamma(4, -3.1+Inf*im) == SF.polygamma(4, 3.1+Inf*im) == 0
    @test SF.polygamma(4, -0.0) == Inf == -SF.polygamma(4, +0.0)
    @test isa([SF.polygamma(3,x) for x in [1.0]], Vector{Float64})
    @test SF.polygamma(3,5) ≈ SF.polygamma(3,5.)
end

# SF.eta, SF.zeta
@testset "eta, zeta" begin
    @test SF.eta(1) ≈ log(2)
    @test SF.eta(2) ≈ pi^2/12
    @test SF.eta(Float32(2)) ≈ SF.eta(2)
    @test SF.eta(Complex64(2)) ≈ SF.eta(2)
    @test SF.zeta(0) ≈ -0.5
    @test SF.zeta(2) ≈ pi^2/6
    @test SF.zeta(Complex64(2)) ≈ SF.zeta(2)
    @test SF.zeta(4) ≈ pi^4/90
    @test SF.zeta(1,Float16(2.)) ≈ SF.zeta(1,2.)
    @test SF.zeta(1.,Float16(2.)) ≈ SF.zeta(1,2.)
    @test SF.zeta(Float16(1.),Float16(2.)) ≈ SF.zeta(1,2.)
    @test isnan(SF.zeta(NaN))
    @test isnan(SF.zeta(1.0e0))
    @test isnan(SF.zeta(1.0f0))
    @test isnan(SF.zeta(complex(0,Inf)))
    @test isnan(SF.zeta(complex(-Inf,0)))

    @test SF.zeta(4.1+0.3im, -3.2+0.1im) ≅ -281.34474134962502296077659347175501181994490498591796647 + 286.55601240093672668066037366170168712249413003222992205im
    @test SF.zeta(4.1+0.3im, 3.2+0.1im) ≅ 0.0121197525131633219465301571139288562254218365173899270675-0.00687228692565614267981577154948499247518236888933925740902im
    @test SF.zeta(4.1, 3.2+0.1im) ≅ 0.0137637451187986846516125754047084829556100290057521276517-0.00152194599531628234517456529686769063828217532350810111482im
    @test 1e-12 > errc(SF.zeta(1.0001, -4.5e2+3.2im), 10003.765660925877260544923069342257387254716966134385170 - 0.31956240712464746491659767831985629577542514145649468090im)
    @test SF.zeta(3.1,-4.2) ≅ SF.zeta(3.1,-4.2+0im) ≅ 149.7591329008219102939352965761913107060718455168339040295
    @test 1e-15 > errc(SF.zeta(3.1+0im,-4.2), SF.zeta(3.1,-4.2+0im))
    @test SF.zeta(3.1,4.2) ≅ 0.029938344862645948405021260567725078588893266227472565010234
    @test SF.zeta(27, 3.1) ≅ 5.413318813037879056337862215066960774064332961282599376e-14
    @test SF.zeta(27, 2) ≅ 7.4507117898354294919810041706041194547190318825658299932e-9
    @test 1e-12 > err(SF.zeta(27, -105.3), 1.3113726525492708826840989036205762823329453315093955e14)

    @test SF.zeta(4, +0.0) == SF.zeta(4, -0.0) ≅ pi^4 / 90
    @test SF.zeta(5, +0.0) == SF.zeta(5, -0.0) ≅ 1.036927755143369926331365486457034168057080919501912811974
    @test SF.zeta(Inf, 1.) == 1
    @test SF.zeta(Inf, 2.) == 0
    @test isnan(SF.zeta(NaN, 1.))

    @test SF.zeta(2 + 1im, -1.1) ≅ SF.zeta(2 + 1im, -1.1+0im) ≅ -64.580137707692178058665068045847533319237536295165484548 + 73.992688148809018073371913557697318846844796582012921247im

    @test SF.zeta(-3.0, 7.0) ≅ -52919/120
    @test SF.zeta(-3.0, -7.0) ≅ 94081/120
    @test SF.zeta(-3.1, 7.2) ≅ -587.457736596403704429103489502662574345388906240906317350719
    @test SF.zeta(-3.1, -7.2) ≅ 1042.167459863862249173444363794330893294733001752715542569576
    @test SF.zeta(-3.1, 7.0) ≅ -518.431785723446831868686653718848680989961581500352503093748
    @test SF.zeta(-3.1, -7.0) ≅ 935.1284612957581823462429983411337864448020149908884596048161
    @test SF.zeta(-3.1-0.1im, 7.2) ≅ -579.29752287650299181119859268736614017824853865655709516268 - 96.551907752211554484321948972741033127192063648337407683877im
    @test SF.zeta(-3.1-0.1im, -7.2) ≅ 1025.17607931184231774568797674684390615895201417983173984531 + 185.732454778663400767583204948796029540252923367115805842138im
    @test SF.zeta(-3.1-0.1im, 7.2 + 0.1im) ≅ -571.66133526455569807299410569274606007165253039948889085762 - 131.86744836357808604785415199791875369679879576524477540653im
    @test SF.zeta(-3.1-0.1im, -7.2 + 0.1im) ≅ 1035.35760409421020754141207226034191979220047873089445768189 + 130.905870774271320475424492384335798304480814695778053731045im
    @test SF.zeta(-3.1-0.1im, -7.0 + 0.1im) ≅ 929.546530292101383210555114424269079830017210969572819344670 + 113.646687807533854478778193456684618838875194573742062527301im
    @test SF.zeta(-3.1, 7.2 + 0.1im) ≅ -586.61801005507638387063781112254388285799318636946559637115 - 36.148831292706044180986261734913443701649622026758378669700im
    @test SF.zeta(-3.1, -7.2 + 0.1im) ≅ 1041.04241628770682295952302478199641560576378326778432301623 - 55.7154858634145071137760301929537184886497572057171143541058im
    @test SF.zeta(-13.4, 4.1) ≅ -3.860040842156185186414774125656116135638705768861917e6
    @test SF.zeta(3.2, -4) ≅ 2.317164896026427640718298719837102378116771112128525719078
    @test SF.zeta(3.2, 0) ≅ 1.166773370984467020452550350896512026772734054324169010977
    @test SF.zeta(-3.2+0.1im, 0.0) ≅ SF.zeta(-3.2+0.1im, 0.0+0im) ≅ 0.0070547946138977701155565365569392198424378109226519905493 + 0.00076891821792430587745535285452496914239014050562476729610im
    @test SF.zeta(-3.2, 0.0) ≅ SF.zeta(-3.2, 0.0+0im) ≅ 0.007011972077091051091698102914884052994997144629191121056378

    @test 1e-14 > err(SF.eta(1+1e-9), 0.693147180719814213126976796937244130533478392539154928250926)
    @test 1e-14 > err(SF.eta(1+5e-3), 0.693945708117842473436705502427198307157819636785324430166786)
    @test 1e-13 > err(SF.eta(1+7.1e-3), 0.694280602623782381522315484518617968911346216413679911124758)
    @test 1e-13 > err(SF.eta(1+8.1e-3), 0.694439974969407464789106040237272613286958025383030083792151)
    @test 1e-13 > err(SF.eta(1 - 2.1e-3 + 2e-3 * im), 0.69281144248566007063525513903467244218447562492555491581+0.00032001240133205689782368277733081683574922990400416791019im)
    @test 1e-13 > err(SF.eta(1 + 5e-3 + 5e-3 * im), 0.69394652468453741050544512825906295778565788963009705146+0.00079771059614865948716292388790427833787298296229354721960im)
    @test 1e-12 > errc(SF.zeta(1e-3+1e-3im), -0.5009189365276307665899456585255302329444338284981610162-0.0009209468912269622649423786878087494828441941303691216750im)
    @test 1e-13 > errc(SF.zeta(1e-4 + 2e-4im), -0.5000918637469642920007659467492165281457662206388959645-0.0001838278317660822408234942825686513084009527096442173056im)

    # Base Julia issue #7169: (TODO: better accuracy should be possible?)
    @test 1e-9 > errc(SF.zeta(0 + 99.69im), 4.67192766128949471267133846066040655597942700322077493021802+3.89448062985266025394674304029984849370377607524207984092848im)
    @test 1e-12 > errc(SF.zeta(3 + 99.69im), 1.09996958148566565003471336713642736202442134876588828500-0.00948220959478852115901654819402390826992494044787958181148im)
    @test 1e-9 > errc(SF.zeta(-3 + 99.69im), 10332.6267578711852982128675093428012860119184786399673520976+13212.8641740351391796168658602382583730208014957452167440726im)
    @test 1e-13 > errc(SF.zeta(2 + 99.69im, 1.3), 0.41617652544777996034143623540420694985469543821307918291931-0.74199610821536326325073784018327392143031681111201859489991im)
end


include(Pkg.dir("ReinforcementLearning", "examples", "maze","maze.jl"))

function main()
    maze_env = MazeEnv((5,5))
    discount=0.99

    print_maze(maze_env.maze)
    println("Start: $(maze_env.start)")
    println("Goal: $(maze_env.goal)")

    policy = Dict{MazeState, MazeAction}()

    # Initialize the policy with random actions
    for state in getAllStates(maze_env)
        policy[state] = rand(getActions(state, maze_env))
    end

    println("Initial Policy: ")
    print_policy(policy)

    V = policy_evaluation(maze_env, policy, discount)
    println("Initial State Values: ")
    print_state_values(V)

    opt_policy = policy_iteration(maze_env, policy, discount)
    println("Final Policy: ")
    print_policy(opt_policy)

    V = policy_evaluation(maze_env, opt_policy, discount)
    println("Final State Values: ")
    print_state_values(V)
end

function print_policy(policy::Dict{MazeState, MazeAction})
    # Print the policy by iterating over keys
    # MazeState -> MazeAction
    # e.g. MazeState(1,1) -> MazeAction(RIGHT)

    for (k,v) in policy
        println("$k -> $v")
    end
end

function print_state_values(V::Dict{MazeState, Float64})
    # Print the state values by iterating over keys
    # MazeState -> Float64
    # e.g. MazeState(1,1) -> 2.0

    for (k,v) in V
        println("$k -> $v")
    end
end

function policy_evaluation(env::MazeEnv, policy::Dict{MazeState, MazeAction}, discount=0.99)
    V = Dict{MazeState, Float64}() # State values
    # Initialize all state values with 0

    for state in getAllStates(env)
        V[state] = 0.0
    end

    eps = 1e-10

    # Evaluate the policy until there is no change between old values and new values
    # The maximum difference between values for a state must be less than eps
    while true
        delta = 0
        for state in getAllStates(env)
            temp = V[state]
            action = policy[state]
            successors = getSuccessors(state,action,env)
            V[state] = sum(map(i->i[3] * (i[2] + discount * V[i[1]]), successors))
            delta = max(delta, abs(temp-V[state]))
        end

        if delta < eps
            break
        end
    end

    return V
end

function policy_iteration(env::MazeEnv, policy::Dict{MazeState, MazeAction}, discount=0.99)
    # evaluate the policy
    # improve the policy
    # Implement the policy iteration algorithm

    while true
        V = policy_evaluation(env, policy, discount)
        policy_stable = true
        for state in getAllStates(env)
            temp = policy[state]
            x0 = map(a->(a,getSuccessors(state,a,env)), getActions(state,env))
            x1 = map(i->(i[1], map(j->j[3] * (j[2] + discount*V[j[1]]), i[2])), x0)
            x2 = map(i->(i[1],sum(i[2])), x1)
            x3 = sort(x2, by=i->i[2], rev=true)
            policy[state] = x3[1][1]
            if temp != policy[state]
                policy_stable = false
            end
        end

        if policy_stable
            break
        end
    end

    return policy
end

!isinteractive() && !isdefined(Core.Main, :load_only) && main(ARGS)


using Compat, Compat.LinearAlgebra
using FillArrays
using MathematicalSystems
using HybridSystems
using Polyhedra

function horizontal_jump_example(lib::Polyhedra.Library, shift::Bool=false)
    A = LightAutomaton(2)
    add_transition!(A, 1, 2, 1)
    add_transition!(A, 2, 1, 2)

    if shift
        H1 = polyhedron(hrep([Matrix(1.0I, 2, 2); -Matrix(1.0I, 2, 2)], [ones(2); ones(2)]), lib)
    else
        H1 = polyhedron(hrep([Matrix(1.0I, 2, 2); -Matrix(1.0I, 2, 2)], [1., 3, 1, -1]), lib)
    end
    H2 = polyhedron(hrep([Matrix(1.0I, 2, 2); -Matrix(1.0I, 2, 2)], [4, 1 + shift, -2, 1]), lib)
    U = polyhedron(hrep(reshape([1., -1.], 2, 1), [2., -2.]), lib)

    S = ConstrainedDiscreteIdentitySystem.(2, [H1, H2])

    s1 = ConstrainedLinearControlDiscreteSystem([1. 0; 0 1], reshape([1.; 0], 2, 1), H1, U)
    s2 = ConstrainedLinearControlDiscreteSystem([1. 0; 0 1], reshape([1.; 0], 2, 1), H2, U)
    Re = [s1, s2]

    sw = AutonomousSwitching()
    Sw = Fill(sw, 2)

    HybridSystem(A, S, Re, Sw)
end


"""
The base type of a fractional integral sense, in FractionalCalculus.jl, all of the fractional integral senses belong to ```FracIntAlg```
"""
abstract type FracIntAlg end

"""
Riemann-Liouville sense fractional integral algorithms

Note this two algorithms belong to direct compute, precise are ensured, but maybe cause more memory allocation and take more compilation time.
"""
abstract type RLInt <: FracIntAlg end

"""
Using the direct mathematic expression:

```math
(J^αf)(t)=\\frac{1}{\\Gamma(α)} \\int_0^t(t-τ)^{α-1}f(τ)dτ
```

By using [QuadGK](https://github.com/JuliaMath/QuadGK.jl) calculate the integration and obtain the value.
"""
struct RL_Direct <: RLInt end

"""
With first derivative known, we can use the Riemann Liouville sense to obtain the fractional integral more effcient.
"""
struct RL_Direct_First_Diff_Known <: RLInt end

"""
@article{LI20113352,
title = {Numerical approaches to fractional calculus and fractional ordinary differential equation},
author = {Changpin Li and An Chen and Junjie Ye},
}
"""

"""
Using piecewise linear interpolation:

```math
    y_n(t)=\\frac{t-t_{i+1}}{t_i-t_{i+1}}y(t_i)+\\frac{t-t_i}{t_{i+1}-t_i}y(t_{i+1})
```

Constitute the original function with the interpolation and implement the summation.
"""
struct RL_Piecewise <: RLInt end

"""
@book{oldham_spanier_1984,
title={The fractional calculus: Theory and applications of differentiation and integration to arbitrary order},
author={Oldham, Keith B. and Spanier, Jerome},
year={1984}}
"""

"""
Using the **Staircase approximation** to approximate the original function and implement the summation.
"""
struct RLInt_Approx <: RLInt end

"""
Deploying the [Linear Interpolation](https://en.wikipedia.org/wiki/Linear_interpolation) between ``f_{j+1}`` and ``f_j``, **RL_LinearInterp** method is more precise than **RLInt_Approx**.
"""
struct RL_LinearInterp <: RLInt end

"""
@article{2009,
title={Matrix approach to discrete fractional calculus II: Partial fractional differential equations},
DOI={10.1016/j.jcp.2009.01.014},
author={Podlubny, Igor and Chechkin, Aleksei and Skovranek, Tomas and Chen, YangQuan and Vinagre Jara, Blas M.},
}

"""

"""
Using [Triangular Strip Matrix](https://en.wikipedia.org/wiki/Triangle_strip) to discrete the integral.
"""
struct RLInt_Matrix <: RLInt end

####################################
###     Type defination done     ###
####################################

"""
Check if the format of nargins is correct
"""
function checks(f, α, start_point, end_point)
    α % 1 != 0 ? nothing : error("Decimal number only!")
    if isa(end_point, Number)
        start_point < end_point ? nothing : DomainError("Domain error! Start point must smaller than end point")
    elseif isa(end_point, AbstractArray)
        start_point < minimum(end_point) ? nothing : DomainError("Vector domain error! Start point must smaller than end point")
    else
        ErrorException("Please input correct point you wan tto compute")
    end

    #The fractional integral of number is relating with the end_point.
    if isa(f, Number)
        f == 0 ? 0 : 2*f*sqrt(end_point/pi)
    end
end

"""
# Riemann Liouville sense fractional integral

    fracint(f::Function, α, start_point, end_point, h, RL_Direct())

### Example:

```julia-repl
julia> fracint(x->x^5, 0.5, 0, 2.5, 1e-8, RL_Direct())
```

Riemann_Liouville fractional integral using complex step differentiation.
Returns a tuple (1.1639316474512205, 1.0183453796725215e-8), which contains the value of this derivative is 1.1639316474512205, and the error estimate is 1.0183453796725215e-8
"""
function fracint(f::Union{Function, Number}, α, start_point, end_point::Real, h, ::RL_Direct)
    checks(f, α, start_point, end_point)

    temp1 = f(start_point) .* (end_point-start_point) .^α
    #Use Complex differentiation to obtain the differentiation
    g(τ) = imag(f(τ .+ 1*im*h) ./ h) .* (end_point-τ) .^α
    temp2 = quadgk(g, start_point, end_point)
    result = (temp1 .+ temp2) ./gamma(α+1)
    return result
end

function fracint(f::Union{Function, Number}, α::Float64, start_point, end_point::AbstractArray, h, ::RL_Direct)::Vector
    ResultArray = Float64[]

    for (_, value) in enumerate(end_point)
        append!(ResultArray, fracint(f, α, start_point, value, h, RL_Direct())[1])
    end
    return ResultArray
end

"""
# Riemann Liouville sense fractional integral with first diff known.

    fracint(f, fd, α, start_point, end_point, RL_Direct_First_Diff_Known())

### Example

```julia-repl
julia> fracint(x->x^5, x->5*x^4, 0.5, 0, 2.5, RL_Direct_First_Diff_Known())
```

With first order derivative known, we can directly use it in the computation of α order fraction integral
"""
function fracint(f::Function, fd::Function, α, start_point, end_point, ::RL_Direct_First_Diff_Known)
    checks(f, α, start_point, end_point)

    #The fractional integral of number is relating with the end_point.
    if typeof(f) <: Number
        if f == 0
            return 0
        else
            return 2*f*sqrt(end_point/pi)
        end
    end

    temp1 = f(start_point) .* (end_point-start_point) .^α
    g(τ) = fd(τ) .* (end_point-τ) .^α
    temp2 = quadgk(g, start_point, end_point)
    result = (temp1 .+ temp2) ./ gamma(α+1)
    return result
end

function fracint(f::Union{Function, Number}, α::Float64, start_point, end_point::AbstractArray, ::RL_Direct_First_Diff_Known)::Vector
    ResultArray = Float64[]

    for (_, value) in enumerate(end_point)
        append!(ResultArray, fracint(f, α, start_point, value, RL_Direct_First_Diff_Known())[1])
    end
    return ResultArray
end

"""
# Riemann Liouville sense fractional integral using piecewise interpolation.

    fracint(f, α, end_point, h, RL_Piecewise())

### Example

```julia-repl
julia> fracint(x->x^5, 0.5, 2.5, 0.0001, RL_Piecewise())
```

By deploying Piecewise interpolation to approximate the original function, with small step size, this method is fast and take little memory allocation.
"""
function fracint(f::Union{Function, Number}, α::Float64, end_point, h, ::RL_Piecewise)::Float64
    checks(f, α, 0, end_point)

    #Init
    n=end_point/h
    result=0

    for i in range(0, n, step=1)
        result = result + W(i, n, α)*f(i*h)
    end

    result1 = result*h^α/gamma(α+2)
    return result1
end
function W(i, n, α)
    if i==0
        return n^α*(α+1-n)+(n-1)^(α+1)
    elseif i==n
        return 1
    else
        return (n-i-1)^(α+1)+(n-i+1)^(α+1)-2*(n-i)^(α+1)
    end
end

function fracint(f::Union{Function, Number}, α::Float64, end_point::AbstractArray, h, ::RL_Piecewise)::Vector
    ResultArray = Float64[]

    for (_, value) in enumerate(end_point)
        append!(ResultArray, fracint(f, α, value, h, RL_Piecewise())[1])
    end
    return ResultArray
end

"""
# Riemann Liouville sense fractional integral approximation.

    fracint(f, α, end_point, h, RLInt_Approx())

### Example

```julia-repl
julia> fracint(x->x^5, 0.5, 2.5, 0.0001, RLInt_Approx())
```
"""
function fracint(f::Union{Function, Number}, α::Float64, end_point, h, ::RLInt_Approx)::Float64
    checks(f, α, 0, end_point)

    α = -α
    n = end_point/h
    result=0

    for i in range(0, n-1, step=1)
        result += (f(end_point-i*end_point/n)+f(end_point-(i+1)*end_point/n))/2*((i+1)^(-α)-i^(-α))
    end

    result1=result*end_point^(-α)*n^α/gamma(1-α)
    return result1
end

function fracint(f::Union{Function, Number}, α::Float64, end_point::AbstractArray, h, ::RLInt_Approx)::Vector
    ResultArray = Float64[]

    for (_, value) in enumerate(end_point)
        append!(ResultArray, fracint(f, α, value, h, RLInt_Approx())[1])
    end
    return ResultArray
end

"""
# Riemann Liouville sense fractional integral using Linear interpolation.

    fracint(f, α, end_point, h, RL_LinearInterp())

### Example

```julia-repl
julia> fracint(x->x^5, 0.5, 2.5, 0.0001, RL_LinearInterp())
```

**RL_LinearInterp** is more complex but more precise.
"""
function fracint(f::Union{Function, Number}, α::Float64, end_point::Number, h, ::RL_LinearInterp)::Float64

    checks(f, α, 0, end_point)

    α = -α
    n = end_point/h
    result = 0

    for i in range(0, n-1, step=1)
        result+=((i+1)*f(end_point-i*end_point/n)-i*f(end_point-(i+1)*end_point/n))/(-α)*((i+1)^(-α)-i^(-α))+(f(end_point-(i+1)*end_point/n)-f(end_point-i*end_point/n))/(1-α)*((i+1)^(1-α)-i^(1-α))
    end

    result1=result*end_point^(-α)*n^α/gamma(-α)
    return result1
end

function fracint(f::Union{Function, Number}, α::Float64, end_point::AbstractArray, h, ::RL_LinearInterp)::Vector
    ResultArray = Float64[]

    for (_, value) in enumerate(end_point)
        append!(ResultArray, fracint(f, α, value, h, RL_LinearInterp())[1])
    end
    return ResultArray
end

"""
# Riemann Liouville sense integral using Triangular Strip Matrix to discrete and compute.

    fracint(f, α, end_point, h, RLInt_Matrix())

Using Triangular Strip Matrix to approximate fractional integral.

### Example

```julia-repl
julia> fracint(x->x^5, 0.5, 2.5, 0.0001, RLInt_Matrix())
```

!!! info
    Triangular Strip Matrix method returns the derivative in the interval ``[0, T]`` in ```Vector```

!!! tip
    With the advancing Triangular Strip Matrix method, you can not only compute fractional integrals, integer order, higher order integral is also supported!!
Try to set α as an integer, arbitrary integer of course! I promise you would enjoy it😏
"""
function fracint(f, α::Number, end_point, h::Float64, ::RLInt_Matrix)
    N=Int64(end_point/h+1)
    tspan=collect(0:h:end_point)
    return J(N, α, h)*f.(tspan)
end

function omega(n, p)
    omega = zeros(n+1)

    omega[1]=1
    for i in range(1, n, step=1)
        omega[i+1]=(1-(p+1)/i)*omega[i]
    end

    return omega

end
function J(N, p, h::Float64)
    result=zeros(N, N)
    temp=omega(N, -p)

    for i in range(1, N, step=1)
        result[i, 1:i]=reverse(temp[1:i])
    end

    return h^p*result
end

## Macros for convenient computing.
"""
    @fracint(f, α, point)

Return the α-order integral of **f** at specific point.

```julia-repl
julia> @fracint(x->x, 0.5, 1)
0.7522525439593486
```
"""
macro fracint(f::Union{Function, Number}, α::Float64, point)
    return :(fracint($f, $α, $point, 0.0001, RLInt_Approx()))
end

"""
    @semifracint(f, point)

Return the semi-integral of **f** at specific point.

```julia-repl
julia> @semifracint(x->x, 1)
0.7522525439593486
```
"""
macro semifracint(f::Union{Function, Number}, point)
    return :(fracint($f, 0.5, $point, 0.0001, RLInt_Approx()))
end


# Time correlated noise

function timecorr_noise(q, timestep, timedecorr)

	alfa = 1 - timestep / timedecorr

	q = alfa*q + sqrt(1-alfa^2)*randn()

	return q

end

# Resampling function

function resample(wk)

	Ns = length(wk)

	u = cumprod(rand(Ns) .^ (1./collect(Float64, Ns:-1:1)))

	u = u[length(u):-1:1]

	wc = cumsum(wk)

	label = zeros(Int64, Ns)

	k = 1
	for i = 1:Ns
		while wc[k] < u[i]
			k = k + 1
		end
		label[i] = k
	end

	return label

end


export relaxationMap

"""
  Calculates the relaxationMap based on the pixel values of the image

  ...
  # Arguments
  * `image::Matrix` : Image whose values determine the relaxationrate at each pixel.
  * `minRelaxation::Float64=5.0` : minimum relaxationrate measured in [Hz]
  * `maxRelaxation::Float64=125.0` : maximum relaxationrate measured in [Hz]
  ...
"""
function relaxationMap(image::Matrix,minRelaxation::Float64=5.0,maxRelaxation::Float64=125.0)
  # consistency check
  if minRelaxation > maxRelaxation
    warning("Minimal frequency > Maximalfrequency! Both will be swapped now")
    minRelaxation,maxRelaxation = maxRelaxation,minRelaxation
  end

  relaxMap = zeros(size(image))
  # normalize first
  minVal = minimum(image)
  maxVal = maximum(image)
  z = image .- minVal ./ (maxVal-minVal)

  # now denormalize
  relaxMap = z .* (maxRelaxation - minRelaxation) .+ minRelaxation

  return relaxMap

end