module GraphingUtilities
    using Statistics, Plots, ..Representation
    function plot_all(fitness_values::Array{Array{Float64, 1}, 1},
                        num_generations::Int, population::Representation.Population,
                        true_values::Array{Float64, 1}, x_values::Array{Float64, 1},
                        hit_ratios::Array{Array{Float64, 1}, 1},
                        population_size::Int64,
                        save_name::Union{Nothing, AbstractString})

        error_plot = plot(xlabel="Generation", ylabel="Error")
        plot_average_fitness!(fitness_values, num_generations)
        plot_max_fitness!(fitness_values, num_generations)
        func_plot = plot_both_functions(population, true_values, x_values)
        hit_ratio_plot = plot_metric(hit_ratios, num_generations, population_size,"Hit Ratio")
        if save_name != nothing
            savefig(error_plot, save_name * "_error.png")
            savefig(func_plot, save_name * "_funcs.png")
            savefig(hit_ratio_plot, save_name * "_hr.png")
        else
            display(error_plot)
            display(func_plot)
            display(hit_ratio_plot)
        end
    end

    function plot_metric(metric_values::Array{Array{Float64, 1}, 1},
                            num_generations::Int, population_size::Int64,
                            metric_name::AbstractString)
        averages = [mean(generation_values) for generation_values in metric_values]
        averages = averages / (population_size + 1)
        plot(1:num_generations, averages, lw=3,
                xlabel="Generation", ylabel=metric_name, label=metric_name)
    end

    function plot_average_fitness!(fitness_values::Array{Array{Float64, 1}, 1},
                                    num_generations::Int)
        averages = [mean(generation_fitvalues) for generation_fitvalues in fitness_values]
        plot!(1:num_generations, averages, lw=3,
                xlabel="Generation", ylabel="Error", label="Mean Fitness")
    end

    function plot_max_fitness!(fitness_values::Array{Array{Float64, 1}, 1},
                                num_generations::Int)
        minimums = [minimum(generation_fitvalues) for generation_fitvalues in fitness_values]
        plot!(1:num_generations, minimums, lw=3, label="Max fitness")
    end

    function plot_both_functions(population::Representation.Population,
                                 true_values::Array{Float64, 1},
                                 x_values::Array{Float64, 1})
        func_plot = plot(xlabel="X", ylabel="Y")
        fittest = sort(population.members, by=i->i.fitness)[end]
        pred_values = [kernels_to_values(x, fittest.gauss_kernels) for x in true_values]
        plot!(x_values, pred_values, lw=2, label="Pred function")
        plot!(x_values, true_values, lw=2, label="True function")
        return func_plot
    end

    function kernels_to_values(x::Float64,
                                kernels::Array{Tuple{Float64, Float64, Float64}, 1})
        sum = Float32(0.0)
        for kernel in kernels
            (weight, c, γ) = kernel
            sum += weight * exp(-γ*(c - x)^2)
        end
        return sum
    end

end


import FFTW.Frequencies

"""The Mel spectrogram"""
struct MelSpectrogram{T, F <: Union{Frequencies,AbstractRange}} <: DSP.Periodograms.TFR{T}
    power::Matrix{T}
    mels::F
    time::StepRangeLen{Float64}
end
heatmap(tfr::MelSpectrogram) = log10(tfr.power)
DSP.freq(tfr::MelSpectrogram) = tfr.mels
Base.time(tfr::MelSpectrogram) = tfr.time

"""A TFR subtype where each column represents an MFCC vector"""
struct MFCC{T, F <: Union{Frequencies,AbstractRange}} <: DSP.Periodograms.TFR{T}
    mfcc::Matrix{T}
    number::F
    time::StepRangeLen{Float64}
end
heatmap(tfr::MFCC) = tfr.mfcc
DSP.freq(tfr::MFCC) = tfr.number
Base.time(tfr::MFCC) = tfr.time

""""""
function hz_to_mel(frequencies::Union{F, AbstractArray{F}}) where {F <: Real}
    f_min = 0f0
    f_sp = 200f0 / 3

    mels = collect((frequencies - f_min) / f_sp)

    min_log_hz = 1000f0
    min_log_mel = (min_log_hz - f_min) / f_sp
    logstep = log(6.4f0) / 27f0

    @inbounds for i = 1:length(mels)
        if frequencies[i] >= min_log_hz
            mels[i] = min_log_mel + log(frequencies[i] / min_log_hz) / logstep
        end
    end

    mels
end

""""""
function mel_to_hz(mels::Union{F, AbstractArray{F}}) where {F <: Real}
    f_min = 0f0
    f_sp = 200f0 / 3
    frequencies = collect(f_min + f_sp * mels)

    min_log_hz = 1000f0
    min_log_mel = (min_log_hz - f_min) / f_sp
    logstep = log(6.4f0) / 27f0

    @inbounds for i = 1:length(frequencies)
        if mels[i] >= min_log_mel
            frequencies[i] = min_log_hz * exp(logstep * (mels[i] - min_log_mel))
        end
    end

    frequencies
end

""""""
function mel_frequencies(nmels::Int = 128, fmin::Real = 0.0f0, fmax::Real = 11025f0)
    min_mel = hz_to_mel(fmin)[1]
    max_mel = hz_to_mel(fmax)[1]

    mels = linspace(min_mel, max_mel, nmels)
    mel_to_hz(mels)
end

""""""
function mel(samplerate::Real, nfft::Int, nmels::Int = 128, fmin::Real = 0f0, fmax::Real = samplerate/2f0)
    weights = zeros(Float32, nmels, (nfft >> 1) + 1)
    fftfreqs = fft_frequencies(samplerate, nfft)
    melfreqs = mel_frequencies(nmels + 2, fmin, fmax)
    enorm = 2f0 ./ (melfreqs[3:end] - melfreqs[1:nmels])

    for i in 1:nmels
        lower = (fftfreqs - melfreqs[i]) / (melfreqs[i+1] - melfreqs[i])
        upper = (melfreqs[i+2] - fftfreqs) / (melfreqs[i+2] - melfreqs[i+1])

        weights[i, :] = max(0, min(lower, upper)) * enorm[i]
    end

    weights
end

function melspectrogram(audio::SampleBuf{T, 1}, windowsize::Int = 1024, hopsize::Int = windowsize >> 2;
                        nmels::Int = 128, fmin::Real = 0f0, fmax::Real = audio.samplerate.val / 2f0) where T
    samplerate = audio.samplerate.val
    nfft = DSP.nextfastfft(windowsize)
    S = spectrogram(audio, windowsize, hopsize).power
    data = mel(samplerate, nfft, nmels, fmin, fmax) * S
    nframes = size(data, 2)
    MelSpectrogram(data, linspace(hz_to_mel(fmin)[1], hz_to_mel(fmax)[1], nmels), (0.0:nframes-1) * hopsize / samplerate)
end

function mfcc(audio::SampleBuf{T, 1}, windowsize::Int = 1024, hopsize::Int = windowsize >> 2;
              nmfcc::Int = 20, nmels::Int = 128, fmin::Real = 0f0, fmax::Real = audio.samplerate.val / 2f0) where T
    if nmfcc >= nmels
        error("number of mfcc components should be less than the number of mel frequency bins")
    end

    M = melspectrogram(audio, windowsize, hopsize; nmels = nmels, fmin = fmin, fmax = fmax)
    mfcc = dct(nmfcc, nmels) * power(M)

    for frame in 1:size(mfcc, 2)
        mfcc[:, frame] /= norm(mfcc[:, frame])
    end

    MFCC(mfcc, 1:nmfcc, time(M))
end


export SymSemiseparable

struct SymSemiseparable <: SymSemiseparableMatrix
    n::Int64
    p::Int64
    U::AbstractArray
    V::AbstractArray
end

# Constuctors
function SymSemiseparable(U::AbstractArray, V::AbstractArray)
    if size(U,1) == size(V,1) && size(U,2) == size(V,2)
        return SymSemiseparable(size(U,1),size(U,2),U,V);
    else
        error("Dimension mismatch between generators U and V")
    end
end

# Mappings
mul!(y::AbstractArray, L::SymSemiseparable, x::AbstractArray) =
    ss_mul_mat!(y, L.U, L.V, x);
mul!(y::AbstractArray, L::AdjointOperator{SymSemiseparable}, x::AbstractArray) =
    ss_mul_mat!(y, L.A.U, L.A.V, x);
function inv!(y, K::SymSemiseparable, b::AbstractArray)
	L = SymSemiseparableChol(K);
	y[:,:] = L'\(L\b);
end
function inv!(y, K::AdjointOperator{SymSemiseparable}, b::AbstractArray)
	L = SymSemiseparableChol(K.A);
	y[:,:] = L'\(L\b);
end

#####################################################################
#### Extended generator representable {p}-semiseperable matrices ####
#####################################################################

#### Matrix-matrix product ####
function ss_mul_mat!(Y::AbstractArray, U::AbstractArray, V::AbstractArray, X::AbstractArray)
    n, m = size(U);
    mx = size(X,2);
    Vbar = zeros(m,mx);
    Ubar = U'*X;
    for i = 1:n
        tmpV = V[i,:];
        tmpU = U[i,:];
        Ubar -= tmpU .* X[i:i,:];
        Vbar += tmpV .* X[i:i,:];
        Y[i,:] = Vbar'*tmpU + Ubar'*tmpV;
    end
end

#### Matrix vector product ####
function ss_create_v(U::AbstractArray, W::AbstractArray)
    n,m = size(U)
    V = zeros(n,m)
    P = zeros(m,m);
    for i = 1:n
        tmpW = W[i,:]
        tmpU = U[i,:]
        tmpV = tmpW*(tmpU'*tmpW)
        V[i,:] = tmpV + P*tmpU
        P += tmpW*tmpW';
    end
    return V
end


function linsolve(operator, b, x₀, alg::BiCGStab, a₀::Number=0, a₁::Number=1)
    # Initial function operation and division defines number type
    y₀ = apply(operator, x₀)
    T = typeof(dot(b, y₀) / norm(b) * one(a₀) * one(a₁))
    α₀ = convert(T, a₀)
    α₁ = convert(T, a₁)
    # Continue computing r = b - a₀ * x₀ - a₁ * operator(x₀)
    r = one(T)*b # r = mul!(similar(b, T), b, 1)
    r = iszero(α₀) ? r : axpy!(-α₀, x₀, r)
    r = axpy!(-α₁, y₀, r)
    x = mul!(similar(r), x₀, 1)
    normr = norm(r)
    S = typeof(normr)

    # Algorithm parameters
    maxiter = alg.maxiter
    tol::S = alg.tol
    numops = 1 # operator has been applied once to determine r
    numiter = 0

    # Check for early return
    if normr < tol
        if alg.verbosity > 0
            @info """BiCGStab linsolve converged without any iterations:
             *  norm of residual = $normr
             *  number of operations = 1"""
        end
        return (x, ConvergenceInfo(1, r, normr, numiter, numops))
    end

    # First iteration
    numiter += 1
    r_shadow = mul!(similar(r), r, 1)     # shadow residual
    ρ = dot(r_shadow, r)

    # Method fails if ρ is zero.
    if ρ ≈ 0.0
        @warn """BiCGStab linsolve errored after $numiter iterations:
        *   norm of residual = $normr
        *   number of operations = $numops"""
        return (x, ConvergenceInfo(0, r, normr, numiter, numops))
    end

    ## BiCG part of the algorithm.
    p = mul!(similar(r), r, 1)
    v = do_apply!(operator, α₀, α₁, p)
    numops += 1

    σ = dot(r_shadow, v)
    α = ρ / σ

    s = mul!(similar(r), r, 1)
    axpy!(-α, v, s)  # half step residual

    xhalf = mul!(similar(x), x, 1)
    axpy!(+α, p, xhalf) # half step iteration

    normr = norm(s)

    # Check for early return at half step.
    if normr < tol
        # Replace approximate residual with the actual residual.
        s = mul!(similar(b), b, 1)
        axpy!(-1, do_apply!(operator, α₀, α₁, xhalf), s)
        numops += 1

        normr_act = norm(s)
        if normr_act < tol
            if alg.verbosity > 0
                @info """BiCGStab linsolve converged at iteration $(numiter-1/2):
                 *  norm of residual = $normr_act
                 *  number of operations = $numops"""
            end
            return (xhalf, ConvergenceInfo(1, s, normr_act, numiter, numops))
        end
    end

    ## GMRES part of the algorithm.
    t = do_apply!(operator, α₀, α₁, s)
    numops += 1

    ω = dot(t, s) / dot(t, t)

    mul!(x, xhalf, 1)
    axpy!(+ω, s, x)    # full step iteration

    mul!(r, s, 1)
    axpy!(-ω, t, r)      # full step residual

    # Check for early return at full step.
    normr = norm(r)
    if normr < tol
        # Replace approximate residual with the actual residual.
        mul!(r, b, 1)
        axpy!(-1, do_apply!(operator, α₀, α₁, x), r)
        numops += 1

        normr_act = norm(r)
        if normr_act < tol
            if alg.verbosity > 0
                @info """BiCGStab linsolve converged at iteration $(numiter):
                *  norm of residual = $normr_act
                *  number of operations = $numops"""
            end
            return (x, ConvergenceInfo(1, r, normr_act, numiter, numops))
        end
    end

    while numiter < maxiter
        if alg.verbosity > 0
            msg = "BiCGStab linsolve in iter $numiter: "
            msg *= "normres = "
            msg *= @sprintf("%12e", normr)
            @info msg
        end

        numiter += 1
        ρold = ρ
        ρ = dot(r_shadow, r)
        β = (ρ / ρold) * (α / ω)

        axpy!(-ω, v, p)
        axpby!(1, r, β, p)

        v = do_apply!(operator, α₀, α₁, p)
        numops += 1

        σ = dot(r_shadow, v)
        α = ρ / σ

        s = mul!(s, r, 1)
        s = axpy!(-α, v, s)     # half step residual

        xhalf = mul!(xhalf, x, 1)
        xhalf = axpy!(+α, p, xhalf)     # half step iteration

        normr = norm(s)

        if alg.verbosity > 0
            msg = "BiCGStab linsolve in iter $(numiter-1/2): "
            msg *= "normres = "
            msg *= @sprintf("%12e", normr)
            @info msg
        end

        # Check for return at half step.
        if normr < tol
            # Compute non-approximate residual.
            #= s = mul!(similar(b), b, 1)
            s = iszero(α₀) ? s : axpy!(-α₀, xhalf, s)
            axpy!(-α₁, apply(operator, xhalf), s)
            numops += 1 =#
            s = mul!(similar(b), b, 1)
            axpy!(-1, do_apply!(operator, α₀, α₁, xhalf), s)
            numops += 1

            normr_act = norm(s)
            if normr_act < tol
                if alg.verbosity > 0
                    @info """BiCGStab linsolve converged at iteration $(numiter-1/2):
                    *  norm of residual = $normr_act
                    *  number of operations = $numops"""
                end
                return (xhalf, ConvergenceInfo(1, s, normr_act, numiter, numops))
            end
        end

        ## GMRES part of the algorithm.
        t = do_apply!(operator, α₀, α₁, s)
        numops += 1

        ω = dot(t, s) / dot(t, t)

        mul!(x, xhalf, 1)
        axpy!(+ω, s, x)    # full step iteration

        mul!(r, s, 1)
        axpy!(-ω, t, r)    # full step residual

        # Check for return at full step.
        normr = norm(r)
        if normr < tol
            # Replace approximate residual with the actual residual.
            mul!(r, b, 1)
            axpy!(-1, do_apply!(operator, α₀, α₁, x), r)
            numops += 1

            normr_act = norm(r)
            if normr_act < tol
                if alg.verbosity > 0
                    @info """BiCGStab linsolve converged at iteration $(numiter):
                    *  norm of residual = $normr_act
                    *  number of operations = $numops"""
                end
                return (x, ConvergenceInfo(1, r, normr_act, numiter, numops))
            end
        end
    end

    if alg.verbosity > 0
        @warn """BiCGStab linsolve finished without converging after $numiter iterations:
        *   norm of residual = $normr
        *   number of operations = $numops"""
    end
    return (x, ConvergenceInfo(0, r, normr, numiter, numops))
end

function do_apply!(operator, α₀, α₁, x)
    y = apply(operator, x)
    if α₀ != zero(α₀) || α₁ != one(α₁)
        axpby!(α₀, x, α₁, y)
    end

    return y
end


macro left_associative(f::Symbol, x::Symbol, y::Symbol, z::Symbol)
    return quote
        $(esc(f))($(esc(f))($(esc(x)), $(esc(y))), $(esc(z))),
        $(esc(f))($(esc(x)), $(esc(f))($(esc(y)), $(esc(z))))
    end
end

macro right_associative(f::Symbol, x::Symbol, y::Symbol, z::Symbol)
    return quote
        $(esc(f))($(esc(x)), $(esc(f))($(esc(y)), $(esc(z)))),
        $(esc(f))($(esc(f))($(esc(x)), $(esc(y))), $(esc(z)))
    end
end

"""
f is commutative if f(x, y) = f(y, x)
"""
macro commutative(f::Symbol, x::Symbol, y::Symbol)
    return quote
        $(esc(f))($(esc(x)), $(esc(y))),
        $(esc(f))($(esc(y)), $(esc(x)))
    end
end

"""
A function f is left distributive over g if f(x, g(y, z)) = g(f(x, y), f(x, z))
"""
macro distributive(f::Symbol, g::Symbol, x::Symbol, y::Symbol, z::Symbol)
    return quote
        $(esc(f))($(esc(x)), $(esc(g))($(esc(y)), $(esc(z)))),
        $(esc(g))($(esc(f))($(esc(x)), $(esc(y))), $(esc(f))($(esc(x)), $(esc(z))))
    end
end

"""
A function f is left factorisable over g iff f(g(x, y), g(x, z)) = g(x, f(y, z))
"""
macro factorisable(f::Symbol, g::Symbol, x::Symbol, y::Symbol, z::Symbol)
    return quote
        $(esc(f))($(esc(g))($(esc(x)), $(esc(y))), $(esc(g))($(esc(x)), $(esc(z)))),
        $(esc(g))($(esc(x)), $(esc(f))($(esc(y)), $(esc(z))))
    end
end

"""
f is idempotent if f(x, x) = x
"""
macro idempotent(f::Symbol, x::Symbol)
    return quote
        $(esc(f))($(esc(x)), $(esc(x))), $(esc(x))
    end
end

"""
A function f is invloute if f(f(x)) = x
"""
macro involute(f::Symbol, x::Symbol)
    return quote
        $(esc(f))($(esc(f))($(esc(x)))), $(esc(x))
    end
end

macro re_involute(f::Symbol, x::Symbol)
    return quote
        $(esc(x)), $(esc(f))($(esc(f))($(esc(x))))
    end
end


using LightGraphs
using ArgParse
using MPI

include("utils.jl")

parser = ArgParseSettings(description = "Crear matriz de Hashimoto.")

@add_arg_table parser begin
    "--output","-o"
    "file"
end

args = parse_args(parser)

output_file = get(args, "output", STDOUT)
if is(output_file, nothing)
    output_file = STDOUT
else
    output_file = args["output"]
end

function debug(msg)
    write(STDERR, msg)
end

debug("Output File: $(output_file)\n")

g = loadgraph(args["file"],:gml)

NBM, edgeidmap = nbm_matrix(g)

println(full(NBM))


module SIWPD
export
    siwpd,
    makesiwpdtree

using
    Wavelets

using
    ..Utils,
    ..DWT

"""
    siwpd(x, wt[, L=maxtransformlevels(x), d=L])

Computes the Shift-Invariant Wavelet Packet Decomposition originally developed
by Cohen, Raz & Malah on the vector `x` using the discrete wavelet filter `wt`
for `L` levels with depth `d`.
"""
function siwpd(x::AbstractArray{T}, wt::OrthoFilter,
        L::Integer=maxtransformlevels(x), d::Integer=L) where T<:Number

    g, h = WT.makereverseqmfpair(wt, true)
    si = Vector{T}(undef, length(wt)-1)                 # tmp filter vector
    return siwpd(x, wt, h, g, si, L, d)
end

function siwpd(x::AbstractVector{Tx}, wt::OrthoFilter, h::Vector{T},
        g::Vector{T}, si::Vector{T}, L::Integer, d::Integer=L) where
        {Tx<:Number, T<:Number}

    n = length(x)
    @assert isdyadic(n)
    @assert 0 <= L <= maxtransformlevels(n)
    @assert 1 <= d <= L

    # decomposed array y
    y = Array{T,2}(undef, (n,2^(L+1)-1))
    y[:,1] = x

    for node in axes(y,2)
        level = floor(Int, log2(node)) # current node level (starts from 0)
        nodelen = nodelength(n, level)
        # decomposition depth of current node
        dₗ = (0 <= level <= L-d) ? d : L-level
        decompose!(y, wt, h, g, si, node, nodelen,
            level, dₗ, 0, zeroshiftdecomposition=false)
    end

    return y
end

function decompose!(y::AbstractArray{Ty,2}, wt::OrthoFilter,
        h::Vector{T}, g::Vector{T}, si::Vector{T},
        node::Integer, nodelen::Integer, level::Integer, d::Integer,
        shift::Integer; zeroshiftdecomposition::Bool=true) where
        {Ty<:Number, T<:Number}

    # base case
    @assert d > 0 || return nothing
    @assert 0 <= level <= maxtransformlevels(size(y,1))
    @assert d <= maxtransformlevels(size(y,2)+1) - level
    @assert 0 <= shift < size(y,1)
    filtlen = length(wt)

    pstart = shift*nodelen + 1                                  # start index
    pend = (shift+1)*nodelen                                    # end index
    parent = y[pstart:pend, node]

    cnodelen = nodelen ÷ 2
    cstart = shift*cnodelen + 1
    lchild = @view y[:, getchildindex(node,:left)]
    rchild = @view y[:, getchildindex(node,:right)]

    # non-shifted subtree
    Wavelets.Transforms.filtdown!(      # scaling coefficients
        g, si, lchild, cstart, cnodelen, parent, 1, 0, false
    )
    Wavelets.Transforms.filtdown!(      # detail coefficients
        h, si, rchild, cstart, cnodelen, parent, 1, -filtlen+1, true
    )
    if zeroshiftdecomposition   # pursue the same thing with d-1 if shift > 0
        decompose!(                     # decompose left child
            y, wt, h, g, si, getchildindex(node,:left), cnodelen, level+1, d-1, shift
        )
        decompose!(                     # decompose right child
            y, wt, h, g, si, getchildindex(node,:right), cnodelen, level+1, d-1, shift
        )
    end

    # shifted subtree
    parent = circshift(parent,1)
    cstart = (shift + 1<<level)*cnodelen + 1
    Wavelets.Transforms.filtdown!(      # scaling coefficients
        g, si, lchild, cstart, cnodelen, parent, 1, 0, false
    )
    Wavelets.Transforms.filtdown!(      # detail coefficients
        h, si, rchild, cstart, cnodelen, parent, 1, -filtlen+1, true
    )
    # pursue the same thing with d-1
    decompose!(                         # decompose shifted left child
        y, wt, h, g, si, getchildindex(node,:left), cnodelen, level+1, d-1, shift+1<<level
    )
    decompose!(                         # decomposed shifted right child
        y, wt, h, g, si, getchildindex(node,:right), cnodelen, level+1, d-1, shift+1<<level
    )

    return nothing
end

"""
    makesiwpdtree(n, L, d)

Returns the multi-level, multi-depth binary tree corresponding to the Shift-
Invariant Wavelet Packet Decomposition.
"""
function makesiwpdtree(n::Integer, L::Integer, d::Integer)
    @assert 0 <= L <= maxtransformlevels(n)
    @assert 1 <= d <= L

    tree = Vector{BitVector}(undef, 2^(L+1)-1)
    for i in eachindex(tree)
        level = floor(Int, log2(i))
        len = nodelength(n, level)
        nshift = n ÷ len        # number of possible shifts for subspace Ω(i,j)
        node = falses(nshift)
        k = ceil(Int, nshift / 1<<d)  # "skips" between shifts
        node[1:k:end] .= true         # mark nodes present in tree
        tree[i] = node
    end
    return tree
end

end # end module


"""
DNRM2 returns the euclidean norm of a vector via the function
name, so that

   DNRM2 := sqrt( x'*x )

-- This version written on 25-October-1982.
   Modified on 14-October-1993 to inline the call to DLASSQ.
   Sven Hammarling, Nag Ltd.
"""
function tron_dnrm2(n,x,incx)
    one = 1.0
    zero = 0.0

    if n < 1 || incx < 1
        xnorm = zero
    elseif n == 1
        xnorm = abs(x[1])
    else
        scale = zero
        ssq = one

        # The following loop is equivalent to this call to the LAPACK
        # auxiliary routine:
        # CALL DLASSQ( N, X, INCX, SCALE, SSQ )

        for ix=1:incx:1+(n-1)*incx
            if x[ix] != zero
                absxi = abs(x[ix])
                if scale < absxi
                    ssq = one + ssq*(scale/absxi)^2
                    scale = absxi
                else
                    ssq = ssq + (absxi/scale)^2
                end
            end
        end
        xnorm = scale*sqrt(ssq)
    end

    return xnorm
end

dnrm2(n,x,incx) = tron_dnrm2(n, x, incx)


"""
`IMWARP` - Apply affine transform to image using bilinear interpolation

    warped_img, warped_offset = imwarp(img, tform, offset)

* `img`: 2D array, image (todo: extend to Image type)
* `tform`: 3x3 matrix, affine transform (defined as row vector x matrix)
* `offset`: 2-element array, position of img[1,1] in global space
* `warped_img`: with pixel values the same type as original image (for Int type,
  pixel values are rounded)
* `warped_offset`: 2-element array, position of warped_img[1,1] in global space

The bounding box of the warped image is defined as the smallest
integer-valued rectangle that contains the affine transform of the
bounding box of the original image.

This means that `warped_offset` is constrained to be integer-valued,
though `offset` is allowed to have floating point values.  The integer
constraint removes the need for further interpolation in any
subsequent fusing of multiple warped tiles.

Bilinear interpolation, with extrapolation using zero fill value.

### Definitions

Global position of `img` pixels (analogous definitions for `warped_img`):

* [1,1] pixel has position (offset[1] - 0.5, offset[2] - 0.5) in global space
* [i,j] pixel has position (offset[1]+i-0.5, offset[2]+j-0.5)

e.g. with no offset, [i, j] pixel has median([i-1:i, j-1:j]) location in global space.

Affine transform of a global position:

* homogeneous coordinates [x, y, 1]  [ax + by + c, dx + ey + f, 1]
* or equivalently [x, y, 1]  [x, y, 1] * tform

        where `tform` = [a d 0;
                         b e 0;
                         c f 1]

Note that:

1. transform is *row* vector x matrix (right hand side matrix multiplication)
2. definition compatible with [MATLAB](http://www.mathworks.com/help/images/ref/affine2d-class.html), but not AffineTransforms.jl
3. meaning of transform depends on whether the image is in ij or xy format

Affine transform of an image (two equivalent definitions, provide scaling isn't an issue):

1. The value of `img` at a position is equal to the value of
the `warped_img` at the transformed position.
2. The value of `warped_img` at a position is equal to the value of
`img` at the inverse transformed position.

We apply definition 2 as it's compatible with gridding and interpolation.

Bounding box of an image of size (m,n):

* smallest rectangle in global space that contains the positions of the [1,1]
  and [m,n] pixels
* represented by the 4-tuple (offset[1],offset[2],m,n)

    (offset[1],offset[2])  ___________
                          |           |
                    height|           |
                       m  |           |
                          |___________|
                                n      (offset[1]+m,offset[2]+n)
                              width

"""
function imwarp{T}(img::Union{Array{T}, SharedArray{T}}, tform, offset=[0.0,0.0]; parallel = false)
  bb = BoundingBox{Float64}(offset..., size(img, 1), size(img, 2))
  wbb = tform_bb(bb, tform)
  tbb = snap_bb(wbb)
  warped_img = parallel && nprocs() != 1 ? SharedArray(T, tbb.h, tbb.w) : zeros(T, tbb.h, tbb.w)
  return imwarp!(warped_img, img, tform, offset; parallel = parallel)
end

function imwarp!{T}(warped_img::Union{Array{T}, SharedArray{T}}, img::Union{Array{T}, SharedArray{T}}, tform, offset=[0.0,0.0]; parallel = false)
  # img bb rooted at offset, with height and width calculated from image
  bb = BoundingBox{Float64}(offset..., size(img, 1), size(img, 2))
  # transform original bb to generate new bb (may contain continuous values)
  wbb = tform_bb(bb, tform)
  # snap transformed bb to the nearest exterior integer values
  tbb = snap_bb(wbb)
  # construct warped_img, pixels same Type as img, size calculated from tbb
  # WARNING: should have zero values, but unclear whether guaranteed by similar
  # warped_img = similar(img, tbb.h+1, tbb.w+1)
  #warped_img = zeros(T, tbb.h, tbb.w)
  if size(warped_img) != (tbb.h, tbb.w)
    println("The supplied output array size is incorrect. Aborting...")
    return;
  end
  # offset of warped_img from the global origin
  warped_offset = [tbb.i, tbb.j]
  #tform[3, 1:2] -= 1.0
  M = inv(tform)   # inverse transform in global space
#  M[3,1:2] -= offset'-1.0   # include conversion to pixel space of original img
  M[3,1:2] -= offset'-0.5   # include conversion to pixel space of original img
  x_max = size(img, 1);
  y_max = size(img, 2);

  function proc_range(idx, arr)
	worker_procs = setdiff(procs(), myid());
	nchunks = length(worker_procs);
	if nchunks == 0 return 1:length(arr); end
	if idx == myid() return 1:0; end
	splits = [round(Int64, s) for s in linspace(0, length(arr), nchunks + 1)];
	return splits[findfirst(worker_procs, idx)]+1:splits[findfirst(worker_procs, idx) + 1]
  end

  if parallel && nprocs() != 1
    if !(typeof(warped_img) <: SharedArray)
    	println("The supplied output array is not a SharedArray despite the parallel keyword set to true. Aborting...")
	return;
    end
    if !(typeof(img) <: SharedArray)
	  img_shared = SharedArray(eltype(img), size(img)...)
	  img_shared[:] = img;
	  img = img_shared;
	end
	@sync for p in procs()
      		j_range = proc_range(p, 1:size(warped_img, 2));
      		@async remotecall_wait(p, compute_and_write_columns!, warped_img, img, M, j_range, x_max, y_max, warped_offset)
	end
  else
  # cycle through all the pixels in warped_img
  for j = 1:size(warped_img,2)
    for i = 1:size(warped_img,1) # cycle through column-first for speed
	compute_and_write_pixel!(warped_img, img, M, i, j, x_max, y_max, warped_offset)
      end
    end
  end
  warped_img, warped_offset
end

function compute_and_write_columns!(warped_img, img, M, j_range, x_max, y_max, warped_offset)
  for j in j_range
    for i in 1:size(warped_img, 1)
	compute_and_write_pixel!(warped_img, img, M, i, j, x_max, y_max, warped_offset)
    end
  end
end

function compute_and_write_pixel!(warped_img, img, M, i, j, x_max, y_max, warped_offset)
        # convert from pixel to global space
        # (we index to zero, then add on the offset)
        @fastmath @inbounds u = i-0.5+warped_offset[1]
	@fastmath @inbounds v = j-0.5+warped_offset[2]
        # apply inv(tform), conversion back to pixel space included
        # x, y = [u, v, 1] * M - but writing it out moves faster
        @fastmath @inbounds x = M[1,1]*u + M[2,1]*v + M[3,1]
	@fastmath @inbounds y = M[1,2]*u + M[2,2]*v + M[3,2]
        # x, y = M[1,1]*u + M[1,2]*v + M[1,3], M[2,1]*u + M[2,2]*v + M[2,3]  # faster but differs by a matrix transpose

        # Slow...
        #warped_img[i,j] = round(Uint8, bilinear(img, x, y))
        #warped_img[i,j] = bilinear(img, x, y)
        # Bilinear interpolation
        fx, fy = floor(Int64, x), floor(Int64, y)
        wx, wy = x-fx, y-fy
	rwx, rwy = 1.0 - wx, 1.0 - wy
        # if 1 <= fx && fx+1 <= x_max && 1 <= fy && fy+1 <= y_max
        if 1 <= fx <= x_max - 1 && 1 <= fy <= y_max -1   # normal case
                # Expansion of p = [1-wx wx] * img[fx:fx+1, fy:fy+1] * [1-wy; wy]
                @fastmath @inbounds pff = rwy * rwx * img[fx,fy]
                @fastmath @inbounds pxf = rwy * wx * img[fx+1,fy]
		@fastmath @inbounds pfy = wy * rwx * img[fx,fy+1]
		@fastmath @inbounds pxy = wy * wx * img[fx+1,fy+1]
		@fastmath writepixel(warped_img, i, j, pff + pxf + pfy + pxy);
	else
	  if 1 <= fx <= x_max - 1
		if fy == 0
		@fastmath @inbounds pfy = wy * rwx * img[fx,fy+1]
		@fastmath @inbounds pxy = wy * wx * img[fx+1,fy+1]
		@fastmath writepixel(warped_img, i, j, pfy + pxy);
		elseif fy == y_max
                @fastmath @inbounds pff = rwy * rwx * img[fx,fy]
                @fastmath @inbounds pxf = rwy * wx * img[fx+1,fy]
		@fastmath writepixel(warped_img, i, j, pff + pxf);
	      end
	  elseif 1 <= fy <= y_max - 1
	    	if fx == 0
                @fastmath @inbounds pxf = rwy * wx * img[fx+1,fy]
		@fastmath @inbounds pxy = wy * wx * img[fx+1,fy+1]
		@fastmath writepixel(warped_img, i, j, pxf + pxy);
		elseif fx == x_max
                @fastmath @inbounds pff = rwy * rwx * img[fx,fy]
		@fastmath @inbounds pfy = wy * rwx * img[fx,fy+1]
		@fastmath writepixel(warped_img, i, j, pff + pfy);
	      end
	    elseif fx == 0 && fy == 0
		@fastmath @inbounds pxy = wy * wx * img[fx+1,fy+1]
		@fastmath writepixel(warped_img, i, j, pxy);
	    elseif fx == 0 && fy == y_max
                @fastmath @inbounds pxf = rwy * wx * img[fx+1,fy]
		@fastmath writepixel(warped_img, i, j, pxf);
	    elseif fx == x_max && fy == 0
		@fastmath @inbounds pfy = wy * rwx * img[fx,fy+1]
		@fastmath writepixel(warped_img, i, j, pfy);
	    elseif fx == x_max && fy == y_max
                @fastmath @inbounds pxy = rwy * rwx * img[fx,fy]
		@fastmath writepixel(warped_img, i, j, pxy);
	  end
	end

end

function writepixel{T<:Integer}(img::Array{T},i,j,pixelvalue)
  @fastmath @inbounds img[i,j]=round(T,pixelvalue)
end

function writepixel{T<:AbstractFloat}(img::Array{T},i,j,pixelvalue)
   @inbounds img[i,j]=pixelvalue
end

function writepixel{T<:Integer}(img::SharedArray{T},i,j,pixelvalue)
	@fastmath @inbounds img[i,j]=round(T,pixelvalue)
end

function writepixel{T<:AbstractFloat}(img::SharedArray{T},i,j,pixelvalue)
   @inbounds img[i,j]=pixelvalue
end

function writepixel{T<:Integer}(img::SharedArray{T},i,j,pixelvalue)
  img[i,j]=round(T,pixelvalue)
end

function writepixel{T<:AbstractFloat}(img::SharedArray{T},i,j,pixelvalue)
  img[i,j]=pixelvalue
end


#sinekernel.jl

n=200
x=(0:.025:1)*3*pi
include("hermitekernel.jl")
K=hermitekernel(n-1,x/sqrt(2*n))*pi/sqrt(2*n)

## Plot
using Winston
module Plot3D
    using Winston
    # Warning: the 3D interface is very new at the time
    # of writing this and is likely to not be available
    # and/or have changed
    if Winston.output_surface == :gtk
        include(Pkg.dir("Winston","src","canvas3d_gtk.jl"))
    else
        include(Pkg.dir("Winston","src","canvas3d.jl"))
    end
end
xx = Float64[xx for xx = x, yy = x]
yy = Float64[yy for xx = x, yy = x]
Plot3D.plot3d(Plot3D.surf(xx,K,yy))

Plot3D.plot3d(Plot3D.surf(
    (u,v)->u,
    (u,v)->sin(u-v)./(u-v),
    (u,v)->v,
    x, x))


@testset "psibound" begin

  @testset "psi_lower" begin
    sp = PrimesSet(2, 100);
    fb = []; for x=sp push!(fb, fmpz(x)); end;
    fb = FactorBase(fb)
    @test length(find(x->issmooth(fb, fmpz(x)), 1:256)) == psi_lower(255, 100)[1][end]
  end
end


export interpolate,solution,mass,momentum,energy,massdiff,momentumdiff,energydiff

"""
    interpolate(mesh,vector;n=2^3)

Interpolate a vector `vector` of values on a uniform collocation grid defined by `mesh`.

Return `(new_mesh,new_vector)` a new uniform mesh with `n` times as many values, and the vector of values at these collocation points.

"""
function interpolate(mesh::Mesh,vector;n=2^3::Int)

    fourier=fft(vector)
    m=Int(mesh.N/2)
    new_fourier = [fourier[1:m] ;zeros((n-1)*2*m) ;fourier[m+1:end]]

    new_mesh = Mesh((xmin=mesh.xmin,xmax=mesh.xmax,N=n*mesh.N))
    new_vector=ifft(new_fourier)*n
    if all((vector[i] isa Real) for i in length(vector))
        new_vector=real.(new_vector)
    end
    return new_mesh,new_vector
end

"""
    interpolate(mesh,vector,x;fast)

Interpolate a vector `vector` of values on a uniform collocation grid defined by `mesh`, on collocation points given by `x`.

If the collocation points `x` are regularly spaced and the optional keyword argument `fast` is set to `true` (default is `false`),
then the algorithm is faster and uses less allocations, but is less precise.

Return the vector of values on collocation points.

"""
function interpolate(mesh::Mesh,vector,x;fast=false)

    fourier=fft(vector)
    k = mesh.k
	x₀ = mesh.xmin
	if fast == false
		#same as new_vector=exp.(1im*(x.-x₀)*k')*fourier/length(k)
		new_vector = similar(x,Complex); z = complex.(zero(k));
		for i in 1:length(x)
			z .= exp.(-1im*(x[i].-x₀)*k)
			new_vector[i] = real(dot(z,fourier))
		end
		new_vector ./= length(k)
	else
        if !(x[2:end].-x[2]≈x[1:end-1].-x[1])
            @error("Collocation points must be equally spaced.")
        end
		new_vector = similar(x,Complex); z = exp.(-1im*(x[1].-x₀)*k);eidk=exp.(-1im*(x[2]-x[1])*k);
		for i in 1:length(x)
			new_vector[i] = real(dot(z,fourier))
			z .*= eidk
		end
		new_vector ./= length(k)
	end

    if all((vector[i] isa Real) for i in length(vector))
        new_vector=real.(new_vector)
    end
    return new_vector
end

"""
    solution(pb::Problem;T,x,interpolation)

Give the solution of a solved initial-value problem at a given time `T`.

# Arguments
- Argument `pb` is of type `Problem`.
- Keyword argument `T` is optional, the last computed time is returned by default.
- Keyword argument `x` is optional, if provided the solution is interpolated to the collocation vector `x`.
- Keyword argument `interpolation` is optional, if an integer is provided the solution is interpolated on as many collocation points (if `true`, then the default value `2^3` is chosen).

# Return values
Return `(η,v,x,t)` where
- `η` is the surface deformation at collocation points;
- `v` is the tangential velocity (derivative of the trace of the velocity potential) at collocation points;
- `x` is the vector of collocation points;
- `t` the time (first computed time greater or equal to provided `T`).

"""
function solution(p::Problem; T=nothing, x=nothing, interpolation = false)
	if isnothing(T) T = p.times.ts[end] end
	T=min(max(T,0),p.times.ts[end])
	index = findfirst(p.times.ts.>=T)
	t=p.times.ts[index]

	if Symbol(typeof(p.model)) == :WaterWaves
		if !isnothing(x) || interpolation != false
			@warn "Cannot interpolate non-regularly spaced mesh."
		end
		(η,v,x) = (p.model.mapfro)(p.data.U[index])

	else
		(η,v,y) = (p.model.mapfro)(p.data.U[index])
		mesh=Mesh(y)
		if isnothing(x)
			x = y
		else
			η = interpolate(mesh,η,x)
			v = interpolate(mesh,v,x)
		end
		if interpolation == true
			new_mesh,η = interpolate(mesh,η)
			new_mesh,v = interpolate(mesh,v)
			x = new_mesh.x
		elseif isa(interpolation,Int)
			new_mesh,η = interpolate(mesh,η;n=interpolation)
			new_mesh,v = interpolate(mesh,v;n=interpolation)
			x = new_mesh.x
		end
	end
	return η,v,x,t
end

"""
    mass(pb::Problem;T)

Compute the excess of mass of a solved initial-value problem `pb` at a given time `T`.

Keyword argument `T` is optional, the last computed time is used by default.

"""
function mass(p::Problem; T=nothing)
	η,v,x = solution(p;T=T)
	if !(x[2:end].-x[2]≈x[1:end-1].-x[1])
		@error("The excess of mass cannot be computed because the solution is defined on a non-regularly spaced mesh.")
	else
		return sum(η)*(x[2]-x[1])
	end
end

"""
    momentum(pb::Problem;T)

Compute the horizontal impulse of a solved initial-value problem `pb` at a given time `T`.

Keyword argument `T` is optional, the last computed time is used by default.

"""
function momentum(p::Problem; T=nothing)
	η,v,x = solution(p;T=T)
	if !(x[2:end].-x[2]≈x[1:end-1].-x[1])
		@error("The horizontal impulse cannot be computed because the solution is defined on a non-regularly spaced mesh.")
	else
		return sum(η.*v)*(x[2]-x[1])
	end
end

"""
    energy(pb::Problem;T)

Compute the excess of mass of a solved initial-value problem `pb` at a given time `T`.

Keyword argument `T` is optional, the last computed time is used by default.

"""
function energy(p::Problem; T=nothing)
	η,v,x = solution(p;T=T)
	if !(x[2:end].-x[2]≈x[1:end-1].-x[1])
		@error("The energy cannot be computed because the solution is defined on a non-regularly spaced mesh.")
	end
	mesh=Mesh(x)
	U=p.model.mapto(Init(mesh,η,v));
	∂ₓ=1im*mesh.k;
	p.model.f!(U);fftm=-U[:,1]./∂ₓ;fftm[1]=0;
	m=real(ifft(fftm));
	@. mesh.dx/2*($sum(η^2) + $sum(v*m))
end

"""
    massdiff(pb::Problem;T,rel)

Compute the difference of excess of mass of a solved initial-value problem `pb` between given time `T` and initial time.

Keyword argument `T` is optional, the last computed time is used by default.

If keyword argument `rel=true` (default is false), then compute the relative difference (with initial value as reference).

"""
function massdiff(p::Problem; T=nothing,rel=false)
	η,v,x = solution(p;T=T)
	η0,v0,x0 = solution(p;T=0)
	if !(x[2:end].-x[2]≈x[1:end-1].-x[1])
		@error("The excess of mass difference cannot be computed because the solution is defined on a non-regularly spaced mesh.")
	else
		if rel==false return sum(η-η0)*(x[2]-x[1]) else return sum(η-η0)/sum(η0) end
	end
end

"""
    momentumdiff(pb::Problem;T,rel)

Compute the difference of horizontal impulse of a solved initial-value problem `pb` between given time `T` and initial time.

Keyword argument `T` is optional, the last computed time is used by default.

If keyword argument `rel=true` (default is false), then compute the relative difference (with initial value as reference).

"""
function momentumdiff(p::Problem; T=nothing,rel=false)
	η,v,x = solution(p;T=T)
	η0,v0,x0 = solution(p;T=0)
	if !(x[2:end].-x[2]≈x[1:end-1].-x[1])
		@error("The horizontal impulse difference cannot be computed because the solution is defined on a non-regularly spaced mesh.")
	else
		if rel==false return sum((η-η0).*v+η0.*(v-v0))*(x[2]-x[1]) else return sum((η-η0).*v+η0.*(v-v0))/sum(η0.*v0) end
	end
end

"""
    energydiff(pb::Problem;T,rel)

Compute the difference of energy of a solved initial-value problem `pb` between given time `T` and initial time.

Keyword argument `T` is optional, the last computed time is used by default.

If keyword argument `rel=true` (default is false), then compute the relative difference (with initial value as reference).

"""
function energydiff(p::Problem; T=nothing, rel=false)
	η,v,x = solution(p;T=T)
	η0,v0,x0 = solution(p;T=0)
	if !(x[2:end].-x[2]≈x[1:end-1].-x[1])
		@error("The energy difference cannot be computed because the solution is defined on a non-regularly spaced mesh.")
	end
	mesh=Mesh(x)
	U=p.model.mapto(Init(mesh,η,v));
	U0=p.model.mapto(Init(mesh,η0,v0));
	∂ₓ=1im*mesh.k;
	p.model.f!(U);fftm=-U[:,1]./∂ₓ;fftm[1]=0;
	p.model.f!(U0);fftm0=-U0[:,1]./∂ₓ;fftm0[1]=0;
	m=real(ifft(fftm));	m0=real(ifft(fftm0));
	if rel == false
		return @. mesh.dx/2*($sum((η-η0)*η+η0*(η-η0)) + $sum((v-v0)*m+v0*(m-m0)))
	else
		return @. ($sum((η-η0)*η+η0*(η-η0)) + $sum((v-v0)*m+v0*(m-m0)))/($sum(η0^2) + $sum(v0*m0))
	end
end


using Soss

μdist = @model ν begin
    s ~ Gamma(ν , ν)
    z ~ Normal()
    return sqrt(s)*z
end

σdist = @model begin
    x ~ Normal()
    return abs(x)
end

m = @model begin
    μ ~ μdist(ν=1.0)
    σ ~ σdist()
    x ~ Normal(μ,σ) |> iid(10)
    return x
end

simulate(μdist(ν=1.0))

simulate(σdist())

simulate(m())

rand(m())

x = randn(2)

xform(m() | (x=x, μ = (z = 1.0,)))

dynamicHMC(m() | (x=x, μ = (z = 1.0,)))

_data = (x=x, μ = (z = 1.0,))

# function show(io::IO, t::NamedTuple)
#     n = nfields(t)
#     for i = 1:n
#         # if field types aren't concrete, show full type
#         if typeof(getfield(t, i)) !== fieldtype(typeof(t), i)
#             show(io, typeof(t))
#             print(io, "(")
#             show(io, Tuple(t))
#             print(io, ")")
#             return
#         end
#     end
#     if n == 0
#         print(io, "NamedTuple()")
#     else
#         typeinfo = get(io, :typeinfo, Any)
#         print(io, "(")
#         for i = 1:n
#             print(io, fieldname(typeof(t),i), " = ")
#             show(IOContext(io, :typeinfo =>
#                            t isa typeinfo <: NamedTuple ? fieldtype(typeinfo, i) : Any),
#                  getfield(t, i))
#             if n == 1
#                 print(io, ",")
#             elseif i < n
#                 print(io, ", ")
#             end
#         end
#         print(io, ")")
#     end
# end


# This file is a part of EuclidianNormalizingFlows.jl, licensed under the MIT License (MIT).

function center_stretch(x::T1, a::T2, b::T3, c::T4) where {T1<:Real,T2<:Real,T3<:Real,T4<:Real}
    R = float(promote_type(T1, T2, T3, T4))
    exp_abs_bx = exp(abs(b * x))
    convert(R, sign(x) * log((sqrt((1 - exp_abs_bx)^2 * exp(2 * b*a) + 4* exp_abs_bx) - (1 - exp_abs_bx) * exp(b*a))/2)/b + c)
end

function center_contract(x::T1, a::T2, b::T3, c::T4) where {T1<:Real,T2<:Real,T3<:Real,T4<:Real}
    R = float(promote_type(T1, T2, T3, T4))
    x_unshifted = x - c
    convert(R, (log(1 + exp(b * (x_unshifted-a))) - log(1 + exp(-b * (x_unshifted+a)))) / b)
end

function center_contract_ladj(x::T1, a::T2, b::T3, c::T4) where {T1<:Real,T2<:Real,T3<:Real,T4<:Real}
    R = float(promote_type(T1, T2, T3, T4))
    x_unshifted = x - c
    dy_dx = 1/(1 + exp(-b * (x_unshifted - a))) + 1/(1 + exp(b * (x_unshifted + a)))
    log(abs(dy_dx))
end

@with_kw struct CenterStretch{T<:Union{Real,AbstractVector{<:Real}}} <: Function
    a::T = 0.0
    b::T = 1.0
    c::T = 0.0
end

@functor CenterStretch

Base.:(==)(a::CenterStretch, b::CenterStretch) = a.a == b.a && a.b == b.b && a.c == b.c
Base.isequal(a::CenterStretch, b::CenterStretch) = isequal(a.a, b.a) && isequal(a.b, b.b) && isequal(a.c, b.c)
Base.hash(x::CenterStretch, h::UInt) = hash(x.c, hash(x.b, hash(x.a, hash(:CenterStretch, hash(:EuclidianNormalizingFlows, h)))))

(f::CenterStretch)(x::Union{Real,AbstractVecOrMat{<:Real}}) = center_stretch.(x, f.a, f.b, f.c)

function with_logabsdet_jacobian(f::CenterStretch, x::Union{Real,AbstractVecOrMat{<:Real}})
    y = f(x)
    neg_ladjs = center_contract_ladj.(y, f.a, f.b, f.c)
    (y, - sum_ladjs(neg_ladjs))
end

inverse(f::CenterStretch) = CenterContract(f.a, f.b, f.c)

@with_kw struct CenterContract{T<:Union{Real,AbstractVector{<:Real}}} <:Function
    a::T = 0.0
    b::T = 1.0
    c::T = 0.0
end

@functor CenterContract

Base.:(==)(a::CenterContract, b::CenterContract) = a.a == b.a && a.b == b.b && a.c == b.c
Base.isequal(a::CenterContract, b::CenterContract) = isequal(a.a, b.a) && isequal(a.b, b.b) && isequal(a.c, b.c)
Base.hash(x::CenterContract, h::UInt) = hash(x.c, hash(x.b, hash(x.a, hash(:CenterContract, hash(:EuclidianNormalizingFlows, h)))))

(f::CenterContract)(x::Union{Real,AbstractVecOrMat{<:Real}}) = center_contract.(x, f.a, f.b, f.c)

function with_logabsdet_jacobian(f::CenterContract, x::Union{Real,AbstractVecOrMat{<:Real}})
    y = f(x)
    ladjs = center_contract_ladj.(x, f.a, f.b, f.c)
    (y, sum_ladjs(ladjs))
end

inverse(f::CenterContract) = CenterStretch(f.a, f.b, f.c)


using MomentBasedEstimators
using Test

facts("Testing basic interface") do
    context("Test from vignette for the R gmm package. Automatic Differentiation") do
        include("normal_dist.jl")
        cft = [3.843, 2.067]
        cfe = coef(step_1)
        sdt = [0.06527666675890574,0.04672629429325811]
        sde = stderror(step_1, TwoStepGMM())
        for j = 1:2
            @test cfe[j] ≈ cft[j] atol = 0.001
            @test sde[j] ≈ sdt[j] atol = 0.00001
        end
        Je, pe = MomentBasedEstimators.J_test(gmm_iid_mgr)
        ## This is the objective value, which is the J-test
        ## for other softwares
        ov     = 1.4398836656920428
        Jt, pt = (1.4378658264483137,0.23048500853597673)
        @test Je ≈ Jt atol = 0.0000001
        @test pe ≈ pt atol = 0.0000001
        @test objval(gmm_iid_mgr) ≈ ov atol = 0.0001
        @test objval(gmm_iid_mgr) < ov
    end

    context("Example 13.5 from Greene (2012) -- verified with Stata") do
        include("gamma_dist.jl")
        cf_stata = [3.358432, .1244622]
        cfe = coef(two_step)
        for j = 1:2
            @test cfe[j] ≈ cf_stata[j] atol=1e-3
        end
        Je, pe = MomentBasedEstimators.J_test(two_step)
        ## The following test is conditional on version.
        ## What changed is the tol for pinv between 0.3 and 0.4
        ##
        if VERSION < v"0.4-"
            Jt, pt = (1.7433378725860427,0.41825292894063326)
        else
            Jt, pt = (3.0875508893757986,0.21357324340764258)
        end
        ## This is the stata J-test
        ov = 1.97522
        @test Je ≈ Jt atol=1e-6
        @test pe ≈ pt atol=1e-6
        @test objval(two_step) ≈ ov atol = 01e-03
        @test objval(two_step) < ov
    end

    context("Instrumental variables -- verified with Stata") do
        include("instrumental_variables.jl")
        cf_stata = [-.0050209, .0708816, .0593661]
        cfe = coef(gmm2s)
        V = vcov(gmm2s)
        for j = 1:2
            @fact cfe[j] --> roughly(cf_stata[j], atol=1e-3)
        end

        V_stata = [ .00478911  -.00272551   .00163441;
                    -.00272551   .01026473  -.00201291;
                    .00163441   -.00201291   .00983372]

        for j = 1:length(V)
            @fact V[j] --> roughly(V_stata[j], atol = 0.01)
        end

        Je, pe = MomentBasedEstimators.J_test(gmm2s)
        ov = 1.191
        @fact objval(gmm2s) --> roughly(ov, atol = 0.02)
    end
    context("Instrumental variables large --- verified by asymptotics") do
        include("instrumental_variables_large.jl")
        @test status(gmm_base)    == :Optimal
        @test status(gmm_one)     == :Optimal
        @test status(el_base)     == :Optimal
        @test status(kl_base)     == :Optimal
        @test status(cue_base)    == :Optimal
        @test status(cue_uncon)   == :Optimal
        @test status(kl_ana_grad) == :Optimal
        @test status(kl_ana_full) == :Optimal

        @test coef(el_base) ≈ coef(kl_base)  atol = 0.01
        @test coef(kl_base) ≈ coef(kl_ana_full)

        @test objval(kl_base) ≈ objval(kl_ana_full)

        @test vcov(kl_base)  ≈ vcov(kl_ana_full)
        @test vcov(kl_base)  ≈ vcov(kl_ana_full)
        @test vcov(gmm_base) ≈ vcov(el_base) atol = 0.01

        @test vcov(el_base, weighted = false)      ≈ vcov(gmm_base) atol = 0.001
        @test vcov(kl_base, weighted = false)      ≈ vcov(gmm_base) atol = 0.001
        @test vcov(kl_ana_grad, weighted = false)  ≈ vcov(gmm_base) atol = 0.001
        @test vcov(kl_ana_full, weighted = false)  ≈ vcov(gmm_base) atol = 0.001
        @test vcov(cue_base, weighted = false)     ≈ vcov(gmm_base) atol = 0.001

        @test vcov(el_base, weighted = true)       ≈ vcov(gmm_base) atol = 0.001
        @test vcov(kl_base, weighted = true)       ≈ vcov(gmm_base) atol = 0.001
        @test vcov(kl_ana_grad, weighted = true)   ≈ vcov(gmm_base) atol = 0.001
        @test vcov(kl_ana_full, weighted = true)   ≈ vcov(gmm_base) atol = 0.001
        @test vcov(cue_base, weighted = true) ≈ vcov(gmm_base) atol =  0.001

        @test stderr(gmm_base)' --> sqrt(vcov(gmm_base))

        @test stderr(kl_base)' --> sqrt(vcov(kl_base))
        @test stderr(el_base)' --> sqrt(vcov(el_base))

        @test stderr(kl_base, weighted = true) --> stderr(kl_base)
        @test stderr(el_base, weighted = true) --> stderr(el_base)

        @test vcov(kl_base_truncated)  --> roughly(vcov(kl_base), 0.01)
        @test vcov(cue_base_truncated) --> roughly(vcov(cue_base), 0.01)

        @test J_test(kl_base_truncated)[1] --> roughly(J_test(kl_base)[1], 0.05)
        @test J_test(cue_base_truncated)[1]  --> roughly(J_test(cue_base)[1], 0.16)
        @test J_test(kl_base_truncated)[2] --> roughly(J_test(kl_base)[2], 0.02)
        @test J_test(cue_base_truncated)[2]  --> roughly(J_test(cue_base)[2], 0.02)

        tmp = J_test(gmm_base)
        @fact tmp[1] --> roughly(7.937738532483664, 1e-07)
        @fact tmp[2] --> roughly(0.5404326890480416, 1e-07)

        tmp = J_test(el_base)
        @fact tmp[1] --> roughly(7.825790062416562, 1e-07)
        @fact tmp[2] --> roughly(0.5517934008321658, 1e-07)

        @fact coeftable(gmm_base).cols[1] --> coef(gmm_base)
        @fact coeftable(gmm_base).cols[2] --> stderr(gmm_base)

        @fact coeftable(el_base).cols[1] --> coef(el_base)
        @fact coeftable(el_base).cols[2] --> stderr(el_base)

    end
    ## context("Instrumental variables large --- verified by asymptotics") do
    ##     Vgmm = vcov(gmm)
    ##     Vmd  = vcov(md)

    ##     for j = 1:length(V)
    ##         @fact Vgmm[j] --> roughly(Vmd[j], atol = 0.00001)
    ##     end

    ##     Vmd_1 = vcov(md, false, :unweighted)
    ##     Vmd_2 = vcov(md, false, :weighted)

    ##     for j = 1:length(V)
    ##         @fact Vmd_1[j] --> roughly(Vmd_2[j], atol = 0.00001)
    ##     end
    ## end
end

## facts("Test utilities") do
##     context("test row_kron") do
##         h = ["a" "b"; "c" "d"]
##         z = ["1" "2" "3"; "4" "5" "6"]
##         want = ["a1" "a2" "a3" "b1" "b2" "b3"; "c4" "c5" "c6" "d4" "d5" "d6"]
##         @fact MomentBasedEstimators.row_kron(h, z) --> want

##         # now test on some bigger matrices
##         a = randn(400, 3)
##         b = randn(400, 5)
##         out = MomentBasedEstimators.row_kron(a, b)
##         @fact size(out) --> (400, 15)

##         rows_good = true
##         for row=1:400
##             rows_good &= out[row, :] == kron(a[row, :], b[row, :])
##         end
##         @fact rows_good --> true
##     end

##     context("test max_args") do
##         foo(x, y, z) = nothing  # standard 3 args
##         bar(x, z=100) = nothing  # standard 2 args with default value
##         baz = (x, y, z)-> nothing  # anonymous 3 args
##         qux(a; b=100) = nothing  # standard 1 with 1 kwarg (kwarg not counted)

##         @fact MomentBasedEstimators.max_args(foo) --> 3
##         @fact MomentBasedEstimators.max_args(bar) --> 2
##         @fact MomentBasedEstimators.max_args(baz) --> 3
##         @fact MomentBasedEstimators.max_args(qux) --> 1
##     end
## end

FactCheck.exitstatus()


immutable HyperbolicIntercept{T} <: Model
    β₀::T
    a::T
    α::T
end

function Base.show(io::IO, ::Type{HyperbolicIntercept})
    return print(io, "Intercept Hyperbolic")
end

function Base.show(io::IO, m::HyperbolicIntercept)
    @printf(
        io,
        "Intercept Hyperbolic (β₀ = %.3f, a = %.3f, α = %.3f)",
        m.β₀,
        m.a,
        m.α,
    )
end

function initial_parameters(::Type{HyperbolicIntercept})
    return Float64[0.0, log(1.0), log(1.0)]
end

canonical_parameters(m::HyperbolicIntercept) = (m.β₀, m.a, m.α)

function HyperbolicIntercept(parameters::Vector)
    β₀ = parameters[1]
    a = exp(parameters[2])
    α = exp(parameters[3])
    return HyperbolicIntercept(β₀, a, α)
end

@inline function predict(
    m::HyperbolicIntercept,
    x2::Real,
    t2::Real,
    x1::Real,
    t1::Real,
)
    β₀, a, α = m.β₀, m.a, m.α
    u1 = x1 * (1 / (1 + α * t1))
    u2 = x2 * (1 / (1 + α * t2))
    z = β₀ + a * (u2 - u1)
    p = invlogit(z)
    return p
end

@inline function gradient_component(
    m::HyperbolicIntercept,
    x2::Real,
    t2::Real,
    x1::Real,
    t1::Real,
)
    β₀, a, α = m.β₀, m.a, m.α
    d_t2 = 1 / (1 + α * t2)
    d_t1 = 1 / (1 + α * t1)
    gr = (
        1.0,
        (x2 * d_t2 - x1 * d_t1) * a,
        a * (x1 * t1 * d_t1^2 - x2 * t2 * d_t2^2) * α,
    )
    return gr
end


# Copyright 2017, Iain Dunning, Joey Huchette, Miles Lubin, and contributors    #src
# This Source Code Form is subject to the terms of the Mozilla Public License   #src
# v.2.0. If a copy of the MPL was not distributed with this file, You can       #src
# obtain one at https://mozilla.org/MPL/2.0/.                                   #src

# # The factory schedule example

# This is a Julia translation of part 5 from "Introduction to to Linear
# Programming with Python"
# available at https://github.com/benalexkeen/Introduction-to-linear-programming
#
# For 2 factories (A, B), minimize the cost of production over the course of 12
# months while meeting monthly demand. Factory B has a planned outage during
# month 5.
#
# It was originally contributed by @Crghilardi.

using JuMP
import GLPK
import Test

function example_factory_schedule()
   ## Sets in the problem:
   months, factories = 1:12, [:A, :B]
   ## This function takes a matrix and converts it to a JuMP container so we can
   ## refer to elements such as `d_max_cap[1, :A]`.
   containerize(A::Matrix) = Containers.DenseAxisArray(A, months, factories)
   ## Maximum production capacity in (month, factory) [units/month]:
   d_max_cap = containerize([
         100000	50000;
         110000	55000;
         120000	60000;
         145000	100000;
         160000	0;
         140000	70000;
         155000	60000;
         200000	100000;
         210000	100000;
         197000	100000;
         80000	120000;
         150000	150000;
   ])
   ## Minimum production capacity in (month, factory) [units/month]:
   d_min_cap = containerize([
         20000	20000;
         20000	20000;
         20000	20000;
         20000	20000;
         20000	0;
         20000	20000;
         20000	20000;
         20000	20000;
         20000	20000;
         20000	20000;
         20000	20000;
         20000	20000;
   ])
   ## Variable cost of production in (month, factory) [$/unit]:
   d_var_cost = containerize([
         10	5;
         11	4;
         12	3;
         9	5;
         8	0;
         8	6;
         5	4;
         7	6;
         9	8;
         10	11;
         8	10;
         8	12
   ])
   ## Fixed cost of production in (month, factory) # [$/month]:
   d_fixed_cost = containerize([
         500	600;
         500	600;
         500	600;
         500	600;
         500	0;
         500	600;
         500	600;
         500	600;
         500	600;
         500	600;
         500	600;
         500	600
   ])
   ## Demand in each month [units/month]:
   d_demand = [
      120_000,
      100_000,
      130_000,
      130_000,
      140_000,
      130_000,
      150_000,
      170_000,
      200_000,
      190_000,
      140_000,
      100_000,
   ]
   ## The model!
   model = Model(GLPK.Optimizer)
   ## Decision variables
   @variables(model, begin
      status[m in months, f in factories], Bin
      production[m in months, f in factories], Int
   end)
   ## The production cannot be less than minimum capacity.
   @constraint(
      model,
      [m in months, f in factories],
      production[m, f] >= d_min_cap[m, f] * status[m, f],
   )
   ## The production cannot be more tha maximum capacity.
   @constraint(
      model,
      [m in months, f in factories],
      production[m, f] <= d_max_cap[m, f] * status[m, f],
   )
   ## The production must equal demand in a given month.
   @constraint(model, [m in months], sum(production[m, :]) == d_demand[m])
   ## Factory B is shut down during month 5, so production and status are both
   ## zero.
   fix(status[5, :B], 0.0)
   fix(production[5, :B], 0.0)
   ## The objective is to minimize the cost of production across all time
   ##periods.
   @objective(
      model,
      Min,
      sum(
         d_fixed_cost[m, f] * status[m, f] + d_var_cost[m, f] * production[m, f]
         for m in months, f in factories
      )
   )
   ## Optimize the problem
   optimize!(model)
   ## Check the solution!
   Test.@testset "Check the solution against known optimal" begin
      Test.@test termination_status(model) == MOI.OPTIMAL
      Test.@test objective_value(model) == 12_906_400.0
      Test.@test value.(production)[1, :A] == 70_000
      Test.@test value.(status)[1, :A] == 1
      Test.@test value.(status)[5, :B] == 0
      Test.@test value.(production)[5, :B] == 0
   end
   println("The production schedule is:")
   println(value.(production))
   return
end

example_factory_schedule()


#==========================================================
This routine initializes an optimization problem to recover
a CNOT gate on a single qudit with 4 energy levels (and 2
guard states). The drift Hamiltonian in the rotating frame
is
              H_0 = - 0.5*ξ_a(a^†a^†aa),
where a is the annihilation operator for the qudit.
Here the control Hamiltonian includes the usual symmetric
and anti-symmetric terms
     H_{sym} = p(t)(a + a^†),    H_{asym} = q(t)(a - a^†)
which come from the rotating frame approximation and hence
we refer to these as "coupled" controls.
The problem parameters are:
                ω_a =  2π × 4.10595   Grad/s,
                ξ_a =  2π × 2(0.1099) Grad/s.
We use Bsplines with carrier waves with frequencies
0, ξ_a, 2ξ_a Grad/s.
==========================================================#
using LinearAlgebra
using Plots
using FFTW
using DelimitedFiles
using Printf
using Ipopt
using Random

Base.show(io::IO, f::Float64) = @printf(io, "%20.13e", f)

using Juqbox # quantum control module

Nosc = 1 # Number of coupled sub-systems = oscillators
N = 4 # Number of essential energy levels

Nguard = 2 # Number of extra guard levels
Ntot = N + Nguard # Total number of energy levels

T = 100.0 # Duration of gate.

# frequencies (in GHz, will be multiplied by 2*pi to get angular frequencies in the Hamiltonian matrix)
fa = 4.10336
xa = 0.2198
rot_freq = [fa] # Used to calculate the lab frame ctrl function

# setup drift Hamiltonian
number = Diagonal(collect(0:Ntot-1))

H0 = -0.5*(2*pi)*xa* (number*number - number) # xa is in GHz

# lowering matrix
amat = Bidiagonal(zeros(Ntot),sqrt.(collect(1:Ntot-1)),:U) # standard lowering matrix
adag = Array(transpose(amat));
Hsym_ops=[Array(amat + adag)]
Hanti_ops=[Array(amat - adag)]
H0 = Array(H0)

# Estimate time step
maxctrl = 0.001*2*pi * 8.5 #  9, 10.5, 12, 15 MHz

nsteps = calculate_timestep(T, H0, Hsym_ops, Hanti_ops, [maxctrl])
println("# time steps: ", nsteps)

Nfreq = 3 # number of carrier frequencies

Nctrl = length(Hsym_ops) # Here, Nctrl = 1
om = zeros(Nctrl,Nfreq)

om[1:Nctrl,2] .= -2.0*pi *xa       # Note negative sign
om[1:Nctrl,3] .= -2.0*pi* 2.0*xa

println("Carrier frequencies [GHz]: ", om[1,:]./(2*pi))

maxamp = zeros(Nfreq)

if Nfreq >= 3
    const_fact = 0.45
    maxamp[1] = maxctrl*const_fact
    maxamp[2:Nfreq] .= maxctrl*(1.0-const_fact)/(Nfreq-1) # split the remainder equally
else
    # same threshold for all frequencies
    maxamp .= maxctrl/Nfreq
end

maxpar = maximum(maxamp)

# Initial basis with guard levels
U0 = initial_cond([N], [Nguard])

# CNOT target
gate_cnot =  zeros(ComplexF64, N, N)
gate_cnot[1,1] = 1.0
gate_cnot[2,2] = 1.0
gate_cnot[3,4] = 1.0
gate_cnot[4,3] = 1.0

# Initial basis with guard levels
U0 = initial_cond([N], [Nguard])

utarget = U0 * gate_cnot

omega1 = Juqbox.setup_rotmatrices([N], [Nguard], [fa])

# Compute Ra*utarget
rot1 = Diagonal(exp.(im*omega1*T))

# target in the rotating frame
vtarget = rot1*utarget

params = Juqbox.objparams([N], [Nguard], T, nsteps, Uinit=U0, Utarget=vtarget, Cfreq=om, Rfreq=rot_freq,
                          Hconst=H0, Hsym_ops=Hsym_ops, Hanti_ops=Hanti_ops)

# initial parameter guess

# D1 smaller than 5 does not work
D1 = 10 # Number of B-spline coefficients per segment
nCoeff = 2*Nctrl*Nfreq*D1 # factor '2' is for sin/cos

# Random.seed!(12456)

startFromScratch = true # false
startFile="cnot-pcof-opt.dat"

# initial parameter guess
if startFromScratch
    pcof0 = maxpar*0.01 * rand(nCoeff)
    println("*** Starting from random pcof with amplitude ", maxpar*0.01)
else
    # use if you want to read the initial coefficients from file
    pcof0 = vec(readdlm(startFile))
    println("*** Starting from B-spline coefficients in file: ", startFile)
    nCoeff = length(pcof0)
    D1 = div(nCoeff, 2*Nctrl*Nfreq)  # number of B-spline coeff per control function
end

# min and max coefficient values
minCoeff, maxCoeff = Juqbox.assign_thresholds_freq(maxamp, Nctrl, Nfreq, D1)

samplerate = 32 # only used for plotting
casename = "cnot1" # base file name (used in optimize-once.jl)

maxIter = 75 # 0  # optional argument
lbfgsMax = 250 # optional argument
ipTol = 1e-5   # optional argument
acceptTol = ipTol # 1e-4 # acceptable tolerance
acceptIter = 15

println("*** Settings ***")
println("System Hamiltonian coefficients [GHz]: (fa, xa) =  ", fa, xa)
println("Total number of states, Ntot = ", Ntot, " Total number of guard states, Nguard = ", Nguard)
println("Using B-spline basis functions with carrier wave, # freq = ", Nfreq)
println("Number of coefficients per spline = ", D1, " Total number of parameters = ", nCoeff)
for q=1:Nfreq
    println("Carrier frequency: ", om[q]/(2*pi), " GHz, max parameter amplitude = ", 1000*maxamp[q]/(2*pi), " MHz")
end
println("Tikhonov coefficients: tik0 = ", params.tik0)

wa = Juqbox.Working_Arrays(params,nCoeff)
prob = Juqbox.setup_ipopt_problem(params, wa, nCoeff, minCoeff, maxCoeff, maxIter=maxIter, lbfgsMax=lbfgsMax, startFromScratch=startFromScratch)

println("Initial coefficient vector stored in 'pcof0'")


function tryincrement(energy :: Vector{Vector{Int}}, x, y)
	if (1 <= x <= length(energy)) && (1 <= y <= length(first(energy)))
		energy[x][y] += 1
	end
end

function part1()
	lines = readlines("inputD11.txt")
	energy = [[parse(Int, i) for i in line] for line in lines]

	flashes = 0
	for step in 1:100
		energy = [[i+1 for i in row] for row in energy]
		flashed = Tuple{Int, Int}[]
		oldflashed = 0
		running = true
		while running
			for (x, row) in enumerate(energy)
				for (y, val) in enumerate(row)
					if val >= 10 && !((x, y) in flashed)
						push!(flashed, (x, y))
						tryincrement(energy, x - 1, y - 1)
						tryincrement(energy, x - 1, y)
						tryincrement(energy, x - 1, y + 1)
						tryincrement(energy, x, y - 1)
						tryincrement(energy, x, y + 1)
						tryincrement(energy, x + 1, y - 1)
						tryincrement(energy, x + 1, y)
						tryincrement(energy, x + 1, y + 1)
					end
				end
			end

			numflashed = length(flashed)

			if numflashed == oldflashed
				running = false
				flashes += numflashed
			else
				oldflashed = numflashed
			end
		end

		for x in 1:length(energy)
			for y in 1:length(energy[x])
				if energy[x][y] >= 10
					energy[x][y] = 0
				end
			end
		end
	end

	return flashes
end

function part2()
	lines = readlines("inputD11.txt")
	energy = [[parse(Int, i) for i in line] for line in lines]

	flashes = 0
	step = 0
	while true
		step += 1
		energy = [[i+1 for i in row] for row in energy]
		flashed = Tuple{Int, Int}[]
		oldflashed = 0
		running = true
		while running
			for (x, row) in enumerate(energy)
				for (y, val) in enumerate(row)
					if val >= 10 && !((x, y) in flashed)
						push!(flashed, (x, y))
						tryincrement(energy, x - 1, y - 1)
						tryincrement(energy, x - 1, y)
						tryincrement(energy, x - 1, y + 1)
						tryincrement(energy, x, y - 1)
						tryincrement(energy, x, y + 1)
						tryincrement(energy, x + 1, y - 1)
						tryincrement(energy, x + 1, y)
						tryincrement(energy, x + 1, y + 1)
					end
				end
			end

			numflashed = length(flashed)

			if numflashed == oldflashed
				running = false
				flashes += numflashed
			else
				oldflashed = numflashed
			end
		end

		for x in 1:length(energy)
			for y in 1:length(energy[x])
				if energy[x][y] >= 10
					energy[x][y] = 0
				end
			end
		end

		if iszero(energy)
			return step
		end
	end
end

println(part1())
println(part2())


####################################################################
# In this example we demonstrate using the Proximal Gradient solver
# with a prox operator that enforces that the real part of the
# eigenvalues alpha is nonpositive.
#
# We test the l2 penalty here (a more or less arbitrary choice)

using RobustDMD

T = Float32

# using PyPlot
#--------------------------------------------------------------------
# Generate DMD Synthetic Data
#--------------------------------------------------------------------
println("generate synthetic data...")
# dimensions
m = 100;    # temporal dimension
n = 100;    # spatial dimension
h = n; # h is not used by this solver (only Trim)
k = 3;      # number of modes
# generate data
sigma = T(1e-4); # size of background noise
mu = T(0.0); # size of spikes (set to zero here for l2 fitting example)
p = T(0.1); # frequency of spikes (percentage of corrupted entries)
X, Xclean, t, alphat, betat = genDMD(m, n, k, sigma, mu; seed=123456, p=p);

#--------------------------------------------------------------------
# Initial Guess
#--------------------------------------------------------------------
println("initialize alpha and B...")
# create initial guess using trap. rule with exact dmd
alpha0, B0 = dmdexactestimate(m,n,k,X,t);

#--------------------------------------------------------------------
# Proximal Gradient L2
#--------------------------------------------------------------------
println("Proximal Gradient l2 loss experiment...")
# loss functions
lossf = (z) -> l2_func(z);
lossg = (z) -> l2_grad!(z);
l2Params = DMDParams(k, X, t, lossf, lossg);
l2DMD = DMDVariables(alpha0, B0, l2Params);
# wrapper: sets up inner solve specialized for l2
l2Svars = DMDVPSolverVariablesLSQ(l2Params)

# prox function, projects to real part <= 0.0
function prox_mr(αr)
    k = length(αr) >> 1;
    for i = 1:k
        αr[i<<1-1] = min(T(0.0), αr[i<<1-1]);
    end
end
options = PG_options(1000,T(1e-6),true,true,10,prox_mr);

# apply solver
stats = PG_solve_DMD!(l2DMD, l2Params, l2Svars, options);

# check performance
err0 = besterrperm(alpha0, alphat);
err1 = besterrperm(l2DMD.alpha, alphat);

println("err exact DMD ..... ", err0)
println("err l2 fit ........ ", err1)


using Test
using OMEinsum
using CuArrays

CuArrays.allowscalar(false)

@testset "cuda einsum" begin
    a = [randn(fill(3, i)...) for i=1:4]
    ca = a .|> CuArray
    ixs = ((1,2), (2,3))
    xs = (ca[2], ca[2])
    @test loop_einsum!(EinCode(ixs, (1,3)), xs, zeros(3,3) |> CuArray, OMEinsum.get_size_dict(ixs, xs)) ≈ ca[2]*ca[2]
    for f in [ein"ij,jk->ik", ein"ii->", ein"ijj ->i", ein"ij,ik,il->jkl", ein"ii->i", ein"ijl->i", ein"i->ii", ein"ij,jk,kl->il", ein"ij,ij,ij -> ij"]
        @show f
        cins = map(ix->ca[length(ix)], OMEinsum.getixs(f))
        ins = map(ix->a[length(ix)], OMEinsum.getixs(f))
        @test f(cins...) isa CuArray
        @test f(ins...) ≈ f(cins...)
    end
end

@testset "fallback - getindex IR error" begin
    a = rand(ComplexF64,2,2,2)
    ca = CuArray(a);
    @test Array(ein"npu,por,dom,lmn -> urdl"(ca,ca,ca,ca)) ≈ ein"npu,por,dom,lmn -> urdl"(a,a,a,a)
end

@testset "isbatchmul" begin
    for (ixs, iy) in [(((1,2), (2,3)), (1,3)), (((1,2,3), (2,3)), (1,3)),
                        (((7,1,2,3), (2,4,3,7)), (1,4,3)),
                        (((3,), (3,)), (3,)), (((3,1), (3,)), (3,1))
                        ]
        xs = ([randn(ComplexF64, fill(4,length(ix))...) |> CuArray for ix in ixs]...,)
        @test OMEinsum.batched_contract(ixs[1], xs[1], ixs[2], xs[2], iy) |> Array ≈ loop_einsum(EinCode(ixs, iy), xs, OMEinsum.get_size_dict(ixs, xs))
        @test EinCode(ixs, iy)(xs...) |> Array ≈ loop_einsum(EinCode(ixs, iy), xs, OMEinsum.get_size_dict(ixs, xs))
    end
end


struct ClusterTopKBatchQs <: BatchPQs
    model::SVDD.OCClassifier
    sequentialQs::SequentialPQs
    k::Int

    function ClusterTopKBatchQs(model::SVDD.OCClassifier, sequential_strategy::SequentialPQs; k::Int)::ClusterTopKBatchQs
        (k < 1) && throw(ArgumentError("Invalid batch size k=$(k)."))
        return new(model, sequential_strategy, k)
    end
end

"""
Ensures selection of an informative, representative and diverse batch
by clustering the most informative observations and then using the k
cluster centers as batch query.

Number of most informative observations selected = 10*batch_size,
Number of clusters computed = batch_size
"""
function select_batch(qs::ClusterTopKBatchQs, x::Array{T, 2}, labels::Dict{Symbol, Vector{Int}}, candidate_indices::Vector{Int})::Vector{Int} where T <: Real
    num_observations = length(candidate_indices)
    if num_observations <= qs.k
        return candidate_indices
    end

    m = min(num_observations, 10 * qs.k)

    candidate_scores = qs_score(qs.sequentialQs, x, labels)[candidate_indices]
    descending_indices = sortperm(candidate_scores; rev=true)
    best_m = x[:, candidate_indices[descending_indices[1:m]]]

    distances = Distances.pairwise(Distances.Euclidean(), best_m, dims=2)
    clustering = kmedoids(distances, qs.k)

    medoid_indices = clustering.medoids

    return return candidate_indices[descending_indices[1:m]][medoid_indices]
end


using Distributions
using Base.Test

for d in (Truncated(Normal(0, 1), -1, 1),
          Truncated(Normal(3, 10), 7, 8),
          Truncated(Normal(-5, 1), -Inf, -10))
    @test all(insupport(d, rand(d, 1000)))
end

d = Truncated(Normal(0, 1), -0.1, +0.1)

@test pdf(d, 0.0) > pdf(Normal(0, 1), 0.0)
@test pdf(d, -1.0) == 0.0
@test pdf(d, +1.0) == 0.0

@test logpdf(d, 0.0) > logpdf(Normal(0, 1), 0.0)
@test isinf(logpdf(d, -1.0))
@test isinf(logpdf(d, +1.0))

@test cdf(d, -1.0) == 0.0
@test cdf(d, -0.09) < cdf(Normal(0, 1), -0.09)
@test cdf(d, 0.0) == 0.5
@test cdf(d, +0.09) > cdf(Normal(0, 1), +0.09)
@test cdf(d, +1.0) == 1.0

@test quantile(d, 0.01) > -0.1
@test abs(quantile(d, 0.5) - 0.0) < 1e-8
@test quantile(d, 0.99) < +0.1

@test abs(cdf(d, quantile(d, 0.01)) - 0.01) < 1e-8
@test abs(cdf(d, quantile(d, 0.50)) - 0.50) < 1e-8
@test abs(cdf(d, quantile(d, 0.99)) - 0.99) < 1e-8


# Contains script to run SIRSDE model using command line.

using LinearAlgebra
using StochasticDiffEq
using Base: @kwdef

using Random
using NPZ
using ArgParse

# Notation
# s: Susceptible
# i: Infected
# r: Removed/recovered/dead
# β: Infection rate
# γ: recovery rate
# R₀: infection_rate/recovery rate  (i.e. β/γ)
# σ: Controls volitility of R₀
# η: Controls mean reversion strength of R₀

"""
SIR model described using stochastic differential equations.
Ref: Inspired by https://julia.quantecon.org/continuous_time/covid_sde.html
"""
@kwdef struct SIRSDETask
    "Volatitility of R₀."
    σ::Float64 = 0.05
    "Mean reversion strength of R₀."
    η::Float64 = 0.05
    "Number of time steps."
    T::Int = 365
    "Number of individuals"
    N::Int = 100000
    "Initial proportions of [s,i,r]."
    sir_0::Vector{Float64} = [0.999, 0.001, 0]
    "Deterministic portion of SDE."
    f::Function = _sir_f!
    "Stochastic portion of SDE."
    g::Function = _sir_g!
    "Parameter names."
    θ_names::Vector{String} = ["β", "γ"]
    name::String = "SIRSDE"
end

"""
Deterministic portion of SDE (s, i, r are described using proportions).
"""
function _sir_f!(du, u, p, t)
    s, i, r, R₀ = u
    (; γ, R̄₀, η, σ) = p
    β = γ * R₀
    du[1] = -β * s * i        # ds/dt
    du[2] = β * s * i - γ * i   # di/dt
    du[3] = γ * i           # dr/dt
    du[4] = η * (R̄₀(t, p) - R₀) # dR₀/dt
    return nothing
end

"""
Stochastic portion of SDE (R₀ volatitility)
"""
function _sir_g!(du, u, p, t)
    s, i, r, R₀ = u
    (; γ, R̄₀, η, σ) = p
    du[1:3] .= 0
    du[4] = σ * sqrt(abs(R₀))
    return nothing
end

"""
Batched simulations using SIR model. θ has columns [β γ].
"""
function simulate(
    task::SIRSDETask,
    θ::AbstractMatrix{Float64})
    @assert all(θ .>= 0)
    (; σ, η, T, sir_0, f, g, N) = task

    x = Array{Float64}(undef, size(θ, 1), T)  # To store infection data
    for (θᵢ, xᵢ) in zip(eachrow(θ), eachrow(x))
        β, γ = θᵢ
        p = (
            R̄₀_ref=β / γ,  # Fixed reference (R̄₀ must be a function)
            R̄₀=(t, p) -> p.R̄₀_ref,
            γ=γ,
            σ=σ,
            η=η,
        )
        u_0 = [sir_0; β / γ]
        prob = SDEProblem(f, g, u_0, (0, T), p)
        sol = solve(prob, SOSRI())
        xᵢ .= [sol(t)[2] for t in 0:(T-1)]
    end
    return x .* N
end

function parse_commandline()
    s = ArgParseSettings()
    @add_arg_table s begin
        "--seed"
        help = "Seed for simulations"
        arg_type = Int
        required = true

        "--theta_path"
        help = "Path to prior samples (npz file) to use for simulations"
        arg_type = String
        required = true

        "--output_path"
        help = "Path to output raw simulations"
        arg_type = String
        required = true
    end
    return parse_args(s)
end

function main()
    args = parse_commandline()
    Random.seed!(args["seed"])
    θ = npzread(args["theta_path"])["theta"]
    θ = convert(Matrix{Float64}, θ)
    task = SIRSDETask()
    x = simulate(task, θ)
    npzwrite(args["output_path"], x=x)
end

main()


using ModelingToolkit: operation, istree, arguments
# Method of lines discretization scheme
struct MOLFiniteDifference{T,T2} <: DiffEqBase.AbstractDiscretization
    dxs::T
    time::T2
    upwind_order::Int
    centered_order::Int
end

# Constructors. If no order is specified, both upwind and centered differences will be 2nd order
MOLFiniteDifference(dxs, time; upwind_order = 1, centered_order = 2) =
    MOLFiniteDifference(dxs, time, upwind_order, centered_order)

function SciMLBase.symbolic_discretize(pdesys::ModelingToolkit.PDESystem,discretization::DiffEqOperators.MOLFiniteDifference)

    pdeeqs = pdesys.eqs isa Vector ? pdesys.eqs : [pdesys.eqs]
    t = discretization.time
    nottime = filter(x->~isequal(x.val, t.val),pdesys.indvars)
    nspace = length(nottime)
    depvars = pdesys.depvars

    # Discretize space
    space = map(nottime) do x
        xdomain = pdesys.domain[findfirst(d->isequal(x.val, d.variables),pdesys.domain)]
        @assert xdomain.domain isa IntervalDomain
        dx = discretization.dxs[findfirst(dxs->isequal(x.val, dxs[1].val),discretization.dxs)][2]
        dx isa Number ? (xdomain.domain.lower:dx:xdomain.domain.upper) : dx
    end
    # Get tspan
    tdomain = pdesys.domain[findfirst(d->isequal(t.val, d.variables),pdesys.domain)]
    @assert tdomain.domain isa IntervalDomain
    tspan = (tdomain.domain.lower,tdomain.domain.upper)

    # Build symbolic variables
    indices = CartesianIndices(((axes(s)[1] for s in space)...,))
    depvarsdisc = map(depvars) do u
        [Num(Variable{Symbolics.FnType{Tuple{Any}, Real}}(Base.nameof(ModelingToolkit.operation(u.val)),II.I...))(t) for II in indices]
    end
    spacevals = map(y->[Pair(nottime[i],space[i][y.I[i]]) for i in 1:nspace],indices)

    ### INITIAL AND BOUNDARY CONDITIONS ###
    # Build symbolic maps for boundaries
    edges = reduce(vcat,[[vcat([Colon() for j in 1:i-1],1,[Colon() for j in i+1:nspace]),
      vcat([Colon() for j in 1:i-1],size(depvarsdisc[1],i),[Colon() for j in i+1:nspace])] for i in 1:nspace])

    #edgeindices = [indices[e...] for e in edges]
    edgevals = reduce(vcat,[[nottime[i]=>first(space[i]),nottime[i]=>last(space[i])] for i in 1:length(space)])
    edgevars = [[d[e...] for e in edges] for d in depvarsdisc]

    bclocs = map(e->substitute.(pdesys.indvars,e),edgevals) # location of the boundary conditions e.g. (t,0.0,y)
    edgemaps = Dict(bclocs .=> [spacevals[e...] for e in edges])
    initmaps = substitute.(depvars,[t=>tspan[1]])

    # Generate map from variable (e.g. u(t,0)) to discretized variable (e.g. u₁(t))
    subvar(depvar) = substitute.((depvar,),edgevals)
    # depvarbcmaps will dictate what to replace the variable terms with in the bcs
    depvarbcmaps = reduce(vcat,[subvar(depvar) .=> edgevar for (depvar, edgevar) in zip(depvars, edgevars)])
    # depvarderivbcmaps will dictate what to replace the Differential terms with in the bcs
    if nspace == 1
        # 1D system
        left_weights(j) = DiffEqOperators.calculate_weights(discretization.upwind_order, space[j][1], space[j][1:2])
        right_weights(j) = DiffEqOperators.calculate_weights(discretization.upwind_order, space[j][end], space[j][end-1:end])
        central_neighbor_idxs(i,j) = [i+CartesianIndex([ifelse(l==j,-1,0) for l in 1:nspace]...),i,i+CartesianIndex([ifelse(l==j,1,0) for l in 1:nspace]...)]
        left_idxs = central_neighbor_idxs(CartesianIndex(2),1)[1:2]
        right_idxs(j) = central_neighbor_idxs(CartesianIndex(length(space[j])-1),1)[end-1:end]
        # Constructs symbolic spatially discretized terms of the form e.g. au₂ - bu₁
        derivars = [[dot(left_weights(1),depvar[left_idxs]), dot(right_weights(1),depvar[right_idxs(1)])]
                    for depvar in depvarsdisc]
        # Create list of all the symbolic Differential terms evaluated at boundary e.g. Differential(x)(u(t,0))
        subderivar(depvar,s) = substitute.((Differential(s)(depvar),),edgevals)
        # Create map of symbolic Differential terms with symbolic spatially discretized terms
        depvarderivbcmaps = reduce(vcat,[subderivar(depvar, s) .=> derivars[i]
                                       for (i, depvar) in enumerate(depvars) for s in nottime])
   else
        # Higher dimension
        # TODO: Fix Neumann and Robin on higher dimension
        depvarderivbcmaps = []
   end

    # Generate initial conditions and bc equations
    u0 = []
    bceqs = []
    for bc in pdesys.bcs
        if ModelingToolkit.operation(bc.lhs) isa Sym && ~any(x -> isequal(x, t.val), ModelingToolkit.arguments(bc.lhs))
            # initial condition
            # Assume in the form `u(...) ~ ...` for now
            push!(u0,vec(depvarsdisc[findfirst(isequal(bc.lhs),initmaps)] .=> substitute.((bc.rhs,),spacevals)))
        else
            # Algebraic equations for BCs
            i = findfirst(x->occursin(x.val,bc.lhs),first.(depvarbcmaps))
            bcargs = first(depvarbcmaps[i]).val.arguments
            # Replace Differential terms in the bc lhs with the symbolic spatially discretized terms
            # TODO: Fix Neumann and Robin on higher dimension
            lhs = nspace == 1 ? substitute(bc.lhs,depvarderivbcmaps[i]) : bc.lhs

            # Replace symbol in the bc lhs with the spatial discretized term
            lhs = substitute(lhs,depvarbcmaps[i])
            rhs = substitute.((bc.rhs,),edgemaps[bcargs])
            lhs = lhs isa Vector ? lhs : [lhs] # handle 1D
            push!(bceqs,lhs .~ rhs)
        end
    end

    u0 = reduce(vcat,u0)
    bceqs = reduce(vcat,bceqs)

    ### PDE EQUATIONS ###
    interior = indices[[2:length(s)-1 for s in space]...]
    eqs = vec(map(Base.product(interior,pdeeqs)) do p
        II,eq = p

        # Create a stencil in the required dimension centered around 0
        # e.g. (-1,0,1) for 2nd order, (-2,-1,0,1,2) for 4th order, etc
        order = discretization.centered_order
        stencil(j) = CartesianIndices(Tuple(map(x -> -x:x, (1:nspace.==j) * (order÷2))))

        # TODO: Generalize central difference handling to allow higher even order derivatives
        # The central neighbour indices should add the stencil to II, unless II is too close
        # to an edge in which case we need to shift away from the edge
        # Calculate buffers
        I1 = oneunit(first(indices))
        Imin = first(indices) + I1 * (order÷2)
        Imax = last(indices) - I1 * (order÷2)
        # Use max and min to apply buffers
        central_neighbor_idxs(II,j) = stencil(j) .+ max(Imin,min(II,Imax))
        central_neighbor_space(II,j) = vec(space[j][map(i->i[j],central_neighbor_idxs(II,j))])
        central_weights(II,j) = DiffEqOperators.calculate_weights(2, space[j][II[j]], central_neighbor_space(II,j))
        central_deriv(II,j,k) = dot(central_weights(II,j),depvarsdisc[k][central_neighbor_idxs(II,j)])

        central_deriv_rules = [(Differential(s)^2)(u) => central_deriv(II,j,k) for (j,s) in enumerate(nottime), (k,u) in enumerate(depvars)]
        valrules = vcat([depvars[k] => depvarsdisc[k][II] for k in 1:length(depvars)],
                        [nottime[j] => space[j][II[j]] for j in 1:nspace])

        # TODO: Use rule matching for nonlinear Laplacian

        # TODO: upwind rules needs interpolation into `@rule`
        #forward_weights(i,j) = DiffEqOperators.calculate_weights(discretization.upwind_order, 0.0, [space[j][i[j]],space[j][i[j]+1]])
        #reverse_weights(i,j) = DiffEqOperators.calculate_weights(discretization.upwind_order, 0.0, [space[j][i[j]-1],space[j][i[j]]])
        #upwinding_rules = [@rule(*(~~a,(Differential(nottime[j]))(u),~~b) => IfElse.ifelse(*(~~a..., ~~b...,)>0,
        #                        *(~~a..., ~~b..., dot(reverse_weights(i,j),depvarsdisc[k][central_neighbor_idxs(i,j)[1:2]])),
        #                        *(~~a..., ~~b..., dot(forward_weights(i,j),depvarsdisc[k][central_neighbor_idxs(i,j)[2:3]]))))
        #                        for j in 1:nspace, k in 1:length(depvars)]

        rules = vcat(vec(central_deriv_rules),valrules)
        substitute(eq.lhs,rules) ~ substitute(eq.rhs,rules)
    end)

    # Finalize
    defaults = pdesys.ps === nothing || pdesys.ps === SciMLBase.NullParameters() ? u0 : vcat(u0,pdesys.ps)
    ps = pdesys.ps === nothing || pdesys.ps === SciMLBase.NullParameters() ? Num[] : first.(pdesys.ps)
    # Combine PDE equations and BC equations
    sys = ODESystem(vcat(eqs,unique(bceqs)),t,vec(reduce(vcat,vec(depvarsdisc))),ps,defaults=Dict(defaults))
    sys, tspan
end

function SciMLBase.discretize(pdesys::ModelingToolkit.PDESystem,discretization::DiffEqOperators.MOLFiniteDifference)
    sys, tspan = SciMLBase.symbolic_discretize(pdesys,discretization)
    simpsys = structural_simplify(sys)
    prob = ODEProblem(simpsys,Pair[],tspan)
end


const sf = ReMat(ds[:Batch])
# check alternative constructors
@test sf == ReMat(ds[:Batch],ones(length(ds[:Batch])))

const Yield = convert(Array,ds[:Yield])

# check size methods
@test size(sf) == (30, 6)
@test size(sf,1) == 30
@test size(sf,2) == 6
@test size(sf,3) == 1

dd = fill(5., 6)
@test sf'ones(30) == dd

@test ones(30)'sf == dd'
const crp = sf'sf
@test isa(crp, Diagonal{Float64})
const crp1 = copy(crp)
@test crp1 == crp
@test crp[2,6] == 0
@test crp[6,6] == 5
@test size(crp) == (6,6)
@test crp.diag == fill(5.,6)
const rhs = sf'Yield
@test rhs == [7525.0,7640.0,7820.0,7490.0,8000.0,7350.0]
@test A_ldiv_B!(crp,copy(rhs)) == [1505.,1528.,1564.,1498.,1600.,1470.]

const L = ReTerms.LT(sf)
L[:θ] = [0.5]

@test isa(scale!(L,crp),Diagonal)
@test crp.diag == fill(2.5,6)
@test copy!(crp1,crp) == crp

const sf1 = ReMat(psts[:Sample])
const sf2 = ReMat(psts[:Batch])
@test size(sf1) == (60,30)
@test size(sf2) == (60,10)

const crp11 = sf1'sf1
const pr21 = sf2'sf1
const crp22 = sf2'sf2

@test isa(crp11,Diagonal{Float64})
@test isa(crp22,Diagonal{Float64})
@test isa(pr21,SparseMatrixCSC{Float64})


#=
ported from:
COTD Entry submitted by John W. Ratcliff [jratcliff@verant.com]

THIS IS A CODE SNIPPET WHICH WILL EFFICIEINTLY TRIANGULATE ANY
POLYGON/CONTOUR (without holes) AS A STATIC CLASS.  THIS SNIPPET
IS COMPRISED OF 3 FILES, TRIANGULATE.H, THE HEADER FILE FOR THE
TRIANGULATE BASE CLASS, TRIANGULATE.CPP, THE IMPLEMENTATION OF
THE TRIANGULATE BASE CLASS, AND TEST.CPP, A SMALL TEST PROGRAM
DEMONSTRATING THE USAGE OF THE TRIANGULATOR.  THE TRIANGULATE
BASE CLASS ALSO PROVIDES TWO USEFUL HELPER METHODS, ONE WHICH
COMPUTES THE AREA OF A POLYGON, AND ANOTHER WHICH DOES AN EFFICENT
POINT IN A TRIANGLE TEST.
SUBMITTED BY JOHN W. RATCLIFF (jratcliff@verant.com) July 22, 2000

 **********************************************************************/
 ************ HEADER FILE FOR TRIANGULATE.H ***************************/
 **********************************************************************/
 *****************************************************************/
 ** Static class to triangulate any contour/polygon efficiently **/
 ** You should replace Vector2d with whatever your own Vector   **/
 ** class might be.  Does not support polygons with holes.      **/
 ** Uses STL vectors to represent a dynamic array of vertices.  **/
 ** This code snippet was submitted to FlipCode.com by          **/
 ** John W. Ratcliff (jratcliff@verant.com) on July 22, 2000    **/
 ** I did not write the original code/algorithm for this        **/
 ** this triangulator, in fact, I can't even remember where I   **/
 ** found it in the first place.  However, I did rework it into **/
 ** the following black-box static class so you can make easy   **/
 ** use of it in your own code.  Simply replace Vector2d with   **/
 ** whatever your own Vector implementation might be.           **/
 *****************************************************************/
=#

function area{N, T}(contour::AbstractVector{Point{N, T}})
    n = length(contour)
    A = zero(T)
    p=n; q=1
    while q <= n
        A += cross(contour[p], contour[q])
        p = q; q +=1
    end
    return A*T(0.5)
end

"""
 InsideTriangle decides if a point P is Inside of the triangle
 defined by A, B, C.
"""
function InsideTriangle{T<:Point}(A::T, B::T, C::T, P::T)
    a = C-B; b = A-C; c = B-A
    ap = P-A; bp = P-B; cp = P-C
    a_bp = a[1]*bp[2] - a[2]*bp[1];
    c_ap = c[1]*ap[2] - c[2]*ap[1];
    b_cp = b[1]*cp[2] - b[2]*cp[1];
    t0 = zero(eltype(T))
    return ((a_bp >= t0) && (b_cp >= t0) && (c_ap >= t0))
end

function snip{N, T}(
        contour::AbstractVector{Point{N, T}}, u, v, w, n, V
    )
    A = contour[V[u]]
    B = contour[V[v]]
    C = contour[V[w]]
    x = (
        ((B[1]-A[1])*(C[2]-A[2])) -
        ((B[2]-A[2])*(C[1]-A[1]))
    )
    if 0.0000000001f0 > x
        return false
    end

    for p=1:n
        ((p == u) || (p == v) || (p == w)) && continue;
        P = contour[V[p]]
        if InsideTriangle(A, B, C, P)
            return false;
        end
    end
    return true;
end

"""
Triangulates a Polygon given as a `contour`::AbstractArray{Point} without holes.
It will return a Vector{`facetype`}, defining indexes into `contour`
"""
function polygon2faces{P<:Point}(
        contour::AbstractArray{P}, facetype=GLTriangle
    )
    #= allocate and initialize list of Vertices in polygon =#
    result = facetype[]

    n = length(contour)
    if n < 3
        error("Not enough points in the contour. Found: $contour")
    end
    #= we want a counter-clockwise polygon in V =#
    if 0 < area(contour)
        V = Int[i for i=1:n]
    else
        V = Int[i for i=n:-1:1]
    end

    nv = n

    #=  remove nv-2 Vertices, creating 1 triangle every time =#
    count = 2*nv   #= error detection =#
    v=nv
    while nv>2
        #= if we loop, it is probably a non-simple polygon =#
        if 0 >= count
            return result
        end
        count -= 1

        #= three consecutive vertices in current polygon, <u,v,w> =#
        u = v; (u > nv) && (u = 1) #= previous =#
        v = u+1; (v > nv) && (v = 1) #= new v =#
        w = v+1; (w > nv) && (w = 1) #= next =#
        if snip(contour, u, v, w, nv, V)
            #= true names of the vertices =#
            a = V[u]; b = V[v]; c = V[w];
            #= output Triangle =#
            push!(result, facetype(Triangle{Int}(a, b, c)))
            #= remove v from remaining polygon =#
            s = v; t = v+1
            while t<=nv
                V[s] = V[t]
                s += 1; t += 1
            end
            nv -= 1
            #= resest error detection counter =#
            count = 2*nv
        end
    end

    return result
end

function topoint{T}(::Type{Point{3, T}}, p::Point{2, T})
    Point{3, T}(p[1], p[2], T(0))
end
function topoint{T}(::Type{Point{3, T}}, p::Point{3, T})
    p
end
function topoint{T}(::Type{Point{2, T}}, p::Point{3, T})
    Point{2, T}(p[1], p[2])
end

@compat function (::Type{M}){M<:AbstractMesh, P<:Point}(
        points::AbstractArray{P}
    )
    faces = polygon2faces(points, facetype(M))
    VT = vertextype(M)
    GLNormalMesh(faces=faces, vertices=VT[topoint(VT, p) for p in points])
end


using CubicEoS
using LinearAlgebra

function quadratic(M::AbstractMatrix)
    @assert isposdef(M)
    @assert abs(det(M)) > 0

    F(x) = x' * M * x
    dF!(g, x) = begin g .= (M' + M) * x; g end
    ddF!(H, x) = begin H .= (M' + M); H end

    return F, dF!, ddF!
end

function quadratic(M::AbstractMatrix, b::AbstractVector)
    @assert isposdef(M)
    @assert abs(det(M)) > 0

    F(x) = x' * M * x - 2 * x' * b
    dF!(g, x) = begin g .= (M' + M) * x .- 2 .* b; g end
    ddF!(H, x) = begin H .= (M' + M); H end

    return F, dF!, ddF!
end

function system_test(M::AbstractMatrix, b::AbstractVector)
    f, df, ddf = quadratic(M, b)
    x₀ = 1000 .* rand(size(M, 1))
    optresult = CubicEoS.newton(f, df, ddf, x₀;
        maxiter=100,
        gtol=1e-3,
        constrain_step=(x, δ)->1.0,
    )
    dump(optresult)
    @show norm(optresult.argument, 2)

    @show f(optresult.argument)

    @show M \ b
    @show f(M \ b)
end

function quadratic_test(M::AbstractMatrix)
    f, df, ddf = quadratic(M)

    x₀ = 1000 .* rand(size(M, 1))
    optresult = CubicEoS.newton(f, df, ddf, x₀;
        maxiter=100,
        gtol=1e-6,
        constrain_step=(x, δ)->1.0,
    )
    dump(optresult)
    @show norm(optresult.argument, 2)
end

function quadratic_test(n::Integer=20)
    A = rand(n, n)
    return quadratic_test(A*A')
end

# quadratic_test(40)
# let A = [1 2; 4 5]
#     system_test(A*A', [1, 1])
# end
let n = 10, A = rand(n, n)
    system_test(A*A', rand(n))
end


#   Unit "filters" of the FourierAnalysis Package for julia language
#
#   MIT License
#   Copyright (c) 2019-2022,
#   Marco Congedo, CNRS, Grenobe, France:
#   https://sites.google.com/site/marcocongedo/home

# ? CONTENTS :
#   This unit implements filtering procedures interfacing the DSP.jl package

FilterDesign = Union{ZeroPoleGain, FIRWindow}

#######################################################################
"""
```julia
function filterbank(x         :: Vector{T},
                    sr        :: Int,
                    bandwidth :: IntOrReal    = 2;
                filtkind      :: FilterDesign = Butterworth(2),
                fmin          :: IntOrReal    = bandwidth,
                fmax          :: IntOrReal    = sr÷2,
                ⏩           :: Bool         = true) where T<:Real
```

Pass signal vector `x` throught a bank of band-pass filters,
given sampling rate `sr` and `bandwidth` parameter.

The kind of filters is specified by optional keyword argument `filtkind`,
of type [FilterDesign](@ref), using the DSP package.
By default `filtkind` is a forward-backward (linear phase response)
Butterworth IIR filter of order 2 in each direction
(hence, 4th order total). See [notes on DSP package useful functions](@ref)
for tips on how to design other IIR and FIR filters.

Return 2-tuple `(f, Y)`, where `f` is a vector holding the center
frequencies of the filter bank band-pass regions
and `Y` a matrix holding in the ``i^{th}`` column the signal `x`
band-pass iltered by the ``i^{th}`` band-pass filter.
Hence, ```size(Y, 1)=length(x)``` and ```size(Y, 2)=length(f)```.

The filter bank is designed by means of argument `bandwidth`
and optional keyword arguments `fmin` and `fmax`.
These three arguments can be passed either as integers or real numbers.
All band-pass regions have bandwidth equal to the `bandwidth` argument
and overlap with adjacent band-pass regions.
By default the lower limit of the first band-pass region is set at
`bandwidth` Hz, and successive band-pass regions are defined up to,
but excluding, the Nyquist frequency (``sr/2``).
If `fmin` is specified (in Hz), the center frequency of the first band-pass
region is set as close as possible, but not below, `fmin`.
If `fmax` is specified (in Hz), the center frequency of the last band-pass
region is set as close as possible, but not above, `fmax`.

Here are some examples of filter bank definitions given different arguments
(`sr`=16 in all examples).

| bandwidth | fmin | fmax | center frequencies |       band-pass regions       |
|:---------:|:----:|:----:|:------------------:|:-----------------------------:|
|     4     |   -  |  -   |         4          |              2-6              |
|     2     |   -  |  -   | 2, 3, 4, 5, 6      |    1-3, 2-4, 3-5, 4-6, 5-7    |
|     2     |   3  |  7   | 3, 4, 5, 6         |    2-4, 3-5, 4-6, 5-7         |
|     1     |   3  |  5   | 3, 3.5, 4, 4.5, 5  | 2.5-3.5, 3-4, 3.5-4.5, 4-5, 4.5-5.5 |
|    1.1    |   3  |  5   | 2.75, 3.3, 3.85, 4.4, 4.95 | 2.2-3.3, 2.75-8.85, 3.3-4.4, 3.85-4.95, 4.4-5.5 |
|    1.9    |   3  |  5   | 2.85, 3.8, 4.75    | 1.9-3.8, 2.85-4.75, 3.8-5.7 |

!!! note "Nota Bene"
    At least `sr` samples should be trimmed at the beginning and end of the
    output signal `Y`, as those samples are severely distorted by the filtering
    process.

    If keyword optional argument ⏩ is true (default), the filtering is
    multi-threaded across band-pass filters. See [Threads](@ref).

This function is called by the following functions operating
on time-frequency reprsentations: [`TFanalyticsignal`](@ref),
[`TFamplitude`](@ref), [`TFphase`](@ref), [`meanAmplitude`](@ref),
[`concentration`](@ref), [`meanDirection`](@ref), [`comodulation`](@ref),
[`coherence`](@ref).

**Examples**:
```julia
using FourierAnalysis, DSP, Plots
# generate a sinusoidal + noise
f, sr, t = 8, 128, 512
v=sinusoidal(1., f, sr, t, 0)
x=v+randn(t)
flabels, Y=filterbank(x, 128)
flabels, Y=filterbank(x, 128; fmin=4, fmax=32)
flabels, Y=filterbank(x, 128, 4; fmin=4, fmax=32)
flabels, Y=filterbank(x, 128, 4;
                      filtkind=Chebyshev2(8, 10),
                      fmin=4,
                      fmax=16)
# trick for plotting the signal filtered in the band-pass regions
for i=1:size(Y, 2) Y[:, i].+=convert(eltype(Y), i)*1.5 end
mylabels=Array{String}(undef, 1, length(flabels))
for i=1:length(flabels) mylabels[1, i]=string(flabels[i])*" Hz" end
plot(Y; c=:grey, labels=mylabels)
```
"""
function filterbank(x         :: Vector{T},
                    sr        :: Int,
                    bandwidth :: IntOrReal    = 2;
                filtkind      :: FilterDesign = Butterworth(2),
                fmin          :: IntOrReal    = bandwidth,
                fmax          :: IntOrReal    = sr÷2,
                ⏩           :: Bool         = true) where T<:Real

    df, dm, ff, Bp=digitalfilter, filtkind, filtfilt, Bandpass # aliases for DSP.jl stuff
    fb=_ffilterbank(sr, bandwidth; fmin=fmin, fmax=fmax) # get limits for band-passes
    nf=length(fb)-2 # eliminate last frequency, which may be the Nyquist frequency
    flt=[df(Bp(fb[f], fb[f+2]; fs=sr), dm) for f=1:nf] # f band-pass filters
    x_=[zeros(T, sr); x; zeros(T, sr)]     # Before filtering, `x` is padded with `sr` zeros at both sides in order to symmetrize the distortion of DSP.jl butterworth filters.
    Y=Matrix{T}(undef, length(x_), nf)
    _thread(⏩, nf) ? (@threads for f=1:nf Y[:, f]=ff(flt[f], x_) end) :
                               (for f=1:nf Y[:, f]=ff(flt[f], x_) end)
    return fb[2:end-1], Y[sr+1:end-sr, :] # trim the padded segments in Y
end

##################################################################
# internal function: Create a vector holding necessary information
# to build a filter bank, given sampling rate `rt`, the `bandwidth`
# and optional keyword parameters `fmin` and `fmax`.
# `bandwidth`, `fmax` and `fmin` can be either Integers or Floats.
# Let y be the output vector, a filter bank with length(y)-2 filters
# can be constructed with band-pass regions (in Hz) going from y[i-1] to y[i+1],
# where y[i+1]-y[i-1] = `bandwidth` for all i in [2:end-1].
# All the length(y)-2 filters are centered at y[i].
# By default the filter bank is defined up to, but exclude, the Nyquist frequency.
# If `fmin` is specified (in Hz), the lowest center frequency is >= `fmin`.
# If `fmax` is specified (in Hz), the highest center frequency is <= `fmax`.
#
# Examples:
#
# sr=16
# bw=4
# _ffilterbank(sr, bw) # [2.0, 4.0, 6.0]
#
# bw=2
# _ffilterbank(sr, bw) # [1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0]
# _ffilterbank(sr, bw; fmin=3, fmax=7) # [2.0, 3.0, 4.0, 5.0, 6.0, 7.0]
#
# bw=1
# _ffilterbank(sr, bw; fmin=3, fmax=5) # [2.5, 3.0, 3.5, 4.0, 4.5, 5.0, 5.5]
#
# bw=1.1
# _ffilterbank(sr, bw; fmin=3, fmax=5) # [2.2, 2.75, 3.3, 3.85, 4.4, 4.95, 5.5]
#
# bw=1.9
# _ffilterbank(sr, bw; fmin=3, fmax=5) # [1.9, 2.85, 3.8, 4.75, 5.7]
function _ffilterbank(sr        :: Int,
                      bandwidth :: IntOrReal;
                  fmin          :: IntOrReal = bandwidth,
                  fmax          :: IntOrReal = sr÷2)
    f=collect(range(0, step=bandwidth/2, stop=sr÷2))
    popfirst!(f)
    a=max(1, (findmin([abs(g-fmin) for g∈f])[2])-1)
    b=min(length(f), (findmin([abs(g-fmax)  for g∈f])[2])+1)
    f[b]>=sr÷2 ? (return f[a:b-1]) : (return f[a:b])
end