module GmshDiscreteModelsTests

using Gridap
using GridapGmsh
using Gridap.Geometry
using Gridap.Helpers
using Test

function check_interpolation(model)
  order = 2
  reffe = ReferenceFE(lagrangian,Float64,order)
  V = FESpace(model,reffe)
  if num_dims(model) == 2
    u = x -> x[1]^2 + 2*x[2]^2
  elseif num_dims(model) == 3
    u = x -> x[1]^2 + 2*x[2]^2 + 3*x[3]^2
  else
    @unreachable
  end
  uh = interpolate(u,V)
  e = u - uh
  Ω = Triangulation(model)
  dΩ = Measure(Ω,2*order)
  @test sum(∫( e*e )dΩ) < 1.0e-9
end

mshfile = joinpath(@__DIR__,"t1.msh")
model = GmshDiscreteModel(mshfile)
test_discrete_model(model)
writevtk(model,"model")
check_interpolation(model)

mshfile = joinpath(@__DIR__,"twoTetraeder.msh")
model = GmshDiscreteModel(mshfile; renumber=true)
test_discrete_model(model)
check_interpolation(model)

mshfile = joinpath(@__DIR__,"..","demo","demo.msh")
model = GmshDiscreteModel(mshfile)
test_discrete_model(model)
check_interpolation(model)

mshfile = joinpath(@__DIR__,"square.msh")
model = GmshDiscreteModel(mshfile)
test_discrete_model(model)
check_interpolation(model)

mshfile = joinpath(@__DIR__,"cube.msh")
model = GmshDiscreteModel(mshfile)
test_discrete_model(model)
check_interpolation(model)

end # module


#module snake_forward

#export parallel_forward

using Dierckx

function grouper(arr)
    arr = append!(arr,arr[1])
    result = Array[]
    group = []
    i = 1
    while i < length(arr)
        group = [i]
        i = i+1
        if  i <= length(arr) && arr[i-1] < arr[i]
            while i <= length(arr) && arr[i-1] < arr[i]
                if i == length(arr)
                    push!(group,1)
                else
                    push!(group,i)
                end
                i = i+1
            end
            push!(result, group)
            i = i-1
        elseif i <= length(arr) && arr[i-1] > arr[i]
            while i <= length(arr) && arr[i-1] > arr[i]
                if i == length(arr)
                    push!(group,1)
                else
                    push!(group,i)
                end
                i = i+1
            end
            push!(result, group)
            i = i-1
        end
    end
    if result[end][end] == result[1][1] && sign(result[end][end-1]-result[end][end]) == sign(result[1][1]-result[1][2])
        e = pop!(result)
        pop!(e)
        prepend!(result[1],e)
    end

    return result
end

function piece_wise_spline(splines, bins)
    values = zeros(Float64, length(bins))
    for i = 1:length(bins)
        b = bins[i]
        values[i] = sum(map(spl -> spl(b), splines))
    end
    return values
end

function project_curve(vertices, angle, bins)
    projection = [cos(angle) sin(angle)]'; # projection direction [along with detector]
    normal = [-projection[2] projection[1]]'; # normal direction [along with rays]

    # preparing for work
    vertex_coordinates = (vertices*projection)[:,1]; # vertex coordinates in projection
    distance = vertices*normal;# distances from vertex to projection
    groups = grouper(vertex_coordinates)
    projection = zeros(Float64, length(bins))
    splines = []
    for i = 1:length(groups)
        coordinates_ordered = vertex_coordinates[groups[i]]
        heights = distance[groups[i]]
        if coordinates_ordered[1] > coordinates_ordered[2]
            reverse!(coordinates_ordered)
            heights = -reverse(heights)
        end
        spl= Spline1D(coordinates_ordered,heights, k=1, bc="zero")
        push!(splines, spl)
    end
    return piece_wise_spline(splines, bins)
end

#forward model of list of vertices
function parallel_forward(vertices,angles,bins)
    N = size(vertices,1);
    indices = 1:N
    sinogram = zeros(Float64, length(bins),length(angles));
    vertex_coordinates = zeros(Float64,N,length(angles));
    position = zeros(Float64,N,1); # pre-allocating

    for k = 1:length(angles)

        angle = angles[k]
        projection = project_curve(vertices, angle, bins)

        sinogram[:,k] = projection
    end

    return sinogram
end

#end


# https://turing.ml/dev/tutorials/02-logistic-regression/

# Import Turing and Distributions.
using Turing, Distributions

# Import RDatasets.
using RDatasets

# Import MCMCChains, Plots, and StatsPlots for visualizations and diagnostics.
using MCMCChains, Plots, StatsPlots

# We need a logistic function, which is provided by StatsFuns.
using StatsFuns: logistic

# Functionality for splitting and normalizing the data
using MLDataUtils: shuffleobs, stratifiedobs, rescale!

# Set a seed for reproducibility.
using Random
Random.seed!(0);

# Turn off progress monitor.
Turing.setprogress!(false)

# Import the "Default" dataset.
data = RDatasets.dataset("ISLR", "Default");

# Show the first six rows of the dataset.
first(data, 6)

# Convert "Default" and "Student" to numeric values.
# if else --> condition ? true_value : false_value
data[!, :DefaultNum] = [r.Default == "Yes" ? 1.0 : 0.0 for r in eachrow(data)]
data[!, :StudentNum] = [r.Student == "Yes" ? 1.0 : 0.0 for r in eachrow(data)]

# Delete the old columns which say "Yes" and "No".
select!(data, Not([:Default, :Student]))

# Show the first six rows of our edited dataset.
first(data, 6)

# summary
describe(data)

# train and split (; enable to change default argument `at`)
function split_data(df, target; at = 0.70)
    shuffled = shuffleobs(df)
    trainset, testset = stratifiedobs(row -> row[target], shuffled, p = at)
end

features = [:StudentNum, :Balance, :Income]
numerics = [:Balance, :Income]
target = :DefaultNum

trainset, testset = split_data(data, target, at = 0.05)

# Rescale variables for efficient parameter searching.
for feature in numerics
    μ, σ = rescale!(trainset[!, feature], obsdim=1) # rescale!() overwrite the memory
    rescale!(testset[!, feature], μ, σ, obsdim=1)
end

describe(trainset)
describe(testset)

# Turing requires data in matrix form, not dataframe
train = Matrix(trainset[:, features])
test = Matrix(testset[:, features])
train_label = trainset[:, target]
test_label = testset[:, target]

# Bayesian logistic regression (LR)
"""
logistic regression

    x is our set of independent variables;
    y is the element we want to predict;
    n is the number of observations we have; and
    σ is the standard deviation we want to assume for our priors.
"""
@model function logistic_regression(x, y, n, σ)
    intercept ~ Normal(0, σ)

    student ~ Normal(0, σ)
    balance ~ Normal(0, σ)
    income ~ Normal(0, σ)

    for i = 1:n
        v = logistic(intercept + student*x[i,1] + balance*x[i,2] + income*x[i,3])
        y[i] ~ Bernoulli(v)
    end
end

# Retrieve the number of observations.
n, _ = size(train)

# Sample using HMC.
m = logistic_regression(train, train_label, n, 1)
chain = sample(m, HMC(0.05, 10), MCMCThreads(), 1_500, 3)
describe(chain)
# size(chain)
plot(chain)

# the labels to use
l = [:student, :balance, :income]

# Use the corner function. Requires StatsPlots and MCMCChains.
corner(chain, l)

# Fortunately the corner plot appears to demonstrate unimodal distributions for each of our parameters, so it should be straightforward to take the means of each parameter's sampled values to estimate our model to make predictions.

# making prediction
function prediction(x::Matrix, chain, threshold)
    # Pull the means from each parameter's sampled values in the chain.
    intercept = mean(chain[:intercept])
    student = mean(chain[:student])
    balance = mean(chain[:balance])
    income = mean(chain[:income])

    # Retrieve the number of rows.
    n, _ = size(x)

    # Generate a vector to store our predictions.
    v = Vector{Float64}(undef, n)

    # Calculate the logistic function for each element in the test set.
    for i in 1:n
        num = logistic(intercept .+ student * x[i,1] + balance * x[i,2] + income * x[i,3])
        if num >= threshold
            v[i] = 1
        else
            v[i] = 0
        end
    end
    return v
end;

# Set the prediction threshold.
threshold = 0.07 # 0.5

# Make the predictions.
predictions = prediction(test, chain, threshold)

# Calculate MSE for our test set.
loss = sum((predictions - test_label).^2) / length(test_label)

# Calculate percentage of correctly predicted.
defaults = sum(test_label)
not_defaults = length(test_label) - defaults

predicted_defaults = sum(test_label .== predictions .== 1)
predicted_not_defaults = sum(test_label .== predictions .== 0)

println("Defaults: $defaults
    Predictions: $predicted_defaults
    Percentage defaults correct $(predicted_defaults/defaults)")

println("Not defaults: $not_defaults
    Predictions: $predicted_not_defaults
    Percentage non-defaults correct $(predicted_not_defaults/not_defaults)")

# 課題
# 行列でモデルのpriorや尤度を書く
# バーンインを捨てる


function make_regression2(n=100, p=3, c=5;intercept=true, noise=0.1, rng=nothing)
    if rng === nothing
        rng = Random.GLOBAL_RNG
    elseif rng isa Integer
        rng = Random.MersenneTwister(rng)
    end

    X = randn(rng, n, p)
    X = MLJBase.augment_X(randn(rng, n, p), intercept)
    A = randn(rng, p + Int(intercept), c)
    Y = (X * A) .+ (noise .* randn(rng, n, c))
    return MLJBase.table(X), MLJBase.table(Y, names=[Symbol("y$(i)") for i = 1:c])
end

function test_regression(model, X, y)
    fitresult, report, cache = fit(model, 0, X, y)
    yhat = predict(model, fitresult, X)
    fr = fitted_params(model, fitresult)
    return yhat, fr
end

function test_composition_model(ms_model, mlj_model, X, X_array)
    mlj_model_type = typeof(mlj_model)
    Xtr_ms = permutedims(
        MultivariateStats.transform(ms_model, permutedims(X_array))
    )
    fitresult, _, _ = fit(mlj_model, 1, X)
    Xtr_mlj_table = transform(mlj_model, fitresult, X)
    Xtr_mlj = matrix(Xtr_mlj_table)
    # Compare MLJ and MultivariateStats transformed matrices
    @test Xtr_mlj ≈ Xtr_ms
    # test metadata
    d = info_dict(mlj_model_type)
    @test d[:input_scitype] == Table(Continuous)
    @test d[:output_scitype] == Table(Continuous)
    @test d[:name] == string(mlj_model_type)
end


# using PyPlot
# close("all")
using jInv.Mesh
using ForwardHelmholtz
using KrylovMethods
using Base.Test

function fictitiousSourceTest2D(u,m,w,rhs)
  refineMesh(M::RegularMesh) = (getRegularMesh(M.domain, M.n*2))

  M  = getRegularMesh([-1.0 1.0 -1.0 1.0],[32,48])
  N  = 4
  err = zeros(N,2)
  # k=3
  uk = 0.0; sk= 0.0; rk = 0.0;k=1;ut=0
   for k=1:N
      M  = refineMesh(M)
  	  xn = getNodalGrid(M)

	  gamma = getABL(M,true,5*ones(Int64,M.dim),0.1);

  	  uk = u(xn[:,1],xn[:,2])
      mk = m(xn[:,1],xn[:,2]);

      rk = rhs(xn[:,1],xn[:,2]) + 1im*(w^2).*gamma[:].*mk[:].*uk;

	  A = GetHelmholtzOperator(M,mk[:],w,gamma[:],true,false);
	  V  = getVolume(M)

  	  ut = A\(-rk);

  	  res = norm(A*ut + rk,Inf);

      err[k,1] = abs(V[1,1]*dot((ut-uk),A*(ut-uk)))
      err[k,2] = norm(ut-uk,Inf)

	  # figure()
	  # imshow(reshape(mk,M.n[1]+1,M.n[2]+1))
	  # imshow(gamma'); colorbar()
	  # subplot(2,2,1)
	  # imshow(reshape(real(ut),M.n[1]+1,M.n[2]+1));;colorbar()
	  # title("ut")
	  # subplot(2,2,2)
	  # imshow(reshape(real(uk),M.n[1]+1,M.n[2]+1));;colorbar()
	  # title("uk")

	  # subplot(2,2,3)
	  # imshow(reshape(abs(uk-ut),M.n[1]+1,M.n[2]+1));colorbar()
	  # title("error")

      @printf "k=%d, n=[%d,%d], l2_err=%1.3e, factor=%1.3f linf_err=%1.3e\n" k M.n[1] M.n[2] err[k,1] err[max(k-1,1),1]/err[k,1] err[k,2]
   end

  # @test countnz(diff(log(err[:,1])).<-1.8) > 4
end

# Constant slowness
f = 1e-0;
w = 2*pi*f;
slowsq(x,y) = ones(size(x));
fictitiousSourceTest2D((x,y)->cos.(pi*x).*cos.(pi*y), slowsq,w,
      (x,y) -> (- pi.^2.*cos.(pi*x).*cos.(pi*y) - pi.^2.*cos.(pi*x).*cos.(pi*y) + slowsq(x,y).*cos.(pi*x).*cos.(pi*y)*(w.^2)  ))

# # VARYING SLOWNESS TEST
slowsq(x,y) = exp.(-2.0*(x.^2 + y.^2));
fictitiousSourceTest2D((x,y)->cos.(pi*x).*cos.(pi*y), slowsq,w,
      (x,y) -> (- pi.^2.*cos.(pi*x).*cos.(pi*y) - pi.^2.*cos.(pi*x).*cos.(pi*y) + slowsq(x,y).*cos.(pi*x).*cos.(pi*y)*(w.^2)  ))

println("\t== passed ! ==")


struct ConstPotential{T} <: Potential end
ConstPotential() = ConstPotential{Float64}()

UU(phi,  ::ConstPotential) = phi
UUp(phi, ::ConstPotential{T}) where {T} = one(T)

parameters(potential::ConstPotential) = ()

struct SquarePotential{T} <: Potential end
SquarePotential() = SquarePotential{Float64}()

UU(phi,  ::SquarePotential) = -1 + phi*phi / 2
UUp(phi, ::SquarePotential) = phi

parameters(potential::SquarePotential) = ()


using GridInterpolations
using Statistics
using BenchmarkTools

function benchmark_interpolate(grid, data, rand_points)
    for point in eachcol(rand_points)
        interpolate(grid, data, point)
    end
end

function benchmark(GridType::Type, n_dims::Int; n_points_per_dim::Int=15, n_rand_points::Int=100, verbosity::Int=1)

    # construct grid
    grid = GridType((range(0, 1, length=n_points_per_dim) for _ in 1:n_dims)...)
    rand_points = rand(n_dims, n_rand_points)
    rand_data = rand(length(grid))

    trial = @benchmark benchmark_interpolate($grid, $rand_data, $rand_points) samples=10000

    if verbosity > 0
        println("Benchmarking interpolation on $(label(grid))")
        println("dimensionality:          $(n_dims)")
        println("points per dimension:    $(n_points_per_dim)")
        println("number of random points: $(n_rand_points)")
        if verbosity > 1
            show(stdout, MIME"text/plain"(), trial)  # make sure printing is verbose by using MIME
        else
            println("\n", trial)
        end
        println("\n--------------------------------------------------")
    end

    return trial
end

verbosity = 1
for ndims in 1:6

    println("\n##################################################")
    println("### Benchmark for ndims = $(ndims)")
    println("##################################################")

    trial_rectangle = benchmark(RectangleGrid, ndims; verbosity=verbosity)
    trial_simplex = benchmark(SimplexGrid, ndims; verbosity=verbosity)

    # println(ratio(trial_estimate_rectangle, trial_estimate_simplex))
    println(
        "RectangleGrid vs SimplexGrid: ",
        judge(median(trial_rectangle), median(trial_simplex))
    )
    println("##################################################\n")

end


using PlutoUI
using FFTW, Statistics, LinearAlgebra

n = 1000
x = range(0, 1, length=n)
ς = Slider(0.5:0.5:2.0)  # noise level

μ = 3*x.*sin.(2pi*x) # periodic signal in time domain
#μ = 6*sqrt.(abs.(x .- 0.5)) # this one is difficult to estimate

# Model: Signal distorted by white noise
y = μ + ς*randn(n)

# Prior power decay of prior samples in frequency domain
α = Slider(0.5:0.5:2.0)
# α = 3.0 for type integrated Brownian motion
# α = 2.0 for type Brownian motion prior
# α = 1.0 for a very rough prior (pink noise)

H = (α - 1)/2 # equivalent Hurst exponent

# Fourier matrix
# ℱ = [exp(2pi*im*i*j/n) for i in fftshift(-n÷2:n÷2-1), j in fftshift(-n÷2:n÷2-1)]

# Define prior on unit interval in frequency domain
# by choosing power decay
function freq_decay(n, α)  #ifftshift(-n/2:n/2-1)
    fr = 1 ./ fftfreq(n)
    fr[1] = 2n # allow for intercept
    fr = abs.(fr) .^(α/2) # scale frequencies to "unit
    fr/norm(fr)*sqrt(n)
end

# Canonical posterior contraction rate
contract(n, H) =  n^(-H/(1 + 2H))

# Compute posterior mean
γ = ς^(-2) .+ (freq_decay(n, α)).^(-2) # posterior precision in frequency domain
μ̂ = real(ifft(γ.\(fft(ς^(-2)*y))))
m = μ̂ + sqrt(n)*real(ifft(sqrt.(γ).\randn(Complex{Float64},n)));
# note: `fft(x)/sqrt(length(x))` is isometric

# Compute L2 estimation error, compare with theoretical size
@show norm(μ̂ - μ)/sqrt(n),  ς*contract(n, H)

# We can even compute the posterior covariance

# Use circulant property of covariance in time domain, could be done with https://github.com/JuliaMatrices/ToeplitzMatrices.jl
σ = 1.96*sqrt.(abs.((ifft(diag(inv(Diagonal(γ)))))))[1]

# Equivalent to
# Γ = ℱ*Diagonal(freq_decay(n, α).^(-2))*ℱ'/n/n # ≈ ifft(Diagonal(n. + freq_decay(n, α).^(-2)))
# σ = 1.96sqrt.(real(diag(inv((I + Γ)))))

#=
# Plot
using Plots
p = scatter(x, y, markersize=0.5, label="obs")
plot!(x, μ̂, color=:blue, ribbon=σ,fillalpha=0.2, label="posterior mean")
plot!(x, μ, color=:green, label="truth")
z = sqrt(n)*real(ifft(randn(Complex{Float64},n).*abs.(freq_decay(n, α))));
plot!(x, z, label="prior sample")
plot!(x, m, label="posterior sample")

p
=#


using CSV, DataFrames, RCall

data1 = CSV.read("../data/machine1.csv", header=false)[:,1]
data2 = CSV.read("../data/machine2.csv", header=false)[:,1]
data3 = CSV.read("../data/machine3.csv", header=false)[:,1]

function R_ANOVA(allData)
    data = vcat([ [x fill(i, length(x))] for (i, x) in
    			enumerate(allData) ]...)
    df = DataFrame(data, [:Diameter, :MachNo])
    @rput df

    R"""
    df$MachNo <- as.factor(df$MachNo)
    anova <- summary(aov( Diameter ~ MachNo, data=df))
    fVal <- anova[[1]]["F value"][[1]][1]
    pVal <- anova[[1]]["Pr(>F)"][[1]][1]
    """
    println("R ANOVA f-value: ", @rget fVal)
    println("R ANOVA p-value: ", @rget pVal)
end

R_ANOVA([data1, data2, data3])


#RSDeltaSigmaPort: utilities for transfer functions
#-------------------------------------------------------------------------------

"""`y = evalRPoly(roots,x,k=1)`
Compute the value of a polynomial which is given in terms of its roots.
"""
function evalRPoly(roots,x::AbstractArray,k=1)
	y = k * ones(size(x))
	finiteroots = findall(isfinite.(roots))
	roots = roots[finiteroots]
	for i in 1:length(roots)
		y = y.*(x .- roots[i])
	end
	return y
end
evalRPoly(roots,x,k=1) = evalRPoly(roots, [x], k)[1]

"""`h = evalTF(tf,z)`

Evaluates the rational function described by the struct tf
at the point(s) given in the z vector.

TF must be either a zpk object or a struct containing
 - form: 'zp' or 'coeff'
 - zeros,poles,k: if form=='zp'
 - num,den: if form=='coeff'

In Matlab 5, the ss/freqresp() function does nearly the same thing.
"""
function evalTF(tf::ZPKData,z)
	h = tf.k * evalRPoly(tf.z,z) ./ evalRPoly(tf.p,z)
	return h
end

function evalTF(tf,z)
	throw(ErrorException("Transfer function of this type not currently supported."))
#elseif any(strcmp(fieldnames(tf),'form'))
#    if strcmp(tf.form,'zp')
#        h = tf.k * evalRPoly(tf.zeros,z) ./ evalRPoly(tf.poles,z);
#    elseif strcmp(tf.form,'coeff')
#        h = polyval(tf.num,z) ./ polyval(tf.den,z);
#    else
#        fprintf(1,'%s: Unknown form: %s\n', mfilename, tf.form);
#    end
#else	% Assume zp form
#    h = tf.k * evalRPoly(tf.zeros,z) ./ evalRPoly(tf.poles,z);
#end
end

"""`H=evalTFP(Hs, Hz, f)`

Compute the value of a transfer function product Hs*Hz at a frequency f,
where Hs is a cts-time TF and Hz is a discrete-time TF.
Both Hs and Hz are SISO zpk objects.
This function attempts to cancel poles in Hs with zeros in Hz.
"""
function evalTFP(Hs,Hz,f::AbstractArray)
	(szeros, spoles, sk) = _zpkdata(Hs, 1)
	(zzeros, zpoles, zk) = _zpkdata(Hz, 1)

	slim = min(1e-3, max(1e-5, eps(1.0)^(1/(1+length(spoles)))))
	zlim = min(1e-3, max(1e-5, eps(1.0)^(1/(1+length(zzeros)))))

	H = zeros(size(f))
	w = 2*pi*f;	s = j*w;	z=exp.(s)
	for i in 1:length(f)
		wi = w[i];	si = s[i];	zi = z[i]
		cancel = false
		if !isempty(spoles)
			cancel = abs.(si .- spoles) .< slim
		end
		if !any(cancel)
			#wi is far from a pole, so just use the product Hs*Hz
			#H[i] = evalTF(Hs,si) * evalTF(Hz,zi);
			#use evalRPoly directly with z/p/k data, much faster. (David Alldred, Feb 4 2013)
			H[i] = sk * evalRPoly(szeros,si) / evalRPoly(spoles,si) *
				zk * evalRPoly(zzeros,zi) / evalRPoly(zpoles,zi)
		else
			#cancel pole(s) of Hs with corresponding zero(s) of Hz
			cancelz = abs.(zi .- zzeros) .< zlim
			sumcancel = sum(1*cancel); sumcancelz = sum(1*cancelz)
			if sumcancelz > sumcancel
				H[i] = 0.0
			elseif sumcancelz < sumcancel
				H[i] = Inf
			else
				H[i] = evalRPoly(szeros,si,sk) *
					zi^sumcancel * evalRPoly(zzeros[.!cancelz],zi,zk) /
					(evalRPoly(spoles[.!cancel],si,1)*evalRPoly(zpoles,zi,1))
			end
		end
	end
	return H
end
evalTFP(Hs,Hz,f) = evalTFP(Hs,Hz,[f])[1] #Single value

"""`evalMixedTF(tf,f, df=1e-5)`

Compute the mixed transfer function tf at a frequency f.
tf is a struct array with fields Hs and Hz wich represent
a continuous-time/discrete-time tfs which must be multiplied together
and then added up.
"""
function evalMixedTF(tf::CTDTPrefilters, f; df=1e-5)
	H = zeros(size(f))
	for i = 1:length(tf)
		H = H + evalTFP(tf[i].Hs, tf[i].Hz, f)
	end

	err = findall(isnan.(H) .| isinf.(H))
	if !isempty(err)
		#Need to fill in the holes. !! Use a "nearby" frequency point
		for i in err
			H[i] = evalMixedTF(tf, f[i] .+ df, df=df*10)
		end
	end
	return H
end

#==Calculators
===============================================================================#

"""`g = rmsGain(H,f1,f2,N=100)`

Compute the root mean-square gain of the discrete-time
tf H in the frequency band (f1,f2)
"""
function rmsGain(H,f1,f2,N::Int=100)
	w = collect(range(2*pi*f1, stop=2*pi*f2, length=N))
	g = norm( evalTF(H, exp.(j*w)) ) / sqrt(N)
	return g
end

"""`(pGain, Nimp) = powerGain(num,den,Nimp0=100)`

Calculate the power gain of a TF given in coefficient form.

 - `Nimp` is the recommended number of impulse response samples for use
   in future calls and `Nimp0` is the suggested number to use.
"""
function powerGain(num, den, Nimp0::Int=100)
	Nimp = Nimp0
	unstable = false

	sys = _tf(num,den,1)
	(imp,) = _impulse(sys, Nimp)
	if sum(abs.(imp[Nimp-10:Nimp])) < 1e-8 && Nimp > 50 #Use fewer samples.
		Nimp = round(Int, Nimp/1.3)
	else
		while sum(abs.(imp[Nimp-10:Nimp])) > 1e-6
			Nimp = Nimp*2
			(imp,) = _impulse(sys, Nimp)
			if sum(abs.(imp[Nimp-10:Nimp])) >= 50 || Nimp >= 1e4
				#H is close to being unstable
				unstable = true
				break
			end
		end
	end
	pGain = unstable ? Inf : sum(imp .^ 2)
	return (pGain, Nimp)
end

"""`zpk2 = cancelPZ(zpk1, tol=1e-6)`

Cancel zeros/poles in a zpk system
"""
function cancelPZ(zpk1, tol::Float64=1e-6)
	(z, p) = _zpkdata(zpk1, 1)
	zpk2 = deepcopy(zpk1) #Captures k, and whether tf is in s or z

	#Need to go in reverse order because z gets smaller with each cancellation
	for i = length(z):-1:1
		d = z[i] .- p
		cancel = findall(abs.(d) .< tol)
		if !isempty(cancel)
			deleteat!(p, cancel[1])
			deleteat!(z, i)
		end
	end
	zpk2.z = z
	zpk2.p = p
	return zpk2
end

#==infnorm
===============================================================================#
"""`nabsH(w, H)`

Computes the negative of the absolute value of H
at the specified frequency on the unit circle.

This function is used by infnorm().
"""
function nabsH(w, H)
	z = exp.(j*w)
	return -abs.(evalTF(H, z))
end

"""`(Hinf,fmax) = infnorm(H)`

Find the infinity norm of a z-domain transfer function.

Get a rough idea of the location of the maximum.
"""
function infnorm(H)
	N = 129
	w = range(0, stop=2π, length=N); dw = 2π/(N-1)
	Hval = abs.(evalTF(H, exp.(j*w)))
	wi = argmax(abs.(Hval))[1]
	Hinf = Hval[wi]

	local wmax
	if true #NEEDS_OPTIMIZATION
		opt = Optim.Options(x_tol=1e-8, f_tol=1e-6)
		f2min(w) = nabsH(w, H) #Function to minimize (optimum point)
		lower = w[wi]-dw; upper = w[wi]+dw
		wmax = optimize(f2min, lower, upper, GoldenSection()).minimum
	else
		msg = "Hinf: Warning. Optimization toolbox functions not found.\n" *
			" The result returned may not be very accurate."
		@warn(msg)
		wmax = w[wi]
	end

	Hinf = -nabsH(wmax, H)
	fmax = wmax/(2*pi)
	return (Hinf, wmax)
end

#==calculateTF
===============================================================================#
#Use a loophole to set complex poles and zeros in zpk objects
#MALaforge: I think this tries to implement minreal()
function setPolesAndZeros(z,p,k)
	tol = 3e-5 #tolerance for pole-zero cancellation
	z = deepcopy(z) #Don't change original
	Z = _zpk(ComplexF64[0.0],ComplexF64[],1.0,1.0)
	ztf = _zpk(ComplexF64[],ComplexF64[],k,1.0)
	for i in 1:length(p)
		match = abs.(p[i] .- z) .< tol
		if any(match)
			f = findall(match)
			deleteat!(z, f[1])
		else
			#ztf = ztf/(Z-p[i]) #Original code
			push!(ztf.p, p[i]) #VERIFYME: Think this is intent of original code
		end
	end
	for i in 1:length(z)
		#ztf = ztf*(Z-z[i]) #Original code
		push!(ztf.z, z[i]) #VERIFYME: Think this is intent of original code
	end
	return ztf
end

"""`(NTF,STF) = calculateTF(ABCD, k=1, wantSTF=true)`

Calculate the NTF and STF of a delta-sigma modulator whose loop filter
is described by the ABCD matrix, assuming a quantizer gain of k.
The NTF and STF are zpk objects.
"""
function calculateTF(ABCD, k::Vector=[1], wantSTF::Bool=true)
	nq = length(k)
	n = size(ABCD,1) - nq
	nu = size(ABCD,2) - size(ABCD,1)

	A,B,C,D = partitionABCD(ABCD, nu+nq)
	B1 = B[:,1:nu]
	B2 = B[:,nu+1:end]
	D1 = D[:,1:nu]
	if any(any( D[:,nu+1:end] .!= 0 ))
		throw("D2 must be zero")
	end
	K = diagm(k)
	#Find the noise transfer function by forming the closed-loop
	#system (sys_cl) in state-space form.
	Acl = A + B2*K*C
	Bcl = hcat(B1 + B2*K*D1, B2)
	Ccl = K*C
	Dcl = [K*D1 eye(nq)]
	#map(display, [Acl, Bcl, Ccl, Dcl])

	local NTF, STF
	tol = min(1e-3, max(1e-6, eps(1.0)^(1/(size(ABCD,1)))))
	if false # true || all(imag.(ABCD) .== 0) #real modulator
		#REQUESTHELP
		sys_cl = _ss(Acl,Bcl,Ccl,Dcl,1)
		sys_tf = ControlSystems.tf(sys_cl)
		mtfs = _minreal(sys_tf, tol)
		display(mtfs)
		@show (mtfs.nu, mtfs.ny), (nu, nq)
		#MALaforge: Can't make sense of the following; not sure what structure should be:
		STF = mtfs[:,1:nu]
		NTF = mtfs[:,nu+1:nu+nq]
	else #quadrature modulator and less advanced language features
		p = eig(Acl)
		NTFz = eig(A)
		NTF = setPolesAndZeros(NTFz, p, 1)
		if wantSTF
	        @warn("Sorry. calculateTF cannot compute the STF for a complex modulator with this version of the code.")
   	 end
	    STF=nothing
	end
	return (NTF, STF)
end

#Last line


function parse_input(lines)
    function parse_bag_content(content)
        if content == "no other bags."
            []
        elseif !occursin(",", content)
            words = split(content)
            child_bag = join(words[2:3], " ")
            amount = parse(Int64, words[1])
            [(child_bag, amount)]
        else
            map(x -> begin
                    words = split(x)
                    amount = parse(Int64, words[1])
                    node = join(words[2:3], " ")
                    (node, amount)
                    end, split(content, ", "))
        end
    end

    mapping = Dict{AbstractString, Set{Tuple{AbstractString, Int64}}}()
    for line in lines
        first_part, second_part = split(line, " contain ")
        parent_bag = join(split(first_part)[1:2], " ")
        children = parse_bag_content(second_part)
        for child in children
            vals = get(mapping, parent_bag, Set{Tuple{AbstractString, Int64}}())
            push!(vals, child)
            mapping[parent_bag] = vals
        end
    end
    mapping
end

function nr_inner_bags(mapping, start_bag)
    children = get(mapping, start_bag, Set())
    foldl((acc, (child, amount)) -> acc + amount + amount * nr_inner_bags(mapping, child), children; init=0)
end

function main()
    lines = readlines("../resources/input_07.txt")
    children = parse_input(lines)
    result = nr_inner_bags(children, "shiny gold")
    println(result)
end

main()


using Lehmann
using DelimitedFiles
using Printf
# using Gaston  # for ploting

β = 10.0 # inverse temperature
Euv = 4.0 # UV energy cutoff for the spectral density
eps = 1.0e-8 # accuracy for DLR basis

data = DelimitedFiles.readdlm("yuanyao.txt")
τ = data[:, 1] * β
τ[1] += 1.0e-8
τ[end] -= 1.0e-8
y = data[:, 2]
err = data[:, 3]
w = 1.0 ./ err.^2

# construct DLR basis
dlr = DLR.DLRGrid(:fermi, Euv, β, eps)

kernel = Spectral.kernelT(:fermi, τ, dlr.ω, β)
# y' = kernel \cdot coeff
# use linear fit to find the optimal coeff so that chi2 = sum_i w_i * (y_i-y'_i)^2 is minimized, where w_i = 1/errorbar_i^2
coeff = kernel \ y # linear fit without reweight, this gives an estimate of the initial values for the coefficients

######################### G_dlr(τ) ################################
yp = kernel * coeff
# print comparsion for selected τ point
printstyled("Compare G(τ) <-->  G_dlr(τ)\n", color=:green)
printstyled("G(0⁺)+G(β⁻): ", y[1] + y[end], " vs ", yp[1] + yp[end], "\n", color=:red)
printstyled("maximum dlr fitting error: ", maximum(abs.(y - yp)), "\n", color=:red)
printstyled(" τi       G(τ)        G_dlr(τ)      δG\n", color=:yellow)

for i in 1:25:length(y)
	@printf("%3i    %10.8f    %10.8f   %10.8f\n", i, y[i], yp[i], abs(y[i] - yp[i]))
end

######################### G_dlr(iωn) ################################
n = collect(0:100)
kernelΩ = Spectral.kernelΩ(:fermi, n, dlr.ω, β)
Gn = kernelΩ * coeff

printstyled("Matsubara frequency G_dlr(ωn)\n", color=:green)
printstyled(" n          Re           Im\n", color=:yellow)
for i in 1:length(n)
	@printf("%3i    %10.8f   %10.8f\n", n[i], real(Gn[i]), -imag(Gn[i]))
end

######################### Σ_dlr(iωn) ################################
g0inv = @. -1im * (2 * n + 1) * π / β + 0.4
Ginv = 1.0 ./ Gn
Σ = g0inv - Ginv

printstyled("Matsubara frequency Σ_dlr(ωn)\n", color=:green)
printstyled(" n          Re           Im\n", color=:yellow)
for i in 1:length(n)
	@printf("%3i    %10.8f   %10.8f\n", n[i], real(Σ[i]), -imag(Σ[i]))
end

# set(term="qt")
# p = plot(n, -imag.(Σ))
# display(p)
# readline()


@defcomp co2cycle begin
    b11     = Variable()                #Carbon cycle transition matrix atmosphere to atmosphere
	b21		= Variable()				#Carbon cycle transition matrix biosphere/shallow oceans to atmosphere
    b22     = Variable()                #Carbon cycle transition matrix shallow ocean to shallow oceans
    b32     = Variable()                #Carbon cycle transition matrix deep ocean to shallow ocean
    b33     = Variable()                #Carbon cycle transition matrix deep ocean to deep oceans
    MAT     = Variable(index=[time])    #Carbon concentration increase in atmosphere (GtC from 1750)
    ML      = Variable(index=[time])    #Carbon concentration increase in lower oceans (GtC from 1750)
    MU      = Variable(index=[time])    #Carbon concentration increase in shallow oceans (GtC from 1750)

    b12     = Parameter()               #Carbon cycle transition matrix atmosphere to shallow ocean
    b23     = Parameter()               #Carbon cycle transition matrix shallow to deep ocean
	mateq	= Parameter()				#Equilibrium concentration atmosphere (GtC)
	mueq 	= Parameter()				#Equilibrium concentration in upper strata (GtC)
	mleq	= Parameter()				#Equilibrium concentration in lower strata (GtC)
    E       = Parameter(index=[time])   #Total CO2 emissions (GtCO2 per year)
    mat0    = Parameter()               #Initial Concentration in atmosphere 2010 (GtC)
    ml0     = Parameter()               #Initial Concentration in lower strata 2010 (GtC)
    mu0     = Parameter()               #Initial Concentration in upper strata 2010 (GtC)

    function run_timestep(p, v, d, t)
		#Define function for b11
		v.b11 = 1 - p.b12

		#Define function for b21
		v.b21 = p.b12 * p.mateq/p.mueq

		#Define function for b22
		v.b22 = 1 - v.b21 - p.b23

		#Define function for b32
		v.b32 = p.b23 * p.mueq/p.mleq

		#Define function for b33
		v.b33 = 1 - v.b32

        #Define function for MAT
        if is_first(t)
            v.MAT[t] = p.mat0
        else
            v.MAT[t] = v.MAT[t-1] * v.b11 + v.MU[t-1] * v.b21 + (p.E[t-1]*(5/3.666))
        end

        #Define function for MU
        if is_first(t)
            v.MU[t] = p.mu0
        else
            v.MU[t] = v.MAT[t-1] * p.b12 + v.MU[t-1] * v.b22 + v.ML[t-1] * v.b32
        end

        #Define function for ML
        if is_first(t)
            v.ML[t] = p.ml0
        else
            v.ML[t] = v.ML[t-1] * v.b33 + v.MU[t-1] * p.b23
        end
    end
end


using DifferentialEquations, LinearAlgebra
using Plots
plotly()

#=
m*x`` + b*x` + k*x + a*x^3 = -mg
y = x`
=#

function fun(du,u,p,t)
    m,k,g,a,b = p
    du[1] = u[2]
    du[2] = -1/m*(b*u[2]+ k*u[1]+ a*u[1]^3+ m*g)
end

m, k, g, a = 1220.0, 35600.0, 17.5, 450000.0
b= 1000.0

tspan = (0.0, 10.0)
u0 = [0.0, 5.0]

prob1 = ODEProblem(fun, u0, tspan, (m, k, g, a, b))
sol = solve(prob1, Tsit5(),reltol = 1e-4, abstol=1e-10)

plot(sol)


# -----------------------------------------------
# Total Welfare
# -----------------------------------------------

@defcomp welfare begin

    regions  = Index()                         # 12 regions in RICE2010.

    ρ       = Parameter()                      # Pure rate of time preference.
    η       = Parameter()                      # Elasticity of marginal utility of consumption.
    pop     = Parameter(index=[time, regions]) # Regional population levels (millions of people).
    cpc     = Parameter(index=[time, regions]) # Per capita consumption levels ($1000s).

    UTILITY = Variable()                       # Total economic welfare over model time horizon (using the same name as the default RICE welfare component).

    function run_timestep(p, v, d, t)

        # Calculate total welfare for each period. Note, if η = 1, the social welfare function becomes log(x).
        if is_first(t)
            if p.η == 1.0
                v.UTILITY = sum(log.(p.cpc[t,:]) .* p.pop[t,:]) / (1.0 + p.ρ)^(10*(t.t - 1))
            else
                v.UTILITY = sum((p.cpc[t,:] .^ (1.0 - p.η)) ./ (1.0 - p.η) .* p.pop[t,:]) / (1.0 + p.ρ)^(10*(t.t - 1))
            end
        else
            if p.η == 1.0
                v.UTILITY = v.UTILITY + sum(log.(p.cpc[t,:]) .* p.pop[t,:]) / (1.0 + p.ρ)^(10*(t.t - 1))
            else
                v.UTILITY = v.UTILITY + sum((p.cpc[t,:] .^ (1.0 - p.η)) ./ (1.0 - p.η) .* p.pop[t,:]) / (1.0 + p.ρ)^(10*(t.t - 1))
            end
        end
    end
end


export heaviside
function heaviside(x,c = 0.15,delta = 0.1,epsilon = 0.1*delta)
a = c - delta;
b = c + delta;

y = zeros(eltype(x),size(x));
dy = zeros(eltype(x),size(x));
I = (x .>= (a-epsilon)) .& (x .<= (a+epsilon));
xI = x[I];
height = 1.0/(b-a);
t = (xI .- a .+ epsilon).*((0.25*height)/epsilon)

dy[I] = 2.0*t;
y[I] = (xI .- a .+ epsilon).*t;

I = (x .> a + epsilon) .& (x .<= b - epsilon);
y[I] .= height*(x[I].-a);
dy[I] .= height;
I = (x .> b+epsilon);
y[I] .= 1.0;
dy[I] .= 0.0;
I = (x .> b-epsilon) .& (x .<= b+epsilon);
xI = x[I];
t = (xI.-(b+epsilon)).*(-(0.25*height)/epsilon);
y[I] = (xI.-(b+epsilon)).*t .+ 1.0;
dy[I] = 2.0.*t;
return y,Diagonal(dy[:]);
end

export heaviside!
function heaviside!(x::Union{Array{Float64},Array{Float32}},y::Union{Array{Float64},Array{Float32}},c::Float64 = 0.15,delta::Float64 = 0.1,epsilon::Float64 = 0.1*delta)
a = c - delta;
b = c + delta;
height = 1.0/(b-a);
temp = ((0.25*height)/epsilon);
tmp2 = 0.0;
# y = zeros(size(x));
y[:] .= 0.0;
t1 = a-epsilon;
t2 = a+epsilon;
t3 = b-epsilon;
t4 = b+epsilon;
for k=1:length(x)
	xk = x[k];
	if xk <= t1
		x[k] = 0.0;
	elseif xk >= t4
		x[k] = 1.0;
	elseif xk >= t2 && xk <=t3
		x[k] = height*(xk-a);
		y[k] = height;
	elseif xk >= t1 && xk <=t2
		tmp2 = (xk - t1)*temp;
		x[k] = (xk - t1);
		x[k] *= tmp2;
		y[k] = 2.0;
		y[k] *= tmp2;
	elseif xk >= t3 && xk <=t4
		tmp2 = (t4 - xk)*temp;
		x[k] = (xk-t4).*tmp2 + 1.0;
		y[k] = 2.0*tmp2;
	end
end
return y;
end


algorithms = [DMDPINV(), DMDSVD(1e-2), TOTALDMD(1e-2, DMDPINV()), TOTALDMD(1e-2,DMDSVD(1e-2))]

@info "Starting Dynamic Mode Decomposition tests"
@testset "Dynamic Mode Decomposition" begin

    @info "Starting linear discrete system tests"
    @testset "Linear Discrete System" begin
        # Create some linear data
        A = [0.9 -0.2; 0.0 0.2]
        y = [[10.; -10.]]
        for i in 1:10
            push!(y, A*y[end])
        end
        X = hcat(y...)
        for alg_ in algorithms
            @info "Testing $alg_"
            estimator = DMD(X, alg = alg_)
            @test operator(estimator) ≈ A
            @test isstable(estimator)
            @test eigvals(estimator) ≈ eigvals(A)
            @test eigvecs(estimator) ≈ eigvecs(A)
            @test is_discrete(estimator)
            @test_nowarn update!(estimator, X[:, end-1], X[:,end])
            @test_throws AssertionError outputmap(estimator)
        end
    end

    @info "Starting linear continuous system tests"
    @testset "Linear Continuous System" begin
        A = [-0.9 0.1; 0.0 -0.2]
        f(u, p, t) = A*u
        u0 = [10.0; -20.0]
        prob = ODEProblem(f, u0, (0.0, 10.0))
        sol = solve(prob, Tsit5(), saveat = 0.001)
        X = sol[:,:]
        DX = sol(sol.t, Val{1})[:,:]

        for alg_ in algorithms
            @info "Testing $alg_"
            estimator = gDMD(X, DX, alg = alg_)
            d2 = gDMD(sol.t, X, alg = alg_)
            d3 = gDMD(sol.t, X, alg = alg_, dt = 0.007)
            @test isapprox(generator(estimator), A, atol = 1e-3)
            @test isstable(estimator)
            @test isapprox(eigvals(estimator), eigvals(A), atol = 1e-3)
            @test isapprox(abs.(eigvecs(estimator)), abs.(eigvecs(A)), atol = 1e-3)
            @test isapprox(generator(estimator), generator(d2), atol = 1e-1)
            @test isapprox(generator(estimator), generator(d3), atol = 5e-1)
        end
    end

    @info "Starting big continuous system tests"
    @testset "Big System" begin
        dd(u, p, t) = -0.9*ones(length(u))*u[1] - 0.05*u
        u0 = randn(100)*100.0
        u0[1] = 100.0
        prob = ODEProblem(dd, u0, (0.0, 10.0))
        sol = solve(prob, Tsit5(), saveat = 0.001)
        X = sol[:,:]
        DX = sol(sol.t, Val{1})[:,:]

        for alg_ in algorithms
            @info "Testing $alg_"
            d = gDMD(X, DX, alg = alg_)
            test = ODEProblem(d, u0, (0.0, 10.0))
            sol_ = solve(test, Tsit5(), saveat = 0.001)
            @test norm(sol-sol_, Inf) < 2.0
        end
    end

    @info "Starting low rank linear system tests"
    @testset "Rank Reduction" begin
        K = -0.5*I + [0 0 -0.2; 0.1 0 -0.1; 0. -0.2 0]
        F = qr(randn(20, 3))
        Q = F.Q[:, 1:3]
        dudt(u, p, t) = K*u
        prob = ODEProblem(dudt, [10.0; 0.3; -5.0], (0.0, 10.0))
        sol_ = solve(prob, Tsit5(), saveat = 0.01)

        # True Rank is 3
        alg = TOTALDMD(3, DMDPINV())
        X = Q*sol_[:,:] + 1e-3*randn(20, 1001)
        DX = Q*sol_(sol_.t, Val{1})[:,:] + 1e-3*randn(20, 1001)
        approx = gDMD(X, DX, alg = alg)
        @test Q'*generator(approx)*Q ≈ K atol = 1e-1
        @test Q*K*Q' ≈ generator(approx) atol = 1e-1
        alg = TOTALDMD(0.01, DMDSVD())
        approx = gDMD(X, DX, alg = alg)
        @test Q'*generator(approx)*Q ≈ K atol = 1e-1
        @test Q*K*Q' ≈ generator(approx) atol = 1e-1
    end
end

@info "Starting Extended Dynamic Mode Decomposition tests"
@testset "Extended Dynamic Mode Decomposition" begin

    @info "Starting linear discrete system tests"
    @testset "Linear Discrete System" begin
        # Test for linear system
        function linear_sys(u, p, t)
            x = -0.9 * u[1]
            y = -0.3 * u[2]
            return [x; y]
        end

        u0 = [π; 1.0]
        tspan = (0.0, 50.0)
        prob = DiscreteProblem(linear_sys, u0, tspan)
        sol = solve(prob, FunctionMap())

        @variables u[1:2]
        h = [u[1]; u[2]; sin(u[1]); cos(u[1]); u[1] * u[2]; u[2]^2]
        basis = Basis(h, u)

        estimator = EDMD(sol[:, 1:15], basis, alg = DMDPINV())
        update!(estimator, sol[:, 15:end-1], sol[:, 16:end])
        p1 = DiscreteProblem(estimator, u0, tspan)
        s1 = solve(p1,FunctionMap())
        basis_2 = reduce_basis(estimator, threshold = 1e-5)
        @test basis == estimator.basis
        @test size(basis_2)[1] < size(basis)[1]
        @test sol[:,:] ≈ s1[:,:]

        # Check EDMD
        for alg_ in algorithms
            @info "Testing $alg_"
            estimator_ = EDMD(sol[:,:], basis_2)
            p_ = DiscreteProblem(estimator_, u0, tspan)
            s_ = solve(p_,FunctionMap())
            @test sol[:,:] ≈ s_[:,:]
            @test eigvals(estimator_) ≈ [-0.9; -0.3]
            @test isstable(estimator_)
        end
    end

    @info "Starting linear continuous system tests"
    @testset "Linear Continuous System" begin

        function nonlinear_sys2(du, u, p, t)
            du[1] = u[2]
            du[2] = -0.9u[1]
        end

        u0 = [10.0; -20.0]

        prob_nl = ODEProblem(nonlinear_sys2, u0, (0.0, 10.0))
        sol_nl = solve(prob_nl, Tsit5())

        X = Array(sol_nl)
        DX = sol_nl(sol_nl.t, Val{1})[:,:]

        @variables u[1:2]
        basis = Basis(u, u)

        for alg_ in algorithms
            @info "Testing $alg_"
            estimator = gEDMD(X, DX, basis, alg = alg_)
            estimator_derivative = gEDMD(sol_nl.t, X, basis, alg = alg_)
            estimator_interpolation = gEDMD(sol_nl.t, X, basis, dt = 0.1, alg = alg_)
            @test generator(estimator) ≈ [0  1.0; -0.9 0]
            @test outputmap(estimator) ≈ I(2)
            @test generator(estimator) ≈ generator(estimator_derivative) atol = 1e-1
            @test generator(estimator) ≈ generator(estimator_interpolation) atol = 1e-1
        end
    end

    @info "Starting nonlinear discrete system tests"
    @testset "Nonlinear Discrete System" begin
        function nonlinear_sys(du, u, p, t)
            du[1] = 0.9u[1] + 0.1u[2]^2
            du[2] = sin(u[1]) - 0.1u[1]
        end

        u0 = [π; 1.0]
        tspan = (0.0, 50.0)

        @variables u[1:2]
        h = [u[1]; u[2]; sin(u[1]); u[2]^2]
        basis = Basis(h, u)

        prob = DiscreteProblem(nonlinear_sys, u0, tspan)
        sol = solve(prob,FunctionMap())
        estimator = EDMD(sol[:,:], basis, alg = DMDPINV())
        sys = ODESystem(estimator)
        @test isa(sys, ODESystem)
        dudt = ODEFunction(sys)
        p_ = DiscreteProblem(dudt, u0, tspan)
        s_ = solve(p_,FunctionMap())
        @test sol[:,:] ≈ s_[:,:]

        for alg_ in algorithms
            @info "Testing $alg_"
            estimator_ = EDMD(sol[:,:], basis, alg = alg_)
            p_ = DiscreteProblem(estimator_, u0, tspan)
            s_ = solve(p_,FunctionMap())
            @test sol[:,:] ≈ s_[:,:] atol = 1e-1
        end
    end

    @info "Starting nonlinear continuous system tests"
    @testset "Nonlinear Continuous System" begin

            function slow_manifold(du, u, p, t)
                du[1] = p[1]*u[1]
                du[2] = p[2]*(u[2]-u[1]^2)
            end

            u0 = [3.0; -2.0]
            tspan = (0.0, 10.0)
            p = [-0.05, -0.9]

            prob = ODEProblem(slow_manifold, u0, tspan, p)
            sol = solve(prob, Tsit5(), saveat = 0.2)

            X = Array(sol)
            # This enforces more accurate results
            DX = similar(X)
            for (i,dx) in enumerate(eachcol(DX))
                slow_manifold(dx, X[:, i], p, sol.t[i])
            end

            @variables u[1:2]
            basis = Basis([u; u[1]^2], u)

            for alg_ in algorithms
                @info "Testing $alg_"
                estimator2 = gEDMD(X, DX, basis, alg = alg_)
                A_analytical = [p[1] 0 0; 0 p[2] -p[2]; 0 0 2*p[1]]
                outputmap(estimator2)
                generator(estimator2)
                @test generator(estimator2) ≈ [p[1] 0 0; 0 p[2] -p[2]; 0 0 2*p[1]] atol =  1e-3
                @test outputmap(estimator2) ≈ [1 0 0 ; 0 1 0] atol = 1e-3
                @test abs.(modes(estimator2)) ≈ abs.(eigvecs(A_analytical)) atol = 1e-3
                @test frequencies(estimator2) ≈ eigvals(A_analytical) atol = 1e-3
            end

    end
end

@info "Starting Dynamic Mode Decomposition with Control tests"
@testset "Dynamic Mode Decomposition with Control" begin

    # Define measurements from unstable system with known control input
    X = [4 2 1 0.5 0.25; 7 0.7 0.07 0.007 0.0007]
    U = [-4 -2 -1 -0.5]
    B = Float32[1; 0]

    # But with a little more knowledge
    for alg_ in algorithms
        @info "Testing $alg_"
        sys = DMDc(X, U, B = B, alg = alg_)
        @test operator(sys) ≈[1.5 0; 0 0.1]
        @test inputmap(sys) ≈ [1.0; 0.0]
        @test !isstable(sys)
        @test_nowarn eigen(sys)
    end

    sys = DMDc(X, U, B = B)
    sys2 = DMDc(X, U, B = B, alg = DMDSVD())
    @test operator(sys2) ≈ operator(sys)
    sys3 = gDMDc(X[:, 1:end-1], X[:, 2:end], U, B = B)
    @test generator(sys3) ≈ [1.5 0; 0 0.1]

    A = [0.9 0.3; -0.2 0.9]
    B = [1.0 0;0 0.5]
    X = zeros(Float64, 2, 11)
    U = zeros(Float64, 2, 10)
    X[:, 1] = [1.0; -3.0]
    for i in 1:10
        U[:, i] .= [sin(2.0/5.0*i); cos(5.0/7.0*i)]
        X[:, i+1] .= A*X[:,i]+B*[sin(2.0/5.0*i); cos(5.0/7.0*i)]
    end

    for alg_ in algorithms
        @info "Testing $alg_"
        sys4 = DMDc(X, U, alg = alg_)
        @test operator(sys4) ≈ A
        @test inputmap(sys4) ≈ B
        sys5 = gDMDc(X[:, 1:end-1], X[:, 2:end], U[:, 1:end])
        @test_throws AssertionError operator(sys5)
        @test generator(sys5) ≈ A
        @test inputmap(sys5) ≈ B
    end

    for alg_ in algorithms
        @info "Testing $alg_"
        sys6 = gDMDc(collect(0.0:9.0), X[:, 1:end-1], U, B = B, alg = alg_)
        @test exp(generator(sys6)) ≈ A atol = 1e-1
        @test inputmap(sys6) ≈ B
    end

end


# using OED
using jInv.Utils
using Base.Test
using OED

println("=== solve 2D QP ===")
n = 2

G = eye(n)
c = 0*[1;1.]

Ae = [1 1.]
be = [1.2]
me = size(Ae,1)

Ai = eye(n)
bi = 0.0*ones(n)
mi = size(Ai,1)

x0  = rand(n)+3.4
y0  = rand(mi)+0*1.2
le0 = rand(me)
li0 = rand(mi)

println("\n\t---test Interior Point Method for QP ---")

print("\t\tsolve 2D QP with inequality and equality constraints...\n\t\t")
xas,yas,leas,lias,hisas = ipqp(G,c,Ae,be,Ai,bi,x0,y0,le0,li0,
			maxIter=20,ftol=1e-30,solveKKT=OED.solveFullKKT)
@test all(Ai*xas-bi .>0)
# @test norm(xas-0) < 1e-3
@test all(yas .> 0)
@test all(lias .> 0)
@test norm(Ai*xas-yas-bi) < 1e-14
@test maximum(yas.*lias) < 1e-2
@test maximum(G*xas - Ae'*leas - Ai'*lias + c)< 1e-15

xas,yas,leas,lias,hisas = ipqp(G,c,Ae,be,Ai,bi,x0,y0,le0,li0,
			maxIter=20,ftol=1e-30)
@test all(Ai*xas-bi .>0)
# @test norm(xas-0) < 1e-3
@test all(yas .> 0)
@test all(lias .> 0)
@test norm(Ai*xas-yas-bi) < 1e-14
@test maximum(yas.*lias) < 1e-2
@test maximum(G*xas - Ae'*leas - Ai'*lias + c)< 1e-15

xas,yas,leas,lias,hisas = ipqp(G,c,Ae,be,Ai,bi,x0,y0,le0,li0,
			maxIter=20,ftol=1e-30,solveKKT=OED.solveAugKKT)
@test all(Ai*xas-bi .>0)
# @test norm(xas-0) < 1e-3
@test all(yas .> 0)
@test all(lias .> 0)
@test norm(Ai*xas-yas-bi) < 1e-14
@test maximum(yas.*lias) < 1e-2
@test maximum(G*xas - Ae'*leas - Ai'*lias + c)< 1e-15

print("test passed!\n")

print("\t\tsolve 2D QP without equality constraints...\n\t\t")
n = 2

G = eye(n)
c = 0*[1;1.]

Ae = zeros(0,n)
be = zeros(0)
me = size(Ae,1)

Ai = eye(n)
bi = 0.0*ones(n)
mi = size(Ai,1)

x0  = rand(n)+3.4
y0  = rand(mi)+0*1.2
le0 = rand(me)
li0 = rand(mi)

xas,yas,leas,lias,hisas = ipqp(G,c,Ae,be,Ai,bi,x0,y0,le0,li0,out=1,
			maxIter=20,ftol=1e-30,storeInterm=true,solveKKT=OED.solveFullKKT)
@test all(Ai*xas[:,end]-bi .>0)
# @test norm(xas-0) < 1e-3
@test norm(Ai*xas[:,end]-yas[:,end]-bi) < 1e-14
@test maximum(yas[:,end].*lias[:,end]) < 1e-2
@test all(yas .> 0)
@test all(lias .> 0)
@test maximum(G*xas[:,end] -  Ai'*lias[:,end] + c)< 1e-15

xas,yas,leas,lias,hisas = ipqp(G,c,Ae,be,Ai,bi,x0,y0,le0,li0,out=1,
			maxIter=20,ftol=1e-30,storeInterm=true)
@test all(Ai*xas[:,end]-bi .>0)
# @test norm(xas-0) < 1e-3
@test norm(Ai*xas[:,end]-yas[:,end]-bi) < 1e-14
@test maximum(yas[:,end].*lias[:,end]) < 1e-2
@test all(yas .> 0)
@test all(lias .> 0)
@test maximum(G*xas[:,end] -  Ai'*lias[:,end] + c)< 1e-15

xas,yas,leas,lias,hisas = ipqp(G,c,Ae,be,Ai,bi,x0,y0,le0,li0,out=1,
			maxIter=20,ftol=1e-30,storeInterm=true,solveKKT=OED.solveAugKKT)
@test all(Ai*xas[:,end]-bi .>0)
# @test norm(xas-0) < 1e-3
@test norm(Ai*xas[:,end]-yas[:,end]-bi) < 1e-14
@test maximum(yas[:,end].*lias[:,end]) < 1e-2
@test all(yas .> 0)
@test all(lias .> 0)
@test maximum(G*xas[:,end] -  Ai'*lias[:,end] + c)< 1e-15
print("test passed!\n")
println("\n\t---[IPQP] all tests passed---")


export Operator

"""
    abstract type Operator{T<:Number} <: AbstractMatrix{Number} end

A matrix representing a linear operator that acts upon a complex-valued Hilbert space.
The `Operator` is an abstract type that parents all linear operator in quantum mechanics.
These matrix types can represent quantum states, evolution, and measurements, each with
their individual constraint.
These constraints are place upon the children of th `Operator` abstract type.
"""
abstract type Operator{T<:Number} <: AbstractMatrix{Number} end
Base.size(O :: Operator) = size(O.M)
Base.getindex(O :: Operator, id :: Vararg{Int,2}) = getindex(O.M, id...)
Base.setindex!(O :: Operator, val, id :: Vararg{Int,2}) = (O.M[id...] = val)

"""
Operator Multiplication:

```julia
*(operators :: Vararg{Operator}) :: Matrix
```
"""
*(operators :: Vararg{Operator}) :: Matrix = *(map(O -> O.M, operators)...)

"""
Kronecker Product:

```julia
kron(operators :: Vararg{Operator}) :: Matrix
```
"""
kron(operators :: Vararg{Operator}) :: Matrix =  kron(map(O -> O.M, operators)...)

"""
Matrix Rank:

```julia
rank(O :: Operator) :: Int64
```
"""
rank(O :: Operator) :: Int64 = rank(O.M, atol=O.atol)

"""
Matrix Square Root:

```julia
sqrt(O :: Operator) :: Matrix
```
"""
sqrt(O :: Operator) :: Matrix = sqrt(O.M)

"""
[`Operator`](@ref) types can multiply [`Bra`](@ref) and [`Ket`](@ref) types.

``O|\\psi\\rangle = | \\psi' \\rangle``:

```julia
*(O :: Operator, ket :: Ket) :: Vector
```

``\\langle \\psi |O = \\langle \\psi'|``:

```julia
*(bra :: Bra, O :: Operator) :: Adjoint{T, Vector{T}} where T <: Number
```

Inner product, ``\\langle \\psi |O|\\sigma\\rangle = \\langle \\psi|\\sigma'\\rangle``:

```julia
*(bra :: Bra, O :: Operator, ket :: Ket) :: Number
```
"""
*(O :: Operator, ket :: Ket) :: Vector = O.M * ket.ψ
*(bra :: Bra, O :: Operator) :: Adjoint{T, Vector{T}} where T <: Number = bra.ψ * O.M
*(bra :: Bra, O :: Operator, ket :: Ket) :: Number = bra.ψ * O.M * ket.ψ

*(O :: Operator, M :: Matrix{<:Number}) :: Matrix = O.M * M
*(M :: Matrix{<:Number}, O :: Operator) :: Matrix = M * O.M


module Poisson

using TimerOutputs
using IterativeSolvers: cg
using Preconditioners: AMGPreconditioner, SmoothedAggregation
using SparseArrays

using Gridap
import Gridap: ∇

export poisson

# Define manufactured functions
u(x) = x[1] + x[2]
∇u(x) = VectorValue(1.0,1.0,0.0)
∇(::typeof(u)) = ∇u
f(x) = 0.0

# Define forms of the problem
a(v,u) = ∇(v)*∇(u)
l(v) = v*f

# Define norms to measure the error
l2(u) = inner(u,u)
h1(u) = a(u,u) + l2(u)

"""
n: number of elements per direction
"""
function poisson(n::Integer)

  reset_timer!()

  domain = (0,1,0,1,0,1)
  partition = (n,n,n)
  @timeit "model" model = CartesianDiscreteModel(domain,partition)

  order = 1
  @timeit "V" V = TestFESpace(
    model=model,dirichlet_tags="boundary", conformity=:H1,
    reffe=:Lagrangian, order=order, valuetype=Float64)

  @timeit "U" U = TrialFESpace(V,u)

  @timeit "trian" trian = Triangulation(model)

  degree = 2
  @timeit "quad" quad = CellQuadrature(trian,degree)

  @timeit "t_Ω" t_Ω = AffineFETerm(a,l,trian,quad)

  @timeit "op" op = AffineFEOperator(SparseMatrixCSC{Float64,Int32},V,U,t_Ω)

  A = get_matrix(op)
  b = get_vector(op)

  @timeit "AMG setup" p = AMGPreconditioner{SmoothedAggregation}(A)

  @timeit "PCG"  x = cg(A,b,verbose=true,Pl=p)

  @timeit "uh" uh = FEFunction(U,x)

  @timeit "e" e = u - uh

  @timeit "el2" el2 = sqrt(sum( integrate(l2(e),trian,quad) ))
  @timeit "eh1" eh1 = sqrt(sum( integrate(h1(e),trian,quad) ))

  @show el2
  @show eh1

  print_timer()

end

end # module


function convolute(a,b)
    len_a = length(a)
    len_b = length(b)
    len_c = len_a + len_b -1
    T_B = zeros(Float64, len_c, len_a)
    c = zeros(Float64, len_c)

    #Initialize T_B
    for j=1:len_a
        for i=j:len_c
            k = i-j+1
            if(k<=len_b)
                T_B[i,j] = b[k]
            else
                T_B[i,j] = 0
            end
        end
    end

    #Multiply T(B) with a to get c
    for i=1:len_c
        for j=1:1
            sum = 0
            for k=1:len_a
                sum+=T_B[i,k]*a[k]
            end
            c[i,j] = sum
        end
    end

    return c
end

u = Array{Int64}(undef, 50)
for i=1:50
    x = rand(Int,1)
    if(x[1]%2==0)
        u[i]=1
    else
        u[i]=-1
    end
end

c = [1,0.7,-0.3]
y = convolute(u, c)

using Plots
pyplot()
plot([y])

h = [0.9, -0.5, 0.5, -0.4, 0.3, -0.3, 0.2, -0.1]
z = convolute(y, h)
plot([z])

using DSP
a=[1]
b=[1,2]
c = conv(a,b)
print(c)


# SPDX-License-Identifier: MIT

using ArgParse
using HDF5

function parse_commandline()
    s = ArgParseSettings()
    @add_arg_table s begin
        "features"
            required = true
            help = "HDF5 archive containing the features"
        "out"
            required = true
            help = "output HDF5 file containing the statistics"
    end
    s.description = """
    Extract statistics from the a set of utterances. The list
    of utterances to use is read from the standard input.
    """
    parse_args(s)
end

function main(args)
    h5open(args["features"], "r") do f

        # Initialize the accumulators with the 1st utterance
        data = read(f[readline(stdin)])
        D, N = size(data)
        x = dropdims(sum(data, dims = 2), dims = 2)
        xxᵀ = sum([c*c' for c in eachcol(data)])

        uttcount = 0
        for line in eachline(stdin)
            uttcount += 1
            @debug "reading $(uttcount)th utterance: $line"
            data = read(f[line])
            N += size(data, 2)
            x += dropdims(sum(data, dims = 2), dims = 2)
            xxᵀ += sum([c*c' for c in eachcol(data)])
        end
        μ = x ./ N
        Σ = (xxᵀ .- μ*μ') ./ N

        h5open(args["out"], "w") do fout
            fout["N"] = N
            fout["dim"] = D
            fout["mean"] = μ
            fout["cov"] = Σ
        end
    end
end

args = parse_commandline()
main(args)


using Random, LinearAlgebra, Distributions
import Distributions: MvNormal
import LinearAlgebra: cholesky, Symmetric

using SEC

⊗(a, b) = a * b'
Random.seed!(1234)

# Here we go through an example where the theory "works perfectly"

# Define a linear forward map
# input Space
Mo = 2
# output space
M = 2
H = randn(M, Mo)
forward_map(x) = H * x

# Create artificial solution
x = randn(Mo) .- 30
y̅ = forward_map(x)

# Define an initial ensemble distribution
MoMo = randn(Mo, Mo)
Σ = MoMo' * MoMo + I
prior = MvNormal(Σ)
# this choice implies prior ∝ exp( - x' Σ⁻¹ x)

# Implicitly define the likelihood function via the covariance
MM = randn(M, M)
Γ = (MM' * MM + I)
# implies likelihood  exp(- T * dot(y - H * x, Γ⁻¹ * (y - H * x)) )
# where T is the final time of the simulation
# Observe that
# likelihood = exp(- dot(y - H * x, Γ⁻¹ * (y - H * x)) ) for T = 1

# number of steps
N = 100
# timestep size (h = 1/N implies T=1 at the final time)
Δt = 1 / N
# number of ensemble members
J = 4000 * Mo

# Construct Posterior
HΓHΣ = Symmetric((H' * (cholesky(Γ) \ H) + cholesky(Σ) \ I))
cHΓHΣ = cholesky(HΓHΣ) # LU fails
rhs = (H' * (cholesky(Γ) \ y̅[:]))
posterior_μ = cHΓHΣ \ rhs
posterior_Σ = cHΓHΣ \ I
posterior = MvNormal(posterior_μ, posterior_Σ)

# Construct empirical prior
u = [rand(prior) for i = 1:J]
u₀ = copy(u)

timeseries = []
push!(timeseries, copy(u))

# Viewed as a minimization problem, we are minimizing
# dot(y - H * x, Γ⁻¹ * (y - H * x)) + dot(x, Σ⁻¹ x)
# whose solution is
# (H' Γ⁻¹ H + Σ⁻¹)⁻¹ H' Γ⁻¹ y
# Alternatively we can view the posterior as
# posterior ∝ exp(-dot(y - H * x, Γ⁻¹ * (y - H * x)) - x' Σ⁻¹ x)
# which implies μ = (H' Γ⁻¹ H + Σ⁻¹)⁻¹ H' Γ⁻¹ y
# and           σ = (H' Γ⁻¹ H + Σ⁻¹)⁻¹

# EKI Algorithm
for _ = 1:N
    # Likelihood perturbation for algorithm, (constructed from before)
    ξ = MvNormal(1 / Δt * Γ)
    SEC.Calibrate.eki_step!(u, forward_map, y̅, J, ξ, Γ, Δt)
    push!(timeseries, copy(u))
end

# This
approx = mean(u)
if length(approx) < 4
    println("mean appro = ", approx)
end
# should converge to this
exact = posterior.μ
if length(exact) < 4
    println("mean exact = ", exact)
end
println("mean relative error = ", norm(approx - exact) / norm(exact))
println("loss relative error = ", norm(H * approx - H * exact) / norm(H * exact))

exact_cov = posterior.Σ
approx_cov = cov(u)
if length(exact) < 4
    @show approx_cov
    @show exact_cov
end
println("cov relative error = ", norm(approx_cov - exact_cov) / norm(exact_cov))

##
using GLMakie, Printf

fig = Figure()
ax = Axis(fig[2, 1])
sc_init = scatter!(ax, [(timeseries[1][i][1], timeseries[1][i][2]) for i in eachindex(u)], color = :red)
sc_final = scatter!(ax, [(timeseries[end][i][1], timeseries[end][i][2]) for i in eachindex(u)], color = :blue)

time_slider = Slider(fig, range = 1:length(timeseries), startvalue = 1)
ti = time_slider.value

ensemble = @lift [(timeseries[$ti][i][1], timeseries[$ti][i][2]) for i in eachindex(u)]
sc_transition = scatter!(ax, ensemble, color = :purple)

scatter!(ax, [(exact[1], exact[2])], marker = '⋆', color = :yellow, markersize = 30)

ax.xlabel = "c¹"
ax.ylabel = "c²"

time_string = @lift("Time = " * @sprintf("%0.2f", ($ti - 1) / (length(timeseries) - 1)))
fig[3, 1] = vgrid!(
    Label(fig, time_string, width = nothing),
    time_slider,
)

c¹ = @lift([timeseries[$ti][i][1] for i in eachindex(timeseries[end])])
c² = @lift([timeseries[$ti][i][2] for i in eachindex(timeseries[end])])

ax.xlabelsize = 25
ax.ylabelsize = 25

ax_above = Axis(fig[1, 1])
ax_side = Axis(fig[2, 2])

ax_above.ylabel = "probability density"
ax_side.xlabel = "probability density"

limits!(ax, -50, 10, -8, 4)
xlims!(ax_above, ax.limits[][1]...)
ylims!(ax_above, 0, 0.4)
ylims!(ax_side, ax.limits[][2]...)

hideydecorations!(ax_side, ticks = false, grid = false)
hidexdecorations!(ax_above, ticks = false, grid = false)

# colsize!(fig.layout, 1, Relative(2 / 3))
# rowsize!(fig.layout, 1, Relative(1 / 3))
colgap!(fig.layout, 10)
rowgap!(fig.layout, 10)

d1 = density!(ax_above, c¹, color = (:purple, 0.15), label = "density c¹", strokewidth = 1)
d1lims = ax_above.limits[][1][1]:ax_above.limits[][1][2]
tmpx = collect(range(d1lims[1], d1lims[end], length = 1000))
σ = prior.Σ[1, 1]
μ = prior.μ[1]
tmpy = @. 1 / sqrt(2 * π * σ) * exp(-0.5 * (tmpx - μ)^2 / σ)
ln1 = lines!(ax_above, tmpx, tmpy, linewidth = 4, color = :red)
σ = posterior.Σ[1, 1]
μ = posterior.μ[1]
tmpy = @. 1 / sqrt(2 * π * σ) * exp(-0.5 * (tmpx - μ)^2 / σ)
lines!(ax_above, tmpx, tmpy, linewidth = 4, color = :blue)

d2 = density!(ax_side, c², color = (:purple, 0.15), label = "density c²", strokewidth = 1, direction = :y)
dlims = ax_side.limits[][2][1]:ax_side.limits[][2][2]
tmpx = collect(range(dlims[1], dlims[end], length = 1000))
σ = prior.Σ[2, 2]
μ = prior.μ[2]
tmpy = @. 1 / sqrt(2 * π * σ) * exp(-0.5 * (tmpx - μ)^2 / σ)
lines!(ax_side, tmpy, tmpx, linewidth = 4, color = :red)
σ = posterior.Σ[2, 2]
μ = posterior.μ[2]
tmpy = @. 1 / sqrt(2 * π * σ) * exp(-0.5 * (tmpx - μ)^2 / σ)
ln2 = lines!(ax_side, tmpy, tmpx, linewidth = 4, color = :blue)

Legend(fig[1, 2],
    [sc_init, sc_final, sc_transition, ln1, ln2],
    ["prior ensemble", "posterior ensemble", "transition ensemble", "exact marginal prior", "exact marginal posterior"])

#update!(fig.scene)
display(fig)

seconds = 5
fps = 30
frames = round(Int, fps * seconds)
frames = 101
fps = 30
record(fig, pwd() * "/example.mp4"; framerate = fps) do io
    for i = 1:frames
        ti[] = i
        sleep(1 / fps)
        recordframe!(io)
    end
end


using Phonetics
using Test
using DelimitedFiles
using StatsBase
using DataFrames
using Statistics
using Random
using DynamicAxisWarping
using Distances

@testset "Vowel space and density" begin

  f = Formants("sample_formants.txt")
  v = VowelSpace(f)
  @test area(v, unstandardize=true) ≈ 1920.3803415872135
  @test vdi(v, unstandardize=true) ≈ 766.8716362331788
end

@testset "Random formant generation" begin

  dat = generateFormants(10^7, cats=["uw"], gender=["w"])
  datμ = mean(Array(dat[:, 1:2]), dims=1)
  @test isapprox(datμ, [459.67 1105.52], atol=1)

  dat = generateFormants(10^7, cats=["aa"], gender=["w"])
  datμ = mean(Array(dat[:, 1:2]), dims=1)
  @test isapprox(datμ, [916.36 1525.83], atol=1)

  dat = generateFormants(30, cats=["iy", "aa"], gender=["w", "m"])
  @test dat.vowel == repeat(["iy", "aa"], inner=60)
  @test dat.gender == repeat(["w", "m"], inner=30, outer=2)
end

@testset "Normalization" begin

  dat = generateFormants(50)
  # Use gender as speaker for tests to give two separate speakers

  lobCustom = copy(dat)
  rename!(lobCustom, :gender => :speaker)

  lobCustom[lobCustom.speaker .== "m", :f1] = zscore(lobCustom[lobCustom.speaker .== "m", :f1])
  lobCustom[lobCustom.speaker .== "w", :f1] = zscore(lobCustom[lobCustom.speaker .== "w", :f1])

  lobCustom[lobCustom.speaker .== "m", :f2] = zscore(lobCustom[lobCustom.speaker .== "m", :f2])
  lobCustom[lobCustom.speaker .== "w", :f2] = zscore(lobCustom[lobCustom.speaker .== "w", :f2])

  lobFunc = lobanov(dat.f1, dat.f2, dat.vowel, dat.gender)
  # check for approximate equality because sum and related functions
  # called on the view genereated by `groupby` produces a different value
  # than sum called on the original array (due to how the view is generated
  #  and how order matters when performing sequential floating point operations).
  @test lobCustom.f1 ≈ lobFunc.f1
  @test lobCustom.f2 ≈ lobFunc.f2
  @test lobCustom.vowel == lobFunc.vowel
  @test lobCustom.speaker == lobFunc.speaker

  nICustom = copy(dat)
  rename!(nICustom, :gender => :speaker)

  mf1 = nICustom[nICustom.speaker .== "m", :f1]
  nICustom[nICustom.speaker .== "m", :f1] = log.(mf1) .- mean(log.(mf1))
  wf1 = nICustom[nICustom.speaker .== "w", :f1]
  nICustom[nICustom.speaker .== "w", :f1] = log.(wf1) .- mean(log.(wf1))

  mf2 = nICustom[nICustom.speaker .== "m", :f2]
  nICustom[nICustom.speaker .== "m", :f2] = log.(mf2) .- mean(log.(mf2))
  wf2 = nICustom[nICustom.speaker .== "w", :f2]
  nICustom[nICustom.speaker .== "w", :f2] = log.(wf2) .- mean(log.(wf2))

  nIFunc = neareyI(dat.f1, dat.f2, dat.vowel, dat.gender)
  @test nICustom.f1 ≈ nIFunc.f1
  @test nICustom.f2 ≈ nIFunc.f2
  @test nICustom.vowel == nIFunc.vowel
  @test nICustom.speaker == nIFunc.speaker

  nECustom = copy(dat)

  rename!(nECustom, :gender => :speaker)

  mf1 = nECustom[nECustom.speaker .== "m", :f1]
  mfall = [mf1; nECustom[nECustom.speaker .== "m", :f2]]
  nECustom[nECustom.speaker .== "m", :f1] = log.(mf1) .- mean(log.(mfall))

  wf1 = nECustom[nECustom.speaker .== "w", :f1]
  wfall = [wf1; nECustom[nECustom.speaker .== "w", :f2]]
  nECustom[nECustom.speaker .== "w", :f1] = log.(wf1) .- mean(log.(wfall))

  mf2 = nECustom[nECustom.speaker .== "m", :f2]
  nECustom[nECustom.speaker .== "m", :f2] = log.(mf2) .- mean(log.(mfall))

  wf2 = nECustom[nECustom.speaker .== "w", :f2]
  nECustom[nECustom.speaker .== "w", :f2] = log.(wf2) .- mean(log.(wfall))

  nEFunc = neareyE(dat.f1, dat.f2, dat.vowel, dat.gender)
  @test nECustom.f1 ≈ nEFunc.f1
  @test nECustom.f2 ≈ nEFunc.f2
  @test nECustom.vowel == nEFunc.vowel
  @test nECustom.speaker == nEFunc.speaker
end

@testset "Acoustic distance" begin

  rng = MersenneTwister(9)
  x = rand(rng, 1000)
  y = rand(rng, 3000)
  z = rand(rng, 10000)
  a = [Sound(x, 8000), Sound(y, 8000), Sound(z, 8000)]
  amfcc = Phonetics.sound2mfcc.(a)
  @test acdist(a[1], a[2]) ≈ dtw(amfcc[1], amfcc[2])[1]
  @test distinctiveness(a[1], a[2:3]) ≈ mean([acdist(a[1], a[2]), acdist(a[1], a[3])])
  dtwr = maximum(size(s, 2) for s in amfcc)
  @test avgseq(a) == dba(amfcc, DTW(dtwr, SqEuclidean()), init_center=amfcc[2], show_progress=false)[1]
end

const SAMPLE_CORPUS = [
["K", "AE1", "T"], # cat
["K", "AA1", "B"], # cob
["B", "AE1", "T"], # bat
["T", "AE1", "T", "S"], # tats
["M", "AA1", "R", "K"], # mark
["K", "AE1", "B"], # cab
]

@testset "Uniqueness point" begin

  ## Unique at second character
  desired = DataFrame(Query=[["K", "AA1", "B"]], UPT=[2])
  @test upt(SAMPLE_CORPUS, [["K", "AA1", "B"]]; inCorpus=true) == desired

  ## Unique at first character
  desired = DataFrame(Query=[["HH", "AE1", "T"]], UPT=[1])
  @test upt(SAMPLE_CORPUS, [["HH", "AE1", "T"]]; inCorpus=false) == desired

  ## Unique at the last character
  desired = DataFrame(Query=[["K", "AE1", "D"]], UPT=[3])
  @test upt(SAMPLE_CORPUS, [["K", "AE1", "D"]]; inCorpus=false) == desired

  ## Not unique
  desired = DataFrame(Query=[["T", "AE1", "T"]], UPT=[4])
  @test upt(SAMPLE_CORPUS, [["T", "AE1", "T"]]; inCorpus=false) == desired
end

@testset "Phonological neighborhood density" begin

  # Showing progress

  ## Neighbors with bat and cab
  desired = DataFrame(Query=[["K", "AE1", "T"]], PND=[2])
  @test pnd(SAMPLE_CORPUS, [["K", "AE1", "T"]]; progress=true) == desired

  ### Neighbors with cat and bat
  desired = DataFrame(Query=[["HH", "AE1", "T"]], PND=[2])
  @test pnd(SAMPLE_CORPUS, [["HH", "AE1", "T"]]; progress=true) == desired

  ### No neighbors
  desired = DataFrame(Query=[["L", "IH1", "V"]], PND=[0])
  @test pnd(SAMPLE_CORPUS, [["L", "IH1", "V"]]; progress=true) == desired

  ## Not showing progress

  ### Neighbors with bat and cab
  desired = DataFrame(Query=[["K", "AE1", "T"]], PND=[2])
  @test pnd(SAMPLE_CORPUS, [["K", "AE1", "T"]]; progress=false) == desired

  ### Neighbors with cat and bat
  desired = DataFrame(Query=[["HH", "AE1", "T"]], PND=[2])
  @test pnd(SAMPLE_CORPUS, [["HH", "AE1", "T"]]; progress=false) == desired

  ### No neighbors
  desired = DataFrame(Query=[["L", "IH1", "V"]], PND=[0])
  @test pnd(SAMPLE_CORPUS, [["L", "IH1", "V"]]; progress=false) == desired
end

@testset "Phonotactic probability" begin

  #=
  **Co-occurrence**

  20 total monophones
  K:      4
  AE1:    4
  T:      4
  S:      1
  M:      1
  AA1:    2
  B:      3
  R:      1

  ["K", "AE1", "T"], get counts
  [4, 4, 4], divide by total monophones
  [0.2, 0.2, 0.2], take product
  0.2^3 = 0.008
  =#
  freq = [1,1,1,1,1,1]
  p = prod([4,4,4] / 20)
  desired = DataFrame(Query=[["K", "AE1", "T"]], Probability=[p])
  prb = phnprb(SAMPLE_CORPUS, freq, [["K", "AE1", "T"]])
  @test prb == desired

  #=

  **Co-occurrence diphones**

  26 total diphones
  . K:    3
  K  AE1: 2
  AE1 T:  3
  T .:    2
  K AA1:  1
  AA1 B:  1
  B .:    2
  . B:    1
  B AE1:  1
  . T:    1
  T AE1:  1
  T S:    1
  S .:    1
  . M:    1
  M AA1:  1
  AA1 R:  1
  R K:    1
  K .:    1
  AE1 B:  1

  [(., K), (K, AE1), (AE1, T), (T .)]
  [3, 2, 3, 2]
  [3/26, 2/26, 3/26, 2/26]
  36/(26^4) = 7.9e-5
  =#
  p = prod([3,2,3,2]/26)
  desired = DataFrame(Query=[["K", "AE1", "T"]], Probability=[p])
  prb = phnprb(SAMPLE_CORPUS, freq, [["K", "AE1", "T"]]; nchar=2)
  @test prb == desired

  #=
  **Co-occurrence diphones, no padding**

  14 total diphones
  K  AE1: 2
  AE1 T:  3
  K AA1:  1
  AA1 B:  1
  B AE1:  1
  T AE1:  1
  T S:    1
  M AA1:  1
  AA1 R:  1
  R K:    1
  AE1 B:  1

  [(K, AE1), (AE1, T)]
  [2, 3]
  [3/14, 2/14] # take product
  6/(14^2) = 7.9e-5
  =#
  p = prod([3,2]/14)
  desired = DataFrame(Query=[["K", "AE1", "T"]], Probability=[p])
  prb = phnprb(SAMPLE_CORPUS, freq, [["K", "AE1", "T"]]; nchar=2, pad=false)
  @test prb == desired

  #=
  **Positional*

  20 total monophones
  K 1:    3
  AE1 2:  4
  AA1 2:  2
  B 3:    2
  B 1:    1
  T 3:    3
  T 1:    1
  S 4:    1
  M 1:    1
  R 3:    1
  K 4:    1

  ["K", "AE1", "T"], get counts
  [3, 4, 3], divide by total monophones
  [3/20, 4/20, 3/20], take product
  36/8000 = 0.0045
  =#
  p = prod([3,4,3]/20)
  desired = DataFrame(Query=[["K", "AE1", "T"]], Probability=[p])
  prb = phnprb(SAMPLE_CORPUS, freq, [["K", "AE1", "T"]]; positional=true)
  @test prb == desired

  # Phone not in corpus
  p = 0
  desired = DataFrame(Query=[["K", "AE1", "F"]], Probability=[p])
  @test phnprb(SAMPLE_CORPUS, freq, [["K", "AE1", "F"]]) == desired

  # With non-1 frequencies; doubling each frequency should produce same output
  freq = [2,2,2,2,2,2]
  p = prod([8,8,8] / 40)
  desired = DataFrame(Query=[["K", "AE1", "T"]], Probability=[p])
  prb = phnprb(SAMPLE_CORPUS, freq, [["K", "AE1", "T"]])
  @test prb == desired

end

@testset "Vantage point tree" begin

  tree = TextVPTree(SAMPLE_CORPUS, lev)

  # Test nneighbors
  res = collect(keys(nneighbors(tree, ["K", "AE1", "T"], 2)))
  res = sort(join.(res, " "))
  @test res == ["B AE1 T", "K AE1 B"]

  ## Test on point not inside of the tree
  res = collect(keys(nneighbors(tree, ["HH", "AE1", "T"], 2)))
  res = sort(join.(res, " "))
  @test res == ["B AE1 T", "K AE1 T"]

  # Test radius search
  res = radiusSearch(tree, ["K", "AE1", "T"], 1)
  res = sort(join.(res, " "))
  @test res == ["B AE1 T", "K AE1 B"]

  ## Test on point not inside of the tree
  res = radiusSearch(tree, ["HH", "AE1", "T"], 1)
  res = sort(join.(res, " "))
  @test res == ["B AE1 T", "K AE1 T"]
end


using ApproxFun, Test
    import ApproxFun: testfunctional, testbandedoperator

@testset "Fractional" begin
    @testset "Jupyer example" begin
        S = Legendre() ⊕ JacobiWeight(0.5,0.,Ultraspherical(1))
        Q½ = LeftIntegral(S,0.5)

        y = (I+Q½)\1
        @test values(y)[1] ≈ 0.33627096683893143

        S = Legendre()⊕JacobiWeight(0.5,0.,Ultraspherical(1))
        testfunctional(rdirichlet(S))
        B=rdirichlet(S)
        D=LeftDerivative(1.5) : S
        testbandedoperator(D)
    end

    ## Avazzadev et al

    # Example 1

    x=Fun(0..1)
    Q=gamma(0.5)*LeftIntegral(0.5)
    f=(2/105*sqrt(x)*(105-56x^2+48x^3))
    u=Q\f
    @test norm(u-(x^3-x^2+1))<100eps()

    # Example 2

    x=Fun(0..1)
    Q=gamma(0.5)*LeftIntegral(0.5)
    u=Q\(exp(x)-1)
    @test norm(u-exp(x)*erf(sqrt(x))/sqrt(π)) < 100eps() # 5.0036177384681187e-14

    # Example 3
    x=Fun(0..1)
    Q=gamma(1/5)*LeftIntegral(1/5)
    u=Q\(x+1)
    @test norm(u-(1+1.25x)*sin(0.8π)/(π*x^(1/5))) < 10eps()

    # Example 4
    x=Fun(0..1)
    Q=gamma(1-1/3)*LeftIntegral(1-1/3)
    u=Q\x^(7/6)
    @test norm(u-7*gamma(1/6)/(18*sqrt(π)*gamma(2/3))*sqrt(x)) < 100eps()

    # Example 5

    d=Interval(0,1)
    x=Fun(d)
    f=x+4/3*x^(3/2)
    S=Legendre(d)⊕JacobiWeight(.5,0.,Jacobi(.5,.5,d))
    Q=gamma(.5)*LeftIntegral(S,.5)

    @test sum(f/sqrt(1-x)) ≈ last(Q*f)

    L=I+Q

    @test last(L.ops[2]*f) ≈ last(Q*f)

    @test last(L*f) ≈ last(f)+last(Q*f)

    u=L\f
    @test norm(u-x)  < 10eps()

    # Example 6

    d=Interval(0,1)
    x=Fun(d)
    f=x^2+16/15*x^(5/2)
    S=Legendre(d)⊕JacobiWeight(.5,0.,Jacobi(.5,.5,d))
    Q=gamma(.5)*LeftIntegral(S,.5)
    L=I+Q
    u=L\f
    @test norm(u-x^2) < 10eps()

    # Example 7

    d=Interval(0.,1.)
    x=Fun(d)
    f=2sqrt(x)
    S=Legendre(d)⊕JacobiWeight(.5,0.,Jacobi(.5,.5,d))
    Q=gamma(.5)*LeftIntegral(S,.5)
    L=I+Q
    u=L\f

    @test norm(1-exp(π*x)*erfc(sqrt(π*x))-u) < 100eps()

    # Example 8

    d=Interval(0.,1.)
    x=Fun(d)
    f=1/(x+1)+2*Fun(x->asinh(sqrt(x))/sqrt(1+x),JacobiWeight(.5,0.,d))
    S=Legendre(d)⊕JacobiWeight(.5,0.,Jacobi(.5,.5,d))
    Q=gamma(.5)*LeftIntegral(S,.5)
    L=I+Q
    u=L\f
    norm((u-1/(x+1)).coefficients) < 1000eps()   # 1.2011889731154679e-14

    ## Test for bug

    QL = LeftIntegral(0.5)	: Legendre()					→	JacobiWeight(0.5,0.,Ultraspherical(1))
    QU = LeftIntegral(0.5)	: JacobiWeight(0.5,0.,Ultraspherical(1))	→	Legendre()

    λ=0.25
    L=[λ*I QU; QL λ*I]
    @test L[2,5] ≈ 0.

    ## Paper examples

    S=Legendre()⊕JacobiWeight(0.5,0.,Ultraspherical(1))
    Q½=LeftIntegral(S,0.5)

    y=(I+Q½)\1

    x=Fun()
    @test norm(exp(1+x)*erfc(sqrt(1+x))-y) < 100eps()

    S=Legendre()⊕JacobiWeight(0.5,0.,Ultraspherical(1))
    x=Fun()
    Q½=LeftIntegral(S,0.5)

    y=(I+exp(-(1+x)/2)*Q½[exp((1+x)/2)])\exp(-(1+x)/2)

    @test norm(y-exp((1+x)/2)*erfc(sqrt(1+x))) < 100eps()
end


# Testing the model-building interface

# All models should have:
# 1. An initial distribution, μ0(θ)
# 2. A transition distribution, f(x,t,θ)
# 3. An observation distribution g(x,t,θ)
# 4. A proposal distribution q(x,y,t,θ)
# 5. A weight function ξ(x,y,t,θ)

δ = 1.0
φ = 0.95
ση = 1.0
σε = 1.0

θ0 = [δ;atanh(φ);ση;σε]

β = [0.0;1.0]

N = 1000

@testset "LinearStateSpaceModel" begin
    @testset "Generic model interface" begin
        arh = ARH(β)
        @test typeof(arh) == LinearStateSpaceModel

        μ0 = initial_distribution(arh,θ0)

        @test mean(μ0) == [δ/(1-φ)]
        @test cov(μ0) == fill(ση^2/(1-φ^2),1,1)

        x0 = rand(μ0)

        u1 = [1.0]

        f  = transition_distribution(arh,x0,u1,1,θ0)

        @test mean(f) == φ*x0 .+ δ
        @test cov(f) == fill(ση^2,1,1)

        x1 = rand(f)

        g  = observation_distribution(arh,x1,u1,1,θ0)

        @test mean(g) == x1
        @test cov(g) == fill(σε^2,1,1)

        y1 = rand(g)

        # TODO: Proposal distributions for LinearStateSpaceModels (#4)
        q = proposal_distribution(arh,x0,y1,u1,1,θ0)

        @test mean(q) == φ*x0 .+ δ
        @test cov(q) == fill(ση^2,1,1)

        ξ = weight_function(arh,x0,y1,u1,1,θ0)

        @test ξ == 0.0
    end

    @testset "Linear model interface" begin
        arh = ARH([0.0;1.0])

        μ = initial_mean(arh,θ0)

        @test μ == fill(δ/(1-φ),1)

        Σ0 = initial_covariance(arh,θ0)

        @test Σ0 == fill(ση^2/(1-φ^2),1,1)

        F = state_transition_matrix(arh,θ0)

        @test F == fill(φ,1,1)

        Q = state_covariance(arh,θ0)

        @test Q == fill(ση^2,1,1)

        G = state_input_matrix(arh,θ0)

        @test G == fill(δ,1,1)

        H = observation_matrix(arh,θ0)

        @test H == fill(β[2],1,1)

        R = observation_covariance(arh,θ0)

        @test R == fill(σε^2,1,1)

        Γ = observation_input_matrix(arh,θ0)

        @test Γ == fill(β[1],1,1)

        Θ = state_noise_transformation_matrix(arh,θ0)

        @test Θ == fill(1.0,1,1)
    end
end

@testset "Kalman filtering and smoothing" begin
    arh = ARH(β)

    X,Y = Filtering.simulate(arh,θ0,1000,fill([1.0],N))

    @test length(X) == N+1
    @test length(Y) == N

    kf = kalman_filter(arh,θ0,Y,fill([1.0],length(Y)))

    ks = kalman_smoother(arh,θ0,kf)
end

@testset "Particle filtering and smoothing" begin
    arh = ARH(β)

    X,Y = Filtering.simulate(arh,θ0,N,fill([1.0],N))

    pf = particle_filter(arh,θ0,Y,100,fill([1.0],N))

    B = 2
    ps = particle_smoother(arh,θ0,Y,pf,B,fill([1.0],N))

    @test size(ps) == (1,N+1,B)
end


using ControlSystems
using LinearAlgebra

"""
    v, xn, xmax, y = simulateDSM(u, ABCD, nlev=2, x0=0)
    v, xn, xmax, y = simulateDSM(u, ntf, nlev=2, x0=0)

Compute the output of a general delta-sigma modulator with input `u`, a
structure described by `ABCD`, an initial state `x0` (default zero) and a
quantizer with a number of levels specified by `nlev`.

Alternatively, the modulator may be described by an `ntf`. The `ntf` is a
TransferFunction object. (The stf is assumed to be 1).

The structure that is simulated is the controllable canonical form
generated by ss().
"""
function simulateDSM(u, arg2, nlev=2, x0=0)
    if arg2 isa TransferFunction
        ntf = arg2
        order = length(ntf.matrix[1].z)

        ABCD = ss(-1/ntf)
        C = ABCD.C
        Sinv = orth([C' Matrix(I,order,order)]) / norm(C)
        S = inv(Sinv)
        C = C * Sinv
        if C[1] < 0
            S = -S
            Sinv = -Sinv
        end

        A = S * ABCD.A * Sinv
        B = S * ABCD.B
        B = [-B B]
        C = [1; zeros(order-1)]
        D = 1
    elseif arg2 isa Array
        if size(arg2,2) > 2 && size(arg2,2) == size(arg2,1)+1 # ABCD dimensions OK
            ABCD = arg2
            order = size(ABCD, 1) - 1
            A = ABCD[1:order, 1:order]
            B = ABCD[1:order, (order+1):(order+2)]
            C = ABCD[order+1, 1:order][:]
            D = ABCD[order+1, order+1]
        else
            error("ABCD argument does not have proper dimensions")
        end
    else
        error("arg2 is neither an ABCD matrix nor an NTF")
    end

    N = length(u)
    v = zeros(Int, N)
    y = zeros(N)
    xn = zeros(order, N)
    xmax = abs(x0)
    if x0 == 0
        x0 = zeros(order)
    end

    for i = 1:N
        y[i] = dot(C, x0) + D*u[i]
        v[i] = ds_quantize(y[i], nlev)
        x0 = A*x0 + B*[u[i]; v[i]]
        xn[:,i] = x0
        xmax = max.(abs.(x0), xmax)
    end

    return v, xn, xmax, y
end

# return an orthormal basis for the columns of A
function orth(A)
    return svd(A).U[:,1:rank(A)]
end

function ds_quantize(y, n)
    if iseven(n)
        v = 2 * floor(Int, 0.5 * y) + 1
    else
        v = 2 * floor(Int, 0.5 * (y + 1))
    end
    L = n - 1
    if v >= L
        v = L
    elseif v <= -L
        v = -L
    end
    return v
end


using MAT, Plots

@views function generate_grf2D(lx=100.0, ly=100.0, sf=1.0, cl=(10.0, 8.0), k_m=100.0, nh=10000, nx=64, ny=64;
                               cov_typ="expon", do_reset=true, do_viz=true, do_save=false)
    # Derived numerics
    dx, dy  = lx/nx, ly/ny  # numerical grid step size
    me = 0
    co1, co2 = 0, 0
    # Array allocation
    Yf      = @zeros(nx, ny)
    # Visu init
    if do_viz
        X, Y = -lx/2:dx:lx/2, -ly/2:dy:ly/2
    end

    if cov_typ=="expon"
        # Generate the 2D exponential covariance function
        wtime_it = grf2D_expon!(Yf, sf, cl, nh, nx, ny, dx, dy; me, co1, co2, do_reset)
    elseif cov_typ=="gauss"
        # Generate the 2D Gaussian covariance function
        wtime_it = grf2D_gauss!(Yf, sf, cl[1]/2.0, nh, k_m, nx, ny, dx, dy; me, co1, co2, do_reset)
    else
        error("Undefined covariance function.")
    end

    # Performance
    A_eff    = 2/1e9*nx*ny*sizeof(Data.Number)  # Effective main memory access per iteration [GB] (Lower bound of required memory access: H and dHdτ have to be read and written (dHdτ for damping): 4 whole-array memaccess; B has to be read: 1 whole-array memaccess)
    T_eff    = A_eff/wtime_it                   # Effective memory throughput [GB/s]
    if (me==0) @printf("T_eff = %1.2f GB/s \n", round(T_eff, sigdigits=3)) end

    # Visualisation
    if do_viz
        display(heatmap(X, Y, Array(Yf)', aspect_ratio=1, xlims=(X[1],X[end]), ylims=(Y[1],Y[end]), c=:hot, title="2D RandomField"))
    end
    if do_save  file = matopen("grf2D_$(cov_typ).mat", "w"); write(file, "grf2D", Array(Yf)); close(file)  end

    return Yf
end


using BasicBSpline
using Plots
gr()

##
p = 2
k = Knots(1:8)
P = BSplineSpace(p, k)
plot([t -> bsplinebasis₊₀(P, i, t) for i in 1:dim(P)], 1, 8, ylims = (0, 1))
savefig("docs/src/img/bsplinebasisplot.png")

##
p = 2
k = Knots(1:8)
P = BSplineSpace(p, k)
plot([t -> bsplinebasis′₊₀(P, i, t) for i in 1:dim(P)], 1, 8)
savefig("docs/src/img/bsplinebasisderivativeplot.png")

##
p = 2
k = Knots(1:8)
P = BSplineSpace(p, k)
plot(t -> sum(bsplinebasis₊₀(P, i, t) for i in 1:dim(P)), 1, 8, ylims = (0, 1))
savefig("docs/src/img/sumofbsplineplot.png")

##
p = 2
k = Knots(1:8) + p * Knots([1, 8])
P = BSplineSpace(p, k)
plot(t -> sum(bsplinebasis₊₀(P, i, t) for i in 1:dim(P)), 1, 8, ylims = (0, 1))
savefig("docs/src/img/sumofbsplineplot2.png")

##
p = 2
k = Knots(1:8) + p * Knots([1, 8])
P = BSplineSpace(p, k)
plot(t -> sum(bsplinebasis(P, i, t) for i in 1:dim(P)), 1, 8, ylims = (0, 1))
savefig("docs/src/img/sumofbsplineplot3.png")

##
P1 = BSplineSpace(1, Knots([1, 3, 5, 8]))
P2 = BSplineSpace(1, Knots([1, 3, 5, 6, 8, 9]))
P3 = BSplineSpace(2, Knots([1, 1, 3, 3, 5, 5, 8, 8]))
plot([t -> bsplinebasis₊₀(P1, i, t) for i in 1:dim(P1)], 1, 9, ylims = (0, 1))
savefig("docs/src/img/bsplineplotP1.png")
plot([t -> bsplinebasis₊₀(P2, i, t) for i in 1:dim(P2)], 1, 9, ylims = (0, 1))
savefig("docs/src/img/bsplineplotP2.png")
plot([t -> bsplinebasis₊₀(P3, i, t) for i in 1:dim(P3)], 1, 9, ylims = (0, 1))
savefig("docs/src/img/bsplineplotP3.png")

plot(
    plot([t -> bsplinebasis₊₀(P1, i, t) for i in 1:dim(P1)], 1, 9, ylims = (0, 1), legend = false),
    plot([t -> bsplinebasis₊₀(P2, i, t) for i in 1:dim(P2)], 1, 9, ylims = (0, 1), legend = false),
    plot([t -> bsplinebasis₊₀(P3, i, t) for i in 1:dim(P3)], 1, 9, ylims = (0, 1), legend = false),
    layout = (3, 1),
    link = :x,
)
savefig("docs/src/img/subbsplineplot.png")

##
A12 = changebasis(P1, P2)
A13 = changebasis(P1, P3)

plot(
    plot([t -> bsplinebasis₊₀(P1, i, t) for i in 1:dim(P1)], 1, 9, ylims = (0, 1), legend = false),
    plot([t -> sum(A12[i, j] * bsplinebasis₊₀(P2, j, t) for j in 1:dim(P2)) for i in 1:dim(P1)], 1, 9, ylims = (0, 1), legend = false),
    plot([t -> sum(A13[i, j] * bsplinebasis₊₀(P3, j, t) for j in 1:dim(P3)) for i in 1:dim(P1)], 1, 9, ylims = (0, 1), legend = false),
    layout = (3, 1),
    link = :x,
)
savefig("docs/src/img/subbsplineplot2.png")

##
p = 3
k = Knots([0.00, 1.50, 2.50, 5.50, 8.00, 9.00, 9.50, 10.0])
P0 = FastBSplineSpace(0, k)
P1 = FastBSplineSpace(1, k)
P2 = FastBSplineSpace(2, k)
P3 = FastBSplineSpace(3, k)
plot(
    plot([t -> bsplinebasis(P0, i, t) for i in 1:dim(P0)], 0, 10, ylims = (0, 1), legend = false),
    plot([t -> bsplinebasis(P1, i, t) for i in 1:dim(P1)], 0, 10, ylims = (0, 1), legend = false),
    plot([t -> bsplinebasis(P2, i, t) for i in 1:dim(P2)], 0, 10, ylims = (0, 1), legend = false),
    plot([t -> bsplinebasis(P3, i, t) for i in 1:dim(P3)], 0, 10, ylims = (0, 1), legend = false),
    layout = (2, 2),
)
savefig("docs/src/img/cover.png")


using Test
using Statistics

function bin2dec(b)
    return sum(bit * 2^(ex-1) for (ex, bit) in enumerate(reverse(b)))
end

"In a vector of 0s and 1s, return the more frequent symbol."
function morefrequent(vec)
    nones = sum(vec)
    n = length(vec)
    return nones >= length(vec)-nones ? 1 : 0
end

function gamma_epsilon(bm)
    binary_gamma = [morefrequent(c) for c in eachcol(bm)]
    binary_epsilon = [1-bit for bit in binary_gamma]
    return bin2dec(binary_gamma), bin2dec(binary_epsilon)
end

function day03a(bm)
    gam, eps = gamma_epsilon(bm)
    return gam*eps
end

function filterrows(f::Function, bm)
    i = 0
    while size(bm)[1] > 1
        i += 1
        bit = f(bm[:,i])
        bm = bm[ bm[:,i].==bit , :]
    end
    return bin2dec(bm[1,:])
end

function oxygen(bm)
    result = filterrows(bm) do col
        morefrequent(col)
    end
    return result
end

function co2(bm)
    result = filterrows(bm) do col
        1-morefrequent(col)
    end
    return result
end

function day03b(bm)
    ox = oxygen(bm)
    co = co2(bm)
    return ox * co
end

function day03(fpath)
    bm = parsebinary(fpath)
    return (day03a(bm), day03b(bm))
end


# Add Max Trust Constraints

using SparseArrays

include("../constraints/constraints.jl")

"""
    makeMaxThrustConstraint(NSteps::Int64, nDim::Int64, tMax::Float64)

Creates an [`AL_Multiple_simpleCone`](@ref) constraint such that the trust
(or equivalently control) vector is capped by `tMax`.

Usage: `NSteps` states the number of timesteps in the trajectory. `nDim` states
the dimensionality of the problem (e.g. 1D, 2D, or 3D). `tMax` is the max
thrust.
"""
function makeMaxThrustConstraint(NSteps::Int64, nDim::Int64, tMax::Float64)
    # sizeTraj = 3 * NSteps * nDim + 2 * nDim

    # This helper function gets the index of ux for any k
    startUInd(k) = (2 * nDim + 1) + (3 * nDim * (k - 1))

    indicatorList = Int64[startUInd(k) for k in 1:NSteps]

    return makeAL_Multiple_simpleCone(tMax, nDim, indicatorList)
end


function fast_non_dominated_sort!(pop)
    n = length(pop)

    dom_list = [ Int[] for i in 1:n ]
    rank = zeros(Int, n)
    dom_count = zeros(Int, n)

    for i in 1:n
        for j in i+1:n
            comparison = compare(pop[i], pop[j])
            if comparison == 1 #is_better(pop[i], pop[j])
                push!(dom_list[i], j)
                dom_count[j] += 1
            elseif comparison == 2 #is_better(pop[j], pop[i])
                push!(dom_list[j], i)
                dom_count[i] += 1
            end
        end
        if dom_count[i] == 0
            rank[i] = 1
        end
    end

    k = UInt16(2)
    while any(==(k-one(UInt16)), (rank[p] for p in 1:n)) #ugly workaround for #15276
        for p in 1:n
            if rank[p] == k-one(UInt16)
                for q in dom_list[p]
                    dom_count[q] -= one(UInt16)
                    if dom_count[q] == zero(UInt16)
                        rank[q] = k
                    end
                end
            end
        end
        k += one(UInt16)
    end

    for i in 1:length(pop)
        pop[i].rank = rank[i]
    end

    sort!(pop, by = s -> s.rank, alg = Base.Sort.QuickSort)
    pop
end

function update_crowding_distance!(pop)

    for sol in pop
        sol.crowding = 0.0
    end

    if length(pop[1].f) == 2
        sort!(pop, by = x -> x.f[1])
        pop[1].f[1] == pop[end].f[1] && return #Don't waste time if all indivs are the same
        pop[1].crowding = pop[end].crowding = Inf

        width_y1 = (pop[end].f[1] - pop[1].f[1])
        width_y2 = (pop[1].f[2] - pop[end].f[2])
        for i = 2:length(pop)-1
            pop[i].crowding = (pop[i+1].f[1] - pop[i-1].f[1]) / width_y1 + (pop[i-1].f[2] - pop[i+1].f[2]) / width_y2
        end

        return pop
    end

    for j = 1:length(first(pop).f) # Foreach objective
        let j = j
            sort!(pop, by = x -> x.f[j]) #sort by the objective value
        end
        pop[1].crowding = pop[end].crowding = Inf #Assign infinite value to extremas
        if pop[1].f[j] != pop[end].f[j]
            for i = 2:length(pop)-1
                pop[i].crowding += (pop[i+1].f[j] - pop[i-1].f[j]) / (pop[end].f[j] - pop[1].f[j])
            end
        end
    end
    pop
end

function truncate_population!(population, N)
    fast_non_dominated_sort!(population)

    let f::Int = 1
        ind = 0
        indnext = findlast(x -> x.rank == f, population)
        while 0 < indnext <= N
            ind = indnext
            f += 1
            indnext = findlast(x -> x.rank == f, population)
        end
        indnext == 0 && (indnext = length(population))
        update_crowding_distance!(view(population, ind+1:indnext))
        sort!(view(population, ind+1:indnext), by = x -> x.crowding, rev = true, alg = PartialQuickSort(N-ind))
    end
    deleteat!(population, N+1:length(population))
end


#' # Solving Day 23 with Julia

#' [Day 23](https://adventofcode.com/2018/day/23) of the [Advent of Code](https://adventofcode.com/) this year
#' was a tough problem I had trouble solving. The basic idea is that a
#' thousand tiny nanobots, each with a 3D location in space and a range representing
#' the maximum [manhattan distance](https://en.wikipedia.org/wiki/Taxicab_geometry)
#' they could interact with, were going to teleport me as long as I was in range of the
#' nanobot that was doing the teleporting. Not knowing for sure which bot that would be,
#' participants had to find the point in 3D space that had the most overlapping nanobots' ranges.
#' My first brute-force approach was to get the cube containing all the nanobots and check
#' each point within. If two or more points have the number of nanobots' overlapping ranges, choose
#' the point closest to 0,0,0 and the answer is the manhattan distance between this point and 0,0,0.
#'
#' After realizing my input had nanobots' locations listed as pos=<-66538252,24214519,54774103>, r=94247941
#' I knew the bruteforce approach wouldn't work. We're talking about a cube thats about
BigInt(100000000)^3
#' ((100 million cubed = 1 septillion) units I'll need to check! Way too many. Some people came up with clever ways to do this. I decided to try
#' [BlackBoxOptim.jl](https://github.com/robertfeldt/BlackBoxOptim.jl).

#' ## Setup
#' After making a structure for my nanobots
struct Nanobot
    x::Int64
    y::Int64
    z::Int64
    r::Int64
end

#' and reading the input:
function getlines(path)::Array{String}
    retval = []
    open(path) do file
        for line in eachline(file)
            push!(retval, line)
        end
    end
    return retval
end
#' I parsed the lines to get positions for my nanobots:
function parse_input()::Array{Nanobot,1}
    lines = getlines("23.txt")
    re = r"^pos=<(-?\d+),(-?\d+),(-?\d+)>, r=(\d+)$"
    nanobots = []
    for line in lines
        matches = match(re, line)
        try
            parts = [parse(Int64, matches[i]) for i in 1:4]
            nanobot = Nanobot(parts...)
            push!(nanobots, nanobot)
        catch
            println(line)
        end
    end
    return nanobots
end
nanobots = parse_input()

#' Helper to calculate Manhattan distance:
function mdist(x,y,z,x2,y2,z2)::Int64
    return abs(x-x2)+abs(y-y2)+abs(z-z2)
end

#' Scoring function to get score of a point in 3D space (this actually returns a function. I didn't want
#' to be doing the nanobot parse_input() function every time):
function score()
    nanobots = parse_input()
    return (w) -> begin
        x = round(w[1])
        y = round(w[2])
        z = round(w[3])
        score = 0
        for n in nanobots
            if mdist(x,y,z,n.x,n.y,n.z) <= n.r
                score += 1
            end
        end
        return float(1000-score)+(float(mdist(x,y,z,0,0,0))/848312125.0)
    end
end
score_func = score()
#' *Note: I will be passing in a 3D point as an array of floating point numbers since
#' that is what BlackBoxOptim will do. In the function I split it into x, y, and z components
#' before rounding them to their integer values and using those. The final score I return is mostly
#' just the score of that point (the number of nanobots it is in range of) with a small part (less than 1 total)
#' to break ties so that two points with the same score will tell me the points that are closer to 0,0,0.*
#' ## The Magic
#' Next up we will be
using BlackBoxOptim
#'
#' BlackBoxOptim needs a range that it will check in. From some other code (not copied here) I found the minimum
#' and maximum values for any of the x, y, and z points for any of the nanobots, including the range they could communicate with.
#' My minimum will be
range_min = -300000000.0
#' and my maximum will be
range_max = 400000000.0
#' (again, both floating point numbers since that is what BlackBoxOptim needs)
#'
#' At this point all the hard work is done. Now you just need to make the optimizer and run it:
res = bboptimize(score_func;
    Method=:adaptive_de_rand_1_bin,
    SearchRange = (range_min,range_max),
    NumDimensions = 3,
    MaxTime = 60.0,
    TraceInterval = 10.0,
    PopulationSize=1000,
    TraceMode=:compact)
#'
#' The first argument is the function to optimize. Given a vector (in this case 3 dimensions) with each
#' value in the search range find the vector that returns a minimum score from the function.
#' The method I have here was just suggested to me by the BlackBoxOptim docs. I have no idea what it's actually doing.
#' I set the max time to 10 minutes and population size to 1000 because I was about to run off to lunch. Even though I
#' got back before the ten minutes was up I was able to see it had converged on a solution and stopped it early,
#' using the best intermediate results it had at that point.
#'
point = map(x->convert(Int64,round(x)), best_candidate(res))
println(point)
println(score_func([point[1],point[2],point[3]]))
#' Pop that through the mdist function to get the distance from 0,0,0 and I had my answer.
mdist(0,0,0,point...)


# Example: Maximum-likelihood estimate of the

using SDE
using Lyap
using LinProc

using Winston
using Optim

srand(7)
d = 2 # dimension
M = 500 #number of observations
TT = .1*M #total time span

# normalize values, returns values in interval [0,1]
function norma!(x)
 minx = min(x)
 x = (x - minx) / (max(x)-minx)
 x
end

grid = TT*norma!(sort!(rand(M))) # a grid of random design points in the interval [0, TT]

# diffusion coefficient (assumed to be known in this example)
sigma = [ 0.9   .2; -.2  .7]
A = sigma*sigma'

# starting point
u = [0.,0.]

# true drift function, b(x) = B0*x + beta0 (unknown, used to obtain observations)

B0 = [  -0.2  -1.0;  # true mean reversion matrix
	 0.5  -0.4]
beta0 = [0.,0.]

# many functions in LinProc expect the solution to the
# Lyapunov equation given B and A as argument instead of A

lambda0 = lyap(B0', -A) # solves B*lambda + lambda*B' = -A

# and many functions need the distances between the design points

dt = diff(grid)

# simulate exact Ornstein--Uhlenbeck process with parameter B0, beta0, A (lambda0)
X =  linexact(u, B0, beta0, lambda0, dt)

# as comparison: loglikelihood of true B
llB0 =  linll(X, B0, beta0, lambda0, dt)
println("True mean reversion matrix B0")
print(B0)
println("Likelihood of B0 ", round(llB0,3))

# looks like this
println("Plotting observations")
plot(X[1,:], X[2,:])

# objective function for maximum likelihood

function objective(Y)

	assert(length(Y) == d*d) # Y is a vector of length d*d parametrizing all stable matrices B
	B = LinProc.stable(Y, d, 0.02) # obtain stable matrix with eigenvalues with real part < 0.02 corresponding to numbers in Y
	lambda = lyap(B', -A)	# lambda depends on B

	ob =  #minimize likelihood
	try
		-linll(X, B, beta0, lambda, dt) # negative discrete observations loglikelihood
	catch y #catch numerical singularies
		if isa(y, Base.PosDefException)
			println("Skip numerical indefinite matrix")
			1.0E16 # move away!
		elseif isa(y, Base.Singular)
			println("Skip numerical singular matrix")
			1.0E16	# move away!
		else
		 throw(y)
		end

	end
	ob
end

# find maximum likelihood estimate for B
println("Maximize likelihood...")
O = optimize(objective, ones(4))
println(O)
B = round(LinProc.stable(O.minimum,2,0.02),3)
llmax = -O.f_minimum
print("\nEstimated mean reversion matrix B (log-likelihood ",round(llmax,3),")\n",round(B,3), "\nTrue B0 (log-likelihood ", round(llB0,3),")\n",B0 )


using Test, PolynomialBases

if !haskey(ENV, "JULIA_PKGEVAL") # sympy is not install on https://github.com/JuliaComputing/NewPkgEval.jl
  import SymPy
  x, α = SymPy.symbols("x, alpha")

  @test 0 == SymPy.simplify( gegenbauer(x, 0, α) - 1 )
  @test 0 == SymPy.simplify( gegenbauer(x, 1, α) - ( 2α*x ) )
  @test 0 == SymPy.simplify( gegenbauer(x, 2, α) - ( -α + 2α*(1+α)*x^2 ) )
  @test 0 == SymPy.simplify( gegenbauer(x, 3, α) - ( -2α*(1+α)*x + 4*α*(1+α)*(2+α)*x^3/3 ) )

  @test_skip gegenbauer(x, 4, α)
end
@inferred gegenbauer(10., 4, 3)


grade(bits::Unsigned) = count_ones(bits)

bits_scalar() = unsigned(0)
bits_first_of_grade(k) = (unsigned(1) << k) - unsigned(1)

bits_basis_vector(i) = unsigned(1) << (i - 1)
# bits_has_index(bits, i) = isone(bits >> (i - 1) & 1)

function bits_to_indices(bits)
	indices = Int[]
	i = 1
	while bits > 0
		if isone(bits & 1)
			push!(indices, i)
		end
		i += 1
		bits >>= 1
	end
	indices
end

function indices_to_bits(indices)
	bits = bits_scalar()
	for i ∈ indices
		bits += bits_basis_vector(i)
	end
	bits
end

"""
Return the smallest uint larger than the one given which has
the same number of binary ones.
Algorithm is [Gosper's hack](http://graphics.stanford.edu/~seander/bithacks.html#NextBitPermutation).

```
julia> GeometricAlgebra.next_bit_permutation(0b1011) |> bitstring
"00001101"
```
"""
function next_bit_permutation(v::Unsigned)
	I = one(v)
	t = v | (v - I)
	(t + I) | (((~t & -~t) - I) >> (trailing_zeros(v) + I))
end

struct FixedGradeBits
	grade::Int
end

function Base.iterate(fgb::FixedGradeBits, bits=bits_first_of_grade(fgb.grade))
	(bits, next_bit_permutation(bits))
end

Base.IteratorSize(::FixedGradeBits) = Base.IsInfinite()

"""
	bits_of_grade(k[, n])

Generate basis blade bits of grade `k` in ascending order.
Yields all basis blades in the dimension `n`, if given, otherwise iterate indefinitely.

Example
===
```jldoctest
julia> bits_of_grade(2, 4) .|> UInt8 .|> bitstring
```
"""

bits_of_grade(k) = FixedGradeBits(k)
bits_of_grade(k, n) = Iterators.take(FixedGradeBits(k), binomial(n, k))

"""
	bits_to_indices(bits)

Return the positions of the ones in the unsigned integer `bits`.

Used to convert between representations of a unit blade.
See also [`$(repr(indices_to_bits))`](@ref).

Examples
===
```jldoctest
julia> GeometricAlgebra.bits_to_indices(0b1001101)
4-element Vector{Int64}:
 1
 3
 4
 7
```
"""
bits_to_indices

"""
	indices_to_bits(indices)

Create unsigned integer with bits at the positions given in the vector `indices`.

Used to convert between representations of a unit blade.
See also [`$(repr(bits_to_indices))`](@ref).

Examples
===
```jldoctest
julia> GeometricAlgebra.indices_to_bits([1, 2, 5]) |> UInt16 |> bitstring
"0000000000010011"
```

"""
indices_to_bits

# # this version is 10% slower
# function bits_to_linear_index(bits::Unsigned)
# 	i = 1
# 	for b ∈ FixedGradeBits(grade(bits))
# 		if b >= bits
# 			return i
# 		end
# 		i += 1
# 	end
# end
function bits_to_linear_index(bits::Unsigned)
	i = 1
	u = bits_first_of_grade(grade(bits))
	while u < bits
		u = next_bit_permutation(u)
		i += 1
	end
	i
end
function linear_index_to_bits(i, k)
	bits = bits_scalar()
	for b ∈ Iterators.take(FixedGradeBits(k), i)
		bits = b
	end
	bits
end

"""
Compute sign flips of blade product due to transposing basis vectors.
(The full sign of the product will also depend on the basis norms.)
"""
function sign_from_swaps(a::Unsigned, b::Unsigned)
	swaps = 0
	while b > 0
		a >>= 1
		swaps += count_ones(a)*(b & 1)
		b >>= 1
	end
	iseven(swaps) ? 1 : -1
end

"""
Compute the overall factor arising from the geometric product between
repeated basis vectors.
"""
function factor_from_squares(sig, squares)
	factor = 1
	bv = 1
	while squares > 0
		if isone(squares & 1)
			factor *= sig[bv]
		end
		squares >>= 1
		bv += 1
	end
	factor
end

function geometric_prod_bits(sig, a::Unsigned, b::Unsigned)
	factor = sign_from_swaps(a, b)*factor_from_squares(sig, a & b)
	bits = a ⊻ b
	factor, bits
end


RD.@autodiff struct CartpoleCost{T} <: TO.CostFunction
    Q::Vector{T}
    R::Vector{T}
    function CartpoleCost(Q::AbstractVector, R::AbstractVector)
        T = Base.promote_eltype(Q,R)
        new{T}(Q,R)
    end
end
function RD.evaluate(cost::CartpoleCost, x, u)
    y = x[1]
    θ = x[2]
    ydot = x[3]
    θdot = x[4]
    J = cost.Q[2] * (1 .- sin(θ/2))
    J += 0.5* (cost.Q[1] * y^2 + cost.Q[3] * ydot^2 + cost.Q[4] * θdot^2)
    if !isempty(u)
        J +=  0.5 * cost.R[1] * u[1]^2
    end
    return J
end
RD.state_dim(::CartpoleCost) = 4
RD.control_dim(::CartpoleCost) = 1
Base.copy(cost::CartpoleCost) = CartpoleCost(copy(cost.Q), copy(cost.R))
RD.default_diffmethod(::CartpoleCost) = RD.ForwardAD()

@testset "Nonlinear Cartpole" begin
## Initialize model
model = RobotZoo.Cartpole()
N = 101
tf = 3.0
dt = tf / (N-1)

## Set up cost function
Q = [0.1,0.1,0.1,0.1] * dt
R = [0.1] * dt
Qf = [100,1000,100,100.]
costfun = CartpoleCost(Q,R)
cfg = costfun.gradcfg

costfun_term = CartpoleCost(Qf,R*0)
obj = Objective(costfun, costfun_term, N)

## Define problem
x0 = @SVector zeros(4)
xf = SA[0,pi,0,0]

# Solve w/ iLQR
prob = Problem(model, obj, x0, tf, xf=xf)
ilqr = Altro.iLQRSolver(prob,
    cost_tolerance=1e-3, gradient_tolerance=1e-2, use_static=Val(false))
ilqr.opts.verbose = 2
b = benchmark_solve!(ilqr)
err = states(ilqr)[end] - xf
@test err'err < 1e-3
# if VERSION >= v"1.5"
#     @test b.allocs == 0
# end

# Add constraints
cons = ConstraintList(4,1,N)
add_constraint!(cons, GoalConstraint(xf), N)
prob = Problem(model, obj, x0, tf, xf=xf, constraints=cons)
solver = ALTROSolver(prob, cost_tolerance_intermediate=1e-2, show_summary=false, use_static=Val(false))
pn = solver.solver_pn
solver.opts.ρ_primal = 1e-3
solver.opts.projected_newton = true
solve!(solver)
A = Altro.getKKTMatrix(pn)
istriu(A[1:5,1:5])
Altro.cost_hessian!(pn)
err = states(ilqr)[end] - xf
@test err'err < 1e-4

end