# Some utility functions

#================================ Compensated Summation =================================#
"""
    comp_sum(sum_value,sum_error,addend)

Kahan (1965) compensated summation for `T<:Real`.
...
# Arguments
- `sum_value::Real` : Current value of the sum.
- `sum_error::Real` : Error from truncation/rounding accumulated from prior steps.
- `addend::Real` : New value to be added to the sum.
...
"""
function comp_sum(sum_value::T,sum_error::T,addend::T) where {T <: Real}
    tmp = zero(T)
    sum_error += addend
    tmp = sum_value + sum_error
    sum_error = (sum_value - tmp) + sum_error
    sum_value = tmp
    return sum_value,sum_error
end

"""
    comp_sum_matrix!(sum_value,sum_error,addend)

Kahan (1965) compenstated summation for `::Matrix{<:Real}`.
...
# Arguments
- `sum_value::Matrix{<:Real}` : Current value of the sum.
- `sum_error::Matrix{<:Real}` : Error from truncation/rounding accumulated from prior steps.
- `addend::Matrix{<:Real}` : New value to be added to the sum.
...
"""
function comp_sum_matrix!(sum_value::Array{T,2},sum_error::Array{T,2},addend::Array{T,2}) where {T <: Real}
    tmp = zero(T)
    @inbounds for i in eachindex(sum_value)
        sum_error[i] += addend[i]
        tmp = sum_value[i] + sum_error[i]
#        sum_error[i] = (sum_value[i] - tmp) + sum_error[i]
        sum_error[i] += sum_value[i] - tmp
        sum_value[i] = tmp
    end
    return
end

"""
  Copies portion of Jacobian for multiplication
"""
     function copy_submatrix!(s::State{T},d::Derivatives{T},indi::Int64,indj::Int64,sevn::Int64)  where T <: AbstractFloat
       # Pick out indices for bodies i & j:
       @inbounds for k2=1:sevn, k1=1:7
           d.jac_tmp1[k1,k2] = s.jac_step[ indi+k1,k2]
       end
       @inbounds for k2=1:sevn, k1=1:7
           d.jac_err1[k1,k2] = s.jac_error[indi+k1,k2]
       end
       @inbounds for k2=1:sevn, k1=1:7
           d.jac_tmp1[7+k1,k2] = s.jac_step[ indj+k1,k2]
       end
       @inbounds for k2=1:sevn, k1=1:7
           d.jac_err1[7+k1,k2] = s.jac_error[indj+k1,k2]
       end
       # Copy current time derivatives for multiplication purposes:
       @inbounds for k1=1:7
           d.dqdt_tmp1[  k1] = s.dqdt[indi+k1]
       end
       @inbounds for k1=1:7
           d.dqdt_tmp1[7+k1] = s.dqdt[indj+k1]
       end
     return
     end

"""
  Copies back:
"""
     function ypoc_submatrix!(s::State{T},d::Derivatives{T},indi::Int64,indj::Int64,sevn::Int64)  where T <: AbstractFloat
       # Copy back to the Jacobian:
       @inbounds for k2=1:sevn, k1=1:7
           s.jac_step[ indi+k1,k2]=d.jac_tmp1[k1,k2]
       end
       @inbounds for k2=1:sevn, k1=1:7
           s.jac_error[indi+k1,k2]=d.jac_err1[k1,k2]
       end
       @inbounds for k2=1:sevn, k1=1:7
           s.jac_step[ indj+k1,k2]=d.jac_tmp1[7+k1,k2]
       end
       @inbounds for k2=1:sevn, k1=1:7
           s.jac_error[indj+k1,k2]=d.jac_err1[7+k1,k2]
       end
       # Copy back time derivatives:
       @inbounds for k1=1:7
           s.dqdt[indi+k1] = d.dqdt_ij[  k1]
       end
       @inbounds for k1=1:7
           s.dqdt[indj+k1] = d.dqdt_ij[7+k1]
       end
    return
    end
#================================ Integrator Utilities ==================================#

function cubic1(a::T, b::T, c::T) where {T <: Real}
    a3 = a*third
    Q = a3^2 - b*third
    R = a3^3 + 0.5*(-a3*b + c)
    R2 = R^2; Q3=Q^3
    if R2 < Q3
        # println("Error in cubic solver ",R^2," ",Q^3)
        return -c/b
    else
        A = -sign(R)*cbrt(abs(R) + sqrt(R2 - Q3))
        if A == 0.0
            B = 0.0
        else
            B = Q/A
        end
        x1 = A + B - a3
        return x1
    end
    return
end

function G3(gamma::T,beta::T,sqb::T;gc=convert(T,0.5)) where {T <: Real}
    if gamma < gc
        return G3_series(gamma,beta,sqb)
    else
#        sqb = sqrt(abs(beta))
        if beta >= 0
            return (gamma-sin(gamma))/(sqb*beta)
        else
            return (gamma-sinh(gamma))/(sqb*beta)
        end
    end
end

function G3_series(gamma::T,beta::T,sqb::T) where {T <: Real}
    #epsilon = eps(gamma)
    # Computes G_3(\beta,s) using a series tailored to the precision of s.
    #x2 = -beta*s^2
    x2 = -sign(beta)*gamma^2
    term = one(T)
    g3 = one(T)
    g31 = 2g3
    g32 = 2g3
    n=0
    iter = 0
    ITMAX = 100
    # Terminate series when required precision reached:
    #while abs(term) > epsilon*abs(g3)
    while true
        g32 = g31
        g31 = g3
        n += 1
        term *= x2/((2n+3)*(2n+2))
        g3 += term
        iter +=1
        if iter >= ITMAX || g3 == g32 || g3 == g31
            break
        end
    end
#    g3 *= gamma^3/(6*sqrt(abs(beta^3)))
    g3 *= -x2*gamma/(6*beta*sqb)
    return g3::T
end

function H1(gamma::T,beta::T;gc=convert(T,0.5)) where {T <: Real}
    #x=sqrt(abs(beta))*s
    if gamma < gc
        return H1_series(gamma,beta)
    else
        if beta >= 0
            return (4sin(0.5*gamma)^2 -gamma*sin(gamma))/beta^2
        else
            return (-4sinh(0.5*gamma)^2+gamma*sinh(gamma))/beta^2
        end
    end
end

function H1_series(gamma::T,beta::T) where {T <: Real}
    # Computes H_1(\beta,s) using a series tailored to the precision of s.
    #epsilon = eps(gamma)
    #x2 = -beta*s^2
    x2 = -sign(beta)*gamma^2
    term = one(T)
    h1 = one(T)
    h11 = 2h1
    h12 = 2h1
    n=0
    iter = 0
    ITMAX = 100
    # Terminate series when required precision reached:
    #while abs(term) > epsilon*abs(h1)
    while true
        h12 = h11
        h11 = h1
        n += 1
        term *= x2*(n+1)
        term /= (2n+4)*(2n+3)*n
        h1 += term
        iter +=1
        if iter >= ITMAX || h1 == h12 || h1 == h11
            break
        end
    end
#    h1 *= gamma^4/(12*beta^2)
    h1 *= x2^2/(12*beta^2)
    return h1::T
end

function H2(gamma::T,beta::T,sqb::T;gc=convert(T,0.5)) where {T <: Real}
    #x=sqb*s
    if gamma < gc
        return H2_series(gamma,beta,sqb)
    else
#        sqb = sqrt(abs(beta))
        if beta >= 0
            return (sin(gamma)-gamma*cos(gamma))/(sqb*beta)
        else
            return (sinh(gamma)-gamma*cosh(gamma))/(sqb*beta)
        end
    end
end

function H2_series(gamma::T,beta::T,sqb::T) where {T <: Real}
    # Computes H_2(\beta,s) using a series tailored to the precision of s.
    #epsilon = eps(gamma)
    #x2 = -beta*s^2
    x2 = -sign(beta)*gamma^2
    term = one(T)
    h2 = one(T)
    h21 = 2h2
    h22 = 2h2
    n=0
    iter = 0
    ITMAX = 100
    # Terminate series when required precision reached:
    #while abs(term) > epsilon*abs(h2)
    while true
        h22 = h21
        h21 = h2
        n += 1
        term *= x2
        term /= (4n+6)*n
        h2 += term
        iter += 1
        if iter >= ITMAX || h2 == h22 || h2 == h21
            break
        end
    end
#    h2 *= gamma^3/(3*sqrt(abs(beta^3)))
    h2 *= -x2*gamma/(3*beta*sqb)
    return h2::T
end

function H3(gamma::T,beta::T,sqb::T;gc=convert(T,0.5)) where {T <: Real}
    # This is H_3 = G_1 G_2 - 3 G_3:
    if gamma < gc
        return H3_series(gamma,beta,sqb)
    else
        if beta >= 0
#            return (4*sin(gamma)-sin(gamma)*cos(gamma)-3*gamma)/(beta*sqrt(beta))
            return (4*sin(gamma)-sin(gamma)*cos(gamma)-3*gamma)/(beta*sqb)
        else
#            return (4*sinh(gamma)-sinh(gamma)*cosh(gamma)-3*gamma)/(beta*sqrt(-beta))
            return (4*sinh(gamma)-sinh(gamma)*cosh(gamma)-3*gamma)/(beta*sqb)
        end
    end
end

function H3_series(gamma::T,beta::T,sqb::T) where {T <: Real}
    # Computes H_3(\beta,s) using a series tailored to the precision of gamma:
    #epsilon = eps(gamma)
    #x2 = -beta*s^2
    x2 = -sign(beta)*gamma^2
    term = convert(T,1//30)
    h3 = convert(T,1//10)
    h31 = 2h3
    h32 = 2h3
    n=0
    iter = 0
    ITMAX = 100
    # Terminate series when required precision reached:
    #while abs(term) > epsilon*abs(h3)
    four2n = one(T)*4
    while true
        h32 = h31
        h31 = h3
        n += 1
        term *= x2
        term /= (2n+4)*(2n+5)
        four2n *= 4
#        h3 += term*(4^(n+1)-1)
        h3 += term*(four2n-1)
        iter += 1
        if iter >= ITMAX || h3 == h32 || h3 == h31
            break
        end
    end
#    h3 *= -gamma^5/(beta*sqrt(abs(beta)))
    h3 *= -x2^2*gamma/(beta*sqb)
    return h3::T
end

function H5(gamma::T,beta::T,sqb::T;gc=convert(T,0.5)) where {T <: Real}
    # This is G_1 G_2 -(2+G_0) G_3 = H_2 - 2 G_3:
    if gamma < gc
        return H5_series(gamma,beta,sqb)
    else
        if beta >= 0
#            return (3sin(gamma)-2gamma-gamma*cos(gamma))/(beta*sqrt(beta))
            return (3sin(gamma)-2gamma-gamma*cos(gamma))/(beta*sqb)
        else
#            return (3sinh(gamma)-2gamma-gamma*cosh(gamma))/(beta*sqrt(-beta))
            return (3sinh(gamma)-2gamma-gamma*cosh(gamma))/(beta*sqb)
        end
    end
end

function H5_series(gamma::T,beta::T,sqb::T) where {T <: Real}
    # Computes H_5(\beta,s) using a series tailored to the precision of gamma:
    #epsilon = eps(gamma)
    #x2 = -beta*s^2
    x2 = -sign(beta)*gamma^2
    term = one(T)/60
    h5 = copy(term)
    h51 = 2h5
    h52 = 2h5
    n=0
    iter = 0
    ITMAX = 100
    # Terminate series when required precision reached:
    #while abs(term) > epsilon*abs(h5)
    while true
        h52 = h51
        h51 = h5
        n += 1
        term *= x2*(n+1)
        term /= (2n+5)*(2n+4)*n
        h5 += term
        iter += 1
        if iter >= ITMAX || h5 == h52 || h5 == h51
            break
        end
    end
#    h5 *= -gamma^5/(beta*sqrt(abs(beta)))
    h5 *= -x2^2*gamma/(beta*sqb)
    return h5::T
end

function H6(gamma::T,beta::T;gc=convert(T,0.5)) where {T <: Real}
    # This is 2 G_2^2 -3 G_1 G_3:
    if gamma < gc
        return H6_series(gamma,beta)
    else
        if beta >= 0
            return (9-8cos(gamma) -cos(2gamma) -6gamma*sin(gamma))/(2beta^2)
        else
            return (9-8cosh(gamma)-cosh(2gamma)+6gamma*sinh(gamma))/(2beta^2)
        end
    end
end

function H6_series(gamma::T,beta::T) where {T <: Real}
    # Computes H_6(\beta,s) using a series tailored to the precision of gamma:
    #epsilon = eps(gamma)
    #x2 = -beta*s^2
    x2 = -sign(beta)*gamma^2
    term = convert(T,1//360)
    h6 = convert(T,1//40)
    h61 = 2h6
    h62 = 2h6
    n=0
    iter = 0
    ITMAX = 100
    # Terminate series when required precision reached:
    #while abs(term) > epsilon*abs(h6)
    four2n = one(T)*16
    while true
        h62 = h61
        h61 = h6
        n += 1
        term *= x2
        term /= (2n+5)*(2n+6)
        four2n *= 4
#        h6 += term*(4^(n+2)-3*n-7)
        h6 += term*(four2n-3*n-7)
        iter +=1
        if iter >= ITMAX || h6 == h62 || h6 == h61
            break
        end
    end
#    h6 *= gamma^6/(beta*abs(beta))
    h6 *= -x2^3/(beta^2)
    return h6::T
end

function H7(gamma::T,beta::T,sqb::T;gc=convert(T,0.5)) where {T <: Real}
    # This is H_7 = G_1 G_2 (1-2 G_0) + 3 G_0^2 G_3:
    if gamma < gc
        return H7_series(gamma,beta,sqb)
    else
        if beta >= 0
#            return (3*cos(gamma)*(gamma*cos(gamma)-sin(gamma))+sin(gamma)^3)/(beta*sqrt(beta))
            return (3*cos(gamma)*(gamma*cos(gamma)-sin(gamma))+sin(gamma)^3)/(beta*sqb)
        else
#            return (3*cosh(gamma)*(gamma*cosh(gamma)-sinh(gamma))-sinh(gamma)^3)/(beta*sqrt(-beta))
            return (3*cosh(gamma)*(gamma*cosh(gamma)-sinh(gamma))-sinh(gamma)^3)/(beta*sqb)
        end
    end
end

function H7_series(gamma::T,beta::T,sqb::T) where {T <: Real}
    # Computes H_7(\beta,s) using a series tailored to the precision of gamma:
    #epsilon = eps(gamma)
    #x2 = -beta*s^2
    x2 = -sign(beta)*gamma^2
    term = -convert(T,3//20160)*x2
    h7 = convert(T,1//10-11//840*x2)
    h71 = 2h7
    h72 = 2h7
    n=0
    iter = 0
    ITMAX = 100
    # Terminate series when required precision reached:
    #while abs(term) > epsilon*abs(h7)
    four2n = one(T)*128
    nine2n = one(T)*729
    while true
        h72 = h71
        h71 = h7
        n += 1
        term *= x2
        term /= (2n+6)*(2n+7)
        four2n *= 4
        nine2n *= 9
#        h7 += term*(9^(3+n)-1-(5+2*n)*2^(7+2*n))
        h7 += term*(nine2n-1-(5+2*n)*four2n)
        iter += 1
        if iter >= ITMAX || h7 == h72 || h7 == h71
            break
        end
    end
#    h7 *= gamma^5/(beta*sqrt(abs(beta)))
    h7 *= x2^2*gamma/(beta*sqb)
    return h7::T
end

function H8(gamma::T,beta::T,sqb::T;gc=convert(T,0.5)) where {T <: Real}
    # This is H_8 = G_1 G_2 - 3 G_0 G_3:
    if gamma < gc
        return H8_series(gamma,beta,sqb)
    else
        if beta >= 0
#            return (-3gamma*cos(gamma) +sin(gamma) +sin(2gamma))/(beta*sqrt(beta))
            return (-3gamma*cos(gamma) +sin(gamma) +sin(2gamma))/(beta*sqb)
        else
#            return (-3gamma*cosh(gamma)+sinh(gamma)+sinh(2gamma))/(beta*sqrt(-beta))
            return (-3gamma*cosh(gamma)+sinh(gamma)+sinh(2gamma))/(beta*sqb)
        end
    end
end

function H8_series(gamma::T,beta::T,sqb::T) where {T <: Real}
    # Computes H_8(\beta,s) using a series tailored to the precision of gamma:
    #epsilon = eps(gamma)
    #x2 = -beta*s^2
    x2 = -sign(beta)*gamma^2
    term = convert(T,1//120)
    h8 = convert(T,3//20)
    h81 = 2h8
    h82 = 2h8
    n=0
    iter = 0
    ITMAX = 100
    # Terminate series when required precision reached:
    #while abs(term) > epsilon*abs(h8)
    four2n = one(T)*32
    while true
        h82 = h81
        h81 = h8
        n += 1
        term *= x2
        term /= (2n+4)*(2n+5)
        four2n *= 4
#        h8 += term*(2^(5+2n)-14-6n)
        h8 += term*(four2n-14-6n)
        iter +=1
#        println("H8: ",iter," ",h8," ",gamma," ",beta)
        if iter >= ITMAX || h8 == h82 || h8 == h81
            break
        end
    end
#    h8 *= gamma^5/(beta*sqrt(abs(beta)))
    h8 *= x2^2*gamma/(beta*sqb)
    return h8::T
end

# Faster dot product; assumes 3D vector
@inline function dot_fast(a::Vector{T}, b::Vector{T}) where T<:Real
    a[1]*b[1] + a[2]*b[2] + a[3]*b[3]
end

@inline function dot_fast(a::Vector{T}) where T<:Real
    a[1]*a[1] + a[2]*a[2] + a[3]*a[3]
end


TimeCost = []
D = Distributions.Normal(0.0, 1.0)
for x in 1:10000
  # sampling, and get the time cost (gc time included), then store
  push!(TimeCost, @timed( NovDist.LatinHyperCube(D, 100,100) )[2] )
end
# averaging
StatsBase.mean(TimeCost)

PyPlot.plt[:hist]( tmp, 40, density = true );
PyPlot.grid(true); PyPlot.ylabel("Density"); PyPlot.xlabel("x");
PyPlot.title("PDF of 10000 samples from N(0,1)");
PyPlot.tight_layout()

TimeCost = []
Sigma = [0.5 -0.25; -0.25 1.5];
Mu = [ 0.0, 1.0 ];
D = Distributions.MvNormal(Mu, Sigma)
for x in 1:10000
  push!(TimeCost, @timed( NovDist.LatinHyperCube(D, 100,100) )[2] )
end
StatsBase.mean(TimeCost)
StatsBase.std(TimeCost)

#


using DiffEqFlux, Optim, OrdinaryDiffEq, DiffEqSensitivity, Test

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

function condition(u,t,integrator) # Event when event_f(u,t) == 0
  u[1]
end

function affect!(integrator)
  integrator.u[2] = -integrator.p[2]*integrator.u[2]
end

cb = ContinuousCallback(condition,affect!)
u0 = [50.0,0.0]
tspan = (0.0,15.0)
p = [9.8, 0.8]
prob = ODEProblem(f,u0,tspan,p)
sol = solve(prob,Tsit5(),callback=cb)

function loss(θ)
  sol = solve(prob,Tsit5(),p=[9.8,θ[1]],callback=cb,sensealg=ForwardDiffSensitivity())
  target = 20.0
  abs2(sol[end][1] - target)
end

res = DiffEqFlux.sciml_train(loss,[0.8],BFGS())
@test loss(res.minimizer) < 1

function loss(θ)
  sol = solve(prob,Tsit5(),p=[9.8,θ[1]],callback=cb,sensealg=ReverseDiffAdjoint())
  target = 20.0
  abs2(sol[end][1] - target)
end

res = DiffEqFlux.sciml_train(loss,[0.8],BFGS())
@test loss(res.minimizer) < 1


# This file is part of the IntervalArithmetic.jl package; MIT licensed

using IntervalArithmetic
using Test

setprecision(Interval, Float64)

@testset "Consistency tests" begin

    a = @interval(0.1, 1.1)
    b = @interval(0.9, 2.0)
    c = @interval(0.25, 4.0)

    @testset "Interval types and constructors" begin

        @test isa( @interval(1,2), Interval )
        @test isa( @interval(0.1), Interval )
        @test isa( zero(b), Interval )

        @test zero(b) == 0.0
        @test zero(b) == zero(typeof(b))
        @test one(a) == 1.0
        @test one(a) == one(typeof(a))
        @test one(a) == big(1.0)
        @test !(a == b)
        @test a != b
        @test eps(typeof(a)) === eps(one(typeof(a)))
        @test typemin(typeof(a)) === Interval(-Inf, nextfloat(-Inf))
        @test typemax(typeof(a)) === Interval(prevfloat(Inf), Inf)
        @test typemin(a) === typemin(typeof(a))
        @test typemax(a) === typemax(typeof(a))
        @test typemin(Interval{Int64}) === Interval(typemin(Int64))
        @test typemax(Interval{Int64}) === Interval(typemax(Int64))

        @test a == Interval(a.lo, a.hi)
        @test @interval(1, Inf) == Interval(1.0, Inf)
        @test @interval(-Inf, 1) == Interval(-Inf, 1.0)
        @test @biginterval(1, Inf) == Interval{BigFloat}(1.0, Inf)
        @test @biginterval(-Inf, 1) == Interval{BigFloat}(-Inf, 1.0)
        @test @interval(-Inf, Inf) == entireinterval(Float64)
        @test entireinterval(Int) == entireinterval(Float64)
        @test emptyinterval(Rational{Int}) == ∅
        @test emptyinterval(Int) == ∅

        @test 1 == zero(a)+one(b)
        @test Interval(0,1) + emptyinterval(a) == emptyinterval(a)
        @test @interval(0.25) - one(c)/4 == zero(c)
        @test emptyinterval(a) - Interval(0,1) == emptyinterval(a)
        @test Interval(0,1) - emptyinterval(a) == emptyinterval(a)
        @test a*b == Interval(a.lo*b.lo, a.hi*b.hi)
        @test Interval(0,1) * emptyinterval(a) == emptyinterval(a)
        @test a * Interval(0) == zero(a)
    end

    @testset "inv" begin

        @test inv( zero(a) ) == emptyinterval()
        @test inv( @interval(0, 1) ) == Interval(1, Inf)
        @test inv( @interval(1, Inf) ) == Interval(0, 1)
        @test inv(c) == c
        @test one(b)/b == inv(b)
        @test a/emptyinterval(a) == emptyinterval(a)
        @test emptyinterval(a)/a == emptyinterval(a)
        @test inv(@interval(-4.0,0.0)) == @interval(-Inf, -0.25)
        @test inv(@interval(0.0,4.0)) == @interval(0.25, Inf)
        @test inv(@interval(-4.0,4.0)) == entireinterval(Float64)
        @test @interval(0)/@interval(0) == emptyinterval()
        @test typeof(emptyinterval()) == Interval{Float64}
    end

    @testset "fma consistency" begin
        @test fma(emptyinterval(), a, b) == emptyinterval()
        @test fma(entireinterval(), zero(a), b) == b
        @test fma(entireinterval(), one(a), b) == entireinterval()
        @test fma(zero(a), entireinterval(), b) == b
        @test fma(one(a), entireinterval(), b) == entireinterval()
        @test fma(a, zero(a), c) == c
        @test fma(Interval(1//2), Interval(1//3), Interval(1//12)) == Interval(3//12)
    end

    @testset "∈ tests" begin
        @test !(Inf ∈ entireinterval())
        @test 0.1 ∈ @interval(0.1)
        @test 0.1 in @interval(0.1)
        @test !(-Inf ∈ entireinterval())
        @test !(Inf ∈ entireinterval())

        @test_throws ArgumentError (3..4) ∈ (3..4)
    end

    @testset "Inclusion tests" begin
        @test b ⊆ c
        @test emptyinterval(c) ⊆ c
        @test !(c ⊆ emptyinterval(c))
        @test isinterior(b,c)
        @test !(b ⪽ emptyinterval(b))
        @test emptyinterval(c) ⪽ c
        @test emptyinterval(c) ⪽ emptyinterval(c)
        @test b ⊂ c
        @test !(b ⊂ b)
        @test emptyinterval(c) ⊂ c
        @test !(c ⊂ emptyinterval(c))
        @test c ⊃ b
        @test !(b ⊃ b)
        @test !(emptyinterval(c) ⊃ c)
        @test c ⊃ emptyinterval(c)
        @test c ⊇ b
        @test b ⊇ b
        @test !(emptyinterval(c) ⊇ c)
        @test c ⊇ emptyinterval(c)
        @test isdisjoint(a, @interval(2.1))
        @test !(isdisjoint(a, b))
        @test isdisjoint(emptyinterval(a), a)
        @test isdisjoint(emptyinterval(), emptyinterval())
    end

    @testset "Comparison tests" begin
        @test IntervalArithmetic.islessprime(a.lo, b.lo) == (a.lo < b.lo)
        @test IntervalArithmetic.islessprime(Inf, Inf)
        @test ∅ <= ∅
        @test !(Interval(1.0,2.0) <= ∅)
        @test Interval(-Inf,Inf) <= Interval(-Inf,Inf)
        @test !(Interval(-0.0,2.0) ≤ Interval(-Inf,Inf))
        @test precedes(∅,∅)
        @test precedes(Interval(3.0,4.0),∅)
        @test !(precedes(Interval(0.0,2.0),Interval(-Inf,Inf)))
        @test precedes(Interval(1.0,3.0),Interval(3.0,4.0))
        @test strictprecedes(Interval(3.0,4.0),∅)
        @test !(strictprecedes(Interval(-3.0,-1.0),Interval(-1.0,0.0)))
        @test !(iscommon(emptyinterval()))
        @test !(iscommon(entireinterval()))
        @test iscommon(a)
        @test !(isunbounded(emptyinterval()))
        @test isunbounded(entireinterval())
        @test isunbounded(Interval(-Inf, 0.0))
        @test isunbounded(Interval(0.0, Inf))
        @test !(isunbounded(a))
    end

    @testset "Intersection tests" begin
        @test emptyinterval() == Interval(Inf, -Inf)
        @test (a ∩ @interval(-1)) == emptyinterval(a)
        @test isempty(a ∩ @interval(-1) )
        @test !(isempty(a))
        @test !(emptyinterval(a) == a)
        @test emptyinterval() == emptyinterval()

        @test intersect(a, hull(a,b)) == a
        @test union(a,b) == Interval(a.lo, b.hi)

        # n-ary intersection
        @test intersect(Interval(1.0, 2.0),
                        Interval(-1.0, 5.0),
                        Interval(1.8, 3.0)) == Interval(1.8, 2.0)
        @test intersect(a, emptyinterval(), b) == emptyinterval()
        @test intersect(0..1, 3..4, 0..1, 0..1) == emptyinterval()
    end

    @testset "Hull and union tests" begin
        @test hull(1..2, 3..4) == Interval(1, 4)
        @test hull(Interval(1//3, 3//4), Interval(3, 4)) == @interval(1/3, 4)

        @test union(1..2, 3..4) == Interval(1, 4)
        @test union(Interval(1//3, 3//4), Interval(3, 4)) == @interval(1/3, 4)
    end

    @testset "Special interval tests" begin

        @test entireinterval(Float64) == Interval(-Inf, Inf)
        @test isentire(entireinterval(a))
        @test isentire(Interval(-Inf, Inf))
        @test !(isentire(a))
        @test Interval(-Inf, Inf) ⪽ Interval(-Inf, Inf)

        @test nai(a) === nai(a)
        @test nai(Float64) === DecoratedInterval(NaN)
        @test isnan(interval(nai(BigFloat)).lo)
        @test isnai(nai())
        @test !(isnai(a))

        @test inf(a) == a.lo
        @test sup(a) == a.hi
        @test inf(emptyinterval(a)) == Inf
        @test sup(emptyinterval(a)) == -Inf
        @test inf(entireinterval(a)) == -Inf
        @test sup(entireinterval(a)) == Inf
        @test isnan(sup(nai(BigFloat)))
    end

    @testset "mid" begin

        @test mid(Interval(1//2)) == 1//2
        @test mid(1..2) == 1.5
        @test mid(0.1..0.3) == 0.2
        @test mid(-10..5) == -2.5
        @test mid(-∞..1) == nextfloat(-∞)
        @test mid(1..∞) == prevfloat(∞)
        @test isnan(mid(emptyinterval()))
    end

    @testset "mid with parameter" begin
        @test mid(0..1, 0.75) == 0.75
        @test mid(1..∞, 0.75) > 0
        @test mid(-∞..∞, 0.75) > 0
        @test mid(-∞..∞, 0.25) < 0
    end

    @testset "mid with large floats" begin
        @test mid(0.8e308..1.2e308) == 1e308
        @test mid(-1e308..1e308) == 0
        @test isfinite(mid(0.8e308..1.2e308, 0.75))
        @test isfinite(mid(-1e308..1e308, 0.75))
    end

    @testset "diam" begin

        @test diam( Interval(1//2) ) == 0//1
        @test diam( @interval(1//10) ) == eps(0.1)
        @test diam( @interval(0.1) ) == eps(0.1)
        @test isnan(diam(emptyinterval()))
        @test diam(a) == 1.0000000000000002
    end

    @testset "mig and mag" begin

        @test mig(@interval(-2,2)) == BigFloat(0.0)
        @test mig( Interval(1//2) ) == 1//2
        @test isnan(mig(emptyinterval()))
        @test mag(-b) == b.hi
        @test mag( Interval(1//2) ) == 1//2
        @test isnan(mag(emptyinterval()))
    end

    @testset "cancelplus tests" begin

        x = Interval(-2.0, 4.440892098500622e-16)
        y = Interval(-4.440892098500624e-16, 2.0)
        @test cancelminus(x, y) == entireinterval(Float64)
        @test cancelplus(x, y) == entireinterval(Float64)
        x = Interval(-big(1.0), eps(big(1.0))/4)
        y = Interval(-eps(big(1.0))/2, big(1.0))
        @test cancelminus(x, y) == entireinterval(BigFloat)
        @test cancelplus(x, y) == entireinterval(BigFloat)
        x = Interval(-big(1.0), eps(big(1.0))/2)
        y = Interval(-eps(big(1.0))/2, big(1.0))
        @test cancelminus(x, y) ⊆ Interval(-one(BigFloat), one(BigFloat))
        @test cancelplus(x, y) == Interval(zero(BigFloat), zero(BigFloat))
        @test cancelminus(emptyinterval(), emptyinterval()) == emptyinterval()
        @test cancelplus(emptyinterval(), emptyinterval()) == emptyinterval()
        @test cancelminus(emptyinterval(), Interval(0.0, 5.0)) == emptyinterval()
        @test cancelplus(emptyinterval(), Interval(0.0, 5.0)) == emptyinterval()
        @test cancelminus(entireinterval(), Interval(0.0, 5.0)) == entireinterval()
        @test cancelplus(entireinterval(), Interval(0.0, 5.0)) == entireinterval()
        @test cancelminus(Interval(5.0), Interval(-Inf, 0.0)) == entireinterval()
        @test cancelplus(Interval(5.0), Interval(-Inf, 0.0)) == entireinterval()
        @test cancelminus(Interval(0.0, 5.0), emptyinterval()) == entireinterval()
        @test cancelplus(Interval(0.0, 5.0), emptyinterval()) == entireinterval()
        @test cancelminus(Interval(0.0), Interval(0.0, 1.0)) == entireinterval()
        @test cancelplus(Interval(0.0), Interval(0.0, 1.0)) == entireinterval()
        @test cancelminus(Interval(0.0), Interval(1.0)) == Interval(-1.0)
        @test cancelplus(Interval(0.0), Interval(1.0)) == Interval(1.0)
        @test cancelminus(Interval(-5.0, 0.0), Interval(0.0, 5.0)) == Interval(-5.0)
        @test cancelplus(Interval(-5.0, 0.0), Interval(0.0, 5.0)) == Interval(0.0)
    end

    @testset "mid and radius" begin
        @test radius(Interval(-1//10,1//10)) == diam(Interval(-1//10,1//10))/2
        @test isnan(IntervalArithmetic.radius(emptyinterval()))
        @test mid(c) == 2.125
        @test isnan(mid(emptyinterval()))
        @test mid(entireinterval()) == 0.0
        @test isnan(mid(nai()))
        if VERSION < v"0.7.0-DEV"
            @test_throws ArgumentError nai(Interval(1//2))
        else
            @test_throws InexactError nai(Interval(1//2))
        end
    end

    @testset "abs, min, max, sign" begin

        @test abs(entireinterval()) == Interval(0.0, Inf)
        @test abs(emptyinterval()) == emptyinterval()
        @test abs(Interval(-3.0,1.0)) == Interval(0.0, 3.0)
        @test abs(Interval(-3.0,-1.0)) == Interval(1.0, 3.0)
        @test abs2(Interval(-3.0,1.0)) == Interval(0.0, 9.0)
        @test abs2(Interval(-3.0,-1.0)) == Interval(1.0, 9.0)
        @test min(entireinterval(), Interval(3.0,4.0)) == Interval(-Inf, 4.0)
        @test min(emptyinterval(), Interval(3.0,4.0)) == emptyinterval()
        @test min(Interval(-3.0,1.0), Interval(3.0,4.0)) == Interval(-3.0, 1.0)
        @test min(Interval(-3.0,-1.0), Interval(3.0,4.0)) == Interval(-3.0, -1.0)
        @test max(entireinterval(), Interval(3.0,4.0)) == Interval(3.0, Inf)
        @test max(emptyinterval(), Interval(3.0,4.0)) == emptyinterval()
        @test max(Interval(-3.0,1.0), Interval(3.0,4.0)) == Interval(3.0, 4.0)
        @test max(Interval(-3.0,-1.0), Interval(3.0,4.0)) == Interval(3.0, 4.0)
        @test sign(entireinterval()) == Interval(-1.0, 1.0)
        @test sign(emptyinterval()) == emptyinterval()
        @test sign(Interval(-3.0,1.0)) == Interval(-1.0, 1.0)
        @test sign(Interval(-3.0,-1.0)) == Interval(-1.0, -1.0)

        # Test putting functions in @interval:
        @test log(@interval(-2,5)) == @interval(-Inf,log(5.0))
        @test @interval(sin(0.1) + cos(0.2)) == sin(@interval(0.1)) + cos(@interval(0.2))

        f(x) = 2x
        @test @interval(f(0.1)) == f(@interval(0.1))

        # midpoint-radius representation
        a = @interval(0.1)
        midpoint, radius = midpoint_radius(a)

        @test interval_from_midpoint_radius(midpoint, radius) ==
            Interval(0.09999999999999999, 0.10000000000000002)
    end

    @testset "Precision tests" begin
        setprecision(Interval, 100)
        @test precision(Interval) == (BigFloat, 100)

        setprecision(Interval, Float64)
        @test precision(Interval) == (Float64, 100)

        a = @interval(0.1, 0.3)

        b = setprecision(Interval, 64) do
            @interval(0.1, 0.3)
        end

        @test b ⊆ a

        @test precision(Interval) == (Float64, 100)
    end

    # @testset "Interval rounding tests" begin
    #     # setrounding(Interval, :wide)
    #     @test rounding(Interval) == :wide
    #
    #     @test_throws ArgumentError # setrounding(Interval, :hello)
    #
    #     # setrounding(Interval, :narrow)
    #     @test rounding(Interval) == :narrow
    # end

    @testset "Interval power of an interval" begin

        setprecision(Interval, Float64)

        a = @interval(1, 2)
        b = @interval(3, 4)

        @test a^b == @interval(1, 16)
        @test a^@interval(0.5, 1) == a
        @test a^@interval(0.3, 0.5) == @interval(1, sqrt(2))

        @test b^@interval(0.3) == Interval(1.3903891703159093, 1.5157165665103982)
    end

    @testset "isatomic" begin
        @test isatomic(Interval(1))
        @test isatomic(Interval(2.3, 2.3))
        @test isatomic(emptyinterval())
        @test isatomic(@interval(∞))  # Interval(floatmax(), Inf)

        @test !isatomic(1..2)
        @test !isatomic(Interval(1, nextfloat(1.0, 2)))

    end

    @testset "iszero" begin
        @test iszero(Interval(0))
        @test iszero(Interval(0//1))
        @test iszero(Interval(big(0)))
        @test iszero(Interval(-0.0))
        @test iszero(Interval(-0.0, 0.0))

        @test !iszero(1..2)
        @test !iszero(Interval(0.0, nextfloat(0.0)))
    end

    @testset "Difference between Interval and interval" begin
        @test interval(1, 2) == Interval(1, 2)

        @test inf(Interval(3, 2)) == 3
        @test_logs (:warn,) @test isempty(interval(3, 2))

        @test sup(Interval(Inf, Inf)) == Inf
        @test_logs (:warn,) @test isempty(interval(Inf, Inf))

    end

    @testset "Type stability" begin
        for T in (Float32, Float64, BigFloat)

            xs = [3..4, 0..4, 0..0, -4..0, -4..4, -Inf..4, 4..Inf, -Inf..Inf]

            for x in xs
                for y in xs
                    xx = Interval{T}(x)
                    yy = Interval{T}(y)

                    for op in (+, -, *, /, atan)
                        @inferred op(x, y)
                    end
                end

                for op in (sin, cos, exp, log, tan, abs, mid, diam)
                    @inferred op(x)
                end
            end
        end
    end

end


using NonLinearReactionAdvectionDiffusionWithFrontData;

@testset "Различные вспомогательные функции                " begin

    N = 20;
    X = [i/N for i in 0:N];
    Y = u_init.(X);

    @testset "Initial condition" begin

        @test length(u_init.(X)) == N+1
        @test_broken isapprox(u_init(X[1]), -8, rtol=0.001)
        @test isapprox(u_init(X[end]), 4, rtol=0.001)
    end

    @testset "`strip_borderPoints`" begin
        B = copy(Y)
        B = NonLinearReactionAdvectionDiffusionWithFrontData.strip_borderPoints(B, N)
        @test length(B) == N-1
        @test B == Y[2:N]
        @test_throws AssertionError NonLinearReactionAdvectionDiffusionWithFrontData.strip_borderPoints(B, N)
    end

end

@testset "Формирование сетки                               " begin

    using NonLinearReactionAdvectionDiffusionWithFrontData: shishkin_mesh;

    e = 0.02;
    a, b = 0, 1;
    xtp = 0.3;
    N = 50;
    X = shishkin_mesh(a, b, xtp, e, N);

    @test sort(X) == X
    @test unique(X) == X
    @test length(X) == N + N + 2N + 1

    # Проверка упорядоченности a, x_tp, b
    @test_throws ArgumentError shishkin_mesh(b, a, 0.5, 0.1, N)
    @test_throws ArgumentError shishkin_mesh(a, b, 1.5, 0.1, N)
    @test_throws ArgumentError shishkin_mesh(a, b, -10, 0.1, N)

end


include("IOM/src/share/CyclicData.jl")

using .CyclicData
using CFTime
using NCDatasets
using Dates

test_file = "test.nc"
println("Making test data $(test_file)")

period = 2*365
t = (range(0, period, length=51) |> collect)[1:end-1]
t0 = 0.0
v = cos.( (t .- t0) / 365.0 * 2π) + t * 0.001

# v is of shape (x, y, time)
v = reshape(v, 1, 1, :)

Dataset(test_file, "c") do ds

    defDim(ds, "Nx", 1)
    defDim(ds, "Ny", 1)
    defDim(ds, "time", Inf)

    for (varname, vardata, vardim, attrib) in [
        ("v",     v, ("Nx", "Ny", "time",), Dict()),
        ("time",  t, ("time",), Dict(
            "calendar" => "noleap",
            "units"    => "days since 0000-01-01 00:00:00",
        )),
    ]
        println("Doing varname:", varname)
        var = defVar(ds, varname, Float64, vardim)
        var.attrib["_FillValue"] = 1e20

        for (k, v) in attrib
            var.attrib[k] = v
        end

        rng = []
        for i in 1:length(vardim)-1
            push!(rng, Colon())
        end
        push!(rng, 1:size(vardata)[end])
        var[rng...] = vardata

    end

end

println("Making CyclicDataManager")
_t = DateTimeNoLeap(3,1, 1)
cdm = CyclicDataManager(;
    timetype = DateTimeNoLeap,
    var_file_map = Dict("v" => test_file),
    beg_time = DateTimeNoLeap(0, 1, 1),
    end_time = DateTimeNoLeap(2, 1, 1),
    align_time = _t,
)

d = makeDataContainer(cdm)

println("Interpolating data")
dt = Second(86400)
interp_d = []
interp_t = []

for i = 1:365*5

    global _t

    interpData!(
        cdm,
        _t,
        d,
    )

    push!(interp_d, d["v"][1,1])
    push!(interp_t, _t)

    _t = _t + dt
end

interp_t_day = [ timeencode(_t, "days since 0001-01-01 00:00:00", DateTimeNoLeap) for (_, _t) in enumerate(interp_t) ]

println("Plotting")

using PyPlot
plt = PyPlot

plt.plot(interp_t_day, interp_d)
plt.scatter(t, v, s=20, marker="o")

for i=1:5
    plt.scatter(t .+ period*(i-1), v, s=2, marker="o", color="black")
end

plt.show()


import DataFrames.describe

export acceptance, describe

# Compute acceptance or rejection rate of MCMCChain
function acceptance(c::MCMCChain; lags::Ranges=1:nrow(c.samples), reject::Bool=false)
  rlen = lags.len
  @assert lags[end] <= nrow(c.samples) "Range of acceptance rate not within post-burnin range of MCMC chain"

  if reject
    return (rlen-sum(c.diagnostics["accept"][lags]))*100/rlen
  else
    return sum(c.diagnostics["accept"][lags])*100/rlen
  end
end

# TODO 1: Compute MCMC quantiles based on
# Flegal J.M, Galin L.J, Neath R.C. Markov Chain Monte Carlo Estimation of Quantiles. arXiv, 2013
# TODO 2: Include these MCMC estimates of quantiles in describe()

# describe() provides summary statistics for MCMCChain objects, similarly to dataframes' describe()
describe(c::MCMCChain) = describe(STDOUT, c)

function describe(io, c::MCMCChain)
  for i in 1:ncol(c.samples)

    col = c.samples[i]
    println(io, colnames(c.samples)[i])

    if all( isna(col) )
      println(col, " * All NA * ")
        return
    end

    filtered = float(removeNA(col))
    qs = quantile(filtered, [0, 1])
    nrows = nrow(c.samples)
    varimse = mcvar_imse(filtered)
    variid = var(filtered)/nrows
    mcerror = sqrt(varimse)
    ss = nrows*variid./varimse
    act = varimse./variid

    statNames = ["Min", "Mean", "Max", "MC Error", "ESS", "AC Time"]
    statVals = [qs[1], mean(filtered), qs[2], mcerror, ss, act]
    for i = 1:6
        println(io, string(rpad(statNames[i], 10, " "), " ", string(statVals[i])))
    end
    nas = sum(isna(col))
    println(io, "NAs        $nas")
    println(io, "NA%        $(round(nas*100/length(col), 2))%")

    println(io, )
  end
end


# [src/rkadapt.jl]

"""
Runge-Kutta adaptive-step integration using the specified weights, nodes,
and Runge-Kutta matrix (or Dormand-Prince 5(4) by default).

Implements numerical propagation of an ordinary differential equation
from some initial value over the desired range. This function is similar
to MATLAB's variable-step ODE propagators (e.g., ode45), but is
generalized to any Butcher tableau. Unlike ode45, however, there's no
inherent minimum number of steps that this function will take; if it can
finish integration with a single step, it will, whereas ode45 will always
take at least a certain number of steps, no matter how small the time
interval and how smooth the dynamics. Therefore, this function is
commonly more useful with odehybrid than ode45.

This function is generic for all adaptive-step Runge-Kutta methods. That
is, any adaptive-step Runge-Kutta propagator can be created by passing
the weightes, nodes, and Runge-Kutta matrix (together, the Butcher
tableau) into this function. It will correctly take advantage of the
first-same-as-last property, eliminating one function evaluation per
step when the table has this property. See the examples below.

    t, x = rk4adapt(ode, ts, x0);
    t, x = rk4adapt(ode, ts, x0, options);
    t, x = rk4adapt(ode, ts, x0, a, b, c p);
    t, x = rk4adapt(ode, ts, x0, options, a, b, c, p);

Inputs:

    ode     Ordinary differential equation function (s.t. ẋ = ode(t, x);)
    ts      Time span, [t_start, t_end]
    x0      Initial state (column vector)
    options Options structure from odeset. This function uses the
                InitialStep, MaxStep, OutputFcn, RelTol, and AbsTol fields only.
    a       Runge-Kutta matrix (s-by-s)
    b       Weights (2-by-s), the top row containing the weights for the
                state update
    c       Nodes (s-by-1)
    p       Lowest order method represented by b (e.g., for
                Runge-Kutta-Fehlberg, this is 4).

    (NOTE that if a/b/c/p are not specified, this defaults to Dormand-Prince)

Outputs:

    t       Time history
    x       State history, with each row containing the state corresponding to
                the time in the same row of t.

Example:

    # Simulate an undamped harmonic oscillator for 10s starting from an
    #    initial state of [1; 0] using the default a, b, c, and p
    #    (Dormand-Prince 5(4)):

    t, x = rkadapt((t,x) -> [-x[2]; x[1]], [0 10], [1; 0]);
    plot(t, x);

    # Add options.

    options = ODE_SET(MaxStep = 0.1, InitialStep = 0.05)
    t, x = rkadapt( (t, x) -> [-x[2]; x[1]], [0 10], [1; 0], options)
    plot(t, x);

    # Now simulate with Bogacki-Shampine 3(2).

    a = [  0    0    0    0;
           1/2  0    0    0;
           0    3/4  0    0;
           2/9  1/3  4/9  0];

    b = [ 2/9   1/3  4/9  0;       # For the update
          7/24  1/4  1/3  1/8];    # For the error prediction

    c = [ 0     1/2  3/4  1];

    p = 2;                         # The lower of the two orders

    t, x = rkadapt( (t,x) -> [-x[2]; x[1]], [0 10], [1; 0], a, b, c, p);
    plot(t, x);

See "Runge-Kutta methods" on Wikipedia for discussion of the Butcher
tableau (a, b, and c).

Reference:

Dormand, J. R., and P. J. Prince. "A family of embedded Ruge-Kutta
formulae." _Journal of Computational and Applied Mathematics_ 6.1 (1980):
19-26. Web. 22 Apr. 2014.

See also: odehybrid, ode45, odeset, rkfixed.

Online doc: http://www.anuncommonlab.com/doc/odehybrid/rkadapt.html

Copyright 2014 An Uncommon Lab
"""
function rkadapt(ode, ts, x0, options::ODE_SET, a, b, c, p)

    # Unpack what we care about from the options.
    rel_tol = (options.RelTol == []) ? 1e-3 : options.RelTol
    abs_tol = (options.AbsTol == []) ? 1e-6 : options.AbsTol
    dt_max  = (options.MaxStep == []) ? Inf : options.MaxStep
    dt      = (options.InitialStep == []) ? min(1e-3, dt_max) : min(options.InitialStep, dt_max)

    # See if this table has the first-same-as-last property.
    fsal = all(a[end, :] == b[1, :]);

    ex = 1/(p+1);     # Time step correction exponent
    dt_min = 100 * eps(1.0 * maximum(abs.(ts))); # Minimum time step

    t   = ts[1];       # Time history
    x   = x0[:];       # State history
    tk  = ts[1];       # Current time
    xk  = x0[:];       # Current state
    xkp = xk;          # Proposed state update

    nx = length(x0);    # Number of states
    s  = size(b, 2);    # Length of weights
    d  = zeros(nx, s);  # Matrix of derivatives
    db = b[2, :] - b[1, :];

    # If the user provided an OutputFcn, call it.
    if (options.OutputFcn != [])
        options.OutputFcn(ts, x0, "init");
    end

    # If using a table with first-same-as-last, start the derivative here.
    if fsal
        d[:, 1] = ode(tk, xk);
    end

    # Propagate, updating k to k+1 until we're done until t(k) == ts(2).
    stop = false;
    while tk < (ts[2] - dt_min)

        # Don't step past the end time.
        if tk + dt > ts[2]
            dt = ts[2] - tk;
        end

        # Evaluate the first bit of the derivatives.
        if ~fsal
            d[:, 1] = ode(tk, xk);
        end

        tkp, xkp = [], []  # For scope

        # Try to take a step.
        while true

            # Calculate derivatives.
            for z = 2:s
                dxk = (d[:, 1:z-1])* (dt * a[z, 1:z-1])
                d[:, z] = ode(tk + c[z] * dt, xk + dxk);
            end

            # Update the state and error estimate.
            tkp = tk + dt;
            xkp = xk;
            er  = zeros(nx, 1);
            for z = 1:s
                xkp = xkp + ((dt * b[1, z]) .* d[:, z]);
                er  = er[:, 1] + ((dt * db[z]) .* d[:, z]);
            end

            # Create the scaled error. This single number can now be compared to 1 to see
            # if the worst error satisfies both absolute and relative tolerances. The error is acceptable if
            # it is either less than the relative error or less than the aboslute error.
            scaled_error = maximum(abs.(er) ./ (abs_tol .+ rel_tol .* abs.(xkp)));

            # If the error is small, update the time step and break to the
            # next step.
            if scaled_error <= 1
                dt = min(0.9 * dt * scaled_error^(-ex), min(3.0*dt, dt_max));
                # NOTE - ^This line has a SLIGHTLY different value than the MATLAB equivalent
                break;

            # If the error is too large, update the time step and continue
            # the search for an appropriate time step.
            else

                # If that was already the smallest step we can take, give up.
                if dt == dt_min
                    @warn "Could not meet integration tolerance."
                    break;
                end

                # Cut the time step down faster than we build it up.
                dt = max(0.7 * dt * scaled_error^(-ex), dt_min);
                     # NOTE - Julia and MATLAB slightly differ on last digit
            end

        end

        # Accept the update.
        tk = tkp;
        xk = xkp;

        # For first-same-as-last, we can copy over the derivative at k.
        if fsal
            d[:, 1] = d[:, end];
        end

        # Store.
        t = [t; tk];
        x = [x xk[:]];

        # If the user provided an OutputFcn, call it.
        if (options.OutputFcn != [])
            if (options.OutputFcn(tk, xk, ""))
                break
            end
        end
    end

    # Rectify the ending.
    if ~stop
        t[end] = ts[2];
    end

    # If the user provided an OutputFcn, call it.

    if (options.OutputFcn != [])
        options.OutputFcn([], [], "done")
    end

    return t, x'
end

function dptable()
    """
        Returns default Dormand-Prince Butcher Tableau values
    """

    a = [0           0          0           0        0          0     0;
         1/5         0          0           0        0          0     0;
         3/40        9/40       0           0        0          0     0;
         44/45      -56/15      32/9        0        0          0     0;
         19372/6561 -25360/2187 64448/6561 -212/729  0          0     0;
         9017/3168  -355/33     46732/5247  49/176  -5103/18656 0     0;
         35/384      0          500/1113    125/192 -2187/6784  11/84 0];

    b = [35/384     0  500/1113   125/192  -2187/6784    11/84    0;
         5179/57600 0  7571/16695 393/640  -92097/339200 187/2100 1/40];

    c = [0 1/5 3/10 4/5 8/9 1 1];

    p = 4;

    return a, b, c, p
end

function rkadapt(ode, ts, x0)
    """
        Wrapper function that generates default ODE_SET values and uses the
            Dormand-Prince Butcher Tableau
    """
    options = ODE_SET();
    a, b, c, p = dptable();

    return rkadapt(ode, ts, x0, options, a, b, c, p)
end

function rkadapt(ode, ts, x0, options::ODE_SET)
    """
        Wrapper function that uses the Dormand-Prince Butcher Tableau
    """

    a, b, c, p = dptable();
    return rkadapt(ode, ts, x0, options, a, b, c, p)
end

function rkadapt(ode, ts, x0, a, b, c, p)
    """
        Wrapper function that generates default ODE_SET values
    """
    options = ODE_SET();
    return rkadapt(ode, ts, x0, options, a, b, c, p)
end


module WaveGPU

# using CairoMakie
# using GLMakie
using ArrayFire: AFArray, allowslow, swap!
using DDF
using DiffEqCallbacks
using DifferentialForms
using LinearAlgebra
using Logging: global_logger
using OrdinaryDiffEq
using Printf
using ProgressLogging
using SparseArrays
using StaticArrays
using TerminalLoggers: TerminalLogger

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

walltime() = time_ns() / 1.0e+9

function Base.copyto!(y::AFArray, x::AFArray)
    @assert length(y) == length(x)
    z = copy(x)
    swap!(y, z)
    finalize(z)
    return y
end
function LinearAlgebra.mul!(y::AFArray, A::AFArray, x::AFArray)
    z = A * x
    swap!(y, z)
    finalize(z)
    return y
end

global_logger(TerminalLogger())

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

function wave(::Val{D}, levels::Int; gpu::Bool=false) where {D}
    D::Int
    S = Float32 #Float64
    T = Float32 #Float64

    println("Create manifold...")

    n = 2^levels
    nelts = Tuple(n for d in 1:D)
    mfd = large_hypercube_manifold(Val(D), S; nelts=nelts, optimize_mesh=false)

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

    println("Sample initial conditions...")

    # TODO: use first-order formulation

    k = SVector{D,S}(1 for d in 1:D)
    ω = norm(k)
    function u⁼(t, x)
        local u = cospi(ω * t) * prod(sinpi(k[d] * x[d]) for d in 1:D)
        return Form{D,0}((u,))
    end
    function ∂ₜu⁼(t, x)
        local ∂ₜu = -S(π) *
                    ω *
                    sinpi(ω * t) *
                    prod(sinpi(k[d] * x[d]) for d in 1:D)
        return Form{D,0}((∂ₜu,))
    end

    t₀ = S(0)
    u₀ = sample(Fun{D,Pr,0,D,S,T}, x -> u⁼(t₀, x), mfd)
    ∂ₜu₀ = sample(Fun{D,Pr,0,D,S,T}, x -> ∂ₜu⁼(t₀, x), mfd)

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

    println("Set up equations of motion...")

    # ∂ₜₜu = Δu
    # u̇ := ∂ₜu

    # ∂ₜu = u̇
    # ∂ₜu̇ = Δu

    combine(u, u̇) = [u.values.vec; u̇.values.vec]
    function split(U)
        local n = length(u₀)
        @assert length(U) == 2n
        local uv = U[1:n]
        local u̇v = U[(n + 1):(2n)]
        return (Fun{D,Pr,0,D,S,T}(mfd, IDVector{0}(uv)),
                Fun{D,Pr,0,D,S,T}(mfd, IDVector{0}(u̇v)))
    end

    U₀ = combine(u₀, ∂ₜu₀)

    N = zero(Op{D,Pr,0,Pr,0}, mfd)
    E = one(Op{D,Pr,0,Pr,0}, mfd)
    B = isboundary(Val(Pr), Val(0), mfd)
    L = laplace(Val(Pr), Val(0), mfd)

    function f(U, p, t)
        local u, u̇ = split(U)
        local ∂ₜu = u̇
        local ∂ₜu̇ = L * u
        # local ∂ₜuₜ = sample(Fun{D,Pr,0,D,S,T}, x -> ∂ₜu⁼(t, x), mfd)
        # local ∂ₜₜuₜ = sample(Fun{D,Pr,0,D,S,T}, x -> ∂ₜₜu⁼(t, x), mfd)
        ∂ₜu = (E - B) * ∂ₜu # + B * ∂ₜuₜ
        ∂ₜu̇ = (E - B) * ∂ₜu̇ # + B * ∂ₜₜuₜ
        return combine(∂ₜu, ∂ₜu̇)
    end

    F = [N.values sparse(E.values); L.values N.values]
    F = [(E - B).values N.values; N.values (E - B).values] * F
    f!(∂ₜU, U, p, t) = mul!(∂ₜU, F, U)

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

    println("Set up GPU...")

    if gpu
        allowslow(AFArray, false)
        # copy data to GPU
        U₀ = AFArray(U₀)
        F = AFArray(F)
    end

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

    println("Solve...")

    Δx = minimum(get_volumes(mfd, 1))
    # @show Δx

    t₁ = S(1)

    # TODO: Experiment with linear ODE, Hamiltonian systems

    # TODO: Don't use `saveat`, use a `SavingCallback` instead

    # prob = ODEProblem(f, U₀, (t₀, t₁); saveat=(t₁ - t₀) / 8, save_end=false)
    prob = ODEProblem(f!, U₀, (t₀, t₁); saveat=(t₁ - t₀) / 8, save_end=false)

    dt = Δx / 2

    sol = nothing
    nextoutput = walltime()
    @withprogress name = "ODE" begin
        function progress(U, t, integrator)
            if t == t₁ || walltime() ≥ nextoutput
                iter = integrator.iter
                time = integrator.t
                nsol = norm(integrator.u, Inf)
                @printf "    iter=%d   time=%#.6g   ‖sol‖∞=%#.6g\n" iter time nsol
                @logprogress (t - t₀) / (t₁ - t₀)
                nextoutput = walltime() + 1
            end
        end

        sol = solve(prob, Tsit5(); adaptive=false, dt=dt, alias_u0=true,
                    internalnorm=(u, t) -> S(0),
                    # unstable_check=(dt,u,p,t)->false,
                    callback=FunctionCallingCallback(progress))
    end

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

    println("Analyse solution...")

    # @progress name = "Plotting" for i in axes(sol.t, 1)
    #     # t = (n - i) / S(n) * t₀ + i / S(n) * t₁
    #     # U = sol(t)
    #     t = sol.t[i]
    #     U = sol[i]
    #     u, ∂ₜu = split(U)
    #     uₜ = sample(Fun{D,Pr,0,D,S,T}, x -> u⁼(t, x), mfd)
    #     e = u - uₜ
    #     outdir = "wave$(D)d-$levels"
    #     mkpath(outdir)
    #     plot_function(u, "$outdir/wave$(D)d-u.$(@sprintf "%04d" i - 1).png")
    #     plot_function(e, "$outdir/wave$(D)d-e.$(@sprintf "%04d" i - 1).png")
    # end

    i = last(axes(sol.t, 1))
    t = sol.t[i]
    U = sol[i]
    if gpu
        # copy data from GPU
        U = Array(U)
    end
    u, ∂ₜu = split(U)
    uₜ = sample(Fun{D,Pr,0,D,S,T}, x -> u⁼(t, x), mfd)
    e = u - uₜ
    ne = sqrt(norm(e)^2 / length(e))
    println("    ‖u-u⁼‖₂=$ne")

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

    println("Done.")

    return nothing
end

# D = 2:
#     levels    error ‖u-u⁼‖₂
#     1         0.02805463824876254
#     2         0.0020292411087007196
#     3         0.00013532326532406398
#     4         8.727862238311897e-6
#     5         5.540717968813277e-7
#     6         3.49003314447833e-8
#     7         2.189781209413498e-9
#     8         1.3712800517004272e-10
#     9         8.580900735370499e-12
#     10        5.369060952547811e-13
#     11        3.1564728009539904e-14
#     12        6.75775317464105e-15
#     13        1.5417520244384893e-14

end


# --------------------------------------------------------- #
# Stuff to construct basis matrices of complete polynomials #
# --------------------------------------------------------- #

using Base.Cartesian: @nloops, @nexprs

"""
Construct basis matrix for complete polynomial of degree `d`, given
input data `z`. `z` is assumed to be the degree 1 realization of each
variable. For example, if variables are `q`, `r`, and `s`, then `z`
should be `z = [q r s]`

Output is a basis matrix. In our example, with `d` set to 2 we would have

TODO: update docstring to properly give order of terms

```julia
out = [ones(size(z,1)) q r s q.*r q.*s r.*s q.^2 q.*r q.*s r.^2 r.*s s.^2]
```
"""
:complete_polynomial

immutable Degree{N} end

function n_complete(n::Int, D::Int)
    out = 1
    for d=1:D
        tmp = 1
        for j=0:d-1
            tmp *= (n+j)
        end
        out += div(tmp, factorial(d))
    end
    out
end

@generated function complete_polynomial!{N}(z::Matrix, d::Degree{N},
                                            out::Matrix)
    complete_polynomial_impl!(z, d, out)
end

function complete_polynomial_impl!{T,N}(z::Type{Matrix{T}}, ::Type{Degree{N}},
                                        out::Type{Matrix{T}})
    quote
        nobs, nvar = size(z)
        if size(out) != (nobs, n_complete(nvar, $N))
            error("z, out not compatible")
        end

        # reset first column to ones
        @inbounds for i=1:nobs
            out[i, 1] = one($T)
        end

        # set next nvar columns to input matrix
        @inbounds for n=2:nvar+1, i=1:nobs
            out[i, n] = z[i, n-1]
        end

        ix = nvar+1
        @nloops($N, # number of loops
                i,  # counter
                d->((d == $N ? 1 : i_{d+1}) : nvar),  # ranges
                d->(d == $N ? nothing :
                    (begin
                        ix += 1
                        @inbounds @simd for r=1:nobs
                            tmp = one($T)
                            @nexprs $N-d+1 j->(tmp *= z[r, i_{$N-j+1}])
                            out[r, ix]=tmp
                        end
                    end)),  # preexpr
                Expr(:block, :nothing)  # bodyexpr
                )
        out
    end
end

function complete_polynomial{T}(z::Matrix{T}, d::Int)
    nobs, nvar = size(z)
    out = Array(T, nobs, n_complete(nvar, d))
    complete_polynomial!(z, Degree{d}(), out)::Matrix{T}
end

function complete_polynomial!{T}(z::Matrix{T}, d::Int, out::Matrix{T})
    complete_polynomial!(z, Degree{d}(), out)::Matrix{T}
end


module Tests
using AverageShiftedHistograms, StatsBase, Base.Test, Distributions

info("Messy Output")
show(ash(randn(1000)))
println("\n\n")
info("Begin Tests")

@testset "Kernels" begin
    for kernel in [Kernels.biweight,
                   Kernels.cosine,
                   Kernels.epanechnikov,
                   Kernels.triangular,
                   Kernels.tricube,
                   Kernels.triweight,
                   Kernels.uniform,
                   Kernels.gaussian,
                   Kernels.logistic]
        for u in [0.0, 0.00001, 0.0001, 0.001, 0.01, 0.1, 1.0, 10.0]
            @test kernel(-u) ≈ kernel(u)
        end
    end
end

@testset "Ash" begin
    y = randn(10_000)
    o = ash(y, -4:.1:4; warnout = false)
    @test pdf(o, 0.0) > 0
    @test pdf(o, -10) ≈ 0.0
    @test cdf(o, -10) ≈ 0.0
    @test cdf(o, 10) ≈ 1.0

    for f in [mean, var, std, x -> quantile(x, .4), x -> quantile(x, .4:.1:.6)]
        @test_approx_eq_eps f(o) f(y) .1
    end
    AverageShiftedHistograms.histdensity(o)
    @test quantile(o, 1e-20) ≈ o.rng[1]

    # check that histogram is correct
    h = fit(Histogram, y, (-4:.1:4.1)-.05)
    @test h.weights == o.v

    fit!(o, y; warnout = false)
    @test nobs(o) == 20_000

    o = ash([.1, .1], -1:.1:1)
    @test nout(o) == 0
end

@testset "MVAsh" begin
    x = randn(1000)
    y = x + randn(1000)
    o = ash(x, y)
    show(o)

    mean(o)
    var(o)
    std(o)
    fit!(o, x, y; mx = 3)
    xyz(o)
end

end #module


using ArgParse

s = ArgParseSettings()
@add_arg_table s begin
    "--part2"
    help = "Do part 2"
    action = :store_true
    "--test"
    help = "Run test data"
    action = :store_true
end

parsed_args = parse_args(ARGS, s)
test_suffix = parsed_args["test"] ? "_test" : ""
function parseline(l)
    parts = split(l,": ")
    parse(UInt, parts[2])
end

positions = [parseline(l) for l in readlines("input/day_21$test_suffix.txt")]
scores = [0,0]

mutable struct DeterministicDie
    rolls::Int
    value::Int
end

DeterministicDie() = DeterministicDie(0,100)

function roll!(d::DeterministicDie)
    d.rolls += 1
    d.value += 1
    if d.value > 100
        d.value = 1
    end
    return d.value
end

# Maps a total score from 3 rolls of the Dirac die to the corresponding relative frequency
frequencies = [ (r=3, f=1), (r=4, f=3), (r=5, f=6), (r=6, f=7), (r=7, f=6), (r=8, f=3), (r=9,f=1) ]

function part1()
    d = DeterministicDie()
    function take_turn(player)
        roll = rem(roll!(d) + roll!(d) + roll!(d), 10)
        pos = positions[player] + roll
        if pos > 10
            pos -= 10
        end
        positions[player] = pos
        scores[player] += pos
        scores[player] >=1000
    end
    while true
        if take_turn(1) || take_turn(2)
            break;
        end
    end
    losing_score = min(scores[1], scores[2])
    losing_score * d.rolls
end

function expected_scores()
    function expected_scores_from(pos)
        scores = []
        for roll in frequencies
            landing = (roll.r + pos)
            if landing > 10
                landing -= 10
            end
            push!(scores, (pos=landing, freq=roll.f))
        end
        scores
    end
    map(expected_scores_from, 1:10)
end

function add_score_pos!(state, pos, score, num)
    posuni = get(state, pos, Dict())
    posuni[score] = get(posuni, score, BigInt(0)) + num
    state[pos] = posuni
end

function add_to_field(t, field, number)
    f1 = t[1]
    f2 = t[2]
    if field == 1
        f1 += number
    else
        f2 += number
    end
    (f1, f2)
end

function set_field(t, field, number)
    f1 = t[1]
    f2 = t[2]
    if field == 1
        f1 = number
    else
        f2 = number
    end
    (f1, f2)
end

function part2()
    pos_to_outcomes = expected_scores()

    function play(state, player)
        ns = Dict()
        wins::BigInt = 0
        remaining_universes::BigInt = 0
        for (positions, posuni) in state
            for outcome in pos_to_outcomes[positions[player]]
                for (score, ucount) in posuni
                    new_score = add_to_field(score, player, outcome.pos)
                    num_uni = ucount * outcome.freq
                    if new_score[1] >= 21 || new_score[2] >= 21
                        wins += num_uni
                    else
                        remaining_universes += num_uni
                        new_pos = set_field(positions, player, outcome.pos)
                        add_score_pos!(ns, new_pos, new_score, num_uni)
                    end
                end
            end
        end
        (wins, remaining_universes, ns)
    end

    # Dictionary maps possible scores at this stage to a map from
    # positions and the # of universes where the user has that score in that position
    state = Dict{Tuple{Int, Int}, Dict{Tuple{Int, Int}, BigInt}}(
                    (positions[1], positions[2]) => Dict((0,0) => BigInt(1)))

    wins1::BigInt = 0
    wins2::BigInt = 0
    while true
        (w1, remains, state) = play(state, 1)
        # println("P1: wins $w1, remaining $remains")
        wins1 += w1
        if remains == 0
            break;
        end
        (w2, remains, state) = play(state, 2)
        wins2 += w2
        # println("P2: wins $w2, remaining $remains")
        if remains == 0
            break;
        end
    end
    max(wins1, wins2)
end
p = parsed_args["part2"] ? part2 : part1
answer = p()
println("The answer is: $answer")


using SparseDiffTools
using LightGraphs
using LightGraphs: SimpleGraph
using Test

using Random
Random.seed!(100)

# println("Starting acyclic coloring test...")
#= Test data =#
test_graphs = Vector{SimpleGraph}(undef, 0)
test_graphs_dir = Vector{SimpleDiGraph}(undef, 0)

for _ in 1:5
    nv = rand(5:20)
    ne = rand(1:100)
    graph = SimpleGraph(nv)
    for e in 1:ne
        v1 = rand(1:nv)
        v2 = rand(1:nv)
        while v1 == v2
            v2 = rand(1:nv)
        end
        add_edge!(graph, v1, v2)
    end
    push!(test_graphs, copy(graph))
end

#=
 Coloring needs to satisfy two conditions:

1. every pair of adjacent vertices receives distinct colors
(a distance-1 coloring)

2. For any vertex v, any color that leads to a two-colored path
involving v and three other vertices  is  impermissible  for  v.
In other words, every path on four vertices uses at least three
colors.
=#

#Sample graph from Gebremedhin AH, Manne F, Pothen A. **New Acyclic and Star Coloring Algorithms with Application to Computing Hessians**

#=

(1) ----- (2) ----- (3) ---- (4)
 | \      /          |       |
 |   \   /           |       |
 |     \/            |       |
 (5)---(6) -------- (7) ----(8)
 |      |         /  |       |
 |      |     (11)   |       |
 |      |    /       |       |
 (9) ---(10) ----- (12)-----(13)
  \                 /
   \              /
     ------------
=#

gx = SimpleGraph(13)

add_edge!(gx,1,2)
add_edge!(gx,1,5)
add_edge!(gx,1,6)
add_edge!(gx,2,3)
add_edge!(gx,2,6)
add_edge!(gx,3,4)
add_edge!(gx,3,7)
add_edge!(gx,4,8)
add_edge!(gx,5,6)
add_edge!(gx,5,9)
add_edge!(gx,6,7)
add_edge!(gx,6,10)
add_edge!(gx,7,8)
add_edge!(gx,7,11)
add_edge!(gx,7,12)
add_edge!(gx,8,13)
add_edge!(gx,9,10)
add_edge!(gx,9,12)
add_edge!(gx,10,11)
add_edge!(gx,10,12)
add_edge!(gx,12,13)

push!(test_graphs, gx)

#create directed copies
for g in test_graphs
    dg = SimpleDiGraph(nv(g))
    for e in edges(g)
        src1 = src(e)
        dst1 = dst(e)
        add_edge!(dg, src1, dst1)
        add_edge!(dg, dst1, src1)
    end
    push!(test_graphs_dir, dg)
end

for i in 1:5
    g = test_graphs[i]
    dg = test_graphs_dir[i]

    out_colors = SparseDiffTools.color_graph(g, SparseDiffTools.AcyclicColoring())

    #test condition 1
    for v in vertices(g)
        color = out_colors[v]
        for j in inneighbors(g, v)
            @test out_colors[j] != color
        end
    end
end

for i in 3:4
    g = test_graphs[i]
    dg = test_graphs_dir[i]

    out_colors = SparseDiffTools.color_graph(g, SparseDiffTools.AcyclicColoring())

    #test condition 2
    cycles = simplecycles(dg)
    for c in cycles
        colors = zeros(Int, 0)
        if length(c) > 2
            for v in c
                push!(colors, out_colors[v])
            end
            @test length(unique(colors)) >= 3
        end
    end
    # println("finished testing graph $i")
end

# println("finished testing...")


using GLPlot, GLVisualize, GLAbstraction, Colors, GeometryTypes, FileIO
using Reactive, GLWindow, NIfTI
const If0 = GLVisualize.Intensity{1, Float32}
window = GLPlot.init()

layout = [
    ["Volume Plot", "Slice X"],
    ["Slice Y", "Slice Z"]
]

screens = GLVisualize.layoutscreens(window, layout)
function volume_data(N)
    vol = Float32[sin(x/15.0) + sin(y/15.0) + sin(z/15.0) for x=1:N, y=1:N, z=1:N]
    max = maximum(vol); min     = minimum(vol)
    vol = (vol .- min) ./ (max .- min)
end

vol = volume_data(128)

# to change color_map use the interactive edit menu or pass an array of color
# via color_map = Vector{Colorant}
p1 = glplot(vol, screen = screens[2][1])
slice_screens = vcat(screens[2][2], screens[1]...)
for i = 1:3
    range_s = play_widget(1:size(vol, i))
    slice = map(range_s) do slice_idx
        idx = ntuple(d-> d == i ? slice_idx : (:), Val{3})
        # This conversion is necessary but will be automatic soon!
        If0.(view(vol, idx...))
    end
    plot = glplot(
        slice, screen = slice_screens[i],
        color_norm = Vec2f0(0, 1),
        camera = :orthographic_pixel
    )
    center!(slice_screens[i], :orthographic_pixel)
end


struct ShrikeIndex{T <: AbstractFloat}
    data::Array{T, 2}
    npoints::Int
    ndims::Int
    depth::Int
    ntrees::Int
    random_vectors::Array{T, 2}
    splits::Array{T, 2}
    indexes::Array{Array{Int,1}, 2}
end

"""
    ShrikeIndex(data::Array{T, 2}, depth::Int, ntrees::Int) where T -> ensemble

Constructor for ensemble of sparse random projection trees with voting. Returns `ShrikeIndex` type.
(An ensemble of multiple random projection trees.)

** Type Fields**
* `data::Array{T, 2}`: Contains all data points
* `npoints::Int`: Number of data points
* `ndims::Int`: Dimensionality of the data
* `depth::Int`: maximum depth of the tree. (Depth of 0 means only a root, depth of 1 means root has two children)
* `ntrees::Int`: Number of trees to make
* `random_vectors::AbstractArray`: The random projections used to make the tree
* `splits::Array{T, 2}`: The split values for each node in each tree stored in a 2D array
* `indexes::Array{Array{Int,1}, 2}`: 2D array of datapoint indexes at each leaf node in each tree.
Note that RP forest does not store indexes at non-leaf nodes.

Follows the implementation outlined in:

>**Fast Nearest Neighbor Search through Sparse Random Projections and Voting.**
>Ville Hyvönen, Teemu Pitkänen, Sotirios Tasoulis, Elias Jääsaari, Risto Tuomainen,
>Liang Wang, Jukka Ilmari Corander, Teemu Roos. Proceedings of the 2016 IEEE
>Conference on Big Data (2016)

with some modifications.
"""
function ShrikeIndex(data::AbstractArray{T, 2}, depth::Int, ntrees::Int) where T
    ndims, npoints = size(data)
    maxdepth = floor(Int, log(2.0, npoints) - log(2.0, 1))
    if depth > maxdepth
        depth = maxdepth
    end
    sparse_vecs::Bool = ndims > 500
    random_vectors = random_projections(T, depth*ntrees, ndims, sparse = sparse_vecs)
    nsplits = 2^depth -1 # Number of splits in the tree (Same as number of non-leaf nodes)
    splits = zeros(T, ntrees, nsplits) # Array for storing hyperplane split values
    projections = transpose(data) * random_vectors  # projection[i, j] = the projection of point i onto the jth random vector
    nleafs = 2^depth
    leaf_node_data_idxs = Array{Array{Int, 1}, 2}(undef, nleafs, ntrees) # Array for storing data indexes at the leaf

    for t in 1:ntrees
        prev_level = [collect(1:npoints)]
        next_level = Array{Array{Int, 1}, 2}(undef, 2, 1)
        next_split_idx = 1
        for d in 1:depth
            proj_row = (t-1)*depth + d # Current row of projection values
            for i in 1:length(prev_level)
                idxs = prev_level[i]
                # Compute median value of the projection of all current datapoints
                p = reshape(projections[idxs, proj_row], :)
                med::T = median(p) # I think this requires a sort, so this process can be sped up slightly
                splits[t, next_split_idx] = med
                next_split_idx += 1
                # Separate data points across hyperplane using the median
                mask = p .< med
                # Send points with the projection less than the median to the left child
                next_level[2*i - 1] = idxs[mask]
                # Send points with the projection greater than the median to the right child
                next_level[2*i] = idxs[.!mask]

            end
            prev_level = next_level
            next_level = Array{Array{Int, 1}, 2}(undef, 2^(d+1), 1)
        end
        leaf_node_data_idxs[:, t] = prev_level
    end

    return ShrikeIndex(data, npoints, ndims, depth, ntrees, random_vectors, splits, leaf_node_data_idxs)
end

ShrikeIndex(data::AbstractArray{T, 2}; depth::Int=4, ntrees::Int=5) where T = ShrikeIndex(data, depth, ntrees)

"""
    ShrikeIndex(data::AbstractArray{T, 2}, max)k; depth::Union{Int, Float64}=Inf, ntrees::Int=5) -> shi

Keyword argument version of the constructor that includes intended number of nearest neighbors.

If the default `depth` is used, the constructor sets the tree depth as deep as
possible given `max_k`. This way, the accuracy/memory tradeoff is determined directly by `ntrees` and the
desired `vote_cutoff` (`vote_cutoff` is a parameter passed to `ann` or `knngraph`).

If an argument is passed for `depth`, constructor attempts to use the supplied `depth`, but guarentees
that the depth of the tree is shallow enough to ensure that each leaf has at least k points.
(Without this check, the index may return less than k neighbors when queried.)

**Parameters**

1. `data`: A (dxn) array. Each column is a datapoint with dimension `d`.
2. `max_k`: The maximum number of neighbors that will be queried. If intend
to use the `ShrikeIndex` to approximate at most 10 nearest neigbors of a point,
set `max_k = 10`. This argument is used to infer the deepest tree depth possible
so as to maximize speed,

**Keyword Arguments**

3. `ntrees`: The number of trees in the index. More trees means more accuracy,
more memory and less speed. Use this to tune the speed/accuracy tradeoff.
4. `depth`: The number of splits in the tree. Depth of 0 means only a root,
depth of 1 means root has two children, etc..
"""
function ShrikeIndex(data::AbstractArray{T, 2}, maxk::Int; depth::Union{Int, Float64}=Inf, ntrees::Int=10) where T
    m, n = size(data)
    maxdepth = floor(Int, log(2, n) - log(2, maxk))
    if depth > maxdepth
        safedepth = maxdepth
    else
        safedepth = depth
    end
    return ShrikeIndex(data, safedepth, ntrees)
end

function Base.show(io::IO, shi::ShrikeIndex)
    descr = "ShrikeIndex: \n    $(shi.ntrees) trees \n    $(shi.npoints) datapoints \n    Depth $(shi.depth)"
    print(io, descr)
end

function random_projections(T::Type, nvecs::Int, ndims::Int; sparse::Bool=true)
    # Sparse arrays are not implemented because sparse x dense matmul
    # was slower than dense x dense matmul when benchmarked.
    # Additionally, I'm not sure how to use sparse arrays and still
    # be type stable.
    # a::Float64 = 1. / √ ndims
    # if sparse
    #     R = sprand(ndims, nvecs, a)
    # else
        R = randn(T, ndims, nvecs)
        R ./= sum(R.^2, dims=2) .^ 0.5
    # end
    return R::Array{T, 2}
end


using NLSolvers
NLE_PROBS = Dict()

# Rosenbrock
# Source: MINPACK
NLE_PROBS["rosenbrock"] = Dict()
NLE_PROBS["rosenbrock"]["array"] = Dict()
function F_rosenbrock!(Fx, x)
    Fx[1] = 1 - x[1]
    Fx[2] = 10(x[2] - x[1]^2)
    return Fx
end
function J_rosenbrock!(Jx, x)
    Jx[1, 1] = -1
    Jx[1, 2] = 0
    Jx[2, 1] = -20 * x[1]
    Jx[2, 2] = 10
    return Jx
end
function FJ_rosenbrock!(Fx, Jx, x)
    F_rosenbrock!(Fx, x)
    J_rosenbrock!(Jx, x)
    Fx, Jx
end
function Jvop_rosenbrock!(x)
    function JacV(Fv, v)
        Fv[1] = -1 * v[1]
        Fv[2,] = -20 * x[1] * v[1] + 10 * v[2]
    end
    LinearMap(JacV, length(x))
end

NLE_PROBS["rosenbrock"]["array"]["x0"] = [-1.2, 1.0]
NLE_PROBS["rosenbrock"]["array"]["mutating"] = NLSolvers.VectorObjective(
    F_rosenbrock!,
    J_rosenbrock!,
    FJ_rosenbrock!,
    Jvop_rosenbrock!,
)

function F_powell_singular!(x::Vector, Fx::Vector, Jx::Union{Nothing,Matrix} = nothing)
    if !(Fx isa Nothing)
        Fx[1] = x[1] + 10x[2]
        Fx[2] = sqrt(5) * (x[3] - x[4])
        Fx[3] = (x[2] - 2x[3])^2
        Fx[4] = sqrt(10) * (x[1] - x[4])^2
    end
    if !(Jx isa Nothing)
        fill!(Jx, 0)
        Jx[1, 1] = 1
        Jx[1, 2] = 10
        Jx[2, 3] = sqrt(5)
        Jx[2, 4] = -Jx[2, 3]
        Jx[3, 2] = 2(x[2] - 2x[3])
        Jx[3, 3] = -2Jx[3, 2]
        Jx[4, 1] = 2sqrt(10) * (x[1] - x[4])
        Jx[4, 4] = -Jx[4, 1]
    end
    objective_return(Fx, Jx)
end

function Jvop_powell_singular(x)
    function JacV(Fv, v)
        Fv[1] = v[1] + 10 * v[2]
        Fv[2] = (v[3] - v[4]) * sqrt(5)
        xx23 = 2 * x[2] - 4 * x[3]
        Fv[3] = v[2] * xx23 - v[3] * xx23 * 2
        xx41 = 2 * sqrt(10) * (x[1] - x[4])
        Fv[4] = v[1] * xx41 - v[4] * xx41
    end
    LinearMap(JacV, length(x))
end

#=
NLE_PROBS["quantile"] = Dict()
NLE_PROBS["quantile"]["number"] = Dict()
@inline quantile_f(Fx, x) = log(max(x, 0.000001))
@inline quantile_j(Jx, x) = 1.0/x
@inline function quantile_fj(Fx, Jx, x)
    Fx = quantile_f(Fx, x)
    Jx = quantile_j(Jx, x)
    Fx, Jx
end
NLE_PROBS["quantile"]["number"]["x0"] = 0.5
NLE_PROBS["quantile"]["number"]["mutating"] = quantobj
function f()
    quantile_f(Fx, x) = log(max(x, 0.000001))
    quantile_j(Jx, x) = 1.0/x
    function quantile_fj(Fx, Jx, x)
       Fx = quantile_f(Fx, x)
       Jx = quantile_j(Jx, x)
       Fx, Jx
    end
    quantobj = NLSolvers.VectorObjective(quantile_f, quantile_j, quantile_fj, nothing)

    quantproblem = NEqProblem(quantobj, nothing, NLSolvers.Euclidean(0), NLSolvers.OutOfPlace())

    method = LineSearch(Newton())
    options = NEqOptions()
    state = NLSolvers.init(quantproblem, method, 3.0)
    @time res = solve(quantproblem, 0.4, method, options, state)
    @time res = solve(quantproblem, 0.4, method, options, state)
end

quantile_f(Fx, x) = log(max(x, 0.000001))
quantile_j(Jx, x) = 1.0/x
function quantile_fj(Fx, Jx, x)
   Fx = quantile_f(Fx, x)
   Jx = quantile_j(Jx, x)
   Fx, Jx
end
const quantobj = NLSolvers.VectorObjective(quantile_f, quantile_j, quantile_fj, nothing)

const quantproblem = NEqProblem(quantobj, nothing, NLSolvers.Euclidean(0), NLSolvers.OutOfPlace())

method = LineSearch(Newton())
options = NEqOptions()
state = NLSolvers.init(quantproblem, method, 3.0)
@time res = solve(quantproblem, 0.4, method, options, state)
@time res = solve(quantproblem, 0.4, method, options, state)
=#


module TestRandom
using MXNet
using Base.Test

function test_uniform()
  dims = (100, 100, 2)
  info("random::uniform::dims = $dims")

  low = -10; high = 10
  seed = 123
  mx.srand!(seed)
  ret1 = mx.rand(low, high, dims)

  mx.srand!(seed)
  ret2 = mx.empty(dims)
  mx.rand!(low, high, ret2)

  @test copy(ret1) == copy(ret2)
  @test abs(mean(copy(ret1)) - (high+low)/2) < 0.1
end

function test_gaussian()
  dims = (80, 80, 4)
  info("random::gaussian::dims = $dims")

  μ = 10; σ = 2
  seed = 456
  mx.srand!(seed)
  ret1 = mx.randn(μ, σ, dims)

  mx.srand!(seed)
  ret2 = mx.empty(dims)
  mx.randn!(μ, σ, ret2)

  @test copy(ret1) == copy(ret2)
  @test abs(mean(copy(ret1)) - μ) < 0.1
  @test abs(std(copy(ret1)) - σ) < 0.1
end

test_uniform()
test_gaussian()

end


using DiffEqFlux, OrdinaryDiffEq, Test
using DiffEqFlux: group_ranges

## Test group partitioning helper function
@test group_ranges(10, 4) == [1:4, 4:7, 7:10]
@test group_ranges(10, 5) == [1:5, 5:9, 9:10]
@test group_ranges(10, 10) == [1:10]
@test_throws DomainError group_ranges(10, 1)
@test_throws DomainError group_ranges(10, 11)

## Define initial conditions and time steps
datasize = 30
u0 = Float32[2.0, 0.0]
tspan = (0.0f0, 5.0f0)
tsteps = range(tspan[1], tspan[2], length = datasize)

# Get the data
function trueODEfunc(du, u, p, t)
    true_A = [-0.1 2.0; -2.0 -0.1]
    du .= ((u.^3)'true_A)'
end
prob_trueode = ODEProblem(trueODEfunc, u0, tspan)
ode_data = Array(solve(prob_trueode, Tsit5(), saveat = tsteps))

# Define the Neural Network
nn = FastChain((x, p) -> x.^3,
                FastDense(2, 16, tanh),
                FastDense(16, 2))
p_init = initial_params(nn)

neuralode = NeuralODE(nn, tspan, Tsit5(), saveat = tsteps)
prob_node = ODEProblem((u,p,t)->nn(u,p), u0, tspan, p_init)

function predict_single_shooting(p)
    return Array(neuralode(u0, p))
end

# Define loss function
function loss_function(data, pred)
	return sum(abs2, data - pred)
end

## Evaluate Single Shooting
function loss_single_shooting(p)
    pred = predict_single_shooting(p)
    l = loss_function(ode_data, pred)
    return l, pred
end

res_single_shooting = DiffEqFlux.sciml_train(loss_single_shooting, neuralode.p,
                                          ADAM(0.05),
										  maxiters = 300)

loss_ss, _ = loss_single_shooting(res_single_shooting.minimizer)
println("Single shooting loss: $(loss_ss)")

## Test Multiple Shooting
group_size = 3
continuity_term = 200

function loss_multiple_shooting(p)
    return multiple_shoot(p, ode_data, tsteps, prob_node, loss_function, Tsit5(),
                          group_size; continuity_term,
                          abstol=1e-8, reltol=1e-6) # test solver kwargs
end

res_ms = DiffEqFlux.sciml_train(loss_multiple_shooting, neuralode.p,
                                ADAM(0.05), maxiters = 300)

# Calculate single shooting loss with parameter from multiple_shoot training
loss_ms, _ = loss_single_shooting(res_ms.minimizer)
println("Multiple shooting loss: $(loss_ms)")
@test loss_ms < 10loss_ss

# Test with custom loss function
group_size = 4
continuity_term = 50

function continuity_loss_abs2(û_end, u_0)
    return sum(abs2, û_end - u_0) # using abs2 instead of default abs
end

function loss_multiple_shooting_abs2(p)
    return multiple_shoot(p, ode_data, tsteps, prob_node,
                          loss_function, continuity_loss_abs2, Tsit5(),
                          group_size; continuity_term)
end

res_ms_abs2 = DiffEqFlux.sciml_train(loss_multiple_shooting_abs2, neuralode.p,
                                     ADAM(0.05), maxiters = 300)

loss_ms_abs2, _ = loss_single_shooting(res_ms_abs2.minimizer)
println("Multiple shooting loss with abs2: $(loss_ms_abs2)")
@test loss_ms_abs2 < loss_ss

## Test different SensitivityAlgorithm (default is InterpolatingAdjoint)
function loss_multiple_shooting_fd(p)
    return multiple_shoot(p, ode_data, tsteps, prob_node,
                          loss_function, continuity_loss_abs2, Tsit5(),
                          group_size; continuity_term,
                          sensealg=ForwardDiffSensitivity())
end

res_ms_fd = DiffEqFlux.sciml_train(loss_multiple_shooting_fd, neuralode.p,
                                ADAM(0.05), maxiters = 300)

# Calculate single shooting loss with parameter from multiple_shoot training
loss_ms_fd, _ = loss_single_shooting(res_ms_fd.minimizer)
println("Multiple shooting loss with ForwardDiffSensitivity: $(loss_ms_fd)")
@test loss_ms_fd < 10loss_ss

# Integration return codes `!= :Success` should return infinite loss.
# In this case, we trigger `retcode = :MaxIters` by setting the solver option `maxiters=1`.
loss_fail, _ = multiple_shoot(p_init, ode_data, tsteps, prob_node, loss_function, Tsit5(),
                              datasize; maxiters=1, verbose=false)
@test loss_fail == Inf

## Test for DomainErrors
@test_throws DomainError multiple_shoot(p_init, ode_data, tsteps, prob_node,
                                        loss_function, Tsit5(), 1)
@test_throws DomainError multiple_shoot(p_init, ode_data, tsteps, prob_node,
                                        loss_function, Tsit5(), datasize + 1)

## Ensembles
u0s = [Float32[2.0, 0.0], Float32[3.0, 1.0]]
function prob_func(prob, i, repeat)
    remake(prob, u0 = u0s[i])
end
ensemble_prob = EnsembleProblem(prob_node, prob_func = prob_func)
ensemble_prob_trueODE = EnsembleProblem(prob_trueode, prob_func = prob_func)
ensemble_alg = EnsembleThreads()
trajectories = 2
ode_data_ensemble = Array(solve(ensemble_prob_trueODE, Tsit5(), ensemble_alg, trajectories = trajectories, saveat = tsteps))

group_size = 3
continuity_term = 200
function loss_multiple_shooting_ens(p)
    return multiple_shoot(p, ode_data_ensemble, tsteps, ensemble_prob, ensemble_alg,
                          loss_function, Tsit5(),
                          group_size; continuity_term,
                          trajectories,
                          abstol=1e-8, reltol=1e-6) # test solver kwargs
end

res_ms_ensembles = DiffEqFlux.sciml_train(loss_multiple_shooting_ens, neuralode.p,
                                ADAM(0.05), maxiters = 300)

loss_ms_ensembles, _ = loss_single_shooting(res_ms_ensembles.minimizer)

println("Multiple shooting loss with EnsembleProblem: $(loss_ms_ensembles)")

@test loss_ms_ensembles < 10loss_ss


#=
  These routines return +/-Inf when given +/-Inf.
  That differs from nextfloat(-Inf) == -realmax(), prevfloat(Inf) == realmax()
  (prevfloat(Inf)==Inf makes more sense to me, and likely is more helpful).
  And steps twice when given values of very small magnitude (see paper).
  The alternative implementation, converting to [U]Int and adding/subtracting 1,
  returns NaN when given +/-Inf -- and checking for Inf adds branching.
  ref:
  'Predecessor and Successor In Rounding To Nearest' by Siegried M. Rump
  "The routines deliver the exact answer except for a small range near underflow,
   in which case the true result is overestimated by eta [the value added/subtracted below]."
=#
# exact for |x| > 8.900295434028806e-308
nextNearerToZero(x::Float64)   = (0.9999999999999999*x)-5.0e-324 # (x-1.1102230246251568e-16*x)-5.0e-324
nextAwayFromZero(x::Float64)   = (1.0000000000000002*x)+5.0e-324 # (x+1.1102230246251568e-16*x)+5.0e-324
# exact for |x| > 4.814825f-35
nextNearerToZero(x::Float32)   = (0.99999994f0*x)-1.435f-42      # (x-5.960465f-8*x)-1.435f-42
nextAwayFromZero(x::Float32)   = (1.00000010f0*x)+1.435f-42      # (x+5.960465f-8*x)+1.435f-42
# the multiplicative formulation for Float16 is exact for |x| > Float16(0.25)
# which is quite coarse, we do not use that here
nextNearerToZero(x::Union{BigFloat,Float16}) = signbit(x) ? nextfloat(x) : prevfloat(x)
nextAwayFromZero(x::Union{BigFloat,Float16}) = signbit(x) ? prevfloat(x) : nextfloat(x)

@inline nextFloat{T<:AbstractFloat}(x::T) = signbit(x) ? nextNearerToZero(x) : nextAwayFromZero(x)
@inline prevFloat{T<:AbstractFloat}(x::T) = signbit(x) ? nextAwayFromZero(x) : nextNearerToZero(x)

@inline nextFloat(x::Union{BigFloat,Float16}) = nextfloat(x)
@inline prevFloat(x::Union{BigFloat,Float16}) = prevfloat(x)

function nextFloat{T<:AbstractFloat}(x::T, n::Int)
    if !signbit(n)
        for i in 1:n
            x = nextFloat(x)
        end
    else
        x = prevFloat(x,-n)
    end
    x
end

function prevFloat{T<:AbstractFloat}(x::T, n::Int)
    if !signbit(n)
        for i in 1:n
            x = prevFloat(x)
        end
    else
        x = nextFloat(x,-n)
    end
  x
end

for (F,U,I) in ((:Float16,:UInt16,:Int16), (:Float32,:UInt32,:Int32), (:Float64,:UInt64,:Int64))
  @eval begin
    function nFloatsSeparate{T<:$F}(a::T, b::T)
      isneg = (a > b)
      a,b = minmax(a,b)

      if signbit(a) == signbit(b)
        if signbit(a)
           z = reinterpret($U,abs(a))-reinterpret($U,abs(b))
        else
           z = reinterpret($U,b)-reinterpret($U,a)
        end
      else
        z = reinterpret($U,b)+reinterpret($U,-a)+one($U)
      end

      if z > reinterpret($U, typemax($I))
        throw(ArgumentError(string("separation exceeds typemax(",$I,")")))
      else
        isneg ? -reinterpret($I,z) : reinterpret($I,z)
      end
    end
  end
end

#= presumes xpa == frexp(a)[2] == frexp(b)[2]
function nFloatsSeparate{T<:Float64}(a::T, b::T,xpa::Int)
    isneg = a > b
    x,y = minmax(a,b)
    z = floor(Int64, ((y-x)/2^xpa)*2^53)
    isneg ? -z : z
end
=#


immutable AbsoluteError <: Loss
end

@inline function loss_value(loss::AbsoluteError, p::Real, y::Real)
    return abs(p - y)
end

Base.show(io::IO, loss::AbsoluteError) = print(io, "Absolute Error")


using DataFrames, CSV, Statistics, StatsBase
data = CSV.read("../externals/Core.Math.Data/data/Pejcic_318.csv", copycols = true)

# println("Sample Mean: ", mean(data[:ATT]))
# println("Harmonic <= Geometric <= Arithmetic ",(harmmean(data), geomean(data), mean(data)))
# println("Sample Variance: ",var(data))
# println("Sample Standard Deviation: ",std(data))
# println("Minimum: ", minimum(data))
# println("Maximum: ", maximum(data))
# println("Median: ", median(data))
# println("95th percentile: ", percentile(data, 95))
# println("0.95 quantile: ", quantile(data, 0.95))
# println("Interquartile range: ", iqr(data),"\n")

for col in eachcol(data,true);
    if (eltype(col[2]) <: Real) println(mean(col[2]));
    end;
end;


"""
    six_dof_aero_euler_fixed_mass(x, mass, inertia, forces, moments, h=[0.0, 0.0, 0.0])

Six degrees of freedom dynamic system using Euler angles for attitude representation
and assuming fixed mass.

Flat Earth hypothesis is applied and Earth reference frame is considered
inertial.

It is considered that the aircraft xb-zb plane is a plane of symmetry so that
Jxy and Jyz cross-product of inertia are zero and will not be taken into
account.

The effects of the angular momentum produced by spinning rotors is taken into
account with the optional argument `h`.

# Arguments
- `x::12-element Array{Number,1}`: state vector.
    tas: inertial speed along x_wind axis (m/s)
    α: angle of attack (rad)
    β: angle of side-slip (rad)
    p, q, r: inertial rotatinal velocity expressed in body axis. (rad/s)
    ψ, θ, ϕ: attitude given by Euler angles (zyx). Yaw, pitch, roll. (rad)
    xe, ye, ze: position wrt the inertial system origin expressed in Earth Axis. (m)
- `mass::Number`: total mass of the aircraft (kg)
- `inertia::3×3 Array{Number,2}`: inertia tensor (kg·m²)
- `forces::3-element Array{Number,1}`: total forces expressed in body axis. (N)
- `moments::3-element Array{Number,1}`: total moments expressed in body axis.(N·m)
- `h::3-element Array{Number,1}`: Additional angular momentum contributions such
  as those coming from spinning rotors (kg·m²/s).

# Returns
- `x_dot`: state vector derivative according to the equation of motion,
    inertial properties and applied forces and moments.
"""
function six_dof_aero_euler_fixed_mass(x, mass, inertia, forces, moments, h=[0.0, 0.0, 0.0])

    m = mass
    tas, α, β, p, q, r, ψ, θ, ϕ, xe, ye, ze = x

    Fx, Fy, Fz = forces
    L, M, N = moments

    sψ, cψ = sin(ψ), cos(ψ)
    sθ, cθ = sin(θ), cos(θ)
    sϕ, cϕ = sin(ϕ), cos(ϕ)

    u, v, w = wind2body(tas, 0.0, 0.0, α, β)

    # Linear momentum equations
    u_dot = Fx / m + r * v - q * w
    v_dot = Fy / m - r * u + p * w
    w_dot = Fz / m + q * u - p * v

    tas_dot, α_dot, β_dot = tas_α_β_dot_from_uvw_dot(u, v, w, u_dot, v_dot, w_dot)

    # Angular momentum equations
    Ix = inertia[1, 1]
    Iy = inertia[2, 2]
    Iz = inertia[3, 3]
    Jxz = -inertia[1, 3]

    Jxz2 = Jxz*Jxz
    Γ = (Ix*Iz - Jxz2)
    temp = (Ix + Iz - Iy)

    # Engine angular momentum contribution
    hx, hy, hz = h

    rhy_qhz = (r*hy - q*hz)
    qhx_phy = (q*hx - p*hy)

    pe_dot = Iz * rhy_qhz + Jxz * qhx_phy
    qe_dot = -r*hx + p*hz
    re_dot = Jxz * rhy_qhz + Ix * qhx_phy

    # Angular momentum equations
    p_dot = L*Iz + N*Jxz - q*r*(Iz*Iz - Iz*Iy + Jxz2) + p*q * Jxz * temp + pe_dot
    p_dot /= Γ
    q_dot = M + (Iz - Ix) * p*r - Jxz * (p*p - r*r) + qe_dot
    q_dot /= Iy
    r_dot = L*Jxz + N*Ix + p*q * (Ix*Ix - Ix*Iy + Jxz2) - q*r * Jxz * temp + re_dot
    r_dot /= Γ

    # Angular Kinematic equations
    ψ_dot = (q * sϕ + r * cϕ) / cθ
    θ_dot = q * cϕ - r * sϕ
    # ϕ_dot = p + (q * sϕ + r * cϕ) * tan(θ)
    ϕ_dot = p + ψ_dot * sθ

    # Linear kinematic equations
    xe_dot =  cθ*cψ * u + (sϕ*sθ*cψ - cϕ*sψ) * v + (cϕ*sθ*cψ + sϕ*sψ) * w
    ye_dot =  cθ*sψ * u + (sϕ*sθ*sψ + cϕ*cψ) * v + (cϕ*sθ*sψ - sϕ*cψ) * w
    ze_dot = -sθ    * u +  sϕ*cθ             * v +  cϕ*cθ             * w

    [tas_dot, α_dot, β_dot, p_dot, q_dot, r_dot, ψ_dot, θ_dot, ϕ_dot, xe_dot, ye_dot, ze_dot]
end


# Provides a framework on how to translate between LAI coordinates and physical Z-dimensions
#=

using Distributions:Beta,cdf,pdf
using Parameters

abstract type AbstractVerticalCanopyStructure end

"The beta distribution probability for LAI as function of z/h"
Base.@kwdef struct BetaCanopyStructure{FT} <: AbstractVerticalCanopyStructure
    p::FT = 2.5
    q::FT = 2.5
    B = Beta(p,q)
end

# Bonan ML canopy distribution (Beta function):
function convert_z_to_dLAI!(mod::BetaCanopyStructure, canopy, z)
    @unpack LAI, nlayers, h = canopy
    # LAI = Leaf Area Index
    # h = Canopy height
    LAI * cdf.(mod.B,z/h)
end # functi
=#


using PyPlot
stress_y =  200.0

function vm_upper(a, c)
    vals = f(a[1], a[2], c)
    vm(vals[1], vals[2], 200)
end
vm(a,b) = sqrt(a^2 - a*b + b^2) - stress_y
f(m,c) = [600*cos(c) 600*sin(c)].*m
x_vals = []
max_iter = 100
y_vals = []
for i=0:0.1:(2*pi+0.3)
    wf(x) = f(x, i)
    t = 0.01
    step = 2
    merkki = -1
    s11, s22 = wf(t)
    ii = 0
    while (abs(vm(s11, s22)) > 1e-7) && ii < max_iter
        val = vm(s11, s22)
        if sign(val) != merkki
            merkki *= -1
            step *= -0.5
        end
        t += step
        s11, s22 = wf(t)
        ii += 1
    end
    push!(x_vals, s11)
    push!(y_vals, s22)
end

plot(x_vals, y_vals)
PyPlot.title("von Mises yield surface in 2D")
PyPlot.xlabel("Eig Stress 1")
PyPlot.ylabel("Eig Stress 2")
grid()
show()


using ArrayViews
using Base.Test

avparent = rand(7, 8)
bvparent = rand(8, 5)

_ab = avparent * bvparent

@test _ab == view(avparent, :, :) * bvparent
@test _ab == avparent * view(bvparent, :, :)
@test _ab == view(avparent, :, :) * view(bvparent, :, :)

for j = 1:size(bvparent,2)
    @test_approx_eq _ab[:,j] view(avparent,:,:) * view(bvparent,:,j)
end

@test avparent[:, 2:2:7] * bvparent[1:3, 1:2:5] == view(avparent, :, 2:2:7) * view(bvparent, 1:3, 1:2:5)


export FixedParameterSystem, fix_parameters

"""
    FixedParameterSystem(F:AbstractSystem, parameters)

Construct a system from the given [`AbstractSystem`](@ref) `F` with the given `parameters`
fixed.
"""
struct FixedParameterSystem{S<:AbstractSystem,T} <: AbstractSystem
    system::S
    parameters::Vector{T}
end
FixedParameterSystem(F::System, p) = FixedParameterSystem(ModelKitSystem(F), p)
Base.size(F::FixedParameterSystem) = size(F.system)

ModelKit.variables(F::FixedParameterSystem) = variables(F.system)
ModelKit.parameters(F::FixedParameterSystem) = Variable[]
ModelKit.variable_groups(F::FixedParameterSystem) = variable_groups(F.system)

(F::FixedParameterSystem)(x, p = nothing) = F.system(x, F.parameters)

evaluate!(u, F::FixedParameterSystem, x, p = nothing) =
    evaluate!(u, F.system, x, F.parameters)
evaluate_and_jacobian!(u, U, F::FixedParameterSystem, x, p = nothing) =
    evaluate_and_jacobian!(u, U, F.system, x, F.parameters)
taylor!(u, v::Val, F::FixedParameterSystem, tx, p = nothing) =
    taylor!(u, v, F.system, tx, F.parameters)

"""
    fix_parameters(F::AbstractSystem, p)

Fix the parameters of the given system `F`. Returns a [`FixedParameterSystem`](@ref).
"""
fix_parameters(F::AbstractSystem, p) = FixedParameterSystem(F, p)


"""
    HarmonicAngle(; i, j, k, th0, cth)

A bond angle between three atoms.
"""
struct HarmonicAngle{D, K} <: SpecificInteraction
    i::Int
    j::Int
    k::Int
    th0::D
    cth::K
end

HarmonicAngle(; i, j, k, th0, cth) = HarmonicAngle{typeof(th0), typeof(cth)}(i, j, k, th0, cth)

# Sometimes domain error occurs for acos if the value is > 1.0 or < -1.0
acosbound(x::Real) = acos(clamp(x, -1, 1))

@inline @inbounds function force(a::HarmonicAngle,
                                  coords,
                                  s::Simulation)
    ba = vector(coords[a.j], coords[a.i], s.box_size)
    bc = vector(coords[a.j], coords[a.k], s.box_size)
    cross_ba_bc = ba × bc
    if iszero(cross_ba_bc)
        fz = ustrip.(zero(coords[a.i])) * s.force_unit
        return [a.i, a.j, a.k], [fz, fz, fz]
    end
    pa = normalize(ba × cross_ba_bc)
    pc = normalize(-bc × cross_ba_bc)
    angle_term = -a.cth * (acosbound(dot(ba, bc) / (norm(ba) * norm(bc))) - a.th0)
    fa = (angle_term / norm(ba)) * pa
    fc = (angle_term / norm(bc)) * pc
    fb = -fa - fc
    return [a.i, a.j, a.k], [fa, fb, fc]
end

@inline @inbounds function potential_energy(a::HarmonicAngle,
                                            s::Simulation)
    ba = vector(s.coords[a.j], s.coords[a.i], s.box_size)
    bc = vector(s.coords[a.j], s.coords[a.k], s.box_size)
    return (a.cth / 2) * (acosbound(dot(ba, bc) / (norm(ba) * norm(bc))) - a.th0)^2
end


# This code is modified from part of my master thesis. I tried to use the semiparametric method to
# develop a new Feasible-GLS estimating procedure. In the first step, a standard FGLS estimating
# procedure is implemented with a non-parametrically identified skedactic function Σ. Then, I resemple
# the test statistic using the wild bootstrap method, and then compare the FGLS t-statistic to the
# artificially-generated distribution. This method is proved to have great performance in finite samples
# in terms of lower size distortion and better testing power. The following code demonstrates the method
# using k-nearest neighbors estimator in the first step. Also, a simple leave-one-out cross validation
# is applied to choose the optimal bandwidth.

using LinearAlgebra, Distributions, StatsBase, BenchmarkTools

function KNN(h, X, y, e)
    #-----------------------------------------------------#
    # h : bandwidth                                       #
    # X : Nx5 matrix of constant and explanatory variable #
    # y : Nx1 matrix of explained variable                #
    # e : Nx1 matrix of residuals                         #
    #-----------------------------------------------------#
    N = length(y)
    w = zeros(N, N)
    for pp in 1:N
        x_normsort = sortperm(view.(Ref(w), 1:size(w,1), :))
        v = x_normsort .<= h
        v[pp] = 0
        w[:, pp] = v ./ sum(v)
    end
    s2hat = transpose(w) * e.^2
    small = mean(e.^2) / 10
    s2hat[s2hat .< small] .= small
    return(s2hat)
end

function CrossValidation(H, X, y, e)
    #-----------------------------------------------------#
    # H : vector of bandwidth candidates                  #
    #-----------------------------------------------------#
    CV = []
    for r in 1:length(H)
        s2hat_cv = KNN(H[r], X, y, e)
        error = sum((e.^2 .- s2hat_cv).^2)
        CV = append!(CV, error)
    end
    h = H[argmin(CV)]
    s2hat = KNN(h, X, y, e)
    Σ = Diagonal(s2hat)
    β̂ = (inv(transpose(X) * inv(Σ) * X) * transpose(X) * inv(Σ) * y)
    return(tuple(β̂, Σ))
end

function WildBootstrap(iter, X, y, e, β̂, Σ)
    N = length(y)
    t_memory = []
    for b in 1:iter
        u = float(rand(Bernoulli(0.5), 100))
        u[u.==0] .= -1
        ỹ = X * β̂ + e.*u
        β̃5 = (inv(transpose(X) * inv(Σ) * X) * transpose(X) * inv(Σ) * ỹ)[5, :]
        tstat = β̃5 / sqrt(inv(transpose(X) * inv(Σ) * X)[5, 5])
        t_memory = append!(t_memory, tstat)
    end
    return(sort(t_memory))
end

# DGP
x1 = ones(100)
x25 = rand(Normal(3, 1), 100, 4)
X = hcat(x1, x25)
β = [1, 1, 1, 1, 0]
ε = rand(100)
y = X * β + ε
β̂ = inv(transpose(X) * X) * transpose(X) * y
e = y - X * β̂

# FGLS estimation
H = range(1, stop = 8, length = 10) * (2.50773668907 + 0.021528881383 * (100^(4 / 5)))
FGLS = CrossValidation(H, X, y, e)
β̂5 = FGLS[1][5, :]
Σ = FGLS[2]
tstat = β̂5 / sqrt(inv(transpose(X) * inv(Σ) * X)[5, 5])

# Resempling t-statistic using wild bootstrap
T = WildBootstrap(1000, X, y, e, β̂, Σ)
uppercrit = percentile(T, 75)
lowercrit = percentile(T, 25)
if tstat[1] >= uppercrit || tstat[1] <= lowercrit
    println("Reject the null hypothesis")
else
    println("Do not reject the null hypothesis")
end

# Here are the executing time of the functions
@btime CrossValidation(H, X, y, e)
@btime WildBootstrap(1000, X, y, e, β̂, Σ)
# The CrossValidation function is slow since it contains multi-layers of loops.

# The following functions use multi-threading to improve the speed.
function KNN_multi(h, X, y, e)
    #-----------------------------------------------------#
    # h : bandwidth                                       #
    # X : Nx5 matrix of constant and explanatory variable #
    # y : Nx1 matrix of explained variable                #
    # e : Nx1 matrix of residuals                         #
    #-----------------------------------------------------#
    N = length(y)
    w = zeros(N, N)
    Threads.@threads for pp in 1:N
        x_normsort = sortperm(view.(Ref(w), 1:size(w,1), :))
        v = x_normsort .<= h
        v[pp] = 0
        w[:, pp] = v ./ sum(v)
    end
    s2hat = transpose(w) * e.^2
    small = mean(e.^2) / 10
    s2hat[s2hat .< small] .= small
    return(s2hat)
end

function CrossValidation_multi(H, X, y, e)
    #-----------------------------------------------------#
    # H : vector of bandwidth candidates                  #
    #-----------------------------------------------------#
    CV = []
    Threads.@threads for r in 1:length(H)
        s2hat_cv = KNN_multi(H[r], X, y, e)
        error = sum((e.^2 .- s2hat_cv).^2)
        CV = append!(CV, error)
    end
    h = H[argmin(CV)]
    s2hat = KNN_multi(h, X, y, e)
    Σ = Diagonal(s2hat)
    β̂ = (inv(transpose(X) * inv(Σ) * X) * transpose(X) * inv(Σ) * y)
    return(tuple(β̂, Σ))
end

# It is around 2 times faster when using 4 threads on my computer
@btime CrossValidation_multi(H, X, y, e)


# -*- coding: utf-8 -*-
# ---
# jupyter:
#   jupytext:
#     formats: ipynb,jl:hydrogen
#     text_representation:
#       extension: .jl
#       format_name: hydrogen
#       format_version: '1.3'
#       jupytext_version: 1.11.2
#   kernelspec:
#     display_name: Julia 1.8.0-DEV
#     language: julia
#     name: julia-1.8
# ---

# %%
using Plots
using LinearAlgebra
using Fmt

# %%
f(a) = -a + √(a^2 - 2)
a = range(√2, 100, 1000)
plot(a, f; ylim=(-1.5, 0), label="")

# %%
A(a) = Float64[
     0  1
    -2 -2a
]

U(a, t) = exp(t*A(a))

# %%
@gif for a in [0:0.02:1.4; fill(1.4, 40); 1.4:0.04:5]
    t = range(0, 4π; length=1000)
    u = (t -> U(a, t)[1, 1]).(t)
    v = (t -> U(a, t)[2, 1]).(t)
    P = plot(t, [u v]; label=["u1" "v1"], ylim=(-1, 1))
    u = (t -> U(a, t)[1, 2]).(t)
    v = (t -> U(a, t)[2, 2]).(t)
    plot!(t, [u v]; label=["u2" "v2"], ylim=(-1, 1), ls=:dash)
    plot!(; title=f"a = {$a:.2f}")
end

# %%
for a in 0.1:0.1:2
    t = reverse(range(0, 10π; length=5001))
    n = accumulate(max, norm.(U.(a, t)))
    k = findfirst(t -> norm(U(a, t)) > 0.1, t)
    @show a, t[k], norm(U(a, t[k]))
end

# %%
t = range(0, 4π; length=1000)
u = (t -> U(t)[1, 1]).(t)
v = (t -> U(t)[2, 1]).(t)
plot(t, [u v])

# %%
t = range(0, 10; length=1000)
u = (t -> U(t)[1, 2]).(t)
v = (t -> U(t)[2, 2]).(t)
plot(t, [u v])

# %%
a, V = eigen(A)

# %%
V/0.40824829046386296

# %%
p = [
    -1 + 1im
    2
]

A*p - (-1 - 1im)*p

# %%
q = [
    -1 - 1im
    2
]

A*q - (-1 + 1im)*q

# %%
[minimum(real.(eigvals(Float64[0 1; -2 -2a]))) for a in 0:0.1:2]

# %%
using SymPy

# %%
@vars a

# %%
A = [
    0 1
    -2 -2
]

# %%
A^2//2

# %%
A^3//6

# %%
A^4//24

# %%
B = [
    0 1
    -2 -2a
]

# %%
B^2/factorial(2)

# %%
expand.(B^3)/factorial(3)

# %%
expand.(B^4)/factorial(4)

# %%