@testset "test_zpk" begin
# Naming convention:
# ------------------
# {type}_{dims}
# type: C: Continuous, D: Discrete
# dims: "npnuny" (np = # poles)

# CONTINUOUS
C_111 = zpk([-2],[-5],1)
C_211 = zpk([-1+sqrt(2)im,-1-sqrt(2)im], [-5,-3],1)
C_212 = zpk(vecarray(2, 1, [-1+sqrt(2)im,-1-sqrt(2)im], [-2]), vecarray(2, 1, [-5,-3], [-5,-3]), fill(1, 2, 1))
C_221 = zpk(vecarray(1, 2, [-1+sqrt(2)im,-1-sqrt(2)im], [-2]), vecarray(1, 2, [-5,-3], [-5,-3]), fill(1, 1, 2))
C_222 = [C_221; C_221]
C_022 = zpk([4 0; 0 4])
s = zpk("s")

# DISCRETE
D_111 = zpk([-2], [0.5], 1, 0.005)
D_211 = zpk([-1+sqrt(2)im,-1-sqrt(2)im], [-0.3, 0.5], 1, 0.005)
D_221 = zpk(vecarray(1, 2, [-1+sqrt(2)im,-1-sqrt(2)im], [-2]), vecarray(1, 2, [-0.3, 0.5], [-0.3, 0.5]), [1 1], 0.005)
D_222 = [D_221; D_221]
D_022 = zpk([4 0; 0 4], 0.005)
z = zpk("z", 0.005)

# TESTS
# Constructors
@test s == zpk([0], Int64[], 1)
@test z == zpk([0], Int64[], 1, 0.005)
@test C_022 == zpk(vecarray(Int64, 2, 2, Int64[], Int64[], Int64[], Int64[]), vecarray(Int64, 2, 2, Int64[], Int64[], Int64[], Int64[]), [4 0; 0 4])
@test D_022 == zpk(vecarray(2, 2, Int64[], Int64[], Int64[], Int64[]), vecarray(2, 2, Int64[], Int64[], Int64[], Int64[]), [4 0; 0 4], 0.005)
@test C_022 == [zpk(4) 0;0 4]
#TODO We might want to fix this
@test_broken D_022 == [zpk(4, 0.005) 0;0 4]

# Test constructors with empty matrices of type Any
@test zpk([-1.0], [], 1.0) == zpk([-1.0], Float64[], 1.0)
@test zpk([], [-1.0], 1.0) == zpk(Float64[], [-1.0], 1.0)
@test zpk([], [], 1.0) == zpk(Float64[], Float64[], 1.0)
@test zpk([], [1.0+im,1.0-im], 1.0) == zpk(ComplexF64[], [1.0+im,1.0-im], 1.0)

#TODO improve polynomial accuracy se these are equal
# Addition
@test C_111 + C_111 ≈ zpk([-2,-5], [-5,-5], 2)
@test C_222 + C_222 ≈
  zpk(vecarray(2, 2, [-1+sqrt(2)im,-1-sqrt(2)im], [-2], [-1+sqrt(2)im,-1-sqrt(2)im], [-2]), vecarray(2, 2, [-5,-3], [-5,-3], [-5,-3], [-5,-3]), [2 2;2 2])
z1 = [-2.5+sqrt(9-2.5^2)im, -2.5-sqrt(9-2.5^2)im]
z2 = [-4.5-sqrt(4.5^2-17), -4.5+sqrt(4.5^2-17)]

@test C_222 + 1 ≈ zpk(vecarray(2, 2, z1, z2, z1, z2), vecarray(2, 2, [-3, -5], [-3, -5], [-3, -5], [-3, -5]), [2 1; 2 1])
@test D_111 + D_111 ≈ zpk([-2],[0.5],2,0.005)

# Subtraction
@test C_111 - C_211 ≈ zpk([-1], [-5,-3.0], 3.0)
@test minreal(zpk(tf([0,3.0,18,15], [1,13,55,75])), 1e-6) ≈ zpk([-1.0], [-5,-3.0], 3.0)

@test 1 - C_222 ≈ zpk(tf(vecarray(2, 2, [0,6,12], [1,7,13], [0,6,12], [1,7,13]),
  vecarray(2, 2, [1,8,15], [1,8,15], [1,8,15], [1,8,15])))
@test D_111 - D_211 - zpk(tf([0,0.3,-2.55,1.2], [1,-0.7,-0.05,0.075], 0.005)) ≈
  zpk(tf([0], [1], 0.005))

# Multiplication
@test C_111 * C_221 ≈ zpk(tf(vecarray(1, 2, [1,4,7,6], [0,1,4,4]),
  vecarray(1, 2, [1,13,55,75], [1,13,55,75])))
@test C_212 * C_111 ≈ zpk(tf(vecarray(2, 1, [1,4,7,6], [0,1,4,4]),
  vecarray(2, 1, [1,13,55,75], [1,13,55,75])))
@test 4*C_222 ≈ zpk(tf(vecarray(2, 2, [4,8,12], [0,4,8], [4,8,12], [0,4,8]),
  vecarray(2, 2, [1,8,15], [1,8,15], [1,8,15], [1,8,15])))
@test D_111 * D_221 - zpk(tf(vecarray(1, 2, [1,4,7,6], [0,1,4,4]),
  vecarray(1, 2, [1,-0.7,-0.05,0.075], [1.0,-0.7,-0.05,0.075]), 0.005)) ≈
  zpk(tf(vecarray(1, 2, [0], [0]), vecarray(1, 2, [1], [1]), 0.005))

# Division
@test 1/C_111 ≈ zpk(tf([1,5], [1,2]))
@test C_212/C_111 ≈ zpk(tf(vecarray(2, 1, [1,7,13,15], [0,1,7,10]),
  vecarray(2, 1, [1,10,31,30], [1,10,31,30])))
@test 1/D_111 ≈ zpk(tf([1.0,-0.5], [1.0,2.0], 0.005))

# Indexing
@test size(C_222) == (2, 2)
@test size(C_212) == (2, 1)
@test C_222[1,1] ≈ zpk(tf([1, 2, 3], [1, 8, 15]))
@test C_222[1:1,1] ≈ zpk(tf([1, 2, 3], [1, 8, 15]))
@test C_222[1,1:2] == C_221
@test size(C_222[1,Int64[]]) == (1,0)

# Test that number of poles matter
@test !(zpk(Int64[],[1,1],1) == zpk(Int64[],[1],1))

# Test SispZpk and SisoTf operations
C_212_tf = tf(C_212)
C_111_tf = tf(C_111)
# Add
@test C_212_tf + C_212 ≈ 2*C_212
@test C_212 + C_212_tf ≈ 2*C_212
# Approx
@test C_212_tf + C_212 ≈ 2*C_212_tf
# Minus
@test 2*C_212_tf - C_212 ≈ C_212
# Multiply
@test C_212_tf*C_111 ≈ C_212*C_111
@test C_212_tf*C_111 ≈ C_212_tf*C_111_tf

# TODO test printing when it is implemented better

# Tests of minreal
@test minreal(zpk([-5.0], [-5.0, -5.0], 1.0)) == zpk(Float64[], [-5.0], 1.0)
@test minreal(zpk([-1.0, -2.0], [-2.0, -2.0], 1.0)) == zpk([-1.0], [-2.0], 1.0)

# Tests of minreal (roudning and cancelling complex roots with real roots)
@test minreal(zpk([-2.0+1e-10im,-2.0-1e-10im], [-2.0], 1.0)) == zpk([-2.0], Float64[], 1.0)
@test minreal(zpk([-2.0], [-2.0+1e-10im,-2.0-1e-10im], 1.0)) == zpk(Float64[], [-2.0], 1.0)
@test minreal(zpk([-1.0, -2.0, -2.0+1e-10im,-2.0-1e-10im], [-2.0, -2.0], 1.0)) == zpk([-1.0, -2.0], Float64[], 1.0)
@test minreal(zpk([-1.0, -2.0+1e-10im,-2.0-1e-10im,-2.0], [-2.0, -2.0], 1.0)) == zpk([-1.0, -2.0], Float64[], 1.0)
@test minreal(zpk([-2.0, -2.0], [-1.0, -2.0, -2.0+1e-10im,-2.0-1e-10im], 1.0)) ==  zpk(Float64[], [-1.0, -2.0], 1.0)
@test minreal(zpk([-2.0, -2.0], [-1.0, -2.0+1e-10im,-2.0-1e-10im,-2.0], 1.0)) == zpk(Float64[], [-1.0, -2.0], 1.0)

# Test of minreal (systems with no poles / no zeros)
@test minreal(zpk([-1.0, -2.0], Float64[], 2.5)) == zpk([-1.0, -2.0], Float64[], 2.5)
@test minreal(zpk(Float64[], [-1.0, -2.0], 2.5)) == zpk(Float64[], [-1.0, -2.0], 2.5)

# Type stability Continuous and discrete time
# QUESTION: why not use typeof instead of eltype(fill())
@test eltype(fill(zpk("s"),2)) <: TransferFunction
@test eltype(fill(zpk([1],[1,1],1),2)) <: TransferFunction
@test eltype(fill(zpk(Int64[],[1,1],1.0),2)) <: TransferFunction
@test eltype(fill(zpk([1 2; 3 4],0.001),2)) <: TransferFunction
@test eltype(fill(zpk(1)+zpk(2),2)) <: TransferFunction
@test eltype(fill(zpk(1,0.005)/zpk(2, 0.005),2)) <: TransferFunction
@test eltype(fill(zpk(1)+1,2)) <: TransferFunction

@test eltype([tf(1,1), zpk(1,1)]) <: TransferFunction

zpk(tf([1 2; 3 4])) == zpk([1 2; 3 4])

# Errors
@test_throws ErrorException C_111 + C_222             # Dimension mismatch
@test_throws ErrorException C_111 - C_222             # Dimension mismatch
@test_throws ErrorException C_111 * C_222             # Dimension mismatch
@test_throws ErrorException [s 0; 1]                  # Dimension mismatch
@test_throws ErrorException D_111 + C_111             # Sampling time mismatch
@test_throws ErrorException D_111 - C_111             # Sampling time mismatch
@test_throws ErrorException D_111 * C_111             # Sampling time mismatch
D_diffTs = zpk(tf([1], [2], 0.1))
@test_throws ErrorException D_111 + D_diffTs          # Sampling time mismatch
@test_throws ErrorException D_111 - D_diffTs          # Sampling time mismatch
@test_throws ErrorException D_111 * D_diffTs          # Sampling time mismatch
@test_throws ErrorException zpk([1], [2], 1, -0.1)        # Negative samping time
@test_throws ErrorException zpk("s", 0.01)             # s creation can't be discrete
@test_throws ErrorException zpk("z", 0)                # z creation can't be continuous
@test_throws ErrorException zpk("z")                   # z creation can't be continuous
# Remove this when inferec is implemented
@test_throws ErrorException [z 0]                     # Sampling time mismatch (inferec could be implemented)

@test typeof(zpk(tf([1], [2], 0.1))) == TransferFunction{ControlSystems.SisoZpk{Float64,Complex{Float64}}}
@test typeof(zpk([-0.5], [], 1)) == TransferFunction{ControlSystems.SisoZpk{Float64,Float64}}
end


using Test

using MathOptInterface
const MOI = MathOptInterface
const MOIT = MathOptInterface.Test
const MOIU = MathOptInterface.Utilities
const MOIB = MathOptInterface.Bridges

include("../utilities.jl")

mock = MOIU.MockOptimizer(MOIU.Model{Float64}())
config = MOIT.TestConfig()

@testset "RSOC" begin
    bridged_mock = MOIB.Constraint.RSOC{Float64}(mock)

    MOIT.basic_constraint_tests(
        bridged_mock, config,
        include = [(F, S)
                   for F in [MOI.VectorOfVariables, MOI.VectorAffineFunction{Float64},
                             MOI.VectorQuadraticFunction{Float64}]
                   for S in [MOI.RotatedSecondOrderCone]])

    mock.optimize! = (mock::MOIU.MockOptimizer) -> MOIU.mock_optimize!(mock, [0.5, 1.0, 1/√2, 1/√2],
                          (MOI.SingleVariable,                MOI.EqualTo{Float64}) => [-√2, -1/√2],
                          (MOI.VectorAffineFunction{Float64}, MOI.SecondOrderCone)  => [[3/2, 1/2, -1.0, -1.0]])
    MOIT.rotatedsoc1vtest(bridged_mock, config)
    mock.optimize! = (mock::MOIU.MockOptimizer) -> MOIU.mock_optimize!(mock, [1/√2, 1/√2],
                          (MOI.VectorAffineFunction{Float64}, MOI.SecondOrderCone)  => [[3/2, 1/2, -1.0, -1.0]])
    MOIT.rotatedsoc1ftest(bridged_mock, config)
    ci = first(MOI.get(bridged_mock, MOI.ListOfConstraintIndices{MOI.VectorAffineFunction{Float64}, MOI.RotatedSecondOrderCone}()))
    test_delete_bridge(bridged_mock, ci, 2, ((MOI.VectorAffineFunction{Float64}, MOI.SecondOrderCone, 0),))
end


using StatisticalRethinking
using DiffEqBayes, DynamicHMC, DataFrames
using BenchmarkTools
using OrdinaryDiffEq, RecursiveArrayTools, ParameterizedFunctions

using StatsPlots
gr(size=(600,900))

ProjDir = @__DIR__
cd(ProjDir)
isdir(ProjDir*"/tmp") && rm(ProjDir*"/tmp", recursive=true)

f = @ode_def LotkaVolterraTest begin
    dx = a*x - b*x*y
    dy = -c*y + d*x*y
end a b c d

u0 = [30.0, 4.0]
tspan = (0.0, 21.0)
p = [0.55, 0.028, 0.84, 0.026]

prob = ODEProblem(f,u0,tspan,p)
sol = solve(prob,Tsit5())

t = collect(range(1,stop=20,length=20))

df = CSV.read("$(ProjDir)/../lynx_hare.csv", delim=",")

lv = "
  functions {
    real[] dz_dt(real t,       // time
                 real[] z,     // system state {prey, predator}
                 real[] theta, // parameters
                 real[] x_r,   // unused data
                 int[] x_i) {
      real u = z[1];
      real v = z[2];

      real alpha = theta[1];
      real beta = theta[2];
      real gamma = theta[3];
      real delta = theta[4];

      real du_dt = (alpha - beta * v) * u;
      real dv_dt = (-gamma + delta * u) * v;

      return { du_dt, dv_dt };
    }
  }
  data {
    int<lower = 0> N;          // number of measurement times
    real ts[N];                // measurement times > 0
    real y_init[2];            // initial measured populations
    real<lower = 0> y[N, 2];   // measured populations
  }
  parameters {
    real<lower = 0> theta[4];   // { alpha, beta, gamma, delta }
    real<lower = 0> z_init[2];  // initial population
    real<lower = 0> sigma[2];   // measurement errors
  }
  transformed parameters {
    real z[N, 2]
      = integrate_ode_rk45(dz_dt, z_init, 0, ts, theta,
                           rep_array(0.0, 0), rep_array(0, 0),
                           1e-5, 1e-3, 5e2);
  }
  model {
    theta[{1, 3}] ~ normal(1, 0.5);
    theta[{2, 4}] ~ normal(0.05, 0.05);
    sigma ~ lognormal(-1, 1);
    z_init ~ lognormal(10, 1);
    for (k in 1:2) {
      y_init[k] ~ lognormal(log(z_init[k]), sigma[k]);
      y[ , k] ~ lognormal(log(z[, k]), sigma[k]);
    }
  }
  generated quantities {
    real y_init_rep[2];
    real y_rep[N, 2];
    for (k in 1:2) {
      y_init_rep[k] = lognormal_rng(log(z_init[k]), sigma[k]);
      for (n in 1:N)
        y_rep[n, k] = lognormal_rng(log(z[n, k]), sigma[k]);
    }
  }
";

tmpdir = ProjDir*"/tmp"
sm = SampleModel("lynx_hare", lv; tmpdir=tmpdir)

N = size(df, 1) -1
lv_data = Dict(
  "N" => N,
  "ts" => collect(1:N),
  "y_init" => [30, 4],
  "y" => Array(df[2:end, [:Hare, :Lynx]])
)

@time rc = stan_sample(sm, data=lv_data)

if success(rc)

  dfa = read_samples(sm, output_format=:dataframe)
  p = Particles(dfa)
  p[Symbol("theta.1")] |> display
  p[Symbol("theta.2")] |> display
  p[Symbol("theta.3")] |> display
  p[Symbol("theta.4")] |> display
  p[Symbol("sigma.1")] |> display
  p[Symbol("sigma.2")] |> display
  p[Symbol("z_init.1")] |> display
  p[Symbol("z_init.2")] |> display

  println()
  sdf = StanSample.read_summary(sm)
  sdf[8:15, [1, 2, 4, 5, 7, 8, 9]] |> display

  original_stan_result = "
                mean se_mean    sd    10%    50%    90% n_eff Rhat
    theta[1]   0.549   0.002 0.065  0.469  0.545  0.636  1163    1
    theta[2]   0.028   0.000 0.004  0.023  0.028  0.034  1281    1
    theta[3]   0.797   0.003 0.091  0.684  0.791  0.918  1125    1
    theta[4]   0.024   0.000 0.004  0.020  0.024  0.029  1170    1
    sigma[1]   0.248   0.001 0.045  0.198  0.241  0.306  2625    1
    sigma[2]   0.252   0.001 0.044  0.201  0.246  0.310  2808    1
    z_init[1] 33.960   0.056 2.909 30.363 33.871 37.649  2684    1
    z_init[2]  5.949   0.011 0.533  5.294  5.926  6.644  2235    1
  ";

  p1 = plot()
  scatter!(vcat(0, t), df[:, :Hare], lab="Obs hare")
  scatter!(vcat(0, t), vcat(30, [mean(dfa[:, Symbol("y_rep.$i.1")]) for i in 1:20]),
    lab="Pred hare")
  hares = transpose(convert(Array, VectorOfArray(vcat(
    [hpdi(dfa[:, Symbol("z_init.1")])],
    [hpdi(dfa[:, Symbol("y_rep.$i.1")]) for i in 1:20]
  ))))
  mu = vcat(
    mean(dfa[:, Symbol("z_init.1")]),
    [mean(dfa[:, Symbol("y_rep.$i.1")]) for i in 1:20]
  )
  plot!(vcat(0, t), mu; ribbon=hares, alpha=0.4, lab="89% hpd", color=:lightgrey)

  plot!(sol)

  p2 = plot()
  scatter!(vcat(0, t), vcat(4, [mean(dfa[:, Symbol("y_rep.$i.2")]) for i in 1:20]),
    lab="Pred lynx")
  scatter!(vcat(0, t), df[:, :Lynx], lab="Obs Lynx")
  lynxs = transpose(convert(Array, VectorOfArray(vcat(
    [hpdi(dfa[:, Symbol("z_init.2")])],
    [hpdi(dfa[:, Symbol("y_rep.$i.2")]) for i in 1:20]
  ))))
  mu = vcat(
    mean(dfa[:, Symbol("z_init.2")]),
    [mean(dfa[:, Symbol("y_rep.$i.2")]) for i in 1:20]
  )
  plot!(vcat(0, t), mu; ribbon=lynxs, alpha=0.4, lab="89% hpd", color=:lightgrey)

  plot!(sol)

  plot(p1, p2, layout=(2,1))
  savefig("$(ProjDir)/fig_01.png")

end


using DrWatson
@quickactivate :Continuation

using SparseArrays, Pardiso, LinearAlgebra
using PyCall, Printf

ddir = "/media/hdd/chimera/pathfollow_fftg001/"
#ddir = "../../chimera/pathfollow_fftg001/"

function readJac(filename)
    py"""
    import numpy as np
    import scipy.sparse as sp
    def read_jac(filename):
        mat = sp.coo_matrix(sp.load_npz(filename))
        return mat.row + 1, mat.col + 1, mat.data
    """
    sparse(py"read_jac"(filename)...)
end

function readJac(N, σ, α, γ)
    wdir = params_to_fname(N, σ, α, γ)
    if isfile(wdir * "/jac2.npz")
        readJac(wdir * "/jac2.npz")
    else
        readJac(wdir * "/jac1.npz")
    end
end

function float2str(fl::Real)
    replace(string(fl), "." => "")
end

function a2str(α)
    replace((@sprintf "a%.4f" α), "." => "")
end

function params_to_fname(N, σ, α, γ)
    ddir * "N$N" * "s040" * a2str(α) * "g" * float2str(γ)
end

N, σ, α, γ = 128, 0.4, 0.8073, 0.01
jac = readJac(N, σ, α, γ)

using PyPlot
pygui(true)
spy(jac)

diff = jac - transpose(jac)

maximum(diff)

ps = PardisoSolver()

val, vec = eigen(jac)

using BenchmarkTools

# 2.86 GB for N=64 ~ 7s
@btime lu(jac);
F = lu(jac);

detlog = sum(log10.(Complex.(diag(F.L)))) + sum(log10.(Complex.(diag(F.U))))

det(F)


"""
    calculateUtility(weights::Vector{Float64}, objs::Vector{Obj})

    returns the utility "profit/weight" for both the objectives, for the variables
"""
function calculateUtility(weights::Vector{Float64}, objs::Vector{Obj})
    utility1 = zeros(length(weights))
    utility2 = zeros(length(weights))

    for iter = 1:length(weights)
        utility1[iter] = objs[1].profits[iter]/weights[iter]
        utility2[iter] = objs[2].profits[iter]/weights[iter]
    end

    return(utility1, utility2)
end

"""
    permOrder(prob::Problem, order::String = "random")

    returns the permutation vector to order variables according to the given order. A reverse permutation vector is returned too.
"""
function permOrder(prob::Problem, order::String = "random"; verbose = false)

    @assert prob.nbObj == 2 "This problem is 20KP"
    @assert order in ("averageUtility", "bestMin", "rankSum", "random") "Unknown criteria for order the variables"

    u1, u2 = calculateUtility(prob.constraint.weights, prob.objs)

    if order == "bestMin"

        worst = zeros(prob.nbVar)

        for iter in 1:prob.nbVar
            worst[iter] = min(u1[iter], u2[iter])
        end
        verbose && println(worst)

        perm = sortperm(worst, rev=true)
        verbose && println(perm)

    elseif order == "averageUtility"

        avg = zeros(prob.nbVar)
        for iter = 1:prob.nbVar
            avg[iter] = (u1[iter] + u2[iter])/2
        end

        perm = sortperm(avg, rev=true)

    elseif order == "rankSum"

        rank1 = sortperm(u1)
        rank2 = sortperm(u2)

        meanRank = zeros(prob.nbVar)

        for iter in 1:prob.nbVar
            meanRank[iter] = (rank1[iter] + rank2[iter])
        end

        perm = sortperm(meanRank)

    else #random
        perm = Random.shuffle(1:prob.nbVar)
    end

    revPerm = Vector{Int}(undef, prob.nbVar)
    for iter = 1:prob.nbVar
        revPerm[perm[iter]] = iter
    end

    return(perm, revPerm)
end


using OffsetArrays

function reg_term(omega, B = size(omega, 2); alpha = 0.01)
  reg = 0.0
  for b = 1:B
    t = maximum(@views omega[:, b]) - omega[b, b]
    reg += t
  end
  return alpha * reg
end

function reg_term_turbo(omega, B = size(omega, 2); alpha = 0.01)
  reg = 0.0
  @turbo for b = 1:B
    m = -Inf
    for i in axes(omega, 1)
      m = max(m, omega[i, b])
    end
    t = m - omega[b, b]
    reg += t
  end
  return alpha * reg
end

function awmean_lv(x::AbstractArray{T1}, σ::AbstractArray{T2}) where {T1<:Number,T2<:Number}
  n = length(x)
  T3 = promote_type(T1, T2)
  T = sizeof(T3) ≤ 4 ? Float32 : Float64
  sum_of_values = sum_of_weights = χ2 = zero(T)
  @turbo for i = 1:n
    sum_of_values += x[i] / (σ[i] * σ[i])
    sum_of_weights += 1 / (σ[i] * σ[i])
  end
  wx = sum_of_values / sum_of_weights

  @turbo for i = 1:n
    χ2 += (x[i] - wx) * (x[i] - wx) / (σ[i] * σ[i])
  end
  mswd = χ2 / (n - 1)
  wσ = sqrt(one(T) / sum_of_weights)
  return wx, wσ, mswd
end
function awmean_simd(
  x::AbstractArray{T1},
  σ::AbstractArray{T2},
) where {T1<:Number,T2<:Number}
  n = length(x)
  T3 = promote_type(T1, T2)
  T = sizeof(T3) ≤ 4 ? Float32 : Float64
  sum_of_values = sum_of_weights = χ2 = zero(T)
  @fastmath @inbounds @simd for i = 1:n
    sum_of_values += x[i] / (σ[i] * σ[i])
    sum_of_weights += 1 / (σ[i] * σ[i])
  end
  wx = sum_of_values / sum_of_weights

  @fastmath @inbounds @simd for i = 1:n
    χ2 += (x[i] - wx) * (x[i] - wx) / (σ[i] * σ[i])
  end
  mswd = χ2 / (n - 1)
  wσ = sqrt(one(T) / sum_of_weights)
  return wx, wσ, mswd
end

function test_awmean(::Type{T}) where {T}
  for n ∈ 2:100
    if T <: Integer
      x = view(rand(T(-50):T(100), n + 32), 17:n+16)
      σ = view(rand(T(1):T(10), n + 32), 17:n+16)
    else
      x = view((randn(T, n + 32) .+= T(2)), 17:n+16)
      σ = view(rand(T, n + 32), 17:n+16)
    end
    wx, wσ, mswd = awmean_simd(x, σ)
    @test iszero(@allocated((wxlv, wσlv, mswdlv) = awmean_lv(x, σ)))
    wxlv, wσlv, mswdlv = awmean_lv(x, σ)
    isfinite(wx) && @test wx ≈ wxlv
    isfinite(wσ) && @test wσ ≈ wσlv
    isfinite(mswd) &&
      @test mswd ≈ mswdlv atol = eps(typeof(mswdlv)) rtol = sqrt(eps(typeof(mswdlv)))
  end
end

function logℒ_fast(α, β, t, c, x)
  eα = abs(α)
  n, k = size(x)

  (n == length(t) == length(c) && length(β) == k + 1) || throw(DimensionMismatch())
  s = zero(typeof(α))
  @inbounds for i = 1:n
    xb = sum(@inbounds(x[i, j] * β[j+1]) for j = 1:k) + β[1]
    ti = t[i]
    s += (1 - (c[i] == ti)) * (log(eα) + (eα - 1) * log(ti) + xb) - ti^eα * exp(xb)
  end
  return s
end
function logℒ_fast_turbo(α, β, t, c, x)
  eα = abs(α)
  n, k = size(x)

  (n == length(t) == length(c) && length(β) == k + 1) || throw(DimensionMismatch())
  s = zero(typeof(α))
  @turbo for i = 1:n
    xb0 = 0.0
    for j = 1:k
      xb0 += x[i, j] * β[j+1]
    end
    xb = xb0 + β[1]
    ti = t[i]
    s += (1 - (c[i] == ti)) * (log(eα) + (eα - 1) * log(ti) + xb) - ti^eα * exp(xb)
  end
  return s
end

function test_logℒ(n, k)
  t = rand(n)
  c = copy(t)
  b = rand(n) .> 0.5
  c[b] .= rand.()
  x = rand(n, k)
  α = 2.85
  β = rand(k + 1)
  @test logℒ_fast(α, β, t, c, x) ≈ logℒ_fast_turbo(α, β, t, c, x)
end

function not_an_outer_reduct!(r, N::Int, x = 2.0, y = nothing) # there was a bug where this was classified as one
  @turbo for i ∈ eachindex(r)
    acc = y === nothing ? x : r[i]
    for n ∈ 1:N
      acc += 0
    end
    r[i] = acc
  end
  r
end
function tk(v::Vector{T}, c_::Vector{T}, n::Int) where {T}
  l00 = l10 = l01 = l11 = zero(T)
  @turbo for j ∈ 1:n-1
    a00 = v[3n-3j+1]
    a01 = -v[3n-3j+2]
    a10 = v[3n-3j+2]
    a11 = v[3n-3j+3]
    b00 = c_[4j+1]
    b01 = c_[4j+2]
    b10 = c_[4j+3]
    b11 = c_[4j+4]
    l00 += a00 * b00 + a01 * b10
    l01 += a00 * b01 + a01 * b11
    l10 += a10 * b00 + a11 * b10
    l11 += a10 * b01 + a11 * b11
  end
  l00 + l01 + l10 + l11
end
function tk2(v::Vector{T}, c_::Vector{T}, n::Int) where {T}
  l00 = l10 = l01 = l11 = zero(T)
  @turbo for j ∈ 1:n-1
    i = n - j
    a00 = v[3i+1]
    a01 = -v[3i+2]
    a10 = v[3i+2]
    a11 = v[3i+3]
    b00 = c_[4j+1]
    b01 = c_[4j+2]
    b10 = c_[4j+3]
    b11 = c_[4j+4]
    l00 += a00 * b00 + a01 * b10
    l01 += a00 * b01 + a01 * b11
    l10 += a10 * b00 + a11 * b10
    l11 += a10 * b01 + a11 * b11
  end
  l00 + l01 + l10 + l11
end
function tk_base(v::Vector{T}, c_::Vector{T}, n::Int) where {T}
  l00 = l10 = l01 = l11 = zero(T)
  @inbounds @simd for j ∈ 1:n-1
    a00 = v[3n-3j+1]
    a01 = -v[3n-3j+2]
    a10 = v[3n-3j+2]
    a11 = v[3n-3j+3]
    i = n - j
    a00 = v[3i+1]
    a01 = -v[3i+2]
    a10 = v[3i+2]
    a11 = v[3i+3]
    b00 = c_[4j+1]
    b01 = c_[4j+2]
    b10 = c_[4j+3]
    b11 = c_[4j+4]
    l00 += a00 * b00 + a01 * b10
    l01 += a00 * b01 + a01 * b11
    l10 += a10 * b00 + a11 * b10
    l11 += a10 * b01 + a11 * b11
  end
  l00 + l01 + l10 + l11
end

function noturbosum(x, kern)
  ks = zero(eltype(x))
  @inbounds @fastmath for i = 2:size(x, 1)-1
    for j in axes(kern, 1), m in axes(kern, 2)
      ks += x[i+j, i+m] * kern[j, m]
    end
  end
  ks
end
function outersum(x, kern)
  ks = zero(eltype(x))
  @turbo for i = 2:size(x, 1)-1
    for j in axes(kern, 1), m in axes(kern, 2)
      ks += x[i+j, i+m] * kern[j, m]
    end
  end
  ks
end
function insum(x, kern)
  ks = zero(eltype(x))
  @turbo for i = 2:size(x, 1)-1
    s1 = zero(eltype(x))
    for j in axes(kern, 1), m in axes(kern, 2)
      s1 += x[i+j, i+m] * kern[j, m]
    end
    ks += s1
  end
  ks
end
function in2sum(x, kern)
  ks = zero(eltype(x))
  @turbo for i = 2:size(x, 1)-1
    for j in axes(kern, 1)
      s0 = zero(eltype(x))
      for m in axes(kern, 2)
        s0 += x[i+j, i+m] * kern[j, m]
      end
      ks += s0
    end
  end
  ks
end

@testset "Outer Reductions" begin
  for T ∈ [Float32, Float64, Int32, Int64]
    test_awmean(T)
    v = rand(T, 4 * 127)
    c = rand(T, 4 * 127)
    @test tk(v, c, 127) ≈ tk2(v, c, 127) ≈ tk_base(v, c, 127)
  end
  @test all(==(7.4), not_an_outer_reduct!(Vector{Float64}(undef, 5), 17, 7.4))
  for n ∈ 1:20, k ∈ 1:5
    test_logℒ(n, k)
  end
  omega = rand(87, 87)
  @test reg_term(omega) ≈ reg_term_turbo(omega)

  x = rand(100, 100)
  kern = OffsetArray(rand(3, 3), -2, -2)
  s = noturbosum(x, kern)
  @test s ≈ outersum(x, kern)
  @test s ≈ insum(x, kern)
  @test s ≈ in2sum(x, kern)
end


"""
prob \\

for the prob function specifically \\
Arguments:	dec_x = array, the decimalised random variable \\
						x_dist_dim = dimension of the probability distribution of x \\
						sampling = the sampling method \\
Returns:		probs, The probability distribution of x \\
"""

function prob(dec_x::Array{Int, 1}, x_dist_dim::Int, sampling::String)
	edges=(0:x_dist_dim) - 0.5
	bin_counts = fit(Histogram, dec_x, edges).weights
	if sampling == "naive" # TODO add more sampling methods
		probs = bin_counts/length(dec_x)
	end
	return probs
end


module SymbolicMath

import Base: +, -, *, /, ^, cos, sin, log, exp, abs, real, imag, convert
import Base.==

abstract type AbstractTerm end
struct ExprTerm <: AbstractTerm
    ex
end
convert(::Type{ExprTerm}, ex) = ExprTerm(ex)

macro ExprTerm(ex)
    ExprTerm(ex)
end

function createsymbol(s::Symbol, m::Module)
    @eval m $s = $(Meta.quot(s)) |> ExprTerm
end
function createsymbol(s::Symbol)
    createsymbol(s, Main)
end

macro symbols(args::Symbol...)
    createsymbol.(args, Ref(__module__))
end
macro symbol(s::Symbol)
    createsymbol(s, __module__)
end

# euqality comparison
==(t1::ExprTerm, t2::ExprTerm) = (t1.ex == t2.ex)
==(t::ExprTerm, other) = (t.ex == other)
==(other, t::ExprTerm) = (t == other)

# operator definitions
binary_opeators = [:+, :-, :*, :/, :^]
unary_operators = [:cos, :sin, :log, :exp, :abs, :real, :imag]

for op in binary_opeators
    @eval $op(t1::ExprTerm, t2::ExprTerm) = Expr(:call, $(Meta.quot(op)), t1.ex, t2.ex) |> ExprTerm
    @eval $op(t::ExprTerm, other) = Expr(:call, $(Meta.quot(op)), t.ex, other) |> ExprTerm
    @eval $op(other, t::ExprTerm) = Expr(:call, $(Meta.quot(op)), other, t.ex) |> ExprTerm
end
for op in unary_operators
    @eval $op(t::ExprTerm) = Expr(:call, $(Meta.quot(op)), t.ex) |> ExprTerm
end

export @symbol, @symbols, @ExprTerm, ExprTerm

end # module SymbolicMath


using ChaosTools, Test, Statistics, DynamicalSystemsBase

println("\nTesting transit time statistics...")
@testset "Transit time statistics" begin

@testset "Standard map (exact)" begin
# INPUT
ds = Systems.standardmap()
T = 10000 # maximum time

# period 3 of standard map
u0 = SVector(0.8121, 1.6243)
εs = sort!([4.0, 2.0, 0.01]; rev=true)
# radius of 4.0 covers the first 2 points of the period 3 while 2.0 covers only
# the first point. Therefore:
# all first return times must be 1, and all second return times must be 2,
# and 3rd must be same as second (because it doesn't matter how close are)
exits, entries = exit_entry_times(ds, u0, εs, T)
transits, returns = transit_return(exits, entries)

@test all(x -> length(x) > 5, exits)
@test all(x -> length(x) > 5, entries)
@test all(issorted, exits)
@test all(issorted, entries)
@test all(isequal(2), transits[1])
@test all(isequal(1), transits[2])
@test transits[2] == transits[3]
@test all(isequal(1), returns[1])
@test all(isequal(2), returns[2])
@test returns[2] == returns[3]

τ, c = mean_return_times(ds, u0, εs, T)
@test τ == mean.(returns)
@test length(unique(c)) == 1
@test c[1] == T÷3 # should return exactly 1 every 3 steps.

### quasiperiodic around period 3:
u0 = SVector(0.877, 1.565)
εs = sort!([4.0, 0.5, 0.1]; rev=true)
exits, entries = exit_entry_times(ds, u0, εs, T)
transits, returns = transit_return(exits, entries)

@test all(issorted, exits)
@test all(issorted, entries)
@test all(x -> length(x) > 5, exits)
@test all(x -> length(x) > 5, entries)

# For ε=4.0, nothing changes with the before
@test all(isequal(1), returns[1])
@test all(isequal(2), transits[1])

# Similarly, 0.5 should be the same as before
@test all(isequal(1), transits[2])
@test all(isequal(2), returns[2])

# But now, the third entry is different, because it has the size of the quasiperiodic
# stability island torous
@test returns[3] ≠ returns[2]
@test transits[3] ≠ transits[2]
@test any(>(3), returns[3])
@test all(isequal(1), transits[3]) # still need only one step to exit

τ, c = mean_return_times(ds, u0, εs, T)
@test τ == mean.(returns)
@test length(unique(c)) == 2
@test c[1] == T÷3
@test c[2] == T÷3
@test c[3] < T÷3

end

@testset "Towel map (boxes)" begin

to = Systems.towel()
tr = trajectory(to, 5000; Ttr = 10)
u0 = tr[3000]

# With these boxes, in the first 5 steps, the trajectory enters the y and z range
# but not the x range. Therefore it should NOT enter the box. See figure!
εs = [
    SVector(0.05, 0.05, 0.125),
    SVector(0.005, 0.005, 0.025),
]

# Visual guidance
# using PyPlot
# tr0 = trajectory(to, 5, u0)
# fig, axs = subplots(1,3)
# comb = ((1, 2), (1, 3), (2, 3))
# for i in 1:3
#     j, k = comb[i]
#     ax = axs[i]
#     ax.scatter(tr[:, j], tr[:, k], s = 2, color = "C$(i-1)")
#     ax.scatter([u0[j]], [u0[k]], s = 20, color = "k")
#     ax.plot(tr0[:, j], tr0[:, k], color = "k")
#     for l in 1:length(εs)
#         rect = matplotlib.patches.Rectangle(
#         u0[[j, k]] .- εs[l][[j, k]], 2εs[l][j], 2εs[l][k],
#         alpha = 0.25, color = "k"
#         )
#         ax.add_artist(rect)
#     end
# end

exits, entries = exit_entry_times(to, u0, εs, 10000)
transits, returns = transit_return(exits, entries)

@test all(issorted, exits)
@test all(issorted, entries)
@test length(exits[1]) > length(exits[2])
@test returns[1][1] > 5
@test mean(returns[1]) < mean(returns[2])

τ, c = mean_return_times(to, u0, εs, 10000)
@test τ == mean.(returns)
@test c[1] > c[2] > 0

end
#
@testset "Continuous Roessler" begin
# %%
# using OrdinaryDiffEq: Tsit5
alg = DynamicalSystemsBase.DEFAULT_SOLVER

ro = Systems.roessler(ones(3))
u0 = SVector(
    4.705494942754781,
    -10.221120945130545,
    0.06186563933318555
)
εs = sort!([1.0, 0.1, 0.01]; rev=true)
avg_period = 6.0

# # Visual guidance
# using PyPlot
# tr = trajectory(ro, 5000, u0; alg)
# tr0 = trajectory(ro, avg_period, u0; alg)
# tr2 = trajectory(ro, 3avg_period, u0; alg)
# fig, axs = subplots(1,3)
# comb = ((1, 2), (1, 3), (2, 3))
# for i in 1:3
#     j, k = comb[i]
#     ax = axs[i]
#     # ax.plot(tr[:, j], tr[:, k], lw = 2.0, color = "C$(i-1)", alpha = 0.5, marker = "o", ms = 2)
#     ax.plot(tr[:, j], tr[:, k], lw = 2.0, color = "C$(i-1)")
#     ax.scatter([u0[j]], [u0[k]], s = 20, color = "k")
#     ax.plot(tr2[:, j], tr2[:, k], color = "C4", lw = 1.0, ls = "--")
#     ax.plot(tr0[:, j], tr0[:, k], color = "k", lw = 1.0, ls = "--")
#     if eltype(εs[1]) <: Vector
#         for l in 1:length(εs)
#             rect = matplotlib.patches.Rectangle(
#             u0[[j, k]] .- εs[l][[j, k]], 2εs[l][j], 2εs[l][k],
#             alpha = 0.1, color = "k"
#             )
#             ax.add_artist(rect)
#         end
#     else
#         for l in 1:length(εs)
#             circ = matplotlib.patches.Circle(
#                 u0[[j, k]], εs[l]; alpha = 0.1, color = "k"
#             )
#             ax.add_artist(circ)
#         end
#     end
# end

# %%
# We know the average period of the Roessler system. Therefore the return times
# cannot be possibly smaller than it (because u0 is in the xy plane)
# We also know (from the plot) that the first returns after around 3 periods.
# We aaaalso know (by zooming in the plot) that the innermost ball is recurred exactly twice,
# however one of the two crossings is grazing and thus likely to not be spotted by the
# algorithm
τ, c = mean_return_times(ro, u0, εs, 3avg_period; diffeq = (alg = alg,), i=20)
@test c[1] == 1
@test c[2] == c[3] == 0
@test 2avg_period < τ[1] < 3avg_period
@test issorted(c; rev=true)

τ, c = mean_return_times(ro, u0, εs, 5000.0; diffeq = (alg = alg,), i=20)
@test all(τ .> avg_period/2)
@test 0 < c[3] ≤ 2
@test issorted(c; rev=true)

x = sort!(MathConstants.e .^ (-4:0.5:-1); rev = true)
Ts = 10.0 .^ range(3, 6, length = 7)
is = range(10; step = 4, length = 7)
τd, cd_ = mean_return_times(ro, u0, x, Ts; i=is, dmin =10.0)
@test issorted(τd)
@test all(z -> z > 0, cd_)

# figure()
# plot(log.(x), log.(τd); marker ="o")
# d = -ChaosTools.slope(log.(x), log.(τd))
# The slope of the above plot should approximate the fractal dimension
# but I don't find this to be true unfortunately...

# Test with hyper rectangles. Both same outcome because we are anyway in the flat part
εs = [
    SVector(0.1, 0.1, 0.5),
    SVector(0.1, 0.1, 0.05),
]
τ2, c2 = mean_return_times(ro, u0, εs, 5000.0; diffeq = (alg = alg,), i=20)

@test τ2[1] < τ[2]

end
end


module VariationalOnlineStats

using Statistics
using Base.Iterators

export MedianOnline

struct MedianOnline{T}
    med::T   # median
    mad::T   # mean absolute deviation
    n::Int
end
(::Type{MedianOnline{T}})(x) where T = MedianOnline{T}(x, zero(T), 1)

MedianOnline(x::T) where {T<:AbstractFloat} = MedianOnline{T}(x)

function MedianOnline{T}(v::AbstractVector{<:Number}) where {T<:AbstractFloat}
    m = MedianOnline{T}(first(v))
    for x in drop(v, 1)
        m = median(m, x)
    end
    m
end
MedianOnline(v::AbstractVector{T}) where {T<:AbstractFloat} = MedianOnline{T}(v)

@inline function Statistics.median(m::MedianOnline{T}, x) where {T}
    f = 1/(m.n + 1)  # fraction of contribution to mad
    Δx = x - m.med
    aΔx = abs(Δx)
    mad = (1-f) * m.mad + f * aΔx
    Δv = mad/m.n   # separation of the closest point among n from an
                   # exponential distribution with the given mad
    med = m.med + sign(Δx) * min(aΔx, Δv)
    MedianOnline{T}(med, mad, m.n+1)
end

(::Type{T})(m::MedianOnline{T}) where {T<:Number} = m.med
(::Type{T})(m::MedianOnline{T}) where T = m.med
(::Type{T})(m::MedianOnline{<:Number}) where {T<:Number} = convert(T, m.med)

end # module


module SumBassSolver
using DataStructures, Ipopt, JuMP, Roots, SigmoidalProgramming, Optim

export Bass, Bass_derivative, Bass_inverse, Bass_derivative_inverse, Bass_inflection
export sumBassSolver
export sumBassHomogeneousSolver, sumBassDiffStartSolver
export sumSigmoidSolverSB

include("Bass.jl")
include("UdellBoyd.jl")
include("SrivastavaBullo.jl")
include("LocalOpt.jl")
include("HomogeneousSolver.jl")
include("DiffstartSolver.jl")
include("SrivastavaBulloGen.jl")

@doc raw"""
    sumBassSolver(p,q,m,c0,alpha,F,T,n,Pi;alg="UB", localopt=false)

This function maximizes the sum Bass functions and returns the maximizer
and maximum. `alg` is one of UB (Udell and Boyd's algorithm) or
SB (Srivastava and Bullo's algorithm). `localopt` indicates making a local search
after the algorithms terminate.
"""
function sumBassSolver(p,q,m,c0,alpha,F,T,Pi::Real;alg = "SB",localopt=false)
    n = length(p) # Number of sites
    if sum(F) > Pi
        throw(DomainError(Pi,"the problem is unfeasible because the capacity is too small."))
    end
    if n==0
        throw(DomainError(p,"inputs need to be of size at least 1"))
    end
    if length(q) != n || length(m) != n || length(c0) != n || length(alpha) != n || length(F) != n || length(T) != n
        throw(DomainError("p,q,m,c0,alpha,F,T","inputs must be vectors of the same length."))
    end
    if sum(c0 .> m) > 0
        throw(DomainError("c0,m","existing clients needs to be smaller than the market potential."))
    end

    if alg == "UB"
        maximizer,maximum = sumBassUB(p,q,m,c0,alpha,F,T,Pi)
    elseif alg == "SB"
        maximizer,maximum = sumBassSB(p,q,m,c0,alpha,F,T,Pi)
    else
        throw(DomainError(alg,"alg is one of UB (Udell and Boyd's algorithm) or SB (Srivastava and Bullo's algorithm)."))
    end

    if localopt
        maximizer,maximum = sumBassLocal(p,q,m,c0,alpha,F,T,Pi,maximizer)
    end

    return maximizer,maximum
end

end


module TestLQcontrol

using QuantEcon
using Base.Test
using FactCheck

rough_kwargs = Dict(:atol => 1e-13, :rtol => 1e-4)

# set up
q    = 1.
r    = 1.
rf   = 1.
a    = .95
b    = -1.
c    = .05
β    = .95
n    = 0.
capT = 1
lq_scalar = LQ(q, r, a, b, c, n, β, capT, rf)

Q  = [0. 0.; 0. 1]
R  = [1. 0.; 0. 0]
rf = eye(2) .* 100
A  = fill(0.95, 2, 2)
B  = fill(-1.0, 2, 2)
lq_mat = LQ(Q, R, A, B, bet=β, capT=capT, rf=rf)

facts("Testing lqcontrol.jl") do
    context("Test scalar sequences with exact by hand solution") do
        x0 = 2.0
        x_seq, u_seq, w_seq = compute_sequence(lq_scalar, x0)
        # solve by hand
        u_0 = (-2.*lq_scalar.A*lq_scalar.B*lq_scalar.bet*lq_scalar.rf) /
           (2.*lq_scalar.Q+lq_scalar.bet*lq_scalar.rf*2lq_scalar.B^2)*x0
        x_1 = lq_scalar.A * x0 + lq_scalar.B * u_0 + w_seq[end]

        @fact u_0[1] --> roughly(u_seq[end]; rough_kwargs...)
        @fact x_1[1] --> roughly(x_seq[end]; rough_kwargs...)
    end

    context("test matrix solutions") do
        x0 = randn(2) * 25
        x_seq, u_seq, w_seq = compute_sequence(lq_mat, x0)

        @fact sum(u_seq) --> roughly(0.95 * sum(x0); rough_kwargs...)
        @fact x_seq[:,end] --> roughly(zeros(x0); rough_kwargs...)
    end

    context("test stationary matrix") do
        x0 = randn(2) .* 25
        P, F, d = stationary_values(lq_mat)

        f_answer = [-.95 -.95; 0. 0.]
        p_answer = [1. 0; 0. 0.]

        val_func_lq = (x0' * P * x0)[1]
        val_func_answer = x0[1]^2

        @fact f_answer --> roughly(F; rough_kwargs...)
        @fact val_func_lq --> roughly(val_func_answer; rough_kwargs...)
        @fact p_answer --> roughly(P; rough_kwargs...)
    end

    context("test runs a (n,k,j) = (2,1,1) model") do
        # == Model parameters == #
        r = 0.05
        bet = 1 / (1 + r)
        T = 45
        c_bar = 2.0
        sigma = 0.25
        mu = 1.0
        q = 1e6

        # == Formulate as an LQ problem == #
        Q = 1.0
        R = zeros(2, 2)
        Rf = zeros(2, 2); Rf[1, 1] = q
        A = [1.0+r -c_bar+mu;
             0.0 1.0]
        B = [-1.0, 0.0]
        C = [sigma, 0.0]

        # == Compute solutions and simulate == #
        lq = LQ(Q, R, A, B, C; bet=bet, capT=T, rf=Rf)
        x0 = [0.0, 1.0]
        xp, up, wp = compute_sequence(lq, x0)
        @fact true --> true  # just assert true if we made it to this point
    end

end  # facts
end  # module


module DynamicTimeWarp

"""
    dtw(
        sequence1, sequence2,
        dist=(a, b)->(a - b)'*(a - b)
    ) -> (path1, path2, cost)

Compute the set of indices `path1, path2` that give the alignment of two
sequences over their last dimension by dynamic time warping.

Accepts as an optional parameter the distance function to be used in the
evaluation of the optimum path. The two arguments passed to the distance
function are views of the original sequences over the last dimension preserving
their shape. In a vector, for example, the distances will be computed between
two scalars, in a 2 by n array between 2 by 1 vectors, in a 2 by 3 by n between
2 by 3 arrays and so on.

Returns the warping paths and the total cost of the optimum path.

See also: [`linkseq`](@ref)
"""
function dtw(
	sequence1::AbstractArray,
	sequence2::AbstractArray,
	dist::Function=(a, b)->(a - b)'*(a - b)
)

	length1 = size(sequence1)[end]
	length2 = size(sequence2)[end]

	# The DTW is calculated over the last dimension of the array, the next
	# function ensures that the shape of the original elements in the sequence
	# will be preserved during the computation of the distance.
	d = (a, b) -> dist(
		selectdim(sequence1, ndims(sequence1), a),
		selectdim(sequence2, ndims(sequence2), b)
	)

	initialCost = d(1, 1)
	cost = Matrix{typeof(initialCost)}(undef, length1, length2)
	cost[1, 1] = initialCost

	# Fill the first line...
	for i in 2:length1
		cost[i, 1] = cost[i - 1, 1] + d(i, 1)
	end

	# ...and the first column of the cost matrix.
	for j in 2:length2
		cost[1, j] = cost[1, j - 1] + d(1, j)
	end

	# Compute the accumulated cost of the remaining elements
	for i in 2:length1
		for j in 2:length2
			cost[i, j] = d(i, j) +
				min(cost[i - 1, j], cost[i - 1, j - 1], cost[i, j - 1])
		end
	end

	return trackback(cost)..., cost[end]

end

"""
    trackback(cost::AbstractMatrix) -> (path1, path2)

Compute the lowest cost path from a cost array. Returns two vectors
representing the path.
"""
function trackback(cost::AbstractMatrix)

	i, j = size(cost)
	# Start to create the path from the last element of the cost matrix.
	path1, path2 = [i], [j]

	while i > 1 && j > 1
		least = argmin(cost[i-1:i,j-1:j])
		# Everytime that the minimal cost is reached by going back along one
		# dimension, decrease the corresponding index.
		push!(path1, (least[1] == 1 ? i-=1 : i))
		push!(path2, (least[2] == 1 ? j-=1 : j))
	end

	# One of the indexes may still be greater than one if the path hits a border
	# of the cost matrix, in this case the path must be completed with a straight
	# line to the first element of the cost matrix.
	if i > 1
		append!(path1, i-1:-1:1)
		append!(path2, ones(length(path1) - length(path2)))
	end

	if j > 1
		append!(path2, j-1:-1:1)
		append!(path1, ones(length(path2) - length(path1)))
	end

	return reverse(path1), reverse(path2)
end

"""
    linkseq(sequence1, sequence2, sr=1, noflinks=0) -> (x, y)

Create vectors containing the horizontal and the vertical coordinates of each
pair of corresponding points of two sequences computated by using the dtw.

The returned vectors can be used to plot the paths linking the two sequences
over an existing plot:
```jldoctest
julia> p = plot(0:1/100:length(sequence1), [sequence1, sequence2])
julia> x, y = linkseq(sequence1, sequence2, 100, 20)
julia> plot!(p, x, y);
```

**Parameters:**

`sequence1`, `sequence2`: Sequences for evaluating the dtw

`sr`: The sampling rate to be used in the horizontal axis, should match the
original sampling rate of the sequence.

`noflinks`: The number of links between pairs of corresponding points to be
created. If zero is received create links between all pairs.
"""
function linkseq(sequence1::AbstractArray, sequence2::AbstractArray, sr=1, noflinks=0)

	path1, path2 = dtw(sequence1, sequence2)

	# Recreate the original independent axis
	len1 = size(sequence1)[end]
	len2 = size(sequence2)[end]
	t = range(0; step=1/sr, length=max(len1, len2))

	links = 1 : (noflinks == 0 ? 1 : (length(path1) ÷ noflinks)) : length(path1)

	# Create an array of ranges that will be used to preserve the original shape
	# of the sequence
	s = map(n->1:n, size(sequence1)[1:end-1])

	x = Array{typeof(t[1])}(undef, 2, length(links))
	y = Array{typeof(sequence1[1])}(undef, 2, length.(s)..., length(links))

	j = 1
	for l in links
		x[:,j] = [t[path1[l]]; t[path2[l]]]
		y[:, s..., j] = [sequence1[s..., path1[l]]; sequence2[s..., path2[l]]]
		j+=1
	end

	return (x, y)

end

export dtw, linkseq

end # module


function DOB(ϕ, alpharange, alphastep, tN, tstep)
    alphas = collect(alpharange[1]:alphastep:alpharange[2])
    alphacount = length(alphas)

    result = zeros(tN, tN)
    phi = ϕ.(alphas)

    for k=1:alphacount
        result = result .+ phi[k]*alphastep*D(tN, alphas[k], tstep)
    end
    return result
end

function DOF(ϕ, alpharange, alphastep, tN, tstep)
    alphas = collect(alpharange[1]:alphastep:alpharange[2])
    alphacount = length(alphas)

    result = zeros(tN, tN)

    phi = ϕ.(alphas)

    for k=1:alphacount
        result = result .+ phi[k]*alphastep*F(tN, alphas[k], tstep)
    end
    return result
end

function DORANORT(ϕ, alpharange, alphastep, tN, tstep)
    alphas = collect(alpharange[1]:alphastep:alpharange[2])
    alphacount = length(alphas)

    result = zeros(tN, tN)

    phi = ϕ.(alphas)

    for k=1:alphacount
        result = result .+ phi[k]*alphastep*ranort(alphas[k], tN, tstep)
    end
    return result
end

function ranort(alpha, N)
    k=collect(0:N-1)
    rc = ((-1)*ones(size(k))).^k.*gamma.(alpha+1).*(gamma.(alpha*0.5 .-k.+1).*gamma.(alpha*0.5 .+ k.+1)).^(-1)
    rc = rc*(cos(alpha*π*0.5))

    R = zeros(N, N)

    for m=1:N
        R[m, m:N] = rc[1:N-m+1]
    end

    for i=1:N-1
        for j=i:N
            R[j, i] = R[i, j]
        end
    end
    return R
end
# Multiple dispatch for ranort
function ranort(alpha, N, h)
    R = ranort(alpha, N)
    R = R*h^(-alpha)
    return R
end


"""
    RecombinedBSplineBasis{k, T}

Functional basis defined from the recombination of a [`BSplineBasis`](@ref)
in order to satisfy certain homogeneous boundary conditions (BCs).

# Extended help

The basis recombination technique is a common way of applying BCs in Galerkin
methods. It is described for instance in Boyd 2000 (ch. 6), in the context of
a Chebyshev basis. In this approach, the original basis, ``\\{b_i(x), 1 ≤ i ≤ N\\}``,
is "recombined" into a new basis, ``\\{ϕ_j(x), 1 ≤ j ≤ M\\}``, so that each
basis function ``ϕ_j`` individually satisfies the chosen BCs.

The length ``M`` of the recombined basis is always smaller than the length ``N``
of the original basis.
The difference, ``δ = N - M``, is equal to the number of boundary conditions.
In the simplest (and most common) case, a single BC is applied on each boundary,
leading to ``δ = 2``.
More generally, as described further below, it is possible to simultaneously
impose different BCs, which further decreases the number of degrees of freedom
(increasing ``δ``).

Thanks to the local support of B-splines, basis recombination involves only a
little portion of the original B-spline basis. For instance, since there is only
one B-spline that is non-zero at each boundary, removing that function from the
basis is enough to apply homogeneous Dirichlet BCs. Imposing BCs for derivatives
is slightly more complex, but still possible.

Note that, when combining basis recombination with [collocation methods](@ref
collocation-api), there must be **no** collocation points at the boundaries,
or the resulting collocation matrices may not be invertible.

## Order of the boundary condition

In this section, we consider the simplest case where a single homogeneous
boundary condition, ``\\mathrm{d}^n u / \\mathrm{d}x^n = 0``, is to be satisfied
by the basis.

The recombined basis requires the specification of a `Derivative` object
determining the order of the homogeneous BCs to be applied at the two
boundaries.
Linear combinations of `Derivative`s are also supported.
The order of the B-spline needs to be ``k ≥ n + 1``, since a B-spline
of order ``k`` is a ``C^{k - 1}``-continuous function (except on the knots
where it is ``C^{k - 1 - p}``, with ``p`` the knot multiplicity).

Some usual choices are:

- `Derivative(0)` sets homogeneous [Dirichlet
  BCs](https://en.wikipedia.org/wiki/Dirichlet_boundary_condition) (``u = 0`` at
  the boundaries) by removing the first and last B-splines, i.e. ``ϕ_1 = b_2``;

- `Derivative(1)` sets homogeneous [Neumann
  BCs](https://en.wikipedia.org/wiki/Neumann_boundary_condition) (``u' = 0`` at
  the boundaries) by adding the two first (and two last) B-splines, i.e. ``ϕ_1 =
  b_1 + b_2``.

- more generally, `α Derivative(0) + β Derivative(1)` sets homogeneous [Robin
  BCs](https://en.wikipedia.org/wiki/Robin_boundary_condition) by defining
  ``ϕ_1`` as a linear combination of ``b_1`` and ``b_2``.
  Here it's important to note that `Derivative(1)` denotes the [normal
  derivative](https://en.wikipedia.org/wiki/Directional_derivative#Normal_derivative)
  at the boundary, ``\\frac{∂u}{∂n}``, which is equal to ``-\\frac{∂u}{∂x}`` on
  the left boundary.

Higher order BCs are also possible.
For instance, `Derivative(2)` recombines the first three B-splines into two
basis functions that satisfy ``ϕ_1'' = ϕ_2'' = 0`` at the left boundary, while
ensuring that lower and higher-order derivatives keep degrees of freedom at the
boundary.
Note that simply adding the first three B-splines, as in ``ϕ_1 = b_1 + b_2 +
b_3``, makes the first derivative vanish as well as the second one, which is
unwanted.

For `Derivative(2)`, the chosen solution is to set ``ϕ_i = α_i b_i + β_i b_{i +
1}`` for ``i ∈ \\{1, 2\\}``.
The ``α_i`` and ``β_i`` coefficients are chosen such that ``ϕ_i'' = 0`` at the
boundary.
Moreover, they satisfy the (somewhat arbitrary) constraint ``α_i + β_i = 2`` for
each ``i``, for consistency with the Neumann case described above.
This generalises to higher order BCs.
Note that, since each boundary function ``ϕ_i`` is defined from only two
neighbouring B-splines, its local support stays minimal, hence preserving the
small bandwidth of the resulting matrices.

Finally, note that in the current implementation, it is not possible to impose
different boundary conditions on both boundaries.

## Multiple boundary conditions

As an option, the recombined basis may simultaneously satisfy homogeneous BCs of
different orders. In this case, a tuple of `Derivative`s must be passed.

For more details on the supported combinations of BCs, see the different
`RecombinedBSplineBasis` constructors documented further below.

---

    RecombinedBSplineBasis(op::AbstractDifferentialOp, B::BSplineBasis)

Construct `RecombinedBSplineBasis` from B-spline basis `B`, satisfying
homogeneous boundary conditions associated to the given differential operator.

For instance, `op = Derivative(0)` and `op = Derivative(1)` correspond to
homogeneous Dirichlet and Neumann BCs, respectively.

Linear combinations of differential operators are also supported.
For instance, `op = Derivative(0) + λ Derivative(1)` corresponds to homogeneous
Robin BCs.

Higher-order derivatives are also allowed, being only limited by the order of
the B-spline basis.

## Examples

```jldoctest RecombinedBSplineBasis
julia> B = BSplineBasis(BSplineOrder(4), -1:0.2:1)
13-element BSplineBasis of order 4, domain [-1.0, 1.0]
 knots: [-1.0, -1.0, -1.0, -1.0, -0.8, -0.6, -0.4, -0.2, 0.0, 0.2, 0.4, 0.6, 0.8, 1.0, 1.0, 1.0, 1.0]

julia> R_neumann = RecombinedBSplineBasis(Derivative(1), B)
11-element RecombinedBSplineBasis of order 4, domain [-1.0, 1.0]
 knots: [-1.0, -1.0, -1.0, -1.0, -0.8, -0.6, -0.4, -0.2, 0.0, 0.2, 0.4, 0.6, 0.8, 1.0, 1.0, 1.0, 1.0]
 BCs left:  (D{1},)
 BCs right: (D{1},)

julia> R_robin = RecombinedBSplineBasis(Derivative(0) + 3Derivative(1), B)
11-element RecombinedBSplineBasis of order 4, domain [-1.0, 1.0]
 knots: [-1.0, -1.0, -1.0, -1.0, -0.8, -0.6, -0.4, -0.2, 0.0, 0.2, 0.4, 0.6, 0.8, 1.0, 1.0, 1.0, 1.0]
 BCs left:  (D{0} + 3 * D{1},)
 BCs right: (D{0} + 3 * D{1},)
```

---

    RecombinedBSplineBasis(ops::Tuple{Vararg{AbstractDifferentialOp}},
                           B::BSplineBasis)

Construct `RecombinedBSplineBasis` simultaneously satisfying homogeneous BCs
associated to multiple differential operators.

Currently, the following specific combinations of differential operators are
supported:

1. all derivatives up to order `m`:

        ops = (Derivative(0), ..., Derivative(m))

   This boundary condition is obtained by removing the first `m + 1` B-splines
   from the original basis.

   For instance, if `(Derivative(0), Derivative(1))` is passed, then the basis
   simultaneously satisfies homogeneous Dirichlet and Neumann BCs at the two
   boundaries.
   The resulting basis is ``ϕ_1 = b_3, ϕ_2 = b_4, …, ϕ_{N - 4} = b_{N - 2}``.

2. an extension of the above, with an additional differential operator of order
   `n` at the end:

        ops = (Derivative(0), ..., Derivative(m), D(n))

   The operator `D(n)` may be a `Derivative`, or a linear combination of
   derivatives.
   The only restriction is that its maximum degree must satisfy `n ≥ m + 1`.

   One example is the combination of homogeneous Dirichlet BCs, ``u = 0`` on the
   boundaries, with Robin BCs for the derivative, ``u' + λ u'' = 0``, which
   corresponds to `ops = (Derivative(0), Derivative(1) + λ Derivative(2))`.

In all cases, the degrees of the differential operators must be in increasing
order. For instance, `ops = (Derivative(1), Derivative(0))` fails with an error.

## Examples

```jldoctest RecombinedBSplineBasis
julia> ops = (Derivative(0), Derivative(1));

julia> R1 = RecombinedBSplineBasis(ops, B)
9-element RecombinedBSplineBasis of order 4, domain [-1.0, 1.0]
 knots: [-1.0, -1.0, -1.0, -1.0, -0.8, -0.6, -0.4, -0.2, 0.0, 0.2, 0.4, 0.6, 0.8, 1.0, 1.0, 1.0, 1.0]
 BCs left:  (D{0}, D{1})
 BCs right: (D{0}, D{1})

julia> ops = (Derivative(0), Derivative(1) - 4Derivative(2));

julia> R2 = RecombinedBSplineBasis(ops, B)
9-element RecombinedBSplineBasis of order 4, domain [-1.0, 1.0]
 knots: [-1.0, -1.0, -1.0, -1.0, -0.8, -0.6, -0.4, -0.2, 0.0, 0.2, 0.4, 0.6, 0.8, 1.0, 1.0, 1.0, 1.0]
 BCs left:  (D{0}, D{1} + -4 * D{2})
 BCs right: (D{0}, D{1} + -4 * D{2})
```
"""
struct RecombinedBSplineBasis{
            k, T, Parent <: BSplineBasis{k,T},
            DiffOps <: Tuple{Vararg{AbstractDifferentialOp}},
            RMatrix <: RecombineMatrix{Q,DiffOps} where Q,
        } <: AbstractBSplineBasis{k,T}
    B :: Parent   # original B-spline basis
    ops :: DiffOps  # list of differential operators for BCs
    M :: RMatrix  # basis recombination matrix

    function RecombinedBSplineBasis(ops::Tuple{Vararg{AbstractDifferentialOp}},
                                    B::BSplineBasis{k,T}) where {k,T}
        Parent = typeof(B)
        M = RecombineMatrix(ops, B)
        RMatrix = typeof(M)
        Ops = typeof(ops)
        new{k,T,Parent,Ops,RMatrix}(B, ops, M)
    end

    RecombinedBSplineBasis(op::AbstractDifferentialOp, args...) =
        RecombinedBSplineBasis((op, ), args...)
end

Base.:(==)(A::RecombinedBSplineBasis, B::RecombinedBSplineBasis) =
    A === B ||
    constraints(A) == constraints(B) &&
    parent(A) == parent(B)

function Base.summary(io::IO, R::RecombinedBSplineBasis)
    summary_basis(io, R)
    cl, cr = constraints(R)
    print(io, ", BCs {left => ", cl, ", right => ", cr, "}")
    nothing
end

function Base.show(io::IO, R::RecombinedBSplineBasis)
    summary_basis(io, R)
    cl, cr = constraints(R)
    let io = IOContext(io, :compact => true, :limit => true)
        print(io, "\n knots: ", knots(R))
        print(io, "\n BCs left:  ", cl)
        print(io, "\n BCs right: ", cr)
    end
end

"""
    RecombinedBSplineBasis(ops, args...; kwargs...)

Construct [`RecombinedBSplineBasis`](@ref) from B-spline basis, satisfying
homogeneous boundary conditions associated one or more differential operators.

This variant does not require a previously constructed [`BSplineBasis`](@ref).
Arguments are passed to the `BSplineBasis` constructor.
"""
RecombinedBSplineBasis(order, args...; kwargs...) =
    RecombinedBSplineBasis(order, BSplineBasis(args...; kwargs...))

"""
    parent(R::RecombinedBSplineBasis)

Get original B-spline basis.
"""
Base.parent(R::RecombinedBSplineBasis) = R.B

"""
    recombination_matrix(R::AbstractBSplineBasis)

Get [`RecombineMatrix`](@ref) associated to the recombined basis.

For non-recombined bases such as [`BSplineBasis`](@ref), this returns the
identity matrix (`LinearAlgebra.I`).
"""
recombination_matrix(R::RecombinedBSplineBasis) = R.M
recombination_matrix(B::AbstractBSplineBasis) = LinearAlgebra.I

"""
    length(R::RecombinedBSplineBasis)

Returns the number of functions in the recombined basis.
"""
@inline Base.length(R::RecombinedBSplineBasis) =
    length(parent(R)) - sum(num_constraints(R))

boundaries(R::RecombinedBSplineBasis) = boundaries(parent(R))

knots(R::RecombinedBSplineBasis) = knots(parent(R))
order(::Type{<:RecombinedBSplineBasis{k}}) where {k} = k
Base.eltype(::Type{RecombinedBSplineBasis{k,T}}) where {k,T} = T

"""
    constraints(R::AbstractBSplineBasis) -> (left, right)
    constraints(A::RecombineMatrix) -> (left, right)

Return the constraints (homogeneous boundary conditions) that the basis
satisfies on each boundary.

Constraints are returned as a tuple `(left, right)` indicating the BCs that are
satisfied on each boundary. Each element is a tuple of differential operators
specifying the BCs.

For example, if both Dirichlet and Neumann BCs are satisfied on the left
boundary, then `left = (Derivative(0), Derivative(1))`.

For non-recombined bases such as [`BSplineBasis`](@ref), this returns a tuple of
empty tuples: `((), ())`, since no BCs are satisfied on either boundary.
"""
constraints(R::AbstractBSplineBasis) = constraints(recombination_matrix(R))

"""
    num_constraints(R::AbstractBSplineBasis) -> (Int, Int)
    num_constraints(A::RecombineMatrix) -> (Int, Int)

Returns the number of constraints (number of BCs to satisfy) on each boundary.

For instance, if `R` simultaneously satisfies Dirichlet and Neumann boundary
conditions on each boundary, this returns `(2, 2)`.

Note that for non-recombined bases such as [`BSplineBasis`](@ref), the number of
constraints is zero, and this returns `(0, 0)`.
"""
num_constraints(B::AbstractBSplineBasis) = num_constraints(recombination_matrix(B))

"""
    num_recombined(R::AbstractBSplineBasis) -> (Int, Int)
    num_recombined(A::RecombineMatrix) -> (Int, Int)

Returns the number of recombined functions in the recombined basis for each
boundary.

For instance, if `R` satisfies Neumann boundary conditions on both boundaries,
then only the first and last basis functions are different from the original
B-spline basis, e.g. ``ϕ_1 = b_1 + b_2``, and this returns `(1, 1)`.

For non-recombined bases such as [`BSplineBasis`](@ref), this returns zero.
"""
num_recombined(R::AbstractBSplineBasis) = num_recombined(recombination_matrix(R))

function nonzero_in_segment(R::RecombinedBSplineBasis, n)
    # Same as the original B-spline basis on the interval n - δl.
    δl = num_constraints(R)[1]
    nonzero_in_segment(parent(R), n - δl; N = length(R))
end

function support(R::RecombinedBSplineBasis, j)
    δl = num_constraints(R)[1]
    support(parent(R), j + δl)
end

@propagate_inbounds function evaluate(R::RecombinedBSplineBasis, j, args...)
    B = parent(R)
    A = recombination_matrix(R)
    is = nzrows(A, j)
    @assert !isempty(is)

    i1 = is[1]
    ϕ = evaluate(B, i1, args...)
    T = typeof(ϕ)

    if length(is) == 1  # avoid accessing A[i1, j] (we know it's 1)
        # @assert A[i1, j] == 1
        return ϕ
    end

    ϕ::T *= A[i1, j]

    for i ∈ is[2:end]
        ϕ::T += A[i, j] * evaluate(B, i, args...)
    end

    ϕ::T
end


using Pkg
using ABCDeGraphGenerator
using Random

@info "Usage: julia abcd_sampler.jl config_filename"
@info "For the syntax of config_filename see example_config.toml file"

filename = ARGS[1]
conf = Pkg.TOML.parsefile(filename)
isempty(conf["seed"]) || Random.seed!(parse(Int, conf["seed"]))

μ = haskey(conf, "mu") ? parse(Float64, conf["mu"]) : nothing
ξ = haskey(conf, "xi") ? parse(Float64, conf["xi"]) : nothing
if !(isnothing(μ) || isnothing(ξ))
    throw(ArgumentError("inconsistent data: only μ or ξ may be provided"))
end

n = parse(Int, conf["n"])

τ₁ = parse(Float64, conf["t1"])
d_min = parse(Int, conf["d_min"])
d_max = parse(Int, conf["d_max"])
d_max_iter = parse(Int, conf["d_max_iter"])
@info "Expected value of degree: $(ABCDeGraphGenerator.get_ev(τ₁, d_min, d_max))"
degs = ABCDeGraphGenerator.sample_degrees(τ₁, d_min, d_max, n, d_max_iter)
open(io -> foreach(d -> println(io, d), degs), conf["degreefile"], "w")

τ₂ = parse(Float64, conf["t2"])
c_min = parse(Int, conf["c_min"])
c_max = parse(Int, conf["c_max"])
c_max_iter = parse(Int, conf["c_max_iter"])
@info "Expected value of community size: $(ABCDeGraphGenerator.get_ev(τ₂, c_min, c_max))"
coms = ABCDeGraphGenerator.sample_communities(τ₂, c_min, c_max, n, c_max_iter)
open(io -> foreach(d -> println(io, d), coms), conf["communitysizesfile"], "w")

isCL = parse(Bool, conf["isCL"])
islocal = haskey(conf, "islocal") ? parse(Bool, conf["islocal"]) : false

p = ABCDeGraphGenerator.ABCDParams(degs, coms, μ, ξ, isCL, islocal)
edges, clusters = ABCDeGraphGenerator.gen_graph(p)
# uncomment to assert validity of configuration model output
# if !isCL
#     using StatsBase
#     @info "assertion #1: length of edges is $(length(edges)), sum of degrees is $(sum(degs))"
#     @assert length(edges) == sum(degs)/2
#     @info "assertion #2: counting degrees"
#     degcounter = zeros(Int32, length(degs))
#     for e in edges
#         addcounts!(degcounter, [e...], 1:length(degs))
#     end
#     @assert degcounter == degs
# end
open(conf["networkfile"], "w") do io
    for (a, b) in sort!(collect(edges))
        println(io, a, "\t", b)
    end
end
open(conf["communityfile"], "w") do io
    for (i, c) in enumerate(clusters)
        println(io, i, "\t", c)
    end
end


using OrdinaryDiffEq, Test, DiffEqDevTools, DiffEqOperators, Random

@testset "Out-of-place" begin
μ = 1.01
linnonlin_f2 = (u,p,t) -> μ * u
linnonlin_f1 = DiffEqScalar(μ)
linnonlin_fun = SplitFunction(linnonlin_f1, linnonlin_f2; analytic=(u0,p,t)->u0.*exp.(2μ*t))
prob = SplitODEProblem(linnonlin_fun,1/2,(0.0,1.0))

println("Out-of-place")
Random.seed!(100)
dts = 1 ./2 .^(7:-1:4) #14->7 good plot
sim  = test_convergence(dts,prob,GenericIIF1())
@test abs(sim.𝒪est[:l2]-1) < 0.2
sim  = test_convergence(dts,prob,GenericIIF2())
@test abs(sim.𝒪est[:l2]-2) < 0.2
sim  = test_convergence(dts,prob,LawsonEuler())
@test abs(sim.𝒪est[:l2]-1) < 0.2
sim  = test_convergence(dts,prob,NorsettEuler())
@test abs(sim.𝒪est[:l2]-1) < 0.2
sim  = test_convergence(dts,prob,ETDRK2())
@test abs(sim.𝒪est[:l2]-2) < 0.2
sim  = test_convergence(dts,prob,ETDRK3())
@test abs(sim.𝒪est[:l2]-3) < 0.2
sim  = test_convergence(dts,prob,ETDRK4(),dense_errors=true)
@test abs(sim.𝒪est[:l2]-4) < 0.2
sim  = test_convergence(dts,prob,HochOst4())
@test abs(sim.𝒪est[:l2]-4) < 0.2
sim  = test_convergence(dts,prob,Exprb32())
@test_broken abs(sim.𝒪est[:l2]-3) < 0.2 # order = 1?
sim  = test_convergence(dts,prob,Exprb43())
@test_broken abs(sim.𝒪est[:l2]-4) < 0.2 # order = 2?
sim  = test_convergence(dts,prob,ETD2())
@test abs(sim.𝒪est[:l2]-2) < 0.2
end

@testset "Inplace" begin
println("Inplace")
μ = 1.01
u0 = rand(2)
A = [2.0 -1.0; -1.0 2.0]
linnonlin_f1 = DiffEqArrayOperator(A)
linnonlin_f2 = (du,u,p,t) -> du .= μ .* u
linnonlin_fun_iip = SplitFunction(linnonlin_f1,linnonlin_f2;analytic=(u0,p,t)->exp((A+μ*I)*t)*u0)
prob = SplitODEProblem(linnonlin_fun_iip,u0,(0.0,1.0))

dts = 1 ./2 .^(8:-1:4) #14->7 good plot
sim  = test_convergence(dts,prob,GenericIIF1())
@test abs(sim.𝒪est[:l2]-1) < 0.2

sim  = test_convergence(dts,prob,GenericIIF2())
@test abs(sim.𝒪est[:l2]-2) < 0.1

sim  = test_convergence(dts,prob,LawsonEuler())
@test abs(sim.𝒪est[:l2]-1) < 0.1

sim  = test_convergence(dts,prob,NorsettEuler())
@test abs(sim.𝒪est[:l2]-1) < 0.1

sim  = test_convergence(dts,prob,ETDRK2())
@test abs(sim.𝒪est[:l2]-2) < 0.1

sim  = test_convergence(dts,prob,ETDRK3())
@test abs(sim.𝒪est[:l2]-3) < 0.1

sim  = test_convergence(dts,prob,ETDRK4(),dense_errors=true)
@test abs(sim.𝒪est[:l2]-4) < 0.1
@test abs(sim.𝒪est[:L2]-4) < 0.1

sim  = test_convergence(dts,prob,HochOst4())
@test abs(sim.𝒪est[:l2]-4) < 0.1

sim  = test_convergence(dts,prob,Exprb32())
@test_broken abs(sim.𝒪est[:l2]-3) < 0.1 # order = 1?

sim  = test_convergence(dts,prob,Exprb43())
@test_broken abs(sim.𝒪est[:l2]-4) < 0.1 # order = 2?

sim  = test_convergence(dts,prob,ETD2())
@test abs(sim.𝒪est[:l2]-2) < 0.1
end


include("berryphase.jl")

using BerryPhase

const S2 = (length(ARGS) >= 1 ? parse(Int, ARGS[1]) : 1)
const M = (length(ARGS) >= 2 ? parse(Int, ARGS[2]) : 10)

bp = berryphase(dimer(S2,1.0), M)

println("γ/π = ", bp/pi)


# Background Estiamtion

function SigmaClipping(img,Wpsf::Int=10,n::Int=2)
    # Image
    # Estimated psf size for smoothing window
    # Number of interations for clipping

    Im = img
    B = []
    #display(heatmap(Im, title="Raw Image"))

    # Noise Estimation
    σn = sqrt(var(img))

    # Smoothing Filters
    Wl = ones(20*Wpsf,20*Wpsf)
    Ws = ones(10*Wpsf,10*Wpsf);

    for i in 1:n
        # Smooth Image - background estimation
        if isodd(i)
            B = imfilter(Im,reflect(centered(Wl)),"replicate")./sum(Wl[:])
        else
            B = imfilter(Im,reflect(centered(Ws)),"replicate")./sum(Ws[:])
        end
        #display(heatmap(B.+(2*σn)))

        # Image Clipping
        Im = min.(Im,B.+(2*σn))
        #display(heatmap(Im, title=string("Sigma Clipping - Iteration",i)))
    end

    B = imfilter(Im,reflect(centered(Wl)),"replicate")./sum(Wl[:])
    #display(heatmap(B, title = "Background Estimation Image"))

    # Background Estimation Error
    E = zeros(size(img))
    kernal_Wpsf = ones(Wpsf,Wpsf)
    Bigkernal_Wpsf = ones(5*Wpsf,5*Wpsf)
    localmean = imfilter((img-B),reflect(centered(kernal_Wpsf)),"replicate")./sum(kernal_Wpsf[:])
    Biglocalmean = imfilter((img-B),reflect(centered(Bigkernal_Wpsf)),"replicate")./sum(Bigkernal_Wpsf[:])
    for pix in cat(findall(x-> x>σn,localmean), findall(x-> x>10*σn,Biglocalmean), dims=1)
        E[pix] += 1
        E[pix] = min(E[pix],1)
    end

    # Noise Image
    N = 1 .- E

    # Local Mean Residuals
    NI = imfilter((N.*(img-B)),reflect(centered(kernal_Wpsf)),"replicate")
    Nf = imfilter((N),reflect(centered(kernal_Wpsf)),"replicate")
    μr = NI./(Nf.+1e-9)

    Ib = img-B
    σb = sqrt(var(μr))

    return σb, B, Ib
end

export SigmaClipping


# plots kernel density summary of data, with 90% acceptance region, mean and median.
using KernelDensity, Plots

function npdensity(z)
    #pyplot()
    n = size(z,2)
    p = zeros(n)
    for i = 1:n
        x = z[:,i]
        y = kde(x)
        q05 = quantile(x,0.05)
        Plots.plot(range(minimum(x), stop=q05, length=100),z->pdf(y,z), color=:orange, fill=(0,0.5,:orange), label="q05")
        q95 = quantile(x,0.95)
        Plots.plot!(range(q05, stop=q95, length=100),z->pdf(y,z), color=:green, fill=(0,0.5,:green),label="90% non-rejection region")
        Plots.plot!(range(q95, stop=maximum(x), length=100),z->pdf(y,z), color = :orange, fill=(0,0.5,:orange),label="q95")
        m = mean(x)
        Plots.plot!([m,m],[0,pdf(y,m)],color=:blue, label="mean")
        m = median(x)
        if i == 1
            p = Plots.plot!([m,m],[0,pdf(y,m)],color=:black, label="median")
        else
            p = [p, Plots.plot!([m,m],[0,pdf(y,m)],color=:black, label="median")]
        end
    end
    return p
end


struct D2Q13{
    T <: Real,
    Abscissaes <: AbstractMatrix{<:Integer},
    Weights <: AbstractVector{<:Real},
} <: Quadrature
    abscissae::Abscissaes
    weights::Weights
    speed_of_sound_squared::T
end
function D2Q13()
    # High Knudsen Number Thermal Flows with the D2Q13 Lattice Boltzmann Model
    # P. Lopez & Y. Bayazitoglu
    w_0 = 3 / 8
    w_1 = 1 / 12
    w_2 = 1 / 16
    w_3 = 1 / 96
    cs = 2.0

    return D2Q13(
        [   #A #B          #C          #D
            0 1 -1 0 0 1 -1 1 -1 2 -2 0 0
            0 0 0 1 -1 1 -1 -1 1 0 0 -2 2
        ],
        [w_0, w_1, w_1, w_1, w_1, w_2, w_2, w_2, w_2, w_3, w_3, w_3, w_3],
        cs,
    )
end

order(q::D2Q13) = 5
Base.show(io::IO, q::D2Q13) = show(io, "D2Q13")
Base.string(q::D2Q13) = "D2Q13"


"""
    struct DuffyQuadrature{D,T} <: Quadrature{D,T}
      coordinates::Vector{Point{D,T}}
      weights::Vector{T}
    end

Duffy quadrature for simplices in [0,1]^D
"""
struct DuffyQuadrature{D,T} <: Quadrature{D,T}
  coordinates::Vector{Point{D,T}}
  weights::Vector{T}
end

get_coordinates(q::DuffyQuadrature) = q.coordinates

get_weights(q::DuffyQuadrature) = q.weights

"""
    DuffyQuadrature{D}(degree::Integer) where D
"""
function DuffyQuadrature{D}(order::Integer) where D
  x,w = _duffy_quad_data(order,D)
  DuffyQuadrature(x,w)
end

function _duffy_quad_data(order::Integer,D::Int)

  beta = 0
  dim_to_quad_1d = [
    _gauss_jacobi_in_0_to_1(order,(D-1)-(d-1),beta) for d in 1:(D-1) ]

  quad_1d = _gauss_legendre_in_0_to_1(order)
  push!(dim_to_quad_1d,quad_1d)

  x_pos = 1
  w_pos = 2
  dim_to_xs_1d = [quad_1d[x_pos] for quad_1d in dim_to_quad_1d]
  dim_to_ws_1d = [quad_1d[w_pos] for quad_1d in dim_to_quad_1d]

  a = 0.5
  for d in (D-1):-1:1
    ws_1d = dim_to_ws_1d[d]
    ws_1d[:] *= a
    a *= 0.5
  end

  x,w = _tensor_product_duffy(dim_to_xs_1d,dim_to_ws_1d)

  (_duffy_map.(x),w)

end

# Duffy map from the n-cube in [0,1]^d to the n-simplex in [0,1]^d
function _duffy_map(q::Point{D,T}) where {D,T}
  m = zero(mutable(Point{D,T}))
  m[1] = q[1]
  a = one(T)
  for i in 2:D
    a *= (1-q[i-1])
    m[i] = a*q[i]
  end
  Point{D,T}(m)
end

_duffy_map(q::Point{1,T}) where T = q

function _gauss_jacobi_in_0_to_1(order,alpha,beta)
  n = _npoints_from_order(order)
  x,w = gaussjacobi(n,alpha,beta)
  _map_to(0,1,x,w)
end

function _gauss_legendre_in_0_to_1(order)
  n = _npoints_from_order(order)
  x,w = gausslegendre(n)
  _map_to(0,1,x,w)
end

# Transforms a 1-D quadrature from `[-1,1]` to `[a,b]`, with `a<b`.
function _map_to(a,b,points,weights)
  points_ab = 0.5*(b-a)*points .+ 0.5*(a+b)
  weights_ab = 0.5*(b-a)*weights
  (points_ab, weights_ab)
end

function _npoints_from_order(order)
  ceil(Int, (order + 1.0) / 2.0 )
end

function _tensor_product_duffy(
  dim_to_xs_1d::Vector{Vector{T}},
  dim_to_ws_1d::Vector{Vector{W}}) where {T,W}

  D = length(dim_to_ws_1d)
  @assert D == length(dim_to_xs_1d)
  dim_to_n = [length(ws_1d) for ws_1d in dim_to_ws_1d]
  n = prod(dim_to_n)
  xs = zeros(Point{D,T},n)
  ws = zeros(W,n)
  cis = CartesianIndices(tuple(dim_to_n...))
  m = zero(mutable(Point{D,T}))
  _tensor_product_duffy!(xs,ws,dim_to_xs_1d,dim_to_ws_1d,cis,m)
  (xs,ws)
end

function _tensor_product_duffy!(
  xs,ws,dim_to_xs_1d,dim_to_ws_1d,cis::CartesianIndices{D},m) where D
  k = 1
  for ci in cis
    w = 1.0
    for d in 1:D
      xs_1d = dim_to_xs_1d[d]
      ws_1d = dim_to_ws_1d[d]
      i = ci[d]
      xi = xs_1d[i]
      wi = ws_1d[i]
      w *= wi
      m[d] = xi
    end
    xs[k] = m
    ws[k] = w
    k += 1
  end
end


using IPPCore
import IPPDSP
using Base.Test

# write your own tests here

for T = IPPDSP.conv_types
    a = convert(Vector{eval(T)}, [ x = 1:10 ] )
    b = convert(Vector{eval(T)}, [ x = 11:21 ] )
    reference = conv( a, b )
    IPPResult = IPPDSP.conv( a, b )
    @test length( reference ) == length( IPPResult )
    for i = 1:length( reference )
        @test isapprox( reference[i], IPPResult[i] )
    end
end

for T = IPPDSP.xcorr_types
    a = convert(Vector{eval(T)}, [ x = 1:10 ] )
    b = convert(Vector{eval(T)}, [ x = 1:10 ] )
    reference = xcorr( a, b )
    IPPResult = IPPDSP.xcorr( a, b, lowlag = -9 )
    @test length( reference ) == length( IPPResult )
    for i = 1:length( reference )
        @test isapprox( reference[i], IPPResult[i] )
    end
end


"""
    logseriescdf(p::T) where T <: Real

Returns the vector of samples of the discrete cdf of logarithmic distribution
"""
function logseriescdf(p::T) where T <: Real
  cdfs = T.([0.])
  for i in 1:100000000
    @inbounds push!(cdfs, cdfs[i]-(p^i)/(i*log(1-p)))
    # one can change atol here
    if isapprox(cdfs[i],  1.)
      return cdfs
    end
  end
  cdfs
end

"""
    levyel(θ::Real, u1::Real, u2::Real)

u1 and u2 are supposed to be random numbers form [0,1]
An element from Levy stable distribution with parameters α = 1/θ, β = 1,
γ = (cos(pi/(2*θ)))^θ and δ = 0.
Return Float, given parameter ϴ of dostribution
"""
function levyel(θ::Real, u1::Real, u2::Real)
  ϕ = pi*u1-pi/2
  v = quantile.(Exponential(1.), u2)
  γ = (cos(pi/(2*θ)))^θ
  v = ((cos(pi/(2*θ)+(1/θ-1)*ϕ))/v)^(θ-1)
  γ*v*sin(1/θ*(pi/2+ϕ))*(cos(pi/(2*θ))*cos(ϕ))^(-θ)
end

"""
    tiltedlevygen(V0::Real, α::Real; rng::AbstractRNG)

Returns a Real sampled  from the expotencialy tilted Levy stable pdf
f(x; V0, α) = exp(-V0^α) g(x; α)/exp(-V0), where g(x; α) is a stable Levy pdf
with parameters α = 1/θ, β = 1, γ = (cos(pi/(2*θ)))^θ and δ = 0.
"""
function tiltedlevygen(V0::T, α::T; rng::AbstractRNG) where T <: Real
  x = levyel(α, rand(rng, T), rand(rng, T))
  u = rand(rng, T)
  while exp(-V0^α*x)/(1500*exp(-V0)) < u
    x = levyel(α, rand(rng, T), rand(rng, T))
    u = rand(rng, T)
  end
  return x.*V0.^α
end

"""
    Ginv(y::Real, α::Real)

Returns Real, helper for the joe/frank nested copula generator
"""
Ginv(y::Real, α::Real) = ((1-y)*SpecialFunctions.gamma(1-α))^(-1/α)

"""
    InvlaJ(n::Int, α::Real)

Returns Real from the inverse laplacea transform of generator of Joe nested copula
parametered by α
"""
InvlaJ(n::Int, α::T) where T <: Real = 1-1/(n*beta(T(n), 1-α))

"""
  sampleInvlaJ(α::Real, v::Real)

Returns Int, a sample of inverce laplacea transform of generator of Joe nested copula,
given a parameter α and a random numver v ∈ [0,1]
"""
function sampleInvlaJ(α::Real, v::Real)
  if v <= α
    return 1
  else
    G = Ginv(v, α)
    if G > 2^62
      return 2^62
    else
      return (InvlaJ(floor(Int, G), α) < v) ? ceil(Int, G) : floor(Int, G)
    end
  end
end

"""
  elInvlaF(θ₁::Real, θ₀::Real, logseriescdf::Vector{Real}; rng::AbstractRNG)

Returns Int, a single sample of the inverse laplacea transform of the generator
of nested Frank copula
"""
function elInvlaF(θ₁::T, θ₀::T, logseriescdf::Vector{T}; rng::AbstractRNG) where T <: Real
  c1 = 1-exp(-θ₁)
  α = θ₀/θ₁
  if θ₀ <= 1
    v = rand(rng, T)
    X = findlast(logseriescdf .< rand(rng, T))[1]
    while v > 1/((X-α)*beta(X, 1-α))
      v = rand(rng, T)
      X = findlast(logseriescdf .< rand(rng, T))[1]
    end
    return X
  else
    v = rand(rng, T)
    X = sampleInvlaJ(α, rand(rng, T))
    while v > c1^(X-1)
      X = sampleInvlaJ(α, rand(rng, T))
      v = rand(rng, T)
    end
    return X
  end
end

"""
  nestedfrankgen(θ₁::T, θ₀::T, V0::Int, logseriescdf::Vector{T}; rng::AbstractRNG) where T <: Real

Return int, sampled from the Inverse Laplace trensform of nested
Frank copula given parametes θ₁ θ₀ (child and parent)
and an element of V0 - vector of samples of invlaplace of the parents copula
"""
function nestedfrankgen(θ₁::T, θ₀::T, V0::Int, logseriescdf::Vector{T}; rng::AbstractRNG) where T <: Real
    return sum([elInvlaF(θ₁, θ₀, logseriescdf; rng = rng) for j in 1:V0])
end


#abstract type TR1DModel{T} end

abstract type CubicModel{T} <: TR1DModel{T} end

abstract type CubicModel2Pt{T} <: CubicModel{T} end

"Cubic model solution"
function solveModel(m:: CubicModel{T}, t :: T) where T
    disc = m.H^2 - T(2)*m.C*m.g
    d = disc < 0 ? -sign(m.C)*Inf : (-m.H + sqrt(disc)) / m.C
    return d
end

"Cubic model evaluation"
function m(M:: CubicModel{T}, t :: T) where T
    # Horner rule
    return M.f + t*(M.g + t*( T(0.5)*M.H + t*( T(1.0/6.0)*M.C)))
end

"Cubic model derivative evaluation"
function dm(M:: CubicModel{T}, t :: T) where T
    return M.g  +  t*(M.H  +  t*T(0.5)*M.C)
end

"Cubic model predicted reduction"
function Δm(M:: CubicModel{T}, t :: T) where T
    return M.g * t  +  T(0.5) * M.H * t^2  + T(1.0/6.0)*M.C*t^3
end

"Cubic model predicted reduction given two points"
function Δm(M:: CubicModel{T}, t1 :: T, t2 :: T) where T
    c1 = M.g * t1  +  T(0.5) * M.H * t1^2  + T(1.0/6.0)*M.C*t1^3
    c2 = M.g * t2  +  T(0.5) * M.H * t2^2  + T(1.0/6.0)*M.C*t2^3
    return c1 - c2
end

"Taylor cubic polynomial model"
mutable struct Taylor3{T} <: CubicModel{T}
    f :: T
    g :: T
    H :: T
    C :: T
end
Taylor3{T}() where T = Taylor3{T}(0,0,0,0)

"2 points interpolation cubic polynomial model"
mutable struct TwoPoints3{T} <: CubicModel2Pt{T}
    f :: T
    g :: T
    H :: T
    C :: T
end
TwoPoints3{T}() where T = TwoPoints3{T}(0,0,0,0)

function initModel(M:: Taylor3{T}, ϕ :: LineModel, t :: T, ϕt :: T, dϕt :: T) where T
    M.f = ϕt
    M.g = dϕt
    M.H = hess(ϕ, t)
    M.C = tens(ϕ,t)
    return M
end

function updateModel!(M:: Taylor3{T}, ϕ :: LineModel, t :: T, ϕt :: T, dϕt :: T) where T
    M.f = ϕt
    M.g  = dϕt
    M.H = hess(ϕ, t)
    M.C = tens(ϕ,t)
    return M
end

"Secant cubic model"
mutable struct secant3{T} <: CubicModel2Pt{T}
    f :: T
    g :: T
    H :: T
    C :: T
    Q :: T  # to test plain secant
end
secant3{T}() where T = secant3{T}(0,0,0,0,0)

function initModel(M:: CubicModel2Pt{T}, ϕ :: LineModel, t :: T, ϕt :: T, dϕt :: T) where T
    M.f = ϕt
    M.g = dϕt
    M.H, M.C = initHC_s!(ϕ, t, ϕt, dϕt)
    return M
end

function updateModel!(M:: CubicModel2Pt{T}, ϕ :: LineModel, t :: T, ϕt :: T, dϕt :: T) where T
    M.f = ϕt
    M.g = dϕt
    M.H, M.C = updateHC_s!(ϕ, t, ϕt, dϕt)
    return M
end

function initModel(M:: secant3{T}, ϕ :: LineModel, t :: T, ϕt :: T, dϕt :: T) where T
    M.f = ϕt
    M.g = dϕt
    M.H, M.C, M.Q = initHC_s!(ϕ, t, ϕt, dϕt)
    return M
end

function updateModel!(M:: secant3{T}, ϕ :: LineModel, t :: T, ϕt :: T, dϕt :: T) where T
    M.f = ϕt
    M.g = dϕt
    M.H, M.C, M.Q = updateHC_s!(ϕ, t, ϕt, dϕt)
    return M
end

let (tpred, ϕpred, dϕpred) = (NaN, NaN, NaN)

    global function initHC_s!(ϕ :: LineModel, t :: T, ϕt :: T, dϕt :: T) where T
        (tpred, ϕpred, dϕpred) = (t, ϕt, dϕt)
        return T(1), T(0), T(1)
    end

    global function updateHC_s!(ϕ :: LineModel, t :: T, ϕt :: T, dϕt :: T) where T
        # estimate quadratic and cubic coefficients from the hermite interpolant
        a = coefHIN3([t;tpred], [ϕt dϕt; ϕpred dϕpred])
        s = t - tpred
        y = dϕt - dϕpred
        Q = y/s

        tpred, ϕpred, dϕpred = t, ϕt, dϕt

        return T(2.0)*a[3], T(6.0)*a[4], Q
    end

    "Two consecutive secant steps on the cubic model"
    global function solveModel(M:: secant3{T}, t :: T) where T
        # the "Taylor" cubic polynomial fits with the function and its derivative at t and tpred
        # Thus, secant iteration may be performed on the cubic model without knowledge of ϕ.
        # first secant iteration
        #s = t - tpred
        dmt = M.g  #  dm(M, t)
        #dmpred = dm(M, tpred)
        #y = dmt - dmpred
        dS = - (dmt / M.Q)

        # second secant iteration
        t2 = t + dS
        dm2 = dm(M, t2)
        y2 = dm2 - dmt
        d2 = - (dm2 * dS / y2)
        dS2 = dS + d2

        #return dS # for testing, should be equivalent to secant
        #return dS2 # for testing, should be equivalent to secant

        # try the cubic's root
        disc = M.H^2 - 2.0*M.C*M.g
        dC = disc < 0 ? -sign(M.C)*Inf : (-M.H + sqrt(disc)) / M.C
        return dC
    end
end


# DESCRIPTION
# Computes the linearized dynamics of a nonlinear system (acfun), given the
# trim state and control vectors (x0 and u0).
#
# INPUT
# - acfun: name of system dynamics function
# - x0: trim state vector
# - u0: trim control vector
# - xdot_in: state derivative
# - t: time
#
# OUTPUT
# - F, G, M, A, B: coefficient matrices of linearized dynamics
#
# ------------------------------------------------------------------------------

function linearize(acfun,x0,u0,xdot_in,t)
    # init state-space matrices
    F=zeros(NSTATES,NSTATES)
    G=zeros(NSTATES,NCTRLS)
    M=Matrix{Float64}(I, NSTATES, NSTATES)
    A=zeros(NSTATES,NSTATES)
    B=zeros(NSTATES,NCTRLS)
    # F matrix
    x_p=zeros(NSTATES)
    xd_p=zeros(NSTATES)
    u_p=zeros(NCTRLS)
    for k=1:NSTATES
        #x_p=x0
        x_p[:]=x0[:]
        x_p[k]=x_p[k]+DELXLIN[k]
        xdot_p1=acfun(xdot_in,x_p,u0,t)
        #xdot_p1=H60!(xdot_in,x_p,u0,t)
        x_p[k]=x_p[k]-2*DELXLIN[k]
        #xdot_p2=eval(acfun,x_p,u0,xdot_in)
        xdot_p2=acfun(xdot_in,x_p,u0,t)
        F[:,k]=(xdot_p1-xdot_p2)/(2*DELXLIN[k])
    end
    # M matrix
    xd_p=zeros(NSTATES)
    for k=1:NSTATES
        #xd_p=xdot_in
        xd_p[:]=xdot_in[:]
        xd_p[k]=xd_p[k]+DELXLIN[k]
        xdot_p1=acfun(xd_p,x0,u0,t)
        xd_p[k]=xd_p[k]-2*DELXLIN[k]
        xdot_p2=acfun(xd_p,x0,u0,t)
        M[:,k]=M[:,k]-(xdot_p1-xdot_p2)/(2*DELXLIN[k])
    end
    # G matrix
    u_p=zeros(NCTRLS)
    for k=1:NCTRLS
        #u_p=u0
        u_p[:]=u0[:]
        u_p[k]=u_p[k]+DELCLIN[k]
        xdot_p1=acfun(xdot_in,x0,u_p,t)
        u_p[k]=u_p[k]-2*DELCLIN[k]
        xdot_p2=acfun(xdot_in,x0,u_p,t)
        G[:,k]=(xdot_p1-xdot_p2)/(2*DELCLIN[k])
    end
    A=M\F
    B=M\G
    return F, G, M, A, B
end


# Quasi-Newton.

function optimize(f::Function, g::Function, H::Function, x0::Vector{T},
                  approxh::Bool; kmax::Int64 = 5000, δ::Float64 = 1e-6) where {T<:Real}
    k::Int64 = 0
    δ *= δ
    x = x0
    n::Int64 = length(x)
    dfx = ones(n)
    d2fx = eye(n, n)
    while norm(dfx) > δ && k < kmax
        prev = x
        dfx = g(x)
        x -= d2fx * dfx
        y = g(x) - dfx
        s = x - prev
        d2fx = H(d2fx, y, s)
        k += 1
    end
    return x, k
end


using FiniteVolumeMethod
using Test

@testset "FiniteVolumeMethod.jl" begin

    init_steps = 3
    ref_steps = 2

    for dim=1:3
        uniform = TreeGrid(zeros(dim))
        vec = 1 : dim
        vec2 = dim : -1 : 1

        # A uniform grid
        for step=1:init_steps
            FiniteVolumeMethod.refine!(uniform, [uniform.tree], recurse=true)
        end

        # Locally refined grid
        refined = TreeGrid(zeros(dim))
        child = refined.tree
        for step=1:init_steps
            FiniteVolumeMethod.refine!(refined, [child])
            child = child.children[1+mod(step-1, 1<<dim)]
        end

        grids = [uniform, refined]
        expected_orders = [2, 1]

        div_lInf_constant = zeros(ref_steps)
        div_lInf_linear = zeros(ref_steps)
        div_lInf_nonlinear = zeros(ref_steps)
        grad_lInf_constant = zeros(ref_steps)
        grad_lInf_linear = zeros(ref_steps)
        grad_lInf_nonlinear = zeros(ref_steps)
        for (i, grid) ∈ enumerate(grids)
            for step=1:ref_steps
                FiniteVolumeMethod.refine!(grid, [grid.tree], recurse=true)

                # Gradient:
                p = CellVar(x -> pi, grid)
                g = gradient(p, grid)
                grad_lInf_constant[step] = maximum(abs.(g.data))

                p = CellVar(x -> pi + sum(vec .* x), grid)
                gExact = FaceVar(x -> vec, grid, filter = active)
                g = gradient(p, grid)
                grad_lInf_linear[step] = maximum(abs.(g.data - gExact.data))

                p = CellVar(x -> pi + sum(vec .* cos.(x)), grid)
                gExact = FaceVar(x -> -vec .* sin.(x), grid, filter = active)
                g = gradient(p, grid)
                grad_lInf_nonlinear[step] = maximum(abs.(g.data - gExact.data))

                # divergence:
                u = FaceVar(x -> vec, grid)
                d = divergence(u, grid)
                div_lInf_constant[step] = maximum(abs.(d.data))

                u = FaceVar(x -> vec .* x + vec2, grid)
                dExact = CellVar(x -> sum(vec), grid, filter = active)
                d = divergence(u, grid)
                div_lInf_linear[step] = maximum(abs.(d.data - dExact.data))

                u = FaceVar(x -> sum(x.^2) .+ vec .* x + cos.(x), grid)
                dExact = CellVar(x -> 2sum(x) + sum(vec) - sum(sin.(x)), grid, filter = active)
                d = divergence(u, grid)
                div_lInf_nonlinear[step] = maximum(abs.(d.data - dExact.data))
            end

            @test all(grad_lInf_constant .== 0.0)
            @test all(div_lInf_constant .== 0.0)

            # Approximate derivative of a linear function should be exact
            @test all(grad_lInf_linear .== 0.0)
            @test all(div_lInf_linear .== 0.0)

            # Approximate derivative of a nonlinear function should be second order accurate (uniform)
            # and first order accurate at refinement interfaces
            grad_order = log2.(grad_lInf_nonlinear[1:ref_steps-1] ./ grad_lInf_nonlinear[2:ref_steps])
            div_order = log2.(div_lInf_nonlinear[1:ref_steps-1] ./ div_lInf_nonlinear[2:ref_steps])
            @test all(grad_order .>= expected_orders[i] - 1E-1)
            @test all(div_order .>= expected_orders[i] - 1E-1)
        end
    end
end


using GeometricIntegrators.Equations

ϑ₁(t, q, v) = g₁₁(t,q) * v[1] + A₁(t, q)
ϑ₂(t, q, v) = g₂₂(t,q) * v[2] + A₂(t, q)
ϑ₃(t, q, v) = g₃₃(t,q) * v[3] + A₃(t, q)

function ϑ(t, q, v, θ)
    θ[1] = ϑ₁(t,q,v)
    θ[2] = ϑ₂(t,q,v)
    θ[3] = ϑ₃(t,q,v)
    nothing
end

v¹(t, q, p) = g¹¹(t, q) * (p[1] - A₁(t, q))
v²(t, q, p) = g²²(t, q) * (p[2] - A₂(t, q))
v³(t, q, p) = g³³(t, q) * (p[3] - A₃(t, q))

hamiltonian(t,q,p) = (g₁₁(t,q) * v¹(t,q,p)^2 + g₂₂(t,q) * v²(t,q,p)^2 + g₃₃(t,q) * v³(t,q,p)^2) / 2 + φ(t,q)
toroidal_momentum(t,q,p) = p[3]

function charged_particle_3d_pᵢ(qᵢ, vᵢ, tᵢ=0)
    pᵢ = zero(qᵢ)

    if ndims(qᵢ) == 1
        ϑ(tᵢ, qᵢ, vᵢ, pᵢ)
    else
        for i in 1:size(qᵢ,2)
            @views ϑ(tᵢ, qᵢ[:,i], vᵢ[:,i], pᵢ[:,i])
        end
    end
    pᵢ
end

function charged_particle_3d_pode_v(t, q, p, v)
    v[1] = v¹(t,q,p)
    v[2] = v²(t,q,p)
    v[3] = v³(t,q,p)
    nothing
end

function charged_particle_3d_pode_f(t, q, p, f)
    f[1] = dA₁dx₁(t,q) * v¹(t,q,p) + dA₂dx₁(t,q) * v²(t,q,p) + dA₃dx₁(t,q) * v³(t,q,p) + E₁(t,q) +
           (dg₁₁dx₁(t,q) * v¹(t,q,p)^2 + dg₂₂dx₁(t,q) * v²(t,q,p)^2 + dg₃₃dx₁(t,q) * v³(t,q,p)^2) / 2
    f[2] = dA₁dx₂(t,q) * v¹(t,q,p) + dA₂dx₂(t,q) * v²(t,q,p) + dA₃dx₂(t,q) * v³(t,q,p) + E₂(t,q) +
           (dg₁₁dx₂(t,q) * v¹(t,q,p)^2 + dg₂₂dx₂(t,q) * v²(t,q,p)^2 + dg₃₃dx₂(t,q) * v³(t,q,p)^2) / 2
    f[3] = dA₁dx₃(t,q) * v¹(t,q,p) + dA₂dx₃(t,q) * v²(t,q,p) + dA₃dx₃(t,q) * v³(t,q,p) + E₃(t,q) +
           (dg₁₁dx₃(t,q) * v¹(t,q,p)^2 + dg₂₂dx₃(t,q) * v²(t,q,p)^2 + dg₃₃dx₃(t,q) * v³(t,q,p)^2) / 2
    nothing
end

charged_particle_3d_iode_ϑ(t, q, v, θ) = ϑ(t, q, v, θ)

function charged_particle_3d_iode_f(t, q, v, f)
    f[1] = dA₁dx₁(t,q) * v[1] + dA₂dx₁(t,q) * v[2] + dA₃dx₁(t,q) * v[3] + E₁(t,q) +
           (dg₁₁dx₁(t,q) * v[1]^2 + dg₂₂dx₁(t,q) * v[2]^2 + dg₃₃dx₁(t,q) * v[3]^2) / 2
    f[2] = dA₁dx₂(t,q) * v[1] + dA₂dx₂(t,q) * v[2] + dA₃dx₂(t,q) * v[3] + E₂(t,q) +
           (dg₁₁dx₂(t,q) * v[1]^2 + dg₂₂dx₂(t,q) * v[2]^2 + dg₃₃dx₂(t,q) * v[3]^2) / 2
    f[3] = dA₁dx₃(t,q) * v[1] + dA₂dx₃(t,q) * v[2] + dA₃dx₃(t,q) * v[3] + E₃(t,q) +
           (dg₁₁dx₃(t,q) * v[1]^2 + dg₂₂dx₃(t,q) * v[2]^2 + dg₃₃dx₃(t,q) * v[3]^2) / 2
    nothing
end

function charged_particle_3d_iode_g(t, q, v, f)
    f[1] = dA₁dx₁(t,q) * v[1] + dA₂dx₁(t,q) * v[2] + dA₃dx₁(t,q) * v[3] +
           (dg₁₁dx₁(t,q) * v[1]^2 + dg₂₂dx₁(t,q) * v[2]^2 + dg₃₃dx₁(t,q) * v[3]^2) / 2
    f[2] = dA₁dx₂(t,q) * v[1] + dA₂dx₂(t,q) * v[2] + dA₃dx₂(t,q) * v[3] +
           (dg₁₁dx₂(t,q) * v[1]^2 + dg₂₂dx₂(t,q) * v[2]^2 + dg₃₃dx₂(t,q) * v[3]^2) / 2
    f[3] = dA₁dx₃(t,q) * v[1] + dA₂dx₃(t,q) * v[2] + dA₃dx₃(t,q) * v[3] +
           (dg₁₁dx₃(t,q) * v[1]^2 + dg₂₂dx₃(t,q) * v[2]^2 + dg₃₃dx₃(t,q) * v[3]^2) / 2
    nothing
end

# function charged_particle_3d_pode(q₀=qᵢ, v₀=vᵢ)
#     PODE(charged_particle_3d_pode_v, charged_particle_3d_pode_f, q₀, charged_particle_3d_pᵢ(q₀, v₀))
# end
function charged_particle_3d_pode(q₀=qᵢ, p₀=pᵢ)
    PODE(charged_particle_3d_pode_v, charged_particle_3d_pode_f, q₀, p₀)
end

function charged_particle_3d_iode(q₀=qᵢ, p₀=pᵢ)
    IODE(charged_particle_3d_iode_ϑ, charged_particle_3d_iode_f, charged_particle_3d_iode_g,
            q₀, p₀;
            h=hamiltonian, v = (t, q, v) -> nothing)
end


@pyimport mpmath as mpmath

besseli(nu, x) = convert(BigFloat, mpmath.besseli(abs(nu), x))
besselk(nu, x) = convert(BigFloat, mpmath.besselk(abs(nu), x))

dIm(m, γ, ρ) = γ*besseli(m + 1, γ*ρ) + m*besseli(m, γ*ρ)/ρ
dKm(m, γ, ρ) = -γ*besselk(m + 1, γ*ρ) + m*besselk(m, γ*ρ)/ρ

function _get_αβ_start(m, γ, D, ρ, n, ind)

    α = D[ind - 1] * (n[ind - 1])^2 * dKm(m, γ[ind - 1], ρ[ind]) * besseli(m, γ[ind]*ρ[ind])
    α -= D[ind] * (n[ind])^2 * besselk(m, γ[ind - 1]*ρ[ind]) * dIm(m, γ[ind], ρ[ind])

    β = D[ind - 1] * (n[ind - 1])^2 * dIm(m, γ[ind - 1], ρ[ind]) * besseli(m, γ[ind]*ρ[ind])
    β -= D[ind] * (n[ind])^2 * besseli(m, γ[ind - 1]*ρ[ind]) * dIm(m, γ[ind], ρ[ind])

    return α, β
end

function _get_αn_βn(m, αnp1, βnp1, γ, D, ρ, n, ind)

    αn = D[ind] * (n[ind])^2 * dKm(m, γ[ind], ρ[ind + 1])
    αn *= (αnp1*besseli(m, γ[ind + 1]*ρ[ind + 1]) - βnp1*besselk(m, γ[ind + 1]*ρ[ind + 1]))
    αn_aux = D[ind + 1] * (n[ind + 1])^2 * besselk(m, γ[ind]*ρ[ind + 1])
    αn_aux *= (αnp1*dIm(m, γ[ind + 1], ρ[ind + 1]) - βnp1*dKm(m, γ[ind + 1], ρ[ind + 1]))
    αn -= αn_aux

    βn = D[ind] * (n[ind])^2 * dIm(m, γ[ind], ρ[ind + 1])
    βn *= (αnp1*besseli(m, γ[ind + 1]*ρ[ind + 1]) - βnp1*besselk(m, γ[ind + 1]*ρ[ind + 1]))
    βn_aux = D[ind + 1] * (n[ind + 1])^2 * besseli(m, γ[ind]*ρ[ind + 1])
    βn_aux *= (αnp1*dIm(m, γ[ind + 1], ρ[ind + 1]) - βnp1*dKm(m, γ[ind + 1], ρ[ind + 1]))
    βn -= βn_aux

    return αn, βn
end

function _get_α1_β1(m, γ, D, ρ, n)

    ind = length(γ)
    α, β = _get_αβ_start(m, γ, D, ρ, n, ind)
    ind -= 1
    for i in ind:-1:2
        println(α, β)
        α, β = _get_αn_βn(m, α, β, γ, D, ρ, n, i)
    end

    return α, β
end

function _get_A1_B1(m, γ, D, ρ, n, ρ0)

    α1, β1 = _get_α1_β1(m, γ, D, ρ, n)

    A1 = besselk(m, γ[1]*ρ[1])
    A1 *= (β1*besselk(m, γ[1]*ρ0) - α1*besseli(m, γ[1]*ρ0))
    A1 /= D[1]*(α1*besseli(m, γ[1]*ρ[1]) - β1*besselk(m, γ[1]*ρ[1]))

    B1 = β1/D[1]
    B1 /= (α1*besseli(m, γ[1]*ρ[1]) - β1*besselk(m, γ[1]*ρ[1]))
    B1 *= (besseli(m, γ[1]*ρ0)*besselk(m, γ[1]*ρ[1]) - besselk(m, γ[1]*ρ0)*besseli(m, γ[1]*ρ[1]))

    return A1, B1
end

function _integrand_modbessel(m, A1, B1, γ, ρ, ϕ)

    out = A1*besseli(m, γ[1]*ρ[1]) + B1*besselk(m, γ[1]*ρ[1])
    out *= cos(m*ϕ)

    return out
end

function sum_modbessel(k, μa, D, ρ, n, ρ0, ϕ)

    a = 0.0
    A1 = 0.0
    B1 = 0.0
    γ = @. sqrt(μa/D + k^2)

    bessel_index = -200:200
    for m in bessel_index
        A1, B1 = _get_A1_B1(m, γ, D, ρ, n, ρ0)
        a += _integrand_modbessel(m, A1, B1, γ, ρ, ϕ)
    end

    return a
end

function _greens_homogenous(μa, D, ρ, n, ρ0, ϕ, z)

    g = quadgk(k -> cos(k*z)*sum_modbessel(k, μa, D, ρ, n, ρ0, ϕ), 0, 500)

    return g[1]/(2*π^2)
end

function _green_Nradialcyl(μa, D, ρ, n, ρ0, ϕ, z)

    r = sqrt(ρ[1]^2 + ρ0^2 - 2*ρ[1]*ρ0*cosh(ϕ) + z^2)

    g = exp(-r*sqrt(μa[1]/D[1]))/(4*π*D[1]*r)
    gh = _greens_homogenous(μa, D, ρ, n, ρ0, ϕ, z)

    return g + gh
end

function _greens_homogenous(μa, D, ρ, n, ρ0, ϕ, z)

    N = 190
    x, w = gausslegendre(N)
    ub = 150

    green = zeros(BigFloat, N)

    Threads.@threads for m in eachindex(x)
        green[m] = cos(x[m]*ub/2 +ub/2*z)*sum_modbessel(x[m]*ub/2 +ub/2, μa, D, ρ, n, ρ0, ϕ)*w[m]*ub
    end

    return sum(green)/(2*pi^2)
end


# several kernel functions
export rbf_kernel, brbf_kernel, rbf_functional, brbf_functional
rbf_kernel(x, y, σ::Real) = exp(-1/2σ * abs2(x-y))
rbf_kernel(x::BitStr, y::BitStr, σ::Real) = exp(-1/2σ * abs2(buffer(x-y)))
rbf_kernel(σ::Real) = (x, y) -> rbf_kernel(x, y, σ)
brbf_kernel(x, y, σ::Real) = exp(-1/2σ * count_ones(x⊻y))
brbf_kernel(σ::Real) = (x, y) -> brbf_kernel(x, y, σ)

rbf_functional(σ::Real) = StatFunctional{2}(rbf_kernel(σ))
brbf_functional(σ::Real) = StatFunctional{2}(brbf_kernel(σ))

export MMD, rbf_mmd_loss, brbf_mmd_loss
"""
    MMD{T,FT,VT<:AbstractVector{T}}
    MMD(f, probs) -> MMD

MMD loss, `VT` is the typeof probability vector, `FT` is the type of kernel function.
"""
struct MMD{T,FT,VT<:AbstractVector{T}}
    f::FT
    probs::VT
end

function Yao.expect(mmd::MMD, px::NDWeights, py::NDWeights=px)
    px_ = NDWeights(px.values .- mmd.probs)
    py_ = NDWeights(py.values .- mmd.probs)
    expect(StatFunctional{2}(mmd.f), px_, py_)
end

function (::Adjoint{Any,typeof(expect)})(mmd::MMD, circuit::Pair{<:ArrayReg, <:AbstractBlock})
    stat = StatFunctional{2}(mmd.f)
    reg, c = circuit
    out = copy(reg) |> c
    outδ = ArrayReg(witness_vec(stat, probs(out).-mmd.probs).*statevec(out))
    (in, inδ), paramsδ = apply_back((out, outδ), c)
    return outδ => paramsδ.*2
end

@inline function faithful_grad(mmd::MMD, pair::Pair{<:ArrayReg,<:AbstractBlock})
    stat = StatFunctional{2}(mmd.f)
    initial = probs(copy(pair.first) |> pair.second) .- mmd.probs
    wvec = witness_vec(stat, initial)
    map(get_diffblocks(pair.second)) do diffblock
        r1, r2 = _perturb(()->_dropdims(sum(probs(copy(pair.first) |> pair.second) .* wvec, dims=1), dims=1)/2, diffblock, π/2)
        (r2 - r1)*ndims(stat)/2
    end
end

rbf_mmd_loss(σ::Real, probs) = MMD(rbf_kernel(σ), probs)
brbf_mmd_loss(σ::Real, probs) = MMD(brbf_kernel(σ), probs)


#=

# 2: Water collapse (explicit)

```@raw html
	<img src='../assets/collapse_exp.png' alt='missing' width="50%" height="50%" /><br>
```

Simulation of a water column collapsing under its own weight onto dry bottom.
This is, where SPH is more useful than typical mesh-based methods
=#

module collapse_dry

using Printf
using SmoothedParticles
using CSV
using DataFrames

#=
Declare constant parameters
=#

##physical
const dr = 2.0e-2          #average particle distance (decrease to make finer simulation)
const h = 3.0*dr           #size of kernel support
const rho0 = 1000.   	   #fluid density
const m = rho0*dr^2        #particle mass
const c = 50.0             #numerical speed of sound
const g = -9.8*VECY        #gravitational acceleration
const mu = 8.4e-4          #dynamic viscosity of water
const nu = 1.0e-4          #pressure stabilization

##geometrical
const water_column_width = 1.0
const water_column_height = 2.0
const box_height = 3.0
const box_width = 4.0
const wall_width = 2.5*dr

##temporal
const dt = 0.1*h/c
const t_end = 1.0
const dt_frame = t_end/50

##particle types
const FLUID = 0.
const WALL = 1.

#=
Declare variables to be stored in a Particle
=#

mutable struct Particle <: AbstractParticle
	x::RealVector #position
	v::RealVector #velocity
	a::RealVector #acceleration
	P::Float64 #pressure
	rho::Float64 #density
	Drho::Float64 #rate of density
	type::Float64 #particle_type
	Particle(x, type) = new(
		x, VEC0, VEC0,
		0.,
		rho0, 0.,
		type
	)
end

#=
Define geometry and make particles
=#

function make_system()
	grid = Grid(dr, :square)
	box = Rectangle(0., 0., box_width, box_height)
	fluid = Rectangle(0., 0., water_column_width, water_column_height)
	walls = BoundaryLayer(box, grid, wall_width)
	walls = Specification(walls, x -> (x[2] < box_height))
	sys = ParticleSystem(Particle, box + walls, h)
	generate_particles!(sys, grid, fluid, x -> Particle(x, FLUID))
	generate_particles!(sys, grid, walls, x -> Particle(x, WALL))
	return sys
end

#=
Define particle interactions
=#

@inbounds function balance_of_mass!(p::Particle, q::Particle, r::Float64)
	ker = m*rDwendland2(h,r)
	p.Drho += ker*(dot(p.x-q.x, p.v-q.v) + 2*nu*(p.rho-q.rho))
end

function find_pressure!(p::Particle)
	p.rho += p.Drho*dt
	p.Drho = 0.0
	p.P = c^2*(p.rho - rho0)
end

@inbounds function internal_force!(p::Particle, q::Particle, r::Float64)
	if p.type == FLUID
		ker = m*rDwendland2(h,r)
		p.a += -ker*(p.P/rho0^2 + q.P/rho0^2)*(p.x - q.x)
		p.a += +2*ker*mu/rho0^2*(p.v - q.v)
	end
end

function move!(p::Particle)
	p.a = VEC0
	if p.type == FLUID
		p.x += dt*p.v
	end
end

function accelerate!(p::Particle)
	if p.type == FLUID
		p.v += 0.5*dt*(p.a + g)
	end
end

function energy(p::Particle)::Float64
	kinetic = 0.5*m*dot(p.v, p.v)
	potential = -m*dot(g, p.x)
	internal =  0.5*m*c^2*(p.rho - rho0)^2/rho0^2
	return kinetic + potential + internal
end

function compute_max(sys::ParticleSystem)::NTuple{2,Float64}
	H = -Inf
	X = -Inf
	for p in sys.particles
		if p.type == FLUID && p.x[1] > X
			X = p.x[1]
		end
		if p.type == FLUID && p.x[1] > 2*h && p.x[2] > H
			H = p.x[2]
		end
	end
	return (X-1.0,0.5*H)
end

#=
Put everything into a time loop
=#
function main()
	ts = []
	Xs = []
	Hs = []
	sys = make_system()
	out = new_pvd_file("results/collapse_dry")
	#a modified Verlet scheme
	@time for k = 0 : Int64(round(t_end/dt))
	#move particles
		apply!(sys, move!)
		create_cell_list!(sys)
		apply!(sys, balance_of_mass!)
		apply!(sys, find_pressure!)
		apply!(sys, internal_force!)
		apply!(sys, accelerate!)
		#save data at selected frames
		if (k % Int64(round(dt_frame/dt)) == 0)
			@printf("t = %.6e\n", k*dt)
			@printf("E = %.6e\n", sum(energy, sys.particles))
			@printf("\n")
			(X, H) = compute_max(sys)
			push!(Xs, X)
			push!(Hs, H)
			push!(ts, k*dt*sqrt(-2*g[2]))
			save_frame!(out, sys, :v, :P, :rho, :type)
		end
		#accelerate
		apply!(sys, accelerate!)
	end
	save_pvd_file(out)
	df = DataFrame(time = ts, X = Xs, H = Hs)
	CSV.write("results/collapse_dry/data.csv", df)
end ## function main

end ## module


""" model function for an isotope """
function alphamodel(x,var)
    μ = var[1]
    σ = var[2]
    τ = var[3]
    A = var[4]
    amp = A / 2τ
    oneOverSqrt2 = 1.0 / √2
    sigScaledByTau  = σ / τ
    return @.  amp * exp((x - μ ) / τ + (sigScaledByTau^2) ) * erfc( oneOverSqrt2 * ((x - μ ) / σ  +  sigScaledByTau ))
end