using StatsBase
using Random

"""
Used in the computation of cramer's V and cohen's K.
Returns the lagged bivariate probability of two given categories, Pij.
Given i and j two categories, and l a lag,
Pij is the probability to have the category j at time t + l, if we have i at time t.
    inputs :
    - time series : the data to analyse
    - lag : the value of lag at which one wants Pij
    - category1 : the first category
    - category2 : the second category
    output :
    - Pij : the probability to observe j at t + l if we observe i at time t.
"""
function LaggedBivariateProbability(TimeSerie, Lag::Int, Category1, Category2)
    Pij = 0
    lagged_serie_length = length(TimeSerie) - Lag
    for i in Lag+1:lagged_serie_length
        if (TimeSerie[i] == Category1) && (TimeSerie[i - Lag] == Category2)
            Pij += 1/(lagged_serie_length)
        end
    end
    return Pij
end

"""
If an array of lags 'Lag' is provided instead of a single value, computes Pij for each lag value and returns an array.
"""
function LaggedBivariateProbability(TimeSerie, Lags::Array{Int64,1}, Category1, Category2)
    Pij = [LaggedBivariateProbability(TimeSerie,L,Category1,Category2) for L in Lags]
    return Lags,Pij
end

"""
If no categories are explicitely provided, will a CxC symmetric probability matrix, with C the number of != categories.
the element [i,j] of this matrix corresponds to Pij at lag 'Lag'. Use this function if you want to have the contingency table at lag 'Lag'.
"""
function LaggedBivariateProbability(TimeSeries, Lag::Int64)
    categories = sort(unique(TimeSeries))
    Pij = zeros(length(categories), length(categories))
    lagged_serie_length = length(TimeSeries) - Lag
    for (idxi, i) in enumerate(categories)
        for (idxj, j) in enumerate(categories)
            for t in Lag+1:length(TimeSeries)
                if (TimeSeries[t] == i) && (TimeSeries[t - Lag] == j)
                    Pij[idxi, idxj] += 1/(lagged_serie_length)
                end
            end
        end
    end
    return Pij
end

"""
Returns the relative frequency of a given category (of type ::Any).
For example, if we have 3 category a,b and c, and a occurs 300 times in a time series of length 900,
its relative frequency  is 1/3.

"""
function relative_frequency(TimeSerie, idx::Any)
    Pi = 0
    for i in TimeSerie
        if i == unique(TimeSerie)[idx]
            Pi += 1/length(TimeSerie)
        end
    end
    return Pi
end

"""
Computes the lagged cohen coefficient κ describing how much the different categories
are correlated to each others at different lag values in the time-series. K is a measure of agreement.
input:
    -Serie : a categorical time series
    -lag: an integer lag value
output:
    -κ : cohen's coefficient
"""
function cohen_coefficient(Serie, lag = 1)
    if typeof(lag) != Int64
        throw("typeError : 'lag' value needs to be integer.")
    end
    categories = unique(Serie)
    K = 0
    pi_denominateur = 0
    rf_squared = [relative_frequency(Serie,i)^2 for i in 1:length(categories)]
    lagged_pij = LaggedBivariateProbability(Serie, lag)
    for (idxi, i) in enumerate(categories)
        K += (lagged_pij[idxi, idxi] - rf_squared[idxi])
        pi_denominateur += rf_squared[idxi]
    end
    K = K/(1-pi_denominateur)
    return K
end

"""
If 'lag' is provided as an array of lags, computes and returns an array of K for each elements of 'lag'.
"""
function cohen_coefficient(Serie, Lags::Array{Int64,1})
    if typeof(Lags) != Array{Int64,1}
        throw("typeError : 'lag' value needs to be integer.")
    end
    categories = unique(Serie)
    K = zeros(length(Lags))
    rf_squared = [relative_frequency(Serie,i)^2 for i in 1:length(categories)]
    for (lidx,l) in enumerate(Lags)
        k = 0
        pi_denominateur = 0
        lagged_pij = LaggedBivariateProbability(Serie, l)
        for (idxi, i) in enumerate(categories)
            k += (lagged_pij[idxi, idxi] - rf_squared[idxi])
            pi_denominateur += rf_squared[idxi]
        end
        K[lidx] =  k/(1-pi_denominateur)
    end
    return K
end

"""
    cramer_coefficient(Serie, Lags::Array{Int64,1})

Returns the cramer's V coefficient for the given 'Lags' values.

Cramer's v is used in categorical data analysis to test the degree of association between
a given set of categorical variable and another set of variables (example : eye color and hair color).
Here, it measures the association between the categorical values of a time series, and the values at a later lagged time.

V lies in [0,1]. 0 no association, 1 perfect association.
It is symmetric, meaning v(A,B) = v(B,A), so the information contained in the asymmetry between the variables is lost.

    input :
    - Serie : the time series containing the data
    - Lags : Array containing the lags at which V will be computed. If an int is given
                a single point value will be returned.

    output :
    - V : the values of v for the given lags.

"""
function cramer_coefficient(Serie, Lags::Array{Int64,1})
    if typeof(Lags) != Array{Int64,1}
        throw("typeError : 'Lags' needs to be an Array{Int64,1}.")
    end
    Categories = unique(Serie)
    V = zeros(length(Lags))
    d = length(Categories)-1
    rf = [relative_frequency(Serie, i) for i in 1:length(Categories)]
    for (lidx,lag) in enumerate(Lags)
        v = 0
        lagged_pij = LaggedBivariateProbability(Serie, lag)
        for i in 1:d+1
            for j in 1:d+1
                v += ( (lagged_pij[i,j]-rf[i]*rf[j])^2 ) / (rf[i]*rf[j])
            end
        end
        V[lidx] = sqrt(v)/d
    end
    return V
end

function cramer_coefficient(Serie, lag = 1)
    if typeof(lag) != Int64
        throw("typeError : 'lag' value needs to be integer.")
    end
    v = 0
    d = length(unique(Serie))-1
    lagged_pij = LaggedBivariateProbability(Serie, lag)
    rf = [relative_frequency(Serie,i) for i in 1:d+1]
    for i in 1:d+1
        for j in 1:d+1
            v += ((lagged_pij[i,j]-rf[i]*rf[j])^2)/(rf[i]*rf[j])
        end
    end
    return sqrt(v)/d
end

"""
    H(x)

Estimates the probability distribution of the categories in 'x' and computes its entropy.
It characterizes the amount of information carried by the distribution.
it tells how "disperse" the distribution is.

"""
function H(x)
    x_count = countmap(x)
    total_occurences = sum(values(x_count))
    entropy = 0.0
    for occurence in values(x_count)
        px = occurence/total_occurences
        entropy -= px*log2(px)
    end
    return entropy
end

"""
Computing the conditional entropy of y given x: H(Y|X),
Given in bits or shannons (meaning using the log2 for the measurement).
Wikipedia: https://en.wikipedia.org/wiki/Conditional_entropy

"""
function conditional_entropy(y,x)
    xy_count = countmap(collect(zip(y,x)))
    x_count = countmap(x)
    total_occurences = sum(values(xy_count))
    entropy = 0.0
    for xy in keys(xy_count)
        p_xy = xy_count[xy]/total_occurences
        p_x = x_count[xy[2]]/total_occurences
        entropy += p_xy*log2(p_x/p_xy)
    end
    return entropy
end

"""
    theils_u(x, Lags)

Measures what portion of the information associated to the values in 'x' is know at t + lag if the value 'x' is known at t.
Input :
    x : a categorical time-series
    lags : an array of lags onto U is computed.
returns:
    U : an array of U for the given values in 'Lags'.
"""
function theils_u(x, Lags)
    return [(H(x[1:end-l]) - conditional_entropy(x[1:end-l], x[l+1:end])) / H(x[1:end-l]) for l in Lags]
end

"""
    rate_evolution(Series)

A way to test the stationarity of the input categorical time-series 'Series'.
if it varies linearly, then the time series is more or less stationary.
"""
function rate_evolution(Series)
    categories = unique(Series)
    RATE = []
    for c in categories
        init = zeros(length(Series))
        for v in 1:length(Series)
            if Series[v] == c
                init[v] = 1
            end
        end
        push!(RATE,cumsum(init))
    end
    return RATE
end

"""
    bootstrap_CI(Series, lags, coefficient, n_iter = 1000)

Provides 95% a confidence interval by shuffling 'Series' 'n_iter' times, computing the values of 'coefficient'
then finding the value of the top and bottom 2,5% to get the interval.
This is done for every point in 'lags' (can be costly if 'Series' is long).

Input :
    - Series : input array of categorical data
    - lags (Array{Int64,1}) : the lag values at which the analysis is conducted
    - coef_func (**function**) : the function for which the CI needs to be computed.
            'coefficient' can be one of the following **functions** : 'cramer_coefficient, cohen_coefficient, theils_U'.
    - n_iter (Int64) : how many iterations are run for the bootstrap procedure.
    - interval_size (Float64) : the size of the desired confidence interval in percent. Defaults to 0.95 (i.e 95% confidence interval).
returns :
    - top (Array{Float64,1}) : Array of values for the upper limit of the CI.
    - bottom (Array{Float64,1}) : Array of values for the lower limit of the CI.
"""
function bootstrap_CI(Series,  lags, coef_func, n_iter = 1000, interval_size = 0.95)
    interval_bounds = floor(Int, (n_iter - n_iter*interval_size)/2)
    critical_values = zeros(n_iter)
    bootstrap_storage = zeros(n_iter, length(lags))
    for i in 1:n_iter
        bootstrap_storage[i,:] = coef_func(shuffle(Series), lags)
    end
    top_values = sort(bootstrap_storage, dims = 1)[n_iter - interval_bounds,:]
    bottom_values = sort(bootstrap_storage, dims = 1)[interval_bounds,:]
    return top_values, bottom_values
end

export cramer_coefficient, cohen_coefficient, conditional_entropy, LaggedBivariateProbability, H, theils_u, rate_evolution


module UOT

using LinearAlgebra
using JuMP
# solver (library GLPK could be considered but slower)
using Mosek, MosekTools

function euclidian(x::Array{Float64,1}, y::Array{Float64,1})
    return norm(x - y);
end

# structure to store output of compute_flat_metric
mutable struct UOT_output
    cost::Float64
    # matrix of couplings
    P::Array{Float64,2}
    # matrix of costs
    C::Array{Float64,2}
    # vector indicating creation and destruction of mass
    mass_variation::Array{Float64,1}
end

function init_UOT_output(n::Int64 = 1, m::Int64 = 1)
    return UOT_output(Inf, zeros(n, m), zeros(n, m), zeros(n + m));
end

"""
    compute_flat_metric(x, y, a, b, lambda, coupling_mass_variation = true, ground_metric = euclidian)
Compute the flat metric between two point sources distributions given by the locations `x`
with weights `a` and the locations `y` with weights `b`, for the threshold `lambda`. If
`coupling_mass_variation` is set to true then it also computes the coupling and mass variation
between the two distributions.
# Example
```jldoctest
julia> x, y, a, b = [[0.0, 0.0]], [[1.0, 1.0]], [1.0], [0.5];
julia> UOT.compute_flat_metric(x, y, a, b, 1.0)
UOT.UOT_output(1.2071067811865477, [0.5], [1.41421], [0.5, -2.49401e-12])
```
"""
function compute_flat_metric(x::Array{Array{Float64,1},1}, y::Array{Array{Float64,1},1}, a::Array{Float64,1}, b::Array{Float64,1}, lambda::Float64, coupling_mass_variation::Bool = true, ground_metric::Function = euclidian)
    n, m = length(x), length(y);
    out = init_UOT_output(n, m);
    c = cat(a, -b, dims = 1);
    p = n*m;

    # model = Model(with_optimizer(GLPK.Optimizer));
    model = Model(with_optimizer(Mosek.Optimizer, MSK_IPAR_LOG = 0)); # MSK_IPAR_NUM_THREADS number of threads https://www.gams.com/latest/docs/S_MOSEK.html

    @variable(model, f[1:n + m]);
    i = 1;
    for k in 1:n
        for l in 1:m
            e = ground_metric(x[k], y[l]);
            constr = @constraint(model, -e <= f[k] - f[l + n] <= e)
            # Lipschitz constraints of the optimization problem
            set_name(constr, "lip_constraints_$(i)")
            i += 1;
        end
    end
    # Box constraints of the optimization problem
    @constraint(model, bound_constraints, -lambda .<= f .<= lambda);

    @objective(model, Max, c'*f);
    JuMP.optimize!(model);
    out.cost = JuMP.objective_value(model);
    # println(termination_status(model))
    # println(JuMP.solve_time(model))

    # compute coupling and cost matrices from primal and dual solutions
    if coupling_mass_variation
        dual = zeros(p);
        for i in 1:p
            dual[i] = -JuMP.dual.(constraint_by_name(model, "lip_constraints_$(i)"));
        end
        out.mass_variation = -JuMP.dual.(bound_constraints);
        f_opt_value = JuMP.value.(f);

        k = 1;
        for i in 1:n
            for j in 1:m
                out.C[i, j] = f_opt_value[i] - f_opt_value[j + n];
                # to prevent numerical approximations: remove?
                if abs(dual[k]) > 1e-8
                    out.P[i, j] = dual[k];
                end
                k += 1;
            end
        end
    end
    return out;
end

end # module


using MultivariateMixtures: colwise_dot!, logsumexp, logsumexp!, stats!, covariances!

@testset "Utils" begin
    Random.seed!(42)

    a = [1 2 3; 4 5 6.]
    b = [4 5 6; 7 8 9.]
    r = zeros(size(a,2))

    colwise_dot!(r, a, b)
    @testset for i in 1:length(r)
        @test r[i]  == a[:,i] ⋅ b[:,i]
    end

    c = a'/10
    d = copy(c)
    logsumexp!(r, d)
    q = mapslices(r-> log(sum(exp, r)), c, dims=2)
    @testset for (v,w) in  zip(r,q)
        @test v ≈ w atol=1e-15
    end
    @testset for (v,w) in  zip(eachrow(c),eachrow(d))
        @test  exp.(v .- maximum(v)) == w
    end

    s = big(2e-20) - big(1e-40)
    d = [1e-20 log(1e-20)]
    r = zeros(1)
    logsumexp!(r, d)
    @test r[1] ≈ s atol=1e-35
    @test d ≈ [1.0 1e-20] atol=1e-35

    d = [1e-20 log(1e-20)]
    r = logsumexp(d)
    @test r[1] ≈ s atol=1e-35

    r[1] = 0.0
    logsumexp!(r, d, similar(r))
    @test r[1] ≈ s atol=1e-35
    @test d ≈ [1.0 1e-20] atol=1e-35

    d = 2
    s = 100
    k = 3
    n = k*s
    muori = [0.0 0.0 5.0; -5.0 0.0 5.0]
    Cori = [5.0; 1.0; 0.05]
    R = vcat(repeat([1.0 0.0 0.0], s), repeat([0.0 1.0 0.0], s), repeat([0.0 0.0 1.0], s))
    X = vcat(Cori[1]*randn(s,d).+muori[:,1]', Cori[2]*randn(s,d).+muori[:,2]', Cori[3]*randn(s,d).+muori[:,3]')'

    nk, mu = zeros(k), zeros(d,k)
    stats!(nk, mu, X, R)
    @testset for i in nk
        @test i ≈ s
    end
    @testset for i in 1:k
        @test mu[:, i] ≈ muori[:, i] atol=1.0
    end

    C1 = zeros(d,d,k)
    covariances!(C1, nk, mu, X, R, FullNormal, aux=similar(X))
    @testset for i in 1:k
        @test sqrt(det(C1[1,1,i])) ≈ Cori[i] atol=0.3
    end

    C2 = zeros(d,1,k)
    covariances!(C2, nk, mu, X, R, DiagNormal)
    @testset for i in 1:k
        @test sqrt(det(C2[1,1,i])) ≈ Cori[i] atol=0.3
    end

    C3 = zeros(1,1,k)
    covariances!(C3, nk, mu, X, R, IsoNormal, aux=similar(mu))
    @testset for i in 1:k
        @test sqrt(C3[1,1,i]) ≈ Cori[i] atol=0.3
    end

    A,B,C = MultivariateMixtures.new_params(FullNormal, Float64, d, k)
    @test size(A) == (k,)
    @test size(B) == (d,k)
    @test size(C) == (d,d,k)
    A,B,C = MultivariateMixtures.new_params(DiagNormal, Float64, d, k)
    @test size(A) == (k,)
    @test size(B) == (d,k)
    @test size(C) == (d,1,k)
    A,B,C = MultivariateMixtures.new_params(IsoNormal, Float64, d, k)
    @test size(A) == (k,)
    @test size(B) == (d,k)
    @test size(C) == (1,1,k)
    A,B = MultivariateMixtures.auxiliary(FullNormal, X, k)
    @test size(A) == (n,)
    @test size(B) == (d,n)
    A,B = MultivariateMixtures.auxiliary(DiagNormal, X, k)
    @test size(A) == (n,)
    @test size(B) == (d,k)
    A,B = MultivariateMixtures.auxiliary(IsoNormal, X, k)
    @test size(A) == (n,)
    @test size(B) == (d,k)

    F1 = MultivariateMixtures.precision(FullNormal, C1[:,:,1])
    @test F1 isa LowerTriangular
    logpdf!(view(R,:,1), X, mu[:,1], F1)
    r = logpdf(MvNormal(mu[:,1], C1[:,:,1]), X)
    @test sum(view(R,:,1) .- r) ≈ 0.0 atol=1e-10

    F2 = MultivariateMixtures.precision(DiagNormal, C2[:,:,2])
    @test F2 isa Diagonal
    logpdf!(view(R,:,2), X, mu[:,2], F2)
    r = logpdf(MvNormal(mu[:,2], Diagonal(vec(C2[:,:,2]))), X)
    @test sum(view(R,:,2) .- r) ≈ 0.0 atol=1e-10

    F3 = MultivariateMixtures.precision(IsoNormal, C3[:,:,3])
    @test F3 isa UniformScaling
    logpdf!(view(R,:,3), X, mu[:,3], F3)
    r = logpdf(MvNormal(mu[:,3], sqrt(C3[:,:,3][])), X)
    @test sum(view(R,:,3) .- r) ≈ 0.0 atol=1e-8

end


abstract type Growth{R,W} <: CellRule{R,W} end

# Basically we can define this around fitness
# and compute realized λ later

struct LogisticGrowth{R,W,LT,KT,AT,TT} <: CellRule{R,W}
    fitnessmap::LT
    K::KT
    α::AT
    dt::TT
end
LogisticGrowth{R,W}(;
    λ::LT = 1.5,
    K::KT = 100.0,
    α::AT = 1,
    dt::TT = 0.1,
) where {R,W,LT,KT,AT,TT} = LogisticGrowth{R,W}(λ, K, α, dt)

LogisticGrowth(
    layernames::T;
    λ::LT = 1.5,
    K::KT = 100.0,
    α::AT = 1,
    dt::TT = 0.1,
) where {T<:Vector{Symbol},LT,KT,AT,TT} = begin
    rules = Ruleset()
    for sym in layernames
        rules += LogisticGrowth{sym}(; λ = λ, K = K, α = α, dt = dt)
    end
    return rules
end

#LogisticGrowth(fitfunc::FitnessFunction)

function DynamicGrids.applyrule(data, rule::LogisticGrowth, X, index)
    w = fitness(fitnessmap(rule))

    dx = @fastmath rule.λ * X * (1.0 - (X / rule.K)^rule.α)
    return @fastmath X + dx * rule.dt
end


import Base: getindex, size, ndims, convert
export MorletWavelet, MorseWavelet, wavebases, fstd, tstd, ContinuousWaveletTransform, cwt

#
# Mother wavelets, which are convolved with the signal in frequency space
#
abstract MotherWavelet{T<:Real}

# The analytic Morlet wavelet. See:
# Torrence, C., & Compo, G. P. (1998). A practical guide to wavelet
# analysis. Bulletin of the American Meteorological Society, 79(1),
# 61–78.
#
# freq is the frequency in Hz
# k0 is the wave number
immutable MorletWavelet{T} <: MotherWavelet{T}
    freq::Vector{T}
    k0::T
    fourierfactor::T
end
MorletWavelet{T<:Real}(freq::Vector{T}, k0::Real=5.0) =
    MorletWavelet(freq, convert(T, k0), convert(T, (4π)/(k0 + sqrt(2 + k0^2))))

# Generate daughter wavelet (samples x frequencies)
function wavebases{T}(w::MorletWavelet{T}, n::Int, fs::Real=1)
    df = 2π * fs / n
    normconst = sqrt(df) / sqrt(sqrt(π) * n)
    k0 = w.k0

    bases = Array(T, div(n, 2)+1, length(w.freq))
    @inbounds begin
        for k = 1:length(w.freq)
            scale = 1/(w.freq[k] * w.fourierfactor)
            norm = sqrt(scale) * normconst
            bases[1, k] = zero(T)
            for j = 2:size(bases, 1)
                bases[j, k] = norm * exp(-abs2(scale * df * (j-1) - k0)*0.5)
            end
        end
    end
    bases
end

fourierfactor{T}(w::MorletWavelet{T}) = w.fourierfactor
# std in the frequency domain, in units of Hz
fstd{T}(w::MorletWavelet{T}) =
    [(f * w.fourierfactor) / (sqrt(2) * 2π) for f in w.freq]
# std in the time domain, in units of time
tstd{T}(w::MorletWavelet{T}) =
    [1 / (sqrt(2) * f * w.fourierfactor) for f in w.freq]

# Generalized Morse wavelet (first family only). See:
# Olhede, S. C., & Walden, A. T. (2002). Generalized Morse wavelets.
# IEEE Transactions on Signal Processing, 50(11), 2661–2670.
# doi:10.1109/TSP.2002.804066
# Lilly, J. M., & Olhede, S. C. (2009). Higher-order properties of
# analytic wavelets. IEEE Transactions on Signal Processing, 57(1),
# 146–160. doi:10.1109/TSP.2008.2007607
#
# freq is the peak frequency in Hz
# β and γ are wavelet parameters
immutable MorseWavelet{T} <: MotherWavelet{T}
    freq::Vector{T}
    β::T
    γ::T
end
MorseWavelet{T<:Real}(freq::Vector{T}, β::Real, γ::Real) =
    MorseWavelet{T}(freq, convert(T, β), convert(T, γ))

# Generate daughter wavelet (samples x frequencies)
function wavebases{T}(w::MorseWavelet{T}, n::Int, fs::Real=1)
    γ = w.γ
    β = w.β
    # Peak frequency
    ωs = (β/γ)^(1/γ)
    dω0 = fs / n * ωs
    # Olhede & Walden p. 2663, multiplied by sqrt(ωs/(2*pi*f)) for the
    # frequency with an additional correction by 1/n for the
    # unnormalized inverse FFT
    r = (2β+1)/γ
    α0 = 2^(r/2)*sqrt(γ*ωs*fs)/(sqrt(gamma(r))*n)

    bases = Array(T, div(n, 2)+1, length(w.freq))
    for k = 1:length(w.freq)
        f = w.freq[k]
        # Change in angular frequency with each point of the DFT
        dω = dω0/f
        α = α0*f^(-0.5)
        for j = 1:size(bases, 1)
            ω = dω*(j-1)
            bases[j, k] = α * ω^β * exp(-ω^γ)
        end
    end
    bases
end

# Fourier factor based on peak frequency
fourierfactor{T}(w::MorseWavelet{T}) = 2*pi*(w.β/w.γ)^(-1/w.γ)
function fstd{T}(w::MorseWavelet{T})
    γ = w.γ
    β = w.β
    ff = fourierfactor(w)
    σ = sqrt(2^(-2/γ)*(exp(lgamma((2*β+3)/γ)-
                           lgamma((2*β+1)/γ)) -
                       exp(2*(lgamma((2*β+2)/γ)-
                              lgamma((2*β+1)/γ)))))
    [(f * ff * σ) / (2 * pi) for f in w.freq]
end
gammatil(x) = gamma(x)/2^x
function tstd{T}(w::MorseWavelet{T})
    γ = w.γ
    β = w.β
    ff = fourierfactor(w)
    σ = sqrt((β^2*gammatil((2β-1)/γ)+γ^2*
             gammatil((2β+2*γ-1)/γ)-2β*γ*
             gammatil((2β+γ-1)/γ))/gammatil((2β+1)/γ))
    [(σ) / (f * ff) for f in w.freq]
end

#
# Functions for applying wavelets to data
#
immutable ContinuousWaveletTransform{T,S,P1,P2}
    fftin::Vector{T}
    fftout::Vector{S}
    ifftwork::Vector{S}
    bases::Array{T,2}
    coi::Vector{T}
    p1::P1
    p2::P2
end

function ContinuousWaveletTransform{T}(w::MotherWavelet{T}, nfft::Int, fs::Real=1;
                                       coi::Vector=scale!(tstd(w), 2*fs))
    fftin = Array(T, nfft)
    fftout = zeros(Complex{T}, div(nfft, 2)+1)
    ifftwork = zeros(Complex{T}, nfft)
    bases = wavebases(w, nfft, fs)
    length(coi) == size(bases, 2) || isempty(coi) || error("length of coi must match number of frequencies")
    ContinuousWaveletTransform(fftin, fftout, ifftwork, bases, coi, plan_rfft(fftin), plan_bfft!(ifftwork))
end

function evaluate!{T,S<:AbstractFloat}(out::Array{Complex{S}, 2}, t::ContinuousWaveletTransform{T},
                                       signal::AbstractVector{T})
    @inbounds begin
        fftin = t.fftin
        fftout = t.fftout
        ifftwork = t.ifftwork
        bases = t.bases
        coi = t.coi

        nsignal = length(signal)
        nfft = length(fftin)
        nrfft = length(fftout)

        nsignal <= nfft || error("signal exceeds length of transform plan")
        size(out, 1) == length(signal) || error("first dimension of out must match length of signal")
        size(out, 2) == size(bases, 2) || error("second dimension of out must match number of wavelets")

        # Compute signal mean ignoring NaN
        m = 0.0
        for i = 1:nsignal
            v = signal[i]
            m += isnan(v) ? 0.0 : v
        end
        m = convert(T, m/nsignal)
        # Subtract signal mean
        for i = 1:nsignal
            fftin[i] = signal[i] - m
        end
        fftin[nsignal+1:nfft] = zero(T)

        # Interpolate NaN values in input and save their indices to
        # set output to NaN later
        discard_samples = falses(length(signal))
        oldv = zero(eltype(fftin))
        i = 1
        while i <= nsignal
            v = fftin[i]
            if isnan(v)
                nanstart = i

                i += 1
                while i <= nsignal && isnan(fftin[i])
                    i += 1
                end

                nanlen = (i - nanstart)
                st = i > nsignal ? zero(eltype(fftin)) : (fftin[i] - oldv)/(nanlen+1)
                for j = 1:nanlen
                    fftin[nanstart+j-1] = oldv+st*j
                end
                discard_samples[nanstart:i-1] = true
            else
                oldv = v
                i += 1
            end
        end
        # This simplifies replacement with NaN below
        discard_samples[1] = false
        discard_samples[end] = false
        discard_sample_indices = find(discard_samples)

        # Perform FFT of padded signal
        A_mul_B!(fftout, t.p1, fftin)

        for k = 1:size(bases, 2)
            # Multiply by wavelet
            for j = 1:nrfft
                ifftwork[j] = fftout[j] * bases[j, k]
            end

            # We only compute the real FFT, but we may need the imaginary
            # frequencies for some mother wavelets as well
            offset = nrfft + 1 + isodd(nfft)
            for j = nrfft+1:size(bases, 1)
                ifftwork[j] = conj(fftout[offset-j]) * bases[j, k]
            end

            # Zero remaining frequencies
            ifftwork[size(bases, 1)+1:nfft] = zero(Complex{T})

            # Perform FFT
            A_mul_B!(ifftwork, t.p2, ifftwork)

            # Copy to output array
            copy!(out, nsignal*(k-1)+1, ifftwork, 1, nsignal)

            if !isempty(coi)
                # Set NaNs at edges
                coi_length = ceil(Int, coi[k])
                out[1:min(coi_length, nsignal), k] = NaN
                out[max(nsignal-coi_length+1, 1):end, k] = NaN

                # Set NaNs for gaps
                for i in discard_sample_indices
                    out[i, k] = NaN
                    if !discard_samples[i+1]
                        out[i:min(i+coi_length, nsignal), k] = NaN
                    end
                    if !discard_samples[i-1]
                        out[max(i-coi_length, 1):i, k] = NaN
                    end
                end
            end
        end
    end
    out
end

# Friendly interface to ContinuousWaveletTransform
function cwt{T<:Real}(signal::Vector{T}, w::MotherWavelet, fs::Real=1)
    t = ContinuousWaveletTransform(w, nextfastfft(length(signal)), fs)
    evaluate!(Array(Complex{T}, length(signal), size(t.bases, 2)), t, signal)
end


using Plots
sphere(x) = sum(x.^2)

dpi = 600
x = y = range(-3, stop=3, length=50)

l = Vector{Float64}()
for i in x
   for j in y
       v = [i j]
       append!(l, 1/sphere(v))
   end
end

surface(x,y,l, dpi=dpi)
png("./inverse_sphere_surface.png")
contour(x,y,l, dpi=dpi)
png("./inverse_sphere_contour.png")


struct Regularize{N,F}

  "x values of points, normalized to grid index space"
  x :: Vector{Float64}

  "y values of points, normalized to grid index space"
  y :: Vector{Float64}

  "1/dV factor"
  overdv :: Float64

  "weights for each point (e.g. arclengths), divided by dV"
  wgt :: Vector{Float64}

  "Discrete Delta function"
  ddf :: AbstractDDF

  "Radius of the DDF"
  ddf_radius :: Float64

  "Symmetry flag"
  _issymmetric :: Bool

end

"""
    Regularize(x,y,dx,[ddftype=Yang3],[graddir=0],[I0=(1,1)], [weights=1.0], [filter=false],
                       [issymmetric=false])

Constructor to set up an operator for regularizing and interpolating data from/to
points immersed in the grid to/from fields on the grid itself. The supplied
`x` and `y` represent physical coordinates of the immersed points, and `dx`
denotes a uniform physical cell size of the grid. The separate arguments `x` and
`y` can be replaced by a single argument `X` of type `VectorData` holding the
coordinates.

The operations of regularization and interpolation are carried out with a discrete
delta function (ddf), which defaults to the type `Yang3`. Others are also possible,
such as `Roma`, `Goza` or `M3`. The optional argument `graddir`, if set to 1 or 2, will
generate an interpolation operator that evaluates the negative of the
respective component of the gradient of a grid field at the immersed points. The
default value of this argument is 0, which simply interpolates. Note that the
regularization form of this gradient type is also possible.

The optional tuple
`I0` represents the indices of the primary node that coincides with `(x,y) = (0,0)`.
This defaults to `(1,1)`, which leaves one layer of ghost (dual) cells and sets
the physical origin in the lower left corner of the grid of interior dual cells.

Another optional parameter, `weights`, sets the weight of each point in the
regularization. This would generally be set with, say, the differential arc
length for regularization of data on a curve. It can be a vector (of the same length
as x and y) or a scalar if uniform. It defaults to 1.0.

The optional Boolean parameter `filter` can be set to `true` if it is desired to
apply filtering (see Goza et al, J Comput Phys 2016) to the grid data before
interpolating. This is generally only used in the context of preconditioning
the solution for forces on the immersed points.

If the optional Boolean parameter `issymmetric` is set to `true`, then the
regularization and interpolation are constructed to be transposes of each other.
Note that this option overrides any supplied weights. The default of this
parameter is `false`.

The resulting operator can be used in either direction, regularization and
interpolation, with the first argument representing the *target* (the entity
to regularize/interpolate to), and the second argument
the *source* (the entity to regularize/interpolate from). The regularization
does not use the filtering option.

# Example

In the example below, we set up a 12 x 12 grid. Using the default value for `I0`
and setting `dx = 0.1`, the physical dimensions of the non-ghost part of the grid
are 1.0 x 1.0. Three points are set up in the interior, and a vector field is assigned
to them, with the x component of each of them set to 1.0. These data are regularized
to a field of primal edges on the grid, using the Roma DDF kernel.

```jldoctest
julia> x = [0.25,0.75,0.25]; y = [0.75,0.25,0.25];

julia> X = VectorData(x,y);

julia> q = Edges(Primal,(12,12));

julia> dx = 0.1;

julia> H = Regularize(x,y,dx;ddftype=Roma)
Regularization/interpolation operator with non-filtered interpolation
  DDF type CartesianGrids.Roma
  3 points in grid with cell area 0.01

julia> f = VectorData(X);

julia> fill!(f.u,1.0);

julia> H(q,f)
Edges{Primal,12,12,Float64} data
u (in grid orientation)
11×12 Array{Float64,2}:
 0.0  0.0  0.0       0.0     0.0      …  0.0       0.0     0.0      0.0  0.0
 0.0  0.0  0.0       0.0     0.0         0.0       0.0     0.0      0.0  0.0
 0.0  0.0  8.33333  33.3333  8.33333     0.0       0.0     0.0      0.0  0.0
 0.0  0.0  8.33333  33.3333  8.33333     0.0       0.0     0.0      0.0  0.0
 0.0  0.0  0.0       0.0     0.0         0.0       0.0     0.0      0.0  0.0
 0.0  0.0  0.0       0.0     0.0      …  0.0       0.0     0.0      0.0  0.0
 0.0  0.0  0.0       0.0     0.0         0.0       0.0     0.0      0.0  0.0
 0.0  0.0  8.33333  33.3333  8.33333     8.33333  33.3333  8.33333  0.0  0.0
 0.0  0.0  8.33333  33.3333  8.33333     8.33333  33.3333  8.33333  0.0  0.0
 0.0  0.0  0.0       0.0     0.0         0.0       0.0     0.0      0.0  0.0
 0.0  0.0  0.0       0.0     0.0      …  0.0       0.0     0.0      0.0  0.0
v (in grid orientation)
12×11 Array{Float64,2}:
 0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0
 0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0
 0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0
 0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0
 0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0
 0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0
 0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0
 0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0
 0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0
 0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0
 0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0
 0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0
```
"""
function Regularize(x::AbstractVector{T},y::AbstractVector{T},dx::T;
                    ddftype=Yang3,graddir::Int=0,
                    I0::Tuple{Int,Int}=(1,1),
                    weights::Union{T,Vector{T}}=1.0,
                    filter::Bool = false,
                    issymmetric::Bool = false) where {T<:Real}

  _issymmetric = (filter ? false : issymmetric)

  n = length(x)
  @assert length(y)==n
  if !_issymmetric
    if typeof(weights) == T
      wtvec = similar(x)
      fill!(wtvec,weights/(dx*dx))
    else
      @assert length(weights)==n
      wtvec = deepcopy(weights)./(dx*dx)
    end
  else
    # if the regularization and interpolation are symmetric, then the
    # weights are automatically set to be the cell area in order to cancel it
    # in the denominator of the regularization operator.
    wtvec = similar(x)
    fill!(wtvec,1.0)
  end

  baseddf = DDF(ddftype=ddftype,dx=1.0)
  if graddir == 0
    ddf = baseddf
  else
    ddf = GradDDF(graddir,ddftype=ddftype,dx=1.0)
  end

  Regularize{length(x),filter}(x./dx.+I0[1],y./dx.+I0[2],1.0/(dx*dx),
                      wtvec,ddf,_get_regularization_radius(baseddf),_issymmetric)
end

Regularize(x::T,y::T,a...;b...) where {T<:Real} = Regularize([x],[y],a...;b...)

Regularize(x::VectorData,a...;b...) = Regularize(x.u,x.v,a...;b...)

function Base.show(io::IO, H::Regularize{N,F}) where {N,F}
    filter = F ? "filtered" : "non-filtered"
    ddftype,_ = typeof(H.ddf).parameters
    op = H._issymmetric ? "Symmetric regularization/interpolation" : "Regularization/interpolation"
    println(io, "$op operator with $filter interpolation")
    println(io, "  DDF type $ddftype")
    println(io, "  $N points in grid with cell area $(sprint(show,1.0/H.overdv;context=:compact => true))")
end

function _get_regularization_radius(ddf::DDF)
  v = 1.0
  r = 0.0
  dr = 0.01
  while (v = abs(ddf(r))) > eps()
    r += dr
  end
  return r
end

_delta_block(radius,shift) = -radius+shift, radius+shift
_index_range(x,xmin,xmax,n,dn) = max(1,ceil(Int,x+xmin)), min(n-dn,floor(Int,x+xmax))
_distance_list(x,imin,imax,shift) = float(imin)-shift-x:float(imax)-shift-x

"""
    RegularizationMatrix(H::Regularize,f::PointData,u::CellData) -> Hmat

Construct and store a matrix representation of regularization associated with `H`
for data of type `f` to data of type `u`. The resulting matrix `Hmat` can then be
used to apply on point data of type `f` to regularize it to grid data of type `u`,
using `mul!(u,Hmat,f)`. It can also be used as just `Hmat*f`.

If `H` is a symmetric regularization and interpolation operator, then this
actually returns a tuple `Hmat, Emat`, where `Emat` is the interpolation matrix.
"""
struct RegularizationMatrix{TU,TF} <: AbstractMatrix{Float64}
  M :: SparseMatrixCSC{Float64,Int64}
end

"""
    InterpolationMatrix(H::Regularize,u::CellData,f::PointData) -> Emat

Construct and store a matrix representation of interpolation associated with `H`
for data of type `u` to data of type `f`. The resulting matrix `Emat` can then be
used to apply on grid data of type `u` to interpolate it to point data of type `f`,
using `mul!(f,Emat,u)`. It can also be used as just `Emat*u`.
"""
struct InterpolationMatrix{TU,TF} <: AbstractMatrix{Float64}
  M :: SparseMatrixCSC{Float64,Int64}
end

@wraparray RegularizationMatrix M 2
@wraparray InterpolationMatrix M 2

# ======  Regularization and interpolation operators of scalar types ======== #

pointtype = :ScalarData
for (gridtype,ctype,dnx,dny,shiftx,shifty) in @generate_scalarlist(SCALARLIST)

# Regularization
  @eval function (H::Regularize{N,F})(target::$gridtype{$ctype,NX,NY,T,DDT},source::$pointtype{N,S,DT}) where {N,F,NX,NY,S,T,DT,DDT}
        radius = H.ddf_radius
        fill!(target,0.0)
        xmin, xmax = _delta_block(radius,$shiftx)
        ymin, ymax = _delta_block(radius,$shifty)
        nzinds = findall(!iszero,source)
        @inbounds for pt in nzinds
            fact = source[pt]*H.wgt[pt]
            minx, maxx = _index_range(H.x[pt],xmin,xmax,NX,$dnx)
            miny, maxy = _index_range(H.y[pt],ymin,ymax,NY,$dny)
            prangex = _distance_list(H.x[pt],minx,maxx,$shiftx)
            prangey = _distance_list(H.y[pt],miny,maxy,$shifty)
            target[minx:maxx,miny:maxy] .+= fact.*H.ddf(prangex,prangey)
        end
        target
  end

# Interpolation
  @eval function (H::Regularize{N,false})(target::$pointtype{N,S,DT},source::$gridtype{$ctype,NX,NY,T,DDT}) where {N,NX,NY,S,T,DT,DDT}
        radius = H.ddf_radius
        fill!(target,0.0)
        xmin, xmax = _delta_block(radius,$shiftx)
        ymin, ymax = _delta_block(radius,$shifty)
        @inbounds for pt in eachindex(target)
            minx, maxx = _index_range(H.x[pt],xmin,xmax,NX,$dnx)
            miny, maxy = _index_range(H.y[pt],ymin,ymax,NY,$dny)
            prangex = _distance_list(H.x[pt],minx,maxx,$shiftx)
            prangey = _distance_list(H.y[pt],miny,maxy,$shifty)
            target[pt] += sum(source[minx:maxx,miny:maxy].*H.ddf(prangex,prangey))
        end
        target
  end

# Interpolation with filtering
  @eval function (H::Regularize{N,true})(target::$pointtype{N,S,DT},source::$gridtype{$ctype,NX,NY,T,DDT}) where {N,NX,NY,S,T,DT,DDT}
        tmp = typeof(source)()
        radius = H.ddf_radius
        fill!(target,0.0)
        xmin, xmax = _delta_block(radius,$shiftx)
        ymin, ymax = _delta_block(radius,$shifty)
        @inbounds for pt in eachindex(target)
            minx, maxx = _index_range(H.x[pt],xmin,xmax,NX,$dnx)
            miny, maxy = _index_range(H.y[pt],ymin,ymax,NY,$dny)
            prangex = _distance_list(H.x[pt],minx,maxx,$shiftx)
            prangey = _distance_list(H.y[pt],miny,maxy,$shifty)
            tmp[minx:maxx,miny:maxy] .+= H.wgt[pt].*H.ddf(prangex,prangey)
        end
        nzinds = findall(x -> abs(x) > eps(),tmp)
        tmp[nzinds] .= inv.(tmp[nzinds])

        @inbounds for pt in eachindex(target)
            minx, maxx = _index_range(H.x[pt],xmin,xmax,NX,$dnx)
            miny, maxy = _index_range(H.y[pt],ymin,ymax,NY,$dny)
            prangex = _distance_list(H.x[pt],minx,maxx,$shiftx)
            prangey = _distance_list(H.y[pt],miny,maxy,$shifty)
            target[pt] += sum(tmp[minx:maxx,miny:maxy].*
                              source[minx:maxx,miny:maxy].*
                              H.ddf(prangex,prangey))
            #end
        end
        target
  end

  # Construct regularization matrix
  @eval function RegularizationMatrix(H::Regularize{N,F},
    f::$pointtype{N,S,DT},
    u::$gridtype{$ctype,NX,NY,T,DDT}) where {N,F,NX,NY,S,T,DT,DDT}

    linI = LinearIndices(u)
    rad = H.ddf_radius
    xmin, xmax = _delta_block(rad,$shiftx)
    ymin, ymax = _delta_block(rad,$shifty)

    rows = Int64[]
    cols = Int64[]
    vals = eltype(u)[]

    for pt = 1:N
      minx, maxx = _index_range(H.x[pt],xmin,xmax,NX,$dnx)
      miny, maxy = _index_range(H.y[pt],ymin,ymax,NY,$dny)
      prangex = _distance_list(H.x[pt],minx,maxx,$shiftx)
      prangey = _distance_list(H.y[pt],miny,maxy,$shifty)

      I1 = vec(linI[minx:maxx,miny:maxy])
      append!(rows,I1)
      append!(cols,fill(pt,length(I1)))
      append!(vals,H.wgt[pt].*vec(H.ddf(prangex,prangey)))
    end
    Hmat = sparse(rows,cols,vals,length(u),length(f))
    if H._issymmetric
      # In symmetric case, these matrices are identical. (Interpolation is stored
      # as its transpose.)
      return RegularizationMatrix{typeof(u),typeof(f)}(Hmat),InterpolationMatrix{typeof(u),typeof(f)}(Hmat)
    else
      return RegularizationMatrix{typeof(u),typeof(f)}(Hmat)
    end
  end

  # Construct interpolation matrix
  @eval function InterpolationMatrix(H::Regularize{N,false},
    u::$gridtype{$ctype,NX,NY,T,DDT},
    f::$pointtype{N,S,DT}) where {N,NX,NY,S,T,DT,DDT}

    linI = LinearIndices(u)
    rad = H.ddf_radius
    xmin, xmax = _delta_block(rad,$shiftx)
    ymin, ymax = _delta_block(rad,$shifty)

    rows = Int64[]
    cols = Int64[]
    vals = eltype(u)[]

    for pt = 1:N
      minx, maxx = _index_range(H.x[pt],xmin,xmax,NX,$dnx)
      miny, maxy = _index_range(H.y[pt],ymin,ymax,NY,$dny)
      prangex = _distance_list(H.x[pt],minx,maxx,$shiftx)
      prangey = _distance_list(H.y[pt],miny,maxy,$shifty)

      I1 = vec(linI[minx:maxx,miny:maxy])
      append!(rows,I1)
      append!(cols,fill(pt,length(I1)))
      append!(vals,vec(H.ddf(prangex,prangey)))
    end
    Emat = sparse(rows,cols,vals,length(u),length(f))
    InterpolationMatrix{typeof(u),typeof(f)}(Emat)
  end

  # Construct interpolation matrix with filtering. Because we construct
  # the matrix by supplying unit vectors from the points, it is easier
  # to carry out the filtering here rather than call the filtered operator
  # we defined above
  @eval function InterpolationMatrix(H::Regularize{N,true},
    u::$gridtype{$ctype,NX,NY,T,DDT},
    f::$pointtype{N,S,DT}) where {N,NX,NY,S,T,DT,DDT}

    linI = LinearIndices(u)
    rad = H.ddf_radius
    xmin, xmax = _delta_block(rad,$shiftx)
    ymin, ymax = _delta_block(rad,$shifty)

    g = similar(f)
    v = similar(u)
    fill!(g,1.0)
    wt = sparsevec(H(v,g))
    wt.nzval .= inv.(wt.nzval)

    rows = Int64[]
    cols = Int64[]
    vals = eltype(u)[]
    for pt = 1:N
      minx, maxx = _index_range(H.x[pt],xmin,xmax,NX,$dnx)
      miny, maxy = _index_range(H.y[pt],ymin,ymax,NY,$dny)
      prangex = _distance_list(H.x[pt],minx,maxx,$shiftx)
      prangey = _distance_list(H.y[pt],miny,maxy,$shifty)

      I1 = vec(linI[minx:maxx,miny:maxy])
      append!(rows,I1)
      append!(cols,fill(pt,length(I1)))
      append!(vals,wt[I1].*vec(H.ddf(prangex,prangey)))
    end
    Emat = sparse(rows,cols,vals,length(u),length(f))

    InterpolationMatrix{typeof(u),typeof(f)}(Emat)
  end

end

# ===== Regularization and interpolation operators of vector data to edges ===== #
pointtype = :VectorData
for (gridtype,ctype,dunx,duny,dvnx,dvny,shiftux,shiftuy,shiftvx,shiftvy) in @generate_collectionlist(VECTORLIST)

# Regularization
  @eval function (H::Regularize{N,F})(target::$gridtype{$(ctype...),NX,NY,T,DDT},source::$pointtype{N,S,DT}) where {N,F,NX,NY,S,T,DT,DDT}
        H(target.u,source.u)
        H(target.v,source.v)
        target
  end

# Interpolation
  @eval function (H::Regularize{N,F})(target::$pointtype{N,S,DT},source::$gridtype{$(ctype...),NX,NY,T,DDT}) where {N,F,NX,NY,S,T,DT,DDT}
        H(target.u,source.u)
        H(target.v,source.v)
        target
  end

  # Construct regularization matrix
  @eval function RegularizationMatrix(H::Regularize{N,F},src::$pointtype{N,S,DT},target::$gridtype{$(ctype...),NX,NY,T,DDT}) where {N,F,NX,NY,S,T,DT,DDT}

    lenu = length(target.u)
    lenv = length(target.v)
    Hmat = spzeros(lenu+lenv,2N)

    if H._issymmetric
      # In symmetric case, these matrices are identical. (Interpolation is stored
      # as its transpose.)
      Hmat[1:lenu,          1:N]    = RegularizationMatrix(H,src.u,target.u)[1].M
      Hmat[lenu+1:lenu+lenv,N+1:2N] = RegularizationMatrix(H,src.v,target.v)[1].M
      return RegularizationMatrix{typeof(target),typeof(src)}(Hmat),InterpolationMatrix{typeof(target),typeof(src)}(Hmat)
    else
      Hmat[1:lenu,          1:N]    = RegularizationMatrix(H,src.u,target.u).M
      Hmat[lenu+1:lenu+lenv,N+1:2N] = RegularizationMatrix(H,src.v,target.v).M
      return RegularizationMatrix{typeof(target),typeof(src)}(Hmat)
    end
  end

  # Construct interpolation matrix
  @eval function InterpolationMatrix(H::Regularize{N,F},src::$gridtype{$(ctype...),NX,NY,T,DDT},target::$pointtype{N,S,DT}) where {N,F,NX,NY,S,T,DT,DDT}

    # note that we store interpolation matrices in the same shape as regularization matrices
    lenu = length(src.u)
    lenv = length(src.v)
    Emat = spzeros(lenu+lenv,2N)
    Emat[1:lenu,          1:N]    = InterpolationMatrix(H,src.u,target.u).M
    Emat[lenu+1:lenu+lenv,N+1:2N] = InterpolationMatrix(H,src.v,target.v).M
    InterpolationMatrix{typeof(src),typeof(target)}(Emat)
  end

end

# ======  Regularization and interpolation operators of tensor data to edge gradients ==== #
# Here, u describes both diagonal components and v the off diagonal
# We do not use the scalar-wise operations here because there is double use of
# the same ddf evaluation, so this saves us quite a bit of time.

pointtype = :TensorData
for (gridtype,ctype,dunx,duny,dvnx,dvny,shiftux,shiftuy,shiftvx,shiftvy) in @generate_collectionlist(TENSORLIST)

# Regularization
  @eval function (H::Regularize{N,F})(target::$gridtype{$(ctype...),NX,NY,T,DDT},source::$pointtype{N,S,DT}) where {N,F,NX,NY,S,T,DT,DDT}
    radius = H.ddf_radius
    fill!(target.dudx,0.0)
    fill!(target.dvdy,0.0)
    xmin, xmax = _delta_block(radius,$shiftux)
    ymin, ymax = _delta_block(radius,$shiftuy)
    @inbounds for pt in 1:N
      minx, maxx = _index_range(H.x[pt],xmin,xmax,NX,$dunx)
      miny, maxy = _index_range(H.y[pt],ymin,ymax,NY,$duny)
      prangex = _distance_list(H.x[pt],minx,maxx,$shiftux)
      prangey = _distance_list(H.y[pt],miny,maxy,$shiftuy)
      tmp = H.wgt[pt].*H.ddf(prangex,prangey)
      target.dudx[minx:maxx,miny:maxy] .+= source.dudx[pt].*tmp
      target.dvdy[minx:maxx,miny:maxy] .+= source.dvdy[pt].*tmp
    end
    fill!(target.dudy,0.0)
    fill!(target.dvdx,0.0)
    xmin, xmax = _delta_block(radius,$shiftvx)
    ymin, ymax = _delta_block(radius,$shiftvy)
    @inbounds for pt in 1:N
      minx, maxx = _index_range(H.x[pt],xmin,xmax,NX,$dvnx)
      miny, maxy = _index_range(H.y[pt],ymin,ymax,NY,$dvny)
      prangex = _distance_list(H.x[pt],minx,maxx,$shiftvx)
      prangey = _distance_list(H.y[pt],miny,maxy,$shiftvy)
      tmp = H.wgt[pt].*H.ddf(prangex,prangey)
      target.dudy[minx:maxx,miny:maxy] .+= source.dudy[pt].*tmp
      target.dvdx[minx:maxx,miny:maxy] .+= source.dvdx[pt].*tmp
    end
    target
  end

  # @eval function (H::Regularize{N,F})(target::$ctype,source::$ftype) where {N,F,NX,NY,S,T}
  #   H(target.dudx,source.dudx)
  #   H(target.dudy,source.dudy)
  #   H(target.dvdx,source.dvdx)
  #   H(target.dvdy,source.dvdy)
  #   target
  # end

# Interpolation
  @eval function (H::Regularize{N,false})(target::$pointtype{N,S,DT},source::$gridtype{$(ctype...),NX,NY,T,DDT}) where {N,NX,NY,S,T,DT,DDT}
    radius = H.ddf_radius
    fill!(target.dudx,0.0)
    fill!(target.dvdy,0.0)
    xmin, xmax = _delta_block(radius,$shiftux)
    ymin, ymax = _delta_block(radius,$shiftuy)
    @inbounds for pt in 1:N
      minx, maxx = _index_range(H.x[pt],xmin,xmax,NX,$dunx)
      miny, maxy = _index_range(H.y[pt],ymin,ymax,NY,$duny)
      prangex = _distance_list(H.x[pt],minx,maxx,$shiftux)
      prangey = _distance_list(H.y[pt],miny,maxy,$shiftuy)
      tmp = H.ddf(prangex,prangey)
      target.dudx[pt] += sum(source.dudx[minx:maxx,miny:maxy].*tmp)
      target.dvdy[pt] += sum(source.dvdy[minx:maxx,miny:maxy].*tmp)
    end
    fill!(target.dudy,0.0)
    fill!(target.dvdx,0.0)
    xmin, xmax = _delta_block(radius,$shiftvx)
    ymin, ymax = _delta_block(radius,$shiftvy)
    @inbounds for pt in 1:N
      minx, maxx = _index_range(H.x[pt],xmin,xmax,NX,$dvnx)
      miny, maxy = _index_range(H.y[pt],ymin,ymax,NY,$dvny)
      prangex = _distance_list(H.x[pt],minx,maxx,$shiftvx)
      prangey = _distance_list(H.y[pt],miny,maxy,$shiftvy)
      tmp = H.ddf(prangex,prangey)
      target.dudy[pt] += sum(source.dudy[minx:maxx,miny:maxy].*tmp)
      target.dvdx[pt] += sum(source.dvdx[minx:maxx,miny:maxy].*tmp)
    end
    target
  end

  # @eval function (H::Regularize{N,F})(target::$ftype,source::$ctype) where {N,F,NX,NY,S,T}
  #   H(target.dudx,source.dudx)
  #   H(target.dudy,source.dudy)
  #   H(target.dvdx,source.dvdx)
  #   H(target.dvdy,source.dvdy)
  #   target
  # end

# Interpolation with filtering -- need to speed up
  @eval function (H::Regularize{N,true})(target::$pointtype{N,S,DT},source::$gridtype{$(ctype...),NX,NY,T,DDT}) where {N,NX,NY,S,T,DT,DDT}
    tmp = typeof(source)()
    radius = H.ddf_radius
    fill!(target.dudx,0.0)
    fill!(target.dvdy,0.0)
    xmin, xmax = _delta_block(radius,$shiftux)
    ymin, ymax = _delta_block(radius,$shiftuy)
    @inbounds for pt in 1:N
        minx, maxx = _index_range(H.x[pt],xmin,xmax,NX,$dunx)
        miny, maxy = _index_range(H.y[pt],ymin,ymax,NY,$duny)
        prangex = _distance_list(H.x[pt],minx,maxx,$shiftux)
        prangey = _distance_list(H.y[pt],miny,maxy,$shiftuy)
        tmp.dudx[minx:maxx,miny:maxy] .+= H.wgt[pt].*H.ddf(prangex,prangey)
    end
    nzinds = findall(x -> abs(x) > eps(),tmp.dudx)
    tmp.dudx[nzinds] .= inv.(tmp.dudx[nzinds])
    tmp.dvdy .= tmp.dudx

    @inbounds for pt in 1:N
        minx, maxx = _index_range(H.x[pt],xmin,xmax,NX,$dunx)
        miny, maxy = _index_range(H.y[pt],ymin,ymax,NY,$duny)
        prangex = _distance_list(H.x[pt],minx,maxx,$shiftux)
        prangey = _distance_list(H.y[pt],miny,maxy,$shiftuy)
        d = H.ddf(prangex,prangey)
        target.dudx[pt] += sum(tmp.dudx[minx:maxx,miny:maxy].*
                               source.dudx[minx:maxx,miny:maxy].*d)
        target.dvdy[pt] += sum(tmp.dvdy[minx:maxx,miny:maxy].*
                               source.dvdy[minx:maxx,miny:maxy].*d)
    end

    fill!(target.dudy,0.0)
    fill!(target.dvdx,0.0)
    xmin, xmax = _delta_block(radius,$shiftvx)
    ymin, ymax = _delta_block(radius,$shiftvy)
    @inbounds for pt in 1:N
        minx, maxx = _index_range(H.x[pt],xmin,xmax,NX,$dvnx)
        miny, maxy = _index_range(H.y[pt],ymin,ymax,NY,$dvny)
        prangex = _distance_list(H.x[pt],minx,maxx,$shiftvx)
        prangey = _distance_list(H.y[pt],miny,maxy,$shiftvy)
        tmp.dudy[minx:maxx,miny:maxy] .+= H.wgt[pt].*H.ddf(prangex,prangey)
    end
    nzinds = findall(x -> abs(x) > eps(),tmp.dudy)
    tmp.dudy[nzinds] .= inv.(tmp.dudy[nzinds])
    tmp.dvdx .= tmp.dudy

    @inbounds for pt in 1:N
        minx, maxx = _index_range(H.x[pt],xmin,xmax,NX,$dvnx)
        miny, maxy = _index_range(H.y[pt],ymin,ymax,NY,$dvny)
        prangex = _distance_list(H.x[pt],minx,maxx,$shiftvx)
        prangey = _distance_list(H.y[pt],miny,maxy,$shiftvy)
        d = H.ddf(prangex,prangey)
        target.dudy[pt] += sum(tmp.dudy[minx:maxx,miny:maxy].*
                               source.dudy[minx:maxx,miny:maxy].*d)
        target.dvdx[pt] += sum(tmp.dvdx[minx:maxx,miny:maxy].*
                               source.dvdx[minx:maxx,miny:maxy].*d)
    end
    target
  end

  # Construct regularization matrix
  @eval function RegularizationMatrix(H::Regularize{N,F},src::$pointtype{N,S,DT},target::$gridtype{$(ctype...),NX,NY,T,DDT}) where {N,F,NX,NY,S,T,DT,DDT}

    # note that we only need to compute two distinct matrices, since there are
    # only two types of cell data in this tensor

    lenu = length(target.dudx)
    lenv = length(target.dudy)
    Hmat = spzeros(2lenu+2lenv,4N)

    if H._issymmetric
      # In symmetric case, these matrices are identical. (Interpolation is stored
      # as its transpose.)
      Hdudx, _ = RegularizationMatrix(H,src.dudx,target.dudx)
      Hdudy, _ = RegularizationMatrix(H,src.dudy,target.dudy)
      Hmat[1:lenu,          1:N]    = Hdudx.M
      Hmat[lenu+1:lenu+lenv,N+1:2N] = Hdudy.M
      Hmat[lenu+lenv+1:lenu+2lenv,2N+1:3N] = Hdudy.M
      Hmat[lenu+2lenv+1:2lenu+2lenv,3N+1:4N] = Hdudx.M
      return RegularizationMatrix{typeof(target),typeof(src)}(Hmat),InterpolationMatrix{typeof(target),typeof(src)}(Hmat)
    else
      Hdudx = RegularizationMatrix(H,src.dudx,target.dudx)
      Hdudy = RegularizationMatrix(H,src.dudy,target.dudy)
      Hmat[1:lenu,          1:N]    = Hdudx.M
      Hmat[lenu+1:lenu+lenv,N+1:2N] = Hdudy.M
      Hmat[lenu+lenv+1:lenu+2lenv,2N+1:3N] = Hdudy.M
      Hmat[lenu+2lenv+1:2lenu+2lenv,3N+1:4N] = Hdudx.M
      return RegularizationMatrix{typeof(target),typeof(src)}(Hmat)
    end
  end

  # Construct interpolation matrix
  @eval function InterpolationMatrix(H::Regularize{N,F},src::$gridtype{$(ctype...),NX,NY,T,DDT},target::$pointtype{N,S,DT}) where {N,F,NX,NY,S,T,DT,DDT}

    # note that we store interpolation matrices in the same shape as regularization matrices
    lenu = length(src.dudx)
    lenv = length(src.dudy)
    Emat = spzeros(2lenu+2lenv,4N)
    Edudx = InterpolationMatrix(H,src.dudx,target.dudx)
    Edudy = InterpolationMatrix(H,src.dudy,target.dudy)

    Emat[1:lenu,          1:N]    = Edudx.M
    Emat[lenu+1:lenu+lenv,N+1:2N] = Edudy.M
    Emat[lenu+lenv+1:lenu+2lenv,2N+1:3N] = Edudy.M
    Emat[lenu+2lenv+1:2lenu+2lenv,3N+1:4N] = Edudx.M

    InterpolationMatrix{typeof(src),typeof(target)}(Emat)
  end

end

###### Matrix multiplication extended to these Regularization/Interpolation matrices ########

# Regularize
# We need to restrict the action of the matrices to the point and grid types
# they are built for, but allow the underyling data type (DT parameter)
# to be different, since one might be, e.g., a SubArray and the other an Array.
for f in [:Nodes,:XEdges,:YEdges]
    @eval mul!(u::S1,Hmat::RegularizationMatrix{S2,F},f) where {F,S1<:$f{C,NX,NY,T},S2<:$f{C,NX,NY,T}} where {C,NX,NY,T} = _mul!(u,Hmat,f)
    @eval mul!(f,Emat::InterpolationMatrix{S2,F},u::S1) where {F,S1<:$f{C,NX,NY,T},S2<:$f{C,NX,NY,T}} where {C,NX,NY,T} = _mul!(f,Emat,u)
    @eval (*)(Emat::InterpolationMatrix{S2,F},u::S1) where {F,S1<:$f{C,NX,NY,T},S2<:$f{C,NX,NY,T}} where {C,NX,NY,T} = mul!(F(),Emat,u)
    # This is meant to generate a MethodError for non-matching point types:
    @eval (*)(Emat::InterpolationMatrix{S2,F},u::GridData) where {F,S2<:$f{C,NX,NY,T}} where {C,NX,NY,T} = mul!(F(),Emat,u)
end
# Handle all collected grid data in a stricter fashion, since underlying data
# is always stored as Vector type
mul!(u::G,Hmat::RegularizationMatrix{G,F},f) where {G <: CollectedGridData,F} = _mul!(u,Hmat,f)
mul!(f,Emat::InterpolationMatrix{G,F},u::G) where {G <: CollectedGridData,F} = _mul!(f,Emat,u)
(*)(Emat::InterpolationMatrix{G,F},u::G) where {F,G <: CollectedGridData} = mul!(F(),Emat,u)
# This is meant to generate a MethodError for non-matching point types:
(*)(Emat::InterpolationMatrix{G,F},u) where {F,G <: CollectedGridData} = mul!(F(),Emat,u)

# Now dispatch on the point data type
for f in [:ScalarData,:VectorData,:TensorData]
  @eval _mul!(u,Hmat::RegularizationMatrix{G,S1},f::S2) where {G,S1<:$f{N,T},S2<:$f{N,T}} where {N,T} = _unsafe_mul!(u,Hmat,f)
  @eval _mul!(f::S2,Emat::InterpolationMatrix{G,S1},u) where {G,S1<:$f{N,T},S2<:$f{N,T}} where {N,T} = _unsafe_mul!(f,Emat,u)
  @eval (*)(Hmat::RegularizationMatrix{G,S1},f::S2) where {G,S1<:$f{N,T},S2<:$f{N,T}} where {N,T} = mul!(G(),Hmat,f)
  # This is meant to generate a MethodError for non-matching point types:
  @eval (*)(Hmat::RegularizationMatrix{G,S1},f) where {G,S1<:$f{N,T}} where {N,T} = mul!(G(),Hmat,f)
end

# Regularization without checking types
function _unsafe_mul!(u,Hmat::RegularizationMatrix,f)
  fill!(u,0.0)
  nzv = Hmat.M.nzval
  rv = Hmat.M.rowval
  @inbounds for col = 1:Hmat.M.n
    fj = f[col]
    for j = Hmat.M.colptr[col]:(Hmat.M.colptr[col + 1] - 1)
        u[rv[j]] += nzv[j]*fj
    end
  end
  u
end

# Interpolation without checking types
function _unsafe_mul!(f,Emat::InterpolationMatrix,u)
  fill!(f,0.0)
  nzv = Emat.M.nzval
  rv = Emat.M.rowval
  @inbounds for col = 1:Emat.M.n
      tmp = zero(eltype(f))
      for j = Emat.M.colptr[col]:(Emat.M.colptr[col + 1] - 1)
          tmp += transpose(nzv[j])*u[rv[j]]
      end
      f[col] += tmp
  end
  f
end

# Interpolation of regularization, used for developing the filtering matrix
function mul!(C::Array{Float64},Emat::InterpolationMatrix{G,F},
                              Hmat::RegularizationMatrix{G,F}) where {G<:GridData,F<:PointData}
  fill!(C,0.0)
  Enzv = Emat.M.nzval
  Erv = Emat.M.rowval
  @inbounds for row = 1:Emat.M.n, col = 1:Hmat.M.n
      tmp = zero(eltype(C))
      for j = Emat.M.colptr[row]:(Emat.M.colptr[row + 1] - 1)
          tmp += transpose(Enzv[j])*Hmat[Erv[j],col]
      end
      C[row,col] += tmp
  end
  return C
end

(*)(Emat::InterpolationMatrix,Hmat::RegularizationMatrix) =
        mul!(Array{eltype(Emat),2}(undef,Emat.M.n,Hmat.M.n),Emat,Hmat)

# This fixes a method ambiguity
(*)(Emat::InterpolationMatrix{G,F},Hmat::RegularizationMatrix) where {G <: Union{VectorGridData,TensorGridData},F}=
        mul!(Array{eltype(Emat),2}(undef,Emat.M.n,Hmat.M.n),Emat,Hmat)

function Base.summary(io::IO, H::RegularizationMatrix{TU,TF}) where {TU,TF}
    print(io, "Regularization matrix acting on type $TF and returning type $TU")
end

function Base.summary(io::IO, H::InterpolationMatrix{TU,TF}) where {TU,TF}
    print(io, "Interpolation matrix acting on type $TU and returning type $TF")
end


@testset "Trends" begin
  Random.seed!(123)

  # constant trend
  d = georef((z=rand(100),), CartesianGrid(100))
  z̄ = trend(d, :z)[:z]
  @test all(abs.(diff(z̄)) .< 0.01)

  # linear trend
  μ = range(0,stop=1,length=100)
  ϵ = 0.1rand(100)
  d = georef((z=μ+ϵ,), CartesianGrid(100))
  z̄ = trend(d, :z)[:z]
  @test all([abs(z̄[i] - μ[i]) < 0.1 for i in 1:length(z̄)])

  # quadratic trend
  r = range(-1,stop=1,length=100)
  μ = [x^2 + y^2 for x in r, y in r]
  ϵ = 0.1rand(100,100)
  d = georef((z=μ+ϵ,))
  d̄ = trend(d, :z, degree=2)
  z̄ = reshape(d̄[:z], 100, 100)
  @test all([abs(z̄[i] - μ[i]) < 0.1 for i in 1:length(z̄)])

  if visualtests
    p₁ = heatmap(μ+ϵ, title="z")
    p₂ = heatmap(z̄, title="z trend")
    plt = plot(p₁, p₂, size=(900,300))
    @test_reference "data/trends.png" plt
  end
end


# ##  AccuracyAtTopKernels.jl
# This repository is a complementary material to our paper ["General Framework for Nonlinear Binary Classification on Top Samples"]().
#
# # Installation
#
# This package can be installed using pkg REPL as follows
# ```julia
# (v1.2) pkg> add https://github.com/VaclavMacha/AccuracyAtTopKernels_new.jl
# ```

# # Usage
#
# This package provides following methods:
# * Methods:
#     1. `TopPush(C, surrogate)`
#     2. `TopPushK(K, C, surrogate)`
#     3. `PatMat(τ C, surrogate, surrogate)`
#     4. `PatMatNP(τ C, surrogate, surrogate)`
# * Surrogates:
#     1. `Hinge(ϑ)`
#     2. `Quadratic(ϑ)`
# * Problems:
#     1. primal
#     2. dual
# * Solvers:
#     1. `General`
#     2. `Gradient`
#     3. `Coordinate`
#
# ## Simple example
#
# In this example we use our package and StatsPlots to visualize the solution.
using AccuracyAtTopKernels, StatsPlots, Random

Random.seed!(1234)

function random_circle(n::Int; radius::Tuple = (0,1), origin::Tuple = (0,0))
    rmin, rmax = round.(Int, radius.^2)
    r2 = (rmax - rmin) .* rand(n) .+ rmin
    θ  = 2π .* rand(n)

    x = origin[1] .+ sqrt.(r2) .* cos.(θ)
    y = origin[2] .+ sqrt.(r2) .* sin.(θ)
    return x, y
end

nneg, npos = 1000, 200
xneg, yneg = random_circle(nneg; radius = (3, 5));
xpos, ypos = random_circle(npos; radius = (0, 2));

X = [xneg yneg; xpos ypos];
y = 1:(nneg+npos) .> nneg;

#  As an example, we use the following linearly nonseparable data

plt1 = scatter(X[y .== 0, 1], X[y .== 0, 2], label = "positives")
scatter!(X[y .== 1, 1], X[y .== 1, 2], label = "negatives", dpi = 300)
savefig(plt1, "data.png")

# ![Simple example](./scripts/data.png)

# The following example shows, how to compute the primal and dual problem for ToppushK method. To solve, we use our gradient descent based solver for the primal problem and our coordinate descent based solver for the dual problem.

model  = TopPushK(10, 1, Quadratic(1))
data_p = Primal(X, y);
data_d = Dual(model, X, y; kernel = GaussianKernel());

solution_p = solve(Gradient(maxiter = 1000, optimizer = Descent(0.0001)), model, data_p);
solution_d = solve(Coordinate(maxiter = 1000), model, data_d);

# For simple visual verification that methods works, we use density estimates of classification scores.
scores_p = scores(model, data_p, solution_p.w);
scores_d = scores(model, data_d, solution_d.α, solution_d.β);

plt_p = density(scores_p[y .== 0]; trim = true, fill = (0, 0.15), title = "primal", legend = false)
density!(scores_p[y .== 1]; trim = true, fill = (0, 0.15))
plt_d = density(scores_d[y .== 0]; label = "negatives", trim = true, fill = (0, 0.15), title = "dual with kernel")
density!(scores_d[y .== 1]; label = "positives", trim = true, fill = (0, 0.15))
plt = plot(plt_p, plt_d, layout = (1,2), size = (800, 400), dpi = 300)
savefig(plt, "scores.png")

# ![Comparison of scores.](./scripts/scores.png)


using OVERT

a_lead = -2.0
mu = 0.0001

function acc_dynamics(x::Array{T, 1} where {T <: Real},
	                  u::Array{T, 1} where {T <: Real})
	# x1,x2,x3 are lead vehicle variables
    dx1 = x[2] # dposition
    dx2 = x[3] # dvelocity
    dx3 = -2 * x[3] + 2 * a_lead - mu * x[2]^2 # daccel
	# x4, x5, x6 are ego vehicle variables
	# which is opposite from the notation used in the paper...
	dx4 = x[5]
	dx5 = x[6]
	dx6 = -2 * x[6] + 2 * u[1] - mu * x[5]^2
    return [dx1, dx2, dx3, dx4, dx5, dx6]
end

acc_ẋ₃ = :(-2 * x3 + 2 * $a_lead - $mu * x2^2)
acc_ẋ₆ = :(-2 * x6 + 2 * u1 - $mu * x5^2)

function acc_dynamics_overt(range_dict::Dict{Symbol, Array{T, 1}} where {T <: Real},
	                              N_OVERT::Int,
							      t_idx::Union{Int, Nothing}=nothing)
	if isnothing(t_idx)
		v1 = acc_ẋ₃
		v1_oA = overapprox(v1, range_dict; N=N_OVERT, ϵ=0.5)

		v2 = acc_ẋ₆
		v2_oA = overapprox(v2, range_dict; N=N_OVERT, ϵ=0.5)
	else
		#v1 = "x2_$t_idx^2" # debug idea: try with simpler dynamics
		v1 = "-2 * x3_$t_idx + 2 * $a_lead - $mu * x2_$t_idx^2"
    	v1 = Meta.parse(v1)
		v1_oA = overapprox(v1, range_dict; N=N_OVERT, ϵ=0.5)

		v2 = "-2 * x6_$t_idx + 2 * u1_$t_idx - $mu * x5_$t_idx^2"
		v2 = Meta.parse(v2)
		v2_oA = overapprox(v2, range_dict; N=N_OVERT, ϵ=0.5)
	end
    oA_out = add_overapproximate([v1_oA, v2_oA])
    return oA_out, [v1_oA.output, v2_oA.output]
end

function acc_update_rule(input_vars, control_vars, overt_output_vars)
    integration_map = Dict(input_vars[1] => input_vars[2],
                           input_vars[2] => input_vars[3],
                           input_vars[3] => overt_output_vars[1],
                           input_vars[4] => input_vars[5],
                           input_vars[5] => input_vars[6],
                           input_vars[6] => overt_output_vars[2])
    return integration_map
end

acc_input_vars = [:x1, :x2, :x3, :x4, :x5, :x6]
acc_control_vars = [:u1]

T_gap = 1.4
acc_measurement_model = [[1, 0, 0, -1, -T_gap, 0]::Array{Float64}] # relative distance  - v_ego*Tgap
# but a measurement model can contain multiple rows of measurements!

ACC = OvertProblem(
	acc_dynamics,
	acc_dynamics_overt,
	acc_update_rule,
	acc_input_vars,
	acc_control_vars,
 	acc_measurement_model
 )

"""
Note: Compared this dynamics variable ordering in combination with controller acc_controller.nnet with controller outputs in
https://github.com/souradeep-111/sherlock/blob/master/systems_with_networks/ARCH_2019/ACC/simulation_script.m
with their different variable ordering (xxvvaa) and controller outputs match to 2 decimal places in most cases indicating successful
handling of the differing variable ordering.
Closed loop simulations will not be the same as that above script uses ode45, but it matches closely for the first few steps.
"""


module pSGWT

using PyCall

export sgwt_frame, sgwt_filter_banks

const pygsp = PyNULL()

function __init__()
    copy!(pygsp, pyimport_conda("pygsp", "pygsp", "conda-forge"))
end

function init_Graph(W)
    G = pygsp.graphs.Graph(PyReverseDims(W))
    return G
end

"""
    sgwt_transform(loc, nf, W)

SGWT\\_TRANSFORM perform the SGWT transform with MexicanHat filter as a python wrapper.
See https://pygsp.readthedocs.io/en/stable/tutorials/wavelet.html

# Input Arguments
- `loc::Int64`: the vertex index that the output wavelet centered at.
- `W::Matrix{Float64}`: weighted adjacency matrix.
- `nf::Int64`: default is 6. Number of filters.

# Output Argument
- `frame::Array{Float64, 3}`: a N x N x nf matrix.

"""
function sgwt_frame(W; nf = 6)
    G = init_Graph(W)
    G.estimate_lmax()
    g = pygsp.filters.MexicanHat(G, Nf = nf)
    # np = pyimport("numpy")
    # s = np.zeros(G.N); s[loc] = 1
    # s = g.filter(s, method="chebyshev")
    return g.compute_frame()
end

function sgwt_filter_banks(W, 𝛌; nf = 6)
    G = init_Graph(W)
    G.estimate_lmax()
    g = pygsp.filters.MexicanHat(G, Nf = nf)
    np = pyimport("numpy")
    e = np.array(𝛌)
    return g.evaluate(e)
end

end


struct TimePolynomial{T, P<:AbstractPolynomial{T}} <: AbstractPolynomial{T}
    p::P
    function TimePolynomial{T, P}(p::P) where {T, P<:AbstractPolynomial{T}}
        @assert length(variables(p)) == 1
        new{T, P}(p)
    end
end

TimePolynomial(p::P) where {T, P<:AbstractPolynomial{T}} = TimePolynomial{T, P}(p)

(p::TimePolynomial)(x) = p.p(variable(p) => x)
variable(p::TimePolynomial) = first(variables(p))
differentiate(p::TimePolynomial) = TimePolynomial(differentiate(p, first(variables(p))))
polynomial(p::TimePolynomial) = p.p
nvariables(::TimePolynomial) = 1
variables(p::TimePolynomial) = variables(p.p)
Base.show(io::IO, p::TimePolynomial) = print(io, p.p)

function PiecewiseFunctions.PiecewiseFunction(ps::AbstractVector{<:AbstractPolynomial}, breaks::AbstractVector)
    if eltype(ps) <: TimePolynomial
        error("huh?")
    end
    PiecewiseFunction(TimePolynomial.(ps), breaks)
end

PiecewiseFunctions.PiecewiseFunction(ps::AbstractVector{<:TimePolynomial}, breaks::AbstractVector) =
    PiecewiseFunction{eltype(ps), eltype(breaks), typeof(ps), typeof(breaks)}(ps, breaks)

function cubic_spline(Δt, x0, xf, ẋ0, ẋf)
    @polyvar t
    # p(0) = x0
    # p'(0) = x'0
    # p = a + bt + ct^2 + dt^3
    # a = x0
    # b = x'0
    # b + 2ct + 3dt^2 = x'f
    # a + bt + ct^2 + dt^3 = xf
    # bt + 2ct^2 + 3dt^3 = tx'f
    # 3a + 3bt + 3ct^2 + 3dt^3 = 3xf
    # 3a + 2bt + ct^2 = 3xf - tx'f
    a = x0
    b = ẋ0
    c = (3*xf - Δt*ẋf - 3*a - 2*b*Δt) / (Δt^2)
    d = (ẋf - b - 2*c*Δt) / (3*Δt^2)
    TimePolynomial(a + b*t + c*t^2 + d*t^3)
end

function cubic_spline(times::AbstractVector, positions::AbstractVector, velocities::AbstractVector)
    @assert length(times) > 1
    @assert length(times) == length(positions) == length(velocities)
    PiecewiseFunction(
        [cubic_spline(times[i] - times[i-1], positions[i-1], positions[i], velocities[i-1], velocities[i]) for i in 2:length(times)],
        times
    )
end


get_pre(::Synapse, sys::ODESystem) = sys.V
get_post(::Synapse, sys::ODESystem) = sys.V

my_pre(::Synapse, sys::ODESystem) = sys.Vpre
my_post(::Synapse, sys::ODESystem) = sys.Vpost
post_connector(::Synapse) = :Isyn

mutable struct Chol{T} <: Synapse
    ḡChol::T
    s::T
    Eₛ::T # mV
    k₋::T # ms
    Vth::T # mV
    δ::T   # mV
end
Chol(x) = Chol(x,0.,-80., 0.01, -35., 5.)
s̄(syn::Chol, Vpre)  = 1. / (1. + exp((syn.Vth - Vpre)/syn.δ))
τs(syn::Chol, Vpre) = (1. - s̄(syn, Vpre))/syn.k₋
ionic_current(::Chol, sys::ODESystem) = sys.IChol

mutable struct Glut{T} <: Synapse
    ḡGlut::T
    s::T
    Eₛ::T
    k₋::T
    Vth::T
    δ::T
end
Glut(x) = Glut(x,0.,-70., 0.025, -35., 5.)
s̄(syn::Glut, Vpre)  = 1. / (1. + exp((syn.Vth - Vpre)/syn.δ))
τs(syn::Glut, Vpre) = (1. - s̄(syn, Vpre))/syn.k₋
ionic_current(::Glut, sys::ODESystem) = sys.IGlut

function channel_dynamics(ch::Chol, Vpre, Vpost)
    states = @variables s(t) IChol(t)
    parameters = @parameters ḡChol
    eqs = [ D(s)  ~     (1/τs(ch, Vpre))*(s̄(ch, Vpre) - s),
            IChol ~     -ḡChol*s*(Vpost - ch.Eₛ)]
    current = [eqs[2]]
    defaultmap = [s => ch.s, ḡChol => ch.ḡChol]
    return eqs, states, parameters, current, defaultmap
end

function channel_dynamics(ch::Glut, Vpre, Vpost)
    states = @variables s(t) IGlut(t)
    parameters = @parameters ḡGlut
    eqs = [ D(s) ~ (1/τs(ch, Vpre))*(s̄(ch, Vpre) - s),
            IGlut ~ -ḡGlut*s*(Vpost - ch.Eₛ)]
    current = [eqs[2]]
    defaultmap = [s => ch.s, ḡGlut => ch.ḡGlut]
    return eqs, states, parameters, current, defaultmap
end


include("../src/NLEVP/perturbation.jl")
println("Building WavesAndEigenvalues.jl...")

if "JULIA_WAE_PERT_ORDER" in keys(ENV)
    N=ENV["JULIA_WAE_PERT_ORDER"]
    N=parse(Int,N)
else
    N=16
end
println(" for order $N...")
disk_MN(N)
println("Done!")


## Load Packages
using FreeRay, DataFrames, CSV
using Base.Threads

## Impedance input
# create an empty dataframe
df1 = DataFrame( freq = Float64[],   # 1/3-octave freq.
                sigma = Float64[],  # flow resistivity
                impedMl = Int32[],
                Zc = Complex{Float64}[])

#Zc = Complex{Float64}
# 1/3-octave, freq 15 >2000 Hz
CenterFreq = getCenterFreq(bandwidth=3,lim=(15,2000))

# Impedance models
sigma = [10,200,2000]*1000 # Pa s/m2 (See Table 5.1 Bies and Hansen)

for i_1=1:length(CenterFreq)
        for i_2=1:length(sigma)

                f = CenterFreq[i_1]; σ = sigma[i_2]; Ω = 0.5 # porosity
                local Zc = [  impedanceM1(f,σ),     # tested!
                        impedanceM2(f,σ),     # tested!
                        impedanceM3(f,σ,Ω),    # tested!
                        impedanceM4(f,σ,Ω),    # tested!
                        impedanceM5(f,σ,Ω),    # tested!
                        impedanceM6(f,σ,Ω),    # tested!
                        impedanceM7(f,σ,Ω),    # tested!
                        impedanceM8(f,σ,Ω),    # tested!
                        impedanceM9b(f,σ,Ω)     ]

                for i in 1:9
                        push!(df1, (f, σ, i,Zc[i]))
                end

        end # end i_2
end # end i_1

## Wind input
df2 = DataFrame( alpha = Float64[],   # 1/3-octave freq.
                z0 = Float64[],  # flow resistivity
                windMl = Int32[])

# wind models
alpha = [0.13, 0.21, 0.39]
zz0 = [0.002, 0.03, 0.26]

vref = 2.7; href = 10 # wind speed and ref wind
h = range(0,1000,step = 1.0) # height step

rows = length(h); cols = length(alpha)*8
vh = Array{Float64,2}(undef, rows, cols)

for i_1 = 1:length(alpha)

    α = alpha[i_1]; z0 = zz0[i_1]
    k = (i_1-1)*8 + 1

    # log Linear - law model
    for i=2:length(h)
        vh[i,k] = windprofile_logLL(h[i],z0,vref,href)
    end

    # logarithmic law model
    vh2 = similar(h) # pre-allocation
    for i=2:length(h)
        vh[i,k+1]  = windprofile_logL(h[i],z0,vref,href)
    end

    # DH log model
    vh3 = similar(h) # pre-allocation
    for i=2:length(h)
        vh[i,k+2] = windprofile_DH(h[i],z0,vref,href)
    end

    # power law model
    vh4 = similar(h) # pre-allocation
    for i=2:length(h)
        vh[i,k+3] = windprofile_PL_α(h[i],1/7,vref,href)
    end

    # power law model mean anual
    vh5 = similar(h) # pre-allocation
    for i=2:length(h)
        vh[i,k+4] = windprofile_PL_α(h[i],α,vref,href)
    end

    # power law model Counihan model
    vh6 = similar(h) # pre-allocation
    αC = 0.24 + 0.096*log(z0) + 0.016*(log(z0))^2
    for i=2:length(h)
        vh[i,k+5] = windprofile_PL_α(h[i],αC,vref,href)
    end

    # power law SH model
    vh7 = similar(h) # pre-allocation
    αSH = 1.03 + 0.31*log(z0) + 0.03*(log(z0))^2
    for i=2:length(h)
        vh[i,k+6] = windprofile_PL_α(h[i],αSH,vref,href)
    end

    # power law αP model
    vh8 = similar(h) # pre-allocation
    for i=2:length(h)
        vh[i,k+7] = windprofile_PL_α_P(h[i],z0,vref,href)
    end

    for i in 1:8
        push!(df2, (α, z0, i))
    end

end # end i_1
vh[1,:] .= 0.0
df2.idxWindprofile = 1:24

## Combine dataframe for easily control

df = crossjoin(df1, df2, makeunique = true)
TLoss = Array{Float32,2}(undef, nrow(df), 20)

## Bellhop input setup

function inforEx(df,simID) # extract information in dataframe
    freqSim = df.freq[simID]
    ZcSim = df.Zc[simID]
    idxWind = df.idxWindprofile[simID]
    return freqSim, ZcSim, idxWind
end

# Multiple recievers on grid
receiver = Receiver(
    depth_point = 1,
    range_point = 20,
    depth = Vec2(-1.5f0,-1.5f0),
    range = Vec2(0.5f0,10f0)
)

# Terrain geometry
terrain = Terrain(
    interp_type = "L",
    profile = (Vec2([0f0,5f0,10f0],[0f0,0f0,0f0]))
)

@time @threads for simID = 1:1#nrow(df)
    println(simID)

    freqSim, ZcSim, idxWind = inforEx(df,simID)

    # Point source at 80 m
    source = Source(
        frequency = Float32.(freqSim), # frequency dependence
        height = -80f0
    )

    ### Boundary condition
    Theta, Rmag, Rphase = R_coeff(ZcSim;len=100) # Zc dependence
    brc = Vec3(Theta,Rmag,Rphase)
    trc = Vec3([0f0,45f0,90f0],[0f0,0f0,0f0],[0f0,0f0,0f0])
    reflection = Reflection_Coeff(
        top_coeff = trc,
        bottom_coeff = brc
    )

    # Sound speed profile
    hneg = -h[end:-1:1]
    soundspeed = 343 .+ vh[:,idxWind][end:-1:1]
    sspl = Vec2(Float32.(hneg),Float32.(soundspeed)) # sound speed profile
    ssp = SSP(sound_speed_profile = sspl)

    ### Analysis
    opt = Analysis(
        filename = "Uncertainty_case1_$simID",
        analyse = "CB",
        option1 = "CFW",
        option2 = "F*",
        num_ray = 16001,
        alpha = Vec2(-80.0f0,80.0f0),
        box = Vec2(10f0,1000.0f0),
        step= 0.1
    )

    # Create environment files
    Environment(opt,source, receiver,ssp,terrain,reflection)

    # Run Bellhop
    fn= opt.filename
    filename = "temp\\$fn"
    run_bellhop = `bellhop $filename`
    run(run_bellhop)
    rmfile(filename)

    #p1 = PlotRay("$filename.ray",
    #        xlabs = "Range, m",
    #        ylabs = "Height, m")

    #p2 = PlotShd("$filename.shd";
    #        xlabs = "Range, m",
    #        ylabs = "Transmission loss, dB",
    #        cblabs = "dB",
    #        climb = (40,80))

    #PlotTlr("$filename.shd", -1.5;
    #        xlabs = "Range, m",
    #        ylabs = "Transmission loss, dB",
    #        xlim = (0,10000),
    #        ylim = (20,120))

    pressure, Pos_r_r, Pos_r_z = read_shd("$filename.shd")
    tlt = -20.0f0 * log10.(abs.(pressure))

    TLoss[simID,:] = tlt

end # end loop simID

CSV.write("df.csv", df)
CSV.write("TLoss.csv",  DataFrame(TLoss), writeheader=false)


using ADCME
using LinearAlgebra
using Random

npoints = parse(Int64, ARGS[1])
SEED = parse(Int64, ARGS[2])

Random.seed!(SEED)

x0 = LinRange(0, 1, 100)|>collect
y0 = sin.(π*x0)

θ = Variable(fc_init([1,20,20,20,1]))
y = fc(x0, [20,20,20,1], θ)

idx = Int64.(round.(collect(LinRange(1, 100, npoints+2))))[2:end-1]

loss = sum((y[idx] - y0[idx])^2)

H = hessian(loss, θ)

sess = Session(); init(sess)
BFGS!(sess, loss)
H0 = run(sess, H)
λ = real.(eigvals(H0))
nlambda = length(findall(λ .> λ[end] * 1e-6))

db = Database("hessian.db")
execute(db, """
CREATE TABLE IF NOT EXISTS eigvals (
    seed integer,
    npoints integer,
    nlambda integer,
    PRIMARY KEY (seed, npoints)
)""")
execute(db, """
INSERT OR REPLACE INTO eigvals VALUES ($SEED, $npoints, $nlambda)
""")
close(db)


function part1(input)
    rules = parse_rules(input)
    return count(x -> "shiny gold" in x,
                 values(transitive_contained_colors(rules)))
end

function part2(input)
    rules = parse_rules(input)
    required_bags = 0
    queue = Dict("shiny gold" => 1)
    while !isempty(queue)
        color, quantity = pop!(queue)
        required_bags += quantity
        for (color2, quantity2) in rules[color]
            queue[color2] = get(queue, color2, 0) + quantity * quantity2
        end
    end
    return required_bags - 1
end

function parse_rules(input)
    rules = Dict{String, Dict{String, Int}}()
    for line in readlines(input)
        parse_rule!(rules, line)
    end
    return rules
end

function parse_rule!(rules, line)
    outer, inner = match(r"(.+) bags contain (.+)", line).captures
    rule = get!(rules, outer, Dict{String, Int}())
    inner == "no other bags." && return
    for contained in split(inner, ", ")
        quantity, color = match(r"(\d)+ (.+) bag", contained).captures
        rule[color] = parse(Int, quantity)
    end
end

function transitive_contained_colors(rules)
    contained_colors = Dict{String, Vector{String}}()
    for color in keys(rules)
        found_colors = collect(keys(rules[color]))
        i = 1
        while i <= length(found_colors)
            new_colors = setdiff(keys(rules[found_colors[i]]), found_colors)
            append!(found_colors, new_colors)
            i += 1
        end
        contained_colors[color] = found_colors
    end
    return contained_colors
end


# ------------------------------------------------------------------------------
# Quick version of functions (for testing) (drawn from some of SJV's old code)
function q_nextboundary(ns, a, x, v)
    nconstr = length(a)
    bts = a./(ns*v) - ns*x./(ns*v)
    mask1 = Bool[isapprox((ns*v)[i], 0.0, atol=1.e-10)  for i=1:nconstr]
    mask2 = Bool[isapprox(bts[i], 0.0, atol=1.e-10)  for i=1:nconstr]
    for (i, e) ∈ enumerate(mask1)
        e && (bts[i] = Inf)
    end
    for (i, e) ∈ enumerate(mask2)
        e && (bts[i] = Inf)
    end
    bts+=(bts.<0.0)*Inf
    (t,j)=findmin(bts)
    (t,vec(ns[j,:]))
end
# ------------------------------------------------------------------------------

Random.seed!(123)

p = 50
c = 10
normals = randn(c,p)
intercepts = randn(c)

unconstr = Unconstrained()
polyconstr = Polygonal(normals,intercepts)

x = randn(p)
v = randn(p)

res = nextboundary(unconstr, x, v)

@test reduce(*, map(isnan, res))

(t_b, n_b) = q_nextboundary(normals, intercepts, x, v)
(t, n)     = nextboundary(polyconstr, x, v)

@test abs(t_b - t) <= 1e-12 && norm(n_b - n) <= 1e-12


function smooth_pi0{T<:FloatingPoint}(pValues::Vector{T}, lambda = 0.05:0.05:0.95)
    validPvalues(pValues)
    #validPvalues(lambda) ## TODO check bounds
    n = length(pValues)
    ## CHCK: Only for smoothing (due to cubic spline)?
    if length(lambda) < 4
        throw(ArgumentError())
    end
    if !issorted(lambda)
        throw(DomainError())
    end
    pi0 = Float64[mean(pValues .>= l) / (1-l) for l in lambda]
    ##CoordInterpGrid(lambda, pi0, BCnil, InterpCubic) ## broken
end


#####
##### `mean`
#####

_denom(x, dims::Colon) = length(x)
_denom(x, dims::Integer) = size(x, dims)
_denom(x, dims) = mapreduce(i->size(x, i), Base.mul_prod, unique(dims), init=1)

# TODO: We have `mean(f, x; dims)` as of 1.3.0-DEV.36
# https://github.com/JuliaDiff/ChainRules.jl/issues/85
function rrule(::typeof(mean), x::AbstractArray{<:Real}; dims=:)
    y_sum, sum_pullback = rrule(sum, x; dims=dims)
    n = _denom(x, dims)
    function mean_pullback(ȳ)
        _, ∂sum_x = sum_pullback(ȳ)
        ∂x = unthunk(∂sum_x) / n
        return (NoTangent(), ∂x)
    end
    return y_sum / n, mean_pullback
end

#####
##### variance
#####

function rrule(
    ::typeof(Statistics.var),
    x::AbstractArray{<:Number};
    corrected::Bool=true,
    dims=:,
    mean=mean(x, dims=dims)
)
    y = Statistics.var(x; corrected=corrected, mean=mean, dims=dims)
    function variance_pullback(dy)
        pre = 2 // (_denom(x, dims) - corrected)
        dx = pre .* unthunk(dy) .* (x .- mean)
        return (NoTangent(), ProjectTo(x)(dx))
    end
    y, variance_pullback
end

function rrule(
    ::typeof(Statistics.std),
    x::AbstractArray{<:Number};
    corrected::Bool=true,
    dims=:,
    mean=mean(x, dims=dims)
)
    y = Statistics.std(x; corrected=corrected, mean=mean, dims=dims)
    function std_pullback(dy)
        pre = 1 // (_denom(x, dims) - corrected)
        dx = pre .* unthunk(dy) .* (x .- mean) ./ y
        return (NoTangent(), ProjectTo(x)(dx))
    end
    y, std_pullback
end


include("viaCheb.jl")
using .bycheb
using LinearAlgebra, Plots

method="Explicit"
#method="Implicit"
#method="Mixed"; μ = 0.5

α= 1.0
ν = 30.0
s = 30.0
Lrel = 10

#N=40
N = 71

M = 1

TINY = 0.01

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

function eye(n::Integer)
    diagm(ones(n))
end

nn = N÷2 + N%2
r = bycheb.ring(N,(α,ν,s,Lrel))
ws = sqrt(3*ν/s) # wave speed
v=[ones(nn);zeros(nn)]
#Σ = view(v,    1:nn )
#Φ = view(v, nn+1:e)
minAd1 = minimum( abs.(diff(r.y)./(abs.(r.u[1:end-1]).+ws)))
minAd2 = minimum( abs.(diff(r.y)./(abs.(r.u[2:end]).+ws)))
minDiff = minimum( diff(r.y).^2 / (3*ν) )
dt = TINY * 0.5 * min(minAd1, minAd2, minDiff)

Nm = trunc(Int,1.0/dt)
for i in 1:M*Nm
    global v
    local w
    if method=="Explicit"
        w = [abs.(v[1:nn]); -abs.(v[nn+1:end])]; w[1]=0.0; w[nn+1]=0.0; w./=sum(w[1:nn]);
        #Base.Threads.@threads
        for j in 1:2nn
            v[j] = w[j] + dt*sum((-r.AA[j,:]+r.B[j,:]).*w)
        end
    elseif method=="Implicit"
        w = [abs.(v[1:nn]); -abs.(v[nn+1:end])]; w[1]=0.0; w[nn+1]=0.0; w./=sum(w[1:nn]);
        v = (eye(2*nn) - dt*(-r.AA+r.B))\w
    elseif method=="Mixed"
        w  = [abs.(v[1:nn]); -abs.(v[nn+1:end])]; w[1]=0.0; w[nn+1]=0.0; w./=sum(w[1:nn]);
        vv = w + μ*dt*(-r.AA+r.B)*w
        v = (eye(2*nn) - μ*dt*(-r.AA+r.B))\vv
    end
end
ynn=r.y[1:nn]
Σ = v[1:nn]
Φ = v[nn+1:end];
L0 = r.Ls["L0"]
Lcrit = r.Ls["Lcrit"]
Lvisc = r.Ls["Lvisc"]

Plots.plot(ynn,Σ)
Plots.vline!([L0-Lcrit],label="crit",legend=:bottomleft)
Plots.vline!([L0-Lvisc],label="visc")
Plots.vline!([L0],label="moon")


@testset "SOM training helper functions" begin

    @testset "gridRectangular" begin
        grid = GigaSOM.gridRectangular(5, 5)
        @test size(grid) == (25, 2)
    end

    @testset "Kernels" begin
        @test isapprox(
            gaussianKernel(Vector{Float64}(1:7), 2.0),
            [0.1760326, 0.1209853, 0.0647587, 0.0269954, 0.0087641, 0.0022159, 0.0004363],
            atol = 1e-4,
        )
        @test isapprox(
            bubbleKernel(Vector{Float64}(1:6), 5.0),
            [0.979796, 0.916515, 0.8, 0.6, 0, 0],
            atol = 0.001,
        )
        @test isapprox(
            thresholdKernel(Vector{Float64}(1:10), 5.0),
            [1.0, 1.0, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0],
            atol = 1e-3,
        )
        @test isapprox(
            thresholdKernel(Vector{Float64}(1:10), 10.1),
            [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 0.0, 0.0],
            atol = 1e-3,
        )
    end

    @testset "Radii-generating functions" begin
        radii1 = expRadius().(5.0, 0.5, Vector(1:10), 10)
        radii2 = expRadius(-10.0).(10.0, 0.1, Vector(1:20), 20)
        radii3 = linearRadius.(50.0, 0.001, Vector(1:30), 30)
        @test isapprox(radii1[1], 5.0)
        @test isapprox(radii1[10], 0.5)
        @test isapprox(radii2[1], 10.0)
        @test isapprox(radii2[20], 0.1)
        @test isapprox(radii3[1], 50.0)
        @test isapprox(radii3[30], 0.001)
        @test all(isapprox.(radii1[1:9] ./ radii1[2:10], radii1[1] / radii1[2]))
        #note: radius2 is adjusted and thus not really exponential
        @test all(isapprox.(radii3[1:29] .- radii3[2:30], radii3[1] - radii3[2]))
    end

    @testset "distMatrix" begin
        g = GigaSOM.gridRectangular(2, 2)
        dm = GigaSOM.distMatrix(Euclidean())(g)
        @test size(dm) == (4, 4)
        @test all([dm[i, i] == 0 for i = 1:4])
    end
end


using ASP
using LinearOperators
#include("../src/BPdual.jl")

# Setup a simple test problem.
srand(0)
const m = 600
const n = 2560
const k = 30                # No. of rows, columns, and nonzeros
p = randperm(n)[1:k]  # Position of nonzeros in x
x    = zeros(n)             # Generate sparse solution
x[p] = randn(k)
A = randn(m,n)     # Gaussian m-by-n ensemble
b = A*x            # Compute the RHS vector

#write_matfile("small.mat", A=A, b=b, lambda=λ)
const λ = 0.#0.1*norm(A'*b, Inf)

active = zeros(Int, 0)
state = zeros(Int, n)
S = zeros(m, 0)
R = zeros(0, 0)
y = zeros(m)
bl = -ones(n)
bu = +ones(n)

x, r, inform = bpdual(A, b, λ, bl, bu,
                      trim = 1, loglevel=1)

function partialDCTAdjoint(idx,n,y)
    z = zeros(n)
    z[idx] = y
    idct!(z)
    z *= sqrt(n)
    return z
end
function partialDCTForward(idx,n,x)
    z = dct(x) / sqrt(n)
    return z[idx]
end

m, n = 6000, 25600
bl = -ones(n)
bu = +ones(n)
k = 300
p = randperm(n)[1:k]
x = zeros(n)
x[p] = randn(k)
idx = randperm(n)[1:m]
A = LinearOperator(m, n, Float64, false, false,
                   v -> partialDCTForward(idx,n,v),
                   Nullable{Function}(),
                   w -> partialDCTAdjoint(idx,n,w))
b = A*x

@profile x, r, inform = bpdual(A, b, 0.0, bl, bu)


# utilities for x ↔ scale factor ↔ redshift
a2z(a::T) where T = one(T)/a - one(T)
z2a(z::T) where T = one(T)/(z + one(T))
a2x(a) = log(a)
x2a(x) = exp(x)
z2x(z) = a2x(z2a(z))
x2z(x) = a2z(x2a(x))

# utility function for constructing an interpolator
spline(f, x_grid) = scale(interpolate(f, BSpline(Cubic(Line(OnGrid())))), x_grid)
spline_∂ₓ(f, x_grid) = spline([Interpolations.gradient(f, x)[1] for x in x_grid], x_grid)
spline_∂ₓ²(f, x_grid) = spline([Interpolations.hessian(f, x)[1] for x in x_grid], x_grid)

δ_kron(i, j) = (i == j) ? 1 : 0

#utilities for mapping between comoving momenta and unit interval
to_ui(lq,lqmi,lqma) = -1 + (1- (-1)) / (lqma-lqmi) * (lq-lqmi)
from_ui(x,lqmi,lqma) = lqmi + (lqma- lqmi) / (1- (-1)) * (x- (-1))
dxdq(q,logqmin,logqmax) = (1+to_ui(1+logqmin,logqmin,logqmax))/(q*log(10))
xq2q(x,logqmin,logqmax) = 10.0 ^ from_ui(x,logqmin,logqmax)

# utilities for applying an FFTLog transformation ===

struct FFTLogPlan{T, OT, AA<:AbstractArray{Complex{T},1},
                  AAR<:AbstractArray{T,1}, PT<:Plan, IPT<:Plan}
    L::T
    N::Int
    μ::OT
    q::T
    r₀::T
    k₀r₀::T
    uₘ::AA
    r::AAR
    k::AAR
    fftplan!::PT
    ifftplan!::IPT
end

function plan_fftlog(r::AA, μ, q, k₀r₀=1.0;
                     kropt=true) where {T, AA<:AbstractArray{T,1}}
    logrmin = log(first(r))
    logrmax = log(last(r))
    r₀ = exp((logrmin + logrmax)/2)
    @assert logrmin < logrmax
    N = length(r)
    L = logrmax - logrmin
    dlnr = L / (N - 1)
    if kropt
        k₀r₀ = k₀r₀_low_ringing(N, μ, q, L, k₀r₀)
    end
    k₀ = k₀r₀ / r₀
    Nhalf = N ÷ 2
    n = range(-Nhalf,Nhalf,length=N)
    k = reverse(k₀ .* exp.(n .* L / N))

    m = fftfreq(N, N)  # get indicies that go from [-N/2] to [N/2]
    uₘ_coeff = similar(r, Complex{T})
    for i in eachindex(m)
        uₘ_coeff[i] = uₘ(m[i], μ, q, dlnr, k₀r₀, N)
    end
    uₘ_coeff[N÷2+1] = real(uₘ_coeff[N÷2+1])  # eq 19
    fftplan! = plan_fft!(uₘ_coeff)
    ifftplan! = plan_ifft!(uₘ_coeff)
    return FFTLogPlan(L, N, μ, q, r₀, k₀r₀, uₘ_coeff, r, k, fftplan!, ifftplan!)
end

U_μ(μ, x) = exp(x * log(2.) - lgamma(0.5 * (μ + 1 - x)) + lgamma(0.5 * (μ + 1 + x)))
uₘ(m, μ, q, dlnr, k₀r₀, N) = (k₀r₀)^(-2π * im * m / (dlnr * N)) * U_μ(μ, q + 2π * im * m / (dlnr * N) )

function k₀r₀_low_ringing(N, μ, q, L, k₀r₀=1.0)
    # from pyfftlog
    dlnr = L / (N-1)
    xp = (μ + 1 + q) / 2
    xm = (μ + 1 - q) / 2
    y = π * im / 2 / dlnr
    zp = lgamma(xp + y)
    zm = lgamma(xm + y)
    arg = log(2 / k₀r₀) / dlnr + imag(zp + zm) / π
    return k₀r₀ * exp((arg - round(arg))* dlnr)
end

function mul!(Y, pl::FFTLogPlan, A)
    Y .= A
    Y .*= (pl.r).^(-pl.q)
    pl.fftplan! * Y
    Y .*= pl.uₘ
    pl.ifftplan! * Y
    Y .*= (pl.r).^(pl.q)
end

function ldiv!(Y, pl::FFTLogPlan, A)
    Y .= A
    Y .*= (pl.r).^(-pl.q)
    pl.fftplan! * Y
    Y ./= pl.uₘ
    pl.ifftplan! * Y
    Y .*= (pl.r).^(pl.q)
end


# by @walra356
using CairoMakie
CairoMakie.activate!() #HIDE
let
    supertitle = "adding a crosshair to exposure heatmap of camera chip"
  # define transformation from pixel coordinates to physical coordinates
    edges(p,Δx,x0) = collect(p .* Δx) .-(x0+0.5Δx)
  # parameters camera chip (pixel coordinates)
    dimX = 40; dimY = 30                        # pixel dimensions camera chip
    (px, py) = (1:dimX, 1:dimY)                 # pixel positions
    (ix, iy) = (10, 3)                          # selected pixel
  # physical coordinates
    (Δx, Δy) = (4.0, 5,0)                       # pixel lattice periods (μm)
    (Ox, Oy) = (edges(ix,Δx,0), edges(iy,Δy,0)) # manual offset (μm)
    (x, y) = (edges(px,Δx,Ox), edges(py,Δy,Oy)) # pixel positions (μm)
  # create exposure matrix σ (model alternative for measurement file)
    valmax = 500.0                              # maximum value exposure matrix
    exposure(i, j , ix, iy, wx, wy) = 0.9valmax *
                        (exp(-(((i-ix)/wx)^2 + ((Δy/Δx)*(j-iy)/wy)^2)) + 0.1)
    σ = round.(Int, [exposure(i, j, ix, iy, 6, 6) for i=1:dimX, j=1:dimY])
    σ = σ[px,py]                                # exposure matrix
    footnote = "Origin of coordinate system was shifted by ($Ox μm, $Oy μm)
                w.r.t. left-bottom edge of camera chip
                \nPixel lattice periods: (Δx = $Δx μm, Δy = $Δy μm);
                pixel position of light spot: (ix, y0) = ($(ix), $(iy))"

  # start actual plot
    theme = Theme(fontsize = 12, colormap = :gist_earth, resolution = (750,500))
    set_theme!(theme)
    fig = Figure()
  # collect attributes
    fsize = fig.scene.attributes.fontsize.val
    attr = (xlabelsize = 6fsize/5, ylabelsize = 6fsize/5, titlesize = 7fsize/5,
            xautolimitmargin = (0,0), yautolimitmargin = (0,0), )
    attr1 = (attr..., title = "raw data",
            xticks = [px[1], ix, px[end]] , yticks = [py[1], iy, py[end]] ,
            xlabel = "x (pixel)", ylabel = "y (pixel)",
            aspect = (Δx * size(σ,1))/(Δy * size(σ,2)), )
    attr2 = (attr..., title = "physical scale",
            xticks = [x[1]-Δx/2, x[ix], 50, x[end]+Δx/2] ,
            yticks = [y[1]-Δy/2, y[iy], 50, y[end]+Δy/2],
            xlabel = "x (μm)", ylabel = "y (μm)",
            aspect = (Δx * size(σ,1))/(Δy * size(σ,2)), )
    attrc = (label = "exposure (counts)", ticksize=6fsize/5, tickalign = 1,
            width = 15, height = Relative(0.98), ) # tweaked
  # create axes, add contents
    ax1 = Axis(fig; attr1...)
    ax2 = Axis(fig; attr2...)
    hm1 = heatmap!(ax1, px, py, σ)
    hm2 = heatmap!(ax2, x, y, σ)
    axc = Colorbar(fig, hm1; attrc...)
    axn = Label(fig, text = footnote, textsize = 6fsize/5)
    axt = Label(fig, text = supertitle, textsize = 2fsize)
    lnh = lines!(ax2, x , fill(0, length(x)), color = :white, linewidth = 1)
    lnv = lines!(ax2, fill(0, length(y)), y, color = :white, linewidth = 1)
  # create layout and show figure
    fig[1,1] = ax1
    fig[1,2] = ax2
    fig[1,3] = axc
    fig[2,:] = axn
    fig[0,:] = axt
    fig
    save(joinpath(@__DIR__, "output", "heatmapScaleCrosshair.png"), fig, px_per_unit = 2.0) # HIDE
end
using Pkg # HIDE
Pkg.status(["CairoMakie"]) # HIDE


# ---
# title: 350. Intersection of Two Arrays II
# id: problem350
# author: Tian Jun
# date: 2020-10-31
# difficulty: Easy
# categories: Hash Table, Two Pointers, Binary Search, Sort
# link: <https://leetcode.com/problems/intersection-of-two-arrays-ii/description/>
# hidden: true
# ---
#
# Given two arrays, write a function to compute their intersection.
#
# **Example 1:**
#
#
#
#     Input: nums1 = [1,2,2,1], nums2 = [2,2]
#     Output: [2,2]
#
#
# **Example 2:**
#
#
#
#     Input: nums1 = [4,9,5], nums2 = [9,4,9,8,4]
#     Output: [4,9]
#
# **Note:**
#
#   * Each element in the result should appear as many times as it shows in both arrays.
#   * The result can be in any order.
#
# **Follow up:**
#
#   * What if the given array is already sorted? How would you optimize your algorithm?
#   * What if _nums1_ 's size is small compared to _nums2_ 's size? Which algorithm is better?
#   * What if elements of _nums2_ are stored on disk, and the memory is limited such that you cannot load all elements into the memory at once?
#
#
## @lc code=start
using LeetCode

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


using GaussQuadrature

T = Float64

print("\nFloating point data type is ", T, '\n')
@printf("\teps = %0.2e\n\n", eps(T))

n = 10
d = rand(T, n)
e = rand(T, n)
z = zero(d)

A = SymTridiagonal(d, e[1:end-1])
D, V = eig(A)

steig!(d, e, z, 30)
idx = sortperm(d)
d = d[idx]
z = z[idx]

@printf("\nEigenvalues:\n\n")
@printf("%20s  %20s  %12s\n\n", "eig", "steig!", "difference")
for i=1:length(d)
    @printf("%20.15f  %20.15f  %12.2e\n", d[i], D[i], d[i]-D[i])
end

@printf("\nFirst component of eigenvectors:\n\n")
@printf("%20s  %20s  %12s\n\n", "eig", "steig!", "difference")
for i=1:length(d)
    @printf("%20.15f  %20.15f  %12.2e\n", z[i], V[1,i],
            abs(z[i])-abs(V[1,i]))
end


using SparseArrays
using SparseUtils
using Serialization

function create_serialize_sparse_array(nr=30, nc=70, density=0.01)
    sparse_array = SparseArrays.sprandn(nr, nc, density)
    open("sparse_array.dat", "w") do io
        serialize(io, sparse_array)
    end
    return nothing
end

function deserialize_sparse_array()
    return open("sparse_array.dat", "r") do io
        res = deserialize(io)
    end
end


#
#  File:    lo1.jl
#
#  Purpose: Demonstrates how to solve small linear
#           optimization problem using the MOSEK Python API.
##

using Mosek

printstream(msg::String) = print(msg)

############################
## Define problem data

bkc = [MSK_BK_FX
       MSK_BK_LO
       MSK_BK_UP]

# Bound values for constraints
blc = [30.0, 15.0, -Inf]
buc = [30.0, +Inf, 25.0]

# Bound keys for variables
bkx = [ MSK_BK_LO
        MSK_BK_RA
        MSK_BK_LO
        MSK_BK_LO ]

# Bound values for variables
blx = [   0.0,  0.0,    0.0,    0.0]
bux = [+Inf, 10.0, +Inf, +Inf]

numvar = length(bkx)
numcon = length(bkc)

# Objective coefficients
c = [ 3.0, 1.0, 5.0, 1.0 ]

# Below is the sparse representation of the A
# matrix stored by column.
A = sparse([1, 2, 1, 2, 3, 1, 2, 2, 3],
           [1, 1, 2, 2, 2, 3, 3, 4, 4],
           [3.0, 2.0, 1.0, 1.0, 2.0, 2.0, 3.0, 1.0, 3.0 ],
           numcon,numvar)

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

maketask() do task
    putstreamfunc(task,MSK_STREAM_LOG,printstream)

    putobjname(task,"lo1")

    # Append 'numcon' empty constraints.
    # The constraints will initially have no bounds.
    appendcons(task,numcon)
    for i=1:numcon
        putconname(task,i,@sprintf("c%02d",i))
    end

    # Append 'numvar' variables.
    # The variables will initially be fixed at zero (x=0).
    appendvars(task,numvar)
    for j=1:numvar
        putvarname(task,j,@sprintf("x%02d",j))
    end

    putclist(task,[1,2,3,4], c)
    putacolslice(task,1,numvar+1,A)

    putvarboundslice(task, 1, numvar+1, bkx,blx,bux)

    # Set the bounds on constraints.
    # blc[i] <= constraint_i <= buc[i]
    putconboundslice(task,1,numcon+1,bkc,blc,buc)

    # Input the objective sense (minimize/maximize)
    putobjsense(task,MSK_OBJECTIVE_SENSE_MAXIMIZE)

    # Solve the problem
    optimize(task)

    # Print a summary containing information
    # about the solution for debugging purposes
    solutionsummary(task,MSK_STREAM_MSG)

    # Get status information about the solution
    solsta = getsolsta(task,MSK_SOL_BAS)

    if solsta in     [ MSK_SOL_STA_OPTIMAL,
                      MSK_SOL_STA_NEAR_OPTIMAL ]
        xx = getxx(task,MSK_SOL_BAS)
        print("Optimal solution:")
        println(xx)
    elseif solsta in [ MSK_SOL_STA_DUAL_INFEAS_CER,
                      MSK_SOL_STA_PRIM_INFEAS_CER,
                      MSK_SOL_STA_NEAR_DUAL_INFEAS_CER,
                      MSK_SOL_STA_NEAR_PRIM_INFEAS_CER ]
        println("Primal or dual infeasibility certificate found.\n")
    elseif solsta == MSK_SOL_STA_UNKNOWN
        println("Unknown solution status")
    else
        @printf("Other solution status (%d)\n",solsta)
    end
end


@registerN Charlier AbstractCDOP1 μ
export Charlier

"""
    Charlier{μ}

References: [Koekoek and Swarttouw §1.12](https://arxiv.org/pdf/math/9602214.pdf)
"""
Charlier

abcde(::Type{<:Charlier{μ}}) where {μ} = NamedTuple{(:a, :b, :c, :d, :e)}((0, 1, 0, -1, μ))

basis_symbol(::Type{<:Charlier{μ}}) where {μ} = "cᵘ"
Polynomials.domain(::Type{<:Charlier{μ}}) where {μ} = Polynomials.Interval(0, Inf)
weight_function(::Type{<:Charlier{μ}}) where {μ} = x -> μ^x / gamma(1 + x)

function k1k0(P::Type{<:Charlier{μ}}, n::Int) where {μ}
    -one(eltype(P)) / μ
end

function classical_hypergeometric(P::Type{<:Charlier{μ}}, n::Int, x) where {μ}
    pFq((-n, -x), (), -1 / μ)
end


export savitsky_golay, central_difference

#Polynomial smoothing with the Savitsky Golay filters
#
# Sources
# ---------
# Theory: http://www.ece.rutgers.edu/~orfanidi/intro2sp/orfanidis-i2sp.pdf
# Python Example: http://wiki.scipy.org/Cookbook/SavitzkyGolay
# Modified from https://github.com/BBN-Q/Qlab.jl/blob/master/src/SavitskyGolay.jl

#This can't handle NaNs or skipped data
function savitsky_golay(x::Vector, windowSize::Integer, polyOrder::Integer; deriv::Integer=0)

#Some error checking
    @assert isodd(windowSize) "Window size must be an odd integer."
    @assert polyOrder < windowSize "Polynomial order must me less than window size."

    halfWindow = round(Int64,(windowSize-1)/2)

    #Setup the S matrix of basis vectors.
    S = zeros(windowSize, polyOrder+1)
    for ct = 0:polyOrder
        S[:,ct+1] = collect(-halfWindow:halfWindow).^(ct)
    end

    #Compute the filter coefficients for all orders
    #From the scipy code it seems pinv(S) and taking rows should be enough
    G = S*pinv(S'*S)

    #Slice out the derivative order we want
    filterCoeffs = G[:,deriv+1] * factorial(deriv);

    #Pad the signal with the endpoints and convolve with filter
    paddedX = [x[1]*ones(halfWindow); x; x[end]*ones(halfWindow)]
    y = conv(filterCoeffs[end:-1:1], paddedX)

    #Return the valid midsection
    return y[2*halfWindow+1:end-2*halfWindow]

end

#=
Outlier removal
=#

function outlier_removal_min(cov1,min_ex,ind_range=(1,length(cov1)))

    cov2=cov1[ind_range[1]:ind_range[2]]

    outlier_inds=cov2 .< percentile(cov2,min_ex)

    itp_out = interpolate((find(.!outlier_inds),), cov2[.!outlier_inds], Gridded(Linear()))

    for i in find(outlier_inds)[2:(end-1)]
        cov2[i] = itp_out[i]
    end

    cov1[ind_range[1]:ind_range[2]]=cov2
    nothing
end

function outlier_removal_max(cov1,max_ex,ind_range=(1,length(cov1)))

    cov2=cov1[ind_range[1]:ind_range[2]]

    outlier_inds=cov2 .> percentile(cov2,max_ex)

    itp_out = interpolate((find(.!outlier_inds),), cov2[.!outlier_inds], Gridded(Linear()))

    for i in find(outlier_inds)[2:(end-1)]
        cov2[i] = itp_out[i]
    end

    cov1[ind_range[1]:ind_range[2]]=cov2
    nothing
end
function outlier_removal_twosided(cov1,min_ex,max_ex,ind_range=(1,length(cov1)))

    cov2=cov1[ind_range[1]:ind_range[2]]

    outlier_inds1=cov2 .< percentile(cov2,min_ex)
    outlier_inds2=cov2 .> percentile(cov2,max_ex)

    outlier_inds = outlier_inds1 .| outlier_inds2

    itp_out = interpolate((find(.!outlier_inds),), cov2[.!outlier_inds], Gridded(Linear()))

    for i in find(outlier_inds)[2:(end-1)]
        cov2[i] = itp_out[i]
    end

    cov1[ind_range[1]:ind_range[2]]=cov2
    nothing
end

#Central Differences
#=
One implementation only calculates if points are tracked on either side
=#

function central_difference(x::Array{T,1}) where T
    y=zeros(Float64,length(x))

    for i=2:length(y)-1
        y[i]=(x[i+1] - x[i-1]) / 2
    end
    y
end

function central_difference(x::Array{T,1},tracked) where T
    y=zeros(Float64,length(x))
    diff_tracked=deepcopy(tracked)

    for i=2:length(y)-1
        if (tracked[i-1]&tracked[i+1])
            y[i]=(x[i+1] - x[i-1]) / 2
        else
            diff_tracked[i] = false
        end
    end

    tracked=diff_tracked

    y
end

function count_consecutive_bits(x,con_thres)
    interp_inds = Array{Int64,1}()

    i = 2
    while (i < (length(x)-con_thres))
        if !(x[i]) #false detected
            j = 1
            while (j<con_thres)
                if (x[i+j])

                    for k=1:j
                        push!(interp_inds,i+k)
                    end
                    break
                end
                j = j + 1
            end
            i = i+j
        else
            i = i+1
        end
    end

    interp_inds
end

function remove_two_sided_outlier(x::Array{T,1},t,min_p,max_p) where T

    min_x = percentile(x[t],min_p)
    max_x = percentile(x[t],max_p)

    for i=1:length(t)

        if t[i]
            if (x[i]<min_x)|(x[i]>max_x)
                t[i] = false
            end
        end
    end
    nothing
end

function interpolate_forward(x::Array{T,1},t,interp_ids::Array{Int64,1}) where T

    interp_x = interpolate((findall(t),),x[t],Gridded(Linear()))

    for i in interp_ids
        try
            x[i] = interp_x(i)
        catch
        end
    end
end

function reset_tracked(t,interp_ids::Array{Int64,1})
   t[interp_ids] .= true
end