#=
exp_mult.jl
translated from matlab/C file mri_exp_mult_mex.c
originally by Sangwoo Lee, University of Michigan, Jun, 2004
based on the 2003 file int_tim_seg.m by Brad Sutton
2004-6-21 modified by JF
2020-02 by Daniel Wan
=#

export exp_mult

"""
    D = exp_mult(A, u, v ; warnboth)

Memory efficient and fast implementation of `D = A' * exp(-u * v^T)`
that is useful for B0-field-corrected MRI image reconstruction.

in:
* `A [N L]` matrix
* `u [N]` vector
* `v [M]` vector
* `warnboth` warn if both `u` and `v` are complex; default: true

out:
* `D [L M]` complex vector: `D = A' * exp(-u * v^T)`
`D_lm = sum_n A_nl^* exp(-u_n v_m)`
"""
function exp_mult(A, u::AbstractVector{<:Number}, v::AbstractVector{<:Number}
        ; warnboth::Bool = true)

    N, L = size(A)
    n = size(u,1)
    M = size(v,1)

    ~isreal(u) && ~isreal(v) && warnboth &&
        @warn "only one of u and v should be complex"

    T = promote_type(eltype(u), eltype(v), ComplexF32)
    D = zeros(T, L, M)

    for m = 1:M
        col = exp.(-u * v[m]) # [N] mth column of N × M matrix B = exp(-u * v^T)
        D[:,m] = A' * col
    end
    return D
end


"""
estimate_condeig(V)

Estimate the condition number of an upper triangular matrix V; the matrix
V is expected to be either a `Matrix{ComplexF64}` or `Matrix{Complex{BigFloat}}`.

The condition number is first estimated converting the matrix in standard floating
point; if the estimate is not accurate enough, it is replaced with the condition number
of |V|; if the latter is too loose, it is computed in higher precision.
"""
function estimate_condeig(V::UpperTriangular)

    # Compute a floating point version of V
    VF = convert(Matrix{ComplexF64}, V)
    p = 1

    nF = opnorm(VF, p)
    kV = nF * opnorm(inv(VF), p)

    if kV <= 1e-2 / eps()
        return kV
    end

    VV = -abs.(VF)
    VV = VV - diagm(2 * diag(VV))
    kV2 = opnorm(inv(VV), p) * nF

    if kV2 <= kV * 1.0e4
        return kV2
    end

    kV3 = opnorm(VF, p) * opnorm(convert(Matrix{ComplexF64}, inv(V)), p)

    return kV3
end

"""
build_eigendecomp_tree!(T::BivMatTree, maxkV::Float64, A::Union{Matrix{ComplexF64}, Matrix{Float64}}, meth::Algorithm, use_mp = true)

This recursive routine is used to construct the eigenvector matrices related to the atomic blocks of the
matrix A (i.e., the diagonal blocks corresponding to the selected partitioning).
"""
function build_eigendecomp_tree!(T::BivMatTree, maxkV::Float64, A::Union{Matrix{ComplexF64}, Matrix{Float64}}, meth::Algorithm, use_mp = true)
    if isa(T.A11, BivMatTreeTail)

        uh = eps() / T.kV / maxkV;
        d_uh = convert(Int64, max(32, ceil(log10(1 / uh))));

        if use_mp
            ok = false
            while !ok
                with_digits(d_uh) do
                    ok = true
                    T1 = mp(T.Ttilde, d_uh)

                    # FIXME: Need to handle the 2x2 blocks in the real Schur form
                    (T.D, T.V) = eigentri(T1);
                    T.kV = estimate_condeig(T.V);

                    uh = eps() / T.kV / maxkV;
                    d_uh_new = convert(Int64, max(32, ceil(log10(1 / uh))));

                    if d_uh_new > d_uh
                        d_uh = d_uh_new
                        ok = false
                    end
                end
            end
        else
            (T.D, T.V) = eigentri(T.Ttilde);
            T.kV = cond(T.V)
        end
    else
        build_eigendecomp_tree!(T.A11, maxkV, A, meth, use_mp);
        build_eigendecomp_tree!(T.A22, maxkV, A, meth, use_mp);
    end
end

"""
build_partition_tree!(T, A, ind, strategy, method, delta = 0.1, bs = 1, mergeblocks = true)

Recursive function constructing the partition tree for a matrix A, according to the given parameters for
    separating the eigenvalues.
    """
    function build_partition_tree!(T, A, ind, strategy, method, delta = 0.1, bs = 1, mergeblocks = true)

        l = length(ind);

        maxkV = 1.0;

        if l == 1
            T.ind = ind[1];
            nblocks = 1;

            if method == Diag
                (T.Ttilde, T.kV) = condeigvec(A[T.ind, T.ind]);
                maxkV = T.kV;
            else
                T.Ttilde = A[T.ind, T.ind];
            end

            T.A11 = BivMatTreeTail();
            T.A22 = BivMatTreeTail();

            return nblocks, maxkV;
        elseif sum(map(length, ind)) <= bs
            T.ind = reduce(vcat, ind);

            nblocks = 1;

            if method == Diag
                (T.Ttilde, T.kV) = condeigvec(A[T.ind, T.ind]);
                maxkV = T.kV;
            else
                T.Ttilde = A[T.ind, T.ind];
            end

            T.A11 = BivMatTreeTail();
            T.A22 = BivMatTreeTail();

            return nblocks, maxkV;
        else
            T.ind = reduce(vcat, ind);

            if strategy == "balanced"
                v = cumsum(map(length, ind));
                _, ii = findmin(map(abs, v .- v[end]/2));
                ind1 = ind[1:ii];
                ind2 = ind[ii + 1: end];
            elseif strategy == "single"
                v = cumsum(map(length, ind[end:-1:1]));
                i = max(1, sum(v .< bs));
                ind1 = ind[1:end - i];
                ind2 = ind[end-i+1:end];
            else
                error("Unsupported strategy argument")
            end

            i1 = ind1[1][1]; i2 = ind1[end][end];
            j1 = ind2[1][1]; j2 = ind2[end][end];

            T.A11 = newBivMatTree();
            T.A22 = newBivMatTree();

            nblocks1, kV1 = build_partition_tree!(T.A11, A, ind1, strategy, method, delta, bs, mergeblocks);
            nblocks2, kV2 = build_partition_tree!(T.A22, A, ind2, strategy, method, delta, bs, mergeblocks);

            maxkV = max(kV1, kV2);

            nblocks = nblocks1 + nblocks2;

            T.sylv = sylvester(A[i1:i2, i1:i2], -A[j1:j2, j1:j2], -A[i1:i2, j1:j2])

            ninv = norm(T.sylv) / norm(A[i1:i2, j1:j2]);

            # If the Sylvester equation is
            # too ill-conditioned we merge the corresponding blocks
            if ninv > 10 / delta
                if mergeblocks
                    T.ind = reduce(vcat, ind);
                    T.sylv = nothing;
                    T.A11 = BivMatTreeTail();
                    T.A22 = BivMatTreeTail();
                    nblocks = 1;
                    if method == Diag
                        T.Ttilde, T.kV = condeigvec(A[T.ind, T.ind]);
                        maxkV = T.kV;
                    else
                        T.Ttilde = A[T.ind, T.ind]
                    end
                else
                    warn("Ill-conditioned Sylvester equation: results may be inaccurate");
                end
            end

            return nblocks, maxkV
        end
    end

    function fun2m_preprocessing(A, deltaA, method, bs, strategy)

        if istriu(A)
            TA = A; UA = one(A);
            diagTA = diag(TA);
            FA = Schur(TA, UA, diag(TA))
        else
            FA = schur(A);
            TA = FA.T; UA = FA.Z;
            diagTA = diag(TA);
        end

        # Determine reordering of the Schur forms into block forms.
        ordA = blocking(TA, deltaA);
        ordA, indA = swapping(ordA);  # Gives the blocking.
        ordA::Array{LinearAlgebra.BlasInt,1} = maximum(ordA) .- ordA .+ 1;    # Since ORDSCHUR puts highest index top left.
        ordschur!(FA, ordA)
        TA = FA.T; UA = FA.Z;

        mergeblocks = method != Taylor;

        treeA = newBivMatTree();

        nblocksA, maxkVA = build_partition_tree!(treeA, TA, indA, strategy, method, deltaA, bs, mergeblocks);

        if method == Diag || method == DiagNoHp
            use_mp = method == Diag
            build_eigendecomp_tree!(treeA, maxkVA, A, method, use_mp);
        end

        return UA, TA, treeA, nblocksA
    end

    """
    fun2m(f, A, B, C; <keyword arguments>)

    Evaluate the bivariate matrix functions X = f{A, B^T}(C); the function f should have
    the prototype:

    f = (x,y,i,j) -> f^(i,j)(x,y),

    which denotes the i-th derivative of f with respect to x, the j-th derivative with respect to y, evaluate at a point (x, y).

    Note that, unless `BivMatFun.Taylor` is used (see below), this function is always called with `i = j = 0`.

    # Arguments
    Several optional arguments can be specified to control the behavior of `fun2m`:
    - `delta::Float64 = 0.05`: Minimum separation between the eigenvalues, used to determined the partitioning.
    - `method::Algorithm = BivMatFun.Diag`: Algorithm used to evaluate the function. The default is `BivMatFun.Diag`, which implements a mixed precision perturb and diagonalize approach. Other options are `BivMatFun.Taylor` (using Taylor expansions at the atomic blocks), or `BivMatFun.DiagNoHp`, which uses perturb-and-diagonalize avoiding multiprecision. The last option is provided for benchmarks only, and is not expected to be accurate. The only option that needs to evaluate the derivatives is `BivMatFun.Taylor`. For all other cases the function `f` is always called with `i = j = 0`.
    - `bs::Union{Nothing, Int64} = nothing`: Minimum block-size to use in the partitioning. Specify `nothing` to automatically choose an optimal value based on the chosen `method`.
    - `min_digits::Int64 = 0`: Minimum number of digits to use in multiprecision computations. Use `0` (default) to not pose any constraint.
    - `strategy::String = "balanced"`: strategy to use for splitting the blocks. Valid options are "balanced" or "single".
    - `user_function = nothing`: if different from nothing, this function is used to evaluate the atomic blocks. It needs to have the prototype (A, B, C) -> X.
    - `parallel::Bool = true`: Use the multithreaded implementation.
    """
    function fun2m(f, A, B, C; delta::Float64 = 0.05, method::Algorithm = Diag, bs::Union{Nothing, Int64} = nothing,
        min_digits::Int64 = 0, strategy::String = "balanced", user_function = nothing, parallel::Bool = true)

        info = Fun2MInfo(nothing, nothing, nothing, nothing, nothing, nothing, nothing, nothing, nothing);

        # Default parameters.
        if bs === nothing
            if method == Taylor
                bs = 1;
            elseif method == Diag
                bs = 4;
            elseif method == DiagNoHp
                bs = 4;
            else
                bs = 1;
            end
        end

        # Checking input dimensions
        m = size(A, 1);
        n = size(B, 1);

        if m != size(A, 2)
            error("A must be square")
        end

        if n != size(B, 2)
            error("B must be square")
        end

        if m != size(C, 1) || n != size(C, 2)
            error("Incompatible dimensions");
        end

        # When at least one among A and B is a scalar
        if m == 1 || n == 1
            F = scalar_fun(f, A, B, C);
            return;
        end

        if parallel
            t1 = Threads.@spawn fun2m_preprocessing(A, delta, method, bs, strategy)
            t2 = Threads.@spawn fun2m_preprocessing(B, delta, method, bs, strategy)

            UA, TA, treeA, info.nblocksA = fetch(t1)
            UB, TB, treeB, info.nblocksB = fetch(t2)
        else
            UA, TA, treeA, info.nblocksA = fun2m_preprocessing(A, delta, method, bs, strategy)
            UB, TB, treeB, info.nblocksB = fun2m_preprocessing(B, delta, method, bs, strategy)
        end

        info.time_fun2mric = @timed begin

            C = UA' * C * UB;
            F = similar(C)
            info.digits = fun2m_ric!(f, F, TA, TB, C, treeA, treeB, method, min_digits, 0, user_function, parallel);
            F = UA * F * UB';

        end # End of time fun2mric

        if isreal(A) && isreal(B) && isreal(C) && norm(imag(F),1) <= 10*n*eps()*norm(F,1)
            F = real(F);
        end

        return F, info;
    end

    # ----------------------------------Recursive part---------------------------------------------------
    function fun2m_ric!(f, F, A, B, C, treeA::BivMatTree, treeB::BivMatTree, method::Algorithm, min_digits::Int64, level::Int64, user_function, parallel::Bool)
        # Computes the bivariate matrix functions on triangularized and blocked
        # coefficients via recursion and Sylvester solution
        # Base cases

        digits = 0;

        (m, n) = size(C);

        if isa(treeA, BivMatTreeTail) || isa(treeB, BivMatTreeTail)
            F .= 0;
            return digits;
        end

        iA = treeA.ind; iB = treeB.ind;

        if isa(treeA.A11, BivMatTreeTail) && isa(treeB.A11, BivMatTreeTail) # atomic block
            F[:], digits = fun2m_atom(f, A[iA, iA], B[iB, iB], C, treeA, treeB, method, min_digits, user_function)
            return digits;
        else # if at least one of the two has children

            treeA_A11 = treeA.A11; treeA_A22 = treeA.A22;
            treeB_A11 = treeB.A11; treeB_A22 = treeB.A22;

            if ! isa(treeA.A11, BivMatTreeTail)
                m1 = length(treeA.A11.ind);
            else
                m1 = length(iA);
                treeA_A11 = treeA; # copy the node into the first child
            end
            if ! isa(treeB.A11, BivMatTreeTail)
                n1 = length(treeB.A11.ind);
            else
                n1 = length(iB);
                treeB_A11 = treeB; # copy the node into the first child
            end

            # Split the problem by halving the blocks of A and B
            V1 = (treeA.sylv !== nothing) ? treeA.sylv : zeros(m1, 0)
            W1 = (treeB.sylv !== nothing) ? treeB.sylv : zeros(n1, 0)

            C11 = C[1:m1, 1:n1] + V1 * C[m1+1:end,1:n1]
            C21 = C[m1+1:end,1:n1]
            C12 = -C[1:m1,1:n1] * W1 + C[1:m1,n1+1:end] - V1 * C[m1+1:end,1:n1] * W1 + V1 * C[m1+1:end,n1+1:end]
            C22 = -C[m1+1:end,1:n1] * W1 + C[m1+1:end,n1+1:end]

            F11 = similar(C11)
            F21 = similar(C21)
            F12 = similar(C12)
            F22 = similar(C22)

            if parallel && level <= 2
                t11 = Threads.@spawn fun2m_ric!(f, F11, A, B, C11, treeA_A11, treeB_A11, method, min_digits, level+1, user_function, parallel);
                t21 = Threads.@spawn fun2m_ric!(f, F21, A, B, C21, treeA_A22, treeB_A11, method, min_digits, level+1, user_function, parallel);
                t12 = Threads.@spawn fun2m_ric!(f, F12, A, B, C12, treeA_A11, treeB_A22, method, min_digits, level+1, user_function, parallel);
                t22 = Threads.@spawn fun2m_ric!(f, F22, A, B, C22, treeA_A22, treeB_A22, method, min_digits, level+1, user_function, parallel);
                digits = max(fetch(t11), fetch(t12), fetch(t21), fetch(t22))
            else
                digits11 = fun2m_ric!(f, F11, A, B, C11, treeA_A11, treeB_A11, method, min_digits, level+1, user_function, parallel);
                digits21 = fun2m_ric!(f, F21, A, B, C21, treeA_A22, treeB_A11, method, min_digits, level+1, user_function, parallel);
                digits12 = fun2m_ric!(f, F12, A, B, C12, treeA_A11, treeB_A22, method, min_digits, level+1, user_function, parallel);
                digits22 = fun2m_ric!(f, F22, A, B, C22, treeA_A22, treeB_A22, method, min_digits, level+1, user_function, parallel);
                digits = max(digits11, digits12, digits21, digits22);
            end

            F[1:m1,1:n1] = F11 - V1 * F21;
            F[1:m1,n1+1:end] = F11 * W1 - V1 * F21 * W1 + F12 - V1 * F22;
            F[m1+1:end,1:n1] = F21;
            F[m1+1:end,n1+1:end] = F21 * W1 + F22;

            return digits;
        end

    end


export discretize,timedirichlet

# Bivariate functions have BandedMatrix
op_eltype{T,D}(sp::Space{T,D,2})=BandedMatrix{promote_type(eltype(sp),eltype(domain(sp)))}
op_eltype_realdomain{T,D}(sp::Space{T,D,2})=BandedMatrix{promote_type(eltype(sp),real(eltype(domain(sp))))}

include("OperatorSchur.jl")
include("KroneckerOperator.jl")
include("dekron.jl")
include("diagop.jl")

include("factorizations.jl")
include("pdesolve.jl")
include("kron.jl")

## PDE

lap(d)=Laplacian(d)

function Laplacian(d::Union{ProductDomain,TensorSpace},k::Integer)
    @assert length(d)==2
    Dx2=Derivative(d,[2,0])
    Dy2=Derivative(d,[0,2])
    if k==1
        LaplacianWrapper(Dx2+Dy2,k)
    else
        LaplacianWrapper((Dx2+Dy2)^k,k)
    end
end

grad(d::ProductDomain)=[Derivative(d,[1,0]),Derivative(d,[0,1])]

for op in (:dirichlet,:neumann,:diffbcs)
    @eval begin
        function $op(d::Union{ProductDomain,TensorSpace},k...)
            @assert length(d)==2
            Bx=$op(d[1],k...)
            By=$op(d[2],k...)
            if isempty(Bx)
                I⊗By
            elseif isempty(By)
                Bx⊗I
            else
                [Bx⊗I;I⊗By]
            end
        end
    end
end

function timedirichlet(d::Union{ProductDomain,TensorSpace})
    @assert length(d.domains)==2
    Bx=dirichlet(d.domains[1])
    Bt=dirichlet(d.domains[2])[1]
    [I⊗Bt;Bx⊗I]
end

function *(B::Operator,f::ProductFun)
    if isafunctional(B)
        Fun(map(c->B*c,f.coefficients),space(f,2))
    else
        ProductFun(map(c->B*c,f.coefficients),space(f))
    end
end

*(f::ProductFun,B::Operator)=(B*(f.')).'


using CalibrationErrorsDistributions
using Distances
using LinearAlgebra
using PDMats
using Random
using Test

using CalibrationErrorsDistributions:
    Wasserstein, SqWasserstein, MixtureWasserstein, SqMixtureWasserstein

Random.seed!(1234)

@testset "CalibrationErrorsDistributions" begin
    @testset "distances" begin
        @testset "Wasserstein distance" begin
            include("distances/wasserstein.jl")
        end
    end

    @testset "Normal" begin
        include("normal.jl")
    end
    @testset "MvNormal" begin
        include("mvnormal.jl")
    end

    @testset "deprecations" begin
        include("deprecated.jl")
    end
end


using Test
using PolynomialBasis
using ImplicitDomainQuadrature
# using Revise
using CutCellDG
include("../useful_routines.jl")

function reinitialization_error(distancefunction, nelmts, polyorder)
    L, W = 1.0, 1.0
    basis = LagrangeTensorProductBasis(2, polyorder)
    dim,nf = size(interpolation_points(basis))
    numqp = required_quadrature_order(polyorder) + 4
    quad = tensor_product_quadrature(2, numqp)

    mesh = CutCellDG.CGMesh([0.0, 0.0], [L, W], [nelmts, nelmts], nf)
    dx = CutCellDG.element_size(mesh)
    levelset = CutCellDG.LevelSet(distancefunction, mesh, basis)

    cutmesh = CutCellDG.CutMesh(mesh, levelset)

    refseedpoints, seedcellids =
        CutCellDG.seed_zero_levelset(2, levelset, cutmesh)
    spatialseedpoints =
        CutCellDG.map_to_spatial(refseedpoints, seedcellids, mesh)

    nodalcoordinates = CutCellDG.nodal_coordinates(cutmesh)
    boundingradius = 1.5*maximum(dx)
    tol = minimum(dx)^polyorder
    signeddistance = CutCellDG.distance_to_zero_levelset(
        nodalcoordinates,
        spatialseedpoints,
        seedcellids,
        levelset,
        tol,
        boundingradius,
    )

    err = uniform_mesh_L2_error(
        signeddistance',
        x -> distancefunction(x)[1],
        basis,
        quad,
        mesh,
    )

    den = integral_norm_on_uniform_mesh(distancefunction, quad, mesh, 1)

    return err[1] / den[1]
end

xc = [0.5, 0.5]
rad = 0.25
polyorder = 2
powers = [2, 3, 4, 5]
nelmts = [2^i + 1 for i in powers]
# nelmts = 5

dx = 1.0 ./ nelmts
# distancefunction(x) = circle_distance_function(x,xc,rad)

err = [
    reinitialization_error(
        x -> circle_distance_function(x, xc, rad),
        ne,
        polyorder,
    ) for ne in nelmts
]
rate = convergence_rate(dx, err)
@test all(rate .> 1.95)

corner = [0.5, 0.5]
err = [
    reinitialization_error(
        x -> corner_distance_function(x, corner),
        ne,
        polyorder,
    ) for ne in nelmts
]
rate = convergence_rate(dx, err)
@test all(rate .> 1.0)


@doc raw"""
    LocalLevelCycle(y::Vector{Fl}) where Fl

The local level model with a cycle component is defined by:
```math
\begin{gather*}
    \begin{aligned}
        y_{t} &=  \mu_{t} + c_{t} + \varepsilon_{t} \quad &\varepsilon_{t} \sim \mathcal{N}(0, \sigma^2_{\varepsilon})\\
        \mu_{t+1} &= \mu_{t} + \eta_{t} \quad &\eta_{t} \sim \mathcal{N}(0, \sigma^2_{\eta})\\
        c_{t+1} &= c_{t} \cos(\lambda_c) + c_{t}^{*} \sin(\lambda_c)\ \quad & \tilde\omega_{t} \sim \mathcal{N}(0, \sigma^2_{\tilde\omega})\\
        c_{t+1}^{*} &= -c_{t} \sin(\lambda_c) + c_{t}^{*} \sin(\lambda_c) \quad &\tilde\omega^*_{t} \sim \mathcal{N}(0, \sigma^2_{\tilde\omega})\\
    \end{aligned}
\end{gather*}
```

# Example
```jldoctest
julia> model = LocalLevelCycle(rand(100))
LocalLevelCycle model
```

See more on TODO RJ_TEMPERATURE

# References
 * Durbin, James, & Siem Jan Koopman. (2012). "Time Series Analysis by State Space Methods: Second Edition." Oxford University Press. pp. 48
"""
mutable struct LocalLevelCycle <: StateSpaceModel
    hyperparameters::HyperParameters
    system::LinearUnivariateTimeInvariant
    results::Results

    function LocalLevelCycle(y::Vector{Fl}) where Fl

        # Define system matrices
        Z = Fl[1, 1, 0]
        T = Fl[
            1 0 0
            0 0 0
            0 0 0
        ]
        R = Matrix{Fl}(I, 3, 3)
        d = zero(Fl)
        c = zeros(Fl, 3)
        H = one(Fl)
        Q = Matrix{Fl}(I, 3, 3)

        system = LinearUnivariateTimeInvariant{Fl}(y, Z, T, R, d, c, H, Q)

        # Define hyperparameters names
        names = ["sigma2_ε", "sigma2_η", "sigma2_ω", "λ_c"]
        hyperparameters = HyperParameters{Fl}(names)

        return new(hyperparameters, system, Results{Fl}())
    end
end

# Obligatory methods
function default_filter(model::LocalLevelCycle)
    Fl = typeof_model_elements(model)
    a1 = zeros(Fl, num_states(model))
    P1 = Fl(1e6) .* Matrix{Fl}(I, num_states(model), num_states(model))
    steadystate_tol = Fl(1e-5)
    return UnivariateKalmanFilter(a1, P1, num_states(model), steadystate_tol)
end

function initial_hyperparameters!(model::LocalLevelCycle)
    Fl = typeof_model_elements(model)
    observed_variance = var(model.system.y[findall(!isnan, model.system.y)])
    initial_hyperparameters = Dict{String,Fl}(
        "sigma2_ε" => observed_variance,
        "sigma2_η" => observed_variance,
        "sigma2_ω" => one(Fl),
        # Durbin and Koopman (2012) comment possible values
        # in their book pp. 48
        "λ_c" => Fl(2 * pi / 12),
    )
    set_initial_hyperparameters!(model, initial_hyperparameters)
    return model
end

function constrain_hyperparameters!(model::LocalLevelCycle)
    Fl = typeof_model_elements(model)
    constrain_variance!(model, "sigma2_ε")
    constrain_variance!(model, "sigma2_η")
    constrain_variance!(model, "sigma2_ω")
    # Durbin and Koopman (2012) comment possible values in their book pp. 48
    constrain_box!(model, "λ_c", Fl(2 * pi / 100), Fl(2 * pi / 1.5))
    return model
end

function unconstrain_hyperparameters!(model::LocalLevelCycle)
    Fl = typeof_model_elements(model)
    unconstrain_variance!(model, "sigma2_ε")
    unconstrain_variance!(model, "sigma2_η")
    unconstrain_variance!(model, "sigma2_ω")
    # Durbin and Koopman (2012) comment possible values in their book pp. 48
    unconstrain_box!(model, "λ_c", Fl(2 * pi / 100), Fl(2 * pi / 1.5))
    return model
end

function fill_model_system!(model::LocalLevelCycle)
    model.system.H = get_constrained_value(model, "sigma2_ε")
    model.system.Q[1] = get_constrained_value(model, "sigma2_η")
    model.system.Q[5] = get_constrained_value(model, "sigma2_ω")
    model.system.Q[9] = get_constrained_value(model, "sigma2_ω")
    model.system.T[5] = cos(get_constrained_value(model, "λ_c"))
    model.system.T[6] = -sin(get_constrained_value(model, "λ_c"))
    model.system.T[8] = sin(get_constrained_value(model, "λ_c"))
    model.system.T[9] = cos(get_constrained_value(model, "λ_c"))
    return model
end

function reinstantiate(::LocalLevelCycle, y::Vector{Fl}) where Fl
    return LocalLevelCycle(y)
end

has_exogenous(::LocalLevelCycle) = false


@testset "Augmented Unscented Kalman filter" begin

    @testset "Time update" begin

        x = randn(6)
        A = randn(6,6)
        P = A'A
        B = randn(6,6)
        Q = B'B
        F = randn(6,6)
        f(x) = F * x
        f(x, noise) = F * x .+ noise

        tu = time_update(x, P, F, Q)
        tu_aug = time_update(x, P, f, Augment(Q))
        @test get_covariance(tu_aug) ≈ get_covariance(tu)
        @test get_state(tu_aug) ≈ get_state(tu)

        f!(y, x) = mul!(y, F, x)
        f!(y, x, noise) = y.= @~ F * x .+ noise
        tu_inter = AUKFTUIntermediate(6)
        tu_aug_inplace = time_update!(tu_inter, x, P, f!, Augment(Q))
        @test get_covariance(tu_aug_inplace) ≈ get_covariance(tu)
        @test get_state(tu_aug_inplace) ≈ get_state(tu)
    end

    @testset "Measurement update" begin

        x = randn(6)
        A = randn(6,6)
        P = A'A
        B = randn(3,3)
        R = B'B
        y = randn(3)
        H = randn(3,6)
        h(x) = H * x
        h(x, noise) = H * x .+ noise

        mu = measurement_update(x, P, y, H, R)
        mu_aug = measurement_update(x, P, y, h, Augment(R))
        @test get_covariance(mu_aug) ≈ get_covariance(mu)
        @test get_state(mu_aug) ≈ get_state(mu)

        h!(y, x) = mul!(y, H, x)
        h!(y, x, noise) = y .= @~ H * x .+ noise
        mu_inter = AUKFMUIntermediate(6, 3)
        mu_ukf_inplace = measurement_update!(mu_inter, x, P, y, h!, Augment(R))
        @test get_covariance(mu_ukf_inplace) ≈ get_covariance(mu)
        @test get_state(mu_ukf_inplace) ≈ get_state(mu)
    end
end


export solve_with_linear_programming;
using JuMP
using GLPK;

"""
Solve the TSP using linear programming for a given cost matrix!!!
Returns a CheapestRute struct with the rute, the total rute cost and the
cost per interval for the computed rute.
"""
function solve_with_linear_programming(costmatrix::Matrix{T})::CheapestRute where T
    checkmatrix(costmatrix);

    #matrix size
    ms = size(costmatrix,1);

    #Lineal Programming problem
    model = Model(with_optimizer(GLPK.Optimizer));

    #Desition variables
    @variable(model, x[1:ms,1:ms], Bin);

    #One go one left constraints
    @constraint(model, [j=1:ms], sum(x[i,j] for i in 1:ms) == 1);
    @constraint(model, [i=1:ms], sum(x[i,j] for j in 1:ms) == 1);

    #Subtour elimination constraints
    #One constraints
    @constraint(model, [i=1:ms], x[i,i] == 0);

    #Others
    @variable(model, u[1:ms]);
    rhv = ms - 1; #rigth hand value
    for i in 1:ms
        for j in 2:ms
            if i != j
                @constraint(model, u[i] - u[j] + ms*x[i,j] <= rhv)
            end
        end
    end

    #Objetive function
    @objective(model, Min, sum(costmatrix[i,j] * x[i,j] for i in 1:ms, j in 1:ms));
    optimize!(model);

    #Result
    bm = value.(x);

    #Reformating
    cheapest_rute = pick_rute!(bm);
    return CheapestRute(cheapest_rute, rute_total_cost(costmatrix, cheapest_rute), rute_stretches_costs(costmatrix, cheapest_rute));

end


function calculate_losses!(result, data)
    for (l, branch) in result["solution"]["branch"]
        branch["ploss"] = branch["pf"] + branch["pt"]
    end
    result["totalloss"] = sum(branch["ploss"] for (l, branch) in result["solution"]["branch"])

    result["totalload"] = sum(load["pd"] for (l, load) in data["load"])

    result["totalgen"] = sum(gen["pg"] for (l, gen) in result["solution"]["gen"])

    return result["totalloss"]
end

"This function accepts a data dictionary, and returns a new data dictionary
with perturbation applied"
function create_impedance_perturbation(data_input, α, ϵ, λ)
    # Create a copy of the input so that a new dataset is returned
    data = deepcopy(data_input)

    # First apply the Laplace noise to each branch
    distribution = Distributions.Laplace(0, 3 * α / ϵ)
    sum_g = 0
    sum_b = 0
    sum_g_shunt = 0
    sum_b_shunt = 0
    for (l, branch) in data["branch"]
        noise = Random.rand(distribution, 1)[1]
        noise_shunt = Random.rand(distribution, 1)[1]
        z = branch["br_r"] + im*branch["br_x"]
        y = 1 / z
        g = real(y)
        b = imag(y)
        g_shunt = branch["g_to"] #assumes it is equal to branch["g_fr"]
        b_shunt = branch["b_to"] #assumes it is equal to branch["b_fr"]
        # println("g is: ", g)
        # println("b is: ", b)

        # Handle the case where resistance is 0 to avoid undefined behaviour
        if branch["br_r"] != 0
            r = b / g # Algorithm 1 and eq. (15)
            pert_b = b + noise
            pert_g = pert_b / r
        else
            pert_g = 0
            pert_b = b + noise # Just apply perturbation to b
        end

        pert_b_shunt = b_shunt + noise_shunt
        pert_g_shunt = g_shunt # Removed the perturbation of g_shunt

        branch["g_obj"] = pert_g
        branch["b_obj"] = pert_b
        branch["g_shunt_obj"] = pert_g_shunt
        branch["b_shunt_obj"] = pert_b_shunt
        # println("perturbed g is: ", pert_g)
        # println("perturbed b is: ", pert_b)
        sum_g += g
        sum_b += b
        sum_g_shunt += g_shunt
        sum_b_shunt += b_shunt
    end

    # Apply the noisy mean limit values for s4 and s5
    n = length(data["branch"])
    lap_μ = Distributions.Laplace(0, 3 * α / (n * ϵ))

    μ_g = (1 / n) * sum_g + Random.rand(lap_μ, 1)[1] # Eq 16
    μ_b = (1 / n) * sum_b + Random.rand(lap_μ, 1)[1] # Eq 17
    μ_g_shunt = (1 / n) * sum_g_shunt + Random.rand(distribution, 1)[1]
    μ_b_shunt = (1 / n) * sum_b_shunt + Random.rand(distribution, 1)[1]
    # In the case that random perturbations create negative values, floor these at 0
    if μ_g_shunt < 0
        μ_g_shunt = 0
    end
    if μ_b_shunt < 0
        μ_b_shunt = 0
    end

    # Add noisy mean limit values to our data dictionary
    # Note that we use min and max to handle negative parameters
    data["g_lb"] = min(μ_g / λ, μ_g * λ)
    data["g_ub"] = max(μ_g / λ, μ_g * λ)
    data["b_lb"] = min(μ_b / λ, μ_b * λ)
    data["b_ub"] = max(μ_b / λ, μ_b * λ)
    data["g_lb_shunt"] = min(μ_g_shunt / λ, μ_g_shunt * λ)
    data["g_ub_shunt"] = max(μ_g_shunt / λ, μ_g_shunt * λ)
    data["b_lb_shunt"] = min(μ_b_shunt / λ, μ_b_shunt * λ)
    data["b_ub_shunt"] = max(μ_b_shunt / λ, μ_b_shunt * λ)
    @show data["b_lb_shunt"], data["b_ub_shunt"]

    return data
end

function overwrite_impedances_in_data!(result, data)
    for (l, branch) in result["solution"]["branch"]
        y = branch["g"] + im* branch["b"]
        z = 1/y
        r = real(z)
        x = imag(z)
        b = branch["b_shunt"]
        data["branch"][l]["br_r"] = r
        data["branch"][l]["br_x"] = x
        data["branch"][l]["b_fr"] = b
        data["branch"][l]["b_to"] = b
    end
end


module IntegrateTests

using Test
using Gridap.Arrays
using Gridap.Fields
using Gridap.Fields: OtherMockBasis, MockBasis, MockField
using FillArrays
using Gridap.TensorValues

p1 = Point(2,2)
p2 = Point(4,2)
p3 = Point(1,3)
p4 = Point(5,2)
x = [p1,p2,p3,p4]
np = length(x)

d = 2
v = 3.0
ndof = 8
i = MockField{d}(v)
w = fill(1/np,np)
j = TensorValue(1.0,0.0,0.0,1.0)

@test integrate(i,x,w,j) == 9.0

ri = 5.0
r = MockBasis{d}(ri,ndof)

@test integrate(r,x,w,j) == fill(15.0,ndof)

l = 10
ai = Fill(i,l)
aw = fill(w,l)
aj = fill(j,l)
ax = fill(x,l)
ar = Fill(r,l)

s = integrate(ai,ax,aw,aj)
test_array(s,fill(9.0,l))

s = integrate(ar,ax,aw,aj)
test_array(s,fill(fill(15.0,ndof),l))

c = fill(1.0,ndof)
f = OtherMockBasis{d}(ndof)
ϕ = lincomb(f,c)
j = ∇(ϕ)
w = fill(1/np,np)
b = attachmap(r,ϕ)

@test integrate(i,x,w,j) == 38016.0

@test integrate(b,x,w,j) == fill(63360.0,ndof)

l = 10
ac = fill(c,l)
af = Fill(f,l)
aϕ = lincomb(af,ac)
aj = ∇(aϕ)
aw = Fill(w,l)
ax = Fill(x,l)
ar = Fill(r,l)
ab = attachmap(ar,aϕ)

s = integrate(ab,ax,aw,aj)
test_array(s,fill(fill(63360.0,ndof),l))

end # module


using SimpleGF2
using LinearAlgebra
using Test

A = ones(GF2,3,3)
@test det(A) == GF2(4)
@test rank(A) == 1
@test nullity(A) == 2
@test A*A==A
@test A+A == 0A
B = [ 1 1 1; 0 0 0; 0 0 0]
@test rref(A) == GF2.(B)
@test size(nullspace(A)) == (3,2)
v = ones(GF2,3)
x = solve(A,v)
@test A*x == v

@test GF2(3) == 5


using DiffEqFinancial, StochasticDiffEq
using Test

# write your own tests here
u0 = [1.;0.5]
σ =  0.25
prob = HestonProblem(1.,1.,σ,1.,1.,u0,(0.0,1.0))
sol = solve(prob,SRIW1(),adaptive=false,dt=1/10)

prob = BlackScholesProblem((t)->t^2,(t,u)->1.,σ,0.5,(0.0,1.0))
sol = solve(prob,SRIW1())


"""
    F = JopSigmoid(spc, c)

where `F` is the sigmoid operator 1/(1+e^(-c/x)) with domain and range given by `spc::JetSpace`, and scalar value c.
"""
function JopSigmoid(spc::JetSpace{T}, c = 1) where {T}
    tmp = zeros(spc)
    JopNl(dom = spc, rng = spc, f! = JopSigmoid_f!, df! = JopSigmoid_df!, df′! = JopSigmoid_df′!, s = (c=T(c), tmp=tmp))
end
export JopSigmoid

JopSigmoid_f!(d::AbstractArray{T}, m::AbstractArray{T}; c, tmp) where {T} = d .= (T(1) .+ exp.(-m ./ c)).^T(-1)

function JopSigmoid_df!(δd::AbstractArray{T}, δm::AbstractArray{T}; mₒ, c, tmp) where {T}
    tmp .= ( T(1) .+ exp.(-mₒ ./ c) ).^T(-1)
    δd .= δm .* (T(1) ./ c) .* (tmp .* (T(1) .- tmp))
end

function JopSigmoid_df′!(δm::AbstractArray{T}, δd::AbstractArray{T}; mₒ, c, tmp) where {T}
    tmp .= ( T(1) .+ exp.(-mₒ ./ c) ).^T(-1)
    δm .= δd .* conj((T(1) ./ c) .* (tmp .* (T(1) .- tmp)))
end


using SolveDSGE

filename = "model1b.txt"
path = joinpath(@__DIR__,filename)
process_model(path)
dsge = retrieve_processed_model(path)

x = [0.05, 21.4, 1.97, 2.8]

tol = 1e-8
maxiters = 1000
ss_obj = compute_steady_state(dsge,x,tol,maxiters)
ss = ss_obj.zero

N   = PerturbationScheme(ss,1.0,"first")
NN  = PerturbationScheme(ss,1.0,"second")
NNN = PerturbationScheme(ss,1.0,"third")

soln_fo = solve_model(dsge,N)
soln_so = solve_model(dsge,NN)
soln_to = solve_model(dsge,NNN)

P = ChebyshevSchemeDet(ss,chebyshev_nodes,[21,21],3,[0.0960769 26.0; -0.0960769 8.0],tol,1e-6,maxiters)
PP = ChebyshevSchemeDet(ss,chebyshev_nodes,[21,21],4,[0.0960769 26.0; -0.0960769 8.0],tol,1e-6,maxiters)
PPP = ChebyshevSchemeDet(ss,chebyshev_nodes,[71,71],6,[0.0960769 26.0; -0.0960769 8.0],tol,1e-6,maxiters)

soln_nla = solve_model(dsge,P)
soln_nlb = solve_model(dsge,soln_fo,PP)
soln_nlc = solve_model(dsge,soln_so,PPP,2)
soln_nld = solve_model(dsge,soln_to,P)
soln_nle = solve_model(dsge,soln_nld,PP)
soln_nlf = solve_model(dsge,soln_nle,PPP,2)

L = SmolyakSchemeDet(ss,chebyshev_gauss_lobatto,4,[0.0960769 26.0; -0.0960769 8.0],tol,1e-6,maxiters)
LL = SmolyakSchemeDet(ss,clenshaw_curtis_equidistant,4,[0.0960769 26.0; -0.0960769 8.0],tol,1e-6,maxiters)
LLL = SmolyakSchemeDet(ss,chebyshev_gauss_lobatto,5,[0.0960769 26.0; -0.0960769 8.0],tol,1e-6,maxiters)

soln_nlg = solve_model(dsge,L)
soln_nlh = solve_model(dsge,soln_to,LL)
soln_nli = solve_model(dsge,soln_nlh,LLL,2)
soln_nlj = solve_model(dsge,soln_nlf,LL)

M = PiecewiseLinearSchemeDet(ss,[21,21],[0.0960769 26.0; -0.0960769 8.0],tol,1e-6,maxiters)
MM = PiecewiseLinearSchemeDet(ss,[31,31],[0.0960769 26.0; -0.0960769 8.0],tol,1e-6,maxiters)

soln_nlk = solve_model(dsge,M)
soln_nll = solve_model(dsge,soln_to,MM)
soln_nlm = solve_model(dsge,soln_nlf,MM,2)
soln_nln = solve_model(dsge,soln_nlj,MM)
soln_nlo = solve_model(dsge,soln_nln,MM)

simulated_data1 = simulate(soln_nlf,0.95*ss[1:2],100000)


oddturns(w,d) = Int(floor(round(w/d;digits=6)))
eventurns(w,d) = Int(floor(round((w-d/2)/d;digits=6)))

oddwidth(n::Integer,d::Unitful.Length) = n*d
evenwidth(n::Integer,d::Unitful.Length) = (n+0.5)*d
function layerheight(n::Integer,d::Unitful.Length)
    @assert n>0
    y = d
    if n>1
        y += (n-1)*(d/2)*√3
    end
    y
end

"""
Calculate the orthocyclic coil winding for a give wire diameter, returning a `WindingLayers`.

ideal_fill(w, h, d)

    w => Horizontal width of coil for layers
    h => Vertical height to fill with layers
    d => Diameter of Wire

    ←           w             →
    ###########################
    #           ↑             #
    #                         #
    #           h             #
    #                         #
    #           ↓             #
    ###########################
                ↑
              (id)
"""
function ideal_fill(w,h,d)

    # layers=length(d/2:d*sqrt(3)/2:h-d/2)
    #TODO Improve this to the rounding isn't necessary
    layers = ideal_layers(h,d)
    odd_layer= oddturns(w,d)
    even_layer= eventurns(w,d)

    odd_turns = length(1:2:layers)*odd_layer
    even_turns = length(2:2:layers)*even_layer

    WindingLayers(layers, odd_layer, even_layer, odd_turns, even_turns, odd_turns+even_turns)
end

"""
ideal_fill(coil::CoilGeometry, awg::AWG, [fill::Real=1.0])

    coil => CoilGeometry
    awg => AWG of the wire
    fillfactor => fillfactor of the wire
"""
function ideal_fill(coil::CoilGeometry, awg::AWG, fillfactor::T=1.0) where {T<:Real}
    zero(T) < fillfactor <= one(T) || throw(DomainError(fillfactor))
    w=coil.len
    h=(coil.od-coil.id)/2
    ideal_fill(w,h,awg.d_insul/sqrt(fillfactor))
end

"""
Calculate the number of layers of wire.  Given the `h` height of the layter, and the `d` diameter of the wire.

    ideal_layers(h, d)

    ###########################
    #           ↑             #
    #                         #
    #           h             #
    #                         #
    #           ↓             #
    ###########################
                ↑
              (id)
"""
ideal_layers(h,d) = h≳d ? 1+floor(round( (h-d)/(d*√3/2) ;digits=6)) : 0
ideal_layers(coil::CoilGeometry,d::Unitful.Length) = ideal_layers((coil.od-coil.id)/2,d)

"""
Construct the minimum size coil to fit the specified winding

    enclosewinding(layers, oddlayer, evenlayer, id::Unitful.Length, d::Unitful.Length)
    enclosewinding(layers, oddlayer, evenlayer, id::Unitful.Length, gauge::AWG)
"""
function enclosewinding(layers, oddlayer, evenlayer, id::Unitful.Length, d::Unitful.Length)
    @assert layers>0
    @assert evenlayer == oddlayer || evenlayer == oddlayer-1
    # od = id + 2d + (layers-1)*d*√3
    # len = oddlayer > evenlayer ? d*oddlayer : d/2 + d*evenlayer
    w=oddlayer>evenlayer ? oddwidth(oddlayer,d) : evenwidth(evenlayer,d)
    od=2layerheight(layers,d) + id
    f = unit(id)
    CoilGeometry(id,f(od),f(w))
end
enclosewinding(layers, oddlayer, evenlayer, id::Unitful.Length, gauge::AWG) =
    enclosewinding(layers, oddlayer, evenlayer, id, gauge.d_insul)

"""
Construct the minimum size `coil` to fit the `winding` within the limits of the existing coil.

enclosewinding(coil::CoilGeometry, winding::WindingLayers, d::Unitful.Length)
enclosewinding(coil::CoilGeometry, winding::WindingLayers, gauge::AWG)
"""
function enclosewinding(coil::CoilGeometry, winding::WindingLayers, d::Unitful.Length)
#TODO Set check to make sure the calculated winding is within the limits of the existing coil
    new_coil = enclosewinding(winding.layers, winding.odd_layers, winding.even_layers, coil.od, d)
    @assert new_coil.id >= coil.id && new_coil.od <= coil.od && new_coil.len <= coil.len
    @assert isvalidcoil(new_coil)
    new_coil
end
enclosewinding(coil::CoilGeometry, winding::WindingLayers, gauge::AWG) = enclosewinding(coil,winding,gauge.d_insul)


# This is the HMM from the particle filter chapter (ch5).

using Stochy, DataStructures

@pp function hmm(states, observations)
    if observations == list()
        reverse(states)
    else
        local prevstate = first(states),
              state = flip(prevstate ? 0.9 : 0.1)
        factor(state == first(observations) ? 0 : -1)
        hmm(state..states, tail(observations))
    end
end

dist = @pp enum() do
    hmm(list(false), list(true,true,true,true))
end

println("Exact:\n", dist)

dist = @pp smc(100) do
    hmm(list(false), list(true,true,true,true))
end

println("SMC:\n", dist)


"""
Creates the (normalized) half range Chebyshev polynomials of the first kind.

These orthonormal polynomials `(p_k(t))` on [-1,1] are orthogonal with respect to the weight
```
w(t) = [(1-t)(t-m(T))]^{-1/2}
```
with
```
m(T) = 1-2cosec^2(pi/(2T))
```

They appear in the context of Fourier extensions. There, an even function `f(x)` on
`[-1,1]` is approximated with a cosine series in `x` on `[-T,T]`. By the cosine mapping
```
    y = m(x) = cos(pi/T x)
```
this problem is transformed. The Fourier Extension problem on `f` is then equivalent with
the approximation of `f(m^{-1}(y))` with orthogonal polynomials `(q_k(y))` on `[cos(pi/T),1]`

By another mapping the interval `[cos(pi/T),1]` is mapped to `[-1,1]` and the polynomials `(q_k(y))` are mapped
to the polynomials `(p_k(t))`

See Huybrechs 2010, On the Fourier Extension of nonperiodic functions
    Adcock, Huybrechs, Vaquero 2014, On the numerical stability of Fourier Extension

The `indicator_function_nodes` refer to the boundary points of the domain of the function `f(x)` is approximated on.
It is assumed that `indicator_function_nodes` has the form
```
    [-1,b_1,a_2,b_2, ..., a_n, 1 ] with -1<b_1<a_2<b_2<...<a_n<1.
```
By default it is the vector [-1.,1.].

See also `HalfRangeChebyshevIkind`
"""
HalfRangeChebyshevIkind(n::Int, T::ELT, indicator_function_nodes::Vector{ELT}=default_indicator_nodes(ELT); options...) where ELT =
    HalfRangeChebyshev(n, ELT(-1//2), T, indicator_function_nodes; options...)

"""
Creates the (normalized) half range Chebyshev polynomials of the second kind.

These orthonormal polynomials `(p_k(t))` on [-1,1] are orthogonal with respect to the weight
```
w(t) = [(1-t)(t-m(T))]^{1/2}
```
with
```
m(T) = 1-2cosec^2(pi/(2T))
```

They appear in the context of Fourier extensions. There, an odd function `f(x)` on
`[-1,1]` is approximated with a sine series in `x` on `[-T,T]`.

See also `HalfRangeChebyshevIkind`
"""
HalfRangeChebyshevIIkind(n::Int, T::ELT, indicator_function_nodes::Vector{ELT}=default_indicator_nodes(ELT); options...) where ELT =
    HalfRangeChebyshev(n, ELT(1//2), T, indicator_function_nodes; options...)

m_forward = (x,T)->2*(cos(pi/T*x)-cos(pi/T))/(1-cos(pi/T))-1
m_inv = (t,T)->T/pi*acos(cos(pi/T)+(1-cos(pi/T))/2*(t+1))
weight_of_indicator(T,indicator) = t->.5*(indicator(-m_inv(t,T))+indicator(m_inv(t,T)))
default_indicator_nodes(ELT) = [-ELT(1),ELT(1)]

function HalfRangeChebyshev(n::Int, α, T::ELT, indicator_function_nodes::Vector{ELT}; options...) where ELT
    my_quadrature_rule = n->_halfrangechebyshevweights(n, α, T, indicator_function_nodes)
    OrthonormalOPSfromQuadrature(n, my_quadrature_rule, Interval(-one(ELT), one(ELT)); options...)
end

function _halfrangechebyshevweights(n, α::ELT, T::ELT, indicator_function_nodes::Vector{ELT}) where {ELT}
    @assert indicator_function_nodes[1] == -1 && indicator_function_nodes[end] == 1
    @assert reduce(&, indicator_function_nodes[1:end-1] .< indicator_function_nodes[2:end], init=true)
    @assert iseven(length(indicator_function_nodes))

    if α < 0
        C = 2T/pi
    else
        C = T*(1-cos(pi/T))^2/2/pi
    end
    if length(indicator_function_nodes)==2
        nodes, weights = gaussjacobi(n, α, ELT(0))
        Λ = weight_of_indicator(T,x->1)
        modified_weights = weights.*((nodes.-m_forward(T,T)).^α).*Λ.(nodes)*C
        nodes, modified_weights
    else
        indicator = indicator_function(indicator_function_nodes)
        mapped_nodes = sort(unique(map(x->m_forward(x,T), push!(indicator_function_nodes,0))))
        pop!(indicator_function_nodes)
        no_intervals = length(mapped_nodes)-1
        n_interval = n
        N = n_interval*no_intervals
        nodes = zeros(ELT,N)
        weights = zeros(ELT,N)

        for i in 1:no_intervals
            nodes_interval, weights_interval = gaussjacobi(n_interval, ELT(0), ELT(0))
            a = mapped_nodes[i]
            b = mapped_nodes[i+1]
            nodes_interval[:] .= a + (b-a)/2*(nodes_interval+1)
            weights_interval[:] .= weights_interval*(b-a)/2
            Δ = weight_of_indicator(T,indicator)
            weights_interval[:] .= C*weights_interval.*(1-nodes_interval).^α.*(nodes_interval-m_forward(T,T)).^α.*Δ.(nodes_interval)

            nodes[1+(i-1)*n_interval:i*n_interval] .= nodes_interval[:]
            weights[1+(i-1)*n_interval:i*n_interval] .= weights_interval[:]
        end
        nodes, weights
    end
end

indicator_function(nodes) = x-> reduce(|, false, nodes[1:2:end] .<= x .<= nodes[2:2:end])

function WaveOPS(n::Int,omega::ELT; options...) where {ELT}
    my_quadrature_rule = n->_wavePolynomialweight(n, omega)
    BasisFunctions.OrthonormalOPSfromQuadrature(n, my_quadrature_rule, Interval(-one(ELT), one(ELT)), (x->(abs(x)<=1) ? exp(1im*omega*x) : ELT(0)); options...)
end

function _wavePolynomialweight(n, omega::ELT) where ELT
    nodes, weights = gaussjacobi(n,ELT(0),ELT(0))
    weights = weights.*exp.(1im.*omega.*nodes)
    nodes, weights
end


using TensorOperations
using Printf

"""
replace this with table lookup
"""
function get_nchk(n,k)
    return calc_nchk(n,k)
end

function get_fiedler(A)

    dim = size(A)[1]
    dim > 1 || return [1.0]
    size(A)[1] == size(A)[2] || error(" a not square")
    D = zeros(size(A)[1])
    for i in 1:dim
        D[i] = sum(A[i,:])
    end
    L = Diagonal(D) - A
    F = eigen(L)

    @printf(" eigenvalues of laplacian\n")
    for (ii,i) in enumerate(F.values)
        @printf(" %4i = %12.8f\n", ii, i)
    end
    return F.vectors[:,2]
end

function fiedler_sort(C,K)
    fvec = get_fiedler(abs.(C'*K*C))
    perm, = bubble_sort(fvec)
    return C[:,perm]
end


# PROJECT_SDC    Project a point onto the semidefinite cone.
#
#   project_sdc(V), where V is a symmetric n x n matrix, is the
#   projection of V onto the semidefinite cone.
function project_sdc(V)
    W, D = eig(V)
    X = W * max(0, D) * W'
    return X
end


using HypergeometricFunctions
using SpecialFunctions

"""Kravchuk.jl contains the functions to evolve a set of N + 1 amplitude points
over the discrete and finite Kravchuk oscillator realization"""

# Kravchuk symmetric polynomial definition. This particular functions is using
# the resource given in HypergeometricFunctions.jl, the method 2, who avoids
# floating-points problems, but has a flaw for the definitions in the corners
# when n = 2j and m = j (energy and position respectively).
kravd(n, m, j) = HypergeometricFunctions._₂F₁general2(-n, -m, -2j, 2)

# Normalizing terms and binomial re-definition
A(n, j) = (-1)^n / 2^j
binom(a, j) = binomial(2j, a)

# Primitive Kravchuk symmetric function. Needs a fix for the flaw described above
kravp(n, m, j) = A(n, j) * sqrt(binom(n, j) * binom(m + j, j)) * kravd(n, m + j, j)

# Conditional ad-hoc solution for the problem in the _2F1 function
function krav(n, m, j)
    if n == 2j && m == j
        return kravp(0, -j, j)
    else
        return kravp(n, m, j)
    end
end

# Exponential counting the rotation angles of the fractional evolution
kerk(n, α) = exp(-1im * pi * α * n / 2)

# Evolution kernel, this is intended to be used as standalone
frk(n, m, α, j) = sum([krav(l, n, j) * kerk(l, α) * krav(l, m, j) for l in 0:2j])

# Evolution of initial amplitudes in the vector v. This functions is intended
# to be used when the evolution is required, just given v of size 2j + 1, and
# the numbers of steps p
function evol(v, p, j)
    θ = range(0, stop = 2, length = p)
    em = zeros(Float64, (2j + 1, p))
    for r in 1:p
        for s in -j:j
        em[s + 1 + j, r] = abs2(sum([frk(s, m - j - 1, θ[r], j) * v[m] for m in 1:(2j +1)]))
        end
    end
    return em
end


"""
$(FUNCTIONNAME)(μ,Lmax)
Computes the normalized Π matrix elements with generalized spherical functions (normalized by sqrt((l-m)!/(l+m)!)) ). See eq 15 in Sanghavi 2014
- `μ` cos(θ) of angle θ
- `Lmax` max `l` Polynomial degree to be computed (m adjusted accordingly)
The function returns matrices containing ``P_l^m(\\mu)``,  ``R_l^m(\\mu)``, ``-T_l^m(\\mu)``, all normalized by ``\\sqrt{\\frac{(l-m)!}{(l+m)!}}``
"""
function compute_associated_legendre_PRT(μ,Lmax)
     # Note: P_l^m(μ) double-checked against Matlab, really hard to find other benchmarks for R and T!
     # Can probably be sped up but it can be pre-computed anyhow as angles μ and Lmax will be fixed per run
    FT = eltype(μ)

    # Create Arrays for the associate legendre coefficients (Siewert, eq. 10)
    P = (zeros(FT,length(μ),Lmax,Lmax));
    R = (zeros(FT,length(μ),Lmax,Lmax));
    T = (zeros(FT,length(μ),Lmax,Lmax));

    # Following Suniti Sanghavi code, looks somewhat different than Siewert as normalization is built in.
    for  m=0:Lmax-1
        for l=m:Lmax-1
            for iμ in eachindex(μ)
                smu = sqrt(1.0 - μ[iμ]^2)
                cmu = μ[iμ]
                #indices for arrays
                im = m+1;
                il = l+1;

                if m==0
                    if l==0 # then !eq.28a
                        P[iμ,il,im] = 1
                        R[iμ,il,im] = 0
                        T[iμ,il,im] = 0
                    elseif l==1 # then !eq.28b
                        P[iμ,il,im] = cmu
                        R[iμ,il,im] = 0
                        T[iμ,il,im] = 0
                    elseif l==2 # then !eq.28c, 29, 30
                        cA = 0.5*(3.0*cmu*cmu-1.0)
                        cB = 0.5*sqrt(1.5)*smu*smu

                        P[iμ,il,im] = cA
                        R[iμ,il,im] = cB
                        T[iμ,il,im] = 0.0
                    else #!eq.30, 31a, 31b
                        Y_lm1 = l-1
                        X_lm1 = l

                        P[iμ,il,im] = (P[iμ,il-1,im] * (2l-1) * cmu -  P[iμ,il-2,im] * Y_lm1 ) / X_lm1

                        Y_lm1 = sqrt( (l+1) * (l-3) )
                        X_lm1 = sqrt( l*l - 4 )

                        R[iμ,il,im] = (R[iμ,il-1,im] * (2*l-1) * cmu -  R[iμ,il-2,im] * Y_lm1) / X_lm1
                        T[iμ,il,im] = 0.0
                    end

                elseif m==1 # then
                    if l==1 # then !eq.32a
                        m1 = sqrt(0.5)
                        P[iμ,il,im] = m1*smu
                        R[iμ,il,im] = 0.0
                        T[iμ,il,im] = 0.0
                    elseif l==2 # then !eq.32b, 33a, 33b
                        m1 = sqrt(1/6)
                        cA = 3cmu*smu
                        cB = sqrt(1.5)*smu
                        P[iμ,il,im] = m1*cA
                        R[iμ,il,im] = -m1*cmu*cB
                        T[iμ,il,im] = m1*cB
                    else #!eq.34, 35a, 35b, 35c
                        m1 = sqrt((l-1)/(l+1))
                        m2 = m1 * sqrt( (l-2) / l )

                        Y_lm1 = (l-1+m)
                        X_lm1 = (l-m)

                        P[iμ,il,im] = (m1 * P[iμ,il-1,im] * (2l-1) * cmu
                                    - m2 * P[iμ,il-2,im] * Y_lm1 ) / X_lm1

                        Z_lm1 = (2m * (2l-1) ) / (l * (l-1))
                        Y_lm1 = ( (l+m-1) / (l-1)) * sqrt( (l-3) * (l+1) )
                        X_lm1 = ( (l-m)   /  l   ) * sqrt( (l*l-4) )

                        R[iμ,il,im] = (m1*R[iμ,il-1,im] * (2l-1) * cmu
                                    -m2 * R[iμ,il-2,im] * Y_lm1 +m1 * T[iμ,il-1,im] * Z_lm1 ) /X_lm1

                        T[iμ,il,im] = (m1*T[iμ,il-1,im] * (2l-1) * cmu
                                    -m2 * T[iμ,il-2,im] * Y_lm1 +m1 * R[iμ,il-1,im] * Z_lm1 ) /X_lm1
                    end
                else
                    if l==m # then !eq.36, 37
                        fact1=1.0
                        fact2=1.0

                        sfull = smu
                        shalf = sfull/2
                        for i=1:m
                            fact1 = fact1*((2i-1)*sfull) / sqrt((i*(i+m)))
                            if i>2  #then
                                fact2 = fact2 * shalf * sqrt((m+i)/(i-2))
                            else
                                fact2 = fact2 * shalf
                            end
                        end
                        if smu>1e-8  # then
                            Km = (fact2)
                            Aii= Km * (1.0+cmu*cmu) / (smu*smu)
                            Aij= Km * (2cmu) / (smu*smu)
                        else
                            if m==2 # then
                                Aii=0.5
                                Aij=0.5
                            else
                                Aii=0.0
                                Aij=0.0
                            end
                        end
                        P[iμ,il,im] = fact1
                        R[iμ,il,im] =  Aii
                        T[iμ,il,im] = -Aij

                    elseif l==(m+1) # then !eq.38, 35a, 35b
                        # typo 1 and l??
                        m1 = sqrt((1)/(l+m))

                        Y_lm1 = (l-1+m)
                        X_lm1 = (l-m)

                        P[iμ,il,im] = ( m1 * P[iμ,il-1,im] * (2l-1) * cmu ) / X_lm1

                        Z_lm1 = (2m * (2l-1)) / (l*(l-1))
                        Y_lm1 = ((l+m-1) / (l-1)) * sqrt( (l-3) * (l+1) )
                        X_lm1 = ( (l-m) /l ) * sqrt(l*l-4)

                        R[iμ,il,im] = ( m1 * R[iμ,il-1,im] * (2l-1) * cmu
                                    + m1 * T[iμ,il-1,im] * Z_lm1 ) / X_lm1
                        T[iμ,il,im] = ( m1 * T[iμ,il-1,im] * (2l-1) * cmu
                                    + m1 * R[iμ,il-1,im] * Z_lm1 ) / X_lm1

                    else #!eq.38, 35a, 35b
                        m1 = sqrt( (l-m) / (l+m) )
                        m2 = m1 * sqrt( (l-m-1) / (l+m-1) )

                        Y_lm1 = (l-1+m)
                        X_lm1 = (l-m)

                        P[iμ,il,im] = ( m1 * P[iμ,il-1,im] * (2l-1) * cmu
                                    - m2 * P[iμ,il-2,im] * Y_lm1 ) / X_lm1

                        Z_lm1 = (2m*(2l-1)) / (l*(l-1))
                        Y_lm1 = ( (l+m-1) / (l-1) ) * sqrt( (l-3) * (l+1) )
                        X_lm1 = ( (l-m) / l ) * sqrt( l*l-4 )

                        R[iμ,il,im] = ( m1 * R[iμ,il-1,im] * (2l-1) * cmu
                                    - m2 * R[iμ,il-2,im] * Y_lm1
                                    + m1 * T[iμ,il-1,im] * Z_lm1) / X_lm1

                        T[iμ,il,im] = ( m1 * T[iμ,il-1,im] * (2l-1) * cmu
                                    - m2 * T[iμ,il-2,im] * Y_lm1
                                    + m1 * R[iμ,il-1,im] * Z_lm1) / X_lm1

                    end
                end
            end
       end
    end
    # Note: from Suniti's code, -T is actually computed, so I return the "true" T here:
    return P,R,-T
end

"""
$(FUNCTIONNAME)(μ, nmax, π, τ)
Computes the associated Legendre functions  amplitude functions `π` and `τ` in Mie theory (stored internally). See eq 6 in Sanghavi 2014
- `μ` cosine of the scattering angle
- `nmax` max number of legendre terms (depends on size parameter, see [`get_n_max`](@ref))
Functions returns `π` and `τ` (of size `[nmax,length(μ)]`)
"""
function compute_mie_π_τ(μ, nmax)
    FT = eltype(μ)
    # Allocate arrays:
    π_ = zeros(FT,length(μ),nmax)
    τ_ = zeros(FT,length(μ),nmax)

    # BH book, pages 94-96:
    π_[:,1] .= 1.0;
    π_[:,2] .= 3μ;
    τ_[:,1] .= μ;
    # This is equivalent to 3*cos(2*acos(μ))
    τ_[:,2] .= 6μ.^2 .-3;
    for n=2:nmax-1
        for i in eachindex(μ)
            π_[i,n+1] = ((2n + 1) * μ[i] * π_[i,n] - (n+1) * π_[i,n-1]) / n
            τ_[i,n+1] = (n+1) * μ[i] * π_[i,n+1] - (n+2)*π_[i,n]
            # @show n+1,μ[i], π_[n+1,i], τ_[n+1,i], π_[n,i]
        end
    end
    return π_, τ_
end

"""
$(FUNCTIONNAME)(x,nmax)
Returns the associated legendre functions Pᵢ, P²ᵢ, R²ᵢ, and T²ᵢ as a function of x and i=1:nmax
- `x` array of locations to be evaluated [-1,1]
- `nmax` max number of legendre terms (depends on size parameter, see [`get_n_max`](@ref))
The function returns `Pᵢ`, `P²ᵢ`, `R²ᵢ`, and `T²ᵢ`, for a size distribution, this can be pre-computed with nmax derived from the maximum size parameter.
"""
function compute_legendre_poly(x,nmax)
    FT = eltype(x)
    @assert nmax > 1
    #@assert size(P) == (nmax,length(x))
    P⁰ = zeros(length(x),nmax);
    P² = zeros(length(x),nmax);
    R² = zeros(length(x),nmax);
    T² = zeros(length(x),nmax);
    # 0th Legendre polynomial, a constant
    P⁰[:,1] .= 1;
    P²[:,1] .= 0;
    R²[:,1] .= 0;
    T²[:,1] .= 0;
    # 1st Legendre polynomial, x
    P⁰[:,2]  = x;
    P²[:,2] .= 0;
    R²[:,2] .= 0;
    T²[:,2] .= 0;

    # 2nd Legendre polynomial, x
    #P¹[2,:] = x;
    P²[:,3] .= 3   * (1 .- x.^2);
    R²[:,3] .= sqrt(1.5) * (1 .+ x.^2);
    T²[:,3] .= sqrt(6) * x;

    for n=2:nmax-1
        for i in eachindex(x)
            l = n-1;
            P⁰[i,n+1] = ((2l + 1) * x[i] * P⁰[i,n] - l * P⁰[i,n-1])/(l+1)
            if n>2
                ia = (2l+1) * x[i];
	            ib = sqrt( (l+2) * (l-2) ) * (l+2) / (l);
	            ic = 4.0 * (2l+1) / ( (l+1)*l );
	            id = sqrt( (l+3) * (l-1) ) * (l-1) / (l+1);
                P²[i,n+1] = ( ia * P²[i,n] - (l+2) * P²[i,n-1] ) / (l-1)
                R²[i,n+1] = ( ia * R²[i,n] - ib * R²[i,n-1] - ic * T²[i,n] ) / id;
	            T²[i,n+1] = ( ia * T²[i,n] - ib * T²[i,n-1] - ic * R²[i,n] ) / id;
            end
        end
    end
    return P⁰, P², R², T²
end

# Following Milthorpe, 2014, includes the Condon–Shortley phase
function compute_legendre_P(μ , Lmax)
    FT = eltype(μ)
    da = zeros(FT,Lmax+1,Lmax+1)
    sintheta = sqrt(1.0 .- μ*μ)
    temp = sqrt(0.5/π)
    da[1,1] = sqrt(0.5/π);
    if Lmax > 0
        SQRT3 = sqrt(3.0)
        da[1,2] = μ * SQRT3 * temp
        SQRT3DIV2 = -sqrt(3.0 / 2.0)
        temp = SQRT3DIV2 * sintheta * temp
        da[2,2] = temp

        for l=2:Lmax
            il = l+1
            for m=0:l-2
                im = m+1
                A =  sqrt( (4l^2-1) / (l^2 - m^2) )
                B = -sqrt( ((l-1)^2-m^2) / (4*(l-1)^2 - 1) )
                da[il,im] = A * (μ * da[il-1,im] + B * da[il-2,im] )
            end
            da[il-1,il] = μ * sqrt(2*(l-1)+3) * temp;
            temp = -sqrt(1 + 0.5/l ) * sintheta * temp
            da[il,il] = temp
        end
    end
    ll = collect(Float64,0:Lmax)
    ll = sqrt.( (2*ll .+ 1)/2π)

    return da./ll'
end


# SIAR model
# https://arxiv.org/pdf/2012.00443.pdf
using Logging

using StructuralIdentifiability

logger = Logging.SimpleLogger(stdout, Logging.Info)
global_logger(logger)

ode = @ODEmodel(
    S'(t) = -S(t) * (betaIa * Ia(t) + betaIs * Is(t) + betaH * H(t) + betaT * T(t)) * Ninv,
    E'(t) = S(t) * (betaIa * Ia(t) + betaIs * Is(t) + betaH * H(t) + betaT * T(t)) * Ninv - (alphaEIa + alphaEIs) * E(t),
    Ia'(t) = alphaEIa * E(t) - (alphaIaIs + alphaIaRu) * Ia(t) - xi * Ia(t),
    Is'(t) = alphaEIs * E(t) + alphaIaIs * Ia(t) - (alphaIsH + alphaIsT + alphaIsRu + alphaIsD) * Is(t),
    H'(t) = alphaIsH * Is(t) + xi * Ia(t) - (alphaHT + alphaHRd) * H(t),
    T'(t) = alphaIsT * Is(t) + alphaHT * H(t) - (alphaTRd + alphaTD) * T(t),
    Rd'(t) = alphaHRd * H(t) + alphaTRd * T(t),
    D'(t) = alphaIsD * Is(t) + alphaTD * T(t),
    y1(t) = H(t),
    y2(t) = T(t),
    y3(t) = Rd(t),
    y4(t) = D(t),
    y5(t) = Ninv
)

@time println(assess_identifiability(ode))


using Test, FinancialMonteCarlo,Statistics;
@show "Black Scholes Model"
S0=100.0;
K=100.0;
r=[0.00,0.02];
T=1.0;
d=0.01;
D=90.0;

Nsim=100000;
Nstep=30;
sigma=0.2;
mc=MonteCarloConfiguration(Nsim,Nstep);
toll=0.8

rfCurve=ZeroRate(r,T);

FwdData=Forward(T)
EUData=EuropeanOption(T,K)

S=S0.*exp.(simulate(BrownianMotion(sigma,-sigma*sigma*0.5,Underlying(0.0,0.0)),rfCurve,mc,T));

Payoff=exp(-integral(rfCurve,T))*S[:,end];
Price=mean(Payoff)


import StaticArrays: SUnitRange

# Computing Integrals Involving the Matrix Exponential (elements of Van Loan '78)
function _integrate_expAt_B(A::StaticMatrix{N,N}, B::StaticMatrix{N,M}, dt::T) where {N,M,T}
    S = promote_type(eltype(A), eltype(B), T)
    X = exp([[A B]; zeros(SMatrix{M,N+M,S})]*dt)
    (eᴬᵗ=X[SUnitRange(1,N),SUnitRange(1,N)], ∫eᴬᵗB=X[SUnitRange(1,N),SUnitRange(N+1,N+M)])
end
function integrate_expAt_B(A::StaticMatrix{N,N}, B::StaticVector{N}, dt) where {N}
    ans = _integrate_expAt_B(A, SMatrix{N,1}(B), dt)
    (eᴬᵗ=ans.eᴬᵗ, ∫eᴬᵗB=SVector(ans.∫eᴬᵗB))
end
function integrate_expAt_B(A::StaticMatrix{N,N}, B::StaticMatrix{N,M}, dt) where {N,M}
    eᴬᵗ, ∫eᴬᵗB₁ = integrate_expAt_B(A, B[:,1], dt)
    (eᴬᵗ=eᴬᵗ, ∫eᴬᵗB=hcat(∫eᴬᵗB₁, ntuple(i -> integrate_expAt_B(A, B[:,i+1], dt).∫eᴬᵗB, Val(M-1))...))
end

function _integrate_expAt_Bt_dtinv(A::StaticMatrix{N,N}, B::StaticMatrix{N,M}, dt::T) where {N,M,T}
    S = promote_type(eltype(A), eltype(B), T)
    X = exp([[A B zeros(typeof(B))]*dt;
             [zeros(SMatrix{M,N+M,S}) SMatrix{M,M,S}(I)];
             zeros(SMatrix{M,N+2*M,S})])
    ∫eᴬᵗB=X[SUnitRange(1,N),SUnitRange(N+1,N+M)]
    (eᴬᵗ=X[SUnitRange(1,N),SUnitRange(1,N)], ∫eᴬᵗB=∫eᴬᵗB, ∫eᴬᵗBtdt⁻¹=∫eᴬᵗB - X[SUnitRange(1,N),SUnitRange(N+M+1,N+2*M)])
end
function integrate_expAt_Bt_dtinv(A::StaticMatrix{N,N}, B::StaticVector{N}, dt) where {N}
    ans = _integrate_expAt_Bt_dtinv(A, SMatrix{N,1}(B), dt)
    (eᴬᵗ=ans.eᴬᵗ, ∫eᴬᵗB=SVector(ans.∫eᴬᵗB), ∫eᴬᵗBtdt⁻¹=SVector(ans.∫eᴬᵗBtdt⁻¹))
end
function integrate_expAt_Bt_dtinv(A::StaticMatrix{N,N}, B::StaticMatrix{N,M}, dt) where {N,M}
    eᴬᵗ, ∫eᴬᵗB₁, ∫eᴬᵗB₁tdt⁻¹ = integrate_expAt_Bt_dtinv(A, B[:,1], dt)
    X₁ = [∫eᴬᵗB₁; ∫eᴬᵗB₁tdt⁻¹]
    X = hcat(X₁, ntuple(i -> (ans = integrate_expAt_Bt_dtinv(A, B[:,i+1], dt); [ans.∫eᴬᵗB; ans.∫eᴬᵗBtdt⁻¹]), Val(M-1))...)
    (eᴬᵗ=eᴬᵗ, ∫eᴬᵗB=X[SUnitRange(1,N),:], ∫eᴬᵗBtdt⁻¹=X[SUnitRange(N+1,2*N),:])
end

function integrate_expAt_B_expATt(A::StaticMatrix{N,N}, B::StaticMatrix{N,N}, dt) where {N}
    X = exp([[-A B]; [zeros(typeof(A)) A']]*dt)
    X[SUnitRange(N+1,2*N),SUnitRange(N+1,2*N)]'*X[SUnitRange(1,N),SUnitRange(N+1,2*N)]
end

# Univariate Optimization (@inline here and in optimal_time necessary to eliminate allocations)
@inline function golden_section(f, a, b; ϵ=1e-3)
    T = typeof((a + b)/2)
    gr = T((1 + sqrt(5))/2)
    fa, fb = f(a), f(b)
    while fa < fb; a /= 2; fa = f(a); end
    m1 = b - (b - a)/gr
    m2 = a + (b - a)/gr
    while abs(m1 - m2) > ϵ
        if f(m1) < f(m2)
            b = m2
        else
            a = m1
        end
        m1 = b - (b - a)/gr
        m2 = a + (b - a)/gr
    end
    (a + b)/2
end

@inline function bisection(f, a, b; ϵ=1e-3)
    # Bisection
    # Broken for Float32 triple integrator with R = [1f-3],
    # q0 = SVector(-6.094263f0, 0.014560595f0, -0.6846263f0)
    # qf = SVector(-6.0940447f0, 0.0f0, 0.0f0)
    f(b) < 0 && return b
    fa = f(a)
    while fa > 0
        a /= 2
        fa = f(a)
        (a == 0 || isnan(fa) || isinf(fa)) && return nothing    # workaround for above
    end
    m = (a + b)/2
    fm = f(m)
    while abs(fm) > ϵ && abs(a - b) > ϵ
        fm > 0 ? b = m : a = m
        m = (a + b)/2
        fm = f(m)
    end
    m
end

@inline function false_position(f, a, b; ϵ=1e-3)
    f(b) < 0 && return b
    fa = f(a)
    while fa > 0
        a /= 2
        fa = f(a)
        (a == 0 || isnan(fa) || isinf(fa)) && return nothing
    end
    fb = f(b)
    x = (a*fb - b*fa)/(fb - fa)
    fx = f(x)
    while abs(fx) > ϵ && abs(a - b) > ϵ
        fx > 0 ? b = x : a = x
        x = (a*fb - b*fa)/(fb - fa)
        fx = f(x)
    end
    x
end

@inline function newton(f, f′, a, b; ϵ=1e-6)
    # Bisection / Newton's method combo
    f(b) < 0 && return b
    fa = f(a)
    while fa > 0
        a /= 2
        fa = f(a)
        (a == 0 || isnan(fa) || isinf(fa)) && return nothing
    end
    x = (a + b)/2
    fx = f(x)
    while abs(fx) > ϵ && abs(a - b) > ϵ
        x = x - fx / f′(x)
        (x < a || x > b) && (x = (a + b)/2)
        fx = f(x)
        fx > 0 ? b = x : a = x
    end
    x
end


        # intersect
        # -------------------------------------------
        # basic intersection
        l₁ = LineSegment2D(Point2D{T}(0,  1), Point2D{T}(2, -1))
        l₂ = LineSegment2D(Point2D{T}(0, -1), Point2D{T}(2,  1))
        hit, p₁ = intersect(l₁, l₂)
        @test hit
        @test p₁ ≈ Point2D{T}(1, 0)
        @test typeof(p₁) == Point2D{T}

        # vertex intersection
        l₂ = LineSegment2D(Point2D{T}(0, -1), Point2D{T}(2, -1))
        hit, p₁ = l₁ ∩ l₂
        @test hit
        @test p₁ ≈ Point2D{T}(2, -1)

        # vertical
        l₁ = LineSegment2D(Point2D{T}(0,  1), Point2D{T}(2,   1))
        l₂ = LineSegment2D(Point2D{T}(1, 10), Point2D{T}(1, -10))
        hit, p₁ = intersect(l₁, l₂)
        @test hit
        @test p₁ ≈ Point2D{T}(1, 1)

        # nearly vertical
        l₁ = LineSegment2D(Point2D{T}(-1, -100000), Point2D{T}(1,  100000))
        l₂ = LineSegment2D(Point2D{T}(-1,   10000), Point2D{T}(1,  -10000))
        hit, p₁ = l₁ ∩ l₂
        @test hit
        @test p₁ ≈ Point2D{T}(0, 0)

        # parallel
        l₁ = LineSegment2D(Point2D{T}(0, 1), Point2D{T}(1, 1))
        l₂ = LineSegment2D(Point2D{T}(0, 0), Point2D{T}(1, 0))
        hit, p₁ = intersect(l₁, l₂)
        @test !hit

        # collinear
        l₁ = LineSegment2D(Point2D{T}(0, 0), Point2D{T}(2, 0))
        l₂ = LineSegment2D(Point2D{T}(0, 0), Point2D{T}(1, 0))
        hit, p₁ = intersect(l₁, l₂)
        @test !hit

        # line intersects, not segment (invalid s)
        l₁ = LineSegment2D(Point2D{T}(0, 0), Point2D{T}(2, 0    ))
        l₂ = LineSegment2D(Point2D{T}(1, 2), Point2D{T}(1, 1//10))
        hit, p₁ = l₁ ∩ l₂
        @test !hit

        # line intersects, not segment (invalid r)
        l₂ = LineSegment2D(Point2D{F}(0, 0), Point2D{F}(2, 0    ))
        l₁ = LineSegment2D(Point2D{F}(1, 2), Point2D{F}(1, 1//10))
        hit, p₁ = intersect(l₁, l₂)
        @test !hit

        # intersect
        l₁ = LineSegment3D(Point3D{T}(0,  1, 0), Point3D{T}(2, -1, 0))
        l₂ = LineSegment3D(Point3D{T}(0, -1, 0), Point3D{T}(2,  1, 0))
        hit, p₁ = intersect(l₁, l₂)
        @test hit
        @test p₁ ≈ Point3D{T}(1, 0, 0)
        @test typeof(p₁) == Point3D{T}

        # intersect
        𝘅₁ = Point2D{T}(0, 0)
        𝘅₂ = Point2D{T}(2, 0)
        𝘅₃ = Point2D{T}(1, 1)
        𝘅₄ = Point2D{T}(1, 0)
        𝘅₅ = Point2D{T}(1, 2)

        # 1 intersection, straight
        q = QuadraticSegment2D(𝘅₁, 𝘅₂, Point2D{T}(1//2, 0))
        l = LineSegment2D(Point2D{T}(1,-1), Point2D{T}(1,1))
        npoints, (point1, point2) = intersect(l, q)
        @test npoints == 1
        @test point1 ≈ Point2D{T}(1, 0)

        # 1 intersection
        q = QuadraticSegment2D(𝘅₁, 𝘅₂, 𝘅₃)
        l = LineSegment2D(𝘅₄, 𝘅₅)
        npoints, (point1, point2) = intersect(l, q)
        @test npoints == 1
        @test point1 ≈ Point2D{T}(1, 1)

        # 2 intersections
        𝘅₄ = Point2D{T}(0, 3//4)
        𝘅₅ = Point2D{T}(2, 3//4)
        l = LineSegment2D(𝘅₄, 𝘅₅)
        npoints, (point1, point2) = l ∩ q
        @test npoints == 2
        @test point1 ≈ Point2D{T}(1//2, 3//4)
        @test point2 ≈ Point2D{T}(3//2, 3//4)

        # 0 intersections
        𝘅₄ = Point2D{T}(0, 3)
        𝘅₅ = Point2D{T}(2, 3)
        l = LineSegment2D(𝘅₄, 𝘅₅)
        npoints, (point1, point2) = intersect(l, q)
        @test npoints == 0

        # intersect
        hit, point = LineSegment(Point2D{F}(0, 1), Point2D{F}(1, 0)) ∩ plane
        @test hit
        @test point ≈ Point2D{F}(1//2, 1//2)

        # Line is in the plane
        hit, point = LineSegment(Point2D{F}(0, 0), Point2D{F}(1, 1)) ∩ plane
        @test !hit

        # Segment stops before plane
        hit, point = LineSegment(Point2D{F}(0, 2), Point2D{F}(1, 3//2)) ∩ plane
        @test !hit

        # Plane is before segment
        hit, point = LineSegment(Point2D{F}(1, 0), Point2D{F}(2, -1)) ∩ plane
        @test !hit

        # intersect
        hit, point = LineSegment(Point3D{F}(1, 2, 0), Point3D{F}(1, 2, 5)) ∩ plane
        @test hit
        @test point ≈ Point3D{F}(1,2,2)

        # Line is in the plane
        hit, point = LineSegment(Point3D{F}(0, 0, 2), Point3D{F}(1, 0, 2)) ∩ plane
        @test !hit

        # Segment stops before plane
        hit, point = LineSegment(Point3D{F}(1, 2, 0), Point3D{F}(1, 2, 1)) ∩ plane
        @test !hit

        # Plane is before segment
        hit, point = LineSegment(Point3D{F}(1, 2, 1), Point3D{F}(1, 2, 0)) ∩ plane
        @test !hit

        #isleft
        l = LineSegment(Point3D{F}(0, 0, 2), Point3D{F}(1, 0, 2))
        @test isleft(Point3D{F}(1,1,2), l, plane)
        @test isleft(Point3D{F}(1,0,2), l, plane)
        @test !isleft(Point3D{F}(1,-1,2), l, plane)

        # intersect
        # Horizontal
        hit, points = LineSegment2D(Point2D{T}(-1, 1), Point2D{T}(4, 1)) ∩ aab
        @test hit
        @test points[1] ≈ Point2D{T}(1, 1)
        @test points[2] ≈ Point2D{T}(3, 1)

        # Horizontal miss
        hit, points = LineSegment2D(Point2D{T}(-1, 5), Point2D{T}(4, 5)) ∩ aab
        @test !hit

        # Vertical
        hit, points = LineSegment2D(Point2D{T}(2, -1), Point2D{T}(2, 5)) ∩ aab
        @test hit
        @test points[1] ≈ Point2D{T}(2, 0)
        @test points[2] ≈ Point2D{T}(2, 2)

        # Vertical miss
        hit, points = LineSegment2D(Point2D{T}(5, -1), Point2D{T}(5, 5)) ∩ aab
        @test !hit

        # Angled
        hit, points = LineSegment2D(Point2D{T}(0, 0), Point2D{T}(5, 2)) ∩ aab
        @test hit
        @test points[1] ≈ Point2D{T}(1, 0.4)
        @test points[2] ≈ Point2D{T}(3, 1.2)

        # Angled miss
        hit, points = LineSegment2D(Point2D{T}(0, 5), Point2D{T}(5, 7)) ∩ aab
        @test !hit

        # intersect
        # Horizontal
        hit, points = LineSegment3D(Point3D{T}(-1, 1, 0), Point3D{T}(4, 1, 0)) ∩ aab
        @test hit
        @test points[1] ≈ Point3D{T}(1, 1, 0)
        @test points[2] ≈ Point3D{T}(3, 1, 0)

        # Horizontal miss
        hit, points = LineSegment3D(Point3D{T}(-1, 5, 0), Point3D{T}(4, 5, 0)) ∩ aab
        @test !hit

        # Vertical
        hit, points = LineSegment3D(Point3D{T}(2, -1, 0), Point3D{T}(2, 5, 0)) ∩ aab
        @test hit
        @test points[1] ≈ Point3D{T}(2, 0, 0)
        @test points[2] ≈ Point3D{T}(2, 2, 0)

        # Vertical miss
        hit, points = LineSegment3D(Point3D{T}(5, -1, 0), Point3D{T}(5, 5, 0)) ∩ aab
        @test !hit

        # Angled
        hit, points = LineSegment3D(Point3D{T}(0, 0, -1), Point3D{T}(5, 2, 1)) ∩ aab
        @test hit
        @test points[1] ≈ Point3D{T}(1, 0.4, -0.6)
        @test points[2] ≈ Point3D{T}(3, 1.2,  0.2)

        # Angled miss
        hit, points = LineSegment3D(Point3D{T}(0, 5, -10), Point3D{T}(5, 7, 20)) ∩ aab
        @test !hit

                # intersect
        # 3 intersections
        l = LineSegment2D(p₁, Point2D{T}(1, 1))
        hit, points = intersect(l, tri)
        @test hit
        @test points[1] ≈ p₁
        @test points[2] ≈ Point2D{T}(1//2, 1//2)

        # 2 intersections
        l = LineSegment2D(Point2D{T}(0, 1//2), Point2D{T}(1//2, 0))
        hit, points = intersect(l, tri)
        @test hit
        @test points[1] ≈ Point2D{T}(1//2, 0)
        @test points[2] ≈ Point2D{T}(0, 1//2)

        # 0 intersections
        l = LineSegment2D(Point2D{T}(-1, -1), Point2D{T}(2, -1))
        hit, points = intersect(l, tri)
        @test !hit

                # intersect
        # line is not coplanar with triangle
        p₄ = Point3D{T}(-1, 1//10, 1//10)
        p₅ = Point3D{T}( 1, 1//10, 1//10)
        l = LineSegment(p₄, p₅)
        hit, point = intersect(l, tri)
        @test hit
        @test point ≈ Point3D{T}(0, 1//10,  1//10)

        # line is coplanar with triangle
        p₄ = Point3D{T}(0, -1, 1//10)
        p₅ = Point3D{T}(0,  2, 1//10)
        l = LineSegment(p₄, p₅)
        hit, point = intersect(l, tri)
        @test !hit

        # no intersection non-coplanar
        p₄ = Point3D{T}(-1, 1//10, -1//10)
        p₅ = Point3D{T}( 1, 1//10, -1//10)
        l = LineSegment(p₄, p₅)
        hit, point = intersect(l, tri)
        @test !hit

        # no intersection coplanar
        p₄ = Point3D{T}(0, -1, 1)
        p₅ = Point3D{T}(0, -1, 0)
        l = LineSegment(p₄, p₅)
        hit, point = intersect(l, tri)
        @test !hit

        # intersects on boundary of triangle
        p₄ = Point3D{T}(-1, 0, 0)
        p₅ = Point3D{T}( 1, 0, 0)
        l = LineSegment(p₄, p₅)
        hit, point = intersect(l, tri)
        @test hit
        @test point ≈ Point3D{T}(0, 0, 0)

                # 4 intersections
        l = LineSegment2D(p₃, p₁)
        hit, points = intersect(l, quad)
        @test hit
        @test points[1] ≈ p₁
        @test points[2] ≈ p₃

        # 2 intersections
        l = LineSegment2D(Point2D{T}(0, 1//2), Point2D{T}(1, 1//2))
        hit, points = intersect(l, quad)
        @test hit
        @test points[1] ≈ Point2D{T}(1, 1//2)
        @test points[2] ≈ Point2D{T}(0, 1//2)

        # 0 intersections
        l = LineSegment2D(Point2D{T}(-1, -1), Point2D{T}(2, -1))
        hit, points = intersect(l, quad)
        @test !hit

                    # intersect
            # 0 intersection
            l = LineSegment2D(Point2D{T}(0, -1), Point2D{T}(4, -1))
            n, points = l ∩ tri6
            @test n == 0

            # 2 intersection
            l = LineSegment2D(Point2D{T}(0, 1), Point2D{T}(4, 1))
            n, points = l ∩ tri6
            @test n == 2
            @test points[1] ≈ Point2D{T}(3, 1)
            @test points[2] ≈ Point2D{T}(1, 1)

            # 4 intersection
            l = LineSegment2D(Point2D{T}(0, 1//10), Point2D{T}(4, 1//10))
            n, points = l ∩ tri6
            @test n == 4
            @test points[1] ≈ Point2D{T}(0.4254033307585166, 1//10)
            @test points[2] ≈ Point2D{T}(1.9745966692414834, 1//10)
            @test points[3] ≈ Point2D{T}(2.1900000000000000, 1//10)
            @test points[4] ≈ Point2D{T}(1//10,              1//10)

            # 6 intersection
            p₁ = Point2D{T}( 1, 0)
            p₂ = Point2D{T}( 0, 0)
            p₃ = Point2D{T}(-1, 0)
            p₄ = Point2D{T}( 1//2, -1//2)
            p₅ = Point2D{T}(-1//2, -1//2)
            p₆ = Point2D{T}(  0,    -2)
            tri6 = QuadraticTriangle(p₁, p₂, p₃, p₄, p₅, p₆)
            l = LineSegment2D(Point2D{T}(-2, -1//4), Point2D{T}(2, -1//4))
            n, points = l ∩ tri6
            @test n == 6
            @test points[1] ≈ Point2D{T}( 0.14644659, -1//4)
            @test points[2] ≈ Point2D{T}( 0.8535534,  -1//4)
            @test points[3] ≈ Point2D{T}(-0.8535534,  -1//4)
            @test points[4] ≈ Point2D{T}(-0.14644665, -1//4)
            @test points[5] ≈ Point2D{T}( 0.9354143,  -1//4)
            @test points[6] ≈ Point2D{T}(-0.9354143,  -1//4)

        # intersect
        # 0 intersections
        l = LineSegment2D(Point2D{T}(0, -1), Point2D{T}(4, -1))
        npoints, points = l ∩ quad8
        @test npoints == 0

        # 2 intersections
        l = LineSegment2D(Point2D{T}(-1, 1), Point2D{T}(4, 1))
        npoints, points = l ∩ quad8
        @test npoints == 2
        @test points[1] ≈ Point2D{T}(2.44444444444, 1)
        @test points[2] ≈ Point2D{T}(0, 1)

        # 4 intersections
        l = LineSegment2D(Point2D{T}(0, 1//10), Point2D{T}(4, 1//10))
        npoints, points = l ∩ quad8
        @test npoints == 4
        @test points[1] ≈ Point2D{T}(0.20557280900008415,       1//10)
        @test points[2] ≈ Point2D{T}(1.9944271909999158,        1//10)
        @test points[3] ≈ Point2D{T}(2.0644444444444447,        1//10)
        @test points[4] ≈ Point2D{T}(0,                         1//10)

        # 6 intersections
        p₁ = Point2D{T}( 1, 0)
        p₂ = Point2D{T}( 0, 0)
        p₃ = Point2D{T}(-1, 0)
        p₄ = Point2D{T}( 0, -2)
        p₅ = Point2D{T}( 1//2, -1//2)
        p₆ = Point2D{T}(-1//2, -1//2)
        p₇ = Point2D{T}(-1//2, -3//2)
        p₈ = Point2D{T}( 1//2, -3//2)
        quad8 = QuadraticQuadrilateral(p₁, p₂, p₃, p₄, p₅, p₆, p₇, p₈)
        l = LineSegment2D(Point2D{T}(-2, -1//4), Point2D{T}(2, -1//4))
        n, points = l ∩ quad8
        @test n == 6
        @test points[1] ≈ Point2D{T}(T[ 0.14644659, -1//4])
        @test points[2] ≈ Point2D{T}(T[ 0.8535534,  -1//4])
        @test points[3] ≈ Point2D{T}(T[-0.8535534,  -1//4])
        @test points[4] ≈ Point2D{T}(T[-0.14644665, -1//4])
        @test points[5] ≈ Point2D{T}(T[-0.9354143,  -1//4])
        @test points[6] ≈ Point2D{T}(T[ 0.9354143,  -1//4])


dbinom(k, n, p) = Distributions.pdf(Distributions.Binomial(n, p), k)

qnorm(p) = Distributions.quantile(Distributions.Normal(0, 1), p)

function findgrid(prior, l, u, n)

  dp = 1 / (n + 1)
  modprior_(p) = .9 * prior(p) + .1 * Distributions.pdf(Distributions.Beta(1, 1), p)
  zmodprior = QuadGK.quadgk(modprior_, l, u)[1]
  modprior(p) = modprior_(p) / zmodprior * (p < l ? 0 : 1)
  zprior = QuadGK.quadgk(prior, l, u)[1]
  modcdf(p) = QuadGK.quadgk(modprior, l, p)[1]
  cprior(p) = prior(p) / zprior * (p < l ? 0 : 1)
  cdf(p) = QuadGK.quadgk(cprior, l, p)[1]
  pivots  = [l + 10^-6.0]
  modcdfs = [0.0]
  cdfs    = [0.0]
  for i in 2:(n + 1)
    push!(pivots, Roots.fzero(p -> modcdf(p) - dp - modcdfs[i - 1], pivots[i - 1], 1))
    push!(modcdfs, modcdf(pivots[i]))
    push!(cdfs, cdf(pivots[i]))
  end
  pivots = (pivots[2:(n + 1)] .+ pivots[1:n]) / 2 # midpoint pivots!
  dcdf = cdfs[2:(n + 1)] - cdfs[1:n] # vector of first order differences
  dcdf = dcdf / sum(dcdf)
  return pivots, dcdf

end

macro checkprob(x...)

  tmp = Expr(:block)
  for xx in x
    name = string(xx)
    push!(tmp.args, :($xx < 0 ? error(@sprintf("%s must be >= 0, is %.5f", $name, $xx)) : true))
    push!(tmp.args, :($xx > 1 ? error(@sprintf("%s must be <= 1, is %.5f", $name, $xx)) : true))
  end
  return esc(tmp)

end


"""
    kernel_1d(window)
Create a kernel to perform a moving average filtering of a 1-d array. It will use window as windows size unless the given vaule is even, in which case it will add one to the given value before creating the kernel. This kernel has to be passed into imfilter or imfilter!
"""
function kernel_1d(window)
    if !isodd(window)
         window += 1
     end
     kernel_t = centered(ones(window)./(window))
 end

 """
    kernel_4d(window_h,window_t,T :: Type :: Float64)
Create a kernel to perform a moving average filtering of a 4-d array along the dimensions 1,2 and 4. It will use window_h (dimensions 1 and 2) and window_t (dimension 4)  as windows sizes unless the given vaules are even, in which case it will add one to the given value before creating the kernel. This kernel has to be passed into imfilter or imfilter!
"""
 function kernel_4d(window_h,window_t,T::Type=Float64)
    kernel_h = if (window_h == 1) | (window_h == false)
        centered([T(1.0)])
    else
        !isodd(window_h) && (window_h += 1 )
        ones(T,window_h)./(window_h)
    end
     if !isodd(window_t)
         window_t += 1
     end

    kernel_t = if (window_t == 1) | (window_t == false)
        centered([T(1.0)])
    else
        !isodd(window_t) && (window_t += 1 )
        ones(T,window_t)./(window_t)
    end

     return kernelfactors(( kernel_h, kernel_h, centered([T(1.0)]) , kernel_t ))
 end

 """
kernel_3d(window_h,window_t,T :: Type :: Float64)
Create a kernel to perform a moving average filtering of a 3-d array along the dimensions 1,2 and 3. It will use window_h (dimensions 1 and 2) and window_t (dimension 3) as windows sizes unless the given vaules are even, in which case it will add one to the given value before creating the kernel. This kernel has to be passed into imfilter or imfilter!
"""
 function kernel_3d(window_h,window_t,T::Type=Float64)
    if !isodd(window_h)
         window_h += 1
     end
     if !isodd(window_t)
         window_t += 1
     end
     kernel_h = ones(T,window_h)./(window_h)
     return  kernelfactors(( centered(kernel_h), centered(kernel_h), centered([T(1.0)]) ))
 end

 """
 kernel_2d(window_h,T :: Type :: Float64)
 Create a kernel to perform a moving average filtering of a 2-d array along the dimensions 1,2. It will use window_h as windows size unless the given vaule is even, in which case it will add one to the given value before creating the kernel. This kernel has to be passed into imfilter or imfilter!
 """
 function kernel_2d(window_h,T::Type=Float64)
    if !isodd(window_h)
         window_h += 1
     end
     kernel_h = ones(T,window_h)./(window_h)
     return  kernelfactors(( centered(kernel_h), centered(kernel_h) ))
 end

 """
 kernel_4d_t(window_t,T :: Type :: Float64)
 Create a kernel to perform a moving average filtering of a 4-d array along the dimension 4. It will use window_t as windows size unless the given vaule is even, in which case it will add one to the given value before creating the kernel. This kernel has to be passed into imfilter or imfilter!
 """
 function kernel_4d_t(window_t,T::Type=Float64)
     if !isodd(window_t)
         window_t += 1
     end
     kernel_t = ones(T,window_t)./(window_t)

     return kernelfactors(( centered([T(1.0)]), centered([T(1.0)]), centered([T(1.0)]), centered(kernel_t) ))
 end

 """
 kernel_3d_t(window_t,T :: Type :: Float64)
 Create a kernel to perform a moving average filtering of a 3-d array along the dimension 3. It will use window_t as windows size unless the given vaule is even, in which case it will add one to the given value before creating the kernel. This kernel has to be passed into imfilter or imfilter!
 """
 function kernel_3d_t(window_t,T::Type=Float64)
     if !isodd(window_t)
         window_t += 1
     end
     kernel_t = ones(T,window_t)./(window_t)
     return  kernelfactors(( centered([T(1.0)]), centered([T(1.0)]), centered(kernel_t) ))
 end

 """
     filter_array_2!(array,smooth_x,smooth_t,position)

 Filter the input array _in-place_ using a moving mean.

 In the first two dimensions the window width is smooth_x and the border is treated as circular (for doubly periodic domains). In the space dimension the width is smooth_t and the border value is replicated.

 This function calls under the hood the imfilter function of Images.jl

 """
function filter_array_2!(buf :: Array{T,4},array::Array{T,4},smooth_x,smooth_time,position) where T <: Real
    sx,sy,sz,st = size(array)
#    filtered = similar(array)
     if position == 2
         filtered = imfilter(imfilter(array, kernel4d(smooth_x,smooth_time),"circular"),kernel_4d_t(smooth_time),"inner")
     else
         ###Filtering in space

         @info typeof(array) typeof(buf)
         @inbounds for t in 1:st
             @Threads.threads for z in 1:sz
                 imfilter!(view(buf,:,:,z,t),array[:,:,z,t], kernel_2d(smooth_x),"circular")
             end
         end
         @info typeof(array) typeof(buf)
     ### filtering in time
         @inbounds for z in 1:sz, y in 1:sy
             @Threads.threads for x in 1:sx
                 array[x,y,z,:] .= imfilter(buf[x,y,z,:], kernel_1d(smooth_time),"symmetric")
             end
         end
     end
    return array
end

 """
 filter_array_2!(array,smooth_x,smooth_t,position)

Filters the input array _in-place_ using a moving mean.

In the first two dimensions the window width is smooth_x and the border is treated as circular (for doubly periodic domains). In the space dimension the width is smooth_t and the border value is replicated.

This function calls under the hood the imfilter function of Images.jl

"""
 function filter_array_2!(array::Array{T,3},smooth_x,smooth_time,position) where T <: Real
     filtered = similar(array)
     sx,sy,st = size(array)
     if position == 2
         filtered = imfilter(imfilter(array, kernel4d(smooth_x,smooth_time),"circular"),kernel4d_t(smooth_time),"inner")
     else
         ###Filtering in space
         for t in 1:st
             filtered[:,:,t] = imfilter(filtered,array[:,:,t], kernel_2d(smooth_x),"circular")
         end
         ### filtering in time
         for y in 1:sy, x in 1:sx
             array[x,y,:] = imfilter(filtered[x,y,:], kernel_1d(smooth_time),"symmetric")
         end
     end
     #return array
 end
 """
     filter_array!(buffer,array,smooth_x,smooth_t,position)

 Filters the input array _in-place_ using a moving mean. In the first two dimensions the window width is smooth_x and the border is treated as circular (for doubly periodic domains). In the space dimension the width is smooth_t and the border value is replicated.

 This function calls under the hood the imfilter function of Images.jl

 The first argument must be a buffer of the same size of array.

 """
 function filter_array!(buf::Array{T,4},array::Array{T,4},smooth_x,smooth_time,position) where T <: Real
     if position == 2
        error("Filter_array: Inner array is not implemented yet")
     else
         imfilter!(buf,array,kernel_4d(smooth_x,smooth_time,T)[1:2],"circular")
         imfilter!(array,buf,(kernel_4d_t(smooth_time,T)[4],),"symmetric")
     end
 #        return filtered
 end

 function filter_array!(buf::Array{T,3},array::Array{T,3},smooth_x,smooth_time,position) where T <: Real
     if position==2
        error("Filter_array: Inner array is not implemented yet")
     else
         imfilter!(buf,array, kernel_3d(smooth_x,smooth_time,T)[1:2],"circular")
         imfilter!(array,buf,(kernel_3d_t(smooth_time,T)[3],),"symmetric")
     end
 end

 """
 filter_array(array,smooth_x,smooth_t,position)

 Filters the input arrayusing a moving mean. In the first two dimensions the window width is smooth_x and the border is treated as circular (for doubly periodic domains). In the space dimension the width is smooth_t and the border value is replicated.

 This function calls under the hood the imfilter function of Images.jl

 The first argument must be a buffer of the same size of array.

 """
 function filter_array(array::Array{T,4},smooth_x,smooth_time,position = 1) where T <: Real
     if position == 2
        asize = size(array)
        tail_size = (asize[4]-1)÷2
        buf1 = Array{T,4}(undef,asize[1],asize[2],asize[3],asize[4] - 2*tail_size)
        buf2 = Array{T,4}(undef,asize[1],asize[2],asize[3],asize[4] - 2*tail_size)
        buf_axes = axes(buf1)
        imfilter!(OffsetArray(buf1,buf_axes[1],buf_axes[2],buf_axes[3],buf_axes[4].+tail_size),array,(kernel_4d_t(smooth_time,T)[3],),Inner())
        imfilter!(buf2,buf1, kernel_4d(smooth_x,smooth_time,T)[1:2],"circular")
     else
         buf1 = similar(array)
         buf2 = similar(array)
         imfilter!(buf1,array,kernel_4d(smooth_x,smooth_time,T)[1:2],"circular")
         imfilter!(buf2,buf1,(kernel_4d_t(smooth_time,T)[4],),"symmetric")
     end
         return buf2
 end

 """
 filter_array(array,smooth_x,smooth_t,position)

 Filters the input arrayusing a moving mean. In the first two dimensions the window width is smooth_x and the border is treated as circular (for doubly periodic domains). In the space dimension the width is smooth_t and the border value is replicated.

 This function calls under the hood the imfilter function of Images.jl

 The first argument must be a buffer of the same size of array.

 """
function filter_array(array::Array{T,3},smooth_x,smooth_time,position = 1) where T <: Real
    if !isodd(smooth_time)
         smooth_time += 1
    end
    if position==2
        asize = size(array)
        tail_size = (asize[3]-1)÷2
        buf1 = Array{T,3}(undef,asize[1],asize[2],asize[3] - 2*tail_size)
        buf2 = Array{T,3}(undef,asize[1],asize[2],asize[3] - 2*tail_size)
        buf_axes = axes(buf1)
        imfilter!(OffsetArray(buf1,buf_axes[1],buf_axes[2],buf_axes[3].+tail_size),array,(kernel_3d_t(smooth_time,T)[3],),Inner())
        imfilter!(buf2,buf1, kernel_3d(smooth_x,smooth_time,T)[1:2],"circular")

     else
        buf1 = similar(array)
        buf2 = similar(array)
        imfilter!(buf1,array, kernel_3d(smooth_x,smooth_time,T)[1:2],"circular")
        imfilter!(buf2,buf1,(kernel_3d_t(smooth_time,T)[3],),"symmetric")
     end
     return buf2
 end

 """
 filter_array_nospace(array,smooth_t,position)

 Filters the input array using a moving mean along the fourth dimension. In the 4th dimension the width is smooth_t and the border value is replicated except if position = 2, in which case it only takes the inner part of the smoothed array.

 This function calls under the hood the imfilter function of Images.jl
 """
 function filter_array_nospace(array::Array{T,4},smooth_time,position = 1) where T <: Real
     if position==2
         filtered = imfilter(array,kernel_4d_t(smooth_time),"inner")
     else
         filtered = imfilter(array,kernel_4d_t(smooth_time),"replicate")
     end
     return filtered
 end
 """
 filter_array_nospace(array,smooth_t,position)

 Filters the input array using a moving mean along the third dimension. In the 3th dimension the width is smooth_t and the border value is replicated except if position = 2, in which case it only takes the inner part of the smoothed array.

 This function calls under the hood the imfilter function of Images.jl
 """
 function filter_array_nospace(array::Array{T,3},smooth_time,position = 1) where T <: Real
     if position==2
         filtered = imfilter(array,kernel_3d_t(smooth_time),"inner")
     else

         filtered = imfilter(array,kernel_3d_t(smooth_time),"replicate")
     end
     return filtered
 end
 """
 filter_array_nospace(array,smooth_t,position=1)

 Filters the input, 1-d array. The border value is replicated except if position = 2, in which case it only takes the inner part of the smoothed array.

 This function calls under the hood the imfilter function of Images.jl
 """
 function filter_array_time(array::Array{T,1},window,position = 1) where T <: Real
     kern = centered(ones(window)./window)
     if position==2
         smooth_t = imfilter(array,kern,"inner")
     else
         smooth_t = imfilter(array,kern,"symmetric")
     end
     return smooth_t
 end


# # Two-dimensional cellular automata
# Agents.jl provides a module (CA2D) to create and plot 2D cellular automata.

using Agents
using AgentsPlots
using Agents.CA2D

# ## 1. Define the rule
# Rules of Conway's game of life: DSR (Death, Survival, Reproduction).
# Cells die if the number of their living neighbors are <D,
# survive if the number of their living neighbors are <=S,
# come to life if their living neighbors are as many as R.
rules = (2, 3, 3)

# ## 2. Build the model
# "CA2D.build_model" creates a model where all cells are by default off ("0")
model = CA2D.build_model(rules = rules, dims = (100, 100), Moore = true)

# Let's make some random cells on
for i in 1:nv(model)
    if rand() < 0.1
        model.agents[i].status = "1"
    end
end

# ## 3. Animate the model

# Animations of 2D Cellular Automata are done through the following syntax:
# (`plot_CA2Dgif` is a function from `AgentsPlots` that creates the animation)

runs = 10
anim = CA2D.ca_run(model, runs, plot_CA2Dgif);

# We can now save the animation to a gif.

AgentsPlots.gif(anim, "game_of_life.gif")


export Semicircle

"""
Wigner semicircle distribution

By default, creates a standardized distribution
with mean 0 and variance 1 (radius 2)
"""
immutable Semicircle{T<:Real} <: ContinuousUnivariateDistribution
    mean::T
    radius::T
    Semicircle(μ::T,r::T) = new(μ,r)
end
Semicircle{T<:Real}(μ::T=0.0, r::T=2.0) = r > 0 ? Semicircle{T}(μ, r) :
    throw(ArgumentError("radius r must be positive, got $r"))

# Distribution function methods
###############################

# cumulative distribution function
function cdf{T<:Real}(d::Semicircle{T}, x::T)
    r, a = d.mean, d.radius
    if insupport(d, x)
        return 0.5 + (x-a)/(π*r^2) * √(r^2 - (x-a)^2) + asin((x-a)/r)/π
    elseif x ≥ a
        return one(T)
    else
	return zero(T)
    end
end

# probability density function
function pdf{T<:Real}(d::Semicircle{T}, x::T)
    r, a = d.mean, d.radius
    if insupport(d, x)
        return 2/(π*r^2) * √(r^2 - (x-a)^2)
    else
	return zero(T)
    end
end

# predicate is x in the support of the distribution?
insupport{T<:Real}(d::Semicircle{T}, x::T)=abs(x)<d.radius

#Entropy methods
################

# entropy of distribution in nats
entropy(X::Semicircle)=log(π*X.radius) - 0.5

#Measures of central tendency methods
#####################################

# mean of distribution
mean(X::Semicircle)=X.mean

# median of distribution
median(X::Semicircle)=X.mean

# mode(s) of distribution as vector
modes{T}(X::Semicircle{T})=T[X.mean]

# kurtosis of the distribution
kurtosis{T}(X::Semicircle{T})=T(2)

# skewness of the distribution
skewness{T}(X::Semicircle{T})=T(0)

# standard deviation of distribution
std(X::Semicircle)=X.radius/2

# variance of distribution
var(X::Semicircle)=std(X)^2

# moment of distribution
function moment{T<:Real}(X::Semicircle{T}, order::Integer)
    a, r = X.mean, X.radius
    if X.mean != 0
        return a^n*hypergeom([(1-n)/2, -n/2], 2, (r/a)^2)
    elseif iseven(order)
        return (0.5*r)^(2order) * T(catalannum(order÷2))
    else
	return zero(T)
    end
end

# cumulant of distribution
function cumulant{T<:Real}(d::Semicircle{T}, order::Integer)
    if d.mean != 0
        throw(ArgumentError("Non-central Semicircle not supported"))
    end

    if order==0
	return one(T)
    elseif iseven(order)
        return (0.5*d.radius)^(2order) * T(lassallenum(order÷2))
    else
        return zero(T)
    end
end

# free cumulant of distribution
function freecumulant{T<:Real}(d::Semicircle{T}, order::Int)
    if order == 0
        return one(T)
    elseif order == 1
        return mean(X)
    elseif order == 2
        return var(X)
    else
        return zero(T)
    end
end

#Generating function methods
############################

# characteristic function
function cf(d::Semicircle, t::Real)
  r = t / d.mean
  2 * besselj(1, r)/r
end

# moment generating function
function mgf(d::Semicircle, t::Real)
  r = t * d.mean
  2 * besseli(1, r)/r
end

#Sampling methods
#################

# random sampler
# Use relationship with beta distribution
function rand(X::Semicircle)
    Y = rand(Beta(1.5, 1.5))
    X.mean + 2 * X.radius * Y - X.radius
end


module TestSimplicialSets
using Test

using SparseArrays, StaticArrays

using Catlab.CategoricalAlgebra.CSets
using CombinatorialSpaces.SimplicialSets

""" Check that the semi-simplicial identities hold in dimension `n`.
"""
function is_semi_simplicial(s::AbstractACSet, n::Int)
  all(∂(n-1, i, s, ∂(n, j, s)) == ∂(n-1, j-1, s, ∂(n, i, s))
      for i in 0:n for j in (i+1):n)
end

const Point2D = SVector{2,Float64}
const Point3D = SVector{3,Float64}

# 1D simplicial sets
####################

s = DeltaSet1D()
add_vertices!(s, 4)
add_sorted_edge!(s, 2, 1)
add_sorted_edges!(s, [2,4], [3,3])
@test src(s) == [1,2,3]
@test tgt(s) == [2,3,4]
@test ∂(1, 0, s) == [2,3,4]
@test ∂(1, 1, s) == [1,2,3]
@test ∂(0, s, E(1))::V == V(2)
@test ∂(1, s, E([1,3]))::V == V([1,3])
@test coface(0, s, V(4))::E == E([3])

# 1D oriented simplicial sets
#----------------------------

s = OrientedDeltaSet1D{Bool}()
add_vertices!(s, 4)
add_edges!(s, [1,2,3], [2,3,4], edge_orientation=[true,false,true])
@test ∂(1, s, 1) == [-1,1,0,0]
@test ∂(1, s, 2) == [0,1,-1,0]

# Boundary operator, dense vectors.
vvec = ∂(1, s, 1, Vector{Int})
@test !issparse(vvec)
@test vvec == [-1,1,0,0]
vvec = ∂(1, s, [1,-1,1])
@test !issparse(vvec)
@test vvec == [-1,0,0,1]
@test ∂(s, EChain([1,-1,1]))::VChain == VChain(vvec)

# Boundary operator, sparse vectors.
vvec = ∂(1, s, 1, SparseVector{Int})
@test issparse(vvec)
@test vvec == [-1,1,0,0]
vvec = ∂(1, s, sparsevec([1,-1,1]))
@test issparse(vvec)
@test vvec == [-1,0,0,1]
@test ∂(s, EChain(sparsevec([1,-1,1]))) == VChain(vvec)
B = ∂(1, s)
@test issparse(B)
@test B*[1,-1,1] == [-1,0,0,1]

# Exterior derivative.
@test d(s, VForm([1,1,1,4]))::EForm == EForm([0,0,3])
@test d(s, VForm([4,1,0,0])) == EForm([-3,1,0])
@test d(0, s) == B'

# Consistent orientation.
s = OrientedDeltaSet1D{Bool}()
add_vertices!(s, 10)
add_edges!(s, 1:4, 2:5)
add_edges!(s, 6:9, 7:10)
@test orient_component!(s, 1, true)
@test orientation(s, E(1:4)) == trues(4)
@test orient_component!(s, 8, true)
@test orientation(s, E(1:8)) == trues(8)

s[:edge_orientation] = rand(Bool, 8)
@test orient!(s)
@test orientation(s, E(1:8)) == trues(8)

# 1D embedded simplicial sets
#----------------------------

s = EmbeddedDeltaSet1D{Bool,Point2D}()
add_vertices!(s, 2, point=[Point2D(-1, 0), Point2D(+1, 0)])
add_edge!(s, 1, 2, edge_orientation=true)
@test volume(1, s, 1) ≈ 2
@test volume(s, E(1)) ≈ 2

# 2D simplicial sets
####################

s = DeltaSet2D()
add_vertices!(s, 3)
glue_triangle!(s, 1, 2, 3)
@test is_semi_simplicial(s, 2)
@test ntriangles(s) == 1
@test map(i -> ∂(2, i, s, 1), (0,1,2)) == (2,3,1)
@test triangle_vertices(s, 1) == [1,2,3]

s′ = DeltaSet2D()
add_vertices!(s′, 3)
glue_sorted_triangle!(s′, 2, 3, 1)
@test s′ == s

# Triangulated commutative square.
s = DeltaSet2D()
add_vertices!(s, 4)
glue_triangle!(s, 1, 2, 3)
glue_triangle!(s, 1, 4, 3)
@test ntriangles(s) == 2
@test triangles(s) == 1:2
@test ne(s) == 5
@test sort(map(Pair, src(s), tgt(s))) == [1=>2, 1=>3, 1=>4, 2=>3, 4=>3]

# 2D oriented simplicial sets
#----------------------------

# Triangle with matching edge orientations.
s = OrientedDeltaSet2D{Bool}()
add_vertices!(s, 3)
add_sorted_edges!(s, [1,2,3], [2,3,1], edge_orientation=[true,true,false])
glue_triangle!(s, 1, 2, 3)
@test orient_component!(s, 1, true)
@test orientation(s, Tri(1)) == true
@test ∂(2, s, 1) == [1,1,1]
@test d(1, s, [45,3,34]) == [82]

# Triangulated square with consistent orientation.
s = OrientedDeltaSet2D{Bool}()
add_vertices!(s, 4)
glue_triangle!(s, 1, 2, 3)
glue_triangle!(s, 1, 3, 4)
s[:edge_orientation] = true
@test orient!(s)
@test orientation(s, Tri(1:2)) == trues(2)
@test ∂(2, s, 1) == [1,1,-1,0,0]
@test ∂(s, TriChain([1,1]))::EChain == EChain([1,1,0,1,-1])
@test d(s, EForm([45,3,34,0,0]))::TriForm == TriForm([14, 34]) # == [45+3-34, 34]
@test d(s, EForm([45,3,34,17,5])) == TriForm([14, 46]) # == [45+3-34, 34+17-5]
@test d(1, s) == ∂(2, s)'

# 2D embedded simplicial sets
#----------------------------

# Standard 2-simplex in ℝ³.
s = EmbeddedDeltaSet2D{Bool,Point3D}()
add_vertices!(s, 3, point=[Point3D(1,0,0), Point3D(0,1,0), Point3D(0,0,1)])
glue_triangle!(s, 1, 2, 3, tri_orientation=true)
@test volume(s, Tri(1)) ≈ sqrt(3)/2

# Euclidean geometry
####################

std_simplex_volume(n::Int) = sqrt(n+1) / factorial(n)

p1, p2 = Point2D(1,0), Point2D(0,1)
@test volume([p1, p2]) ≈ std_simplex_volume(1)

p1, p2, p3 = Point3D(1,0,0), Point3D(0,1,0), Point3D(0,0,1)
@test volume([p1, p2, p3]) ≈ std_simplex_volume(2)

p1, p2, p3, p4 = SVector(1,0,0,0), SVector(0,1,0,0), SVector(0,0,1,0), SVector(0,0,0,1)
@test volume([p1, p2, p3, p4]) ≈ std_simplex_volume(3)

end