"""
Based on:
Wilhelm Burger: Zhang’s Camera Calibration Algorithm: In-Depth Tutorial and Implementation,
Technical Report HGB16-05, University of Applied Sciences Upper Austria, School of
Informatics, Communications and Media, Dept. of Digital Media, Hagenberg, Austria, May
2016. DOI: 10.13140/RG.2.1.1166.1688, http://staff.fh-hagenberg.at/burger/
"""
module CameraCalibration
using Statistics, LinearAlgebra
export estimateHomography, calibrate, getHomographies, getCameraIntrinsics, getExtrinsics, getCameraIntrinsicsB
"""
estimateHomography - returns the estimated homography matrix H, such that b_j = H*a_j.
'''
A and B are in the form 3xN (each column is a point)
'''
"""
function estimateHomography(a::Array{<:Real,2}, b::Array{<:Real,2})
    if size(a) != size(b)
        error("src and dst must have same dimensions")
    end
    n = size(a)[2] #number of points
    #normalisation matrices
    nA = getNormalisationMatrix(a)
    nB = getNormalisationMatrix(b)
    m = zeros((2*n, 9))

    #construct matrix for solving
    for j  = 1:n
        k = 2*j
        ap = nA*a[:,j]
        bp = nB*b[:,j]
        # M_{k,*} = (x′_a, y′_a, 1, 0, 0, 0, −x′_ax′_b , −y′_ax′_b, −x′_b )
        m[k-1,:] = [ap[1] ap[2] 1 0 0 0 -ap[1]*bp[1] -ap[2]*bp[1] -bp[1]]
        # M_{k+1,*} = (0, 0, 0, x′_a , y′_a , 1, −x′_ay′_b, −y′_ay′_b , −y′_b )
        m[k,:] = [0 0 0 ap[1] ap[2] 1 -ap[1]*bp[2] -ap[2]*bp[2] -bp[2]]
    end

    #solve
    u, d, v = svd(m, full = true)
    h = v[:,9] #last column of v
    H = [h[1] h[2] h[3];
         h[4] h[5] h[6]
         h[7] h[8] h[9]]

    H = inv(nB)*H*nA
    return H./H[3,3]
end
"""
getNormalisationMatrix - returns normalisation matrix of points x
'''

'''
"""
function getNormalisationMatrix(x::Array{<:Real, 2})
    m = mean(x[1:2, :], dims = 2)
    v = var(x[1:2, :], dims = 2, corrected = false, mean = m)
    #scale values (182)
    sx = sqrt(2.0/v[1])
    sy = sqrt(2.0/v[2])
    #construct normalisation matrix (181)
    return [sx 0 -sx*m[1]; 0 sy -sy*m[2]; 0 0 1]
end

function refineHomography(h::Array{<:Real, 2}, a::Array{<:Real, 2}, b::Array{<:Real, 2})
    n = size(a)[2]
    X = zeros((2,2*n))
    Y = zoeros((2,2*n))
    for i = 1:n

    end
end

function homographyVal(x::Array{<:Real,2}, h::Array{<:Real, 1})
    n = size(x)[2]
    y = zeros(2*n)
    for j = 1:n
        w = h[7]*x[1,j] + h[8]*x[2,j]+h[9]
        y[j*2-1] = (h[1]*x[1,j] + h[2]*x[2,j]+h[3])/w
        y[j*2] = (h[4]*x[1,j] + h[5]*x[2,j]+h[6])/w
    end
    return y
end

function homographyJac(x::Array{<:Real,2}, h::Array{<:Real, 1})
    n = size(x)[2]
    J = zeros((2*n, 9))
    for j = 1:n
        sx = h[1]*x[1,j]+h[2]*x[2,j] + h[3]
        sy = h[4]*x[1,j]+h[5]*x[2,j] + h[6]
        w = h[7]*x[1,j]+h[8]*x[2,j] + h[9]
        J[2j-1,:]=[x[1,j]/w x[2,j]/w 1/w 0 0 0 -sx*x[1,j]/(w^2) -sx*x[2,j]/(w^2) -sx/(w^2)]
        J[2j,:]=[0 0 0 x[1,j]/w x[2,j]/w 1/w -sy*x[1,j]/(w^2) -sy*x[2,j]/(w^2) -sy/(w^2)]
    end
    return J
end

function calibrate(x::Array{<:Real,2}, u::Array{Array{T1,2},1}) where T1 <: Real
    hListInit = getHomographies(x,u)
    aInit = getCameraIntrinsicsB(hListInit)
    wInit = getExtrinsics(aInit,hListInit)
end

function getHomographies(x::Array{<:Real,2}, u::Array{Array{T1,2},1}) where T1 <: Real
    m = size(u)[1]
    hList = Array{Float64,2}[]
    for i = 1:m
        hInit = estimateHomography(x, u[i])
        # TODO refineHomography
        push!(hList, hInit)
    end
    return hList
end

function getCameraIntrinsics(hs::Array{Array{T1,2},1}) where T1 <: Real
    m = size(hs)[1]
    V = zeros((2*m, 6))

    for i = 1:m
        V[2*i-1,:] = getIntrinsicRowVector(1,2,hs[i])
        V[2*i,:] = getIntrinsicRowVector(1,1,hs[i]) - getIntrinsicRowVector(2,2,hs[i])
    end

    #solve
    u, s, v = svd(V, full = true)
    print(size(v))
    b = v[:,6]

    w = b[1]*b[3]*b[6] - (b[2]^2)*b[6] - b[1]*(b[5]^2) + 2*b[2]*b[4]*b[5] - b[3]*(b[4]^2) # (104)
    d = b[1]*b[3] - b[2]^2 # (105)
    α = sqrt(w/(d*b[1])) # (99)
    β = sqrt(w/d^2 *b[1]) # (100)
    γ = sqrt(w/(d^2 * b[1])) * b[2] # (101)
    uc = (b[2]*b[5] - b[3]*b[4])/d
    vc = (b[2]*b[4] - b[1]*b[5])/d
    return [α γ uc
            0 β vc
            0 0 1]
end

function getCameraIntrinsicsB(hs::Array{Array{T1,2},1}) where T1 <: Real
    m = size(hs)[1]
    V = zeros((2*m, 6))

    for i = 1:m
        V[2*i-1,:] = getIntrinsicRowVector(1,2,hs[i])
        V[2*i,:] = getIntrinsicRowVector(1,1,hs[i]) .- getIntrinsicRowVector(2,2,hs[i])
    end

    #solve
    u, s, v = svd(V, full = true)
    b = v[:,6]
    if b[1] < 0 || b[3] < 0 || b[6] < 0
        b = -b
    end
    bInit = [b[1] b[2] b[4]
             b[2] b[3] b[5]
             b[4] b[5] b[6]]
    C = cholesky(bInit)
    return inv(C.L)' * C.L[3,3]
end

function getIntrinsicRowVector(p::Int64, q::Int64,  h::Array{<:Real,2})
    return [h[1,p]*h[1,q]
            h[1,p]*h[2,q]+h[2,p]*h[1,q]
            h[2,p]*h[2,q]
            h[3,p]*h[1,q]+h[1,p]*h[3,q]
            h[3,p]*h[2,q]+h[2,p]*h[3,q]
            h[3,p]*h[3,q]]'
end

"""
a is intrinsic matrix hs is list of homographies

"""
function getExtrinsics(a::Array{<:Real,2},hs::Array{Array{T1,2},1}) where T1 <: Real
    wList = Array{Float64,2}[]
    m = size(hs)[1]

    for i = 1:m
        push!(wList, estimateViewTransorm(a, hs[i]))
    end
    return wList
end

function estimateViewTransorm(a::Array{<:Real,2}, h::Array{<:Real,2})
    κ = 1/norm(inv(a)*h[:,1])
    r0 = κ*inv(a)*h[:,1]
    r1 = κ*inv(a)*h[:,2]
    r2= cross(r0,r1)
    R = zeros((3, 3))
    R[:,1] = r0
    R[:,2] = r1
    R[:,3] = r2
    # make true rotation matrix
    u, s, v = svd(R, full = true)
    R = v*u'
    t = κ*inv(a)*h[:,3]
    return hcat(R,t)
end
end  # module CameraCalib

"""
tests
asample = [182 535 171 537; 350 358 553 563; 1 1 1 1]
bsample = [0 888 0 888; 0 0 500 500; 1 1 1 1]

testobj = [0. 1. 2. 3. 4. 5. 6. 0. 1. 2. 3 4 5 6 0 1 2 3 4 5 6 0 1 2 3 4 5 6 0 1 2 3 4 5 6;
           4 4 4 4 4 4 4 3 3 3 3 3 3 3 2 2 2 2 2 2 2 1 1 1 1 1 1 1 0 0 0 0 0 0 0;
           1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1]

using CSV, DataFrames
imgpoints = Array{Float64, 2}[]
    for i = 0:10
        push!(imgpoints, vcat(convert(Array{Float64, 2}, CSV.File(string("./test/imgpoints", i, ".csv"), header = false) |> DataFrame)', ones(1, 35)))
    end

using Plots
cord = [-ex[i][:,1:3]'*ex[i][:,4] for i in 1:11]
p = scatter([cord[i][1] for i = 1:11], [cord[i][2] for i = 1:11], [cord[i][3] for i = 1:11],marker=:circle,linewidth=0, group = 1:11)
plot(p, xlabel="X",ylabel="Y",zlabel="Z", size = (800, 800))
"""


using Dates, Random

@testset "TimeAnomaly" begin
    Random.seed!(41516)
    y = Date(2001):Day(1):Date(2025)
    dy = dayofyear.(y)
    cy =  @. 4 + 7.2cos(2π*dy/365.26) + 5.6cos(4π*dy/365.26 + 3π/5)
    r0 = randn(length(dy))/2
    sy = cy .+ r0

    x, r = decompose(y, sy, TimeAnomaly())

    errper = nrmse(cy, x)
    @test errper < 0.1
    errres = nrmse(r0, r)
    @test nrmse(r0, r) < 0.5

    # figure()
    # title("TimeAnomaly")
    # ax1 = subplot(211)
    # plot(y, cy; alpha = 0.75, label = "original")
    # plot(y, x; alpha = 0.75, ls = "dashed", label = "err=$errper")
    # ylabel("x"); legend()
    # subplot(212; sharex = ax1)
    # plot(y, r0; alpha = 0.75, label = "original")
    # plot(y, r; alpha = 0.75, ls = "dashed", label = "err=$errres")
    # ylabel("r"); legend()
end


using Base: Float64
"""
File with definitions of functions for structural analysis of aircraft configurations using beam elements
"""

Fmax = 1e15

"""
Get elasticity matrix for a single beam
"""
beam_get_K(
	L::Fg,
	EA::Fg,
	GJ::Fg,
	EIy::Fg,
	EIz::Fg
) where {Fg <: Real} = - Fg[
	(EA / L) 0.0 0.0 0.0 0.0 0.0 (- EA / L) 0.0 0.0 0.0 0.0 0.0;
	0.0 (12.0 * EIz / L ^ 3) 0.0 0.0 0.0 (6.0 * EIz / L ^ 2) 0.0 (- 12.0 * EIz / L ^ 3) 0.0 0.0 0.0 (6.0 * EIz / L ^ 2);
	0.0 0.0 (12.0 * EIy / L ^ 3) 0.0 (- 6.0 * EIy / L ^ 2) 0.0 0.0 0.0 (- 12.0 * EIy / L ^ 3) 0.0 (- 6.0 * EIy / L ^ 2) 0.0;
	0.0 0.0 0.0 (GJ / L) 0.0 0.0 0.0 0.0 0.0 (- GJ / L) 0.0 0.0;
	0.0 0.0 (- 6.0 * EIy / L ^ 2) 0.0 (4.0 * EIy / L) 0.0 0.0 0.0 (6.0 * EIy / L ^ 2) 0.0 (2.0 * EIy / L) 0.0;
	0.0 (6.0 * EIz / L ^ 2) 0.0 0.0 0.0 (4.0 * EIy / L) 0.0 (- 6.0 * EIz / L ^ 2) 0.0 0.0 0.0 (2.0 * EIz / L);
	(- EA / L) 0.0 0.0 0.0 0.0 0.0 (EA / L) 0.0 0.0 0.0 0.0 0.0;
	0.0 (- 12.0 * EIz / L ^ 3) 0.0 0.0 0.0 (- 6.0 * EIz / L ^ 2) 0.0 (12.0 * EIz / L ^ 3) 0.0 0.0 0.0 (- 6.0 * EIz / L ^ 2);
	0.0 0.0 (- 12.0 * EIy / L ^ 3) 0.0 (6.0 * EIy / L ^ 2) 0.0 0.0 0.0 (12.0 * EIy / L ^ 3) 0.0 (6.0 * EIy / L ^ 2) 0.0;
	0.0 0.0 0.0 ( - GJ / L) 0.0 0.0 0.0 0.0 0.0 (GJ / L) 0.0 0.0;
	0.0 0.0 (- 6.0 * EIy / L ^ 2) 0.0 (2.0 * EIy / L) 0.0 0.0 0.0 (6.0 * EIy / L ^ 2) 0.0 (4.0 * EIy / L) 0.0;
	0.0 (6.0 * EIz / L ^ 2) 0.0 0.0 0.0 (2.0 * EIz / L) 0.0 (- 6.0 * EIz / L ^ 2) 0.0 0.0 0.0 (4.0 * EIz / L)
]


using Enzyme
mutable struct Model
    F::Vector{Float64}
    F_H::Vector{Float64}
    t::Float64
    h::Float64
end

function func_U(t)
    e = exp(1)
    return 2.0*((e^(3.0*t))-(e^3))/((e^(3.0*t/2.0))*(2.0+(e^3)))
end

function func(F, X,t)
    F[2] = X[1]*X[1]+0.5*(func_U(t)*func_U(t))
    F[1] = 0.5*X[1]+ func_U(t)
    return nothing
end

function advance(model)
    F_H = model.F_H
    F = model.F
    t = model.t
    h = model.h
    func(F, F_H,t)
    F[1] = F_H[1] + h/2.0*F[1]
    F[2] = F_H[2] + h/2.0*F[2]
    func(F,F,t+h/2.0)
    model.F[1] = F_H[1] + h*F[1]
    model.F[2] = F_H[2] + h*F[2]
    return nothing
end

function opt_sol(Y,t)
    e = exp(1)
    Y[1] = (2.0*e^(3.0*t)+e^3)/(e^(3.0*t/2.0)*(2.0+e^3))
    Y[2] = (2.0*e^(3.0*t)-e^(6.0-3.0*t)-2.0+e^6)/((2.0+e^3)^2)
    return
end

function opt_lambda(L,t)
    e = exp(1)
    L[1] = (2.0*e^(3-t)-2.0*e^(2.0*t))/(e^(t/2.0)*(2+e^3))
    L[2] = 1.0
    return
end


"""
    InflationConstant <: InflationFunction
    InflationConstant(c)

Metodología de inflación constante con valor interanual `c`.
"""
struct InflationConstant <: InflationFunction
    c::Float32
end
InflationConstant() = InflationConstant(1)

# Método para obtener la variación interanual constante igual a c
function (inflfn::InflationConstant)(cs::CountryStructure)
    t = infl_periods(cs::CountryStructure)
    fill(eltype(cs)(inflfn.c), t)
end

# Variación intermensual correspondiente a la variación interanual constante igual a c
function (inflfn::InflationConstant)(base::VarCPIBase{T}) where T
    v = 100 * ((inflfn.c / 100 + 1)^(1 // 12) - 1)
    fill(T(v), periods(base))
end

# Nombre de la medida
measure_name(inflfn::InflationConstant) = "Variación interanual constante igual a " * string(round(inflfn.c, digits=2))


@symbol_func function K_DR(cur_reactor::AbstractReactor)
  cur_reactor.is_pulsed || return 0.0

  cur_denominator = cur_reactor.sigma_CS

  cur_denominator *= (12/5) * cur_reactor.pi

  cur_denominator -= cur_reactor.B_CS ^ 2

  cur_numerator = 3.0

  cur_numerator *= cur_reactor.B_CS ^ 2

  cur_K = cur_numerator

  cur_K /= cur_denominator

  cur_K
end


using LinearAlgebra
using GaussianDistributions
using CovarianceFunctions
const Kernel = CovarianceFunctions
using Kernel
using Kernel: gramian
using GaussianDistributions: ConditionalMean, ConditionalKernel
using WoodburyIdentity
using SARA: random_sampling, uncertainty_sampling, inner_sampling, integrated_uncertainty_sampling, ucb, integrated_uncertainty
using ForwardDiff

# C = C | (xi, yi, σ^2) # TODO: make efficient by getting rid of hierarchical structure
function integrated_deviation(f, g, x::AbstractVector, w::AbstractVector = fill(1/length(x), length(x)))
    h(x) = (f(x) - g(x))^2
    return sqrt(integral(h, x, w))
end
function integral(f, x::AbstractVector, w::AbstractVector = fill(1/length(x), length(x)))
    dot(w, f.(x))
end

function sampling_path(policy, G::Gaussian, z::AbstractVector, σ::Real, nsample::Int, ground_truth)
    xs = collect(z) # potential points and integration nodes (z)
    C = G
    x, y = zeros(nsample), zeros(nsample)
    d, dd, u, du = zeros(nsample), zeros(nsample), zeros(nsample), zeros(nsample)
    for i in 1:nsample # random sampling
        xi = policy(C, xs)
        println(xi)
        x[i] = xi
        y[i] = g(xi) # noisy observation
        xs = deleteat!(xs, findfirst(==(xi), xs))
        C = G | @views (x[1:i], y[1:i], σ^2)

        # d[i] = mean((mean(C, z) .- ground_truth).^2)
        u[i] = integrated_uncertainty(C, z)
        D = GaussianDistributions.gradient(C) # multi-ouput process
        d[i] = mean((mean(D[2], z) .- ground_truth).^2)
        du[i] = integrated_uncertainty(D[2], z)
    end
    return C, d, u, du
end

# synthetic example for inner loop acquisition
l = 1/4
k = Kernel.Lengthscale(Kernel.EQ(), l)
G = Gaussian(k)

f(x::Real) = sin(2π*x)
σ = 1e-1
g(x::Real) = f(x) + σ*randn() # noisy observations

# grid of potential points
n = 512
z = range(-1, 1, length = n)
# ground_truth = f.(z)
ground_truth = (x->ForwardDiff.derivative(f, x)).(z)

nsample = 32
sample_helper(policy) = sampling_path(policy, G, z, σ, nsample, ground_truth)
println("random")
Cr, dr, ur, dur = sample_helper(random_sampling)

println("uncertainty")
Cu, du, uu, duu = sample_helper(uncertainty_sampling)

println("integrated uncertainty")
policy = (G, x) -> integrated_uncertainty_sampling(G, x, σ)
Ciu, diu, uiu, duiu = sample_helper(policy)

println("gradient uncertainty")
policy = (G, x) -> inner_sampling(G, x, σ)
Ci, dgu, ugu, dugu = sample_helper(policy)

deviations = (ur, du, diu)
uncertainties = (ur, uu, uiu)

using Plots
plotly()
plot(scale = :log10, legend = :outerbottomleft)
plot!(dr, label = "random")
plot!(du, label = "uncertainty")
plot!(diu, label = "integrated uncertainty")
plot!(dgu, label = "gradient uncertainty")
gui()

# plot(yscale = :log10)
# plot!(ur, label = "random")
# plot!(uu, label = "uncertainty")
# plot!(uiu, label = "integrated uncertainty")
# plot!(ui, label = "gradient uncertainty")
# gui()
#
plot(yscale = :log10)
plot!(dur, label = "random")
plot!(duu, label = "uncertainty")
plot!(duiu, label = "integrated uncertainty")
plot!(dugu, label = "gradient uncertainty")
gui()


function get_Yeoh_loading()
    _C = range(0.0,  0.005, length=3)
    C = [SymmetricTensor{2,3}((x + 1.0, x/10, 0.0, 1.0, 0.0, 1.0)) for x in _C]

    return C
end

@testset "Yeoh" begin
    m = Yeoh(μ = 6.8, λ=62.1, c₂ = -6.8/10, c₃ = 6.8/30)

    loading = get_Yeoh_loading()
    check_jld2(m, loading, "Yeoh")#, debug_print=false, OVERWRITE_JLD2=true)
    check_tangents_AD(m,loading)
end


module TestNoEffectTempSize
    using BioEnergeticFoodWebs
    using Test

    # WET MASS (default)
    EC = [0 0 1 ; 0 0 1 ; 0 0 0]
    LFC = [0 1 0 ; 0 0 1 ; 0 0 0]
    bsize_ratio = 10.0 #consumers are 10 times larger than their resource
    p_ec = model_parameters(EC, Z = bsize_ratio)
    p_lfc = model_parameters(LFC, Z = bsize_ratio)
    #body sizes are CR ratio ^ (trophic rank - 1)
    expected_mass_ec = [10.0, 10.0, 1.0]
    @test p_ec[:bodymass] == expected_mass_ec
    expected_mass_lfc = [100.0, 10.0, 1.0]
    @test p_lfc[:bodymass] == expected_mass_lfc

    #if body masses are provided, it overwrites Z
    bm = [12.3, 2.3, 1.0]
    p_bm = model_parameters(EC, Z = bsize_ratio, bodymass = bm)
    @test p_bm[:bodymass] == bm

    # DRY MASS
    dm = [6.3, 5.2, 1.2]
    p_dm = model_parameters(EC, TSR_type = :no_response, dry_mass_293 = dm)
    expected_mass_dm = dm .* 6.5
    @test p_dm[:bodymass] == expected_mass_dm
end

module TestEffectTempSize
    using BioEnergeticFoodWebs
    using Statistics
    using Test

    EC = [0 0 1 ; 0 0 1 ; 0 0 0]
    dm = [1.0, 0.8, 0.2]
    wm = dm .* 6.5
    temp = 295.0
    temp_c = temp-273.15
    temp2 = 285.0
    temp2_c = temp2-273.15

    # MEAN AQUATIC
    # dry mass
    p_aqua_1 = model_parameters(EC, TSR_type = :mean_aquatic, dry_mass_293 = dm, T = temp)
    pcm_aqua_1 = -3.90 .- 0.53 .* log10.(dm)
    TSr_aqua_1 = log.(pcm_aqua_1 ./ 100 .+ 1)
    expected_bm_aqua = (dm .* 6.5) .* exp.(TSr_aqua_1 .* (temp_c-20))
    @test p_aqua_1[:bodymass] == expected_bm_aqua
    p_aqua_2 = model_parameters(EC, TSR_type = :mean_aquatic, dry_mass_293 = dm, T = temp2)
    @test p_aqua_2[:bodymass] > p_aqua_1[:bodymass]
    # wet mass
    p_aquawm = model_parameters(EC, TSR_type = :mean_aquatic, bodymass = wm, T = temp)
    pcm_aquawm = -3.90 .- 0.53 .* log10.(dm)
    TSr_aquawm = log.(pcm_aquawm ./ 100 .+ 1)
    expected_bm_aquawm = wm .* exp.(TSr_aquawm .* (temp_c-20))
    @test p_aquawm[:bodymass] == expected_bm_aqua == expected_bm_aquawm
    # Z
    p_aqua_z = model_parameters(EC, TSR_type = :mean_aquatic, Z = 10.0, T = temp)
    pcm_aqua_z = -3.90 .- 0.53 .* log10.([10.0, 10.0, 1.0] ./ 6.5)
    TSr_aqua_z = log.(pcm_aqua_z ./ 100 .+ 1)
    expected_bm_aqua_z = [10.0, 10.0, 1.0] .* exp.(TSr_aqua_z .* (temp_c-20))
    @test p_aqua_z[:bodymass] == expected_bm_aqua_z

    # MEAN TERRESTRIAL
    # dry mass
    p_terr_1 = model_parameters(EC, TSR_type = :mean_terrestrial, dry_mass_293 = dm, T = temp)
    pcm_terr_1 = -1.72 .+ 0.54 .* log10.(dm)
    TSr_terr_1 = log.(pcm_terr_1 ./ 100 .+ 1)
    expected_bm_terr = (dm .* 6.5) .* exp.(TSr_terr_1 .* (temp_c-20))
    @test p_terr_1[:bodymass] == expected_bm_terr
    p_terr_2 = model_parameters(EC, TSR_type = :mean_terrestrial, dry_mass_293 = dm, T = temp2)
    @test p_terr_2[:bodymass] > p_terr_1[:bodymass]
    # wet mass
    p_terrwm = model_parameters(EC, TSR_type = :mean_terrestrial, bodymass = wm, T = temp)
    pcm_terrwm = -1.72 .+ 0.54 .* log10.(dm)
    TSr_terrwm = log.(pcm_terrwm ./ 100 .+ 1)
    expected_bm_terrwm = wm .* exp.(TSr_terrwm .* (temp_c-20))
    @test p_terrwm[:bodymass] == expected_bm_terr == expected_bm_terrwm
    # Z
    p_terr_z = model_parameters(EC, TSR_type = :mean_terrestrial, Z = 10.0, T = temp)
    pcm_terr_z = -1.72 .+ 0.54 .* log10.([10.0, 10.0, 1.0] ./ 6.5)
    TSr_terr_z = log.(pcm_terr_z ./ 100 .+ 1)
    expected_bm_terr_z = [10.0, 10.0, 1.0] .* exp.(TSr_terr_z .* (temp_c-20))
    @test p_terr_z[:bodymass] == expected_bm_terr_z

    # MAXIMUM
    # dry mass
    p_max_1 = model_parameters(EC, TSR_type = :maximum, dry_mass_293 = dm, T = temp)
    pcm_max_1 = -8
    TSr_max_1 = log.(pcm_max_1 ./ 100 .+ 1)
    expected_bm_max = (dm .* 6.5) .* exp.(TSr_max_1 .* (temp_c-20))
    @test p_max_1[:bodymass] == expected_bm_max
    p_max_2 = model_parameters(EC, TSR_type = :maximum, dry_mass_293 = dm, T = temp2)
    @test p_max_2[:bodymass] > p_max_1[:bodymass]
    # wet mass
    p_maxwm = model_parameters(EC, TSR_type = :maximum, bodymass = wm, T = temp)
    pcm_maxwm = -8
    TSr_maxwm = log.(pcm_maxwm ./ 100 .+ 1)
    expected_bm_maxwm = wm .* exp.(TSr_maxwm .* (temp_c-20))
    @test p_maxwm[:bodymass] == expected_bm_max == expected_bm_maxwm
    # Z
    p_max_z = model_parameters(EC, TSR_type = :maximum, Z = 10.0, T = temp)
    pcm_max_z = -8
    TSr_max_z = log.(pcm_max_z ./ 100 .+ 1)
    expected_bm_max_z = [10.0, 10.0, 1.0] .* exp.(TSr_max_z .* (temp_c-20))
    @test p_max_z[:bodymass] == expected_bm_max_z

    # REVERSE
    # dry mass
    p_rev_1 = model_parameters(EC, TSR_type = :reverse, dry_mass_293 = dm, T = temp)
    pcm_rev_1 = 4
    TSr_rev_1 = log.(pcm_rev_1 ./ 100 .+ 1)
    expected_bm_rev = (dm .* 6.5) .* exp.(TSr_rev_1 .* (temp_c-20))
    @test p_rev_1[:bodymass] == expected_bm_rev
    p_rev_2 = model_parameters(EC, TSR_type = :reverse, dry_mass_293 = dm, T = temp2)
    @test p_rev_2[:bodymass] < p_rev_1[:bodymass]
    # wet mass
    p_revwm = model_parameters(EC, TSR_type = :reverse, bodymass = wm, T = temp)
    pcm_revwm = 4
    TSr_revwm = log.(pcm_revwm ./ 100 .+ 1)
    expected_bm_revwm = wm .* exp.(TSr_revwm .* (temp_c-20))
    @test p_revwm[:bodymass] == expected_bm_rev == expected_bm_revwm
    # Z
    p_rev_z = model_parameters(EC, TSR_type = :reverse, Z = 10.0, T = temp)
    pcm_rev_z = 4
    TSr_rev_z = log.(pcm_rev_z ./ 100 .+ 1)
    expected_bm_rev_z = [10.0, 10.0, 1.0] .* exp.(TSr_rev_z .* (temp_c-20))
    @test p_rev_z[:bodymass] == expected_bm_rev_z

end


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

using SpecialFunctions
using Random123
import AdaptiveRejectionSampling

"""
    struct BAT.GaussianShell <: Distribution{Multivariate,Continuous}

*Experimental feature, not part of stable public API.*

Gaussian Shell (see
[Caldwell et al.](https://arxiv.org/abs/1808.08051) for definition).

Constructors:

* ```BAT.GaussianShell(; r::Real=5, w::Real=2, n::Integer=2)```

Fields:

$(TYPEDFIELDS)

!!! note

    Fields `c` and `lognorm` are considered internal and subject to
    change without deprecation.
"""
struct GaussianShell{T<:Real, V<:AbstractVector{<:Real}, F<:AbstractFloat} <: Distribution{Multivariate,Continuous}
    "The radius of the Gaussian shell distribution."
    r::T

    "Variance of the Gaussian shell distribution"
    w::T

    "Number of dimensions"
    n::Int

    c::V

    lognorm::F
end

function GaussianShell(;r::Real=5, w::Real=2, n::Integer=2)
    c = zeros(n)
    r,w = promote(r, w)
    radial_integral(ρ) = ρ^(n-1)*exp(-(ρ - r)^2 / (2*w^2))
    density_norm = sqrt(2*π*w^2)
    # Normalization for coordinate transform
    radial_norm = (sqrt(2)*π^((n-1)/2)) / (gamma(n/2)*w)*QuadGK.quadgk(radial_integral, 0, r+w*20)[1]
    lognorm = log(density_norm) + log(radial_norm)
    GaussianShell(r, w, n, c, lognorm)
end

function _find_gs_search_range(ndims::Integer, r0::Real, w::Real)
    f(r) = gs_pdf_r(r, ndims, r0, w)
    o = AdaptiveRejectionSampling.Objective(f)

    r_grid = Array(range(maximum([r0/2, r0-w]), stop=sqrt(2*ndims)*w+r0/(w^2) + sqrt(ndims)*r0/w, length=10^6))
    y_grid = f.(r_grid)
    y_grads = o.grad.(r_grid)

    # Both sides of a concave peak
    idx1 = argmax(y_grads)
    idx2 = argmin(y_grads)

    return (r_grid[idx1], r_grid[idx2]+sqrt(ndims)*r0)
end

nball_surf_area(r, ndims) = 2 * π^(ndims/2) / gamma(ndims/2) * r^(ndims-1)
gs_pdf_r(r, ndims, r0, w) = nball_surf_area(r, ndims) * 1/(2π * w^2) * exp(-(abs(r) - r0)^2 / (2 * w^2))

function gauss_shell_radial_samples(rng::AbstractRNG, ndims::Integer, r0::Real, w::Real, n::Integer)
    f(r) = gs_pdf_r(r, ndims, r0, w)
    result = Vector{Float64}()

    # ToDo: AdaptiveRejectionSampling sometimes fails sporadically here,
    # find a fix to replace this workaround:
    ntries::Int = 0
    while length(result) < n
        ntries += 1
        try
            sampler = AdaptiveRejectionSampling.RejectionSampler(f, (10^-10, Inf), search_range=_find_gs_search_range(ndims, r0, w))
            smpls = AdaptiveRejectionSampling.run_sampler!(rng, sampler, n)
            append!(result, smpls)
        catch err
            if !(err isa AssertionError || err isa DomainError) || ntries > 4
                rethrow()
            end
        end
    end

    result
end

function rand_nball_surf_samples!(rng::AbstractRNG, X::AbstractMatrix)
    randn!(rng, X)
    X ./= sqrt.(sum(X .* X, dims = 1))
    return X
end

Distributions.rand(rng::AbstractRNG, d::GaussianShell) = vec(rand(rng, d, 1))

function Distributions.rand(rng::AbstractRNG, d::GaussianShell, n::Int)
    X = Matrix{eltype(d)}(undef, length(d), n)
    Distributions._rand!(rng, d, X)
end

function Distributions._rand!(rng::AbstractRNG, d::GaussianShell, X::AbstractMatrix)
    rand_nball_surf_samples!(rng, X)
    R = gauss_shell_radial_samples(rng, d.n, d.r, d.w, size(X,2))
    X .*= R'
    return X
end

function Distributions._logpdf(d::GaussianShell, x::AbstractArray)
    integral_result = -(sqrt(sum((x .- d.c).^2)) - d.r)^2 / (2*d.w^2)
    result = integral_result - d.lognorm
    return result
end

function Statistics.cov(d::GaussianShell)
    cov(nestedview(rand(bat_determ_rng(), bat_sampler(d), 10^5)))
end

Base.length(d::GaussianShell) = length(d.c)

Base.eltype(d::GaussianShell) = eltype(d.c)

Distributions.params(d::GaussianShell) = (d.r, d.w, d.c)


module clustering

using LinearAlgebra

export connectivity, calculate_hierarchy_matrix

function order_unique( values )
    svals = Set(reshape( values , (length(values)) ))
    rvals = []; for v in svals append!(rvals,v) end
    rvals = sort(rvals)
    return(rvals)
end

function calculate_hierarchy_matrix( distance_matrix::Array{Float64,2} , bVerbose::Bool = false )
    """ This is the saiga/pelican/panda you are looking for:
        (linkage free hierarchical clustering)
        https://github.com/richardtjornhammar/impetuous/blob/master/src/impetuous/hierarchical.py
	around line 118
    """
    NP = size(distance_matrix[:,1])[1]
    uco_v = convert( Array{Float64}, order_unique( distance_matrix ) )
    results = Dict("level"=>[] , "distance"=>[] , "hierarchy_matrix"=>Array{Int64} )
    hmat = []
    for icut in 1:length(uco_v)
        cutoff = uco_v[icut]
	outp = connectivity( distance_matrix, cutoff, bVerbose )
	append!(results["level"]           , icut   )
	append!(results["distance"]        , cutoff )
	cout = reshape( convert( Array{Int64} , outp[:,1] ) ,(1,NP))
	if icut==1
	    hmat = cout
	else
	    hmat = vcat(hmat,cout)
	end
    end
    results["hierarchy_matrix"] = hmat
    results["level"] = convert(Array{Int64,1},results["level"])
    results["distance"] = convert(Array{Float64,1},results["distance"])
    return(results)
end

function connectivity( B::Array{Float64,2} , val::Float64 , bVerbose::Bool=false )
    """
This is a cutoff based clustering algorithm. The intended use is to supply a distance matrix and a cutoff value (then becomes symmetric positive definite).  For a small distance cutoff, you should see all the parts of the system and for a large distance cutoff, you should see the entire system. It has been employed for statistical analysis work as well as the original application where it was employed to segment molecular systems.
        #
	# JULIA ADAPTATION OF MY C++ CODE THAT CAN BE FOUND IN
	# https://github.com/richardtjornhammar/RichTools/blob/master/src/cluster.cc
	# AROUND LINE 2277
	# FOR A DESCRIPTION READ PAGE 30 (16 INTERNAL NUMBERING) of:
	# https://kth.diva-portal.org/smash/get/diva2:748464/FULLTEXT01.pdf
	#
    """
    nr_sq,mr_sq = size(B)
    if nr_sq != mr_sq
        println( "::ERROR::" )
        return(-1)
    end
    N = mr_sq
    res,nvisi,s,NN,ndx,C = [],[],[],[],[],0
    #res = append!(res,[0])
    for i in 1:N
        append!(nvisi,[i])
        append!(res,[0]); append!(res,[0])
        append!(ndx,[i])
    end
    res   = convert(Array{Int64},res  )
    ndx   = convert(Array{Int64},ndx  )
    nvisi = convert(Array{Int64},nvisi)
    if bVerbose
        println(res," ",ndx," ",nvisi)
        println(nvisi[end])
    end
    while length(ndx)>0
        i = pop!(ndx)
        NN = []
        if nvisi[i]>0
            C-=1
            for j in 1:N
                if B[j,i]<=val
                    append!(NN,[j])
                end
            end
            while length(NN)>0
                k = pop!(NN)
                nvisi[k] = C
                for j in 1:N
                    if B[j,k]<=val
                        for q in 1:N
                            if nvisi[q]==j
                                append!(NN,[q])
                            end
                        end
                    end
                end
            end
        end
    end
    if bVerbose
        println("INFO ", C,"\ncluster data:")
        Nc = []
        for i in 1:-1*C
            append!(Nc,[0])
        end
    end
    for q in 1:N
        res[q*2]=q
        res[q*2-1]=nvisi[q]-C+1
        if bVerbose
	    Nc[res[q*2-1]]+=1
            println(" ",res[q*2-1],"\t",res[2*q])
        end
    end
    if bVerbose
        for i in 1:-1*C
            println("Cluster ",i," has ", Nc[i]," elements")
        end
    end
    ret = transpose(reshape(res,(2,N)))
    if bVerbose
        println( ret, size(ret) )
    end
    return ( ret )
end

end


#####
##### Advective tracer fluxes
#####

# Calculate the flux of a tracer quantity c through the faces of a cell.
# In this case, the fluxes are given by u*Ax*T̅ˣ, v*Ay*T̅ʸ, and w*Az*T̅ᶻ.
@inline advective_tracer_flux_x(i, j, k, grid, u, c) = Ax_ψᵃᵃᶠ(i, j, k, grid, u) * ℑxᶠᵃᵃ(i, j, k, grid, c)
@inline advective_tracer_flux_y(i, j, k, grid, v, c) = Ay_ψᵃᵃᶠ(i, j, k, grid, v) * ℑyᵃᶠᵃ(i, j, k, grid, c)
@inline advective_tracer_flux_z(i, j, k, grid, w, c) = Az_ψᵃᵃᵃ(i, j, k, grid, w) * ℑzᵃᵃᶠ(i, j, k, grid, c)

#####
##### Tracer advection operator
#####

"""
    div_flux(i, j, k, grid, U, c)

Calculates the divergence of the flux of a tracer quantity c being advected by
a velocity field U = (u, v, w), ∇·(Uc),

    1/V * [δxᶜᵃᵃ(Ax * u * ℑxᶠᵃᵃ(c)) + δyᵃᶜᵃ(Ay * v * ℑyᵃᶠᵃ(c)) + δzᵃᵃᶜ(Az * w * ℑzᵃᵃᶠ(c))]

which will end up at the location `ccc`.
"""
@inline function div_uc(i, j, k, grid, U, c)
    1/Vᵃᵃᶜ(i, j, k, grid) * (δxᶜᵃᵃ(i, j, k, grid, advective_tracer_flux_x, U.u, c) +
                             δyᵃᶜᵃ(i, j, k, grid, advective_tracer_flux_y, U.v, c) +
                             δzᵃᵃᶜ(i, j, k, grid, advective_tracer_flux_z, U.w, c))
end


"""
    SRational

# Examples
```jldoctest
julia> SInt(3) // SInt(5)
3//5

julia> (SInt(3) // SInt(5)) // (SInt(2) // SInt(1))
3//10
```
"""
struct SRational{N<:SInteger,D<:SInteger} <: Real end

SRational(num::SIntegerZeroType, den::SIntegerZeroType) =
    throw("invalid rational: zero($(eltype(num))//zero($(eltype(den))")

for (S,B) in SI2BI
    @eval begin
        function SRational(num::$S, den::$S)
            num2, den2 = (sign(den) < 0) ? Base.divgcd(-num, -den) : Base.divgcd(num, den)
            SRational{typeof(num),typeof(den)}()
        end
    end
end

SRational(n::SInteger, d::SInteger) = SRational(promote(n,d)...)
SRational(n::SInteger) = SRational(n,one(n))

Base.numerator(::SRational{N,D}) where {N<:SInteger,D<:SInteger} = N()
Base.denominator(::SRational{N,D}) where {N<:SInteger,D<:SInteger} = D()

Base.decompose(x::SRational) = numerator(x), SZero, denominator(x)

Base.:(//)(n::SInteger,  d::SInteger) = SRational(n,d)

function Base.:(//)(x::SRational, y::SInteger)
    xn, yn = Base.divgcd(numerator(x), y)
    xn//(denominator(x) * yn)
end

function Base.:(//)(x::SInteger,  y::SRational)
    xn, yn = Base.divgcd(x, numerator(y))
    (xn * denominator(y))//yn
end
function Base.:(//)(x::SRational, y::SRational)
    xn, yn = Base.divgcd(numerator(x), numerator(y))
    xd, yd = Base.divgcd(denominator(x), denominator(y))
    (xn * yd)//(xd * yn)
end

#//(x::SComplex,  y::SReal) = complex(real(x)//y,imag(x)//y)
#//(x::SNumber, y::SComplex) = x*conj(y)//abs2(y)

#//(X::AbstractArray, y::SNumber) = X .// y

#=
Base.one(::SRational{<:Any,T}) where {V,T<:BaseInteger} = SRational{Tuple{T(1)::T,T(1)::T},T}()
Base.zero(::SRational{<:Any,T}) where {V,T<:BaseInteger} = SRational{Tuple{T(0)::T,T(1)::T},T}()
Base.one(::Type{<:SRational{<:Any,T}}) where {V,T<:BaseInteger} = SRational{Tuple{T(1)::T,T(1)::T},T}()
Base.zero(::Type{<:SRational{<:Any,T}}) where {V,T<:BaseInteger} = SRational{Tuple{T(0)::T,T(1)::T},T}()
=#

# TODO
promote_rule(::Type{SRational{<:Any,T}}, ::Type{S}) where {T<:BaseInteger,S<:SInteger} = SRational{Any,promote_type(T,S)}
promote_rule(::Type{SRational{<:Any,T}}, ::Type{SRational{S}}) where {T<:BaseInteger,S<:BaseInteger} = SRational{Any,promote_type(T,S)}
promote_rule(::Type{SRational{<:Any,T}}, ::Type{S}) where {T<:BaseInteger,S<:SFloat} = promote_type(T, S)


using Printf, Test
using FunctionAccuracyTests

MAX_EXP(n::Val{2}, ::Type{Float32}) = 128.0f0
MAX_EXP(n::Val{2}, ::Type{Float64}) = 1024.0
MAX_EXP(n::Val{:ℯ}, ::Type{Float32}) = 88.72284f0
MAX_EXP(n::Val{:ℯ}, ::Type{Float64}) = 709.7827128933841
MAX_EXP(n::Val{10}, ::Type{Float32}) = 38.53184f0
MAX_EXP(n::Val{10}, ::Type{Float64}) = 308.25471555991675

# min_exp = T(-(exponent_bias(T)+significand_bits(T)) * log(base, big(2)))
MIN_EXP(n::Val{2}, ::Type{Float32}) = -150.0f0
MIN_EXP(n::Val{2}, ::Type{Float64}) = -1075.0
MIN_EXP(n::Val{:ℯ}, ::Type{Float32}) = -103.97208f0
MIN_EXP(n::Val{:ℯ}, ::Type{Float64}) = -745.1332191019412
MIN_EXP(n::Val{10}, ::Type{Float32}) = -45.1545f0
MIN_EXP(n::Val{10}, ::Type{Float64}) = -323.60724533877976

for (func, base) in (exp2=>Val(2), exp=>Val(:ℯ), exp10=>Val(10))
    for T in (Float32, Float64)
        xx = range(MIN_EXP(base,T),  MAX_EXP(base,T), length = 10^6);
        test_acc(func, xx)
    end
end

exp10(Float16(-3.764))
exp10(Float16(-1.76))
asinh(Float16(258.5))
asinh(Float16(-258.5))
@testset "Float16" begin
    @testset "$func" for func in (atan,sinh,cosh,tanh,asinh,
                                  exp,exp2,exp10,expm1,cbrt)
        test_acc(func, -Inf16, Inf16, tol=.501)
    end
    @testset "$func" for func in (sin, cos, tan)
        test_acc(func, -floatmax(Float16),floatmax(Float16), tol=.501)
    end
    @testset "$func" for func in (asin, acos, atanh)
        test_acc(func, -one(Float16),one(Float16), tol=.501)
    end
    test_acc(acosh, one(Float16), Inf16, tol=.501)
    @testset "$func" for func in (log, log2, log10, sqrt)
        test_acc(func, zero(Float16), Inf16, tol=.501)
    end
    test_acc(log1p, -one(Float16), Inf16, tol=.501)
end


    ####### SIMULATION OF MARGINAL ###########################################

    ### +++ INITIALIZATION +++

    ### import packages
    using Random, LinearAlgebra, RDatasets, MixedModels, Distributions, CategoricalArrays

    ### define functions
    vcov2 = function(x, xbar, σ, τ, n, γ)
        m = length(xbar)
        a = γ / τ^2 * m # 1-1 entry
        d = (sum(x.^2) - n * γ * sum(xbar.^2) ) / σ^2 # 2-2 entry
        b = γ / τ^2 / n * sum(x) # 2-1 entry
        [d -b; -b a] / (a * d - b^2) # return inverse (X^t V^-1 X)^-1
    end

    beta = function(vcov1, x, xbar, y, ybar, σ, τ, n, γ)
        c1 = γ / τ^2 * sum(ybar) # = 1^t V^-1 y
        c2 = (x' * y - n * γ * sum(ybar .* xbar))/σ^2 # = x^t V^-1 y
        vcov1 * [c1 c2]'  # (X^tV^-1X)^-1 X^tV^-1y
    end

    ### fix parameters
    p = 2
    nsim = 10000
    Δ = [0:0.1:2;]
    Random.seed!(123)

    ### +++ PER SPECIFICATION SETTING +++

    τ0 = 8^0.5
    σ0 = 4^0.5
    m = 5
    n = 5
    REdistr = "normal"

    ### variable parameters
    N = m * n

    β0 = [-4.9, 0.03]
    x = 10 * randn(N)
    xbar = vec(sum(reshape(x, n, :), dims=1) / n)

    ### conditional evaluation
    if REdistr == "normal"
        v = τ0 * randn(m)
    elseif REdistr == "tdist"
        v = rand(TDist(2), m)
    elseif REdistr == "chi2"
        v = rand(Chisq(τ0^2 / 2), m) .- τ0^2 / 2 - m
    end
    u = β0[1] .+ β0[2] * x + repeat(v, inner = n)
    μ0 = β0[1] .+ β0[2] .* xbar + v

    ### obtain conditional variance
    σ1 = zeros(10000, 1)
    τ1 = zeros(10000, 1)
    for i in 1:10000
        data = DataFrame(x = x, v = repeat(categorical(v), inner = n), y = u + σ0 * randn(N))
        model = fit(MixedModel, @formula(y ~ x + (1 | v)), data)
        τ1[i] = VarCorr(model).σρ.v.σ[1]
        σ1[i] = VarCorr(model).s
    end
    σ1 = mean(σ1) # should be (close to) σ0, but theory for conditional convergence is missing
    τ1 = mean(τ1) # should be (close to) τ0, but theory for conditional convergence is missing

    ### +++ KNOWN DELTA +++

    ###     MARGINAL COVARIANCE MATRIX

    ### helpful parameters
    γ0 = τ1^2 / (τ1^2 + σ1^2 / n)
    vcov0 = vcov2(x, xbar, σ1, τ1, n, γ0) # = (X^tV^-1X)^-1
    d0 = (1 - γ0) .* hcat(fill(1, m), xbar) # d' = l' - b'X
    dd0 = d0' * d0

    ### assemble covariance matrix parts
    g1 = γ0 * σ1^2 / n

    ### assemble inverse of covariance matrix with Woodbury
    Σ0diag = g1
    Σ0i = (I - d0 * inv(Σ0diag * inv(vcov0) + dd0) * d0' ) / Σ0diag

    ###     CONDITIONAL COVARIANCE MATRIX
    l1 = γ0^2 / n * σ1^2

    ### assemble inverse of covariance matrix with Woodbury
    cΣ0diag = l1
    cΣ0i = (I - d0 * inv(cΣ0diag * σ1^2 *
                inv(vcov0 * (2 * γ0 * I + n^2 * dd0 * vcov0)) + dd0) * d0' ) / cΣ0diag

    cΣ0sq = eigvecs(cΣ0i)' * diagm(eigvals(cΣ0i).^0.5) * eigvecs(cΣ0i)

    ###     NONCENTRALITY PARAMETER
    AZv = (γ0 - 1) .* v + n / τ1^2 .* d0 * vcov0 * d0' * v
    λ0 = AZv' * cΣ0i * AZv

    ###     QUANTILES
    qc0 = cquantile(NoncentralChisq(m, λ0), 0.05) # quantile of alpha = 0.05
    qm = cquantile(Chisq(m), 0.05) # quantile of alpha = 0.05

    ### simulation
    Coverage = zeros(5, length(Δ))
    λ = zeros(nsim, 2)
    Volume = ones(nsim, 5)

    for i in 1:nsim

        ### +++ EMPIRICAL +++

        ### variable data
        y = u + σ0 * randn(N) # conditional
        ybar = vec(sum(reshape(y, n, :), dims=1) / n)
        data = DataFrame(x = x, v = repeat(categorical(v), inner = n), y = y)

        ### fit model
        model = fit(MixedModel, @formula(y ~ x + (1 | v)), data)

        ### make sure the variance parameters are positive
        τ = maximum([VarCorr(model).σρ.v.σ[1], 0.001])
        σ = maximum([VarCorr(model).s, 0.001])

        ###     MARGINAL

        ### helpful parameters
        γ = τ^2 / (τ^2 + σ^2 / n)
        vcov1 = vcov2(x, xbar, σ, τ, n, γ) # = (X^tV^-1X)^-1
        d = (1-γ) .* hcat(fill(1, m), xbar) # d' = l' - b'X
        dd = d' * d
        a = n * τ^2 + σ^2

        ### assemble diagonal matrix parts
        g1 = γ * σ^2 / n
        g3 = 2 * σ^4 / τ^2 * γ / N/ (n - 1)

        ### assemble inverse of covariance matrix with Woodbury
        Σdiag = g1 + 2 * g3
        #Σ = Σdiag * I + d * vcov1 * d'
        Σi = (I - d * inv(Σdiag * inv(vcov1) + dd) * d' ) / Σdiag

        ###     CONDITIONAL

        ### assemble diagonal matrix parts
        l1 = γ^2 / n * σ^2

        l3_1 = 2 * σ^2 * τ^2 / N/ a^3
        l3_2 = 8 / N/ (n - 1) / a^4 / n^2 * τ^2 * σ^2 * (
            + (a^2 - σ^2) / n * (
                + σ^6 / a^2 * n
                - (σ^4  / a^2) * (τ + a / 2 * n)
                - (τ^2 - a / 2 + σ^2 * (1 + n^2)) * (n - 1))
            + σ^10 / a^2 * (n^3 - (n - 1)^2 + 1 / n)
            + a * σ^4 * (n - 1 + 1 / n / 2)  +
            - σ^6 * (2 * n - 1 + 1 / n) +
            + σ^8 / a * (1 + (n-1) * (n^2 + 1) / n) / 2)
        l3_3 = - 8 / N/ n * τ^2 * σ^4 / a^4 * (n - 1) * (a^2 - σ^4)
        l3_4 = - 8 / N/ n * τ^2 * σ^4 / a^3 / (n - 1) * (
            + a * ((n - 1) * n - 1)
            + σ^2 * (n + 1))
        l3 = 2 * (l3_1 + l3_2 + l3_3 + l3_4)

        l4 = 2 * σ^4 / N / a^2 * ( 1 / n + ((n - 1) * τ^2 +
                (σ^2 + τ^2)^2) / ((n - 1) * a^2))

        l5 = 2 * n / (n - 1) / N * σ^4 * (σ^2 - 2 * n * τ^2) / a^2

        ### assemble inverse of covariance matrix with Woodbury
        cΣdiag = maximum([l1 + l3 + l4 - l5, 1/100000])
        cΣi = (I - d * inv(cΣdiag * σ^2 *
                    inv(vcov1 * (2 * γ * I + n^2 * dd * vcov1)) + dd) * d' ) / cΣdiag

        ###     COMPUTE MEAN
        β1 = beta(vcov0, x, xbar, y, ybar, σ1, τ1, n, γ0)
        μ1 = (1 - γ0) * (β1[1] .+ β1[2] .* xbar) + γ0 * ybar

        β2 = beta(vcov1, x, xbar, y, ybar, σ, τ, n, γ) # = fixef(model)
        μ2 = (1 - γ) * (β2[1] .+ β2[2] .* xbar) + γ * ybar

        ###     NONCENTRALITY PARAMETER
        ay = d0 * β1 - (1 - γ0) * ybar
        λ[i, 1] = maximum([(ay' * cΣ0i * ay)[1] - tr(cΣ0i) * γ0^2 / n^3 / τ1^4 * σ1^2, 0])

        ay = d * β2 - (1 - γ) * ybar
        λ[i, 2] = maximum([(ay' * cΣi * ay)[1] - tr(cΣi) * σ^2 / a^2 / n, 0])

        ###     QUANTILES
        qc1 = cquantile(NoncentralChisq(m, λ[i, 1]), 0.05) # quantile of alpha = 0.05
        qc2 = cquantile(NoncentralChisq(m, λ[i, 2]), 0.05) # quantile of alpha = 0.05

        ###     COMPUTE STATISTICS
        t1 = (μ1 - μ0) .+ Δ'
        t2 = (μ2 - μ0) .+ Δ'
        Coverage[1,:] += (sum(t1' .* (t1' * Σ0i), dims = 2) .< qm) / nsim
        Coverage[2,:] += (sum(t2' .* (t2' * Σi), dims = 2) .< qm) / nsim
        tmp1 = sum(t1' .* (t1' * cΣ0i), dims = 2)
        Coverage[3,:] += (tmp1 .< qc0) / nsim
        Coverage[4,:] += (tmp1 .< qc1) / nsim
        Coverage[5,:] += (sum(t2' .* (t2' * cΣi), dims = 2) .< qc2) / nsim

        ###     COMPUTE RELATIVE VOLUME
        # (via approximations as the determinant of large matrices is numerically unstable)
        Volume[i, 3] = (Σ0diag / cΣ0diag * qm / qc0)^(-m / 2)
        Volume[i, 4] = (Σ0diag / cΣ0diag * qm / qc1)^(-m / 2)
        Volume[i, 5] = (Σdiag / cΣdiag * qm / qc2)^(-m / 2)

    end


function mdl_shaft_ode!(
    device_states,
    output_ode,
    f0::Float64,
    ω_sys,
    dynamic_device::PSY.DynamicGenerator{M, PSY.SingleMass, A, TG, P},
) where {M <: PSY.Machine, A <: PSY.AVR, TG <: PSY.TurbineGov, P <: PSY.PSS}

    #Obtain indices for component w/r to device
    local_ix = get_local_state_ix(dynamic_device, PSY.SingleMass)

    #Define internal states for component
    internal_states = @view device_states[local_ix]
    δ = internal_states[1]
    ω = internal_states[2]

    #Obtain inner variables for component
    τe = get_inner_vars(dynamic_device)[τe_var]
    τm = get_inner_vars(dynamic_device)[τm_var]

    #Get parameters
    shaft = PSY.get_shaft(dynamic_device)
    H = PSY.get_H(shaft)
    D = PSY.get_D(shaft)

    #Compute 2 states ODEs
    output_ode[local_ix[1]] = 2 * π * f0 * (ω - ω_sys)                    #15.5
    output_ode[local_ix[2]] = (1 / (2 * H)) * (τm - τe - D * (ω - ω_sys))   #15.5

    return
end

function mdl_shaft_ode!(
    device_states,
    output_ode,
    f0::Float64,
    ω_sys,
    dynamic_device::PSY.DynamicGenerator{M, PSY.FiveMassShaft, A, TG, P},
) where {M <: PSY.Machine, A <: PSY.AVR, TG <: PSY.TurbineGov, P <: PSY.PSS}

    #Obtain indices for component w/r to device
    local_ix = get_local_state_ix(dynamic_device, PSY.FiveMassShaft)

    #Define internal states for component
    internal_states = @view device_states[local_ix]
    δ = internal_states[1]
    ω = internal_states[2]
    δ_hp = internal_states[3]
    ω_hp = internal_states[4]
    δ_ip = internal_states[5]
    ω_ip = internal_states[6]
    δ_lp = internal_states[7]
    ω_lp = internal_states[8]
    δ_ex = internal_states[9]
    ω_ex = internal_states[10]

    #Obtain inner variables for component
    τe = get_inner_vars(dynamic_device)[τe_var]
    τm = get_inner_vars(dynamic_device)[τm_var]

    #Get parameters
    shaft = PSY.get_shaft(dynamic_device)
    H = PSY.get_H(shaft)
    H_hp = PSY.get_H_hp(shaft)
    H_ip = PSY.get_H_ip(shaft)
    H_lp = PSY.get_H_lp(shaft)
    H_ex = PSY.get_H_ex(shaft)
    D = PSY.get_D(shaft)
    D_hp = PSY.get_D_hp(shaft)
    D_ip = PSY.get_D_ip(shaft)
    D_lp = PSY.get_D_lp(shaft)
    D_ex = PSY.get_D_ex(shaft)
    D_12 = PSY.get_D_12(shaft)
    D_23 = PSY.get_D_23(shaft)
    D_34 = PSY.get_D_34(shaft)
    D_45 = PSY.get_D_45(shaft)
    K_hp = PSY.get_K_hp(shaft)
    K_ip = PSY.get_K_ip(shaft)
    K_lp = PSY.get_K_lp(shaft)
    K_ex = PSY.get_K_ex(shaft)

    #Compute 10 states ODEs #15.51
    output_ode[local_ix[1]] = 2.0 * π * f0 * (ω - ω_sys)

    output_ode[local_ix[2]] =
        (1.0 / (2.0 * H)) * (
            -τe - D * (ω - ω_sys) - D_34 * (ω - ω_lp) - D_45 * (ω - ω_ex) +
            K_lp * (δ_lp - δ) +
            K_ex * (δ_ex - δ)
        )

    output_ode[local_ix[3]] = 2.0 * π * f0 * (ω_hp - ω_sys)

    output_ode[local_ix[4]] =
        (1.0 / (2.0 * H_hp)) *
        (τm - D_hp * (ω_hp - ω_sys) - D_12 * (ω_hp - ω_ip) + K_hp * (δ_ip - δ_hp))

    output_ode[local_ix[5]] = 2 * π * f0 * (ω_ip - ω_sys)

    output_ode[local_ix[6]] =
        (1.0 / (2 * H_ip)) * (
            -D_ip * (ω_ip - ω_sys) - D_12 * (ω_ip - ω_hp) - D_23 * (ω_ip - ω_lp) +
            K_hp * (δ_hp - δ_ip) +
            K_ip * (δ_lp - δ_ip)
        )

    output_ode[local_ix[7]] = 2.0 * π * f0 * (ω_lp - ω_sys)

    output_ode[local_ix[8]] =
        (1.0 / (2.0 * H_lp)) * (
            -D_lp * (ω_lp - ω_sys) - D_23 * (ω_lp - ω_ip) - D_34 * (ω_lp - ω) +
            K_ip * (δ_ip - δ_lp) +
            K_lp * (δ - δ_lp)
        )

    output_ode[local_ix[9]] = 2 * π * f0 * (ω_ex - ω_sys)

    output_ode[local_ix[10]] =
        (1.0 / (2.0 * H_ex)) *
        (-D_ex * (ω_ex - ω_sys) - D_45 * (ω_ex - ω) + K_ex * (δ - δ_ex))

    return
end


push!(LOAD_PATH,"../../../../src/");
# # Bilambertian Canopy Scattering

# Using packages:
using Plots, Distributions
using CanopyOptics
theme(:ggplot2)

# Compute quadrature points:
μ,w = CanopyOptics.gauleg(20,0.0,1.0)

# Use standard planophile leaf distribution:
LD = CanopyOptics.planophile_leaves2()

# Use a Bilambertian model and specify Reflectance R and Transmission T
BiLambMod = CanopyOptics.BiLambertianCanopyScattering(R=0.4,T=0.2)

# Compute Scattering Matrices 𝐙⁺⁺, 𝐙⁻⁺ for 0th Fourier moment:
𝐙⁺⁺, 𝐙⁻⁺ = CanopyOptics.compute_Z_matrices(BiLambMod, μ, LD, 0)

# Plots matrices (shows single scattering contributions for incoming and outgoing directions)

l = @layout [a  b]
p1 = contourf(μ, μ, 𝐙⁻⁺, title="Z⁻⁺ (Reflection)", xlabel="μꜜ", ylabel="μꜛ")
p2 = contourf(μ, μ, 𝐙⁺⁺, title="Z⁺⁺ (Transmission)", xlabel="μꜛ", ylabel="μꜛ")
plot(p1, p2,  layout = l, margin=5Plots.mm)
plot!(size=(900,350))

# ### Animation over different Beta leaf distributions
steps = 0.1:0.1:5
α = [collect(steps); 5*ones(length(steps))]
β = [ 5*ones(length(steps)); reverse(collect(steps));]
x = 0:0.01:1

anim = @animate for i ∈ eachindex(α)
    LD = CanopyOptics.LeafDistribution(Beta(α[i],β[i]), 2/π)
    Z⁺⁺, Z⁻⁺ = CanopyOptics.compute_Z_matrices(BiLambMod, μ, LD, 0)
    l = @layout [a  b  c]
    p0 = plot(rad2deg.(π * x/2), pdf.(LD.LD,x), legend=false, ylim=(0,3), title="Leaf angle distribution", xlabel="Θ (degrees)")
    p1 = contourf(μ, μ, Z⁻⁺, title="Z⁻⁺ (Reflection)", xlabel="μꜜ", ylabel="μꜛ",clims=(0.5,2))
    p2 = contourf(μ, μ, Z⁺⁺, title="Z⁺⁺ (Transmission)", xlabel="μꜛ", ylabel="μꜛ",clims=(0.5,2))
    plot(p0, p1, p2,  layout = l, margin=5Plots.mm)
    plot!(size=(1100,300))
end

gif(anim, "anim_fps10.gif", fps = 10)


@reexport using Distributions
import Distributions: mean, var, logpdf, pdf, cdf, maximum, minimum, insupport, quantile, rand, rand!, params
export DiscreteUnivariateCopula, pdf_constants
export pmf_copula!, reorder_pmf

struct DiscreteUnivariateCopula{
    DistT <: DiscreteUnivariateDistribution,
    T     <: Real
    } <: DiscreteUnivariateDistribution
    d  :: DistT
    μ :: T
    σ2 :: T
    c0 :: T
    c1 :: T
    c2 :: T
    c  :: T # normalizing constant
    maxvalue :: T
    y_sample :: Vector{T}
    pmf_vec :: Vector{T}
end

"""
    DiscreteUnivariateCopula(d, c0, c1, c2)
The distribution with density `c * P(x = x) * (c0 + c1 * x + c2 * x^2)`, where `f`
is the density of the base distribution `d` and `c` is the normalizing constant.
"""
function DiscreteUnivariateCopula(
    d  :: DistT,
    c0 :: T,
    c1 :: T,
    c2 :: T) where {DistT <: DiscreteUnivariateDistribution, T <: Real}
    μ = mean(d)
    σ2 = var(d)
    c  = inv(c0 + c1 * μ + c2 * (σ2 + abs2(μ)))
    Tc = typeof(c)
    maxvalue = quantile(Base.typename(typeof(d)).wrapper(Distributions.params(d)...), 0.999999999999)
    y_sample = collect(0:maxvalue)
    pmf_vec = zeros(length(y_sample)) # marginal pmf
    DiscreteUnivariateCopula(d, Tc(μ), Tc(σ2), Tc(c0), Tc(c1), Tc(c2), c, Tc(maxvalue), Tc.(y_sample), Tc.(pmf_vec))
end

"""
    pdf_constants(Γ::Matrix{<:Real}, res::Vector{<:Real}, i::Int64, dist::DiscreteUnivariateDistribution)
This function will fill out the appropriate constants, c0, c1, c2 for each conditional distribution to form the `DiscreteUnivariateCopula` structure.
"""
function pdf_constants(Γ::Matrix{T}, res::Vector{T}, i::Int64, dist::DiscreteUnivariateDistribution) where T <: Real
    μ = Distributions.mean(dist)
    σ2 = Distributions.var(dist)
    c_0 = μ^2 * inv(σ2)
    c__0 = μ * inv(sqrt(σ2)) * sum(crossterm_res(res, i, Γ))
    c_1 = -2μ * inv(σ2)
    c__1 = inv(sqrt(σ2)) * sum(crossterm_res(res, i, Γ))
    c_2 = inv(σ2)
    storage = zeros(i-1)
    # first multiply Γ[1:i-1, 1:i-1] *  res[1:i-1]
    mul!(storage, Γ[1:i-1, 1:i-1], res[1:i-1])
    # then multiply to get transpose(res[1:i-1]) *  Γ[1:i-1, 1:i-1] *  res[1:i-1]
    c0 = 1 + 0.5 * dot(storage, res[1:i-1]) +  0.5 * tr(Γ[i+1:end, i+1:end]) + 0.5 * Γ[i, i] * c_0  - c__0
    c1 = 0.5 * Γ[i, i] * c_1  + c__1
    c2 = 0.5 * Γ[i, i] * c_2
    DiscreteUnivariateCopula(dist, c0, c1, c2)
end

"""
    pdf_constants(γ::T, dist::DiscreteUnivariateDistribution)
This function will fill out the appropriate constants, c0, c1, c2 for the univariate marginal distribution to form the `DiscreteUnivariateCopula` structure.
When the number of observations in the cluster is 1
"""
function pdf_constants(γ::T, dist::DiscreteUnivariateDistribution) where T <: Real
    μ = Distributions.mean(dist)
    σ2 = Distributions.var(dist)
    c_0 = μ^2 * inv(σ2)
    c_1 = -2μ * inv(σ2)
    c_2 = inv(σ2)
    c0 = 1 + 0.5 * γ * c_0
    c1 = 0.5 * γ * c_1
    c2 = 0.5 * γ * c_2
    DiscreteUnivariateCopula(dist, c0, c1, c2)
end

"""
    pmf_copula!(dist::DiscreteUnivariateCopula)
This function will get the appropriate probability mass function, using our copula density.
For discrete distributions with countably infinite values in the range, we want to find some approximate maximum value that is large enough so that the probability mass vector sums to about 1.
"""
function pmf_copula!(dist::DiscreteUnivariateCopula) where T<: Real
    for k in 1:length(dist.y_sample)
        dist.pmf_vec[k] = pdf(dist, dist.y_sample[k]) # get the pmf given maximum and our pdf implementation
    end
    nothing
end

"""
    reorder_pmf(pmf::Vector{<:Real}, μ)
This function will re-order the probability mass function, by sorting the vector of probabilities in decreasing order (starting with mean μ).
"""
function reorder_pmf(pmf::Vector{T}, μ) where T <: Real
    listofj = zeros(Int64, length(pmf))
    k = Integer(floor.(μ))
    reordered_pmf = zeros(length(pmf))
    i = 1
    j = k[1]
    while(i < length(pmf) && j > 0 && j < length(pmf))
        listofj[i] = j
        reordered_pmf[i] = pmf[j + 1]
        if i%2 == 1
            j = j + i
            elseif i%2 == 0
            j = j - i
        end
        i = i + 1
    end
    if j == 0
        listofj[i] = 0
        reordered_pmf[i] = pmf[1]
        for s in i+1:length(pmf)
            listofj[s] = s - 1
            reordered_pmf[s] = pmf[s]
            end
        end
    listofj, reordered_pmf
end

"""
    rand(dist::DiscreteUnivariateCopula)
This function will simulate the discrete random variable under our copula model.
"""
function rand(dist::DiscreteUnivariateCopula) where T <: Real
    pmf_copula!(dist) # get pmf under our copula density
    listofj, reordered_pmf = reorder_pmf(dist.pmf_vec, dist.μ) # re-order the pmf
    # reorder_pmf!(dist)
    sample = rand() # generate x from uniform(0, 1)
    (random_deviate, s) = listofj[1], reordered_pmf[1] # if the cumulative probability mass is less than the P(X = listofj[1]) then leave it as the mean
    for i in 2:length(reordered_pmf)
        if sample < s
            random_deviate = listofj[i - 1]
            break
        else
            s += reordered_pmf[i]
        end
    end
    random_deviate
end

"""
    rand(dist::DiscreteUnivariateCopula, n_reps::Int64)
This function will simulate the discrete random variable under our copula model, n_reps times.
"""
function rand(dist::DiscreteUnivariateCopula, n_reps::Int64) where T <: Real
    random_deviate = zeros(Int64, n_reps)
    for l in 1:n_reps
        random_deviate[l] = rand(dist)
    end
    random_deviate
end

"""
    rand!(dist::DiscreteUnivariateCopula, sample::Vector{T})
This function will write over each entry in the specified sample vector, the simulated univariate discrete values under our copula model.
"""
function rand!(dist::DiscreteUnivariateCopula, sample::Vector{T}) where T <: Real
    for i in 1:length(sample)
        sample[i] = rand(dist)
    end
    sample
end

"""
    rand(dist::DiscreteUnivariateCopula)
This function will simulate the discrete random variable under our copula model.
"""
function rand(dist::DiscreteUnivariateCopula{Bernoulli{T}, T}) where T <: Real
    random_deviate = Float64(rand(Bernoulli(QuasiCopula.mean(dist))))
    random_deviate
end

"""
    rand(dist::DiscreteUnivariateCopula, n_reps::Int64)
This function will simulate the discrete random variable under our copula model, n_reps times.
"""
function rand(dist::DiscreteUnivariateCopula{Bernoulli{T}, T}, n_reps::Int64) where T <: Real
    random_deviate = zeros(Int64, n_reps)
    for l in 1:n_reps
        random_deviate[l] = rand(dist)
    end
    random_deviate
end

"""
    rand!(dist::DiscreteUnivariateCopula, sample::Vector{T})
This function will write over each entry in the specified sample vector, the simulated univariate discrete values under our copula model.
"""
function rand!(dist::DiscreteUnivariateCopula{Bernoulli{T}, T}, sample::Vector{T}) where T <: Real
    for i in 1:length(sample)
        sample[i] = rand(dist)
    end
    sample
end


using DomainSets, GridArrays, Base.Broadcast, Test

@testset "broadcast" begin

    broadcast(in, PeriodicEquispacedGrid(10,-1,1) , UnitInterval()) == [i> 5 for i in 1:10]

    uniondomain = UnionDomain(0.0..0.1,0.5..1.0)
    @test broadcast(in, PeriodicEquispacedGrid(10,-1,1) , uniondomain) == [6<=i<=6 || 9<=i<=10 for i in 1:10]

    intersectdomain = IntersectionDomain(-1.0..0.1,0.0..1.0)
    @test broadcast(in, PeriodicEquispacedGrid(10,-1,1) , intersectdomain) == [i==6 for i in 1:10]

    diffdomain = DifferenceDomain(-1.0..0.1,0.0..1.0)
    @test broadcast(in, PeriodicEquispacedGrid(10,-1,1) , diffdomain) == [i<6 for i in 1:10]
end


using Makie
using AbstractPlotting: textslider

function target_func(a,b,x)
    (x/3-a*sin(b*x))^2
end

function main()
    s_x, obs_x = textslider(-π/2. :0.01:π/2., "x", start=0.5)
    alspace = range(-1, stop=1, length=501)
    blspace = range(-π/2, stop=π/2, length=501)
    function update!(z::Array{Float32, 2}, x)
        z = Float32[target_func(a,b,x)
                        for a in alspace, b in blspace]
        @show minimum(z)
        @show argmax(z)
        @show sum(z.-minimum(z).≈0)
        z
    end

    args_n = (obs_x,)
    pos = lift(update!, Node(zeros(Float32, (length(alspace),length(blspace)))), args_n...)
    sur = surface(alspace, blspace, pos)
    axis = sur[Axis]
    axis[:names][:axisnames] = ("a", "b", "a sin(bx)")

    RecordEvents(
        hbox(
            sur,
            vbox(s_x),
            parent=Scene()
        ),
        "output"
    )
end


module TR55

# Calculates the runoff (in) based off:
# S (potential maximum retention after runoff begins)
# P (rainfall (in))
function runoff(S::Number, P::Number)
    Q = (P - (0.2 * S))^2 / (P + (0.8 * S))

    return round(Q, 2)
end

# Calculates the potential maximum retention
# after runoff begins based off:
# CN (runoff curve number)
function max_retention(CN::Int64)
    S = (1000 / CN) - 10

    return round(S, 2)
end

# Calculates the weighted curve number
# for a watershed.
# region - array of dicts w/ the following structure:
#   [
#       {
#           "cn" => 61,
#           "area" => 30
#       },
#       {
#           "cn" => 74,
#           "area" => 70
#       },
#   ]
# area_is_percent - boolean indicating whether or not
# the region areas are measured in percent of area,
# instead of acres or sq mi, etc.
function weighted_cn(regions::Array{Dict{Any,Any}},
                     area_is_percent::Bool)

    total_area = reduce(+, map(x -> x["area"], regions))

    if area_is_percent && total_area != 100
        error("Using percents areas requires that " *
              "region areas sum to 100")
    end

    total_product = reduce(+, map(x -> x["area"] * x["cn"], regions))
    weighted_CN = total_product / total_area

    return convert(Int64, round(weighted_CN))
end

end # module


@testset "Estimation ........................... " begin

  function simonsDesign(r1, n1, r, n)
    nvec = [[n1 for x1 in 0:r1]; [n for x1 in (r1 + 1):n1]]
    cvec = [[Inf for x1 in 0:r1]; [r for x1 in (r1 + 1):n1]]
    return Design(nvec, cvec)
  end

  # Simon's designs for beta = .2, alpha = .05, p1 = p0 +0.2
  # cf. Simon, R "Optimal Two-Stage Designs for Phase II Clinical Trials",
  # Controlled Clinical Trials 10:1-10 (1989). (p. 4)
  sd = simonsDesign( 7, 16, 23, 46) # p0 = 0.4

  mle    = MLEstimator(sd)
  ube    = RBEstimator(sd)
  supp   = support(sd)
  @test all(
    estimate.(mle, supp[:, 1], supp[:, 2]) .==
      (supp[:, 1] + supp[:, 2])./samplesize.(sd, supp[:, 1])
  )
  @test maximum(abs.(bias.(ube, linspace(0, 1)))) ≈ 0.0 atol=1e-10

  ss = SampleSpace(5:20, 50)

  params = MESS(
    ss, .2, .4, alpha = .05, beta = .2, pess = .4
  )

  design = optimaldesign(10, params, solver)

  cpe    = OCEstimator(design, qsolver)

  @test maximum(rmse.(cpe, linspace(0, 1, 10))) <= 0.12

  @test length(incompatibleoutcomes(cpe)) == 0

end


using LinearAlgebra
using Random
using Distributions

# plot features require Gadfly, etc
using ApproxManifoldProducts

# more packages required
using TransformUtils

const LinAlg = LinearAlgebra
const KDE = KernelDensityEstimate
const TU = TransformUtils
const AMP = ApproxManifoldProducts

# temporarily required for experimental versions of on-manifold products
KDE.setForceEvalDirect!(true)

## create two densities

# two densities on a cylinder
p = manikde!(randn(2,100), (:Euclid, :Circular) )

pts2a = 3.0*randn(1,100).+5.0
pts2b = TransformUtils.wrapRad.(0.5*randn(1,100).+pi)
q = manikde!([pts2a;pts2b], (:Euclid, :Circular) )

# approximate the product between hybrid manifold densities
pq = manifoldProduct([p;q], (:Euclid, :Circular), Niter=2)

## better wrap around example

pts1a = 6.0*rand(1,100).-3.0
pts1b = TransformUtils.wrapRad.(0.3*randn(1,100).-0.6*pi)
p = manikde!([pts1a;pts1b], (:Euclid, :Circular) )

pts2a = 6.0*rand(1,100).-3.0
pts2b = TransformUtils.wrapRad.(0.3*randn(1,100).+0.6*pi)
q = manikde!([pts2a;pts2b], (:Euclid, :Circular) )

# approximate the product between hybrid manifold densities
pq = manifoldProduct([p;q], (:Euclid, :Circular), Niter=3)

## import plotting

using Cairo
using Gadfly, Colors
using KernelDensityEstimatePlotting

## Different plots

plotKDE([p;q; pq], levels=3, c=["red";"green";"magenta"])

plotKDE([p;q; pq], dims=[1],c=["red";"green";"magenta"])

pl = plotKDECircular( [marginal(p, [2;]);marginal(q, [2;]); marginal(pq, [2;])] )

pl = Gadfly.plot(y=pGM[1,:],x=pGM[2,:], Geom.histogram2d(xbincount=50,ybincount=30))

## export the plot to image file

pl |> PDF("/tmp/test.pdf",20cm,15cm)
@async run(`evince /tmp/test.pdf`)


@testset "PMA" begin

function factorizationcmp(F1,F2)
	@test size(F1)==size(F2)
	@test size(F1.U)==size(F2.U)
	@test size(F1.S)==size(F2.S)
	@test size(F1.V)==size(F2.V)
	@test size(F1.Vt)==size(F2.Vt)
	@test F1.S ≈ F2.S
	sgn = sign.(diag(F1.U'F2.U))
	@test F1.U.*sgn' ≈ F2.U
	@test F1.V.*sgn' ≈ F2.V
	@test F1.Vt.*sgn ≈ F2.Vt
	nothing
end

extractdims(F::SVD,dims) = SVD(F.U[:,dims], F.S[dims], F.Vt[dims,:])

A = [-8 0 6 0 0 0; -1 9 6 -2 0 -1; -1 0 -2 0 0 0; 0 1 0 1 0 -2; -9 -3 0 0 0 -5; -6 3 -8 -2 -4 8; 0 -1 -1 0 -3 -1; 4 0 0 -6 0 2]

@testset "PCA" begin
	FAns = svd(A)
	FPMA = pma(A,Diagonal(trues(6)); nsv=6)
	factorizationcmp(FAns,FPMA)
	FPMA4 = pma(A,Diagonal(trues(6)); nsv=4)
	factorizationcmp(extractdims(FAns,1:4),FPMA4)
end

@testset "properties" begin
	F = pma(A,Diagonal(trues(6)); nsv=3)
	@test all(in(propertynames(F)), fieldnames(PMA))
	@test :V in propertynames(F)
	for p in propertynames(F)
		@test getproperty(F,p)!=nothing # test that it doesn't throw
	end
end

@testset "TotalSimplexMass" begin
	A2 = hcat(repeat(A[:,1],1,5), repeat(A[:,2],1,2), A[:,3:end]) # five copies of first sample and two copies of second

	G = Matrix(Diagonal(trues(11)))
	G[1:5,1:5] .= true # Connect (identical) samples 1-5
	G[6:7,6:7] .= true # Connect (identical) samples 6-7

	FAns = svd(A2)
	# FPMA = pma(A2,G; nsv=11) # TODO: Add this test case if we add support for computing U,V columns corresponding to zero-valued singular values.
	# factorizationcmp(FAns,FPMA)
	FPMA6 = pma(A2,G; nsv=6) # non-zero singular values
	factorizationcmp(extractdims(FAns,1:6),FPMA6)
	FPMA4 = pma(A2,G; nsv=4)
	factorizationcmp(extractdims(FAns,1:4),FPMA4)
end

@testset "TotalSimplexMass2" begin
	A2 = hcat(repeat(A[:,1],1,4), A[:,2:end]) # four copies of first sample

	G = Matrix(Diagonal(trues(9)))
	G[1:4,1:4] .= LowerTriangular(ones(4,4)) # Simplices of dimensions 1-4 connecting (some of) the 4 identical samples

	FAns = svd(A2)
	# FPMA = pma(A2,G; nsv=9) # TODO: Add this test case if we add support for computing U,V columns corresponding to zero-valued singular values.
	# factorizationcmp(FAns,FPMA)
	FPMA6 = pma(A2,G; nsv=6) # non-zero singular values
	factorizationcmp(extractdims(FAns,1:6),FPMA6)
	FPMA4 = pma(A2,G; nsv=4)
	factorizationcmp(extractdims(FAns,1:4),FPMA4)
end

@testset "DisjointCliques" begin
	G = Bool[1 0 0 0 0 0; 0 1 1 0 0 0; 0 1 1 0 0 0; 0 0 0 1 1 1; 0 0 0 1 1 1; 0 0 0 1 1 1]
	# S = [1 0 0 0 0 0; 0 2/3 1/3 0 0 0; 0 1/3 2/3 0 0 0; 0 0 0 1/2 1/4 1/4; 0 0 0 1/4 1/2 1/4; 0 0 0 1/4 1/4 1/2] # corresponding S matrix
	a,b,c,d = 1/2+sqrt(1/12), 1/2-sqrt(1/12), 2/3, 1/6
	Ssq = [1 0 0 0 0 0; 0 a b 0 0 0; 0 b a 0 0 0; 0 0 0 c d d; 0 0 0 d c d; 0 0 0 d d c ]
	F = svd(A*Ssq)
	FVt = (F.U'A)./F.S
	FAns = SVD(F.U, F.S, FVt)

	FPMA = pma(A,G; nsv=6)
	factorizationcmp(FAns,FPMA)

	FPMA4 = pma(A,G; nsv=4)
	factorizationcmp(extractdims(FAns,1:4),FPMA4)
end

@testset "SymmetricSimplexGraph" begin
	G = Bool[1 0 0 0 1 0; 0 1 1 0 1 1; 0 1 1 1 1 0; 0 0 1 1 1 0; 1 1 1 1 1 1; 0 1 0 0 1 1]

	S = [8/21 1/42 1/42 1/42 4/21 1/42; 1/42 29/70 13/105 31/420 29/140 11/70; 1/42 13/105 29/70 11/70 29/140 31/420; 1/42 31/420 11/70 11/35 11/70 1/42; 4/21 29/140 29/140 11/70 32/35 11/70; 1/42 11/70 31/420 1/42 11/70 11/35]
	Ssq = sqrt(S);
	F = svd(A*Ssq)
	FVt = (F.U'A)./F.S
	FAns = SVD(F.U, F.S, FVt)

	FPMA = pma(A,G; nsv=6)
	factorizationcmp(FAns,FPMA)

	FPMA4 = pma(A,G; nsv=4)
	factorizationcmp(extractdims(FAns,1:4),FPMA4)
end

@testset "AsymmetricSimplexGraph" begin
	G = Bool[1 0 0 0 0 0; 0 1 1 1 1 0; 0 1 1 0 1 1; 0 1 1 1 0 1; 0 0 0 0 1 1; 1 0 0 0 0 1]

	S = [1/3 0 0 0 0 1/6; 0 5/6 1/4 1/3 1/12 0; 0 1/4 3/5 13/60 2/15 1/20; 0 1/3 13/60 23/30 1/20 1/20; 0 1/12 2/15 1/20 4/15 1/20; 1/6 0 1/20 1/20 1/20 13/30]
	Ssq = sqrt(S);
	F = svd(A*Ssq)
	FVt = (F.U'A)./F.S
	FAns = SVD(F.U, F.S, FVt)

	FPMA = pma(A,G; nsv=6)
	factorizationcmp(FAns,FPMA)

	FPMA4 = pma(A,G; nsv=4)
	factorizationcmp(extractdims(FAns,1:4),FPMA4)
end

@testset "AsymmetricSimplexGraph2" begin
	G = Bool[1 0 0 0 0 0; 0 0 1 1 1 0; 0 1 1 0 1 1; 0 1 1 0 0 1; 0 0 0 0 1 1; 1 0 0 0 0 0]

	S = [1/3 0 0 0 0 1/6; 0 4/3 1/6 1/12 1/12 0; 0 1/6 5/6 1/3 1/6 0; 0 1/12 1/3 2/3 1/12 0; 0 1/12 1/6 1/12 1/3 0; 1/6 0 0 0 0 1/3]
	Ssq = sqrt(S);
	F = svd(A*Ssq)
	FVt = (F.U'A)./F.S
	FAns = SVD(F.U, F.S, FVt)

	FPMA = pma(A,G; nsv=6)
	factorizationcmp(FAns,FPMA)

	FPMA4 = pma(A,G; nsv=4)
	factorizationcmp(extractdims(FAns,1:4),FPMA4)
end

end


using JOLI
using WAVEFORM
using PyPlot
using LinearAlgebra
using Flux, CUDA
using NeuralOperators

n = 101*[1;1]
d = 10.0*[1;1]
o = 0.0*[1;1]
v0 = 2000
freq = 20.0
λ = v0/freq

t0 = 0.0
f0 = 20.0
unit = "m/s"
xsrc = [(n.*d)[1]/2]
ysrc = [0.0]
zsrc = [100.0]
xrec = range(0.0,stop=(n.*d)[1]::Float64,length=n[1])
yrec = [0.0]
zrec = [100.0]
freqs = [freq]
nsrc = length(xsrc)*length(ysrc)*length(zsrc)
nfreq = length(freqs)
model = Model{Int64,Float64}(n,d,o,t0,f0,unit,freqs,xsrc,ysrc,zsrc,xrec,yrec,zrec)

comp_n = n
comp_d = d
comp_o = o
npml = round(Int,λ/minimum(comp_d))*[1 1; 1 1];
scheme = WAVEFORM.helm2d_chen9p
cut_pml = true
implicit_matrix = true
srcfreqmask = trues(nsrc,nfreq)
misfit = WAVEFORM.least_squares
lsopts = LinSolveOpts(solver=:lufact);
opts = PDEopts{Int64,Float64}(scheme,comp_n,comp_d,comp_o,cut_pml,implicit_matrix,npml,misfit,srcfreqmask,lsopts);

v = v0*ones(Float64,n...)
v[51:end, :] .= 3000
v = vec(v)

(H,comp_grid,T,DT_adj,P) = helmholtz_system(v,model,freq,opts)
nt = comp_grid.comp_n
q = zeros(eltype(H),tuple(nt...))
q[div.(nt,2)...] = 1.0
q = vec(q)
u = H\q
u = reshape(u,nt...)

clim = 10
imshow(real(u),vmin=-clim,vmax=clim)
savefig("waveform")

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

model = Chain(
    Dense(2, 64),
    FourierOperator(64=>64, (16, 16), relu),
    FourierOperator(64=>64, (16, 16), relu),
    FourierOperator(64=>64, (16, 16), relu),
    FourierOperator(64=>64, (16, 16)),
    Dense(64, 128, relu),
    Dense(128, 2)
)

# Grid
n = [121, 121]
#x = range(0.0, stop=(n[1] - 1).*d[1], length=n[1]) .* ones(1, n[2])
#y = range(0.0, stop=(n[2] - 1).*d[2], length=n[2])' .* ones(n[1], 1)

#x = reshape(x, 1, n[1], n[2], 1)
#y = reshape(y, 1, n[1], n[2], 1)
q = reshape(q, 121, 121)
x = reshape(real(q), 1, n[1], n[2], 1)
y = reshape(real(q), 1, n[1], n[2], 1)

X = cat(x, y, dims=1)
ur = reshape(real(u), 1, n[1], n[2], 1)
ui = reshape(imag(u), 1, n[1], n[2], 1)
Y = cat(ur, ui, dims=1)
Y_ = model(X)

loss(x, y) = sum(abs2, y .- model(x)) / size(x)[end]
opt = Flux.Optimiser(WeightDecay(1f-4), Flux.ADAM(1f-3))

X = X |> gpu
Y = Y |> gpu
model = model |> gpu

epochs = 500
ps = params(model)
loss_history = []
for i=1:epochs
    gs = Flux.gradient(() -> loss(X, Y), ps)
    Flux.update!(opt, ps, gs)
    global loss_history = push!(loss_history, loss(X, Y))
    print("Iter ", i, "; ", loss_history[end], "\n")
end

Y_ = model(X)
imshow(Y_[1,:,:,1], vmin=-10, vmax=10); savefig("_Y_REAL")
imshow(Y[1,:,:,1], vmin=-10, vmax=10); savefig("Y_REAL")
imshow(Y_[1,:,:,1] - Y[1,:,:,1], vmin=-1, vmax=1); savefig("Diff")

u0 = zeros(ComplexF64, length(q))
u1, res1 = FGMRES(H, q, u0; maxiter=20, precond=identity, tol=1e-2)

u0 = pre(q)#zeros(ComplexF64, length(q))
u2, res2 = FGMRES(H, q, u0; maxiter=20, precond=identity, tol=1e-2)

u0 = zeros(ComplexF64, length(q))
u3, res3 = FGMRES(H, q, u0; maxiter=20, precond=pre, tol=1e-2)

u4 = pre(u3)
subplot(2,2,1)
imshow(reshape(real(uT), 121, 121), vmin=-10, vmax=10)
subplot(2,2,2)
imshow(reshape(real(u1), 121, 121), vmin=-10, vmax=10)
subplot(2,2,3)
imshow(reshape(real(u2), 121, 121), vmin=-10, vmax=10)
subplot(2,2,4)
imshow(reshape(real(u4), 121, 121), vmin=-10, vmax=10)
savefig("comparison")

plot(res1)
plot(res2)
plot(res3)
legend(["Orig", "Apriori", "Precon"])
savefig("convergence")

u = reshape(u, 121, 121)
imshow(real.(u)); savefig("u")

function pre(x)
    xr = reshape(real(x), 1, 121, 121, 1)
    xi = reshape(imag(x), 1, 121, 121, 1)
    x = cat(xr, xi, dims=1)
    y = model(x)
    return vec(y[1,:,:,1] + im*y[2,:,:,1])
end


using LinearAlgebra;
using SparseArrays;
using IterativeSolvers;
using Krylov;
using LinearOperators;

function LaLdlt!(vX, mP, mA, vV, ρ = 1e6, σ = 1e-6;
    numFactor = 3, numIterations = 50, ϵSolver = 1e-6, numItrSolver = 500)

    # Direct Solver using Base LDLT

    numElementsX            = size(vX, 1);
    numRowsP, numColsP      = size(mP);
    numRowsA, numColsA      = size(mA);

    ρ¹ = 1 / ρ;

    hDL = ldlt([mP + (σ * sparse(I, numElementsX, numElementsX)) mA'; mA -ρ¹ * sparse(I, numRowsA, numRowsA)]);

    for ii in 1:(numFactor - 1)
        hDL = ldlt([mP + (σ * sparse(I, numElementsX, numElementsX)) mA'; mA -ρ¹ * sparse(I, numRowsA, numRowsA)]);
    end

    for ii in 1:numIterations
        vV .= hDL \ vV;
        vX .= vV[1:numElements];
    end

end

function ItrSolCg!(vX, mP, mA, vV, ρ = 1e6, σ = 1e-6;
    numFactor = 3, numIterations = 50, ϵSolver = 1e-6, numItrSolver = 500)

    # Iterative Solver using IterativeSolver cg!()

    numElementsX            = size(vX, 1);
    numRowsP, numColsP      = size(mP);
    numRowsA, numColsA      = size(mA);

    ρ¹ = 1 / ρ;

    mL = mP + (σ * sparse(I, numElementsX, numElementsX)) + (ρ * (mA' * mA));

    for ii in 1:(numFactor - 1)
        mL = mP + (σ * sparse(I, numElementsX, numElementsX)) + (ρ * (mA' * mA));
    end

    for ii in 1:numIterations
        IterativeSolvers.cg!(vX, mL, vX, abstol = ϵSolver, maxiter = numItrSolver); #<! Also compatible with LinearOperators
    end

end

function KrylovCg!(vX, mP, mA, vV, ρ = 1e6, σ = 1e-6;
    numFactor = 3, numIterations = 50, ϵSolver = 1e-6, numItrSolver = 500)

    # Iterative Solver using IterativeSolver cg!()

    numElementsX            = size(vX, 1);
    numRowsP, numColsP      = size(mP);
    numRowsA, numColsA      = size(mA);

    ρ¹ = 1 / ρ;

    mL = mP + (σ * sparse(I, numElementsX, numElementsX)) + (ρ * (mA' * mA));

    for ii in 1:(numFactor - 1)
        mL = mP + (σ * sparse(I, numElementsX, numElementsX)) + (ρ * (mA' * mA));
    end

    sCgSolver = Krylov.CgSolver(mL, vX);

    for ii in 1:numIterations
        # vT, _ = Krylov.cg(mL, vX, atol = ϵSolver, itmax = numItrSolver);
        vT, _ = Krylov.cg!(sCgSolver, mL, vX, atol = ϵSolver, itmax = numItrSolver);
        vX .= vT;
    end

end

function KrylovCr!(vX, mP, mA, vV, ρ = 1e6, σ = 1e-6;
    numFactor = 3, numIterations = 50, ϵSolver = 1e-6, numItrSolver = 500)

    # Iterative Solver using IterativeSolver cg!()

    numElementsX            = size(vX, 1);
    numRowsP, numColsP      = size(mP);
    numRowsA, numColsA      = size(mA);

    ρ¹ = 1 / ρ;

    mL = mP + (σ * sparse(I, numElementsX, numElementsX)) + (ρ * (mA' * mA));

    for ii in 1:(numFactor - 1)
        mL = mP + (σ * sparse(I, numElementsX, numElementsX)) + (ρ * (mA' * mA));
    end

    for ii in 1:numIterations
        vT, _ = Krylov.cr(mL, vX, atol = ϵSolver, itmax = numItrSolver);
        vX .= vT;
    end

end

function KrylovCgLanczos!(vX, mP, mA, vV, ρ = 1e6, σ = 1e-6;
    numFactor = 3, numIterations = 50, ϵSolver = 1e-6, numItrSolver = 500)

    # Iterative Solver using IterativeSolver cg!()

    numElementsX            = size(vX, 1);
    numRowsP, numColsP      = size(mP);
    numRowsA, numColsA      = size(mA);

    ρ¹ = 1 / ρ;

    mL = mP + (σ * sparse(I, numElementsX, numElementsX)) + (ρ * (mA' * mA));

    for ii in 1:(numFactor - 1)
        mL = mP + (σ * sparse(I, numElementsX, numElementsX)) + (ρ * (mA' * mA));
    end

    for ii in 1:numIterations
        vT, _ = Krylov.cg_lanczos(mL, vX, atol = ϵSolver, itmax = numItrSolver);
        vX .= vT;
    end

end

function LinOpCg!(vX, mP, mA, vV, ρ = 1e6, σ = 1e-6;
    numFactor = 3, numIterations = 50, ϵSolver = 1e-6, numItrSolver = 500)

    # Iterative Solver using IterativeSolver cg!()

    numElementsX            = size(vX, 1);
    numRowsP, numColsP      = size(mP);
    numRowsA, numColsA      = size(mA);

    ρ¹ = 1 / ρ;

    # Define linear operator that models u <- (P + AᵀA) w.
    # We need to allocate one temporary vector.
    vT = zeros(numRowsA); #<! Buffer
    opL = LinearOperator(Float64, numElements, numElements, true, true, (vU, vW, α, β) -> begin
    mul!(vT, mA, vW);
    mul!(vU, mA', vT);
    mul!(vU, mP, vW, one(Float64), ρ);
    vU .= vU .+ (σ .* vW);
end);

for ii in 1:(numFactor - 1)
    opL = LinearOperator(Float64, numElements, numElements, true, true, (vU, vW, α, β) -> begin
        mul!(vT, mA, vW);
        mul!(vU, mA', vT);
        mul!(vU, mP, vW, one(Float64), ρ);
        vU .= vU .+ (σ .* vW);
    end);
end

for ii in 1:numIterations
    vT, _ = Krylov.cg(opL, vX, atol = ϵSolver, itmax = numItrSolver);
    vX .= vT;
end

end

function KrylovTriCg!(vX, mP, mA, vV, ρ = 1e6, σ = 1e-6;
    numFactor = 3, numIterations = 50, ϵSolver = 1e-6, numItrSolver = 500)

    # Iterative Solver using IterativeSolver cg!()

    numElementsX            = size(vX, 1);
    numRowsP, numColsP      = size(mP);
    numRowsA, numColsA      = size(mA);

    ρ¹ = 1 / ρ;

    vVV = rand(numRowsA);

    mPI = mP + (σ * sparse(I, numElementsX, numElementsX));
    # mM = cholesky(mPI);
    mM = opCholesky(mPI);

    mN = ρ * sparse(I, numRowsA, numRowsA);

    for ii in 1:(numFactor - 1)
        mN = ρ * sparse(I, numRowsA, numRowsA);
    end

    for ii in 1:numIterations
        vXX, vVV, _ = tricg(mA', vX, vVV; M = mM, N = mN, atol = ϵSolver, itmax = numItrSolver);
        vX .= vXX;
    end

end

function KrylovTriMr!(vX, mP, mA, vV, ρ = 1e6, σ = 1e-6;
    numFactor = 3, numIterations = 50, ϵSolver = 1e-6, numItrSolver = 500)

    # Iterative Solver using IterativeSolver cg!()

    numElementsX            = size(vX, 1);
    numRowsP, numColsP      = size(mP);
    numRowsA, numColsA      = size(mA);

    ρ¹ = 1 / ρ;

    vVV = rand(numRowsA);

    mPI = mP + (σ * sparse(I, numElementsX, numElementsX));
    # mM = cholesky(mPI);
    mM = opCholesky(mPI);

    mN = ρ * sparse(I, numRowsA, numRowsA);

    for ii in 1:(numFactor - 1)
        mN = ρ * sparse(I, numRowsA, numRowsA);
    end

    for ii in 1:numIterations
        vXX, vVV, _ = trimr(mA', vX, vVV; M = mM, N = mN, atol = ϵSolver, itmax = numItrSolver);
        vX .= vXX;
    end

end


using QMTK
using QMTK.Consts.Pauli
using Compat.Test

chain = Chain(Fixed, 4)
t = kronsum(bonds(chain, 1)) do i, j
    :(σ₁[$i] * σ₁[$j])
end
h = kronprod(σ₁, σ₁, σ₀, σ₀) + kronprod(σ₀, σ₁, σ₁, σ₀) + kronprod(σ₀, σ₀, σ₁, σ₁)

@test t == h

chain = Chain(Periodic, 4)
t = kronsum(sites(chain)) do i
    :(σ₁[$i])
end

h = kronprod(σ₁, σ₀, σ₀, σ₀) + kronprod(σ₀, σ₁, σ₀, σ₀) +
    kronprod(σ₀, σ₀, σ₁, σ₀) + kronprod(σ₀, σ₀, σ₀, σ₁)
@test t == h


@testset "Filters" begin

@testset "Box filter" begin
    # Box filter always returns 1.0
    B = GRAYTR.BoxFilter(1.0, 2.0)
    @test GRAYTR.evaluate(B, 0.0, 1.0) ≈ 1.0
    @test GRAYTR.evaluate(B, 2.0, -1.0) ≈ 1.0
end

@testset "Triangle filter" begin
    T = GRAYTR.TriangleFilter(1.0, 2.0)
    @test GRAYTR.evaluate(T, 0.0, 0.0) ≈ 2.0
    @test GRAYTR.evaluate(T, 0.5, 0.0) ≈ 1.0
    @test GRAYTR.evaluate(T, 0.5, 1.0) ≈ 0.5
    @test GRAYTR.evaluate(T, 10.0, 0.0) ≈ 0.0
end

end


# Acrobot
T = Float64
model = TrajectoryOptimization.Dynamics.acrobot_model
model_d = rk3(model)
n = model.n; m = model.m

# costs
Qf = 100.0*Diagonal(I,n)
Q = (1.0e-2)*Diagonal(I,n)
R = (1e-2)*Diagonal(I,m)

x0 = [0.; 0.; 0.; 0.]
xf = [π; 0.; 0.; 0.];
N = 151
dt = 0.01
U0 = [ones(m) for k = 1:N-1]
obj = TrajectoryOptimization.LQRObjective(Q,R,Qf,xf,N)
goal = goal_constraint(xf)

acrobot = TrajectoryOptimization.Problem(model_d, obj, x0=x0, xf=xf, N=N, dt=dt)
acrobot.constraints[N] += goal
initial_controls!(acrobot, U0)


function igyp02(m)
#
#  generalization of Igarash and Ypma
#
   p = poly([10*(1+im)*ones(m); -1*ones(10-m)])
   z = [10.0*(1+im) m; -1 10-m];
   p, PolyZeros(z)
end


"Abstract barrier function type"
abstract type BarrierFunction <: CertificateFunction end

"""
    ControlBarrierFunction

Control barrier function h for a control affine system.
# Fields
- `h`: function h(x) that represents the CBF
- `∇h`: function ∇h(x) defining the gradient of the CBF
- `α`: extended class K function α(h(x))
"""
struct ControlBarrierFunction <: BarrierFunction
    h
    ∇h
    α
end

"""
	ControlBarrierFunction(h, α)

Construct a CBF given h(x) and extended class K function α(h(x)).
"""
function ControlBarrierFunction(h, α)
    ∇h(x) = gradient(h, x)[1]'
    return ControlBarrierFunction(h, ∇h, α)
end

"""
	(h::ControlBarrierFunction)(x)

Evaluate CBF at state x.
"""
function (h::ControlBarrierFunction)(x)
    return h.h(x)
end


#####
##### Terms in the turbulent kinetic energy equation, all at cell centers
#####

@inline ϕ²(i, j, k, grid, ϕ) = ϕ(i, j, k, grid)^2

@inline function shear_production(i, j, k, grid, closure, velocities, diffusivities)
    ∂z_u² = ℑxzᶜᵃᶜ(i, j, k, grid, ϕ², ∂zᵃᵃᶠ, velocities.u)
    ∂z_v² = ℑyzᵃᶜᶜ(i, j, k, grid, ϕ², ∂zᵃᵃᶠ, velocities.v)
    @inbounds Ku = diffusivities.Kᵘ[i, j, k]
    return Ku * (∂z_u² + ∂z_v²)
end

@inline function buoyancy_flux(i, j, k, grid, closure, velocities, tracers, buoyancy, diffusivities)
    @inbounds Kc = diffusivities.Kᶜ[i, j, k]
    N² = ℑzᵃᵃᶜ(i, j, k, grid, ∂z_b, buoyancy, tracers)
    return - Kc * N²
end

@inline function dissipation(i, j, k, grid, closure, velocities, tracers, buoyancy, clock, tracer_bcs)
    e = tracers.e
    FT = eltype(grid)
    @inbounds eⁱʲᵏ = e[i, j, k]
    ẽ³² = sqrt(abs(eⁱʲᵏ^3))

    ℓ = TKE_mixing_lengthᶜᶜᶜ(i, j, k, grid, closure, velocities, tracers, buoyancy, clock, tracer_bcs)
    Cᴰ = closure.Cᴰ

    return Cᴰ * ẽ³² / ℓ
end


export sunrise, sunset

include("points.jl")

# The built-in deg2rad, rad2deg functions do
# weird things when z is very large
deg2radDP(z) = z * pi/180
rad2degDP(z) = z * 180/pi

isdaylight(dt::DateTime,p::Point,tz::TimeZone) = DateTime(sunrise(Date(dt),p,tz)) <= dt < DateTime(sunset(Date(dt),p,tz))

sunrise(d::Date,p::Point,tz::TimeZone) = calcSunriseSet(true,d,latitude(p),longitude(p),tz)
sunset(d::Date,p::Point,tz::TimeZone) = calcSunriseSet(false,d,latitude(p),longitude(p),tz)

function calcSunriseSet(rise,d::Date,latitude,longitude,tz)
    calcSunriseSet(rise,datetime2julian(DateTime(d)),latitude,longitude,tz)
end

function calcSunriseSet(rise,JD,latitude,longitude,tz)
    timeUTC = calcSunriseSetUTC(rise,JD,latitude,longitude)
    newtimeUTC = calcSunriseSetUTC(rise,JD+timeUTC/1440.0,latitude,longitude)
    dt = ZonedDateTime(julian2datetime(JD+newtimeUTC/1440.0),tz"UTC")
    astimezone(dt,tz)
end

function calcSunriseSetUTC(rise,JD,latitude,longitude)
    t = calcTimeJulianCent(JD)
    eqTime = calcEquationOfTime(t)
    solarDec = calcSunDeclination(t)
    hourAngle = calcHourAngleSunrise(latitude,solarDec)
    if !rise
        hourAngle *= -1
    end
    delta = longitude + rad2degDP(hourAngle)
    720  - (4.0 * delta) - eqTime
end

calcTimeJulianCent(jd) = (jd - 2451545.0)/36525.0

function calcEquationOfTime(t)
  epsilon = calcObliquityCorrection(t)
  l0 = calcGeomMeanLongSun(t)
  e = calcEccentricityEarthOrbit(t)
  m = calcGeomMeanAnomalySun(t)

  y = tan(deg2radDP(epsilon)/2.0);
  y *= y;

  sin2l0 = sin(2.0 * deg2radDP(l0))
  sinm   = sin(deg2radDP(m))
  cos2l0 = cos(2.0 * deg2radDP(l0))
  sin4l0 = sin(4.0 * deg2radDP(l0))
  sin2m  = sin(2.0 * deg2radDP(m))

  Etime = y * sin2l0 - 2.0 * e * sinm + 4.0 * e * y * sinm * cos2l0 - 0.5 * y * y * sin4l0 - 1.25 * e * e * sin2m
  rad2degDP(Etime)*4.0
end

function calcSunDeclination(t)
  e = calcObliquityCorrection(t)
  lambda = calcSunApparentLong(t)

  sint = sin(deg2radDP(e)) * sin(deg2radDP(lambda))
  rad2degDP(asin(sint))
end

function calcObliquityCorrection(t)
  e0 = calcMeanObliquityOfEcliptic(t)
  omega = 125.04 - 1934.136 * t
  e0 + 0.00256 * cosd(omega)
end

function calcGeomMeanLongSun(t)
  L0 = 280.46646 + t * (36000.76983 + t*(0.0003032))

  while L0 > 360.0
    L0 -= 360.0
  end

  while L0 < 0.0
    L0 += 360.0
  end

  L0
end

calcEccentricityEarthOrbit(t) = 0.016708634 - t * (0.000042037 + 0.0000001267 * t)

calcGeomMeanAnomalySun(t) = 357.52911 + t * (35999.05029 - 0.0001537 * t)

function calcSunApparentLong(t)
  o = calcSunTrueLong(t)
  omega = 125.04 - 1934.136 * t
  o - 0.00569 - 0.00478 * sind(omega)
end

function calcMeanObliquityOfEcliptic(t)
  seconds = 21.448 - t*(46.8150 + t*(0.00059 - t*(0.001813)))
  23.0 + (26.0 + (seconds/60.0))/60.0
end

function calcSunTrueLong(t)
  l0 = calcGeomMeanLongSun(t)
  c = calcSunEqOfCenter(t)
  l0 + c
end

function calcSunEqOfCenter(t)
  m = calcGeomMeanAnomalySun(t)
  mrad = deg2radDP(m)
  sinm = sin(mrad)
  sin2m = sin(2mrad)
  sin3m = sin(3mrad)
  C = sinm * (1.914602 - t * (0.004817 + 0.000014 * t)) + sin2m * (0.019993 - 0.000101 * t) + sin3m * 0.000289
end

function calcHourAngleSunrise(latitude,dec)
    latRad = deg2radDP(latitude)
    sdRad  = deg2radDP(dec)
    HAarg = (cos(deg2radDP(90.833))/(cos(latRad)*cos(sdRad))-tan(latRad) * tan(sdRad))
    HA = acos(HAarg)
end


function Mn_series_coeff!(t::Transit_Struct{T}) where {T <: Real}
# Use series expansion to compute M_n. (don't need its derivative, dM_n/dk.)
# Computing leading coefficient (n=0).  Need to compute for
# n_max-3 to n_max.  Also, need to keep track of k^2 < 1
# and k^2 >= 1.  So, Mn_coeff is 3-dimensional array:
# Mn_coeff[2,4,jmax], where first dimension is for k^2 < 1 (1)
# and k^2 >= 1 (2); second dimension is for n_max-3 (1) to n_max (4),
# while third dimension is for the series coefficients.
# Computing leading coefficient (n=0):
for k2 = 0:1
coeff = zero(T)
# Loop over n_max to n_max-3:
for j=1:4
  # m is n_max to n_max-3 (we need both since downward recursion requires two):
  m = t.n_max+j-4
  mhalf = 0.5*m
  if k2 < 1
    coeff = sqrt(pi)*exp(lgamma(mhalf+1.0)-lgamma(mhalf+1.5))
    # Add leading term to M_n:
    t.Mn_coeff[1,j,1] = coeff
    # Now, compute higher order terms until desired precision is reached:
    @inbounds for i=1:t.jmax-1
      coeff *= (2*i-1)^2/(2*i*(1+m+2*i))
      t.Mn_coeff[1,j,i+1] = coeff
    end
  else # k^2 >= 1
    coeff = convert(T,pi)
    # Store leading terms:
    t.Mn_coeff[2,j,1] = coeff
    # Loop over higher order terms:
    if iseven(m)
      # If m is even, then series truncates:
      jmax = div(m,2)
    else
      jmax = t.jmax-1
    end
    for i = 1:jmax
      coeff *= (2+m-2*i)*(1-2*i)/(4*i^2)
      t.Mn_coeff[2,j,i+1] = coeff
    end
  end
end
end
end


using Mimi

@defcomp co2cycle begin
    MAT = Variable(index=[time]) # Carbon concentration increase in atmosphere (GtC from 1750)
    MATSUM = Variable(index=[time]) # Sum of MAT[t] and MAT[t+1] to use in FORC[t] for radiativeforcing component
    MU = Variable(index=[time]) # Carbon concentration increase in shallow oceans (GtC from 1750)
    ML = Variable(index=[time]) # Carbon concentration increase in lower oceans (GtC from 1750)

    E = Parameter(index=[time]) # Total CO2 emissions (GtCO2 per year)
    mat0 = Parameter() # Initial Concentration in atmosphere 2010 (GtC)
    mat1 = Parameter()
    mu0 = Parameter() # Initial Concentration in upper strata 2010 (GtC)
    ml0 = Parameter() # Initial Concentration in lower strata 2010 (GtC)

    #Flow paramaters
    b12 = Parameter() # Carbon cycle transition matrix
    b23 = Parameter() # Carbon cycle transition matrix

    #Parameters for long-run consistency of carbon cycle
    b11 = Parameter() # Carbon cycle transition matrix
    b21 = Parameter() # Carbon cycle transition matrix
    b22 = Parameter() # Carbon cycle transition matrix
    b32 = Parameter() # Carbon cycle transition matrix
    b33 = Parameter() # Carbon cycle transition matrix

    function run_timestep(p, v, d, t)
        #Define function for MAT
        if is_first(t)
            v.MAT[t] = p.mat0
        elseif t.t == 2
            v.MAT[t] = p.mat1
        else
            v.MAT[t] = v.MAT[t-1] * p.b11 + v.MU[t-1] * p.b21 + (p.E[t-1] * 10)
        end

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

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

        #Define function for MATSUM
        if is_first(t)
            v.MATSUM[t] = 0
        else
            v.MATSUM[t] = v.MAT[t] + (v.MAT[t] * p.b11 + v.MU[t] * p.b21 +  (p.E[t] * 10))
        end
    end
end


function mdl_filter_ode!(device_states,
        output_ode,
        current_r,
        current_i,
        sys_Sbase,
        f0,
        device::DynInverter{C,O,VC,DC,P,LCLFilter}) where {C <: Converter,
                                                   O <: OuterControl,
                                                   VC<: VSControl,
                                                   DC<: DCSource,
                                                   P <: FrequencyEstimator}

    #Obtain external states inputs for component
    #TODO: If converter has dynamics, need to reference states:
    #external_ix = device.input_port_mapping[device.converter]
    #vcvd = device_states[external_ix[1]]
    #vcvq = device_states[external_ix[2]]
    external_ix = device.input_port_mapping[device.filter]
    δ = device_states[external_ix[1]]

    #Obtain inner variables for component
    V_tR = device.inner_vars[VR_inv_var]
    V_tI = device.inner_vars[VI_inv_var]
    vcvd = device.inner_vars[Vdcnv_var]
    vcvq = device.inner_vars[Vqcnv_var]

    #Get parameters
    ωb = 2*pi*f0
    lf = device.filter.lf
    rf = device.filter.rf
    cf = device.filter.cf
    lg = device.filter.lg
    rg = device.filter.rg
    MVABase = get_inverter_Sbase(device)
    ωg = 1.0 #TODO: create getter later

    #RI to dq transformation
    V_dq = ri_dq(δ)*[V_tR; V_tI]
    V_g = sqrt(V_tR^2 + V_tI^2)

    #Obtain indices for component w/r to device
    local_ix = device.local_state_ix[device.filter]
    #@show local_ix

    #Define internal states for filter
    internal_states = @view device_states[local_ix]
    icvd = internal_states[1]
    icvq = internal_states[2]
     vod = internal_states[3]
     voq = internal_states[4]
     iod = internal_states[5]
     ioq = internal_states[6]

    #Inputs (control signals) - N/A

    #Compute 6 states ODEs (D'Arco EPSR122 Model)
    #Inverter Output Inductor (internal state)
    #𝜕id_c/𝜕t
    output_ode[local_ix[1]] = ( ωb/lf*vcvd
                              - ωb/lf*vod
                              - ωb*rf/lf*icvd
                              + ωb*ωg*icvq )
    #𝜕iq_c/𝜕t
    output_ode[local_ix[2]] = ( ωb/lf*vcvq
                              - ωb/lf*voq
                              - ωb*rf/lf*icvq
                              - ωb*ωg*icvd )
    #LCL Capacitor (internal state)
    #𝜕vd_o/𝜕t
    output_ode[local_ix[3]] = ( ωb/cf*icvd
                              - ωb/cf*iod  #i_gd was specified; use equivalent i_od
                              + ωb*ωg*voq )
    #𝜕vq_o/𝜕t
    output_ode[local_ix[4]] = ( ωb/cf*icvq
                              - ωb/cf*ioq  #i_gq was specified; use equivalent i_oq
                              - ωb*ωg*vod )
    #Grid Inductance (internal state)
    #𝜕id_o/𝜕t
    output_ode[local_ix[5]] = ( ωb/lg*vod
                              - ωb/lg*V_dq[2] #vgd
                              - ωb*rg/lg*iod
                              + ωb*ωg*ioq )
    #𝜕iq_o/𝜕t
    output_ode[local_ix[6]] = ( ωb/lg*voq
                              + ωb/lg*V_dq[1] #vgq
                              - ωb*rg/lg*ioq
                              - ωb*ωg*iod )

    #Update inner_vars
    device.inner_vars[Vdo_var] = vod
    device.inner_vars[Vqo_var] = voq
    #TODO: If PLL models at PCC, need to update inner vars:
    #device.inner_vars[Vdo_var] = V_dq[q::dq_ref]
    #device.inner_vars[Vqo_var] = V_dq[q::dq_ref]

    #Compute current from the generator to the grid
    I_RI = (MVABase/sys_Sbase)*dq_ri(δ)*[iod; ioq]
    #@show MVABase
    #@show sys_Sbase
    #Update current
    current_r[1] += I_RI[1]
    current_i[1] += I_RI[2]
end


using LightGraphs, MetaGraphs

function read_network_graph(instance_path::String)

    data = load_matpower(instance_path)

    ## Bus load and shunt information
    i_debut, i_fin = find_numarray(1, data)
    checkfor(data, i_debut-1, "mpc.bus")
    nb_bus = i_fin-i_debut+1
    index_bus = Dict( data[i+i_debut-1,1] => i for i=1:nb_bus)
    if index_bus != Dict( i => i for i=1:nb_bus)
        println("!!! Specific numerotation of buses !!! \n")
    end
    ## Bus generator information
    i_debut, i_fin = find_numarray(i_fin+1, data)
    checkfor(data, i_debut-1, "mpc.gen")

    #initialize network graph G
    G = MetaGraph(nb_bus)
    for i in 1:nb_bus
        set_props!(G, i, Dict(:name => "node$i"))
    end

    ## Link information
    i_debut, i_fin = find_numarray(i_fin+1, data)
    checkfor(data, i_debut-1, "mpc.branch")
    for i=i_debut:i_fin
      if data[i, 11] == 0
        #@warn("link $(data[i,1])⟶$(data[i,2]) breaker out of service !")
      else
          orig = index_bus[data[i,1]]
          dest = index_bus[data[i,2]]
          LightGraphs.add_edge!(G, orig, dest)
      end
    end
    return G
end

function read_network_graph_real_numbers(instance_path::String)

    data = load_matpower(instance_path)

    ## Bus load and shunt information
    i_debut, i_fin = find_numarray(1, data)
    checkfor(data, i_debut-1, "mpc.bus")
    nb_bus = i_fin-i_debut+1
    index_bus = Dict( data[i+i_debut-1,1] => i for i=1:nb_bus)
    if index_bus != Dict( i => i for i=1:nb_bus)
        println("!!! Specific numerotation of buses !!! \n")
    end
    ## Bus generator information
    i_debut, i_fin = find_numarray(i_fin+1, data)
    checkfor(data, i_debut-1, "mpc.gen")

    #initialize network graph G
    G = MetaGraph(2*nb_bus)

    #real parts
    for i in 1:nb_bus
        set_props!(G, i, Dict(:name => "node$(i)"))  #bus i => node i for iRe, node nb_bus+i for iIm
    end

    #imaginary parts
    for i in nb_bus+1:2*nb_bus
        set_props!(G, i, Dict(:name => "node$(i)"))
    end

    ## Link information
    i_debut, i_fin = find_numarray(i_fin+1, data)
    checkfor(data, i_debut-1, "mpc.branch")
    for i=i_debut:i_fin
      if data[i, 11] == 0
        #@warn("link $(data[i,1])⟶$(data[i,2]) breaker out of service !")
      else
          orig = index_bus[data[i,1]]
          dest = index_bus[data[i,2]]
          node_orig_Re = orig
          node_orig_Im = nb_bus + orig
          node_dest_Re = dest
          node_dest_Im = nb_bus + dest
          add_edge!(G, node_orig_Re, node_dest_Re) #1 edge in complex = 4 edges in real
          add_edge!(G, node_orig_Re, node_dest_Im) #1 edge in complex = 4 edges in real
          add_edge!(G, node_orig_Im, node_dest_Re) #1 edge in complex = 4 edges in real
          add_edge!(G, node_orig_Im, node_dest_Im) #1 edge in complex = 4 edges in real
      end
    end
    return G
end

using LinearAlgebra, SparseArrays, SuiteSparse

function read_sparsity_pattern(instance_path::String)

    data = load_matpower(instance_path)

    ## Bus load and shunt information
    i_debut, i_fin = find_numarray(1, data)
    checkfor(data, i_debut-1, "mpc.bus")
    nb_bus = i_fin-i_debut+1
    index_bus = Dict( data[i+i_debut-1,1] => i for i=1:nb_bus)

    ## Bus generator information
    i_debut, i_fin = find_numarray(i_fin+1, data)
    checkfor(data, i_debut-1, "mpc.gen")

    #initialize network graph G
    sp = spzeros(nb_bus,nb_bus)

    ## Link information
    i_debut, i_fin = find_numarray(i_fin+1, data)
    checkfor(data, i_debut-1, "mpc.branch")
    for i=i_debut:i_fin
      if data[i, 11] == 0
        #@warn("link $(data[i,1])⟶$(data[i,2]) breaker out of service !")
      else
          orig = index_bus[data[i,1]]
          dest = index_bus[data[i,2]]
          sp[orig,dest] = 1
      end
    end

    sp_sym = sp + sp'

    diag = zeros(nb_bus)

    for i=1:nb_bus
        sum_col = sum(sp_sym[i,:])
        diag[i] = Int(sum_col + 1)
    end
    return sp_sym + Diagonal(diag)
    # return sp+sp'+nb_bus*sparse(I, nb_bus, nb_bus)
end

function read_sparsity_pattern_real(instance_path::String)
    data = load_matpower(instance_path)
    ## Bus load and shunt information
    i_debut, i_fin = find_numarray(1, data)
    checkfor(data, i_debut-1, "mpc.bus")
    nb_bus = i_fin-i_debut+1
    index_bus = Dict( data[i+i_debut-1,1] => i for i=1:nb_bus)

    ## Bus generator information
    i_debut, i_fin = find_numarray(i_fin+1, data)
    checkfor(data, i_debut-1, "mpc.gen")

    sp = spzeros(2*nb_bus,2*nb_bus)

    ## Link information
    i_debut, i_fin = find_numarray(i_fin+1, data)
    checkfor(data, i_debut-1, "mpc.branch")
    for i=i_debut:i_fin
      if data[i, 11] == 0
        #@warn("link $(data[i,1])⟶$(data[i,2]) breaker out of service !")
      else
          orig = index_bus[data[i,1]]
          dest = index_bus[data[i,2]]
          node_orig_Re = orig
          node_orig_Im = nb_bus + orig
          node_dest_Re = dest
          node_dest_Im = nb_bus + dest               #bus i => node i for iRe, node nb_bus+i for iIm
          sp[node_orig_Re, node_dest_Re] = 1
          sp[node_orig_Re, node_dest_Im] = 1
          sp[node_orig_Im, node_dest_Re] = 1
          sp[node_orig_Im, node_dest_Im] = 1
      end
    end
    sp_sym = sp + sp'
    diag = zeros(2*nb_bus)
    for i=1:2*nb_bus
        sum_col = sum(sp_sym[i,:])
        diag[i] = Int(sum_col + 1)
    end
    return sp_sym + Diagonal(diag)
end

function read_correlative_sparsity_graph(instance_path::String)

    data = load_matpower(instance_path)

    ## Bus load and shunt information
    i_debut, i_fin = find_numarray(1, data)
    checkfor(data, i_debut-1, "mpc.bus")
    nb_bus = i_fin-i_debut+1
    index_bus = Dict( data[i+i_debut-1,1] => i for i=1:nb_bus)
    if index_bus != Dict( i => i for i=1:nb_bus)
        println("!!! Specific numerotation of buses !!! \n")
    end
    ## Bus generator information
    i_debut, i_fin = find_numarray(i_fin+1, data)
    checkfor(data, i_debut-1, "mpc.gen")

    #initialize network graph G
    G = MetaGraph(nb_bus)
    for i in 1:nb_bus
        set_props!(G, i, Dict(:name => "node$i"))
    end

    ## Link information
    i_debut, i_fin = find_numarray(i_fin+1, data)
    checkfor(data, i_debut-1, "mpc.branch")
    for i=i_debut:i_fin
      if data[i, 11] == 0
        #@warn("link $(data[i,1])⟶$(data[i,2]) breaker out of service !")
      else
          orig = index_bus[data[i,1]]
          dest = index_bus[data[i,2]]
          LightGraphs.add_edge!(G, orig, dest)
      end
    end

    C =  MetaGraph(nb_bus)
    for i in 1:nb_bus
        set_props!(C, i, Dict(:name => "node$i"))
    end

    for i in 1:nb_bus
        neighbors_i = neighbors(G,i)
        for u in union(i, neighbors_i)
            for v in union(i, neighbors_i)
                if !has_edge(C, u, v) && u != v
                    LightGraphs.add_edge!(C, u, v)
                end
            end
        end
    end
    return C
end

function read_correlative_sparsity_graph_real(instance_path::String)

    data = load_matpower(instance_path)

    ## Bus load and shunt information
    i_debut, i_fin = find_numarray(1, data)
    checkfor(data, i_debut-1, "mpc.bus")
    nb_bus = i_fin-i_debut+1
    index_bus = Dict( data[i+i_debut-1,1] => i for i=1:nb_bus)
    if index_bus != Dict( i => i for i=1:nb_bus)
        println("!!! Specific numerotation of buses !!! \n")
    end
    ## Bus generator information
    i_debut, i_fin = find_numarray(i_fin+1, data)
    checkfor(data, i_debut-1, "mpc.gen")

    #initialize network graph G
    G = MetaGraph(2*nb_bus)
    for i in 1:nb_bus
        set_props!(G, i, Dict(:name => "node$i"))
    end

    ## Link information
    i_debut, i_fin = find_numarray(i_fin+1, data)
    checkfor(data, i_debut-1, "mpc.branch")
    for i=i_debut:i_fin
      if data[i, 11] == 0
        #@warn("link $(data[i,1])⟶$(data[i,2]) breaker out of service !")
      else
          orig = index_bus[data[i,1]]
          dest = index_bus[data[i,2]]
          node_orig_Re = orig
          node_orig_Im = nb_bus + orig
          node_dest_Re = dest
          node_dest_Im = nb_bus + dest               #bus i => node i for iRe, node nb_bus+i for iIm
          LightGraphs.add_edge!(G, node_orig_Re, node_dest_Re)
          LightGraphs.add_edge!(G, node_orig_Re, node_dest_Im)
          LightGraphs.add_edge!(G, node_orig_Im, node_dest_Re)
          LightGraphs.add_edge!(G, node_orig_Im, node_dest_Im)
      end
    end

    C =  MetaGraph(2*nb_bus)
    for i in 1:2*nb_bus
        set_props!(C, i, Dict(:name => "node$i"))
    end

    for i in 1:2*nb_bus
        neighbors_i = neighbors(G,i)
        for u in union(i, neighbors_i)
            for v in union(i, neighbors_i)
                if !has_edge(C, u, v) && u != v
                    LightGraphs.add_edge!(C, u, v)
                end
            end
        end
    end
    return C
end


using Test
using QuantLib

mesher1 = Concentrating1dMesher(0.1, 2.5, 1001)
@test mesher1.locations[1] == range(0.1, stop=2.5, length=1001)[1]
mesher2 = Concentrating1dMesher(0.1, 2.5, 1001, (1.1, 0.25), true)
@test mesher2.locations[2] ≈ 0.10333775170976389 atol=1.0e-8


# function double_integrator_dynamics!(ẋ::AbstractVector{T},x::AbstractVector{T},u::AbstractVector{T}) where T
#     ẋ[1] = x[2]
#     ẋ[2] = u[1]
# end
#
# n = 2
# m = 1
#
# doubleintegrator = Model(double_integrator_dynamics!,n,m)

struct DoubleIntegrator{N,M} <: AbstractModel
    pos::SVector{M,Int}
    vel::SVector{M,Int}
end

function DoubleIntegrator(D=1)
    pos = SVector{D,Int}(1:D)
    vel = SVector{D,Int}(D .+ (1:D))
    DoubleIntegrator{2D,D}(pos,vel)
end

Base.size(::DoubleIntegrator{N,M}) where {N,M} = N,M

@generated function dynamics(di::DoubleIntegrator{N,M}, x, u) where {N,M}
    vel = [:(x[$i]) for i = M+1:N]
    us = [:(u[$i]) for i = 1:M]
    :(SVector{$N}($(vel...),$(us...)))
end


import Base.sin
import Base.cos
import Base.exp
import Base.log
import Base.sec
import Base.sqrt
import Base.tan
import Base.tanh
import Base.inv

begin

Xarray = Array

function sin!(x::Xarray)
    @. x = sin(x)
end

function Base.:sin(x::Xarray)
    return sin.(x)
end

function cos!(x::Xarray)
    @. x = cos(x)
end

function Base.:cos(x::Xarray)
    return cos.(x)
end

function sec!(x::Xarray)
    @. x = sec(x)
end

function Base.:sec(x::Xarray)
    return sec.(x)
end

function tan!(x::Xarray)
    @. x = tan(x)
end

function Base.:tan(x::Xarray)
    return tan.(x)
end

function tanh!(x::Xarray)
    @. x = tanh(x)
end

function Base.:tanh(x::Xarray)
    return tanh.(x)
end

function exp!(x::Xarray)
    @. x = exp(x)
end

function Base.:exp(x::Xarray)
    return exp.(x)
end

function log!(x::Xarray)
    @. x = log(x)
end

function Base.:log(x::Xarray)
    return log.(x)
end

function sqrt!(x::Xarray)
    @. x = sqrt(x)
end

function Base.:sqrt(x::Xarray)
    return sqrt.(x)
end

function inv!(x::Xarray)
    @. x = inv(x)
end

function inv(x::Xarray)
    return inv.(x)
end

end # begin


p=50;setprecision(p); setrounding(BigFloat, RoundUp);
L=BigFloat("10"); n=BigInt(100); iN=convert(Int64,n);
dx=BigFloat("2")*L/(n-1); x=convert(Array{Float64},-L:dx:L)
I=zeros(BigFloat, n, n);for i=1:n I[i,i]=BigFloat("1") end

f=sin.(x)+sin.(x./BigFloat("2"))
fi=sin.(xi)+sin.(xi./BigFloat("2"))
f+=convert(Array{BigFloat},(rand(n).-1/2)./1)

ff=3.2
typeof(ff)
ff16=convert(Float16,ff)
ff32=convert(Float32,ff)
ff16-ff32

scatter(x,f)
plot!(x,f)

i_dd=Array{Int64}(undef,0)
for i=2:length(d)-1
    if (f[i+1]-f[i])*(f[i-1]-f[i])>0
        push!(i_dd,i)
    end
end
scatter!(x[i_dd],f[i_dd])

V=TaylorBigF.calc_Vandermonde(x,n,n+BigInt(length(i_dd)))
xi=(x.+dx/BigFloat("2"))[1:end-1]
Vi=TaylorBigF.calc_Vandermonde(xi,n-1,n+BigInt(length(i_dd)))
ddV=TaylorBigF.calc_Vandermonde_dot_dot(x,n,n+BigInt(length(i_dd)))

Eq1=ddV[i_dd,:]
q1=zeros(BigFloat,length(i_dd))

VV=[V;Eq1]
q_all=[f;q1]
a=VV\q_all
plot(a)

r_root=Array{Float64}(undef,0); for i=1:Int64(n-1) push!(r_root,abs(a[i])^(1/(n-1))) end; r_rootF=convert(Array{Float64},r_root);
plot(r_rootF)
error1=maximum(abs.(f-V*a))
plot(x,f)
pred=V*a
scatter!(x[20:80],pred[20:80])
Vi=TaylorBigF.calc_Vandermonde(xi,n-1,BigInt(150));
fi_comp=Vi*a
error2=maximum(abs.(fi_comp-fi))
scatter!(xi,Vi*a)