@testset "Solve: car" begin
    # ## horizon
    T = 51

    # ## car
    num_state = 3
    num_action = 2
    num_parameter = 0

    function car(x, u, w)
        [u[1] * cos(x[3]); u[1] * sin(x[3]); u[2]]
    end

    function midpoint_explicit(x, u, w)
        h = 0.1 # timestep
        x + h * car(x + 0.5 * h * car(x, u, w), u, w)
    end

    # ## model
    dynamics = Dynamics(midpoint_explicit, num_state, num_action, num_parameter=num_parameter)
    model = [dynamics for t = 1:T-1]

    # ## initialization
    x1 = [0.0; 0.0; 0.0]
    xT = [1.0; 1.0; 0.0]

    # ## rollout
    ū = [1.0e-2 * [1.0; 0.1] for t = 1:T-1]
    w = [zeros(num_parameter) for t = 1:T]
    x̄ = rollout(model, x1, ū, w)

    # ## objective
    ot = (x, u, w) -> 1.0 * dot(x - xT, x - xT) + 1.0e-2 * dot(u, u)
    oT = (x, u, w) -> 0.0 * dot(x - xT, x - xT)
    ct = Cost(ot, num_state, num_action, num_parameter=num_parameter)
    cT = Cost(oT, num_state, 0, num_parameter=0)
    objective = [[ct for t = 1:T-1]..., cT]

    # ## constraints
    ul = -5.0 * ones(num_action)
    uu = 5.0 * ones(num_action)

    p_obs = [0.5; 0.5]
    r_obs = 0.1

    function stage_con(x, u, w)
        e = x[1:2] - p_obs
        [
        ul - u; # control limit (lower)
        u - uu; # control limit (upper)
        r_obs^2.0 - dot(e, e); # obstacle
        ]
    end

    function terminal_con(x, u, w)
        e = x[1:2] - p_obs
        [
        x - xT; # goal
        r_obs^2.0 - dot(e, e); # obstacle
        ]
    end

    cont = Constraint(stage_con, num_state, num_action, indices_inequality=collect(1:5))
    conT = Constraint(terminal_con, num_state, 0, indices_inequality=collect(3 .+ (1:1)))
    constraints = [[cont for t = 1:T-1]..., conT]

    # ## problem
    options = Options(verbose=false,
        line_search=:armijo,
        min_step_size=1.0e-5,
        objective_tolerance=1.0e-3,
        lagrangian_gradient_tolerance=1.0e-3,
        max_iterations=100,
        max_dual_updates=10,
        initial_constraint_penalty=1.0,
        scaling_penalty=10.0)
    s = Solver(model, objective, constraints,
        options=options)
    initialize_controls!(s, ū)
    initialize_states!(s, x̄)

    # ## solve
    solve!(s)

    # ## solution
    x_sol, u_sol = get_trajectory(s)

    @test all([all(stage_con(x_sol[t], u_sol[t], w[t]) .<= options.constraint_tolerance) for t = 1:T-1])
    @test all(abs.(terminal_con(x_sol[T], zeros(0), zeros(0))[1:3]) .<= options.constraint_tolerance)

    # ## allocations
    # info = @benchmark solve!($prob, a, b) setup=(a=deepcopy(x̄), b=deepcopy(ū))
    # @test info.allocs == 0
end


#   Copyright (c) 2020-2022 Velexi Corporation
#
#   Licensed under the Apache License, Version 2.0 (the "License");
#   you may not use this file except in compliance with the License.
#   You may obtain a copy of the License at
#
#       http://www.apache.org/licenses/LICENSE-2.0
#
#   Unless required by applicable law or agreed to in writing, software
#   distributed under the License is distributed on an "AS IS" BASIS,
#   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
#   See the License for the specific language governing permissions and
#   limitations under the License.

"""
divide.jl defines methods for the /(x, y) function
"""

# --- Exports

import Base.:(/)

# --- Method definitions

# ------ Specializations involving an AbstractMultivector instance

# M::AbstractMultivector, B::One
# B::One, M::AbstractMultivector
/(M::AbstractMultivector, B::One) = M
/(B::One, M::AbstractMultivector) = nothing  # TODO

# M::AbstractMultivector, B::Zero
# B::Zero, M::AbstractMultivector
/(B::Zero, M::AbstractMultivector) = B
/(M::AbstractMultivector, B::Zero) = Scalar{typeof(norm(M))}(Inf)

# ------ Specializations involving an AbstractBlade instance

/(B::AbstractBlade, C::AbstractBlade) = B * inv(C)

# B::AbstractBlade, C::One
# B::One, C::AbstractBlade
/(B::AbstractBlade, C::One) = B
/(B::One, C::AbstractBlade) = inv(B)

# B::Zero, C::AbstractBlade
/(B::Zero, C::AbstractBlade) = B

# B::AbstractBlade, x::Real
# x::Real, B::AbstractBlade
/(B::AbstractBlade, x::Real) = Blade(B, volume=volume(B) / x)
/(x::Real, B::AbstractBlade) = x * inv(B)

# ------ Specializations involving a Blade instance

# B::Blade, C::AbstractScalar
# B::AbstractScalar, C::Blade
/(B::Blade, C::AbstractScalar) = Blade(B, volume=volume(B) / value(C))
/(B::AbstractScalar, C::Blade) =
    mod(grade(C), 4) < 2 ?
        Blade(C, volume=value(B) / volume(C)) :
        Blade(C, volume=-value(B) / volume(C))

# B::Blade, C::One
# B::One, C::Blade
/(B::Blade, C::One) = B
/(B::One, C::Blade) = inv(C::Blade)

# B::Blade, C::Zero
# B::Zero, C::Blade
/(B::Blade, C::Zero) = Blade(B, volume=sign(B) * Inf)
/(B::Zero, C::Blade) = B

# ------ Specializations involving a Pseudoscalar instance

# B::Pseudoscalar, C::Pseudoscalar
function /(B::Pseudoscalar, C::Pseudoscalar)
    assert_dim_equal(B, C)
    Scalar{typeof(value(B))}(value(B) / value(C))
end

# B::Pseudoscalar, C::AbstractScalar
# B::AbstractScalar, C::Pseudoscalar
/(B::Pseudoscalar, C::AbstractScalar) = B / value(C)
/(B::AbstractScalar, C::Pseudoscalar) = value(B) / C

# B::Pseudoscalar, C::One
# B::One, C::Pseudoscalar
/(B::Pseudoscalar, C::One) = B
/(B::One, C::Pseudoscalar) = inv(C)

# B::Pseudoscalar, C::Zero
# B::Zero, C::Pseudoscalar
/(B::Pseudoscalar, C::Zero) = Pseudoscalar(B, value=sign(B) * Inf)
/(B::Zero, C::Pseudoscalar) = B

# B::Pseudoscalar, x::Real
# x::Real, B::Pseudoscalar
/(B::Pseudoscalar, x::Real) = Pseudoscalar(B, value=value(B) / x)
/(x::Real, B::Pseudoscalar) =
    mod(dim(B), 4) < 2 ?
        Pseudoscalar(B, value=x / value(B)) :
        Pseudoscalar(B, value=-x / value(B))

# ------ Specializations involving an AbstractScalar instance

# B::AbstractScalar, C::One
# B::One, C::AbstractScalar
/(B::AbstractScalar, C::One) = B
/(B::One, C::AbstractScalar) = inv(C)

# B::AbstractScalar, C::Zero
# B::Zero, C::AbstractScalar
/(B::AbstractScalar, C::Zero) =
    sign(B) > 0 ?
        Scalar{typeof(norm(B))}(Inf) :
        Scalar{typeof(norm(B))}(-Inf)

/(B::Zero, C::AbstractScalar) = B

# B::AbstractScalar, x::Real
# x::Real, B::AbstractScalar
/(B::AbstractScalar, x::Real) = Scalar{typeof(value(B))}(value(B) / x)
/(x::Real, B::AbstractScalar) = Scalar{typeof(value(B))}(x / value(B))

# B::AbstractScalar, v::Vector
# v::Vector, B::AbstractScalar
/(v::Vector{<:Real}, B::AbstractScalar) = Blade(v) / value(B)

# ------ Specializations involving a Scalar instance

# B::Scalar, C::Scalar
/(B::Scalar, C::Scalar) = Scalar{typeof(value(B))}(value(B) / value(C))

# ------ Specializations involving a One instance

# B::One, C::One
/(B::One, C::One) = B

# B::One, C::Zero
# B::Zero, C::One
/(B::One, C::Zero) = inv(C)
/(B::Zero, C::One) = B

# ------ Specializations involving a Zero instance

# B::Zero, C::Zero
/(B::Zero, C::Zero) = Scalar{typeof(value(B))}(NaN)


using EffectiveWaves, Test

#tests the values of the integrand against results from the Mathematica file integrate_hankels.nb, which used no approximations.

# When these tests were done there was an error in the integration domain of B. Likely these will need to be redone.
@testset "Compare integrand values against brute force Mathematica result" begin

    # using OffsetArrays
    import EffectiveWaves: BS_matrices

    function intergrand_kernels(X, a12k; M = M, θin=θin, num_coefs = 2000)

        q = min(Int(floor(a12k/(X[2]-X[1]))),J)
        B, S = BS_matrices(X, a12k; θin=θin, M=M, num_coefs = num_coefs)

        function intergrand(l,j,m,n)
            P = S[j-l,n-m]
            Q = (abs(j-l) <= q) ? (B[j-l,n-m] - S[j-l,n-m]) : zero(Complex{Float64})
            P + Q
        end

        [intergrand(l,j,m,n) for l=0:J, m=-M:M, j=0:J, n=-M:M]
    end

    h=0.02; max_x=2.0;
    J = Int(round(max_x/h))
    x = (0:J).*h
    i1 = 1 + Int(round(1/h)) # for x1 = 1
    i2 = 1 + Int(round(2/h)) # for x1 = 2
    a12k = 1.0

    # # both commented tests below should also pass, but am commenting to save time
    # θin=0.; M=0; n=0;
    #
    # # math was produced by Mathematica
    # math00 = [172.31259187840652, 143.34097100449316, 172.31259187840536]
    # intergrand_quad = intergrand_kernels(x, a12k; θin=θin, M=M, num_coefs = 2000);
    # julia00 = [sum(abs.(intergrand_quad[i,M+1,:,M+1+n])) for i in [1,i1,i2]];
    # @test maximum(abs.(1.0 .- julia00./math00)) < 1e-6
    #
    # math0p50 = [192.61985415285892, 155.98244729650008, 192.61985415285886]
    # θin=0.5;
    # intergrand_quad = intergrand_kernels(x, a12k; θin=θin, M=M, num_coefs = 3000);
    # julia0p50 = [sum(abs.(intergrand_quad[i,M+1,:,M+1+n])) for i in [1,i1,i2]];
    # @test maximum(abs.(1.0 .- julia0p50./math0p50)) < 3e-6

    math0p32 = [241.5144625759003, 211.13367934660897, 181.54133990599098]
    θin=0.3; M=2; n=2;
    intergrand_quad = intergrand_kernels(x, a12k; θin=θin, M=M, num_coefs = 2000);
    julia0p32 = [sum(abs.(intergrand_quad[i,M+1,:,M+1+n])) for i in [1,i1,i2]];
    @test maximum(abs.(1.0 .- julia0p32./math0p32)) < 4e-6

end


"""
    BatchedTridiagonalSolver

A batched solver for large numbers of triadiagonal systems.
"""
struct BatchedTridiagonalSolver{A, B, C, F, T, G, P}
       a :: A
       b :: B
       c :: C
       f :: F
       t :: T
    grid :: G
  params :: P
end

"""
    BatchedTridiagonalSolver(; dl, d, du, f, grid, params=nothing)

Construct a solver for batched tridiagonal systems of the form

                               b(i, j, 1)ϕ(i, j, 1) + c(i, j,   2)ϕ(i, j,   2) = f(i, j, 1),  k = 1
    a(i, j, k-1)ϕ(i, j, k-1) + b(i, j, k)ϕ(i, j, k) + c(i, j, k+1)ϕ(i, j, k+1) = f(i, j, k),  k = 2, ..., N-1
    a(i, j, N-1)ϕ(i, j, N-1) + b(i, j, N)ϕ(i, j, N)                            = f(i, j, N),  k = N

where `dl` stores the lower diagonal coefficients `a(i, j, k)`, `d` stores the diagonal coefficients `b(i, j, k)`,
`du` stores the upper diagonal coefficients `c(i, j, k)`, and `f` stores the right-hand-side terms `f(i, j, k)`. A
`grid` must be passed in.

`dl`, `d`, `du`, and `f` can be specified in three ways to describe different batched systems:
1. A 1D array indicates that the coefficients only depend on `k` and are the same for all the tridiagonal systems, i.e.
   `a(i, j, k) = a(k)`.
2. A 3D array indicates that the coefficients are different for each tridiagonal systems and depend on `(i, j, k)`.
3. A function with the signature `b(i, j, k, grid, params)` that returns the coefficient `b(i, j, k)`.

`params` is an optional named tuple of parameters that is accessible to functions.
"""
function BatchedTridiagonalSolver(arch=CPU(), FT=Float64; dl, d, du, f, grid, params=nothing)
    ArrayType = array_type(arch)
    t = zeros(FT, grid.Nx, grid.Ny, grid.Nz) |> ArrayType

    return BatchedTridiagonalSolver(dl, d, du, f, t, grid, params)
end

@inline get_coefficient(a::AbstractArray{T, 1}, i, j, k, grid, p) where {T} = @inbounds a[k]
@inline get_coefficient(a::AbstractArray{T, 3}, i, j, k, grid, p) where {T} = @inbounds a[i, j, k]
@inline get_coefficient(a::Function, i, j, k, grid, p) = a(i, j, k, grid, p)

"""
    solve_batched_tridiagonal_system!(ϕ, arch, solver)

Solve the batched tridiagonal system of linear equations described by the
`BatchedTridiagonalSolver` `solver` using a modified TriDiagonal Matrix Algorithm (TDMA)
that is still capable of solving singular systems with a zero eigenvalue.

The result is stored in `ϕ` which must have size `(grid.Nx, grid.Ny, grid.Nz)`.

Reference implementation per Numerical Recipes, Press et. al 1992 (§ 2.4).
"""
function solve_batched_tridiagonal_system!(ϕ, arch, solver)
    a, b, c, f, t, grid, params = solver.a, solver.b, solver.c, solver.f, solver.t, solver.grid, solver.params

    event = launch!(arch, grid, :xy,
                    solve_batched_tridiagonal_system_kernel!, ϕ, a, b, c, f, t, grid, params,
                    dependencies=Event(device(arch)))

    wait(device(arch), event)

    return nothing
end

@kernel function solve_batched_tridiagonal_system_kernel!(ϕ, a, b, c, f, t, grid, p)
    Nx, Ny, Nz = grid.Nx, grid.Ny, grid.Nz

    i, j = @index(Global, NTuple)

    @inbounds begin
        β  = get_coefficient(b, i, j, 1, grid, p)
        f₁ = get_coefficient(f, i, j, 1, grid, p)
        ϕ[i, j, 1] = f₁ / β

        @unroll for k = 2:Nz
            cₖ₋₁ = get_coefficient(c, i, j, k-1, grid, p)
            bₖ   = get_coefficient(b, i, j, k,   grid, p)
            aₖ₋₁ = get_coefficient(a, i, j, k-1, grid, p)

            t[i, j, k] = cₖ₋₁ / β
            β    = bₖ - aₖ₋₁ * t[i, j, k]

            # This should only happen on last element of forward pass for problems
            # with zero eigenvalue. In that case the algorithmn is still stable.
            abs(β) < 1e-12 && break

            fₖ = get_coefficient(f, i, j, k, grid, p)

            ϕ[i, j, k] = (fₖ - aₖ₋₁ * ϕ[i, j, k-1]) / β
        end

        @unroll for k = Nz-1:-1:1
            ϕ[i, j, k] = ϕ[i, j, k] - t[i, j, k+1] * ϕ[i, j, k+1]
        end
    end
end


using MyFFT
using Base.Test

FFTW.set_num_threads(1)
fft([0.0im, 0.0im, 0.0im])
myfft([0.0im, 0.0im, 0.0im])
myifft([0.0im, 0.0im, 0.0im])
myrealfft([1.0, 1.0])
myirealfft([0.0im, 0.0im])

n = [1, 2, 4, 256, 512, 1024, 262144, 524288, 1048576, 3, 5, 7, 9, 253, 254, 255, 257, 258, 259]

for i in n
    println("N = $(i):")
    v = rand(i) + im * rand(i)
    println("myfft():")
    myfftClearCache()
    gc()
    @time v_myfft = myfft(v)
    gc()
    @time v_myfft = myfft(v)
    println("fft():")
    gc()
    @time v_fft = fft(v)
    gc()
    @time v_fft = fft(v)

    for j = 1:i
        eps_fft = 2^0.5 * eps() * 2.0 * ceil(log2(2*i-1))^2.0
        eps_myfft = 2^0.5 * eps() * 2.0 * nextpow2(2*i-1)^2.0 * ceil(log2(2*i-1))^2.0
        eps_total = eps_fft + eps_myfft
        @test_approx_eq_eps(real(v_myfft[j]), real(v_fft[j]), eps_total)
        @test_approx_eq_eps(imag(v_myfft[j]), imag(v_fft[j]), eps_total)
    end
end

n = [1, 2, 4, 256, 512, 1024, 253, 254, 255, 257, 258, 259]

for i in n
    println("N = $(i):")
    v = rand(i) + im * rand(i)
    println("myifft(myfft()):")
    myfftClearCache()
    gc()
    @time v2 = myifft(myfft(v))

    for j = 1:i
        eps_fft = 2^0.5 * eps() * 4.0 * ceil(log2(2*i-1))^4.0
        eps_myfft = 2^0.5 * eps() * 4.0 * nextpow2(2*i-1)^4.0 * ceil(log2(2*i-1))^4.0
        eps_total = eps_fft + eps_myfft
        @test_approx_eq_eps(real(v[j]), real(v2[j]), eps_total)
        @test_approx_eq_eps(imag(v[j]), imag(v2[j]), eps_total)
    end
end

n = [2, 4, 6, 8, 256, 250, 252, 254, 258, 260, 262, 1, 3, 5, 7, 251, 253, 255, 257, 259, 261]

for i in n
    println("N = $(i):")
    v = rand(i)
    println("myrealfft():")
    myfftClearCache()
    gc()
    @time v_realfft = myrealfft(v)
    gc()
    @time v_realfft = myrealfft(v)
    println("fft():")
    gc()
    @time v_fft = fft(v)
    gc()
    @time v_fft = fft(v)

    for j = 1:i
        eps_fft = 1.0 * eps() * 2.0 * ceil(log2(2*i-1))^2.0
        eps_myfft = 1.0 * eps() * 2.0 * nextpow2(2*i-1)^2.0 * ceil(log2(2*i-1))^2.0
        eps_total = eps_fft + eps_myfft
        @test_approx_eq_eps(real(v_realfft[j]), real(v_fft[j]), eps_total)
        @test_approx_eq_eps(imag(v_realfft[j]), imag(v_fft[j]), eps_total)
    end
end

for i in n
    println("N = $(i):")
    v = rand(i)
    println("myirealfft(myrealfft()):")
    myfftClearCache()
    gc()
    @time v2 = myirealfft(myrealfft(v))

    for j = 1:i
        eps_fft = 1.0 * eps() * 4.0 * ceil(log2(2*i-1))^4.0
        eps_myfft = 1.0 * eps() * 4.0 * nextpow2(2*i-1)^4.0 * ceil(log2(2*i-1))^4.0
        eps_total = eps_fft + eps_myfft
        @test_approx_eq_eps(v[j], v2[j], eps_total)
    end
end

println("BigFloat:")
x = im * convert(BigFloat, 0)
gc()
@time myfft([x, x, x])
gc()
@time myfft([x, x, x])


using Distributions
using StatsBase
include("./baum_welch_lib.jl")
using Hmm

# 状態数
state_num = 2
# 出力シンボル(この場合は 0 と 1 )
symbol = [0 1]
# 出力シンボルの数
symbol_num = length(symbol)

# これが本当の値
# 遷移確率行列
A = [0.9 0.1; 0.15 0.85]
# 出力確率行列
B = [0.8 0.2; 0.2 0.8]
# 初期確率
ρ = [0.01 0.99]

# これはパラメータ学習の初期値
# 遷移確率行列
eA = [0.4 0.6; 0.3 0.7]
# 出力確率行列
eB = [0.8 0.2; 0.2 0.8]
# 初期確率
eρ = [1/2 1/2]

srand(1234)

function simulate(nSteps)

  observations = zeros(nSteps)
  states = zeros(nSteps)
  states[1] = sample(symbol, Weights(vec(ρ)))
  observations[1] = sample(symbol, Weights(B[Int(states[1])+1, :]))
  for t in 2:nSteps
    states[t] = sample(symbol, Weights(A[Int(states[t-1])+1, :]))
    observations[t] = sample(symbol, Weights(B[Int(states[t])+1, :]))
  end
  return observations,states
end

o1, s = simulate(500)
o2, s = simulate(20)
o3, s = simulate(9)
o4, s = simulate(3)
obs = [o1, o2, o3, o4]

hmm = Hmm.hmm_initialization(eA, eB, eρ)
Hmm.train(hmm, obs, 1e-4, 100000)

println("Actual parameter ρ")
println(ρ)
println("Estimated parameter ρ")
println(hmm.ρ)
println("Actual parameter A")
println(A)
println("Estimated parameter A")
println(hmm.A)
println("Actual parameter B")
println(B)
println("Estimated parameter B")
println(hmm.B)


using DistributionsDiff
using Test
using Distributions
using ChainRulesCore
import ForwardDiff

@testset "gradlogpdf" begin
    @testset "Exponential" begin
        x = 2.0
        θ = 3.0
        E, E_pullback = rrule(Exponential, θ)
        lp, lp_pullback = rrule(logpdf, E, x)

        vm = 1
        (_, res1, res2) = lp_pullback(vm)

        grad = ForwardDiff.gradient(pair -> logpdf(Exponential(pair[2]), pair[1]), [x, θ])
        @test extern(res1) ≈ grad[1]
        @test extern(res2) ≈ grad[2]
    end

    @testset "Normal" begin
        x = 2.0
        μ = 3.0
        σ = 1.0
        N, N_pullback = rrule(Normal, μ, σ)
        lp, lp_pullback = rrule(logpdf, N, x)

        vm = 1
        (_, res1, res2, res3) = lp_pullback(vm)

        grad = ForwardDiff.gradient(pair -> logpdf(Normal(pair[2], pair[3]), pair[1]), [x, μ, σ])
        @test extern(res1) ≈ grad[1]
        @test extern(res2) ≈ grad[2]
        @test extern(res3) ≈ grad[3]
    end

    # @testset "Arcsine" begin
    #     x = 4.0
    #     a = 3.0
    #     b = 5.0
    #     N, N_pullback = rrule(Normal, μ, σ)
    #     lp, lp_pullback = rrule(logpdf, N, x)

    #     vm = 1
    #     (_, res1, res2, res3) = lp_pullback(vm)

    #     grad = ForwardDiff.gradient(pair -> logpdf(Normal(pair[2], pair[3]), pair[1]), [x, μ, σ])
    #     @test extern(res1) ≈ grad[1]
    #     @test extern(res2) ≈ grad[2]
    #     @test extern(res3) ≈ grad[3]
    # end

    @testset "Beta" begin
        x = 0.4
        α = 3.0
        β = 5.0
        B, B_pullback = rrule(Beta, α, β)
        lp, lp_pullback = rrule(logpdf, B, x)

        vm = 1
        (_, res1, res2, res3) = lp_pullback(vm)

        grad = ForwardDiff.gradient(pair -> logpdf(Beta(pair[2], pair[3]), pair[1]), [x, α, β])
        @test extern(res1) ≈ grad[1]
        @test extern(res2) ≈ grad[2]
        @test extern(res3) ≈ grad[3]
    end

end


using MultipleScattering
using Base.Test
using Plots

@testset "Summary" begin

    include("shapetests.jl")
    include("particle_tests.jl")
    include("time_simulation_tests.jl")
    include("moments_tests.jl")

    # test our discretised Fourier transforms
    @testset "Fourier tranforms" begin
      include("fourier_test.jl")
      @test fourier_test()
    end

    @testset "Type stability" begin
        # Define everything as a Float32
        volfrac = 0.01f0
        radius = 1.0f0
        k_arr = collect(LinRange(0.01f0,1.0f0,100))
        simulation = FrequencySimulation(volfrac,radius,k_arr)
        @test typeof(simulation.response[1]) == Complex{Float32}
    end

    @testset "Single scatterer" begin
        include("single_scatter.jl")
        # Test against analytic solution
        @test single_scatter_test()
    end

    @testset "Boundary conditions" begin
        include("boundary_conditions.jl")
        # Test boundary conditions for 4 particles with random properties and positions.
        @test boundary_conditions_test()
    end

    @testset "Plot FrequencySimulation" begin
        # Just run it to see if we have any errors (yes thats a very low bar)
        volfrac = 0.01
        radius = 2.0
        k_arr = collect(LinRange(0.2,1.0,5))
        simulation = FrequencySimulation(volfrac,radius,k_arr)

        plot(simulation)
        plot(simulation,0.2)

        @test true
    end

end


const _parker_params = (
    A = [0.809, 0.330],
    T = [0.17046, 0.365],
    σ = [0.0563, 0.132],
    α = 1.050,
    β = 0.1685,
    s = 38.078,
    τ = 0.483,
)

"""
    aif_parker(t; params, hct = 0.42)

Returns the arterial input function defined in Parker et al. [@Parker2006].
The timepoints `t` are in minutes with the bolus arrival time defined as `t = 0`.
The model parameters from Ref [@Parker2006] will be used by default unless an optional `params` input is provided.
The hematocrit `hct` is used to convert the concentration in blood to plasma and has default value of 0.42.
"""
function aif_parker(t; params = _parker_params, hct = 0.42)
    @extract (A, T, σ, α, β, s, τ) params
    cb = [
        (A[1] / (σ[1] * sqrt(2 * pi))) * exp(-(t - T[1])^2 / (2 * (σ[1])^2)) +
        (A[2] / (σ[2] * sqrt(2 * pi))) * exp(-(t - T[2])^2 / (2 * (σ[2])^2)) +
        α * exp(-β * t) / (1.0 + exp(-s * (t - τ))) for t in t
    ]
    cb[t.<=0] .= 0
    ca = cb ./ (1 - hct)
    return ca
end

const _georgiou_params =
    (A = [0.37, 0.33, 10.06], m = [0.11, 1.17, 16.02], α = 5.26, β = 0.032, τ = 0.129)

"""
    aif_georgiou(t; params, hct = 0.35)

Returns the concentration in blood plasma using the model defined in Georgiou et al. [@Georgiou2018].
The timepoints `t` are in minutes with the bolus arrival defined as `t = 0`.
The model parameters from the paper will be used by default unless an optional `params` input is provided.
The hematocrit `hct` is used to convert the concentration in blood to plasma and has default value of 0.35.
"""
function aif_georgiou(t; params = _georgiou_params, hct = 0.35)
    @extract (A, m, α, β, τ) params
    # The AIF can be split into two parts
    exponential_part = zeros(size(t))
    gammavariate_part = zeros(size(t))

    for (i, t) in enumerate(t)
        exponential_part[i] = sum(A .* exp.(-m * t))
        if t > 7.5
            # Gamma variate part undefined for high N (i.e. t > 7.5 min), so fix its value
            gammavariate_part[i] = 3.035
            continue
        end
        N = fld(t, τ) # `fld` might be more efficient than `floor`
        for j = 0:N
            gammavariate_part[i] += gammavariate((j + 1) * α + j, β, t - j * τ)
        end
    end
    cb = exponential_part .* gammavariate_part
    cb[t.<=0] .= 0
    ca = cb ./ (1 - hct)
    return ca
end

function gammavariate(α, β, t)::Float64
    if t > 0
        gammavalue = (t^α * exp(-t / β)) / (β^(α + 1) * gamma(α + 1))
        if !isnan(gammavalue)
            return gammavalue
        end
    end
    return 0
end

"""
    aif_biexponential(t; params, hct = 0)

Returns an AIF defined by the biexponential model with the form ``D * (a_1 exp(-m_1 t) + a_2 exp(-m_2 t))``.
The `t` input is in minutes with the bolus arriving at 0.
The `params` can be a named tuple, e.g. `params = (D=0.1, a=[4.0, 4.78], m=[0.144, 0.011])`.
The `hct` is used to convert the concentration in blood to plasma.
The default value is set to to zero, i.e. no conversion takes place.
"""
function aif_biexponential(t; params, hct = 0)
    @extract (D, a, m) params
    cb = [D * (a[1] * exp(-m[1] * t) + a[2] * exp(-m[2] * t)) for t in t]
    cb[t.<=0] .= 0
    ca = cb ./ (1 - hct)
    return ca
end

"""
    aif_weinmann(t)

Returns an AIF defined by the biexponential model with parameters from
Tofts & Kermode [@Tofts1991] and Weinmann et al. [@Weinmann1984].
The `t` input is in minutes with the bolus arriving at 0.
The model describes concentration in plasma, therefore a hematocrit is not necessary.
"""
function aif_weinmann(t)
    params = (; D = 0.1, a = [3.99, 4.78], m = [0.144, 0.0111])
    return aif_biexponential(t; params, hct = 0)
end

"""
    aif_fritzhansen(t)

Returns an AIF defined by the biexponential model with parameters based on data from
Fritz-Hansen et al. [@Fritz-Hansen1996].
The model parameters were adopted from Whitcher & Schmid [@Whitcher2011]
The `t` input is in minutes with the bolus arriving at 0.
The model describes concentration in plasma, therefore a hematocrit is not necessary.
"""
function aif_fritzhansen(t; hct = 0)
    params = (; D = 1.0, a = [2.4, 0.62], m = [3.0, 0.016])
    return aif_biexponential(t; params, hct)
end

"""
    aif_orton1(t; params = (aB = 10.4, μB = 12.0, aG = 1.24, μG = 0.169), hct = 0.42)

Returns the concentration in blood plasma using the bi-exponential model defined in
Orton et al. [@Orton2008].
The timepoints `t` are in minutes with the bolus arriving at 0.
The model parameters from the paper will be used by default unless an optional `params` input is provided.
The `hct` is used to convert the concentration in blood to plasma and has default value of 0.42.
"""
function aif_orton1(t; params = (aB = 10.4, μB = 12.0, aG = 1.24, μG = 0.169), hct = 0.42)
    @extract (aB, μB, aG, μG) params
    AB = aB - aB * aG / (μB - μG)
    AG = aB * aG / (μB - μG)
    cb = [AB * exp(-μB * t) + AG * exp(-μG * t) for t in t]
    cb[t.<0] .= 0
    ca = cb ./ (1 - hct)
    return ca
end

"""
    aif_orton2(t; params = (aB = 344, μB = 20.2, aG = 1.24, μG = 0.172), hct = 0.42)

Returns the concentration in blood plasma using the 2nd model defined in
Orton et al. [@Orton2008].
The timepoints `t` are in minutes with the bolus arriving at 0.
The model parameters from the paper will be used by default unless an optional `params` input is provided.
The `hematocrit` is used to convert the concentration in blood to plasma and has default value of 0.42.
"""
function aif_orton2(t; params = (aB = 344, μB = 20.2, aG = 1.24, μG = 0.172), hct = 0.42)
    @extract (aB, μB, aG, μG) params
    AB = aB - aB * aG / (μB - μG)
    AG = aB * aG / (μB - μG)^2
    cb = [AB * t * exp(-μB * t) + AG * (exp(-μG * t) - exp(-μB * t)) for t in t]
    cb[t.<=0] .= 0
    ca = cb ./ (1 - hct)
    return ca
end

"""
    aif_orton3(t; params = (aB = 2.84, μB = 22.8, aG = 1.36, μG = 0.171), hct = 0.42)

Returns the concentration in blood plasma using the 3rd model defined in
Orton et al. [@Orton2008].
The timepoints `t` are in minutes with the bolus arriving at `t = 0`.
The model parameters from the paper will be used by default unless an optional `params` input is provided.
The `hct` is used to convert the concentration in blood to plasma and has default value of 0.42.
"""
function aif_orton3(t; params = (aB = 2.84, μB = 22.8, aG = 1.36, μG = 0.171), hct = 0.42)
    @extract (aB, μB, aG, μG) params
    tb = 2 * pi / μB
    cb = zeros(length(t))
    for (i, t) in enumerate(t)
        if t <= tb
            cb[i] = aB * (1 - cos(μB * t)) + aB * aG * _orton3_f(t, μG, μB)
        else
            cb[i] = aB * aG * _orton3_f(tb, μG, μB) * exp(-μG * (t - tb))
        end
    end
    cb[t.<=0] .= 0
    ca = cb ./ (1 - hct)
    return ca
end

function _orton3_f(t, α, μB)
    (1 / α) * (1 - exp(-α * t)) -
    (1 / (α^2 + μB^2)) * (α * cos(μB * t) + μB * sin(μB * t) - α * exp(-α * t))
end


function create_graph_from_TTC(routes_dict::OrderedDict{Symbol, OrderedDict{Int,Tuple{Float64,Float64}}}
								, n_stops::Int
								, secs_between_stops::Dict{Tuple{Int,Int},Int64}
								, plot_graph::Bool = false
								, write_to_pdf::Bool = false)
	"""
	Description:
		Takes into account real data regarding TTC routes, connections, and distances between stops and create Simple Weighted Directed Graph based on this information.
		Graph info: Vertices --> stops | Edges --> routes | Weights --> seconds between stops
	Returns:
		SimpleWeightedDiGraph with `n_stops` vertices.
	"""
    # basic graph with original routes
    gg = SimpleWeightedDiGraph(n_stops)
    for route in keys(routes_dict)
        for stop in 1:size(collect(keys(routes_dict[route])))[1]-1
            stop_time = secs_between_stops[(collect(keys(routes_dict[route]))[stop], collect(keys(routes_dict[route]))[stop+1])]
            stop_a = collect(keys(routes_dict[route]))[stop]
            stop_b = collect(keys(routes_dict[route]))[stop+1]
            add_edge!(gg, stop_a, stop_b, stop_time)
        end
    end

	# Check if stops on the routes were correctly connected.
	@assert adjacency_matrix(gg)[1,2] > 0.0  "SOMETHING WENT WRONG 1-->2 should have a non-zero weight!"
    @assert adjacency_matrix(gg)[2,1] == 0.0 "SOMETHING WENT WRONG 2-->1 should have zero weight!"
    @assert adjacency_matrix(gg)[616,615] > 0.0 "SOMETHING WENT WRONG 616-->615 should have a non-zero weight!"
    @assert adjacency_matrix(gg)[615,616] == 0.0 "SOMETHING WENT WRONG 615-->616 should have zero weight!"

    ### Adding artificial edges between same location stops ###

    # dictionary mapping node => LLA
    node_id_LLA = Dict{Int,Tuple{Float64,Float64}}()
    for route in collect(values(routes_dict))
        for (stop_id, stop_LLA) in route
           push!(node_id_LLA, stop_id => stop_LLA)
        end
    end

    # identifing same location nodes and assigning transfer times between them
    transfer_time_dict = Dict{Tuple{Int,Int},Int}()
    for (key, value) in node_id_LLA
        obj = [k for k in keys(node_id_LLA) if node_id_LLA[k] == value]
        obj = vec(collect(Iterators.product(obj, obj))) # array
        obj = filter(((xx, yy),)->xx!=yy, obj) # remove same node distances
        obj = Dict(zip(obj,repeat([120], length(obj))))
        merge!(transfer_time_dict, obj)
    end

    # adding fake edges to the graph
    for (key, value) in transfer_time_dict
        add_edge!(gg, key[1], key[2], value)
    end

    # all of the times in 1 object
    merge!(secs_between_stops, transfer_time_dict);

	# PDF or plot graph if needed
    if write_to_pdf == true
        draw(PDF("stops_subway_streetcars_Toronto_graph.pdf", 16cm, 16cm), gplot(gg))
    else
        if plot_graph == true
            gplot(gg)
        end
    end

    println("Graph from TTC data was made")
    return gg
end
println("Graph from TTC functions were read")


# gaussianprocess.jl

"""
    abstract type AbstractGaussianProcess end

Subtypes: GaussianProcess and FaberSchauderExpansionWithGaussianCoefficients.
Supertype: Any

Both subtypes represent a Gaussian process as a series expansion
in a basis of functions, where the vector of functions is multivariate normally
distributed.

See also: GaussianProcess and FaberSchauderExpansionWithGaussianCoefficients.
"""
abstract type AbstractGaussianProcess end

"""
  struct GaussianProcess{S, T} <: AbstractGaussianProcess where {
      S<:AbstractVector{U} where U<:Function, T<:Union{AbstractMvNormal,GaussianVector}}
    basis::S
    distribution::T
  end

Implements a Gaussian process defined as an inner product of a Gaussian vector
with distribution 'distribution' of type GaussianVector or a subtype of AbstractMvNormal
(from the Distributions.jl package) and a vector of functions,
the basis of the function space. For the use of a Gaussian process expanded in
Faber-Schauder functions up to level j, we recommend using
FaberSchauderExpansionWithGaussianCoefficients, as this makes efficiently use of
the sparsity structure of Faber-Schauder functions.

See also: FaberSchauderExpansionWithGaussianCoefficients, AbstractGaussianProcess

# Example

```julia
using Distributions
distribution = MvNormal([k^(-1.0) for k in 1:10])
Π = GaussianProcess([fourier(k) for k in 1:10], distribution)
f = rand(Π)
```
"""
struct GaussianProcess{S, T} <: AbstractGaussianProcess where {
    S<:AbstractVector{U} where U<:Function, T<:Union{AbstractMvNormal,GaussianVector}}
  basis::S
  distribution::T
  function GaussianProcess(basis::S, distribution::T) where {
        S<:AbstractArray{<:Function}, T<:Union{AbstractMvNormal,GaussianVector}}
      length(basis)==length(distribution) || throw(DimensionMismatch(
        "The basis and distribution are not of equal length."))
      new{S, T}(basis, distribution)
  end
end

# Tests need to be written.
"""
    calculateboundssupport(higestlevel::Int)

Internal function, not exported.

Returns a tuple of two Float64 vectors of length 2^(higestlevel+1)
where the i-th element of the first vector is the left bound of the
support of the i-th Faber-Schauder basis function and the i-th
element of the second vector is the right bound of the support of
i-th Faber-Schauder basis function.
"""
function calculateboundssupport(higestlevel::Int)
    higestlevel ≥ 0 || throw(AssertionError("higestlevel should be positive."))
    lengthvector = 2^(higestlevel+1)
    leftboundsupport = Vector{Float64}(undef, lengthvector)
    rightboundsupport = Vector{Float64}(undef, lengthvector)
    leftboundsupport[1] = leftboundsupport[2] = 0.0
    rightboundsupport[1] = rightboundsupport[2] = 1.0
    for j in 1:higestlevel
        for k in 1:2^j
            i = 2^j+k
            leftboundsupport[i] = ldexp(float(k-1), -j)
            rightboundsupport[i] = ldexp(float(k), -j)
        end
    end
    return (leftboundsupport, rightboundsupport)
end

"""
    createFaberSchauderBasisUpToLevelHigestLevel(higestlevel::Int)

Internal function, not exported.

Creates Faber-Schauder basis up to level higestlevel. Returns a
Vector{Function} type of length 2^(higestlevel+1). The first element
of the vector is faberschauderone, and the 2^j+k element is
faberschauder(j,k), with 0≤j≤higestlevel and 1≤k≤2^j.
"""
function createFaberSchauderBasisUpToLevelHigestLevel(higestlevel::Int)
    higestlevel≥0 || throw(AssertionError("The levels are non-negative."))
    # There are 2^(higestlevel+1) basis functions.
    basis = Vector{Function}(undef, 2^(higestlevel+1))
    basis[1] = faberschauderone
    for j in 0:higestlevel
        for k in 1:2^j
            basis[2^j+k] = faberschauder(j,k)
        end
    end
    return basis
end

"""
    struct FaberSchauderExpansionWithGaussianCoefficients{T} <:
            AbstractGaussianProcess where T<:Union{AbstractMvNormal,GaussianVector}
        higestlevel::Int64
        basis::Vector{Function}
        leftboundssupport::Vector{Float64}
        rightboundssupport::Vector{Float64}
        distribution::T
    end

Implements a Gaussian Process with Faber-Schauder functions and optimally exploits
the sparsity structure of Faber-Schauder functions.

Constructors:
    FaberSchauderExpansionWithGaussianCoefficients(higestlevel, distribution)
    FaberSchauderExpansionWithGaussianCoefficients(standarddeviationsperlevel::Vector{Float64})

The user is not allowed to set a basis, which the constructor calculates from the input and
is by its very definition a Faber-Schauder basis. The length of distribution
should be equal to 2^(higestlevel+1). The second constructor defines a Gaussian
process with Faber-Schauder basis with independent coefficients with
length(standarddeviationsperlevel)-1 levels (there is also a level zero), so
2^(length(standarddeviationsperlevel)) number of basis functions, where the standard deviation
of the coefficients belonging to level k is standarddeviationsperlevel[k+1].

# Example

```julia
α = 0.5
Π = FaberSchauderExpansionWithGaussianCoefficients([2^(-j*α) for j in 0:5])
f = rand(Π)
```
"""
struct FaberSchauderExpansionWithGaussianCoefficients{T} <:
         AbstractGaussianProcess where T<:Union{AbstractMvNormal, GaussianVector}
     higestlevel::Int64
     basis::Vector{Function}
     leftboundssupport::Vector{Float64}
     rightboundssupport::Vector{Float64}
     distribution::T
     function FaberSchauderExpansionWithGaussianCoefficients(higestlevel::Int64,
             distribution::T) where {T<:Union{AbstractMvNormal,GaussianVector}}
         higestlevel ≥ 0 || throw(AssertionError("higestlevel should be non-negative."))
         length(distribution) == 2^(higestlevel+1) || throw(AssertionError("The length of the
         distribution is not equal to 2^(higestlevel+1)."))
         basis = createFaberSchauderBasisUpToLevelHigestLevel(higestlevel)
         leftboundssupport, rightboundssupport = calculateboundssupport(
             higestlevel)
         new{T}(higestlevel, basis,
             leftboundssupport, rightboundssupport, distribution)
     end
end

"""
    createvectorofstandarddeviationsfromstandarddeviationsperlevel(standarddeviationsperlevel::AbstractArray{Float64})

Internal function, not exported!

Calculated a vector of standard deviations for all basis functions, where basis function i gets standard deviation
standarddeviationsperlevel[ℓ+1] if ℓ is the level of i (we start at level 0).
"""
function createvectorofstandarddeviationsfromstandarddeviationsperlevel(standarddeviationsperlevel::AbstractArray{Float64})
    lenghtstandarddeviationsperlevel = length(standarddeviationsperlevel)
    lenghtstandarddeviationsperlevel > 0 || throw(AssertionError("standarddeviationsperlevel is of zero length."))
    # The distribution has length 2^lenghtstandarddeviationsperlevel=2^(higestlevel+1).
    vectorofstandarddeviations = Vector{Float64}(undef, 2^lenghtstandarddeviationsperlevel)
    # There are two functions of level zero.
    vectorofstandarddeviations[1:2] .= standarddeviationsperlevel[1]
    # and 2^k of level k, k=1,2,...
    for k in 1:lenghtstandarddeviationsperlevel-1
        vectorofstandarddeviations[2^k+1:2^(k+1)] .= standarddeviationsperlevel[k+1]
    end
    return vectorofstandarddeviations
end

# Constructor
function FaberSchauderExpansionWithGaussianCoefficients(
        standarddeviationsperlevel::AbstractArray{Float64})
    lenghtstandarddeviationsperlevel = length(standarddeviationsperlevel)
    lenghtstandarddeviationsperlevel == 0 && throw(AssertionError("standarddeviationsperlevel is of
        zero length."))
    # We start with level zero.
    higestlevel = lenghtstandarddeviationsperlevel - 1
    vectorofstandarddeviations = createvectorofstandarddeviationsfromstandarddeviationsperlevel(standarddeviationsperlevel)
    distribution = MvNormal(vectorofstandarddeviations)
    return FaberSchauderExpansionWithGaussianCoefficients(higestlevel,
        distribution)
end

#Extends Base.length to objects of a subtypes of AbstractGaussianProcess.
"""
    length(Π::AbstractGaussianProcess)
    length(Π::FaberSchauderExpansionWithGaussianCoefficients)

length returns the number of basis functions, which is equal to the length of the distribution of the
coefficients, which is also equal to the number of coefficients.
For a FaberSchauderExpansionWithGaussianCoefficients object, the length is equal to 2^(higestlevel+1).

# Example
```julia
Π = GaussianProcess([sin, cos], MvNormal([1.,1.]))
length(Π)
#
Π = FaberSchauderExpansionWithGaussianCoefficients([2.0^(-0.5*j) for j in 0:3])
length(Π) # == 2^4 == 16.
```
"""
length(Π::AbstractGaussianProcess)=length(Π.basis)
length(Π::FaberSchauderExpansionWithGaussianCoefficients) = 2^(Π.higestlevel+1)

"""
    sumoffunctions(vectoroffunctions::AbstractArray{<:Function},
        vectorofscalars::AbstractArray{<:Number})

Internal function, not exported.

sumoffunctions calculates the 'inner product' of the function vector and the scalar vector. In
other words, the sum of the functions weighted by vectorofscalars. It returns a
function.
"""
function sumoffunctions(vectoroffunctions::AbstractArray{<:Function},
    vectorofscalars::AbstractArray{<:Number})
  n = length(vectoroffunctions)
  n == length(vectorofscalars) || throw(AssertionError("The vector of functions and the vector
    of scalars should be of equal length"))
  return x -> sum(vectorofscalars[k]*vectoroffunctions[k](x) for k in 1:n)
end

"""
    rand(Π::AbstractGaussianProcess)

rand returns a random function, where the coefficients have distribution Π.distribution
and the basis functions are defined in Π.basis.

# Example

```julia
using Distributions
distribution = MvNormal([k^(-1.0) for k in 1:100])
Π = GaussianProcess([fourier(k) for  k in 1:100], distribution)
f = rand(Π)
```
"""
function rand(Π::AbstractGaussianProcess)
  Z = rand(Π.distribution)
  return sumoffunctions(Π.basis, Z)
end

# Extends Base.mean to GaussianProcess objects.
"""
    mean(Π::AbstractGaussianProcess)

mean calculates the mean of the Gaussian process. It returns a function.

#Examples

```julia
using Distributions
distribution = MvNormal([k^(-1.0) for k in 1:100])
Π = GaussianProcess([fourier(k) for  k in 1:100], distribution)
mean(Π)
```
"""
mean(Π::AbstractGaussianProcess) = sumoffunctions(Π.basis, mean(Π.distribution))


# only linear PHS for now
# coupling using gyrator-type interconnection
function coupled_gyrator(ph1 :: Phs, ports1, ph2 :: Phs, ports2, couple_matrix)
	N1 = size(ph1.J,1); # number of states
	N2 = size(ph2.J,1);

	# split the input ports that are not used for interconnection
	# and creates the new B matrix:
	inputs1 = [];
	for input = 1:size(ph1.B,2)
		if ~(input in ports1)
			inputs1 = push!(inputs1,input);
		end
	end
	inputs2 = [];
	for input = 1:size(ph2.B,2)
		if ~(input in ports2)
			inputs2 = push!(inputs2,input);
		end
	end
	Bnew = blkdiag(ph1.B[:,inputs1],ph2.B[:,inputs2]);
	Bnew[1:N1,(length(inputs1)+1):end] = ph1.B[:,ports1]*couple_matrix*ph2.D[ports2,inputs2];
	Bnew[(N1+1):end, 1:length(inputs1)] = -ph2.B[:,ports2]*couple_matrix*ph1.D[ports1,inputs1] ;

	Jnew = blkdiag(ph1.J, ph2.J);
	Jnew[1:N1, (N1+1):end] = ph1.B[:,ports1]*couple_matrix*(transpose(ph2.B[:,ports2]))
	Jnew[(N1+1):end, 1:(N1)] = -ph2.B[:,ports2]*couple_matrix*(transpose(ph1.B[:,ports1]))

	Dnew = zeros(length(inputs1)+length(inputs2),length(inputs1)+length(inputs2));
	Dnew[1:length(inputs1), 1:length(inputs1)] = ph1.D[inputs1,inputs1]
	Dnew[length(inputs1)+1:end, length(inputs1)+1:end] = ph2.D[inputs2,inputs2]
	Dnew[1:length(inputs1), length(inputs1)+1:end] = ph1.D[inputs1,ports1]*couple_matrix*ph2.D[ports2, inputs2];
	Dnew[length(inputs1)+1:end, 1:length(inputs1)] = - ph2.D[inputs2, ports2]*couple_matrix*ph1.D[ports1,inputs1];
	Qnew = blkdiag((ph1.Q), (ph2.Q));
	phsnew = Phs(Jnew, Bnew, Dnew, Qnew)
	phsnew.TransfMatrix = blkdiag(ph1.TransfMatrix,ph2.TransfMatrix)

	# preserves the names of the states:
	N1 = size(ph1.TransfMatrix,1)
	for key in keys(ph2.StatesNames)
		ph2.StatesNames[key] += N1
	end
	phsnew.StatesNames = merge(ph1.StatesNames, ph2.StatesNames)

	# remove the names of the coupled inputs
	inp1 = copy(ph1.InputsNames)
	deleteat!(inp1, ports1);
	inp2 = copy(ph2.InputsNames)
	deleteat!(inp2, ports2);
	phsnew.InputsNames = [inp1; inp2]

	phsnew.R = blkdiag(ph1.R, ph2.R)
	return phsnew
end

function coupled_transformer(ph1 :: Phs, ports1, ph2 :: Phs, ports2, couple_matrix)
	N1 = size(ph1.J,1); # number of states
	N2 = size(ph2.J,1);

	# split the input ports that are not used for interconnection
	# and creates the new B matrix:
	inputs1 = [];
	for input = 1:size(ph1.B,2)
		if ~(input in ports1)
			inputs1 = push!(inputs1,input);
		end
	end
	inputs2 = [];
	for input = 1:size(ph2.B,2)
		if ~(input in ports2)
			inputs2 = push!(inputs2,input);
		end
	end
	Bnew = blkdiag(ph1.B[:,inputs1],ph2.B[:,inputs2]);
	#Bnew[1:N1,(length(inputs1)+1):end] = ph1.B[:,ports1]*couple_matrix*ph2.D[ports2,inputs2];
	#Bnew[(N1+1):end, 1:length(inputs1)] = -ph2.B[:,ports2]*couple_matrix*ph1.D[ports1,inputs1] ;

	Jnew = blkdiag(ph1.J, ph2.J);
	#Jnew[1:N1, (N1+1):end] = ph1.B[:,ports1]*couple_matrix*(ph2.B[:,ports2].')
	#Jnew[(N1+1):end, 1:(N1)] = -ph2.B[:,ports2]*couple_matrix*(ph1.B[:,ports1].')

	#Dnew = zeros(length(inputs1)+length(inputs2),length(inputs1)+length(inputs2));
	#Dnew[1:length(inputs1), length(inputs1)+1:end] = ph1.D[inputs1,ports1]*couple_matrix*ph2.D[ports2, inputs2];
	#Dnew[length(inputs1)+1:end, 1:length(inputs1)] = - ph2.D[inputs2, ports2]*couple_matrix*ph1.D[ports1,inputs1];
	G = [ph1.B[:,ports1] ; ph2.B[:,ports2]*couple_matrix]
	GD = ph1.D[ports1,ports1] - ph2.D[ports2,ports2]*couple_matrix
	Dnew = blkdiag(ph1.D[inputs1,inputs1], ph2.D[inputs2,inputs2])
	Qnew = blkdiag((ph1.Q), (ph2.Q));
	phnew = Phs(Jnew, Bnew, Dnew, Qnew)
	set_constraint!(phnew, G, GD)
	phnew.TransfMatrix = blkdiag(ph1.TransfMatrix,ph2.TransfMatrix)

	# preserves the names of the states:
	N1 = size(ph1.TransfMatrix,1)
	for key in keys(ph2.StatesNames)
		ph2.StatesNames[key] += N1
	end
	phnew.StatesNames = merge(ph1.StatesNames, ph2.StatesNames)
	phnew.R = blkdiag(ph1.R, ph2.R)
	return phnew
end


function _prepare_approximation(nodes_b::Matrix{T},
                                kernel::RK
                               ) where {T <: AbstractFloat, RK <: ReproducingKernel_0}

    spline = _prepare_approximation(nothing, nodes_b, kernel)
    return spline
end

function _prepare_approximation(nodes::Union{Matrix{T}, Nothing},
                                nodes_b::Matrix{T},
                                kernel::RK
                               ) where {T <: AbstractFloat, RK <: ReproducingKernel_0}

     if isnothing(nodes)
         n_1 = 0
         n = size(nodes_b, 1)
     else
        n = size(nodes, 1)
        n_1 = size(nodes, 2)
        if n != size(nodes_b, 1)
            error("Matrices 'nodes' and 'nodes_b' have mismatched dimensions.")
        end
     end

     n_b = size(nodes_b, 1)
     n_1_b = size(nodes_b, 2)

     min_bound = Vector{T}(undef, n)
     compression::T = T(0.)
     maxx::T = T(0.)
     @inbounds for i = 1:n
         if !isnothing(nodes)
             min_bound[i] = nodes[i,1]
             maxx = nodes[i,1]
         else
             min_bound[i] = nodes_b[i,1]
             maxx = nodes_b[i,1]
         end
         for j = 1:n_1
             min_bound[i] = min(min_bound[i], nodes[i,j])
             maxx = max(maxx, nodes[i,j])
         end
         for j = 1:n_1_b
             min_bound[i] = min(min_bound[i], nodes_b[i,j])
             maxx = max(maxx, nodes_b[i,j])
         end
         compression = max(compression, maxx - min_bound[i])
     end

     if compression <= eps(T(1.0))
         error("Cannot prepare the spline: `nodes` data are not correct.")
     end

     if !isnothing(nodes)
         t_nodes = similar(nodes)
     else
         t_nodes = nothing
     end
     @inbounds for j = 1:n_1
         for i = 1:n
             t_nodes[i,j] = (nodes[i,j] - min_bound[i]) / compression
         end
     end
     t_nodes_b = similar(nodes_b)
     @inbounds for j = 1:n_1_b
         for i = 1:n
             t_nodes_b[i,j] = (nodes_b[i,j] - min_bound[i]) / compression
         end
     end

     if !isnothing(nodes)
         t_nodes_all = [t_nodes t_nodes_b]
     else
         t_nodes_all = t_nodes_b
     end

     if T(kernel.ε) == T(0.0)
         ε = _estimate_ε(t_nodes_all)
         if isa(kernel, RK_H0)
             kernel = RK_H0(ε)
         elseif isa(kernel, RK_H1)
             ε *= T(1.5)
             kernel = RK_H1(ε)
         elseif isa(kernel, RK_H2)
             ε *= T(2.0)
             kernel = RK_H2(ε)
         else
             error("Incorrect `kernel` type.")
         end
     end

     gram = _gram(t_nodes_all, kernel)
     chol = nothing
     try
         chol = cholesky(gram)
     catch
         error("Cannot prepare the spline: Gram matrix is degenerate.")
     end

     cond = _estimate_cond(gram, chol)

     spline = NormalSpline(kernel,
                           compression,
                           t_nodes,
                           t_nodes_b,
                           nothing,
                           nothing,
                           nothing,
                           nothing,
                           nothing,
                           nothing,
                           nothing,
                           nothing,
                           nothing,
                           nothing,
                           min_bound,
                           gram,
                           chol,
                           nothing,
                           nothing,
                           cond,
                           -1
                          )
     return spline
end

function _construct_approximation(spline::NormalSpline{T, RK},
                                  values_lb::Vector{T},
                                  values_ub::Vector{T},
                                  maxiter::Int,
                                  ftol::T,
                                  cleanup::Bool = false
                                 ) where {T <: AbstractFloat, RK <: ReproducingKernel_0}

    if !isnothing(spline._nodes)
         error("Spline object is not correctly prepared ('spline._nodes' is not empty).")
    end

    spline, nit_done = _construct_approximation(spline, T[], values_lb, values_ub, maxiter, ftol, cleanup)
    return spline, nit_done
end

function _construct_approximation(spline::NormalSpline{T, RK},
                                  values::Vector{T},
                                  values_lb::Vector{T},
                                  values_ub::Vector{T},
                                  maxiter::Int,
                                  ftol::T,
                                  cleanup::Bool = false
                                 ) where {T <: AbstractFloat, RK <: ReproducingKernel_0}

    if isnothing(spline._nodes) && isnothing(spline._nodes_b)
        error("Spline object is not correctly prepared ('spline._nodes' is empty and 'spline._nodes_b' is empty).")
    end
    if isnothing(spline._nodes_b)
        error("Spline object is not correctly prepared ('spline._nodes_b' is empty).")
    end

    m1 = 0
    if !isnothing(spline._nodes)
        m1 = size(spline._nodes, 2)
    end
    m2 = size(spline._nodes_b, 2)

    if length(values) != m1
        error("Number of 'values' does not correspond to the number of interpolating nodes ('spline._nodes').")
    end
    if length(values_lb) != m2
        error("Number of 'values_lb' does not correspond to the number of approximating nodes ('spline._nodes_b').")
    end
    if length(values_ub) != m2
        error("Number of 'values_ub' does not correspond to the number of approximating nodes.")
    end

    @inbounds for i = 1:m2
        if values_ub[i] < values_lb[i]
            error("Incorrect bounds: 'values_ub[$i]' is less than 'values_lb[$i]'.")
        end
        if (values_ub[i] - values_lb[i]) <= sqrt(eps(T(1.0)))
            error("Incorrect bounds: 'values_ub[$i]' is almost equal to 'values_lb[$i]'.")
        end
     end

    if isnothing(spline._chol)
        error("Gram matrix was not factorized.")
    end

    umax = T(0.)
    @inbounds for i = 1:m2
        val = abs(values_lb[i])
        if val != Inf && val > umax
            umax = val
        end
        val = abs(values_ub[i])
        if val != Inf && val > umax
            umax = val
        end
    end

    ineq_active = zeros(Int, m2)

    values_b  = Vector{T}(undef, m2)
    incorrect_bounds::Bool = false
    @inbounds for i = 1:m2
        if abs(values_lb[i]) == Inf && abs(values_ub[i]) == Inf
            incorrect_bounds = true
            break
        end
        if abs(values_lb[i]) != Inf && abs(values_ub[i]) != Inf
            values_b[i] = (values_lb[i] + values_ub[i]) / T(2.0)
        end
        if abs(values_lb[i]) == Inf
            values_b[i] = values_ub[i] - umax
        end
        if abs(values_ub[i]) == Inf
            values_b[i] = values_lb[i] + umax
        end
    end
    if incorrect_bounds
        error("Incorrect_bounds. Both 'values_ub[i]' and 'values_lb[i]' are set to Inf.")
    end

    if m1 > 0
        values_all = [values; values_b]
    else
        values_all = values_b
    end

    mu = Vector{T}(undef, size(spline._gram, 1))
    ldiv!(mu, spline._chol, values_all)

    spline = NormalSpline(spline._kernel,
                          spline._compression,
                          spline._nodes,
                          spline._nodes_b,
                          values,
                          values_lb,
                          values_ub,
                          nothing,
                          nothing,
                          nothing,
                          nothing,
                          nothing,
                          nothing,
                          nothing,
                          spline._min_bound,
                          spline._gram,
                          spline._chol,
                          mu,
                          nothing,
                          spline._cond,
                          -1
                         )

    precision = T(1.e-10) # TODO change

    spline, nit_done = _qp(spline, ineq_active, maxiter, ftol, precision, cleanup)
    return spline, nit_done
end

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

function _evaluate(spline::NormalSpline{T, RK}, points::Matrix{T},
                  ) where {T <: AbstractFloat, RK <: ReproducingKernel_0}
    if !isnothing(spline._nodes_b) || !isnothing(spline._d_nodes_b)
        _evaluate_approximation(spline, points)
    else
        _evaluate_interpolation(spline, points)
    end
end

function _evaluate_gradient(spline::NormalSpline{T, RK},
                            point::Vector{T}
                           ) where {T <: AbstractFloat, RK <: ReproducingKernel_0}
    if !isnothing(spline._nodes_b) || !isnothing(spline._d_nodes_b)
        _evaluate_approximation_gradient(spline, point)
    else
        _evaluate_interpolation_gradient(spline, point)
    end
end

function _evaluate_approximation_gradient(spline::NormalSpline{T, RK},
                                          point::Vector{T}
                                         ) where {T <: AbstractFloat, RK <: ReproducingKernel_0}
    # TODO - implement
    error("_evaluate_approximation_gradient: Not implemented.")
end

function _evaluate_approximation(spline::NormalSpline{T, RK}, points::Matrix{T},
                                ) where {T <: AbstractFloat, RK <: ReproducingKernel_0}
    if isnothing(spline)
        error("Spline was not prepared.")
    end
    if isnothing(spline._mu)
        error("Spline was not constructed.")
    end

    if isnothing(spline._ier < 0)
        error("Spline coefficients are not calculated.")
    end

    if size(points, 1) != size(spline._nodes_b, 1)
        if size(points, 1) == 1 && size(points, 2) > 1
            error("Incorrect first dimension of the `points` parameter (use 'evaluate_at' function for evaluating the spline at one point).")
        else
            error("Incorrect first dimension of the `points` parameter (the spline was built in the space of different dimension).")
        end
    end

    if isnothing(spline._nodes)
       n_1 = 0
    else
       n_1 = size(spline._nodes, 2)
    end

    n = size(spline._nodes_b, 1)
    n_1_b = size(spline._nodes_b, 2)
    m = size(points, 2)

    pts = similar(points)
    @inbounds for j = 1:m
        for i = 1:n
            pts[i,j] = (points[i,j] - spline._min_bound[i]) / spline._compression
        end
    end

    spline_values = Vector{T}(undef, m)
    h_values = Vector{T}(undef, n_1)

    if spline._ier != 0 #  optimal solution not found

        h_values_b = Vector{T}(undef, n_1_b)
        @inbounds for p = 1:m
            for i = 1:n_1_b
                h_values_b[i] = _rk(spline._kernel, pts[:,p], spline._nodes_b[:,i])
            end
            if n_1 > 0
                for i = 1:n_1
                    h_values[i] = _rk(spline._kernel, pts[:,p], spline._nodes[:,i])
                end
                spline_values[p] = sum(spline._mu .* [h_values; h_values_b; -h_values_b])
            else
                spline_values[p] = sum(spline._mu .* [h_values_b; -h_values_b])
            end
        end
    else # optimal solution found
        nak = length(spline._active)
        h_values_b = Vector{T}(undef, nak)
        mu_b = Vector{T}(undef, nak)
        @inbounds for p = 1:m
            spline_values[p] = T(0.)
            if n_1 > 0
                for i = 1:n_1
                    h_values[i] = _rk(spline._kernel, pts[:,p], spline._nodes[:,i])
                end
                spline_values[p] += sum(spline._mu[1:n_1] .* h_values)
            end

            for i = 1:nak
                ii = spline._active[i]
                if ii > 0
                    mu_b[i] = spline._mu[ii+n_1]
                    h_values_b[i] = _rk(spline._kernel, pts[:,p], spline._nodes_b[:,ii])
                    s = 0
                else
                    ii = -ii
                    mu_b[i] = spline._mu[ii+n_1+n_1_b]
                    h_values_b[i] = -_rk(spline._kernel, pts[:,p], spline._nodes_b[:,ii])
                    s = 0
                end
            end
            spline_values[p] += sum(mu_b .* h_values_b)
        end
    end # if spline._ier != 0

    return spline_values
end


module TestClustering

# using Revise
using MLJBase
using Test
using Random:seed!
import LinearAlgebra: norm
import Distances: evaluate

# load code to be tested:
import MLJModels
import Clustering # MLJModels.Clustering_ now available for loading
using MLJModels.Clustering_

seed!(132442)

task = load_crabs()

X, y = X_and_y(task)

####
#### KMEANS
####

barekm = KMeans()

fitresult, cache, report = MLJBase.fit(barekm, 1, X)

R = MLJBase.matrix(MLJBase.transform(barekm, fitresult, X))

X_array = MLJBase.matrix(X)

# distance from first point to second center
@test R[1, 2] ≈ norm(view(X_array, 1, :) .- view(fitresult, :, 2))^2
@test R[10, 3] ≈ norm(view(X_array, 10, :) .- view(fitresult, :, 3))^2

p = MLJBase.predict(barekm, fitresult, X)

@test argmin(R[1, :]) == p[1]
@test argmin(R[10, :]) == p[10]

# km = machine(barekm, X)
# fit!(km)

info(barekm)

####
#### KMEDOIDS
####

barekm = KMedoids()

fitresult, cache, report = MLJBase.fit(barekm, 1, X)

R = MLJBase.matrix(MLJBase.transform(barekm, fitresult, X))

@test R[1, 2] ≈ evaluate(barekm.metric, view(X_array, 1, :), view(fitresult, :, 2))
@test R[10, 3] ≈ evaluate(barekm.metric, view(X_array, 10, :), view(fitresult, :, 3))

p = MLJBase.predict(barekm, fitresult, X)

@test all(report.assignments .== p)

# km = machine(barekm, X)
# fit!(km)

info(barekm)

end # module
true


using SumOfSquaresOptimization
using Base.Test

objtol = 1e-4

function equals_upto_perm(a,b)
    if length(a) != length(b)
        return false
    end
    mapreduce(p -> b[p] == a, Base.OrFun(), permutations(1:length(b)))
end
@test equals_upto_perm([1,2],[2,1])

# UNIT TESTS

# monomials & polynomials
a = Dict( :x1 => 3, :x2 => 2 )
b = Dict( :x1 => 1, :x3 => 5 )
c = Dict( :x1 => 4, :x2 => 2, :x3 => 5 )
@test a * b == c
@test SumOfSquaresOptimization.deg(a) == 5
p = Dict(a=>1.0, b=>2.0)
@test Dict(a=>1.0) + Dict(b=>2.0) == p
@test SumOfSquaresOptimization.deg(p) == 6
@test equals_upto_perm(SumOfSquaresOptimization.monoms([:x1,:x2],2), [ Dict(:x1=>2), Dict(:x1=>1, :x2=>1), Dict(:x2=>2) ])
@test SumOfSquaresOptimization.monoms(0) == [SumOfSquaresOptimization.SoSMonom()]
@test SumOfSquaresOptimization.monoms(3) == Array{SumOfSquaresOptimization.SoSMonom,1}()
@test equals_upto_perm(SumOfSquaresOptimization.decomp(Dict(:x1=>4),3), [ (Dict(:x1=>3),Dict(:x1=>1)), (Dict(:x1=>2),Dict(:x1=>2)), (Dict(:x1=>1),Dict(:x1=>3)) ])

# parsing
@test SumOfSquaresOptimization.parsepoly(Program(), parse("x1^3 * x2^2")) == Dict(a => 1.0)
@test SumOfSquaresOptimization.parsepoly(Program(), parse("x1^3 * x2^2 + 2.0 * x1 * x3^5")) == p

# END-TO-END TESTS

# unconstrained minimization: a quadratic bowl
prog = Program(minimize=true)
objective(prog, "x^2 + y^2 - 1")
sol = sossolve(prog,2)
@test status(sol) == :Normal
@test abs(primalobj(sol) + 1) < objtol
@test_throws ArgumentError moment(sol,"x^4")

# unconstrained, unbounded maximization
prog = Program(maximize=true)
objective(prog, "x^2 + y^2 - 1")
sol = sossolve(prog,2)
@test status(sol) == :Unbounded
@test isinf(primalobj(sol))
@test !signbit(primalobj(sol))
@test_throws ArgumentError moment(sol,"x")
@test_throws ArgumentError dumpsol(sol)

# min bisection in the 6-cycle
prog = Program(minimize=true)
for i in 1:6
    for j in [-1,1]
        jp = (i + j + 2*6 - 1) % 6 + 1
        @partobjective(prog, "x%d * (1-x%d)", i, jp)
    end
    @constraint(prog, "x%d^2 == x%d", i, i)
    @partconstraint(prog, :bisect, "x%d", i)
end
@partconstraint(prog, :bisect, "-%d", 6/2)

sol = sossolve(prog,2)
@test status(sol) == :Normal
@test abs(primalobj(sol) - 1.5) < objtol

# test dihedral symmetry on the same program
symmetrize_dihedral!(prog, [symbol("x$i") for i in 1:6])
sol = sossolve(prog,2)
@test status(sol) == :Normal
@test abs(primalobj(sol) - 1.5) < objtol
sol = sossolve(prog,4)
@test status(sol) == :Normal
@test abs(primalobj(sol) - 2.0) < objtol


#-----------------------------------------------------------------------
#   COMBINATION TEST
#-----------------------------------------------------------------------

for i=4:8
    @test length(combX( rand(4,i) ))    ≈   binomial(i,4)
    @test size(combX(rand(4,i))[1])     == size(rand(4,4))
    @test typeof(combX(rand(4,i))[1])   == typeof(rand(4,4))
end

#-----------------------------------------------------------------------
#   OUTLIER DETECTION TEST
#-----------------------------------------------------------------------

V = [2*(rand(4)/2-ones(4)) for _ in 1:10]
V[2] = (10^8)*2*(rand(4)/2-ones(4))
V[7] = (10^4)*ones(4)/2
V[8] = (10^6)*2*(rand(4)/2-ones(4))

OTL = odetc(V)

Vs  = V[OTL[2]]

ξ=1e1

@test OTL[2][8:10] == [7;8;2]

for i=1:7
    @test norm(Vs[i]) < norm(Vs[8])/ξ
    @test norm(Vs[i]) < norm(Vs[9])/ξ
    @test norm(Vs[i]) < norm(Vs[10])/ξ
end


function post(alg::TMJets21a{N}, ivp::IVP{<:AbstractContinuousSystem}, timespan;
              Δt0::TimeInterval=zeroI,
              kwargs...) where {N}

    @unpack orderQ, orderT, abstol, maxsteps, adaptive, minabstol, absorb, disjointness = alg

    # initial time and final time
    t0 = tstart(timespan)
    T = tend(timespan)

    # vector field
    if islinear(ivp) || isaffine(ivp) # TODO: refactor with inplace_field!
        f! = inplace_field!(ivp)
    else
        f! = VectorField(ivp)
    end

    # algorithm optional arguments
    parse_eqs = get(kwargs, :parse_eqs, false)
    params = get(kwargs, :params, nothing)
    check_property = get(kwargs, :check_property, (t, x) -> true)

    n = statedim(ivp)
    ivp_norm = _normalize(ivp)
    X = stateset(ivp_norm)

    # fix the working variables and maximum order in the global
    # parameters struct (_params_TaylorN_)
    set_variables("x", numvars=n, order=2*orderQ)

    # initial set
    X0 = initial_state(ivp_norm)
    X0tm = _initialize(X0, orderQ, orderT)

    # optionally absorb initial remainder
    shrink_wrapping = get(kwargs, :shrink_wrapping, true)
    if shrink_wrapping && isa(X0tm, TaylorModelReachSet)
        if !all(iszero, remainder(X0tm))
            X0tm = _shrink_wrapping(X0tm)
        end
    end

    # call external solver
    TMSol = TaylorModels.validated_integ(f!, X0tm, t0, T, orderQ, orderT,
                                                 abstol, params;
                                                 maxsteps=maxsteps,
                                                 parse_eqs=parse_eqs,
                                                 adaptive=adaptive,
                                                 minabstol=minabstol,
                                                 absorb=absorb,
                                                 check_property=check_property)
    tv = TMSol.time
    xv = TMSol.fp
    xTM1v = TMSol.xTM

    # preallocate flowpipe
    F = Vector{TaylorModelReachSet{N}}()
    sizehint!(F, maxsteps)

    # loop over reach-sets (the first reach-set at the initial time point is ignored)
    @inbounds for i in 2:length(tv)

        # create Taylor model reach-set
        δt = TimeInterval(tv[i] + domain(xTM1v[1, i]))
        Ri = TaylorModelReachSet(xTM1v[:, i], δt + Δt0)

        # check intersection with invariant
        _is_intersection_empty(Ri, X, disjointness) && break

        push!(F, Ri)
    end

    ext = Dict{Symbol, Any}(:tv => tv, :xv => xv, :xTM1v => xTM1v)
    return Flowpipe(F, ext)
end


# y: data
# yhat_generator: function that samples data given the parameters
# algo parameters: Namedtuple of algorithm parameters
# N_samples: number of samples desired
function ABC_MCMC(y,yhat_generator,algo_parameters,N_samples)
    CPUtic()

    proposal = algo_parameters[:proposal]
    proposalRatio = algo_parameters[:proposalRatio]
    sd = algo_parameters[:sd]
    eta_y = algo_parameters[:eta](y)
    size_y = size(y)
    q = length(algo_parameters[:prior])
    thinning_interval = algo_parameters[:thinning]
    N_samples = N_samples + algo_parameters[:burn_in]
    result = zeros(N_samples,q)
    dist = Inf
    p_prev=Tuple(zeros(q))

    # initial point using ABC
    while (dist > algo_parameters[:epsilon])
        p_prev = rand.(algo_parameters[:prior])
        yhat = yhat_generator(p_prev)
        if size_y != size(yhat)
            continue
        end
        dist = algo_parameters[:d](eta_y, algo_parameters[:eta](yhat))
    end

    naccept=0
    k = 1
    i = 0
    p_cand = Tuple(zeros(q))
    while i <= (N_samples*thinning_interval)
        # propose candidate parameters in log space
        valid = true
        while valid
            p_cand = proposal(p_prev, sd)
            valid = any(p_cand .< 0)
        end
        # p_cand = proposal(p_prev, sd)

        # p_cand = kernel(p_prev, sd)[1]
        #generate data
        yhat = yhat_generator(p_cand)
        # yhat = yhat_generator( p_cand)
        if size_y != size(yhat)
            continue
        end
        i += 1

        dist = algo_parameters[:d](eta_y, algo_parameters[:eta](yhat))

        u = rand(Uniform(0,1))

        proposal_ratio = proposalRatio_Normal(p_prev,p_cand,sd)

        log_prior_prev = sum(logpdf.(algo_parameters[:prior], p_prev))
        log_prior_cand = sum(logpdf.(algo_parameters[:prior], p_cand))

        log_alpha = log_prior_cand + sum(log.(proposal_ratio)) - log_prior_prev
        if (i % thinning_interval==0)
            if (log(u) < log_alpha && dist < algo_parameters[:epsilon])
                result[k,:] .= p_cand
                p_prev = p_cand
                naccept += 1
            else
                result[k,:] .= p_prev
            end
            k += 1
        end
    end
    if algo_parameters[:verbose]
        @printf("acceptance rate=%f\n", naccept/N_samples)
    end
    if algo_parameters[:burn_in] > 0
        result = result[(1+algo_parameters[:burn_in]):end,:]
    end
    t_end = CPUtoc_modified(false)

    return result,t_end
end


import CLBLAS

const clblas = CLBLAS
clblas.setup()

using OpenCL: cl

device, ctx, queue = clblas.get_next_compute_context()

N = Unsigned(5)
data = Vector{Float32}(5)
data[1] = 1.1
data[2] = 2.22
data[3] = 3.333
data[4] = 4.4444
data[5] = 5.55555

bufX = cl.Buffer(Float32, ctx, (:r, :copy),hostbuf=data)
bufAsum = cl.Buffer(Float32, ctx, :w, length(bufX))
scratchBuff = cl.Buffer(Float32, ctx, :rw, length(bufX))

offx = Unsigned(0)
incx = cl.cl_int(1)
ncq = cl.cl_uint(1)
clq = queue.id
newl = cl.cl_uint(0)

event = cl.UserEvent(ctx, retain=true)
ptrEvent = [event.id]

clblas.clblasSasum( N, bufAsum.id, 0, bufX.id, 0, incx, scratchBuff.id,
                                ncq, [clq], newl, C_NULL, ptrEvent);

cl.api.clWaitForEvents(cl.cl_uint(1), ptrEvent)

result = Vector{Float32}(length(1))
cl.enqueue_read_buffer(queue, bufAsum, result, Unsigned(0), nothing, true)

expected = data[1:end] |> sum
println(result)
println("------------")
println(expected)

try
    @assert(isapprox(norm(expected - result[1]), zero(Float32)))
    info("success!")
finally
    clblas.teardown()
end


# solve Problem Set 6 Problem 1
#using ArrayViews  # non-copying subarray package
using PETSc

function driver()
  xmin = 0
  xmax = 1
  N = 50  # N+1 = # of grid points per processor
  mpi_rank = MPI.Comm_rank(MPI.COMM_WORLD) + 1
  mpi_size = MPI.Comm_size(MPI.COMM_WORLD)

  Npts_global = (mpi_size -1)*(N+1) + N + 2 - 2
  delta_x = (xmax - xmin)/(Npts_global)
  delta_t = delta_x

  tmax = 4.0

  solve(xmin, xmax, tmax, N, delta_t)
end

function solve(xmin, xmax, tmax, N, delta_t)
# solves the advection-diffusion equation using centered differences in space
# Crank-Nicolson in time
# Dirchlet boundary condition on left, Neumann on the right
# Method of Manufactured Solutions used to verify convergence rate

# Parameters:
# xmin = minimum x coordinate
# xmax = maximum x coordinate
# tmax = maximum time value
# N : N+1 =  number of x points
# r = delta_t/delta_x^2
# sigma = delta_t/delta_x

# Helper Functions:
# ICFunc = initial condition function with signature val = ICFunc(x)
# BCL = left boundary condition function with signature val = BCL(t)
# BCR = right boundary condition function
# source = source function, signature val = source(x, t)
# this function assumes no ghost point on the left, ghost point on the right

# calculate some values
mpi_rank = MPI.Comm_rank(MPI.COMM_WORLD) + 1
mpi_size = MPI.Comm_size(MPI.COMM_WORLD)

if mpi_rank != mpi_size
  mat_size = N+1
else  # last segment has ghost point
  mat_size = N+2
end

Npts_global = (mpi_size -1)*(N+1) + N + 2 - 2
delta_x = (xmax - xmin)/Npts_global

r = delta_t/(delta_x^2)
sigma = delta_t/delta_x
nStep = convert(Int, div(tmax, delta_t))

if mpi_rank == 1
println("tmax = ", tmax)
println("delta_x = ", delta_x)
println("delta_t = ", delta_t)
println("r = ", r)
println("sigma = ", sigma)
println("nStep = ", nStep)
end

# get the Petsc objects
A, u_i, u_ghost, vec_scatter, rhs, ksp, uex, ctx, ghost_offset = createPetscData(mat_size, 3)

# get the ranges to iterate over
idx_range = localpart(u_i)
idx_start = idx_range[1]
idx_end = idx_range[end]
interior_part = (idx_start+1):(idx_end-1)

# apply IC
# Not applying BCL at initial condition
for i in localpart(u_i)
  u_i[i] = ICFunc(xmin + (i-1)*delta_x)
end
# assemble so we can access the values in u_i for the Neumann BC
GridapDistributedPETScWrappers.AssemblyBegin(u_i)
GridapDistributedPETScWrappers.AssemblyEnd(u_i)

if mpi_rank == mpi_size
  u_i[idx_end] = 2*delta_x*BCR(0) + u_i[idx_end-2]
end

# scatter the initial condition u_i to u_ghost, so all procs have the values
# needed to evaluate their stencils
scatter!(vec_scatter, u_i, u_ghost)

# populate LHS matrix A
# do left BC
if mpi_rank == 1
#  println("applying left BC")
  A[1, 1] = 1
end

# do right BC
if mpi_rank == mpi_size
  A[idx_end, idx_end] = 1/(2*delta_x)
  A[idx_end, idx_end-2] = -1/(2*delta_x)  # this BC makes A not Tridiagonal
end

# to matrix interior
stencil_l = -r/2 - sigma/4
stencil_c = 1 + r
stencil_r = -r/2 + sigma/4
for i in interior_part
  A[i, i-1] = stencil_l
  A[i, i] = stencil_c
  A[i, i+1] = stencil_r
end

# do boundary nodes of each proc
if mpi_rank > 1  # all processes except leftmost
#  println("p$mpi_rank doing idx_start")
  A[idx_start, idx_start-1] = stencil_l
  A[idx_start, idx_start] = stencil_c
  A[idx_start, idx_start+1] = stencil_r
end
if mpi_rank != mpi_size # all processes except rightmost
  A[idx_end, idx_end-1] = stencil_l
  A[idx_end, idx_end] = stencil_c
  A[idx_end, idx_end+1] = stencil_r
end

# create rhs stencil
stencil_l = r/2 + sigma/4
stencil_c = 1 - r
stencil_r = r/2 - sigma/4

print("\n")
time = @elapsed for tstep=1:nStep  # loop over timesteps
# advance from timestep tstep to tstep + 1

  if mpi_rank == 1
    println("tstep = ", tstep)
  end

  for i in interior_part
    u_k = u_ghost[ i + ghost_offset ]
    u_k_1 = u_ghost[ i-1 + ghost_offset ]
    u_k_p1 = u_ghost[ i+1 + ghost_offset ]
    src_val = source(xmin + (i-1)*delta_x, (tstep - 0.5)*delta_t)
    rhs[i] = stencil_l*u_k_1 + stencil_c*u_k  + stencil_r*u_k_p1 + delta_t*src_val
  end

  if mpi_rank > 1  # all processes except leftmost
    i = idx_start
    u_k = u_ghost[i + ghost_offset]
    u_k_1 = u_ghost[i-1 + ghost_offset]
    u_k_p1 = u_ghost[i+1 + ghost_offset]
    src_val = source(xmin + (i-1)*delta_x, (tstep - 0.5)*delta_t)
    rhs[i] = stencil_l*u_k_1 + stencil_c*u_k  + stencil_r*u_k_p1 + delta_t*src_val
  end

  if mpi_rank < mpi_size  # all processes except rightmost
    i = idx_end
    u_k = u_ghost[i + ghost_offset]
    u_k_1 = u_ghost[i-1 + ghost_offset]
    u_k_p1 = u_ghost[i+1 + ghost_offset]
    src_val = source(xmin + (i-1)*delta_x, (tstep - 0.5)*delta_t)
    rhs[i] = stencil_l*u_k_1 + stencil_c*u_k  + stencil_r*u_k_p1 + delta_t*src_val
  end

  # apply BC terms to rhs
  if mpi_rank == 1
    rhs[1] = BCL(tstep*delta_t)
  end
  if mpi_rank == mpi_size
    rhs[idx_end] = BCR(tstep*delta_t)
  end

  # solve for next time step, u_i gets overwritten with new solution
  A_ldiv_B!(ksp, rhs, u_i)

  # scatter u_i into u_ghost, which is used to calculate the rhs
  # this is necessary because Petsc does not allow fetching off process values
  scatter!(vec_scatter, u_i, u_ghost)

end

println("time = ", time)

# print numerical solution
GridapDistributedPETScWrappers.AssemblyBegin(u_i)
GridapDistributedPETScWrappers.AssemblyEnd(u_i)
if mpi_rank == 1
  println("u_final = \n")
end
petscview(u_i)

# get exact solution
for i in localpart(uex)
  uex[i] = uExact( (i-1)*delta_x, delta_t*nStep)
end

# print exact solution
GridapDistributedPETScWrappers.AssemblyBegin(uex)
GridapDistributedPETScWrappers.AssemblyEnd(uex)
if mpi_rank == 1
  println("u_ex = \n")
end
petscview(uex)

# calculate error
u_err = uex - u_i
max_err = norm(u_err, Inf)
sleep(2)
if mpi_rank == 1
  println("max error = ", max_err)
end

writeU(u_i, N, nStep)
MPI.Barrier(MPI.COMM_WORLD)

return u_i, delta_t*(nStep)

end

function writeU(u_i, N, tsteps)

  mpi_rank = MPI.Comm_rank(MPI.COMM_WORLD) + 1
  larr = LocalArray(u_i)
  fname = string("u_", tsteps, "_", mpi_rank, ".dat")
  writedlm(fname, larr[1:(N+1)])
end

function ICFunc(x)
  return 2*cos(3*x)
end

function BCZero(x)
  return 0.0
end

function BCL(t)
  return 2*cos(t)
end

function BCR(t)
  return -6*sin(3)*cos(t)
end

function source(x, t)
  return -2*cos(3*x)*sin(t) - 6*sin(3x)*cos(t) + 18*cos(3*x)*cos(t)
end

function uExact(x, t)
  return 2*cos(3*x)*cos(t)
end

function createPetscData(mat_size, stencil_size)
# create all the Petsc objects needed

  mpi_rank = MPI.Comm_rank(MPI.COMM_WORLD) + 1
  mpi_size = MPI.Comm_size(MPI.COMM_WORLD)

  # left hand side matrix
  A = GridapDistributedPETScWrappers.Mat(Float64, GridapDistributedPETScWrappers.C.PETSC_DECIDE, GridapDistributedPETScWrappers.C.PETSC_DECIDE, mlocal=mat_size, nlocal=mat_size, nz=stencil_size, onz=1, comm=MPI.COMM_WORLD)
  A.assembling=true

  # solution at current timestep
  u_i = Vec(Float64, GridapDistributedPETScWrappers.C.VECMPI, comm=MPI.COMM_WORLD);
  u_i.assembling=true
  resize!(u_i, mlocal=mat_size)

  # right hand side
  rhs = Vec(Float64, GridapDistributedPETScWrappers.C.VECMPI, comm=MPI.COMM_WORLD);
  rhs.assembling=true
  resize!(rhs, mlocal=mat_size)

  # exact solution
  uex = Vec(Float64, GridapDistributedPETScWrappers.C.VECMPI, comm=MPI.COMM_WORLD);
  uex.assembling=true
  resize!(uex, mlocal=mat_size)

  # create a ghost vector that includes all points needed for each proc
  # to evaluate the rhs, duplicating the shared points.
  # Set up scattering from u_i to this vector

  # get global indices of local part of u_i
  idx_range = localpart(u_i)
  start_idx = idx_range[1] - 1
  end_idx = idx_range[end] - 1
  # figure out the indices in u_i corresponding to u_ghost
  if mpi_rank == 1  # left boundary (Dirchlet BC)
    idx = collect(start_idx:(end_idx+1))
    ghost_startidx = 0
    ghost_endidx = mat_size
    idx_ghost = collect(ghost_startidx:ghost_endidx)
  elseif mpi_rank == mpi_size  # right boundary (Neumann BC)
    idx = collect( (start_idx-1):end_idx)
    ghost_startidx = mat_size + (mpi_size-2)*(mat_size + 1)
    ghost_endidx = ghost_startidx + mat_size
    idx_ghost = collect(ghost_startidx:ghost_endidx)
  else  # interior
    idx = collect((start_idx-1):(end_idx+1))
    ghost_startidx = mat_size + 1 + (mpi_rank -2)*(mat_size + 2)
    ghost_endidx = ghost_startidx + mat_size + 1
    idx_ghost = collect(ghost_startidx:ghost_endidx)
  end
  # bump everything up to 1-based indexing
  idx += 1
  idx_ghost += 1

  ghost_offset = 2*(mpi_rank-1) # offset from u_i indices to ghost indices
  # set up the vector
  nghost_local = length(idx_ghost)
  u_ghost = Vec(Float64, GridapDistributedPETScWrappers.C.VECMPI, comm=MPI.COMM_WORLD);
  resize!(u_ghost, mlocal=nghost_local)
  u_ghost.assembling=true

  # create the scatter
  is_local = GridapDistributedPETScWrappers.IS(Float64, idx, comm=MPI.COMM_WORLD)
  is_ghost = GridapDistributedPETScWrappers.IS(Float64, idx_ghost, comm=MPI.COMM_WORLD)
  vec_scatter = GridapDistributedPETScWrappers.VecScatter(u_i, is_local, u_ghost, is_ghost)

  ctx = (is_local, is_ghost)  # avoid GC
  ksp = GridapDistributedPETScWrappers.KSP(A, ksp_atol=1e-12, ksp_rtol=1e-14, ksp_monitor="")

  return A, u_i, u_ghost, vec_scatter, rhs, ksp, uex, ctx, ghost_offset
end

# run
driver()


####
# Helpers
###
# Contains statistical and other routines used by data-driven sets
using Distributions, Roots, JuMP, Optim

export boot, calcMeansT, calcSigsBoot
export boot_mu, boot_sigma, bootDY_mu, bootDY_sigma, calc_ab_thresh

###Bootstrapping code
# ideally this should all be moved to some base level function
function boot(data::Vector, fun::Function, prob::Float64, numBoots::Int, f_args...)
	const N = size(data, 1)
	dist = DiscreteUniform(1, N)
	out = zeros(Float64, numBoots)
	indices = collect(1:N)
	for i = 1:numBoots
		rand!(dist, indices)
		out[i] = fun(data[indices], f_args...)
	end
	quantile(out, prob)
end

function boot(data::Matrix, fun::Function, prob::Float64, numBoots::Int, f_args...)
	const N = size(data, 1)
	dist = DiscreteUniform(1, N)
	out = zeros(Float64, numBoots)
	indices = collect(1:N)
	for i = 1:numBoots
		rand!(dist, indices)
		out[i] = fun(data[indices, :], f_args...)
	end
	quantile(out, prob)
end

#search for a valid bracket multiplicatively
#primarily used to prep for root-finding
function bracket(fun, guess; max_iter=32, ratio=2., trace=false)
    low = guess/ratio
    hi  = guess * ratio

    f_low  = fun(low)
    f_hi = fun(hi)
    iter = 0
    trace && println("Bracketing:")
    while((f_low * f_hi >= 0) && (iter < max_iter))
        trace && println("$iter \t $low \t $hi \t $f_low \t $f_hi")
        low /= ratio
        hi  *= ratio
        f_low  = fun(low)
        f_hi = fun(hi)
        iter += 1
    end
    iter == max_iter && error("Bracketing Failed")

    if f_hi * fun(hi/ratio) <= 0
        return hi/ratio, hi
    elseif f_low * fun(low*ratio) <= 0
        return low, low*ratio
    else
        error("iter $iter \t $f_low \t $f_hi")
    end
end

#Assumes both, joint
function calcSigsBoot(data, alpha, numBoots)
    sigf = calcSigsBoot(data, alpha/2., numBoots, :Fwd)
    sigb = calcSigsBoot(data, alpha/2., numBoots, :Back)
    sigf, sigb
end

#####
# b = maximum(data) if x > 0, b= minimum(data) otherwse
function f_sig(x, mu_adj, data_b, b)
    expdata = exp(x*data_b)
    logmeanexp = log(mean(expdata))
    sqrt(-2mu_adj/x + 2/x^2 * logmeanexp)
end

#approximate deriv or rootfinding
#muadj and data_b are already adjusted for numerical stability
function df_sig(x, mu_adj, data_b)
    expdata = exp(x*data_b)
    logmeanexp = log(mean(expdata))
    wght_mean = dot(expdata, data_b) / sum(expdata)
    (x*(wght_mean + mu_adj) -2logmeanexp)/x^3
end

######

#Sgn of guess encodes whether we are looking at Fwd or Backwd
function sigFwdBack(data, guess, trace=false)
    #adjust things for stability
    const b = guess > 0 ? maximum(data) : minimum(data)
    const mu_adj = mean(data)-b
    data_b = data-b

    df_sig_(x) = df_sig(x, mu_adj, data_b)
    #repair bracket if necessary
    low, hi = bracket(df_sig_, guess, trace=trace)

    trace && println("Bracketing Success:\t $low \t $hi")

    #rootfind
    xstar = fzero(df_sig_, low, hi)
    #use root to calc sig
    f_sig(xstar, mu_adj, data_b, b)
end

#Takes a Case Specification
function calcSigsBoot(data, alpha, numBoots, CASE)
    if CASE == :Fwd
        sigFwdBack_(data_) = sigFwdBack(data_, 1.)
    elseif CASE == :Back
        sigFwdBack_(data_) = sigFwdBack(data_, -1.)
    else
        error("Case must be either :Fwd or :Back $CASE")
    end
    boot(data, sigFwdBack_, 1-alpha, numBoots)
end

#calculates the means via t approx
# if joint, bounds hold (jointly) simultaneously at level 1-alpha_
# o.w. bounds hold individually at level 1-alpha_
function calcMeansT(data, alpha_; joint=true)
    const N   = length(data)
    const sig_rt_N = std(data)/sqrt(N)
    dist      = TDist(N-1)
    alpha = joint ? alpha_/2 : alpha_
    mean(data) + quantile(dist, alpha)*sig_rt_N, mean(data) + quantile(dist, 1-alpha)*sig_rt_N
end

#Currently computed using Stephens Approximation
#Journaly of Royal Statistical Society 1970
function KSGamma(alpha, N)
       const sqrt_N = sqrt(N)
       const num = sqrt(.5 * log(2/alpha))
       const denom = sqrt_N + .12 + .11/sqrt_N
       num/denom
end

kappa(eps_) = sqrt(1./eps_ - 1.)

function boot_mu(data, alpha, numBoots)
    const muhat = mean(data, 1)
    myfun(data_b) = norm(mean(data_b, 1) - muhat)
    boot(data, myfun, 1-alpha, numBoots)
end

function boot_sigma(data, alpha, numBoots)
    const covhat = cov(data)
    myfun(data_b) = vecnorm(cov(data_b) - covhat)
    boot(data, myfun, 1-alpha, numBoots)
end

function bootDY_mu(data, alpha, numBoots)
    muhat = mean(data, 1)
    myfun(data_b) = (mu = mean(data_b, 1); ((mu-muhat) * inv(cov(data)) * (mu-muhat)')[1])
    boot(data, myfun, 1-alpha, numBoots)
end

function bootDY_sigma(data, alpha, numBoots)
    mu0  = mean(data, 1)
    sig0 = cov(data)

    function myfun(data_b)
        muhat = mean(data_b, 1)
        sighat = cov(data_b)
        mudiff = (mu0 - muhat)'*(mu0-muhat)
        f(g2) = eigmin(g2*sighat - sig0 -mudiff)

        #If the dist of sighat is degenerate, return Inf
        if eigmin(sighat) <= 0
            println("Infinite 2nd moment matrix")
            return Inf
        end

        #solve
        try
            Roots.fzero(f, [.01, 500])
        catch e
            show(e); println()
            println("Using Manual Bracketing: f(1) $(f(1))  f(2) $(f(2))")
            #do your own bracketing
            if f(1) < 0
                lb = 1; ub = 2
                iter = 0
                const MAX_ITER = 100
                while f(ub) < 0 && iter < MAX_ITER
                    println("$iter \t $(f(ub))")
                    lb = ub
                    ub = ub * 2
                    iter = iter + 1
                end
            else
                lb = .5; ub = 1
                iter = 0
                const MAX_ITER = 100
                while f(lb) > 0 && iter < MAX_ITER
                    ub = lb
                    lb = lb * .5
                    iter = iter + 1
                end
            end
            try
                fzero(f, [lb, ub])
            catch e2
                println("Manual bracketing failed")
                println("Debug Sequence")
                for i = 10.0.^[-2:5]
                    println(f(i))
                end
            end
        end

   end
   boot(data, myfun, 1-alpha, numBoots)
end

### Used by UM and UIOracle
function sort_data_cols(data)
    data_sort = zeros(eltype(data), size(data))
    const d = size(data, 2)
    for i = 1:d
        data_sort[:, i] = sort(data[:, i])
    end
    data_sort
end

########
# LCX Stuff
####

########################
# Bootstrapping computation

#Returns indx of last instance of val, 0 if not found
#Assumes sorted_list, and sort_list[start] >= val
function findlast_sort(val::Float64, sort_list::Vector; TOL::Float64=1e-10, start::Int64=1)
    i::Int64 = 1
    for i = start:length(sort_list)
        if abs(val - sort_list[i]) > TOL
            break
        end
    end
    i-1
end

## Makes a single pass through zetas/zetahats to solve
# 1/N * max_b { sum_i [zeta_i - b] ^+ - sum_i[zetahat_i - b]^+ }
function singlepass!(zetas::Vector, zetahats::Vector)
    sort!(zetas)
    sort!(zetahats)

    vstar::Float64 = mean(zetas) - zetas[1]
    vb::Float64    = mean(zetahats) - zetas[1]

    Gamma::Float64 = vstar - vb
    const N::Int64 = length(zetas)
    pbar::Float64    = 1.0
    hat_indx::Int64  = 1
    hat_indx_::Int64 = 0

    for k = 2:length(zetas)
        vstar += (zetas[k-1] - zetas[k]) * (N-k+1)/N
        hat_indx = findlast_sort(zetas[k-1], zetahats, start=hat_indx_ + 1)
        pbar -=  (hat_indx - hat_indx_)/N
        hat_indx_ = hat_indx
        vb  += (zetas[k-1] - zetas[k]) * pbar
        Gamma = max(Gamma, vstar - vb)
    end
    Gamma
end

#a::Vector, sgns::Vector is used as storage between bootstraps
function f2(boot_sample::Matrix, data::Matrix, numSamples::Int, a::Vector, sgns::Vector)
    Gamma::Float64 = 0.
    for i = 1:numSamples
        randL1!(a, sgns)
        Gamma_ = singlepass!(data*a, boot_sample*a)
        Gamma = max(Gamma, Gamma_)
    end
    Gamma
end

function randL1!(a, sgns)
    a = rand!(a)
    a = a/ sum(a)
    rand!(Bernoulli(), sgns)
    a = a .* (2*sgns-1)
end

#Approximates the threshold by sampling a bunch of abs for each bootstrap rep
function calc_ab_thresh(data::Matrix, alpha::Float64, numBoots::Int, numSamples::Int)
    const N = size(data, 1)
    const d = size(data, 2)
    a::Vector{Float64} = zeros(Float64, d)
    sgns::Vector{Int}  = zeros(Int64, d)
    boot(data, f2, 1-alpha, numBoots, data, numSamples, a, sgns)
end

function compute_cs(boot_indx)
    cs = ones(length(boot_indx))
    for i = boot_indx
        cs[i] = cs[i] - 1
    end
    return cs/length(boot_indx)
end


include("../src/interface.jl")
using DifferentialEquations
using LinearAlgebra
using Plots
using Random

function getmindif(tab::Vector{Vector{BigFloat}})
    nmmin = Inf
    ret = (0, 0)
    for i = 1:size(tab, 1)
        for j = (i+1):size(tab, 1)
            nm = norm(tab[i] - tab[j], Inf)
            if nm < nmmin
                nmmin = nm
                ret = i, j
            end
        end
    end
    return ret, nmmin
end
# henon_heiles
function fct(u, p, t)
    return [0, u[4], -2u[1] * u[2], -u[2] - u[1]^2 + u[2]^2]
end

function fctmain(n_tau, prec)
    Random.seed!(19999909)
    setprecision(512)
    u0 = 2rand(BigFloat, 4) - ones(BigFloat, 4)
    setprecision(prec)
    u0 = BigFloat.(u0)
    println("u0=$u0")
    t_max = big"1.0"
    epsilon = big"1e-7"
    println("epsilon=$epsilon")
    nbmaxtest = 15
    ordmax = 15
    debord = 3
    pasord = 1
    y = ones(Float64, nbmaxtest, div(ordmax - debord, pasord) + 1)
    res_gen = Array{Array{BigFloat,1},2}(undef, nbmaxtest, div(ordmax - debord, pasord) + 1)
    x = zeros(Float64, nbmaxtest)
    A = [0 0 1 0; 0 0 0 0; -1 0 0 0; 0 0 0 0]
    t_0 = big"0.0"
    t_max = big"1.0"
    prob = HiOscODEProblem(fct, u0, (t_0, t_max), missing, A, epsilon)
    nm = NaN
    ordmax += 1
    ordprep = undef
    nb = 10 * 2^(nbmaxtest)
    solref = undef
    while isnan(nm)
        ordmax -= 1
        @time sol = solve(
            prob,
            nb_t = nb,
            order = ordmax,
            getprecision = false,
            nb_tau = n_tau,
            dense = false,
        )
        solref = sol[end]
        nm = norm(solref, Inf)
    end

    tabsol = Array{Array{BigFloat,1},1}(undef, 1)

    tabsol[1] = solref

    println("ordmax=$ordmax")

    solref_gen = solref
    indref = 1

    ind = 1
    for order = debord:pasord:ordmax
        ordprep = order + 2
        println("eps=$epsilon solRef=$solref order=$order")
        nb = 10
        indc = 1
        labels = Array{String,2}(undef, 1, order - debord + 1)
        sol = undef
        par_u0 = missing
        println("preparation ordre $order + 2")
        while indc <= nbmaxtest
            @time solall = solve(
                prob,
                nb_t = nb,
                order = order,
                getprecision = false,
                nb_tau = n_tau,
                par_u0 = par_u0,
                dense = false,
            )
            par_u0 = solall.par_u0
            sol = solall[end]
            push!(tabsol, sol)
            res_gen[indc, ind] = sol
            (a, b), nm = getmindif(tabsol)
            if a != indref
                println("New solref !!!! a=$a, b=$b nm=$nm")
                indref = a
                solref = tabsol[a]
                for i = 1:ind
                    borne = (i < ind) ? size(res_gen, 1) : indc
                    for j = 1:borne
                        nm2 = min(norm(res_gen[j, i] - solref, Inf), 1.1)
                        y[j, i] = nm2 == 0 ? nm : nm2
                    end
                end
            else
                diff = solref - sol
                y[indc, ind] = min(norm(diff, Inf), 1.1)
            end
            x[indc] = 1.0 / nb
            println("result=$y")
            println("log2(y)=$(log2.(y))")
            nb *= 2
            indc += 1
        end
        for i = debord:pasord:order
            labels[1, (i-debord)÷pasord+1] = " eps,order=$(convert(Float32,epsilon)),$i "
        end
        p = Plots.plot(
            x,
            view(y, :, 1:ind),
            xlabel = "delta t",
            xaxis = :log,
            ylabel = "error",
            yaxis = :log,
            legend = :bottomright,
            label = labels,
            marker = 2,
        )
        prec_v = precision(BigFloat)
        eps_v = convert(Float32, epsilon)
        Plots.savefig(p, "out/r4_$(prec_v)_$(eps_v)_$(order)_$(n_tau)_henon_heiles.pdf")
        ind += 1
    end
end
fctmain(32, 512)


"""
    Grids

A simple 1-dimensional uniform grid
for finite difference method.
"""
module Grids

export Grid, over_elems, over_elems_real, over_elems_ghost
export first_elem_above_surface, get_z

"""
    Grid{T}

A simple 1-dimensional uniform grid of
type `T` for finite difference method.
"""
struct Grid{T}
  """
  Number of physical elements
  """
  n_elem :: Int

  """
  Element size
  """
  dz :: T

  """
  Inverse of element size squared
  """
  dzi :: T

  """
  Inverse of element size squared
  """
  dzi2 :: T

  """
  z-coordinate
  """
  z :: Vector{T}

  """
  Number of ghost points per side
  """
  n_ghost :: Int

  """
  z-coordinate at the cell center at the top of the domain
  """
  zc_top :: T

  """
  z-coordinate at the top domain boundary
  """
  zn_top :: T

  """
  z-coordinate at the cell center at the bottom of the domain
  """
  zc_surf :: T

  """
  z-coordinate at the bottom domain boundary
  """
  zn_surf :: T
end

"""
    Grid(z_min::T, z_max::T, n_elem_real::Int, n_ghost::Int = 1)

A simple grid implementation that accepts the domain interval
`z_min` and `z_max`, the number of elements `n_elem_real` and
the number of ghost points per side `n_ghost`.
"""
function Grid(z_min::T, z_max::T, n_elem_real::Int, n_ghost::Int = 1) where T
  n_elem = n_elem_real+2*n_ghost
  dz = (z_max-z_min)/n_elem_real
  dzi = inv(dz)
  dzi2 = dzi^2
  z = [(i-n_ghost)*dz-dz/2 for i in 1:n_elem]
  zc_surf = z[1+n_ghost]
  zn_surf = (z[n_ghost]+z[1+n_ghost])/2
  zc_top = z[end-n_ghost]
  zn_top = (z[end-n_ghost]+z[end-n_ghost+1])/2
  return Grid(n_elem, dz, dzi, dzi2, z, n_ghost, zc_top, zn_top, zc_surf, zn_surf)
end

function Base.show(io::IO, grid::Grid)
  println(io, "----------------- Grid")
  println(io, "z       = ",grid.z)
  println(io, "n_elem  = ",grid.n_elem)
  println(io, "n_ghost = ",grid.n_ghost)
  println(io, "zc_top  = ",grid.zc_top)
  println(io, "zn_top  = ",grid.zn_top)
  println(io, "zc_surf = ",grid.zc_surf)
  println(io, "zn_surf = ",grid.zn_surf)
  println(io, "-----------------")
end

"""
    get_z(grid::Grid, k::Int)

Get the z-coordinate given element index
"""
@inline get_z(grid::Grid, k::Int) = grid.z[k]

"""
    over_elems(grid::Grid)

Get the range of indexes to traverse real and ghost grid elements
"""
@inline over_elems(grid::Grid) = 1:grid.n_elem

"""
    over_elems_real(grid::Grid)

Get the range of indexes to traverse only real grid elements
"""
@inline over_elems_real(grid::Grid) = 1+grid.n_ghost : grid.n_elem-grid.n_ghost

"""
    over_elems_ghost(grid::Grid)

Get the range of indexes to traverse only ghost grid elements
"""
@inline over_elems_ghost(grid::Grid) = setdiff(over_elems(grid), over_elems_real(grid))

"""
    first_elem_above_surface(grid::Grid)

Get the first element index above the surface
"""
@inline first_elem_above_surface(grid::Grid) = 1+grid.n_ghost

end


using MLJLinearModels, StableRNGs
using BenchmarkTools, Random, LinearAlgebra
DO_COMPARISONS = false; include("../testutils.jl")

n, p = 50_000, 500
((X, y, θ), (X1, y1, θ1)) = generate_continuous(n, p;  seed=512, sparse=0.5)

# =============== #
# RIDGE FUNCTIONS #
#
# Hv!           ✅
# =============== #

# No fit_intercept
s = R.scratch(X; i=false)
λ = 0.5
ridge = RidgeRegression(λ; fit_intercept=false)
Hv! = R.Hv!(ridge, X, y, s)
v   = randn(p)
Hv  = similar(v)

# Sept 13, 2019 :: 15.300 ms (0 allocations: 0 bytes)
# May 12, 2020 :: 15.785 ms (22 allocations: 432 bytes)
#              :: 15.332 ms (22 allocations: 432 bytes)
#              :: 15.198 ms (22 allocations: 432 bytes)
@btime Hv!($Hv, $v)

# With fit_intercept
s = R.scratch(X; i=true)
ridge = RidgeRegression(λ)
Hv! = R.Hv!(ridge, X, y, s)
v  = randn(p+1)
Hv = similar(v)

# Sept 13, 2019 :: 26.246 ms (5 allocations: 4.22 KiB)
# (decrease in perf due to views, and need a sum on an array)
# May 12, 2020 :: 120.892 ms (34 allocations: 4.77 KiB) NOTE != rng
#              :: 118.999 ms (35 allocations: 4.81 KiB) NOTE != rng
#              :: 114.046 ms (34 allocations: 768 bytes)
@btime Hv!($Hv, $v)

# =============== #
# LASSO FUNCTIONS #
#
# smooth_fg!
# =============== #

lasso = LassoRegression(0.5, fit_intercept=false)
s = R.scratch(X; i=false)
smooth_fg! = R.smooth_fg!(lasso, X, y, s)
v = randn(p)
g = similar(v)

# Sept 13, 2019 :: 15.508 ms (3 allocations: 390.72 KiB)
# May 12, 2020 :: 16.659 ms (32 allocations: 391.34 KiB)
#              :: 15.868 ms (32 allocations: 391.34 KiB)
@btime smooth_fg!($g, $v);

((X, _, _), (X1, y1, θ1)) = generate_continuous(n, p;  seed=512, sparse=0.1)

λ  = 50
lr = LassoRegression(λ)
J  = objective(lr, X, y1)

fista = FISTA()
ista = ISTA()

# <old> initial: ~650ms ; 1725 allocations;  104.24 MiB
# using cache: ; ~650ms ; 1666 allocations;   95.02 MiB
# May 12, 2020 (note RNG is different)
# -- 780.603 ms (4482 allocations: 97.43 MiB)
# -- 772.273 ms (4488 allocations: 98.20 MiB)
@btime θ_fista = fit($lr, $X, $y1, solver=$fista)

# using cache ;  ~590ms ; 1584 allocations;   91.36 MiB
# May 12, 2020 (note RNG is different)
# -- 655.937 ms (4119 allocations: 91.63 MiB)
# -- 644.725 ms (4125 allocations: 92.40 MiB)
@btime θ_ista  = fit($lr, $X, $y1, solver=$ista)

rr = RidgeRegression(λ)
θ_ridge = fit(rr, X, y1)
cg = CG()

# <old> initial: ~ 220ms; 100 allocations; 3.5 MiB
# using cache:   ~ 216ms;  76 allocations; 458.5KiB
# May 12, 2020 :: 982.551 ms (496 allocations: 467.69 KiB)
#              :: 1.005 s (509 allocations: 1.22 MiB)
@btime θ_ridge_cg = fit($rr, $X, $y1, solver=$cg)


using Test;

direction_to_xy = Dict(
    0 => [0, 1], # north
    90 => [1, 0], # east
    180 => [0, -1], # south
    270 => [-1, 0], # west
)

function turn(start_direction, degrees)
    return mod(start_direction + degrees, 360)
end

function move(start_pos, direction, distance)
    m = direction_to_xy[direction] .* distance
    return start_pos + m
end

function run(fname)
    f = open(fname, "r")
    lines = readlines(f)

    pos = [0, 0]
    direction = 90 # East
    println(pos)
    for l in lines
        command = l[1]
        number = parse(Int, l[2:length(l)])
        println("cmd=", command, " num=", number)

        # Action N means to move north by the given value.
        mov_dir = direction
        if command == 'N'
            pos = move(pos, 0, number)
        elseif command == 'S'
            pos = move(pos, 180, number)
        elseif command == 'E'
            pos = move(pos, 90, number)
        elseif command == 'W'
            pos = move(pos, 270, number)
        elseif command == 'L'
            direction = turn(direction, -1 * number)
        elseif command == 'R'
            direction = turn(direction, number)
        elseif command == 'F'
            pos = move(pos, direction, number)
        end

        println("pos: ", pos, " facing: ", direction_to_xy[direction])
    end

    result = abs(pos[1]) + abs(pos[2])
    println("[part 1] File = ", fname, " ... Result = ", result)

    return result
end

function reflect(start_pos, degrees)
    deg = mod(degrees, 360)

    if deg == 0
        return start_pos
    elseif deg == 90
        # [1,2] .. R90  (L270) .. [2,-1]  # swap and -1 the second element
        return [start_pos[2], -1 * start_pos[1]]
    elseif deg == 180
        # [1,2] .. R180 (L180) .. [-1,-2] # -1 both elements
        return start_pos .* -1
    elseif deg == 270
        # [1,2] .. R270 (L90)  .. [-2,1]  # swap and -1 the first element
        return [-1 * start_pos[2], start_pos[1]]
    else
        throw("invalid input: degrees = " * degrees)
    end
end

function run2(fname)
    f = open(fname, "r")
    lines = readlines(f)

    pos = [10, 1] # waypoint pos
    ship_pos = [0, 0] # ship position
    println(pos)
    for l in lines
        command = l[1]
        number = parse(Int, l[2:length(l)])
        println("cmd=", command, " num=", number)

        if command == 'N'
            pos = move(pos, 0, number)
        elseif command == 'S'
            pos = move(pos, 180, number)
        elseif command == 'E'
            pos = move(pos, 90, number)
        elseif command == 'W'
            pos = move(pos, 270, number)
        elseif command == 'L'
            pos = reflect(pos, -1 * number)
        elseif command == 'R'
            pos = reflect(pos, number)
        elseif command == 'F'
            # Move the ship
            for i in 1:number
                ship_pos += pos
            end
        end

        println("ship_pos: ", ship_pos, " waypoint_pos: ", pos)
    end

    result = abs(ship_pos[1]) + abs(ship_pos[2])
    println("[part 2] File = ", fname, " ... Result = ", result)

    return result
end

@test run("12ex.txt") == 25
run("12.txt")

@test run2("12ex.txt") == 286
run2("12.txt")


## SMA performance: buffer vector version with recursive mean instead of direct summation
using TimeSeries, MarketTechnicals
using Reactive

nma = 50
ohlc_BA = TimeSeries.readtimearray("../../data/OHLC_BA.csv")
sma_ta = sma(ohlc_BA["Close"], nma)

s_ohlc = Reactive.Input(ohlc_BA.values[nma,:])

function smarecbuffer!(prev, val_ohlc)
    buffer, prev_sma = prev

    # update SMA value recursively
    new_sma = prev_sma + (val_ohlc[4] - buffer[1])/float(nma)

    # update buffer
    push!(buffer, val_ohlc[4]) # add new close price
    shift!(buffer) # remove the earliest element

    # new_sma becomes prev_sma in the next call
    return (buffer, new_sma)
end
s_sma = lift(s -> s[2], foldl(smarecbuffer!, (ohlc_BA["Close"].values[1:nma], mean(ohlc_BA["Close"].values[1:nma])), s_ohlc))

vsig = [s_sma.value]
function vsigupd!(vsig::Vector{Float64}, nma::Int64, nmax::Int64)
    for i in (nma + 1):nmax
        push!(s_ohlc, ohlc_BA.values[i,:])
        push!(vsig, s_sma.value)
    end
end

# verify sma values
vsigupd!(vsig, nma, length(ohlc_BA))
println("check if SMA values are correct:")
println(mean(abs(vsig - sma_ta.values)) < 1e-7)
println("")

println("SMA foldl-buffer version with recursive calculation:")
println("reactive signal updates timing")
expr_run = parse("@time vsigupd!(vsig, nma, length(ohlc_BA))")
for i = 1:5
    eval(expr_run)
end

### results
# elapsed time: 0.266140198 seconds (23986336 bytes allocated, 16.14% gc time)
### longer and more complex than mean(buffer): no point for this recursive version

Profile.clear()  # in case we have any previous profiling data
@profile vsigupd!(vsig, nma, length(ohlc_BA))
Profile.print()