mutable struct Item
    index::Int64
    weight::Int64
    profit::Int64
    solution::Bool
end

struct Interval
    first::Int64
    last::Int64
    weight::Int64
end

mutable struct GlobalData
    items::Vector{Item}
    capacity::Int64

    br::Int64
    wsb::Int64
    psb::Int64
    z::Int64
    fsort::Int64
    lsort::Int64

    stack1::Vector{Interval}
    stack2::Vector{Interval}

    estack::Vector{Int64}

    GlobalData() = new([], 0, 0, 0, 0, 0, 0, 0, [], [], [])
end

function solveKnapExpCore(data::KnapData)
    n = length(data.items)
    if n == 0 return 0, Int64[] end

    gd = GlobalData()
    gd.items = [ Item(i, data.items[i].weight, data.items[i].profit, false) for i in 1:n ]
    gd.capacity = data.capacity

    interval = partsort(gd, 1, n, 0)
    gd.fsort = interval.first
    gd.lsort = interval.last

    gd.z = heuristic(gd, 1, n)

    elebranch(gd, 0, gd.wsb - gd.capacity, gd.br - 1, gd.br)

    return gd.z + gd.psb, definesolution(gd)
end

function det(a::Matrix{Int64})
    return a[1, 1] * a[2, 2] - a[1, 2] * a[2, 1]
end

function partsort(gd::GlobalData, f::Int64, l::Int64, ws::Int64)
    d = l - f + 1
    if d > 1
        m = f + d ÷ 2

        if det([ gd.items[f].profit gd.items[f].weight; gd.items[m].profit gd.items[m].weight ]) < 0
            gd.items[f], gd.items[m] = gd.items[m], gd.items[f]
        end

        if d > 2
            if det([ gd.items[m].profit gd.items[m].weight; gd.items[l].profit gd.items[l].weight ]) < 0
                gd.items[m], gd.items[l] = gd.items[l], gd.items[m]

                if det([ gd.items[f].profit gd.items[f].weight; gd.items[m].profit gd.items[m].weight ]) < 0
                    gd.items[f], gd.items[m] = gd.items[m], gd.items[f]
                end
            end
        end
    end

    if d <= 3
        return Interval(f, l, ws)
    else
        mp = gd.items[m].profit
        mw = gd.items[m].weight
        i = f
        j = l
        wi = ws
        while true
            while true
                wi += gd.items[i].weight
                i += 1
                if det([ gd.items[i].profit gd.items[i].weight; mp mw ]) <= 0 break end
            end
            while true
                j -= 1
                if det([ gd.items[j].profit gd.items[j].weight; mp mw ]) >= 0 break end
            end
            if i > j break end
            gd.items[i], gd.items[j] = gd.items[j], gd.items[i]
        end

        if wi > gd.capacity
            push!(gd.stack2, Interval(i, l, wi))
            return partsort(gd, f, i - 1, ws)
        else
            push!(gd.stack1, Interval(f, i - 1, ws))
            return partsort(gd, i, l, wi)
        end
    end
end

function heuristic(gd::GlobalData, f::Int64, l::Int64)
    ps = 0
    ws = gd.capacity

    i = f
    while gd.items[i].weight <= ws
        ws -= gd.items[i].weight
        ps += gd.items[i].profit
        gd.items[i].solution = true
        i += 1
    end

    gd.br = i
    gd.wsb = gd.capacity - ws
    gd.psb = ps

    dz = (gd.capacity - gd.wsb) * gd.items[gd.br].profit ÷ gd.items[gd.br].weight

    empty!(gd.estack)
    gd.z = 0
    if gd.z == dz return gd.z end

    r = gd.capacity - gd.wsb
    for i in gd.br:l
        if (gd.items[i].weight <= r) && (gd.items[i].profit > gd.z)
            empty!(gd.estack)
            push!(gd.estack, gd.items[i].index)
            gd.z = gd.items[i].profit
            if gd.z == dz return gd.z end
        end
    end

    r = gd.wsb + gd.items[gd.br].weight - gd.capacity
    pb = gd.items[gd.br].profit
    for i in gd.br - 1:-1:f
        if (gd.items[i].weight >= r) && (pb - gd.items[i].profit > gd.z)
            empty!(gd.estack)
            push!(gd.estack, gd.items[gd.br].index)
            push!(gd.estack, gd.items[i].index)
            gd.z = pb - gd.items[i].profit
            if gd.z == dz return gd.z end
        end
    end

    return gd.z
end

function reduce(gd::GlobalData, first::Int64, last::Int64)
    pb = gd.items[gd.br].profit
    wb = gd.items[gd.br].weight

    q = det([ gd.z + 1 gd.capacity - gd.wsb; pb wb ])
    i = first
    j = last

    if i <= gd.br
        k = gd.fsort - 1
        while i <= j
            if det([ -gd.items[j].profit -gd.items[j].weight; pb wb ]) < q
                gd.items[i], gd.items[j] = gd.items[j], gd.items[i]
                i += 1
            else
                gd.items[j], gd.items[k] = gd.items[k], gd.items[j]
                j -= 1
                k -= 1
            end
        end
        if k == gd.fsort - 1
            gd.items[first], gd.items[k] = gd.items[k], gd.items[first]
            k -= 1
        end
        last = gd.fsort - 1
        first = k + 1
    else
        k = gd.lsort + 1
        while i <= j
            if det([ gd.items[i].profit gd.items[i].weight; pb wb ]) < q
                gd.items[i], gd.items[j] = gd.items[j], gd.items[i]
                j -= 1
            else
                gd.items[i], gd.items[k] = gd.items[k], gd.items[i]
                i += 1
                k += 1
            end
        end
        if k == gd.lsort + 1
            gd.items[last], gd.items[k] = gd.items[k], gd.items[last]
            k += 1
        end
        first = gd.lsort + 1
        last = k - 1
    end
    return first, last
end

function sorti(gd::GlobalData, stack::Vector{Interval})
    if isempty(stack) return false end

    interval = pop!(stack)
    first, last = reduce(gd, interval.first, interval.last)
    interval = partsort(gd, first, last, interval.weight)
    if interval.first < gd.fsort gd.fsort = interval.first end
    if interval.last > gd.lsort gd.lsort = interval.last end

    return true
end

function elebranch(gd::GlobalData, ps::Int64, ws::Int64, s::Int64, t::Int64)
    improved = false

    if ws <= 0
        if ps > gd.z
            improved = true
            gd.z = ps
            empty!(gd.estack)
        end

        while true
            if t > gd.lsort && !sorti(gd, gd.stack2) break end
            if det([ ps - (gd.z + 1) ws; gd.items[t].profit gd.items[t].weight ]) < 0 break end
            if elebranch(gd, ps + gd.items[t].profit, ws + gd.items[t].weight, s, t + 1)
                improved = true
                push!(gd.estack, gd.items[t].index)
            end
            t += 1
        end
    else
        while true
            if s < gd.fsort && !sorti(gd, gd.stack1) break end
            if det([ ps - (gd.z + 1) ws; gd.items[s].profit gd.items[s].weight ]) < 0 break end
            if elebranch(gd, ps - gd.items[s].profit, ws - gd.items[s].weight, s - 1, t)
                improved = true
                push!(gd.estack, gd.items[s].index)
            end
            s -= 1
        end
    end

    return improved
end

function definesolution(gd::GlobalData)
    sort!(gd.items, by = x -> x.index)
    for index in gd.estack
        gd.items[index].solution = !gd.items[index].solution
    end

    solution = Int64[]
    for item in gd.items
        if item.solution
            push!(solution, item.index)
        end
    end
    return solution
end


@diffusion_process FitzHughDiffusionConjug begin
    :dimensions
    process --> 2
    wiener --> 1

    :parameters
    (ϵ, s, γ, β, σ) --> Float64

    :conjugate
    phi(t, x) -->(
        (-x[2],),
        (x[1]-x[1]^3+(1-3*x[1]^2)*x[2],),
        (one(x[1]),),
        (-x[1],),
        (zero(x[1]),),
        (zero(x[1]),),
    )
    nonhypo(x) --> x[2:2]
    num_non_hypo --> 1

    :additional
    constdiff --> true
end

function b(t, x, P::FitzHughNagumoConjug)
    ℝ{2}(
        x[2],
        (
            (P.ϵ - P.γ)*x[1]
            - P.ϵ*(x[1]^3 + (3.0*x[1]^2 - 1.0)*x[2])
            + P.s
            - P.β
            - x[2]
        )
    )
end

σ(t, x, P::FitzHughNagumoConjug) = ℝ{2}(0.0, P.σ)


using Test, Astro

println("15.a Rise, Set, Transit")
# Meeus chapter 15, pages 103
# Venus 1988 March 20, at Boston (long 71.0833, lat 42.333)
# sidereal time at Greenwich in degrees should be 11h50m58.10 177d.74208

# coordinates of venus at 0h TD
# march 19 RA 40d.68021 Dec = 18d.04761
# march 20 RA 41.73129  Dec = 18d.44092
# march 21 RA 42.78204  Dec = 18d.82742

save_Lat = Astro.latitude
save_Long = Astro.longitude
set_longitude(deg2rad(71.0833))
set_latitude(deg2rad(42.3333))

println("using Longitude $(rad2deg(longitude))°" )
println("using Latitude $(rad2deg(latitude))°" )

println("Only low accuracy version currently available")

println("rise")
ut = cal_to_jd(1988, 3, 20)
ra = deg2rad(41.73129)
decl = deg2rad(18.44092)
# h0 is the 'standard' altitude, deg2rad(-0.5667) for stars/planets, deg2rad(-0.8333) for the sun
h0 = deg2rad(-0.5667)
jd_rise, jd_transit, jd_set = object_rts_low(ut, ra, decl, h0)

println("jd_rise $(fday_to_hms(jd_rise)), jd_transit $(fday_to_hms(jd_transit)), jd_set $(fday_to_hms(jd_set))")

#@test isapprox(jd_rise - ut, 0.51766, atol=0.1)

println("")

# restore lat and long
set_longitude(save_Long)
set_latitude(save_Lat)


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

"""
    stabilitymatrix!(eqsdiff!, t, x, δx, dδx, jac, _δv, params[, jacobianfunc!=nothing])

Updates the matrix `jac::Matrix{Taylor1{U}}` (linearized equations of motion)
computed from the equations of motion (`eqsdiff!`), at time `t` at `x`; `x` is
of type `Vector{Taylor1{U}}`, where `U<:Number`. `δx`, `dδx` and `_δv` are
auxiliary arrays of type `Vector{TaylorN{Taylor1{U}}}` to avoid allocations.
Optionally, the user may provide a Jacobian function `jacobianfunc!` to compute
`jac`. Otherwise, `jac` is computed via automatic differentiation using
`TaylorSeries.jl`.

"""
function stabilitymatrix!(eqsdiff!, t::Taylor1{T}, x::Vector{Taylor1{U}},
        δx::Vector{TaylorN{Taylor1{U}}}, dδx::Vector{TaylorN{Taylor1{U}}},
        jac::Matrix{Taylor1{U}}, _δv::Vector{TaylorN{Taylor1{U}}}, params,
        jacobianfunc! =nothing) where {T<:Real, U<:Number}

    if isa(jacobianfunc!, Nothing)
        # Set δx equal to current value of x plus 1st-order variations
        for ind in eachindex(δx)
            @inbounds δx[ind] = x[ind] + _δv[ind]
        end
        # Equations of motion
        eqsdiff!(dδx, δx, params, t)
        TaylorSeries.jacobian!(jac, dδx)
    else
        jacobianfunc!(jac, x, params, t)
    end
    nothing
end

# Modified from `cgs` and `mgs`, obtained from:
# http://nbviewer.jupyter.org/url/math.mit.edu/~stevenj/18.335/Gram-Schmidt.ipynb
# Classical Gram–Schmidt (Trefethen algorithm 7.1), implemented in the simplest way
# (We could make it faster by unrolling loops to avoid temporaries arrays etc.)
function classicalGS!(A, Q, R, aⱼ, qᵢ, vⱼ)
    m,n = size(A)
    fill!(R, zero(eltype(A)))
    for j = 1:n
        # aⱼ = A[:,j]
        for ind = 1:m
            @inbounds aⱼ[ind] = A[ind,j]
            @inbounds vⱼ[ind] = aⱼ[ind]
        end
        # vⱼ = copy(aⱼ) # use copy so that modifying vⱼ doesn't change aⱼ
        for i = 1:j-1
            # qᵢ = Q[:,i]
            for ind = 1:m
                @inbounds qᵢ[ind] = Q[ind,i]
            end
            @inbounds R[i,j] = dot(qᵢ, aⱼ)
            # vⱼ -= R[i,j] * qᵢ
            @inbounds for ind = 1:m
                vⱼ[ind] -= R[i,j] * qᵢ[ind]
            end
        end
        @inbounds R[j,j] = norm(vⱼ)
        # Q[:,j] = vⱼ / R[j,j]
        for ind = 1:m
            @inbounds Q[ind,j] = vⱼ[ind] / R[j,j]
        end
    end
    return nothing
end

# Modified Gram–Schmidt (Trefethen algorithm 8.1); see also
# http://nbviewer.jupyter.org/url/math.mit.edu/~stevenj/18.335/Gram-Schmidt.ipynb
function modifiedGS!(A, Q, R, aⱼ, qᵢ, vⱼ)
    m,n = size(A)
    fill!(R, zero(eltype(A)))
    for j = 1:n
        # aⱼ = A[:,j]
        for ind = 1:m
            @inbounds aⱼ[ind] = A[ind,j]
            @inbounds vⱼ[ind] = aⱼ[ind]
        end
        # vⱼ = copy(aⱼ) # use copy so that modifying vⱼ doesn't change aⱼ
        for i = 1:j-1
            # qᵢ = Q[:,i]
            for ind = 1:m
                @inbounds qᵢ[ind] = Q[ind,i]
            end
            @inbounds R[i,j] = dot(qᵢ, vⱼ) # ⟵ NOTICE: mgs has vⱼ, clgs has aⱼ
            # vⱼ -= R[i,j] * qᵢ
            @inbounds for ind = 1:m
                vⱼ[ind] -= R[i,j] * qᵢ[ind]
            end
        end
        @inbounds R[j,j] = norm(vⱼ)
        # Q[:,j] = vⱼ / R[j,j]
        for ind = 1:m
            @inbounds Q[ind,j] = vⱼ[ind] / R[j,j]
        end
    end
    return nothing
end

@doc doc"""
    lyap_jetcoeffs!(t, x, dx, jac, varsaux)

Similar to [`jetcoeffs!`](@ref) for the calculation of the Lyapunov spectrum.
Updates *only* the elements of `x` which correspond to the solution of the 1st-order
variational equations ``\dot{\xi}=J \cdot \xi``, where ``J`` is the Jacobian
matrix, i.e., the linearization of the equations of motion. `jac` is the Taylor
expansion of ``J`` wrt the independent variable, around the current initial
condition. `varsaux` is an auxiliary array of type `Array{eltype(jac),3}` to
avoid allocations. Calling this method assumes that `jac` has been computed
previously using [`stabilitymatrix!`](@ref).

"""
function lyap_jetcoeffs!(t::Taylor1{T}, x::AbstractVector{Taylor1{S}},
        dx::AbstractVector{Taylor1{S}}, jac::Matrix{Taylor1{S}},
        varsaux::Array{Taylor1{S},3}) where {T <: Real, S <: Number}
    order = t.order
    # `dofrange` behaves like 1:dof, where `dof = size(jac, 1)`. Used to initialize `b` and `li`
    dofrange = axes(jac, 1)
    # The `view` below allows us to obtain CartesianIndices from `varsaux` when calling `eachindex(b)`
    # `varsaux` is an array with size n×n×n
    b = view(varsaux, dofrange, dofrange, dofrange)
    # We use `li` to map between cartesian and linear indices
    li = LinearIndices((dofrange, dofrange))

    # 0-th order evaluation of variational equations `dx = jac * x`
    # Initialize LHS of variational equations at zero
    dx .= zero(x[1])
    # Compute 0-th Taylor coefficients of matrix product `jac * x` and save into `dx`
    for i in eachindex(b)
        varsaux[i] = Taylor1(constant_term(jac[ i[1], i[3] ]) * constant_term(x[ li[i[3], i[2]] ]), order)
        lin_indx = li[ i[1], i[2]] # map from cartesian to linear index
        dx[lin_indx] = Taylor1(constant_term(dx[lin_indx]) + constant_term(varsaux[i]), order)
    end
    # Recursion relations, 0-th order
    for __idx = eachindex(x)
        (x[__idx]).coeffs[2] = (dx[__idx]).coeffs[1]
    end

    # Compute Taylor coefficients of variational equations `dx = jac * x` up to order `order`
    for ord = 1:order - 1
        ordnext = ord + 1
        # Compute `ord`-th Taylor coefficients of matrix product `jac * x` and save into `dx`
        for i in eachindex(b)
            TaylorSeries.mul!(varsaux[i], jac[ i[1], i[3] ], x[ li[i[3], i[2]] ], ord)
            lin_indx = li[ i[1], i[2]] # map from cartesian to linear index
            TaylorSeries.add!(dx[lin_indx], dx[lin_indx], varsaux[i], ord)
        end
        # Recursion relations, `ord`-th order
        for __idx = eachindex(x)
            (x[__idx]).coeffs[ordnext + 1] = (dx[__idx]).coeffs[ordnext] / ordnext
        end
    end

    return nothing
end

"""
    lyap_taylorstep!(f!, t, x, dx, xaux, δx, dδx, jac, t0, t1, order, abstol, _δv, varsaux, params[, jacobianfunc!])

Similar to [`taylorstep!`](@ref) for the calculation of the Lyapunov spectrum.
`jac` is the Taylor expansion (wrt the independent variable) of the
linearization of the equations of motion, i.e, the Jacobian. `xaux`, `δx`, `dδx`,
`varsaux` and `_δv` are auxiliary vectors, and `params` define the parameters
of the ODEs. Optionally, the user may provide a
Jacobian function `jacobianfunc!` to compute `jac`. Otherwise, `jac` is computed
via automatic differentiation using `TaylorSeries.jl`.

"""
function lyap_taylorstep!(f!, t::Taylor1{T}, x::Vector{Taylor1{U}},
        dx::Vector{Taylor1{U}}, xaux::Vector{Taylor1{U}},
        δx::Array{TaylorN{Taylor1{U}},1}, dδx::Array{TaylorN{Taylor1{U}},1},
        jac::Array{Taylor1{U},2}, t0::T, t1::T, order::Int,
        abstol::T, _δv::Vector{TaylorN{Taylor1{U}}}, varsaux::Array{Taylor1{U},3},
        params, parse_eqs::Bool=true, jacobianfunc! =nothing) where {T<:Real, U<:Number}

    # Dimensions of phase-space: dof
    nx = length(x)
    dof = length(δx)

    # Compute the Taylor coefficients associated to trajectory
    __jetcoeffs!(Val(parse_eqs), f!, t, view(x, 1:dof), view(dx, 1:dof), view(xaux, 1:dof), params)

    # Compute stability matrix
    stabilitymatrix!(f!, t, x, δx, dδx, jac, _δv, params, jacobianfunc!)

    # Compute the Taylor coefficients associated to variational equations
    lyap_jetcoeffs!(t, view(x, dof+1:nx), view(dx, dof+1:nx), jac, varsaux)

    # Compute the step-size of the integration using `abstol`
    δt = stepsize(view(x, 1:dof), abstol)
    δt = min(δt, t1-t0)

    return δt
end

"""
    lyap_taylorinteg(f!, q0, t0, tmax, order, abstol[, f!]; maxsteps::Int=500)

Similar to [`taylorinteg`](@ref) for the calculation of the Lyapunov
spectrum. Note that the number of `TaylorN` variables should be set
previously by the user (e.g., by means of `TaylorSeries.set_variables`) and
should be equal to the length of the vector of initial conditions `q0`.
Otherwise, whenever `length(q0) != TaylorSeries.get_numvars()`, then
`lyap_taylorinteg` throws an `AssertionError`. Optionally, the user may provide
a Jacobian function `jacobianfunc!` to evaluate the current value of the Jacobian.
Otherwise, the current value of the Jacobian is computed via automatic
differentiation using `TaylorSeries.jl`.

"""
function lyap_taylorinteg(f!, q0::Array{U,1}, t0::T, tmax::T,
        order::Int, abstol::T, params = nothing, jacobianfunc! =nothing;
        maxsteps::Int=500, parse_eqs::Bool=true) where {T<:Real, U<:Number}
    # Allocation
    tv = Array{T}(undef, maxsteps+1)
    dof = length(q0)
    xv = Array{U}(undef, dof, maxsteps+1)
    λ = similar(xv)
    λtsum = similar(q0)
    jt = Matrix{U}(I, dof, dof)
    _δv = Array{TaylorN{Taylor1{U}}}(undef, dof)

    # Initial conditions
    @inbounds tv[1] = t0
    @inbounds for ind in eachindex(q0)
        xv[ind,1] = q0[ind]
        λ[ind,1] = zero(U)
        λtsum[ind] = zero(U)
    end
    x0 = vcat(q0, reshape(jt, dof*dof))
    nx0 = length(x0)
    t00 = t0

    # Initialize the vector of Taylor1 expansions
    t = Taylor1(T, order)
    x = Array{Taylor1{U}}(undef, nx0)
    x .= Taylor1.( x0, order )
    @inbounds t[0] = t0

    # If user does not provide Jacobian, check number of TaylorN variables and initialize _δv
    if isa(jacobianfunc!, Nothing)
        @assert get_numvars() == dof "`length(q0)` must be equal to number of variables set by `TaylorN`"
        for ind in eachindex(q0)
            _δv[ind] = one(x[1])*TaylorN(Taylor1{U}, ind, order=1)
        end
    end

    #Allocate auxiliary arrays
    dx = Array{Taylor1{U}}(undef, nx0)
    xaux = Array{Taylor1{U}}(undef, nx0)
    δx = Array{TaylorN{Taylor1{U}}}(undef, dof)
    dδx = Array{TaylorN{Taylor1{U}}}(undef, dof)
    jac = Array{Taylor1{U}}(undef, dof, dof)
    varsaux = Array{Taylor1{U}}(undef, dof, dof, dof)
    fill!(jac, zero(x[1]))
    QH = Array{U}(undef, dof, dof)
    RH = Array{U}(undef, dof, dof)
    aⱼ = Array{U}(undef, dof )
    qᵢ = similar(aⱼ)
    vⱼ = similar(aⱼ)

    # Determine if specialized jetcoeffs! method exists
    parse_eqs = _determine_parsing!(parse_eqs, f!, t, view(x, 1:dof), view(dx, 1:dof), params)

    # Integration
    nsteps = 1
    while t0 < tmax
        δt = lyap_taylorstep!(f!, t, x, dx, xaux, δx, dδx, jac, t0, tmax,
            order, abstol, _δv, varsaux, params, parse_eqs, jacobianfunc!)
        evaluate!(x, δt, x0) # Update x0
        for ind in eachindex(jt)
            @inbounds jt[ind] = x0[dof+ind]
        end
        modifiedGS!( jt, QH, RH, aⱼ, qᵢ, vⱼ )
        t0 += δt
        @inbounds t[0] = t0
        tspan = t0-t00
        nsteps += 1
        @inbounds tv[nsteps] = t0
        @inbounds for ind in eachindex(q0)
            xv[ind,nsteps] = x0[ind]
            λtsum[ind] += log(RH[ind,ind])
            λ[ind,nsteps] = λtsum[ind]/tspan
        end
        for ind in eachindex(QH)
            @inbounds x0[dof+ind] = QH[ind]
        end
        x .= Taylor1.( x0, order )
        if nsteps > maxsteps
            @warn("""
            Maximum number of integration steps reached; exiting.
            """)
            break
        end
    end

    return view(tv,1:nsteps),  view(transpose(xv),1:nsteps,:),  view(transpose(λ),1:nsteps,:)
end

function lyap_taylorinteg(f!, q0::Array{U,1}, trange::AbstractVector{T},
        order::Int, abstol::T, params = nothing, jacobianfunc! = nothing;
        maxsteps::Int=500, parse_eqs::Bool=true) where {T<:Real, U<:Number}
    # Allocation
    nn = length(trange)
    dof = length(q0)
    xv = Array{U}(undef, dof, nn)
    fill!(xv, U(NaN))
    λ = Array{U}(undef, dof, nn)
    fill!(λ, U(NaN))
    λtsum = similar(q0)
    jt = Matrix{U}(I, dof, dof)
    _δv = Array{TaylorN{Taylor1{U}}}(undef, dof)

    # Initial conditions
    @inbounds for ind in eachindex(q0)
        xv[ind,1] = q0[ind]
        λ[ind,1] = zero(U)
        λtsum[ind] = zero(U)
    end

    # Initialize the vector of Taylor1 expansions
    t = Taylor1(T, order)
    x0 = vcat(q0, reshape(jt, dof*dof))
    q1 = similar(q0)
    nx0 = length(x0)
    x = Array{Taylor1{U}}(undef, nx0)
    x .= Taylor1.( x0, order )
    @inbounds t[0] = trange[1]
    @inbounds t0, t1, tmax = trange[1], trange[2], trange[end]
    t00 = trange[1]
    tspan = zero(T)

    # If user does not provide Jacobian, check number of TaylorN variables and initialize _δv
    if isa(jacobianfunc!, Nothing)
        @assert get_numvars() == dof "`length(q0)` must be equal to number of variables set by `TaylorN`"
        for ind in eachindex(q0)
            _δv[ind] = one(x[1])*TaylorN(Taylor1{U}, ind, order=1)
        end
    end

    #Allocate auxiliary arrays
    dx = Array{Taylor1{U}}(undef, nx0)
    xaux = Array{Taylor1{U}}(undef, nx0)
    δx = Array{TaylorN{Taylor1{U}}}(undef, dof)
    dδx = Array{TaylorN{Taylor1{U}}}(undef, dof)
    jac = Array{Taylor1{U}}(undef, dof, dof)
    varsaux = Array{Taylor1{U}}(undef, dof, dof, dof)
    fill!(jac, zero(x[1]))
    QH = Array{U}(undef, dof, dof)
    RH = Array{U}(undef, dof, dof)
    aⱼ = Array{U}(undef, dof )
    qᵢ = similar(aⱼ)
    vⱼ = similar(aⱼ)

    # Determine if specialized jetcoeffs! method exists
    parse_eqs = _determine_parsing!(parse_eqs, f!, t, view(x, 1:dof), view(dx, 1:dof), params)

    # Integration
    iter = 2
    nsteps = 1
    while t0 < tmax
        δt = lyap_taylorstep!(f!, t, x, dx, xaux, δx, dδx, jac, t0, tmax,
            order, abstol, _δv, varsaux, params, parse_eqs, jacobianfunc!)
        evaluate!(x, δt, x0) # Update x0
        tnext = t0+δt
        # # Evaluate solution at times within convergence radius
        while t1 < tnext
            evaluate!(x[1:dof], t1-t0, q1)
            @inbounds xv[:,iter] .= q1
            for ind in eachindex(jt)
                @inbounds jt[ind] = evaluate(x[dof+ind], δt)
            end
            modifiedGS!( jt, QH, RH, aⱼ, qᵢ, vⱼ )
            tspan = t1-t00
            @inbounds for ind in eachindex(q0)
                λ[ind,iter] = (λtsum[ind]+log(RH[ind,ind]))/tspan
            end
            iter += 1
            @inbounds t1 = trange[iter]
        end
        if δt == tmax-t0
            @inbounds xv[:,iter] .= x0[1:dof]
            for ind in eachindex(jt)
                @inbounds jt[ind] = x0[dof+ind]
            end
            modifiedGS!( jt, QH, RH, aⱼ, qᵢ, vⱼ )
            tspan = tmax-t00
            @inbounds for ind in eachindex(q0)
                λ[ind,iter] = (λtsum[ind]+log(RH[ind,ind]))/tspan
            end
            break
        end

        for ind in eachindex(jt)
            @inbounds jt[ind] = x0[dof+ind]
        end
        modifiedGS!( jt, QH, RH, aⱼ, qᵢ, vⱼ )

        t0 = tnext
        @inbounds t[0] = t0
        nsteps += 1
        @inbounds for ind in eachindex(q0)
            λtsum[ind] += log(RH[ind,ind])
        end
        for ind in eachindex(QH)
            @inbounds x0[dof+ind] = QH[ind]
        end
        x .= Taylor1.( x0, order )
        if nsteps > maxsteps
            @warn("""
            Maximum number of integration steps reached; exiting.
            """)
            break
        end
    end

    return transpose(xv), transpose(λ)
end


#! /usr/bin/julia

# Rosetta Code, Sorting algorithms/Bead sort

function beadsort{T<:Integer}(a::Array{T,1})
    (lo, hi) = extrema(a)
    if lo < 1
        throw(DomainError())
    end
    hi = convert(Int, hi)
    len = length(a)
    abacus = falses(len, hi)
    for (i, v) in enumerate(a)
           abacus[i,1:v] = true
    end
    for i in 1:hi
        v = sum(abacus[:,i])
        if v < len
            abacus[1:end-v,i] = false
            abacus[end-v+1:end,i] = true
        end
    end
    return T[sum(abacus[i,:]) for i in 1:len]
end

a = Uint8[rand(1:typemax(Uint8)) for i in 1:20]
println("Sort of Unsigned Bytes:")
println("    Before Sort:")
println("    ", a)
a = beadsort(a)
println("\n    After Sort:")
println("    ", a, "\n")

a = [rand(1:2^10) for i in 1:20]
println("Sort of Integers:")
println("    Before Sort:")
println("    ", a)
a = beadsort(a)
println("\n    After Sort:")
println("    ", a)


# Add Max Trust Angle Constraints

using SparseArrays, LinearAlgebra

include("../constraints/constraints.jl")

function getAlpha(theta, deg = true)
    if deg
        return -tand(theta)
    end
    return -tan(theta)
end

"""
    makeMaxThrustConstraint(NSteps::Int64, nDim::Int64, thetaMax::Float64)

Creates an [`AL_Multiple_simpleCone`](@ref) constraint such that the trust
(or equivalently control) vector angle is capped by `thetaMax`.

Usage: `NSteps` states the number of timesteps in the trajectory. `nDim` states
the dimensionality of the problem (e.g. 1D, 2D, or 3D). `thetaMax` is the max
thrust angle.
"""
function makeMaxAngleConstraint(NSteps::Int64, nDim::Int64, thetaMax::Float64,
                                    deg = true)
    # sizeTraj = 3 * NSteps * nDim + 2 * nDim

    # This helper function gets the index of ux for any k
    startUInd(k) = (2 * nDim + 1) + (3 * nDim * (k - 1))

    indicatorList = Int64[startUInd(k) for k in 1:NSteps]

    sInd = Diagonal([ones(Int64, nDim - 1); 0]) # e.g. Diagonal([1; 1; 0])
    tInd = [zeros(Int64, nDim - 1); 1]          # e.g. [0; 0; 1]

    alpha = getAlpha(thetaMax, deg)

    return makeAL_Multiple_AngleCone(alpha, sInd, tInd,
                                        nDim, indicatorList)
end

function makeMaxAngleConstraint(NSteps::Int64, nDim::Int64, thetaMax::Float64,
                                    sIndicate::Diagonal{Int64,Array{Int64,1}},
                                    tIndicate::Array{Int64,1}, deg = true)
    # sizeTraj = 3 * NSteps * nDim + 2 * nDim

    # This helper function gets the index of ux for any k
    startUInd(k) = (2 * nDim + 1) + (3 * nDim * (k - 1))

    indicatorList = Int64[startUInd(k) for k in 1:NSteps]

    alpha = getAlpha(thetaMax, deg)

    return makeAL_Multiple_simpleCone(alpha, sIndicate, tIndicate,
                                        nDim, indicatorList)
end


using DiscreteAdjoint, OrdinaryDiffEq, ForwardDiff
using Test

@testset "DiscreteAdjoint.jl" begin

    function f(du, u, p, t)
        du[1] = dx = p[1]*u[1] - p[2]*u[1]*u[2]
        du[2] = dy = -p[3]*u[2] + p[4]*u[1]*u[2]
    end
    p = [1.5,1.0,3.0,1.0]; tspan = (0.0, 10.0); u0 = [1.0,1.0];
    prob = ODEProblem(f, u0, tspan, p)
    sol = solve(prob, Tsit5(), u0=u0, p=p, abstol=1e-10, reltol=1e-10, tstops=0:0.1:10.0)

    dg(out,u,p,t,i) = out .= 1
    t = 0:0.1:10.0

    dp_fd, du0_fd = discrete_adjoint(sol, dg, t; autojacvec=ForwardDiffVJP())
    # dp = [8.30536  -159.484  75.2035  -339.195]
    # du0 = [-39.1275  -8.78778]

    dp_rd, du0_rd = discrete_adjoint(sol, dg, t; autojacvec=ReverseDiffVJP())
    # dp = [8.30536  -159.484  75.2035  -339.195]
    # du0 = [-39.1275  -8.78778]

    dp_rdc, du0_rdc = discrete_adjoint(sol, dg, t; autojacvec=ReverseDiffVJP(true))
    # dp = [8.30536  -159.484  75.2035  -339.195]
    # du0 = [-39.1275  -8.78778]

    dp_z, du0_z = discrete_adjoint(sol, dg, t; autojacvec=ZygoteVJP())
    # dp = [8.30536  -159.484  75.2035  -339.195]
    # du0 = [-39.1275  -8.78778]

    # forward AD solution
    function sum_of_solution(x)
        _prob = remake(prob, u0=x[1:2], p=x[3:end])
        sum(solve(_prob, Tsit5(), abstol=1e-10, reltol=1e-10, saveat=0.1))
    end

    dx = ForwardDiff.gradient(sum_of_solution,[u0;p])
    # dx = [-39.1275  -8.78778  8.30536  -159.484  75.2035  -339.195]

    @test isapprox(du0_fd, dx[1:2])
    @test isapprox(dp_fd, dx[3:end])

    @test isapprox(du0_rd, dx[1:2])
    @test isapprox(dp_rd, dx[3:end])

    @test isapprox(du0_rdc, dx[1:2])
    @test isapprox(dp_rdc, dx[3:end])

    @test isapprox(du0_z, dx[1:2])
    @test isapprox(dp_z, dx[3:end])

end


using Random
using MLDatasets

# download mnist train and test dataset
Xtrain, Ytrain = MNIST.traindata()
Xtest,  Ytest  = MNIST.testdata()

# construct a simple mlp network
topology = [784, 256, 256, 256, 128, 10]
operator = [relu, relu, relu, relu, relu]
mlpmodel = MLP(topology, operator)

# or equivalently construct a mlp like this:
mlpmodel = Chain(
    Dense(784,256,relu),
    Dense(256,256,relu),
    Dense(256,256,relu),
    Dense(256,128,relu),
    Dense(128, 10,relu)
)

# training function
function train(Xtrain, Ytrain, Xtest, Ytest, mlpmodel)
    parameter = paramsof(mlpmodel)
    optimizer = Adam(parameter;lr=1e-3,b1=0.9,b2=0.996)
    epoch     = 2
    batchsize = 32
    filesnums = size(Xtrain,3)
    batchnums = div(filesnums,batchsize)
    lossvalue = zeros(batchnums,1)
    idx   = collect(1:filesnums)
    loss  = zeros(epoch,1)
    accc  = zeros(epoch,1)
    for e = 1:epoch
        shuffle!(idx)
        for b = 1:batchnums
            shift = batchsize*(b-1)
            input = zeros(784,0)
            label = zeros(10,0)
            for i = 1:batchsize
                feats = convert.(Float64, reshape(Xtrain[:,:,idx[i+shift]],784,1))
                numbs = tolabel(Ytrain[idx[i+shift]])
                input = hcat(input,feats)
                label = hcat(label,numbs)
            end
            outs = forward(mlpmodel, Variable(input))
            prob = softmax(outs; dims=1)
            Cost = crossEntropyCost(prob, Variable(label))
            backward()
            if b%8==0
                update!(optimizer,parameter;clipvalue=50)
                zerograds!(parameter)
            end
            lossvalue[b] = Cost / batchsize
        end
        accc[e] = test(Xtest, Ytest, mlpmodel)
        loss[e] = sum(lossvalue) / batchnums
        println("epoch[$(e)]","loss: ",loss[e] , " accuracy: ",accc[e])
    end
    return loss,accc
end

# testing function
function test(Xtest, Ytest, mlpmodel)
    acc = 0.0
    num = size(Xtest,3)
    for i = 1:num
        feats = convert.(Float64, reshape(Xtest[:,:,i],784,1))
        label = convert(Int,Ytest[i]) + 1
        result = argmax(softmax(predict(mlpmodel, feats),dims=1))[1]
        acc += (label==result) ? 1.0 : 0.0
    end
    return acc/num
end

function tolabel(x)
    y = zeros(10,1)
    y[x+1] = 1.0
    return y
end

loss, acc = train(Xtrain, Ytrain, Xtest, Ytest, mlpmodel)


"""
    FixedHeatFlow(; name, Q_flow=1.0, T_ref=293.15, alpha=0.0)

Fixed heat flow boundary condition.

This model allows a specified amount of heat flow rate to be "injected" into a thermal system at a given port.
The constant amount of heat flow rate `Q_flow` is given as a parameter. The heat flows into the component to which
the component FixedHeatFlow is connected, if parameter `Q_flow` is positive.

# Parameters:
- `Q_flow`: [W] Fixed heat flow rate at port
- `T_ref`: [K] Reference temperature
- `alpha`: [1/K] Temperature coefficient of heat flow rate
"""
function FixedHeatFlow(; name, Q_flow=1.0, T_ref=293.15, alpha=0.0)
    pars = @parameters begin
        Q_flow=Q_flow
        T_ref=T_ref
        alpha=alpha
    end
    @named port = HeatPort()

    eqs = [
        port.Q_flow ~ -Q_flow * (1 + alpha * (port.T - T_ref))
    ]
    ODESystem(eqs, t, [], pars; systems=[port], name=name)
end

"""
    FixedTemperature(; name, T)

Fixed temperature boundary condition in kelvin.

This model defines a fixed temperature T at its port in kelvin, i.e., it defines a fixed temperature as a boundary condition.

# Parameters:
- `T`: [K] Fixed temperature boundary condition
"""
function FixedTemperature(; name, T)
    @named port = HeatPort()
    pars = @parameters T=T
    eqs = [
        port.T ~ T
    ]
    ODESystem(eqs, t, [], pars; systems=[port], name=name)
end


function update_sig2!(i::Int, s::State, c::Constants, d::Data)
  ss = 0.0

  cardinality = 0

  for j in 1:d.J
    for n in 1:d.N[i]
      k = s.lam[i][n]
      if k > 0
        z = s.Z[j, k]
        l = s.gam[i][n, j]
        ss += (s.y_imputed[i][n, j] - mus(z, l, s, c, d)) ^ 2
        cardinality += 1
      end
    end
  end

  newShape = shape(c.sig2_prior) + cardinality / (2 * c.temper)
  newScale = scale(c.sig2_prior) + ss / (2 * c.temper)

  if c.sig2_range == [0, Inf]
    s.sig2[i] = rand(InverseGamma(newShape, newScale))
  else
    lower, upper = c.sig2_range
    s.sig2[i] = rand_uptrunc_ig(InverseGamma(newShape, newScale), upper)
  end
end

function update_sig2!(s::State, c::Constants, d::Data)
  for i in 1:d.I
    update_sig2!(i, s, c, d)
  end
end


using MendelIHT
using SnpArrays
using DataFrames
using Distributions
using BenchmarkTools
using Random
using LinearAlgebra
using StatsFuns: logistic

function time_logistic_response(
    n :: Int64,        # number of cases
    p :: Int64,        # number of predictors (SNPs)
    k :: Int64,        # number of true predictors per group
    debias :: Bool
)
	Random.seed!(1111)

	#construct snpmatrix, covariate files, and true model b
	x, maf = simulate_random_snparray(n, p)
	xbm = SnpBitMatrix{Float64}(x, model=ADDITIVE_MODEL, center=true, scale=true);
	z = ones(n, 1) # non-genetic covariates, just the intercept
	true_b = zeros(p)
	true_b[1:k] = randn(k)
	shuffle!(true_b)
	correct_position = findall(x -> x != 0, true_b)

	#simulate bernoulli data
	y_temp = xbm * true_b
	prob = logistic.(y_temp) #inverse logit link
	y = [rand(Bernoulli(x)) for x in prob]
	y = Float64.(y)

    #free memory from xbm.... does this work like I think it works?
	xbm = nothing
	GC.gc()

	#time the result and return
    result = @timed L0_logistic_reg(x, z, y, 1, k, glm = "logistic", debias=debias, show_info=false, convg=true, init=false)
    iter = result[1].iter
    runtime = result[2]      # seconds
    memory = result[3] / 1e6 # MB

    return iter, runtime, memory
end

function main(n :: Int64, i :: Int64)
	p = 1000
	k = 10
	debias = false
	response = "logistic"

	#run result: first run on super small data to force compile, then run result
	iter, runtime, memory = time_logistic_response(100, 100, k, debias)
	iter, runtime, memory = time_logistic_response(n, p, k, debias)

	#write result to corresponding file
	if debias
		dir = response * "_results/"
	else
		dir = response * "_results_nodebias/"
	end
	file = string(n) * "_by_" * string(p) * "_run$i"
	open(dir * file, "w") do f
		write(f, "time (seconds), memory (MB), iteration\n")
		write(f, string(runtime) * ", " * string(memory) * ", " * string(iter))
	end
end

# arguments passed from command line
n = parse(Int64, ARGS[1])
i = parse(Int64, ARGS[2]) #i'th run: time result 5 times so we can average (didn't use benchmarktools cuz takes too long)

main(n, i)


# -*- encoding: utf-8 -*-
#
# The MIT License (MIT)
#
# Copyright © 2021 Matteo Foglieni and Riccardo Gervasoni
#

@doc raw"""
    scatter_ray(
        ::Type{DiffuseBRDF},
        pcg::PCG,
        incoming_dir::Vec,
        interaction_point::Point,
        normal::Normal,
        depth::Int64,
        ) :: Ray

Return a `Ray` scattered by a material with a `DiffuseBRDF`.

A `DiffuseBRDF` has a uniform BRDF, i.e.
``f_r(\mathbf{x}, \mathbf{\Psi}\rightarrow\mathbf{\Theta}) = \rho_d / \pi``;
the importance sampling for the `PathTracer` algorithm use consequently the
following PDF:

```math
p(\omega) \propto
    f_r(\mathbf{x}, \mathbf{\Psi}\rightarrow\mathbf{\Theta}) \, \cos(\vartheta)
    = \frac{\rho_d}{\pi} \, \cos(\vartheta)
    \propto \cos(\vartheta)
```
```math
    \Rightarrow \quad
p(\omega) = \frac{\cos(\vartheta)}{\pi}
    \quad \Rightarrow \quad
p(\vartheta ,\varphi) = \frac{\cos(\vartheta) \, \sin(\vartheta) }{2\pi}
```
```math
\Rightarrow \quad
\begin{aligned}
    &p(\vartheta) = 2 \, \cos(\vartheta) \, \sin(\vartheta) \\
    &p(\varphi | \vartheta) = \frac{1}{2 \pi}
\end{aligned}
```

See also: [`DiffuseBRDF`](@ref), [`Ray`](@ref), [`Vec`](@ref),
[`Point`](@ref), [`Normal`](@ref), [`PCG`](@ref)
"""
function scatter_ray(
            ::Type{DiffuseBRDF},
            pcg::PCG,
            incoming_dir::Vec,
            interaction_point::Point,
            normal::Normal,
            depth::Int64,
            )

    e1, e2, e3 = create_onb_from_z(normal)
    cos_θ_sq = random(pcg)
    cos_θ, sin_θ = √(cos_θ_sq) , √(1.0 - cos_θ_sq)
    ϕ = 2.0 * pi * random(pcg)

    return Ray(
                interaction_point,
                e1*cos(ϕ)*cos_θ + e2*sin(ϕ)*cos_θ + e3*sin_θ,
                1.0e-3,   # tmin, be generous here
                Inf,
                depth
            )
end

@doc raw"""
    scatter_ray(
        ::Type{SpecularBRDF},
        pcg::PCG,
        incoming_dir::Vec,
        interaction_point::Point,
        normal::Normal,
        depth::Int64,
        ) :: Ray

Return a `Ray` scattered by a material with a `SpecularBRDF`.

A `SpecularBRDF` has a Dirac delta BRDF, i.e.:

```math
f_r(\mathbf{x}, \mathbf{\Psi} \rightarrow \mathbf{\Theta})
    \propto
\frac{\delta(\sin^2\theta_r - \sin^2\theta) \,
    \delta(\psi_r \pm \pi - \psi)}{\cos\theta},
```

The importance sampling for the `PathTracer` algorithm use consequently the
following PDF:

```math
p(\omega) \propto
    f_r(\mathbf{x}, \mathbf{\Psi}\rightarrow\mathbf{\Theta}) \, \cos(\vartheta)
    \propto \frac{1}{\cos(\vartheta)} \, \cos(\vartheta)
    \propto cost
```

See also: [`SpecularBRDF`](@ref), [`Ray`](@ref), [`Vec`](@ref),
[`Point`](@ref), [`Normal`](@ref), [`PCG`](@ref)
"""
function scatter_ray(
            ::Type{SpecularBRDF},
            pcg::PCG,
            incoming_dir::Vec,
            interaction_point::Point,
            normal::Normal,
            depth::Int64,
            )
    #ray_dir = normalize(Vec(incoming_dir.x, incoming_dir.y, incoming_dir.z))
    ray_dir = normalize(incoming_dir)

    return Ray(
                interaction_point,
                ray_dir - normal*2.0*(Vec(normal) ⋅ ray_dir),
                1e-3,
                Inf,
                depth
            )
end

#=
scatter_ray(
            b::BRDF,
            pcg::PCG,
            incoming_dir::Vec,
            interaction_point::Point,
            normal::Normal,
            depth::Int64,
        ) =
        scatter_ray(
            typeof(b),
            pcg,
            incoming_dir,
            interaction_point,
            normal,
            depth,
        )
=#


export left_env

function LinearAlgebra.dot(ϕ::CuMPS, ψ::CuMPS)
    C = CUDA.ones(eltype(ψ), 1, 1)

    for i ∈ eachindex(ψ)
        M = ψ[i]
        M̃ = conj.(ϕ[i])
        @cutensor C[x, y] := M̃[β, σ, x] * C[β, α] * M[α, σ, y] order = (α, β, σ)
    end
    return tr(C)
end

function left_env(ϕ::CuMPS, ψ::CuMPS)
    l = length(ψ)
    S = promote_type(eltype(ψ), eltype(ϕ))

    L = Vector{CuMatrix{S}}(undef, l+1)
    L[1] = CUDA.ones(eltype(ψ), 1, 1)

    for i ∈ 1:l
        M = ψ[i]
        M̃ = conj.(ϕ[i])

        C = L[i]
        @cutensor C[x, y] := M̃[β, σ, x] * C[β, α] * M[α, σ, y] order = (α, β, σ)
        L[i+1] = C
    end
    return L
end

function right_env(ϕ::CuMPS, ψ::CuMPS)
    L = length(ψ)
    T = promote_type(eltype(ψ), eltype(ϕ))

    R = Vector{CuMatrix{T}}(undef, L+1)
    R[end] = ones(eltype(ψ), 1, 1)

    for i ∈ L:-1:1
        M = ψ[i]
        M̃ = conj.(ϕ[i])

        D = R[i+1]
        @cutensor D[x, y] := M[x, σ, α] * D[α, β] * M̃[y, σ, β] order = (β, α, σ)
        R[i] = D
    end
    return R
end

function LinearAlgebra.dot(ϕ::CuMPS, O::Vector{<:CuMatrix}, ψ::CuMPS)
    S = promote_type(eltype(ψ), eltype(ϕ), eltype(O[1]))
    C = CUDA.ones(S, 1, 1)

    for i ∈ eachindex(ψ)
        M = ψ[i]
        M̃ = conj.(ϕ[i])
        Mat = O[i]
        @cutensor C[x, y] := M̃[β, σ, x] * Mat[σ, η] * C[β, α] * M[α, η, y] order = (α, η, β, σ)
    end
    return tr(C)
end


# Run tests for the inner solves

using NonlinearEigenproblemsTest
using NonlinearEigenproblems
using Test,Random
using LinearAlgebra

Random.seed!(0);
@bench @testset "Inner Solves" begin
    dep=nep_gallery("dep0",200);
    n=size(dep,1);
    Random.seed!(0);

    nn=opnorm(compute_Mder(dep,0));
    errmeasure= (λ,v) -> norm(compute_Mlincomb(dep,λ,v))/nn;

    pnep=create_proj_NEP(dep);

    k = 5
    Q,R=qr(randn(n,k));
    Q = Matrix(Q)
    set_projectmatrices!(pnep,Q,Q)

    logger=ErrorLogger(50,100,0);
    λv,V = inner_solve(DefaultInnerSolver(), ComplexF64, pnep; σ=0.0+0im, neigs=3, tol=eps()*100, inner_logger=logger)
    verify_lambdas(3, pnep, λv, V,eps()*10000)
    E=logger.errs[:,1]; E=E[(!isnan).(E)];

    @test length(E) > 5 #Has done more than 5 iterations
    @test E[end] <eps()*10000 # Error measure fulfills stopping criteria

    logger=ErrorLogger(1,100,0);
    λv,V = inner_solve(NewtonInnerSolver(), ComplexF64, pnep; λv=[0.0,1.0] .+ 0im, V=ones(k, 2), tol=eps()*100, inner_logger=logger)
    verify_lambdas(2, pnep, λv, V,eps()*10000)
    E=logger.errs[:,1]; E=E[(!isnan).(E)];
    @test length(E) > 5 #Has done more than 5 iterations
    @test E[end] <eps()*10000 # Error measure fulfills stopping criteria

    #λv,V=inner_solve(SGIterInnerSolver(),pnep,λv=[0.0],j=1);
    #verify_lambdas(2, pnep, λv, V,eps()*10000)

    λv,V = inner_solve(IARChebInnerSolver(), ComplexF64, pnep; λv=[0,1,2,3] .+ 0.0im)
    verify_lambdas(10, pnep, λv, V, sqrt(eps()))

    # TODO: this test results in a "Rank drop" warning, and a third eigenvalue that's not converged
    λv,V = inner_solve(ContourBeynInnerSolver(), ComplexF64, pnep; λv=[0,1] .+ 0.0im, neigs=3)
    # @test minimum(svdvals(compute_Mder(pnep,λv[1]))) < eps()*100
    verify_lambdas(2, pnep, λv[1:2], V[:,1:2], sqrt(eps()))

    λv,V = inner_solve(NleigsInnerSolver(), ComplexF64, pnep; λv=[0,1,2,3] .+ 0.0im)
    @test size(λv,1) >= 5;
end


# =========================================================================== #
# Test
# =========================================================================== #

function test_norm()

    a = [10.0 11; 5 2]
    println(a)

    println( linalg.norm_L1(a) )
    println( linalg.norm_Linf(a) )

    println( linalg.norm(a, :1) )
    println( linalg.norm(a, :inf) )

end

# =========================================================================== #
# Benchmark
# =========================================================================== #


# Regularizer

abstract Regularizer

## generic methods

value_and_grad{T<:AbstractFloat}(f::Regularizer, θ::StridedArray{T}) = value_and_addgrad!(f, zero(T), similar(θ), one(T), θ)

prox!(f::Regularizer, θ::StridedArray) = prox!(f, θ, θ)

prox(f::Regularizer, θ::StridedArray) = prox!(f, similar(θ), θ, 1.0)
prox(f::Regularizer, θ::StridedArray, λ::Real) = prox!(f, similar(θ), θ, λ)

function value_and_grad!{T<:AbstractFloat,N}(rm::RiskModel, reg::Regularizer, g::StridedArray{T,N}, θ::StridedArray{T,N}, X, y)
    v_risk, _ = value_and_addgrad!(rm, zero(T), g, one(T), θ, X, y)
    v_regr, _ = value_and_addgrad!(reg, one(T), g, one(T), θ)
    return (v_risk + v_regr, g)
end

## ZeroReg: 0

immutable ZeroReg <: Regularizer
end

value{T<:Real}(::ZeroReg, θ::StridedArray{T}) = zero(T)

value_and_addgrad!{T<:Real,N}(::ZeroReg, β::T, g::StridedArray{T,N}, α::T, θ::StridedArray{T,N}) =
    (zero(T), g)

prox!{T<:Real}(::ZeroReg, r::StridedArray{T}, θ::StridedArray{T}, λ::Real) =
    copy!(r, θ)

## SqrL2Reg: (c/2) * ||θ||_2^2

immutable SqrL2Reg{T<:AbstractFloat} <: Regularizer
    c::T
end

SqrL2Reg{T<:AbstractFloat}(c::T) = SqrL2Reg{T}(c)

value{T<:BlasReal}(f::SqrL2Reg{T}, θ::StridedArray{T}) = half(f.c * sumabs2(θ))

function value_and_addgrad!{T<:BlasReal,N}(f::SqrL2Reg{T}, β::T, g::StridedArray{T,N}, α::T, θ::StridedArray{T,N})
    @_checkdims length(g) == length(θ)
    c = f.c * α
    v = zero(T)
    if β == zero(T)
        @inbounds for i in eachindex(θ)
            θ_i = θ[i]
            v += abs2(θ_i)
            g[i] = c * θ_i
        end
    else
        @inbounds for i in eachindex(θ)
            θ_i = θ[i]
            v += abs2(θ_i)
            g[i] = β * g[i] + c * θ_i
        end
    end
    (half(c * v), g)
end

function prox!{T<:BlasReal}(f::SqrL2Reg{T}, r::StridedArray{T}, θ::StridedArray{T}, λ::Real)
    c = convert(T, λ) * f.c
    scale!(r, θ, one(T) / (one(T) + c))
end

## L1Reg: c * ||θ||_1

immutable L1Reg{T<:AbstractFloat} <: Regularizer
    c::T
end

L1Reg{T<:AbstractFloat}(c::T) = L1Reg{T}(c)

value{T<:BlasReal}(f::L1Reg{T}, θ::StridedArray{T}) = f.c * sumabs(θ)

function value_and_addgrad!{T<:BlasReal,N}(f::L1Reg{T}, β::T, g::StridedArray{T,N}, α::T, θ::StridedArray{T,N})
    @_checkdims length(g) == length(θ)
    c = f.c * α
    v = zero(T)
    if β == zero(T)
        for i in eachindex(θ)
            θ_i = θ[i]
            v += abs(θ_i)
            @inbounds g[i] = c * sign(θ_i)
        end
    else
        for i in eachindex(θ)
            θ_i = θ[i]
            v += abs(θ_i)
            @inbounds g[i] = β * g[i] + c * sign(θ_i)
        end
    end
    (c * v, g)
end

function prox!{T<:BlasReal}(f::L1Reg{T}, r::StridedArray{T}, θ::StridedArray{T}, λ::Real)
    @_checkdims size(r) == size(θ)
    c = convert(T, λ) * f.c
    for i in eachindex(θ)
        r[i] = shrink(θ[i], c)
    end
    r
end

## ElasticNet: c1 * ||θ||_1 + c2 * ||θ||_2^2

immutable ElasticReg{T<:AbstractFloat} <: Regularizer
    c1::T
    c2::T
end

ElasticReg{T<:AbstractFloat}(c1::T, c2::T) = ElasticReg{T}(c1, c2)

function value{T<:BlasReal}(f::ElasticReg{T}, θ::StridedArray{T})
    s = zero(T)
    c1 = f.c1
    c2_h = half(f.c2)
    @inbounds for i in eachindex(θ)
        θ_i = θ[i]
        s += c1 * abs(θ_i) + c2_h * abs2(θ_i)
    end
    s
end

function value_and_addgrad!{T<:BlasReal,N}(f::ElasticReg{T}, β::T, g::StridedArray{T,N}, α::T, θ::StridedArray{T,N})
    @_checkdims length(g) == length(θ)
    c1 = f.c1 * α
    c2 = f.c2 * α

    v1 = zero(T)
    v2 = zero(T)
    if β == zero(T)
        @inbounds for i in eachindex(θ)
            θ_i = θ[i]
            v1 += abs(θ_i)
            v2 += abs2(θ_i)
            g[i] = c1 * sign(θ_i) + c2 * θ_i
        end
    else
        @inbounds for i in eachindex(θ)
            θ_i = θ[i]
            v1 += abs(θ_i)
            v2 += abs2(θ_i)
            g[i] = β * g[i] + (c1 * sign(θ_i) + c2 * θ_i)
        end
    end
    (c1 * v1 + half(c2 * v2), g)
end

function prox!{T<:BlasReal}(f::ElasticReg{T}, r::StridedArray{T}, θ::StridedArray{T}, λ::Real)
    @_checkdims size(r) == size(θ)
    c1 = convert(T, λ) * f.c1
    c2 = convert(T, λ) * f.c2
    c = one(T) / (one(T) + c2)
    t = c1 / (one(T) + c2)
    @inbounds for i in eachindex(θ)
        r[i] = shrink(c * θ[i], t)
    end
    r
end

## NonNegReg: θ[i] >= 0 ∀ i
immutable NonNegReg <: Regularizer
end

function value{T<:Real}(::NonNegReg, θ::StridedArray{T})
    s = zero(T)
    @inbounds for i in eachindex(θ)
        θ[i] < s && return Inf      # value = + Inf if outside domain
    end
    s   # value = 0. otherwise
end

value_and_addgrad!{T<:Real,N}(::NonNegReg, β::T, g::StridedArray{T,N}, α::T, θ::StridedArray{T,N}) =
    error("Gradient undefined for the NonNegReg regularizer")

#  NOTE : λ is ignored
function prox!{T<:Real}(::NonNegReg, r::StridedArray{T}, θ::StridedArray{T}, λ::Real)
    @_checkdims size(r) == size(θ)
    c = convert(T, λ)
    s = zero(T)
    for i in eachindex(θ)
        r[i] = θ[i] < s ? s : θ[i]
    end
    r
end

## Simplex: Σ θ[i] = c, θ[i] >= 0 ∀ i
immutable SimplexReg{T<:AbstractFloat} <: Regularizer
    c::T
end

SimplexReg{T<:AbstractFloat}(c::T=1.0) = SimplexReg{T}(c)

function value{T<:BlasReal}(f::SimplexReg{T}, θ::StridedArray{T})
    z = zero(T)
    s = zero(T)
    @inbounds for i in eachindex(θ)
        θ[i] < z && return Inf       # value = + Inf if outside domain
        s += θ[i]
    end

    return abs(s - f.c) <= 1.0e-12 ? z : Inf
end

value_and_addgrad!{T<:BlasReal,N}(f::SimplexReg{T}, β::T, g::StridedArray{T,N}, α::T, θ::StridedArray{T,N}) =
    error("Gradient undefined for the SimplexReg regularizer")

# Source = Efficient Projections on the l1-Ball for Learning in High Dimensions - J.Duchi
# Note : λ is ignored
function prox!{T<:BlasReal}(f::SimplexReg{T}, r::StridedArray{T}, θ::StridedArray{T}, λ::Real)
    @_checkdims size(r) == size(θ)

    z  = zero(T)
    ρ, s = 0, z
    U  = [1:length(θ);]
    cU = length(θ)

    # pivot search by divide and conquer
    while cU > 0
        k  = U[rand(1:cU)]  # pick an element at random
        vk = θ[k]
        dρ, ds = 0, z
        @inbounds for i in 1:cU
            j = U[i]
            θ[j] >= vk || continue
            dρ += 1
            ds += θ[j]
        end

        if (s+ds) - (ρ+dρ)*vk < f.c
            s += ds
            ρ += dρ
            nCu = 0
            @inbounds for i in 1:cU
                j = U[i]
                θ[j] < vk || continue
                nCu += 1
                U[nCu] = U[i]
            end
            cU = nCu
        else
            nCu = 0
            @inbounds for i in 1:cU
                j = U[i]
                θ[j] >= vk || continue
                j != k     || continue
                nCu += 1
                U[nCu] = U[i]
            end
            cU = nCu
        end
    end

    # apply pivot
    @inbounds for i in eachindex(θ)
        r[i]  = max( θ[i] - (s - f.c)/ρ, z )
    end
    r

    r
end

## L1Ball: Σ |θ[i]| = c
immutable L1BallReg{T<:AbstractFloat} <: Regularizer
    c::T
end

L1BallReg{T<:AbstractFloat}(c::T) = L1BallReg{T}(c)

function value{T<:BlasReal}(f::L1BallReg{T}, θ::StridedArray{T})
    z = zero(T)
    s = zero(T)
    @inbounds for i in eachindex(θ)
        s += abs(θ[i])
        s > f.c && return Inf
    end

    return z
end

value_and_addgrad!{T<:BlasReal,N}(f::L1BallReg{T}, β::T, g::StridedArray{T,N}, α::T, θ::StridedArray{T,N}) =
    error("Gradient undefined for the L1BallReg regularizer")

# Source = Efficient Projections on the l1-Ball for Learning in High Dimensions - J.Duchi
# Note : λ is ignored
function prox!{T<:BlasReal}(f::L1BallReg{T}, r::StridedArray{T}, θ::StridedArray{T}, λ::Real)
    @_checkdims size(r) == size(θ)

    z  = zero(T)
    ρ, s = 0, z
    U  = [1:length(θ);]
    cU = length(θ)

    # early exit if within limits
    if sumabs(θ) <= f.c
        copy!(r, θ)
        return r
    end

    # pivot search by divide and conquer
    while cU > 0
        k  = U[rand(1:cU)]
        vk = abs(θ[k])
        dρ, ds = 0, z
        @inbounds for i in 1:cU
            j = U[i]
            abs(θ[j]) >= vk || continue
            dρ += 1
            ds += abs(θ[j])
        end

        if (s+ds) - (ρ+dρ)*vk < f.c
            s += ds
            ρ += dρ
            nCu = 0
            @inbounds for i in 1:cU
                j = U[i]
                abs(θ[j]) < vk || continue
                nCu += 1
                U[nCu] = U[i]
            end
            cU = nCu
        else
            nCu = 0
            @inbounds for i in 1:cU
                j = U[i]
                abs(θ[j]) >= vk || continue
                j != k || continue
                nCu += 1
                U[nCu] = U[i]
            end
            cU = nCu
        end
    end

    # apply pivot
    @inbounds for i in eachindex(θ)
        r[i]  = sign(θ[i]) * max( abs(θ[i]) - (s - f.c)/ρ, z )
    end
    r
end


@testset "macro for generating fast functions" begin
	ε = 100*eps(Float64)
	f(x1,x2,x3) = 3*x3^2 + 2*x2*x1
	@polyvar x y z
	polynomial = 3*z^2 + 2*y*x
	g = @fastfunc polynomial

	t1,t2,t3 = randn(3)
	@test isapprox(f(t1,t2,t3),g(t1,t2,t3),atol=ɛ)
end


# Simple control problem
# Code was initially written by Jenny Hong for EE103
# Translated to lsqpy by Keegan Go
# Written in LinearLeastSquares.jl by Karanveer Mohan and David Zeng
#
# In this control problem, the object starts from the origin

using LinearLeastSquares
using Gadfly

# Some constraints on our motion
# The object should start from the origin, and end at rest
initial_velocity = [-20; 100]
final_position = [100; 100]

T = 100 # The number of timesteps
h = 0.1 # The time between time intervals
mass = 1 # Mass of object
drag = 0.1 # Drag on object
g = [0, -9.8] # Gravity on object

# Declare the variables we need
position = Variable(2, T)
velocity = Variable(2, T)
force = Variable(2, T - 1)

# Create a problem instance
mu = 1
constraints = []

# Add constraints on our variables
for i in 1 : T - 1
  constraints += position[:, i + 1] == position[:, i] + h * velocity[:, i]
end

for i in 1 : T - 1
  acceleration = force[:, i]/mass + g - drag * velocity[:, i]
  constraints += velocity[:, i + 1] == velocity[:, i] + h * acceleration
end

# Add position constraints
constraints += position[:, 1] == 0
constraints += position[:, T] == final_position

# Add velocity constraints
constraints += velocity[:, 1] == initial_velocity
constraints += velocity[:, T] == 0

# Solve the problem
optval = minimize!(sum_squares(force), constraints)

pos = evaluate(position)
p = plot(
  layer(x=[pos[1, 1]], y=[pos[2, 1]], Geom.point, Theme(default_color=color("blue"))),
  layer(x=[pos[1, T]], y=[pos[2, T]], Geom.point, Theme(default_color=color("green"))),
  layer(x=pos[1, :], y=pos[2, :], Geom.line(preserve_order=true)),
  Theme(panel_fill=color("white"))
)
#draw(PNG("position.png", 16cm, 12cm), p)

p = plot(x=1:T, y=sum(evaluate(force).^2, 1), Geom.line, Theme(panel_fill=color("white")))
#draw(PNG("force.png", 16cm, 12cm), p)


using CayleyDickson
using Base.Test: @test, @test_throws

@test begin
    a = CayleyDickson.randomBigInt()
    b = CayleyDickson.randomBigInt()
    l = Exo1SplitBinion(Exo1Real(a, b))
    r = Exo1SplitBinion(a, 0, b, 0)
    l == r
end

@test begin
    a = CayleyDickson.randomBigFloat()
    isreal(Exo1SplitBinion(a))
end

@test begin
    a = 1
    !isreal(Exo1SplitBinion(a, a, a, a))
end

@test begin
    z = random(Exo1SplitBinion{Int})
    z == +(z)
end

@test begin
    a = 1
    real(Exo1SplitBinion(a)) == a
end

@test begin
    a = 1
    b = 2
    c = 3
    l = unreal(Exo1SplitBinion(0, a, b, c))
    r = [a, b, c]
    l == r
end

@test begin
    io = IOBuffer()
    show(io, Exo1SplitBinion(1, 2, 3, 4))
    l = String(take!(io))
    r = "[1: 1, s: 2, a: 3, sa: 4]"
    l == r
end

@test begin
    a = CayleyDickson.randomBigInt()
    b = CayleyDickson.randomBigFloat()
    l = Exo1SplitBinion(SplitBinion(a, b))
    r = Exo1SplitBinion(a, b)
    l == r
end

@test begin
    a = CayleyDickson.randomBigInt()
    b = CayleyDickson.randomBigFloat()
    c = CayleyDickson.randomBigInt()
    l = Exo1SplitBinion(a, b, c)
    r = Exo1SplitBinion(a, b, c, 0)
    l == r
end

@test begin
    x = random(Exo1SplitBinion{BigFloat})
    y = random(Exo1SplitBinion{BigFloat})
    x != y
end

@test begin
    a = CayleyDickson.randomBigInt()
    z = random(Exo1SplitBinion{BigInt})
    l = a + z
    r = z + a
    l == r
end

@test begin
    a = random(SplitBinion{BigInt})
    z = random(Exo1SplitBinion{BigInt})
    l = a + z
    r = z + a
    l == r
end

@test begin
    a = CayleyDickson.randomBigInt()
    z = random(Exo1SplitBinion{BigInt})
    l = a - z
    r = -(z - a)
    l == r
end

@test begin
    a = random(SplitBinion{BigInt})
    z = random(Exo1SplitBinion{BigInt})
    l = a - z
    r = -(z - a)
    l == r
end

@test begin
    x = random(Exo1SplitBinion{BigInt})
    y = random(Exo1SplitBinion{BigInt})
    l = x + y
    r = y + x
    l == r
end

@test begin
    x = random(Exo1SplitBinion{BigInt})
    y = random(Exo1SplitBinion{BigInt})
    l = x * y
    r = y * x
    l != r
end

@test begin
    x = random(Exo1SplitBinion{BigInt})
    y = random(Exo1SplitBinion{BigInt})
    z = random(Exo1SplitBinion{BigInt})
    l = associator(x, y, z)
    r = zero(Exo1SplitBinion{BigInt})
    l == r
end

@test begin
    x = random(Exo1SplitBinion{BigInt})
    y = random(Exo1SplitBinion{BigInt})
    l = x - y
    r = -(y - x)
    l == r
end

@test begin
    x = random(Exo1SplitBinion{BigInt})
    y = random(Exo1SplitBinion{BigInt})
    z = random(Exo1SplitBinion{BigInt})
    l = (x + y) + z
    r = x + (y + z)
    l == r
end

@test begin
    x = random(Exo1SplitBinion{BigInt})
    y = random(Exo1SplitBinion{BigInt})
    z = random(Exo1SplitBinion{BigInt})
    l = (x * y) * z
    r = x * (y * z)
    l == r
end

@test begin
    x = random(Exo1SplitBinion{BigInt})
    conj(conj(x)) == x
end

@test begin
    x = random(Exo1SplitBinion{Rational{BigInt}})
    inv(inv(x)) == x
end

@test begin
    x = random(Exo1SplitBinion{BigInt})
    y = random(Exo1SplitBinion{BigInt})
    l = conj(x * y)
    r = conj(y) * conj(x)
    l == r
end

@test begin
    x = random(Exo1SplitBinion{BigInt})
    y = random(Exo1SplitBinion{BigInt})
    l = conj(x + y)
    r = conj(x) + conj(y)
    l == r
end

@test begin
    x = random(Exo1SplitBinion{BigInt})
    y = random(Exo1SplitBinion{BigInt})
    l = conj(x - y)
    r = conj(x) - conj(y)
    l == r
end

@test begin
    x = random(Exo1SplitBinion{Rational{BigInt}})
    y = random(Exo1SplitBinion{Rational{BigInt}})
    l = inv(x * y)
    r = inv(y) * inv(x)
    l == r
end

@test begin
    x = random(Exo1SplitBinion{Rational{BigInt}})
    y = random(Exo1SplitBinion{Rational{BigInt}})
    l = inv(x / y)
    r = y / x
    l == r
end

@test begin
    x = random(Exo1SplitBinion{Rational{BigInt}})
    y = random(Exo1SplitBinion{Rational{BigInt}})
    l = inv(x \ y)
    r = y \ x
    l == r
end

@test begin
    x = random(Exo1SplitBinion{BigInt})
    y = random(Exo1SplitBinion{BigInt})
    z = random(Exo1SplitBinion{BigInt})
    l = x * (y + z)
    r = (x * y) + (x * z)
    l == r
end

@test begin
    a = CayleyDickson.randomBigInt()
    x = random(Exo1SplitBinion{BigInt})
    y = random(Exo1SplitBinion{BigInt})
    l = a * (x + y)
    r = (a * x) + (a * y)
    l == r
end

@test begin
    a = CayleyDickson.randomBigInt()
    x = random(Exo1SplitBinion{BigInt})
    l = a * x
    r = x * a
    l == r
end

@test begin
    a = random(SplitBinion{BigInt})
    x = random(Exo1SplitBinion{BigInt})
    l = a * x
    r = x * a
    l != r
end

@test begin
    x = random(Exo1SplitBinion{BigInt})
    y = random(Exo1SplitBinion{BigInt})
    z = random(Exo1SplitBinion{BigInt})
    l = (x + y) * z
    r = (x * z) + (y * z)
    l == r
end

@test begin
    x = random(Exo1SplitBinion{BigInt})
    y = random(Exo1SplitBinion{BigInt})
    z = random(Exo1SplitBinion{BigInt})
    l = x * (y - z)
    r = (x * y) - (x * z)
    l == r
end

@test begin
    x = random(Exo1SplitBinion{BigInt})
    y = random(Exo1SplitBinion{BigInt})
    z = random(Exo1SplitBinion{BigInt})
    l = (x - y) * z
    r = (x * z) - (y * z)
    l == r
end

@test begin
    x = random(Exo1SplitBinion{BigInt})
    quadrance(x) > big(0) || quadrance(x) < big(0) ||quadrance(x) == big(0)
end

@test begin
    x = random(Exo1SplitBinion{BigInt})
    y = random(Exo1SplitBinion{BigInt})
    l = quadrance(x * y)
    r = quadrance(x) * quadrance(y)
    l == r
end

@test_throws ErrorException begin
    inv(Exo1SplitBinion(0, 0.0))
end

@test_throws ErrorException begin
    inv(Exo1SplitBinion(0, 0, 1, 1))
end

@test_throws ErrorException begin
    inv(Exo1SplitBinion(1, 1, 2, 1))
end

@test_throws ErrorException begin
    random(Exo1SplitBinion{Int}) / 0
end

@test_throws ErrorException begin
    0 \ random(Exo1SplitBinion{Int})
end

@test begin
    a = CayleyDickson.randomBigRational()
    z = random(Exo1SplitBinion{Rational{BigInt}})
    l = z / a
    r = a \ z
    l == r
end

@test begin
    a = CayleyDickson.randomBigRational()
    z = random(Exo1SplitBinion{Rational{BigInt}})
    l = a / z
    r = z \ a
    l == r
end

@test begin
    z = random(Exo1SplitBinion{BigInt})
    z == z + zero(Exo1SplitBinion{BigInt})
end

@test begin
    z = random(Exo1SplitBinion{BigInt})
    z == z * one(Exo1SplitBinion{BigInt})
end

@test begin
    z = random(Exo1SplitBinion{BigInt})
    z == z + zero(z)
end

@test begin
    z = random(Exo1SplitBinion{BigInt})
    z == z * one(z)
end


module SweepOperator
export sweep!

using LinearAlgebra
import LinearAlgebra: BlasFloat, checksquare

const AMat = AbstractMatrix
const AVec = AbstractVector

"""
    sweep!(A, k ; inv=false)
    sweep!(A, ks; inv=false)

Perform the sweep operation (or inverse sweep if `inv=true`) on matrix `A` on element `k`
(or each element in `ks`).  Only the upper triangle is read/swept.

# Example:

    x = randn(100, 10)
    xtx = x'x
    sweep!(xtx, 1)
    sweep!(xtx, 1, true)
"""
function sweep!(A::AMat, k::Integer, inv::Bool = false)
    sweep_with_buffer!(Vector{eltype(A)}(undef, size(A, 2)), A, k, inv)
end

function sweep_with_buffer!(akk::AVec{T}, A::AMat{T}, k::Integer, inv::Bool = false) where
        {T<:BlasFloat}
    # ensure @inbounds is safe
    p = checksquare(A)
    p == length(akk) || throw(DimensionError("incorrect buffer size"))
    @inbounds d = one(T) / A[k, k]  # pivot
    # get column A[:, k] (hack because only upper triangle is available)
    for j in 1:k
        @inbounds akk[j] = A[j, k]
    end
    for j in (k+1):p
        @inbounds akk[j] = A[k, j]
    end
    BLAS.syrk!('U', 'N', -d, akk, one(T), A)  # everything not in col/row k
    rmul!(akk, d * (-one(T)) ^ inv)
    for i in 1:(k-1)  # col k
        @inbounds A[i, k] = akk[i]
    end
    for j in (k+1):p  # row k
        @inbounds A[k, j] = akk[j]
    end
    @inbounds A[k, k] = -d  # pivot element
    A
end

function sweep!(A::AMat{T}, ks::AVec{I}, inv::Bool = false) where {T<:BlasFloat, I<:Integer}
    akk = zeros(T, size(A, 1))
    for k in ks
        sweep_with_buffer!(akk, A, k, inv)
    end
    A
end
function sweep_with_buffer!(akk::AVec{T}, A::AMat{T}, ks::AVec{I}, inv::Bool = false) where
        {T<:BlasFloat, I<:Integer}
    for k in ks
        sweep_with_buffer!(akk, A, k, inv)
    end
    A
end

end # module


# Plot plasma β in a cut plane from 3D PIC outputs.
#
# Hongyang Zhou, hyzhou@umich.edu

using Batsrus, PyPlot, Printf

filename = "3d_box.out"
data = readdata(filename)

sequence = 65 # cut plane index from -
p_ = 18 # thermal pressure index

X = @view data.x[:,:,:,1]
Y = @view data.x[:,:,:,2]
Z = @view data.x[:,:,:,3]

fig, ax = plt.subplots(1, 1)
fig.set_size_inches(3.3, 6)

W  = @view data.w[:,:,:,p_]
Bx = @view data.w[:,:,:,5]
By = @view data.w[:,:,:,6]
Bz = @view data.w[:,:,:,7]

cut1 = @view X[:,sequence,:]
cut2 = @view Z[:,sequence,:]
Pcut  = @view W[:,sequence,:]
Bxcut = @view Bx[:,sequence,:]
Bycut = @view By[:,sequence,:]
Bzcut = @view Bz[:,sequence,:]
PBcut = hypot.(Bx, By, Bz)

c = ax.contourf(cut1, cut2, Pcut./PBcut)
fig.colorbar(c; ax)
ax.axis("scaled")
title(data.head[:wnames][p_])

xlabel("x"); ylabel("z")

dim = [0.125, 0.013, 0.2, 0.045]
str = @sprintf "it=%d, time=%4.2f" data.head[:it] data.head[:time]
at = matplotlib.offsetbox.AnchoredText(str,
   loc="lower left", prop=Dict("size"=>8), frameon=true,
   bbox_to_anchor=(0., 1.),
   bbox_transform=ax.transAxes)
at.patch.set_boxstyle("round,pad=0.,rounding_size=0.2")
ax.add_artist(at)


using Test
using Example2018

# Polynomials form a ring
# 1. addition is associative
# 2. additive identity
# 3. additive inverse
# 4. addition commues
# 5. multiplication is...

@testset "Polynomials form a ring" begin

	z=Polynomial([0])

	for n in 1:100

	      	deg=rand(0:100)

		p=Polynomial(rand(-1000000:1000000,deg+1))

		@test p+z==p
		@test z+p==p
		@test p+(-p)==z
	end
end


using XConv, LinearAlgebra, Flux, PyPlot, BenchmarkTools
using MLDatasets

BLAS.set_num_threads(Sys.CPU_THREADS)

function getbatch_CIFAR10(bsize, nchin, nchout; gaussian=false)
    X, _ = CIFAR10.traindata();
    gaussian && (X0 = randn(Float32, 32, 32, 3*nchin, bsize))
    !gaussian && (X0 = reshape(X[:, :, :, rand(1:50000, bsize*nchin)], 32, 32, 3*nchin, bsize))
    Y0 = reshape(X[:, :, :, rand(1:50000, bsize*nchout)], 32, 32, 3*nchout, bsize)
    return Float32.(X0), Float32.(Y0)
end

n_bench = 10
BenchmarkTools.DEFAULT_PARAMETERS.samples = n_bench

nx = 64
ny = 64
batchsize = 32
n_in = 2
n_out = 2
stride = (1, 1)
nw   = 3;

# Flux network
C = Conv((nw, nw), n_in=>n_out, identity;pad=1, stride=stride)

batches = [2^k for k=0:8]

tf = zeros(length(batches))
t1 = similar(tf)
t10 = similar(tf)
t50 = similar(tf)
t100 = similar(tf)
angles = zeros(length(batches), 4)

close("all")

# Init plot
fig, axsl = subplots(3, 3, figsize=(10, 5))
fig.suptitle("Conv layer gradient chi-$(n_in), cho-$(n_out)")

for (i, b)=enumerate(batches)
    println("Gradient for batchsize=$b")

    local X = randn(Float32, nx, ny, n_in, b)
    local Y = C(X) - randn(Float32, div(nx, stride[1]), div(ny, stride[2]), n_out, b)
    # local X, Y =  getbatch_CIFAR10(b, n_in, n_out; gaussian=true)

    cdims = DenseConvDims(X, C.weight; stride=C.stride, padding=C.pad, dilation=C.dilation)

    # Run once to get gradient
    @time local g1 = ∇conv_filter(X, Y, cdims)
    @time local g20 = grad_ev(X, Y, 5, nw, stride);
    @time local g21 = grad_ev(X, Y, 10, nw, stride);
    @time local g22 = grad_ev(X, Y, 50, nw, stride);
    @time local g23 = grad_ev(X, Y, 100, nw, stride);

    # Compute similarities
    angles[i, 1] = dot(g20, g1)/(norm(g20)*norm(g1))
    angles[i, 2] = dot(g21, g1)/(norm(g21)*norm(g1))
    angles[i, 3] = dot(g22, g1)/(norm(g22)*norm(g1))
    angles[i, 4] = dot(g23, g1)/(norm(g23)*norm(g1))

    # Benchmark runtime
    # tf[i] = @belapsed ∇conv_filter($X, $Y, $cdims) samples=2
    # t1[i] = @belapsed grad_ev($X, $Y, 5, $nw, $stride) samples=2
    # t10[i] = @belapsed grad_ev($X, $Y, 10, $nw, $stride) samples=2
    # t50[i] = @belapsed grad_ev($X, $Y, 50, $nw, $stride) samples=2
    # t100[i] = @belapsed grad_ev($X, $Y, 100, $nw, $stride) samples=2

    # Plot result
    axsl[i].plot(vec(g20)/norm(g20, Inf), label="LR(s=5)", "-r")
    axsl[i].plot(vec(g21)/norm(g21, Inf), label="LR(s=10)", "-b")
    axsl[i].plot(vec(g22)/norm(g22, Inf), label="LR(s=50)", "-g")
    axsl[i].plot(vec(g23)/norm(g23, Inf), label="LR(s=100))", "-c")
    axsl[i].plot(vec(g1)/norm(g1, Inf), label="Flux", "-k")
    axsl[i].set_title("batchsize=$b")
end
lines, labels = fig.axes[end].get_legend_handles_labels()
fig.legend(lines, labels, loc = "upper left")
tight_layout()

# figure()
# title("10 Gradient computation speedup")
# plot(batches, tf, label="Flux")
# plot(batches,t1, label="LR(s=5)")
# plot(batches,t10, label="LR(s=10)")
# plot(batches,t50, label="LR(s=50)")
# plot(batches,t100, label="LR(s=100)")
# xlabel("Batch size")
# ylabel("Runtime(s)")
# legend()

figure()
title("Similarty x' x2 / (||x|| ||x2||)")
plot(batches,angles[:, 1], label="LR(s=5)")
plot(batches,angles[:, 2], label="LR(s=10)")
plot(batches,angles[:, 3], label="LR(s=50)")
plot(batches,angles[:, 4], label="LR(s=100)")
legend()

@show angles


# Toy model from paper
# include("../src/SFFM.jl")
# using LinearAlgebra, Plots

## Define 1st model
T = [-1.0 1.0; 1.0 -1.0]
C = [1.0; -1.0]
r = (
    r = function (x)
        [abs.((x .<= 1)-(x .> 1)) abs.(((x .> 1))-2*(x .<= 1).*(x .> 0)-(x.==0))]
    end,
    R = function (x)
        [x.*abs.(((x .<= 1)-(x .> 1))) x.*abs.((((x .> 1))-2*(x .<= 1).*(x .> 0)-(x.==0)))]
    end,
)

Bounds = [0 30; -Inf Inf]
model = SFFM.Model( T, C, r, Bounds = Bounds)

## Define mesh
Δ = 5
Nodes = collect(Bounds[1,1]:Δ:Bounds[1,2])
nBases = 3
Basis = "lagrange"
mesh = SFFM.DGMesh(model, Nodes, nBases, Basis=Basis)

## Make matrices
All = SFFM.MakeAll( model, mesh, approxType = "projection")
proj = All
interp = SFFM.MakeAll(model,mesh,approxType="interpolation")

vikramD = copy(interp.D["++"]())
vikramD[sum(model.C.<=0)+1:end-sum(model.C.>=0),:] =
    repeat(All.Matrices.Local.V.w, SFFM.NIntervals(mesh)*SFFM.NPhases(model), 1) .* vikramD[sum(model.C.<=0)+1:end-sum(model.C.>=0),:]

## sims fro gound truth
x₀ = 15
NSim = 50000
IC = (φ = ones(Int, NSim), X = x₀ .* ones(NSim), Y = zeros(NSim))
y = 10
@time sims =
    SFFM.SimSFFM( model, SFFM.FirstExitY(-Inf, y), IC)

## DG stationary dist
# construct initial condition
theNodes = SFFM.CellNodes(mesh)[:,convert(Int,ceil(x₀/Δ))]
basisValues = zeros(length(theNodes))
for n in 1:length(theNodes)
    basisValues[n] = prod(x₀.-theNodes[[1:n-1;n+1:end]])./prod(theNodes[n].-theNodes[[1:n-1;n+1:end]])
end
initpm = [
    zeros(sum(model.C.<=0)) # LHS point mass
    zeros(sum(model.C.>=0)) # RHS point mass
]
initprobs = zeros(Float64,SFFM.NBases(mesh),SFFM.NIntervals(mesh),SFFM.NPhases(model))
initprobs[:,convert(Int,ceil(x₀/Δ)),1] = basisValues'*All.Matrices.Local.V.V*All.Matrices.Local.V.V'.*2/Δ
initdist = SFFM.SFFMDensity(
    initpm,
    initprobs,
    SFFM.CellNodes(mesh),
)
x0 = SFFM.Dist2Coeffs( model, mesh, initdist)
h = 0.0001
@time projyvals = SFFM.EulerDG( proj.D["++"](s = 0), y, x0, h = h)
@time interpyvals = SFFM.EulerDG( interp.D["++"](s = 0), y, x0, h = h)

## plotting
simdensity = SFFM.Sims2Dist(model,mesh,sims,SFFM.SFFMProbability)
projdensity = SFFM.Coeffs2Dist(model,mesh,projyvals,SFFM.SFFMProbability)
interpdensity = SFFM.Coeffs2Dist(model,mesh,interpyvals,SFFM.SFFMProbability)
println("proj error: ",SFFM.starSeminorm( projdensity, simdensity))
println("interp error: ",SFFM.starSeminorm( interpdensity, simdensity))

## plots
# plot solutions
# densities
p = SFFM.plot(model; mesh = mesh,dist = projdensity,color=:blue,label="proj")#,marker=:rtriangle)
p = SFFM.plot!(p,model,mesh,interpdensity,color=:red,label="interp")#,marker=:x)
# plot sims
p = SFFM.plot!(p,model,mesh,simdensity,color=:black,label="sim")#,marker=:ltriangle)

display(p)
## probabilities
simdensity = SFFM.Sims2Dist(model,mesh,sims,SFFM.SFFMDensity)
projdensity = SFFM.Coeffs2Dist(model,mesh,projyvals,SFFM.SFFMDensity)
interpdensity = SFFM.Coeffs2Dist(model,mesh,interpyvals,SFFM.SFFMDensity)
# vikramdensity = SFFM.Coeffs2Dist(model=model,mesh=mesh,Coeffs=vikramyvals,SFFM.SFFMDensity)

## plots
# plot solutions
# densities
p = SFFM.plot(model,mesh=mesh,dist=projdensity,color=:blue,label="proj")#,marker=:rtriangle)
p = SFFM.plot!(p,model,mesh,interpdensity,color=:red,label="interp")#,marker=:x)
# plot sims
p = SFFM.plot!(p,model,mesh,simdensity,color=:black,label="sim")#,marker=:ltriangle)
# savefig(p,"/Users/a1627293/Dropbox/PhD/NotesMaster/whichR/m1.png")
display(p)
## end 1st model

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

## Define 2nd model
T = [-1.0 1.0; 1.0 -1.0]
C = [1.0; -1.0]
r = (
    r = function (x)
        [0.1.*x.+0.05 (x.>-1)]
    end,
    R = function (x)
        [0.1.*x.^2/2.0.+0.05.*x (x.>-1).*x]
    end,
)

Bounds = [0 30; -Inf Inf]
model = SFFM.Model( T, C, r, Bounds = Bounds)

## Define mesh
# Δ = 3
Nodes = collect(Bounds[1,1]:Δ:Bounds[1,2])
# nBases = 3
Basis = "lagrange"
mesh = SFFM.DGMesh(model, Nodes, nBases, Basis=Basis)

## Make matrices
All = SFFM.MakeAll( model, mesh, approxType = "projection")
proj = All
interp = SFFM.MakeAll( model, mesh, approxType="interpolation")

vikramD = copy(interp.D["++"]())
vikramD[sum(model.C.<=0)+1:end-sum(model.C.>=0),:] =
    repeat(All.Matrices.Local.V.w, SFFM.NIntervals(mesh)*SFFM.NPhases(model), 1) .* vikramD[sum(model.C.<=0)+1:end-sum(model.C.>=0),:]

## sims fro gound truth
x₀ = 15
NSim = 50000
IC = (φ = ones(Int, NSim), X = x₀ .* ones(NSim), Y = zeros(NSim))
y = 10
@time sims =
    SFFM.SimSFFM( model, SFFM.FirstExitY(-Inf, y), IC)

## DG stationary dist
# construct initial condition
theNodes = SFFM.CellNodes(mesh)[:,convert(Int,ceil(x₀/Δ))]
basisValues = zeros(length(theNodes))
for n in 1:length(theNodes)
    basisValues[n] = prod(x₀.-theNodes[[1:n-1;n+1:end]])./prod(theNodes[n].-theNodes[[1:n-1;n+1:end]])
end
initpm = [
    zeros(sum(model.C.<=0)) # LHS point mass
    zeros(sum(model.C.>=0)) # RHS point mass
]
initprobs = zeros(Float64,SFFM.NBases(mesh),SFFM.NIntervals(mesh),SFFM.NPhases(model))
initprobs[:,convert(Int,ceil(x₀/Δ)),1] = basisValues'*All.Matrices.Local.V.V*All.Matrices.Local.V.V'.*2/Δ
initdist = SFFM.SFFMDensity(
    initpm,
    initprobs,
    SFFM.CellNodes(mesh),
)
x0 = SFFM.Dist2Coeffs( model, mesh, initdist)
h = 0.0001
@time projyvals = SFFM.EulerDG(proj.D["++"](s = 0), y, x0, h = h)
@time interpyvals = SFFM.EulerDG(interp.D["++"](s = 0), y, x0, h = h)

## plotting
simdensity = SFFM.Sims2Dist( model, mesh, sims,SFFM.SFFMProbability)
projdensity = SFFM.Coeffs2Dist( model, mesh, projyvals,SFFM.SFFMProbability)
interpdensity = SFFM.Coeffs2Dist( model, mesh, interpyvals,SFFM.SFFMProbability)

println("proj error: ",SFFM.starSeminorm( projdensity, simdensity))
println("interp error: ",SFFM.starSeminorm(interpdensity, simdensity))
## plots
# plot solutions
# densities
# plot sims
p = SFFM.plot( model, mesh = mesh, dist = simdensity, color=:black,label="sim")#,marker=:ltriangle)
p = SFFM.plot!(p, model, mesh, projdensity, color=:blue,label="proj")#,marker=:rtriangle)
p = SFFM.plot!(p, model, mesh, interpdensity, color=:red,label="interp")#,marker=:x)

display(p)
## probabilities
simdensity = SFFM.Sims2Dist( model, mesh, sims,SFFM.SFFMDensity)
projdensity = SFFM.Coeffs2Dist( model, mesh, projyvals,SFFM.SFFMDensity)
interpdensity = SFFM.Coeffs2Dist( model, mesh, interpyvals,SFFM.SFFMDensity)

## plots
# plot solutions
# densities
p = SFFM.plot( model, mesh = mesh, dist = projdensity, color=:blue,label="proj")#,marker=:rtriangle)
p = SFFM.plot!(p, model, mesh, interpdensity, color=:red,label="interp")#,marker=:x)

# plot sims
p = SFFM.plot!(p, model, mesh, simdensity, color=:black,label="sim")#,marker=:ltriangle)
# savefig(p,"/Users/a1627293/Dropbox/PhD/NotesMaster/whichR/m2.png")
display(p)
## end 2nd model

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

## Define 3rd model
T = [-1.0 1.0; 1.0 -1.0]
C = [1.0; -1.0]
r = (
    r = function (x)
        [cos.(x).+1.05 x.>-1]
    end,
    R = function (x)
        [sin.(x).+1.05.*x (x.>-1).*x]
    end,
)

Bounds = [0 10*π; -Inf Inf]
model = SFFM.Model( T, C, r, Bounds = Bounds)

## Define mesh
# Δ = 3
Nodes = collect(Bounds[1,1]:Δ:Bounds[1,2])
# nBases = 3
Basis = "lagrange"
mesh = SFFM.DGMesh(model, Nodes, nBases, Basis=Basis)

## Make matrices
All = SFFM.MakeAll( model, mesh, approxType = "projection")
proj = All
interp = SFFM.MakeAll( model, mesh, approxType="interpolation")

vikramD = copy(interp.D["++"]())
vikramD[sum(model.C.<=0)+1:end-sum(model.C.>=0),:] =
    repeat(All.Matrices.Local.V.w, SFFM.NIntervals(mesh)*SFFM.NPhases(model), 1) .* vikramD[sum(model.C.<=0)+1:end-sum(model.C.>=0),:]

## sims fro gound truth
x₀ = 15
NSim = 50000
IC = (φ = ones(Int, NSim), X = x₀ .* ones(NSim), Y = zeros(NSim))
y = 10
@time sims =
    SFFM.SimSFFM( model, SFFM.FirstExitY( -Inf, y), IC)

## DG stationary dist
# construct initial condition
theNodes = SFFM.CellNodes(mesh)[:,convert(Int,ceil(x₀/Δ))]
basisValues = zeros(length(theNodes))
for n in 1:length(theNodes)
    basisValues[n] = prod(x₀.-theNodes[[1:n-1;n+1:end]])./prod(theNodes[n].-theNodes[[1:n-1;n+1:end]])
end
initpm = [
    zeros(sum(model.C.<=0)) # LHS point mass
    zeros(sum(model.C.>=0)) # RHS point mass
]
initprobs = zeros(Float64,SFFM.NBases(mesh),SFFM.NIntervals(mesh),SFFM.NPhases(model))
initprobs[:,convert(Int,ceil(x₀/Δ)),1] = basisValues'*All.Matrices.Local.V.V*All.Matrices.Local.V.V'.*2/Δ
initdist = SFFM.SFFMDensity(
    initpm,
    initprobs,
    SFFM.CellNodes(mesh),
)
x0 = SFFM.Dist2Coeffs( model, mesh, initdist)
h = 0.0001
@time projyvals = SFFM.EulerDG( proj.D["++"](s = 0), y, x0, h = h)
@time interpyvals = SFFM.EulerDG( interp.D["++"](s = 0), y, x0, h = h)

## plotting
simdensity = SFFM.Sims2Dist( model, mesh, sims, SFFM.SFFMProbability)
projdensity = SFFM.Coeffs2Dist( model, mesh, projyvals, SFFM.SFFMProbability)
interpdensity = SFFM.Coeffs2Dist( model, mesh, interpyvals, SFFM.SFFMProbability)
println("proj error: ",SFFM.starSeminorm( projdensity, simdensity))
println("interp error: ",SFFM.starSeminorm( interpdensity, simdensity))

## plots
# plot solutions
# densities
p = SFFM.plot( model, mesh = mesh, dist = projdensity, color=:blue,label="proj")#,marker=:rtriangle)
p = SFFM.plot!(p, model, mesh, interpdensity, color=:red,label="interp")#,marker=:x)
# plot sims
p = SFFM.plot!(p, model, mesh, simdensity, color=:black,label="sim")#,marker=:ltriangle)

display(p)
## probabilities
simdensity = SFFM.Sims2Dist( model, mesh, sims, SFFM.SFFMDensity)
projdensity = SFFM.Coeffs2Dist( model, mesh, projyvals, SFFM.SFFMDensity)
interpdensity = SFFM.Coeffs2Dist( model, mesh, interpyvals, SFFM.SFFMDensity)

## plots
# plot solutions
# densities
p = SFFM.plot( model, mesh = mesh, dist = projdensity, color=:blue,label="proj")#,marker=:rtriangle)
p = SFFM.plot!(p, model, mesh, interpdensity, color=:red,label="interp")#,marker=:x)
# plot sims
p = SFFM.plot!(p, model, mesh, simdensity, color=:black,label="sim")#,marker=:ltriangle)
# savefig(p,"/Users/a1627293/Dropbox/PhD/NotesMaster/whichR/m3.png")
display(p)


using StructuralCausalModels, Test

ProjDir = @__DIR__
cd(ProjDir) #do

N = 100
b_AU = 0.5
b_AC = 3
b_UX = -1
b_UB = 2
b_CB = -1.5
b_CY = 1
b_XY = 5

df = DataFrame(
  :a => rand(Normal(), N)
);
df[!, :u] = rand(Normal(1, 2), N) + b_AU * df[:, :a]
df[!, :c] = rand(Normal(0, 1), N) + b_AC * df[:, :a]
df[!, :b] = rand(Normal(1, 1), N) + b_UB * df[:, :u]
df[!, :x] = rand(Normal(-2, 1), N) + b_UX * df[:, :u]
df[!, :y] = rand(Normal(1, 2), N) + b_XY * df[:, :x] + b_CY * df[:, :c]

u = [:u]                    # Latent variable
d = OrderedDict(
  :y => [:c, :x],
  :b => [:c, :u],
  :x => :u,
  :c => :a,
  :u => :a
  )

dag = DAG("sr6_4_2", d, df=df);

fn = joinpath(mktempdir(), "sr6_4_2.dot")
to_graphviz(dag, fn)
#Sys.isapple() && run(`open -a GraphViz.app $(fn)`)

basisset = basis_set(dag)
allpaths  = all_paths(dag, :x, :y)
backdoorpaths = backdoor_paths(dag, allpaths, :x)
adjustmentsets = adjustment_sets(dag, :x, :y)

@testset "sr6_4_2" begin

  @test length(basisset) == 15
  @test basisset[6][1] == :x
  @test basisset[6][2] == :b
  @test basisset[6][3:end] == [:u]
  @test basisset[8][1] == :x
  @test basisset[8][2] == :a
  @test basisset[8][3:end] == [:u]

  @test allpaths[1] == [:x, :u, :b, :c, :y]
  @test allpaths[2] == [:x, :u, :a, :c, :y]

  @test backdoorpaths[1] == [:x, :u, :b, :c, :y]
  @test backdoorpaths[2] == [:x, :u, :a, :c, :y]

  @test adjustmentsets == [[:u], [:c], [:a]]

end


# Raytracer.jl
# Raytracing for the generation of photorealistic images in Julia
# Copyright (c) 2021 Samuele Colombo, Paolo Galli

# Axis Aligned Bounding Box

"""
    AABB

A type representing an Axis-Aligned Bounding Box.
"""
Base.@kwdef struct AABB
    p_M::Point = Point(1f0, 1f0, 1f0)
    p_m::Point = Point(0f0, 0f0, 0f0)
end

"""
    get_t(ray::Ray, aabb::AABB)

Return the parameter `t` at which [`Ray`](@ref) first hits the [`AABB`](@ref). If no hit exists, return `Inf32`.
"""
function get_t(ray::Ray, aabb::AABB)
    dir = ray.dir
    o = ray.origin
    min_point = replace((aabb.p_m - o) ./ dir, NaN32 => 0)
    max_point = replace((aabb.p_M - o) ./ dir, NaN32 => 0)
    overlap = reduce(intersect, map(t -> Interval(extrema(t)...), zip(min_point, max_point)))
    isempty(overlap) && return Inf32
    t1, t2 = overlap.first, overlap.last
    ray.tmin < t1 < ray.tmax && return t1
    ray.tmin < t2 < ray.tmax && return t2
    return Inf32
end

function get_all_ts(ray::Ray, aabb::AABB)
    dir = ray.dir
    o = ray.origin
    min_point = replace((aabb.p_m - o) ./ dir, NaN32 => 0)
    max_point = replace((aabb.p_M - o) ./ dir, NaN32 => 0)
    overlap = reduce(intersect, map(t -> Interval(extrema(t)...), zip(min_point, max_point)))
    isempty(overlap) && return Vector{Float32}()
    t1, t2 = overlap.first, overlap.last
    isfinite(t1) && (@assert isfinite(t2); return [t1, t2])
    @assert !isfinite(t2)
    return Vector{Float32}()
end


# Function to parse instruction and return address and value
function parse_inst(val)
    reg = r"\[(.*?)\]"
    addr, val = split(val, "=")

    val = parse(Int, val)
    addr = parse(Int, match(reg, addr).captures[1])
    return addr, val
end

# Apply a mask either in part1 or part2 format
function apply_mask(mask, val; part2=false)

    # Part 1: change the value everywhere the mask != X
    if !part2
        bval = collect(bitstring(val)[29:64])
        bmask = collect(mask)
        for i in 1:36
            if bmask[i] != 'X'
                bval[i] = bmask[i]
            end
        end

        bval = join(bval)
        newval = parse(UInt, bval, base=2)
        # println("Changed $val to $bval = $newval")
        return newval

    # Part 2: Change value (address in this case)
    # where mask == X or mask == 1
    else
        bval = collect(bitstring(val)[29:64])
        bmask = collect(mask)
        for i in 1:36
            if bmask[i] == 'X' || bmask[i] == '1'
                bval[i] = bmask[i]
            end
        end

        bval = join(bval)
        return bval
    end
end

# Take the output of apply_mask with part2 output
# and treat X as a floating value, can equal 1 or 0.
# Return all possible combinations using recusion
function get_possible_addrs(masklist)
    newmasklist = []
    for mask in masklist
        bmask = collect(mask)
        if !('X' in bmask) return masklist end
        for (i,c) in enumerate(bmask)
            if c == 'X'
                bmask[i] = '1'
                newmasklist = vcat(newmasklist, join(bmask))
                bmask[i] = '0'
                newmasklist = vcat(newmasklist, join(bmask))
                break
            end
        end
    end
    # This part is the recusion
    return get_possible_addrs(newmasklist)
end

# Sum all values in mem dict
function sum_mem(mem)
    s = 0
    for key in keys(mem)
        s += mem[key]
    end
    return s
end

# Main function to run the program.
# Parses lines and applies the masks correctly.
function run_program(file; part2 = false)
    mem = Dict()

    for line in readlines(file)
        if line[1:6] == "mask ="
            global mask = line[8:length(line)]
        elseif !part2
            addr, val = parse_inst(line)
            # println("mem[$addr] = $val")
            val = apply_mask(mask, val)
            mem[addr] = Int(val)
        else
            addr, val = parse_inst(line)
            masked_addr = apply_mask(mask, addr, part2=true)
            addrlist = get_possible_addrs([masked_addr])
            for a in addrlist
                a_int = parse(UInt, a, base=2)
                mem[a_int] = val
            end

        end
    end
    return sum_mem(mem)
end

println("Part 1: ", run_program("./init_program.txt"))
println("Part 2: ", run_program("./init_program.txt", part2=true))


vcov(m, ::Nothing) = nothing

"""
    criticalvalue(vce::CovarianceEstimator, level::Real, T::Int)

Return the critical value for `vce` at the given `level` when the sample has `T` periods.
"""
criticalvalue(::CovarianceEstimator, level::Real, T::Int) = norminvccdf((1.0-level)/2.0)

"""
    pvalue(vce::CovarianceEstimator, ts::Real, T::Int)

Return the p-value for test statistic `ts` associated with `vce`
when the sample has `T` periods.
"""
pvalue(::CovarianceEstimator, ts::Real, T::Int) = 2.0*normccdf(abs(ts))

"""
    SimpleVCE <: CovarianceEstimator

Simple variance-covariance estimator.
"""
struct SimpleVCE <: CovarianceEstimator end

function vcov(m::OLS, ::SimpleVCE)
    T, K = size(m.X)
    return kron(m.invXX, m.resid'm.resid)./(T-K)
end

show(io::IO, ::MIME"text/plain", ::SimpleVCE) =
    print(io, "Simple variance-covariance estimator")

"""
    HRVCE <: CovarianceEstimator

Eicker-Huber-White heteroskedasticity-robust variance-covariance estimator.
"""
struct HRVCE <: CovarianceEstimator end

function vcov(m::OLS, ::HRVCE)
    T, K = size(m.X)
    return T/(T-K).*kron_fastr(m.invXX, kron_fastr(m.invXX, m.score'm.score)')'
end

function vcov(m::Ridge, ::HRVCE)
    T = size(m.C, 1)
    S = m.score'm.score
    return T/(m.dof_res-m.dof_adj) * m.invCCP * S * m.invCCP
end

show(io::IO, ::MIME"text/plain", ::HRVCE) =
    print(io, "Heteroskedasticity-robust variance-covariance estimator")

"""
    LongRunVariance

Supertype for all long-run variance estimators.
"""
abstract type LongRunVariance end

"""
    HARVCE{LR<:LongRunVariance} <: CovarianceEstimator

Heteroskedasticity-autocorrelation-robust variance-covariance estimator
with a long-run variance estimator of type `LR`.

# Fields
- `lr::LR`: the long-run variance estimator.
- `bw::Function`: bandwidth as a function of the number of periods in sample.
- `cv::Function`: critical value as a function of the level of the test and the bandwidth.
- `pv::Function`: p-value as a function of the test statistic and the bandwidth.
"""
struct HARVCE{LR<:LongRunVariance} <: CovarianceEstimator
    lr::LR
    bw::Function
    cv::Function
    pv::Function
end

"""
    EqualWeightedCosine <: LongRunVariance

Equal-weighted cosine transform. The alias [`EWC`](@ref) is provided for convenience.

# Reference
Lazarus, Eben, Daniel J. Lewis, James H. Stock, and Mark W. Watson. 2018.
"HAR Inference: Recommendations for Practice."
Journal of Business & Economic Statistics 36 (4): 541-559.
"""
struct EqualWeightedCosine <: LongRunVariance end

"""
    EWC <: LongRunVariance

An alias for [`EqualWeightedCosine`](@ref).
"""
const EWC = EqualWeightedCosine

HARVCE(lr::EqualWeightedCosine; bw=_ewcbw, cv=_ewccv, pv=_ewcpv) = HARVCE(lr, bw, cv, pv)

_ewcbw(T::Integer) = round(Int, 0.4*T^(2/3))
_ewccv(level::Real, bw::Int) = tdistinvccdf(bw, (1.0-level)/2.0)
_ewcpv(ts::Real, bw::Int) = 2.0*tdistccdf(bw, abs(ts))

function lrv(vce::HARVCE{EqualWeightedCosine}, sc::AbstractMatrix)
    T, K = size(sc)
    TF = eltype(sc)
    bw = vce.bw(T)
    Z = zeros(Complex{TF}, 4*T, K)
    inds = iseven.(1:2*T)
    copyto!(view(view(Z, 1:2*T, :), inds, :), sc)
    fft!(Z, 1)
    rffts = real.(view(Z, 2:bw+1, :))
    return 2.0/bw * (rffts'rffts)
end

show(io::IO, ::MIME"text/plain", ::EqualWeightedCosine) =
    print(io, "Equal-weighted cosine transform")

function vcov(m::OLS, vce::HARVCE)
    T, K = size(m.X)
    return T/(T-K).*kron_fastr(m.invXX, kron_fastr(m.invXX, lrv(vce, m.score))')'
end

function vcov(m::Ridge, vce::HARVCE)
    T = size(m.C, 1)
    return T/(m.dof_res-m.dof_adj) * m.invCCP * lrv(vce, m.score) * m.invCCP
end

criticalvalue(vce::HARVCE, level::Real, T::Int) = vce.cv(level, vce.bw(T))
pvalue(vce::HARVCE, ts::Real, T::Int) = vce.pv(ts, vce.bw(T))

function show(io::IO, ::MIME"text/plain", vce::HARVCE)
    println(io, "Heteroskedasticity-autocorrelation-robust variance-covariance estimator:")
    print(io, "  Long-run variance estimator: ", sprint(show, MIME("text/plain"), vce.lr))
end


#!/usr/bin/env julia

using Distributed
using Random
using LinearAlgebra
procs_id = addprocs(8)
using DatagenCopulaBased
@everywhere using Distributions
@everywhere using Cumulants
using SymmetricTensors
using CumulantsFeatures
using JLD2
using FileIO
using ArgParse
@everywhere import CumulantsFeatures: reduceband
@everywhere using DatagenCopulaBased
@everywhere using CumulantsFeatures

function main(args)
  s = ArgParseSettings("description")
  @add_arg_table s begin
    "--nu", "-u"
    default = 5
    help = "the number of degrees of freedom for the t-Student copula"
    arg_type = Int
  end
  parsed_args = parse_args(s)
  ν = parsed_args["nu"]

  println(ν)
  @everywhere t = 100_000
  @everywhere n = 50
  @everywhere malf_size = 10
  @everywhere a = 1_000
  data_dir = "."
  test_number = 3
  filename = "tstudent_$(ν)-t_size-$(n)_malfsize-$malf_size-t_$(t)_$a.jld2"

  data = Dict{String, Any}("variables_no" => n,
                         "sample_number" => t,
                         "ν" => ν,
                         "test_number" => test_number,
                         "malf_size" => malf_size,
                         "a" => a,
                         "data" => Dict{String, Dict{String,Any}}())

  known_data_size = 0
  if isfile("$data_dir/$filename")
   data["data"] = load("$data_dir/$filename")["data"]
   known_data_size += length(data["data"])
   println("Already have $known_data_size samples \n Will generate $(test_number-known_data_size) more")
  end

  #true calculations
  println("Calculation started")
  for m=(known_data_size+1):test_number
    @time begin
      println(" > $m ($ν)")
      malf = randperm(n)[1:malf_size]
      Σ = cormatgen_rand(n)
      samples_orig = rand(MvNormal(Σ), t)'

      versions = [(x->x, "original"),
                  (x->vcat(gcop2tstudent(x[1:a, :], malf, ν), x[a+1:end, :]), "malf")]

      cur_dict = Dict{String, Any}("malf" => malf,
                                   "cor_source" => Σ)

      data_dict = @distributed (merge) for (sampler, label)=versions
        println(label)
        samples = sampler(samples_orig)
        Σ_malf = cov(samples)
        Dict("cor_$label" => Σ_malf,
             "x_$label" => samples)
      end

      data["data"]["$m"] = merge(cur_dict, data_dict)
      save("$data_dir/$filename", data)
    end
  end

end

main(ARGS)


using Test
using WignerFamilies
using HalfIntegers
import WignerSymbols  # only a test-dep, for comparisons

@testset "f: nonclassical" begin
    j₂ = 100
    j₃ = 60
    m₂ = 70
    m₃ = -55
    m₁ = -m₂ - m₃
    w3j = wigner3j_f(Float64, j₂, j₃, m₂, m₃)
    j_array = collect(eachindex(w3j))
    reference = [WignerSymbols.wigner3j(Float64, j, j₂, j₃, m₁, m₂) for j in j_array]

    for (i, j) in enumerate(j_array)
        @test w3j[j] ≈ reference[i]
    end

    # also test specifying different types
    w3j = wigner3j_f(BigFloat, j₂, j₃, m₂, m₃)
    for (i, j) in enumerate(j_array)
        @test w3j[j] ≈ reference[i]
    end
    w3j = wigner3j_f(j₂, j₃, m₂, m₃)
    for (i, j) in enumerate(j_array)
        @test w3j[j] ≈ reference[i]
    end
end

##
@testset "f: mᵢ = 0 special case" begin
    j₂, j₃, m₂, m₃ = 5000, 5002, 0, 0
    w3j = wigner3j_f(Float64, j₂, j₃, m₂, m₃)
    j_array = collect(eachindex(w3j))
    for j in [j_array[i] for i in [collect(1:100)..., 5001, 7001, 10001-50, 10001]]
        @test w3j[j] ≈ Float64(WignerSymbols.wigner3j(BigFloat, j, j₂, j₃, -m₂-m₃, m₂))
    end

    j₂, j₃, m₂, m₃ = 5000, 5001, 0, 0
    w3j = wigner3j_f(Float64, j₂, j₃, m₂, m₃)
    j_array = collect(eachindex(w3j))
    for j in [j_array[i] for i in [collect(1:100)..., 5001, 7001, 10001-50, 10001]]
        @test w3j[j] ≈ Float64(WignerSymbols.wigner3j(BigFloat, j, j₂, j₃, -m₂-m₃, m₂))
    end

    for j₂ in 0:100
        j₃, m₂, m₃ = 10, 0, 0
        w3j = wigner3j_f(Float64, j₂, j₃, m₂, m₃)
        j_array = collect(eachindex(w3j))
        for j in j_array
            @test w3j[j] ≈ Float64(WignerSymbols.wigner3j(BigFloat, j, j₂, j₃, -m₂-m₃, m₂))
        end
    end

    for j₂ in 0:100
        j₃, m₂, m₃ = 11, 0, 0
        w3j = wigner3j_f(Float64, j₂, j₃, m₂, m₃)
        j_array = collect(eachindex(w3j))
        for j in j_array
            @test w3j[j] ≈ Float64(WignerSymbols.wigner3j(BigFloat, j, j₂, j₃, -m₂-m₃, m₂))
        end
    end
end
##
@testset "f: edge cases" begin
    j₂, j₃, m₂, m₃ = 2, 2, 4, 4
    w3j = wigner3j_f(Float64, j₂, j₃, m₂, m₃)
    @test length(w3j) == 0
    j₂, j₃, m₂, m₃ = 2, 2, HalfInt(0.5), HalfInt(0.5)
    w3j = wigner3j_f(Float64, j₂, j₃, m₂, m₃)
    @test length(w3j) == 0

    @test wigner3j_f(Float64, 1, 0, 0, 0)[1] ≈ -sqrt(1/3)
    @test wigner3j_f(Float64, 0, 0, 0, 0)[0] ≈ 1.0
    @test wigner3j_f(Float64, 1, 1, 0, 0)[2] ≈ sqrt(2/15)
end

## bottom of page 14 of Raynal et al. On the zeros of 3j coefficients: polynomial degree
# versus recurrence order is an example of sequence of nontrivial zeros
@testset "f: nontrivial zeros" begin
    tol = eps()
    for (X₀, Y₀) in ((43, 25), (109, 63))
        j₂, j₃, m₂, m₃ = Int128.((5000, 5002, 0, 0))
        X(n) = iszero(n) ? X₀ : 7X(n-1) + 12Y(n-1)
        Y(n) = iszero(n) ? Y₀ : 4(n-1) + 7Y(n-1)

        for n_seq in 0:1
            j₂, j₃, m₂, m₃ = (2X(n_seq)+1)/6, (X(n_seq)+2)/6, HalfInt(3/2), HalfInt(3/2)
            w3j = wigner3j_f(Float64, j₂, j₃, m₂, m₃)
            for j in eachindex(w3j)
                @test (abs(w3j[j] -
                    Float64(WignerSymbols.wigner3j(BigFloat, j, j₂, j₃, -m₂-m₃, m₂))) < tol)
            end
        end
    end
end
##
@testset "f: half-integer spin" begin

    w = WignerF( HalfInt(5/2), 5, HalfInt(1/2), HalfInt(-1) )
    m₁ = -w.m₂ - w.m₃
    w3j = get_wigner_array(w)
    wigner3j_f!(w, w3j)

    reference = [WignerSymbols.wigner3j(Float64, j, w.j₂, w.j₃, m₁, w.m₂)
                 for j in eachindex(w3j)]
    for (i, j) in enumerate(eachindex(w3j))
        @test w3j[j] ≈ reference[i]
    end
end

## tests for the Rasch and Yu c index
function confirm_symmetry(maxj)
    j₁, j₂ = rand(0:maxj, 2)
    j₃ = rand(abs(j₁ - j₂):(j₁ + j₂))
    if isodd(j₁ + j₂ + j₃)
        j₃ += 1
    end
    m₁, m₂, m₃ = 0, 0, 0
    c1 = rasch_yu_index(Int128, j₁, j₂, j₃, m₁, m₂, m₃)
    c2 = rasch_yu_index(Int128,
        j₁, (j₂ + j₃ - m₁)/2, (j₂ + j₃ + m₁)/2,
        j₃ - j₂, (j₂ - j₃ - m₁)/2 - m₃, (j₂ - j₃ + m₁)/2 + m₃)

    c1, c2
end

@testset "Rasch & Yu c: Regge symmetry" begin
    for i in 1:2000
        c1, c2 = confirm_symmetry(100)
        @test c1 == c2
    end
end

@testset "utils" begin
    @test all(WignerFamilies.swap_triangular(1:6) .== [1,6,2,5,3,4])
    @test all(WignerFamilies.swap_triangular(2:6) .== [2,6,3,5,4])
end