text:
    # Local compute version

# add more julia processes
nprocs() < 5 ? addprocs(5-nprocs()) : nothing

using Caesar, RoME, Distributions
using YAML, JLD, HDF5
using RoMEPlotting, Gadfly
using AprilTags
using Images, ImageView, ImageDraw
using CoordinateTransformations, Rotations, StaticArrays
using MeshCat
using GeometryTypes

const KDE = KernelDensityEstimate
const IIF = IncrementalInference
const TU = TransformUtils

# temporarily using Python / OpenCV for solvePnP
using PyCall

@pyimport numpy as np
@pyimport cv2

include(joinpath(Pkg.dir("Caesar"),"examples","wheeled","racecar","racecarUtils.jl"))
include(joinpath(Pkg.dir("Caesar"),"examples","wheeled","racecar","cameraUtils.jl"))
include(joinpath(Pkg.dir("Caesar"),"examples","wheeled","racecar","visualizationUtils.jl"))

cfg = loadConfig()

cw, ch = cfg[:intrinsics][:cx], cfg[:intrinsics][:cy]
fx = fy = cfg[:intrinsics][:fx]
camK = [fx 0 cw; 0 fy ch; 0 0 1]
tagsize = 0.172
# k1,k2 = cfg[:intrinsics][:k1], cfg[:intrinsics][:k2] # makes it worse
k1, k2 = 0.0, 0.0

# tag extrinsic rotation
Rx = RotX(-pi/2)
Rz = RotZ(-pi/2)
bTc= LinearMap(Rz) ∘ LinearMap(Rx)

datafolder = ENV["HOME"]*"/data/racecar/straightrun3/"
imgfolder = "images"

# Figure export folder
currdirtime = now()
imgdir = joinpath(ENV["HOME"], "Pictures", "racecarimgs", "$(currdirtime)")
mkdir(imgdir)
mkdir(imgdir*"/tags")

# process images
camlookup = prepCamLookup(175:5:370)
IMGS, TAGS = detectTagsViaCamLookup(camlookup, datafolder*imgfolder, imgdir)
# IMGS[1]
# TAGS[1]

tag_bag = prepTagBag(TAGS)
# tag_bag[0]

# save the tag bag file for future use
@save imgdir*"/tag_det_per_pose.jld" tag_bag

# Factor graph construction
N = 100
fg = initfg()

psid = 0
pssym = Symbol("x$psid")
# first pose with zero prior
addVariable!(fg, pssym, DynPose2(ut=0))
# addFactor!(fg, [pssym], PriorPose2(MvNormal(zeros(3),diagm([0.01;0.01;0.001].^2))))
addFactor!(fg, [pssym], DynPose2VelocityPrior(MvNormal(zeros(3),diagm([0.01;0.01;0.001].^2)),
                                              MvNormal(zeros(2),diagm([0.3;0.01].^2))))

addApriltags!(fg, pssym, tag_bag[psid], lmtype=Pose2, fcttype=DynPose2Pose2)

# writeGraphPdf(fg)

# quick solve as sanity check
tree = wipeBuildNewTree!(fg, drawpdf=true)
inferOverTreeR!(fg,tree, N=N)

# plotKDE(fg, :l1, dims=[3])

# ls(fg)
#
# val = getVal(fg, :l11)

# drawPosesLandms(fg, spscale=0.25)

# @async run(`evince bt.pdf`)

prev_psid = 0
# add other positions
maxlen = (length(tag_bag)-1)
# psid = 5
for psid in 1:1:maxlen #[5;9;13;17;21;25;29;34;39] #17:4:21 #maxlen
  @show psid
  addnextpose!(fg, prev_psid, psid, tag_bag[psid], lmtype=Pose2, odotype=VelPose2VelPose2, fcttype=DynPose2Pose2, DAerrors=0.2)
  # writeGraphPdf(fg)
  if psid % 3 == 0 || psid == maxlen
    tree = wipeBuildNewTree!(fg, drawpdf=true)
    inferOverTree!(fg,tree, N=N)
  end
    ensureAllInitialized!(fg)
    pl = drawPosesLandms(fg, spscale=0.1, drawhist=false)#,   meanmax=:mean,xmin=-3,xmax=6,ymin=-5,ymax=2);
    Gadfly.draw(PNG(joinpath(imgdir,"x$(psid).png"),15cm, 10cm),pl)
    pl = drawPosesLandms(fg, spscale=0.1)#,   meanmax=:mean,xmin=-3,xmax=3,ymin=-2,ymax=2);
    Gadfly.draw(PNG(joinpath(imgdir,"hist_x$(psid).png"),15cm, 10cm),pl)
    pl = plotPose2Vels(fg, Symbol("x$(psid)"), coord=Coord.Cartesian(xmin=-1.0, xmax=1.0))
    Gadfly.draw(PNG(joinpath(imgdir,"vels_x$(psid).png"),15cm, 10cm),pl)
  prev_psid = psid
end

IIF.savejld(fg, file=imgdir*"/racecar_fg_$(currdirtime).jld")

#
#
# ls(fg, :l7)
#
# stuff = IIF.localProduct(fg, :l7)
# plotKDE(stuff[2], levels=3, dims=[3])
#
# tag_bag[28][7][:tRYc]
# tag_bag[31][7][:tRYc]
#
#
# plotKDE(fg, :l1, dims=[3])
#
# getfnctype(getVert(fg, :x28l7f1, nt=:fnc)).z.μ
# getfnctype(getVert(fg, :x31l7f1, nt=:fnc)).z.μ
#
# ls(fg, :l17)
#
# getfnctype(getVert(fg, :x28l17f1, nt=:fnc)).z.μ
# getfnctype(getVert(fg, :x30l17f1, nt=:fnc)).z.μ
#
#
# # fg, = IncrementalInference.loadjld(file="30jul18_9AM.jld")
# # addFactor!(fg, [:l5;:l16], Point2Point2Range(Normal(0.5,1.0)))
# # ls(fg,:l5)
#
#
#
# fid = open("results.csv","w")
# for sym in [ls(fg)[1]...;ls(fg)[2]...]
#   p = getVertKDE(fg, sym)
#   val = string(KDE.getKDEMax(p))
#   println(fid, "$sym, $(val[2:(end-1)])")
# end
# close(fid)

0

#


text:
const _Constant = JuMP._Constant
const _VariableAffExpr{C} = GenericAffExpr{C, VariableRef}
const _DecisionAffExpr{C} = GenericAffExpr{C, DecisionRef}
const _KnownAffExpr{C} = GenericAffExpr{C, KnownRef}
const _VAE = _VariableAffExpr{Float64}
const _DAE = _DecisionAffExpr{Float64}
const _KAE = _KnownAffExpr{Float64}

mutable struct DecisionAffExpr{C} <: JuMP.AbstractJuMPScalar
    variables::GenericAffExpr{C, VariableRef}
    decisions::GenericAffExpr{C, DecisionRef}
    knowns::GenericAffExpr{C, KnownRef}
end
const DAE = DecisionAffExpr{Float64}

DAE() = zero(DAE{Float64})

# Base overrides #
# ========================== #
Base.iszero(aff::DecisionAffExpr) =
    iszero(aff.variables) &&
    iszero(aff.decisions) &&
    iszero(aff.knowns)
Base.zero(::Type{DecisionAffExpr{C}}) where C =
    DecisionAffExpr{C}(
        zero(_VariableAffExpr{C}),
        zero(_DecisionAffExpr{C}),
        zero(_KnownAffExpr{C}))
Base.one(::Type{DecisionAffExpr{C}}) where C =
    DecisionAffExpr{C}(one(_VariableAffExpr{C}),
                       zero(_DecisionAffExpr{C}),
                       zero(_KnownAffExpr{C}))
Base.zero(aff::DecisionAffExpr) = zero(typeof(aff))
Base.one(aff::DecisionAffExpr) =  one(typeof(aff))
Base.copy(aff::DecisionAffExpr{C}) where C =
    DecisionAffExpr{C}(copy(aff.variables),
                       copy(aff.decisions),
                       copy(aff.knowns))
Base.broadcastable(aff::DecisionAffExpr) = Ref(aff)

Base.convert(::Type{DecisionAffExpr{C}}, v::VariableRef) where {C} =
    DecisionAffExpr{C}(_VariableAffExpr{C}(zero(C), v => one(C)),
                       zero(_DecisionAffExpr{C}),
                       zero(_KnownAffExpr{C}))
Base.convert(::Type{DecisionAffExpr{C}}, dv::DecisionRef) where C =
    DecisionAffExpr{C}(zero(_VariableAffExpr{C}),
                       _DecisionAffExpr{C}(zero(C), dv => one(C)),
                       zero(_KnownAffExpr{C}))
Base.convert(::Type{DecisionAffExpr{C}}, kv::KnownRef) where C =
    DecisionAffExpr{C}(zero(_VariableAffExpr{C}),
                       zero(_DecisionAffExpr{C}),
                       GenericAffExpr{C, KnownRef}(zero(C), kv => one(C)))
Base.convert(::Type{DecisionAffExpr{C}}, c::Number) where C =
    DecisionAffExpr{C}(_VariableAffExpr{C}(convert(C, r)),
                       zero(_DecisionAffExpr{C}),
                       zero(_KnownAffExpr{C}))
Base.convert(::Type{DecisionAffExpr{C}}, aff::_VariableAffExpr{C}) where C =
    DecisionAffExpr{C}(aff,
                       zero(_DecisionAffExpr{C}),
                       zero(_KnownAffExpr{C}))
Base.convert(::Type{DecisionAffExpr{C}}, aff::_DecisionAffExpr{C}) where C =
    DecisionAffExpr{C}(zero(_VariableAffExpr{C}),
                       aff,
                       zero(_KnownAffExpr{C}))
Base.convert(::Type{DecisionAffExpr{C}}, aff::_KnownAffExpr{C}) where C =
    DecisionAffExpr{C}(zero(_VariableAffExpr{C}),
                       zero(_DecisionAffExpr{C}),
                       aff)
function Base.convert(::Type{T}, aff::DecisionAffExpr{T}) where T
    if !isempty(aff.variables.terms) && !isempty(aff.decisions.terms) && !isempty(aff.knowns.terms)
        throw(InexactError(:convert, T, aff))
    end
    return convert(T, aff.variables.constant)
end

function Base.isequal(aff::DecisionAffExpr{C}, other::DecisionAffExpr{C}) where {C}
    return isequal(aff.variables, other.variables) &&
        isequal(aff.decisions, other.decisions) &&
        isequal(aff.knowns, other.knowns)
end

Base.hash(aff::DecisionAffExpr, h::UInt) =
    hash(aff.variables.constant,
         hash(aff.variables.terms, h),
         hash(aff.decisions.terms, h),
         hash(aff.knowns.terms, h))

# JuMP overrides #
# ========================== #
function JuMP.drop_zeros!(aff::DecisionAffExpr)
    JuMP.drop_zeros!(aff.variables)
    JuMP.drop_zeros!(aff.decisions)
    JuMP.drop_zeros!(aff.knowns)
    return nothing
end

function JuMP._affine_coefficient(f::DecisionAffExpr, variable::VariableRef)
    return JuMP._affine_coefficient(f.variables, variable)
end
function JuMP._affine_coefficient(f::DecisionAffExpr, decision::DecisionRef)
    return JuMP._affine_coefficient(f.decisions, decision)
end
function JuMP._affine_coefficient(f::DecisionAffExpr, known::KnownRef)
    return JuMP._affine_coefficient(f.knowns, known)
end
function JuMP._affine_coefficient(f::_VariableAffExpr{C}, decision::DecisionRef) where C
    return zero(C)
end
function JuMP._affine_coefficient(f::_VariableAffExpr{C}, known::KnownRef) where C
    return zero(C)
end

function JuMP.map_coefficients_inplace!(f::Function, aff::DecisionAffExpr)
    JuMP.map_coefficients_inplace!(f, aff.variables)
    for (coef, dvar) in linear_terms(aff.decisions)
        aff.decisions.terms[dvar] = f(coef)
    end
    for (coef, kvar) in linear_terms(aff.knowns)
        aff.knowns.terms[kvar] = f(coef)
    end
    return aff
end

function JuMP.map_coefficients(f::Function, aff::DecisionAffExpr)
    return JuMP.map_coefficients_inplace!(f, copy(aff))
end

function JuMP.value(aff::DecisionAffExpr, value::Function)
    return JuMP.value(aff.variables, value) +
        JuMP.value(aff.decisions, value) +
        JuMP.value(aff.knowns, value)
end

function JuMP.constant(aff::DecisionAffExpr)
    return aff.variables.constant
end

function SparseArrays.dropzeros(aff::DecisionAffExpr{C}) where C
    variables = SparseArrays.dropzeros(aff.variables)
    decisions = SparseArrays.dropzeros(aff.decisions)
    knowns = SparseArrays.dropzeros(aff.knowns)
    return DecisionAffExpr(variables, decisions, knowns)
end

function JuMP._assert_isfinite(aff::Union{_DecisionAffExpr, _KnownAffExpr})
    for (coef, dv) in linear_terms(aff)
        isfinite(coef) || error("Invalid coefficient $coef on decision $dv.")
    end
    return nothing
end

function JuMP._assert_isfinite(aff::DecisionAffExpr)
    JuMP._assert_isfinite(aff.variables)
    JuMP._assert_isfinite(aff.decisions)
    JuMP._assert_isfinite(aff.knowns)
    return nothing
end

function JuMP.check_belongs_to_model(aff::DecisionAffExpr, model::AbstractModel)
    JuMP.check_belongs_to_model(aff.variables, model)
    JuMP.check_belongs_to_model(aff.decisions, model)
    JuMP.check_belongs_to_model(aff.knowns, model)
    return nothing
end

function JuMP.function_string(mode, aff::DecisionAffExpr, show_constant=true)
    # Decisions
    ret = ""
    decision_terms = JuMP.function_string(mode, aff.decisions, false)
    if decision_terms != "0"
        ret = decision_terms
    end
    # Variables
    variable_terms = JuMP.function_string(mode, aff.variables, false)
    ret = _add_terms(ret, variable_terms)
    # Knowns
    known_terms = JuMP.function_string(mode, aff.knowns, false)
    ret = _add_terms(ret, known_terms)
    # Constant
    if !JuMP._is_zero_for_printing(aff.variables.constant) && show_constant
        ret = string(ret, JuMP._sign_string(aff.variables.constant),

                     JuMP._string_round(abs(aff.variables.constant)))
    end
    return ret
end

function _add_terms(ret::String, terms::String)
    if terms == ""
        return ret
    end
    if terms != "0"
        if ret == ""
            ret = terms
        else
            if terms[1] == '-'
                ret = ret * " - " * terms[2:end]
            else
                ret = ret * " + " * terms
            end
        end
    end
    return ret
end

# With one factor.
function JuMP.add_to_expression!(aff::DAE, other::Number)
    JuMP.add_to_expression!(aff.variables, other)
    return aff
end

function JuMP.add_to_expression!(aff::DAE, new_var::VariableRef)
    JuMP.add_to_expression!(aff.variables, new_var)
    return aff
end

function JuMP.add_to_expression!(aff::DAE, new_dvar::DecisionRef)
    JuMP.add_to_expression!(aff.decisions, new_dvar)
    return aff
end

function JuMP.add_to_expression!(aff::DAE, new_kvar::KnownRef)
    JuMP.add_to_expression!(aff.knowns, new_kvar)
    return aff
end

function JuMP.add_to_expression!(aff::DAE, new_vae::_VAE)
    JuMP.add_to_expression!(aff.variables, new_vae)
    return aff
end

function JuMP.add_to_expression!(aff::DAE, new_dae::_DAE)
    JuMP.add_to_expression!(aff.decisions, new_dae)
    return aff
end

function JuMP.add_to_expression!(aff::DAE, new_kae::_KAE)
    JuMP.add_to_expression!(aff.knowns, new_kae)
    return aff
end

function JuMP.add_to_expression!(lhs_aff::DAE, rhs_aff::DAE)
    JuMP.add_to_expression!(lhs_aff.variables, rhs_aff.variables)
    JuMP.add_to_expression!(lhs_aff.decisions, rhs_aff.decisions)
    JuMP.add_to_expression!(lhs_aff.knowns, rhs_aff.knowns)
    return lhs_aff
end

# With two factors.
function JuMP.add_to_expression!(aff::DAE, new_coef::_Constant, new_var::VariableRef)
    JuMP.add_to_expression!(aff.variables, new_coef, new_var)
    return aff
end

function JuMP.add_to_expression!(aff::DAE, new_var::VariableRef, new_coef::_Constant)
    JuMP.add_to_expression!(aff.variables, new_coef, new_var)
    return aff
end

function JuMP.add_to_expression!(aff::DAE, new_coef::_Constant, new_dvar::DecisionRef)
    JuMP.add_to_expression!(aff.decisions, new_coef, new_dvar)
    return aff
end

function JuMP.add_to_expression!(aff::DAE, new_dvar::DecisionRef, new_coef::_Constant)
    JuMP.add_to_expression!(aff.decisions, new_coef, new_dvar)
    return aff
end

function JuMP.add_to_expression!(aff::DAE, new_coef::_Constant, new_kvar::KnownRef)
    JuMP.add_to_expression!(aff.knowns, new_coef, new_kvar)
    return aff
end

function JuMP.add_to_expression!(aff::DAE, new_kvar::KnownRef, new_coef::_Constant)
    JuMP.add_to_expression!(aff.knowns, new_coef, new_kvar)
    return aff
end

function JuMP.add_to_expression!(aff::DAE, new_coef::_Constant, new_vae::_VAE)
    JuMP.add_to_expression!(aff.variables, new_coef, new_vae)
    return aff
end

function JuMP.add_to_expression!(aff::DAE, new_vae::_VAE, new_coef::_Constant)
    JuMP.add_to_expression!(aff.variables, new_coef, new_vae)
    return aff
end

function JuMP.add_to_expression!(aff::DAE, new_coef::_Constant, new_dae::_DAE)
    JuMP.add_to_expression!(aff.decisions, new_coef, new_dae)
    return aff
end

function JuMP.add_to_expression!(aff::DAE, new_dae::_DAE, new_coef)
    JuMP.add_to_expression!(aff.decisions, new_coef, new_dae)
    return aff
end

function JuMP.add_to_expression!(aff::DAE, new_coef::_Constant, new_kae::_KAE)
    JuMP.add_to_expression!(aff.knowns, new_coef, new_kae)
    return aff
end

function JuMP.add_to_expression!(aff::DAE, new_kae::_KAE, new_coef::_Constant)
    JuMP.add_to_expression!(aff.knowns, new_coef, new_kae)
    return aff
end

function JuMP.add_to_expression!(lhs_aff::DAE, new_coef::_Constant, rhs_aff::DAE)
    JuMP.add_to_expression!(lhs_aff, new_coef, rhs_aff.variables)
    JuMP.add_to_expression!(lhs_aff, new_coef, rhs_aff.decisions)
    JuMP.add_to_expression!(lhs_aff, new_coef, rhs_aff.knowns)
    return lhs_aff
end

function JuMP.add_to_expression!(lhs_aff::DAE, rhs_aff::DAE, new_coef::_Constant)
    JuMP.add_to_expression!(lhs_aff, rhs_aff.variables, new_coef)
    JuMP.add_to_expression!(lhs_aff, rhs_aff.decisions, new_coef)
    JuMP.add_to_expression!(lhs_aff, rhs_aff.knowns, new_coef)
    return lhs_aff
end

ext: jl


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

@doc doc"""
    BAT.nparams(X::Union{AbstractParamBounds,MCMCIterator,...})

Get the number of parameters of `X`.
"""
function nparams end

abstract type AbstractParamBounds end

function Base.intersect(a::AbstractParamBounds, b::AbstractParamBounds)
    nparams(a) != nparams(b) && throw(ArgumentError("Can't intersect parameter bounds with different number of parameters"))
    unsafe_intersect(a, b)
end

@inline oob(::Type{T}) where {T<:AbstractFloat} = T(NaN)
@inline oob(::Type{T}) where {T<:Integer} = typemax(T)
@inline oob(x::Real) = oob(typeof(x))

@inline isoob(x::AbstractFloat) = isnan(x)
@inline isoob(x::Integer) = x == oob(x)
isoob(xs::AbstractVector) = any(isoob, xs)
# isoob(xs::VectorOfSimilarVectors) = any(isoob, flatview(xs))

@enum BoundsType hard_bounds=1 cyclic_bounds=2 reflective_bounds=3

function Base.intersect(a::BoundsType, b::BoundsType)
    if a == hard_bounds || b == hard_bounds
        hard_bounds
    elseif a == reflective_bounds || b == reflective_bounds
        reflective_bounds
    else
        cyclic_bounds
    end
end

@inline float_iseven(n::T) where {T<:AbstractFloat} = (n - T(2) * floor((n + T(0.5)) * T(0.5))) < T(0.5)

@doc doc"""
    apply_bounds!(params::AbstractVector, bounds::AbstractParamBounds)

*BAT-internal, not part of stable public API.*

Apply `bounds` to parameters `params`.
"""
function apply_bounds! end

@doc doc"""
    apply_bounds(x::<:Real, lo::<:Real, hi::<:Real, boundary_type::BoundsType)

*BAT-internal, not part of stable public API.*

Apply lower/upper bound `lo`/`hi` to value `x`. `boundary_type` may be
`hard_bounds`, `cyclic_bounds` or `reflective_bounds`.
"""
@inline function apply_bounds(x::X, lo::L, hi::H, boundary_type::BoundsType, oobval = oob(x)) where {X<:Real,L<:Real,H<:Real}
    T = float(promote_type(X, L, H))

    offs = ifelse(x < lo, lo - x, x - hi)
    hi_lo = hi - lo
    nwrapped = floor(offs / (hi_lo))
    even_nwrapped = float_iseven(nwrapped)
    wrapped_offs = muladd(-nwrapped, (hi_lo), offs)

    hb = (boundary_type == hard_bounds)
    rb = (boundary_type == reflective_bounds)

    ifelse(
        lo <= x <= hi,
        convert(T, x),
        ifelse(
            hb,
            convert(T, oobval),
            ifelse(
                (x < lo && (!rb || rb && !even_nwrapped)) || (x > lo && rb && even_nwrapped),
                convert(T, hi - wrapped_offs),
                convert(T, lo + wrapped_offs)
            )
        )
    )
end

@doc doc"""
    apply_bounds(x::Real, interval::ClosedInterval, boundary_type::BoundsType)

*BAT-internal, not part of stable public API.*

Specify lower and upper bound via `interval`.
"""
@inline apply_bounds(x::Real, interval::ClosedInterval, boundary_type::BoundsType, oobval = oob(x)) =
    apply_bounds(x, minimum(interval), maximum(interval), boundary_type, oobval)

struct NoParamBounds <: AbstractParamBounds
    ndims::Int
end

# TODO: Find better solution to determine param type if no bounds:
Base.eltype(bounds::NoParamBounds) = Float64

Base.in(params::AbstractVector, bounds::NoParamBounds) = true
Base.in(params::VectorOfSimilarVectors, bounds::NoParamBounds, i::Integer) = true

nparams(b::NoParamBounds) = b.ndims

apply_bounds!(params::Union{AbstractVector,VectorOfSimilarVectors}, bounds::NoParamBounds, setoob = true) = params

unsafe_intersect(a::NoParamBounds, b::NoParamBounds) = a
unsafe_intersect(a::AbstractParamBounds, b::NoParamBounds) = a
unsafe_intersect(a::NoParamBounds, b::AbstractParamBounds) = b

abstract type ParamVolumeBounds{T<:Real, V<:SpatialVolume{T}} <: AbstractParamBounds end

Base.in(params::AbstractVector, bounds::ParamVolumeBounds) = in(params, bounds.vol)

# Random.rand(rng::AbstractRNG, bounds::ParamVolumeBounds) =
#     rand!(rng, bounds, Vector{float(eltype(bounds))}(nparams(bounds)))

# Random.rand(rng::AbstractRNG, bounds::ParamVolumeBounds, n::Integer) =
#     rand!(rng, bounds, Matrix{float(eltype(bounds))}(nparams(bounds), n))
#
# Random.rand!(rng::AbstractRNG, bounds::ParamVolumeBounds, x::StridedVecOrMat{<:Real}) = rand!(rng, spatialvolume(bounds), x)

nparams(b::ParamVolumeBounds) = ndims(b.vol)

@doc doc"""
    spatialvolume(b::ParamVolumeBounds)::SpatialVolume

*BAT-internal, not part of stable public API.*

Returns the spatial volume that defines the parameter bounds.
"""
function spatialvolume end

struct HyperRectBounds{T<:Real} <: ParamVolumeBounds{T, HyperRectVolume{T}}
    vol::HyperRectVolume{T}
    bt::Vector{BoundsType}

    function HyperRectBounds{T}(vol::HyperRectVolume{T}, bt::AbstractVector{BoundsType}) where {T<:Real}
        nd = ndims(vol)
        axes(bt) != (1:nd,) && throw(ArgumentError("bt must have indices (1:ndims(vol),)"))
        (nd > 0) && isempty(vol) && throw(ArgumentError("Cannot create bounds with emtpy volume of $nd dimensions"))
        new{T}(vol, bt)
    end
end

HyperRectBounds(vol::HyperRectVolume{T}, bt::AbstractVector{BoundsType}) where {T<:Real} = HyperRectBounds{T}(vol, bt)
HyperRectBounds(lo::AbstractVector{T}, hi::AbstractVector{T}, bt::AbstractVector{BoundsType}) where {T<:Real} = HyperRectBounds(HyperRectVolume(lo, hi), bt)
HyperRectBounds(lo::AbstractVector{T}, hi::AbstractVector{T}, bt::BoundsType) where {T<:Real} = HyperRectBounds(lo, hi, fill(bt, size(lo, 1)))
HyperRectBounds(intervals::AbstractVector{<:ClosedInterval{<:Real}}, bt) = HyperRectBounds(minimum.(intervals), maximum.(intervals), bt)

Base.similar(bounds::HyperRectBounds) = HyperRectBounds(
        HyperRectVolume(
            fill!(similar(bounds.vol.lo), zero(eltype(bounds.vol.lo))),
            fill!(similar(bounds.vol.hi), one(eltype(bounds.vol.hi)))
        ),
        similar(bounds.bt)
    )

Base.eltype(bounds::HyperRectBounds{T}) where {T} = T

function Base.intersect(a::HyperRectBounds, b::HyperRectBounds)
    c = similar(a)
    for i in eachindex(a.vol.lo, a.vol.hi, a.bt, b.vol.lo, b.vol.hi, b.bt)
        iv_a = a.vol.lo[i]..a.vol.hi[i]
        iv_b = b.vol.lo[i]..b.vol.hi[i]
        if iv_a in iv_b
            c.bt[i] = a.bt[i]
        elseif iv_b in iv_a
            c.bt[i] = b.bt[i]
        else
            c.bt[i] = a.bt[i] ∩ b.bt[i]
        end

        iv_c = iv_a ∩ iv_b
        c.vol.lo[i] = minimum(iv_c)
        c.vol.hi[i] = maximum(iv_c)
    end
    c
end

function Base.vcat(xs::HyperRectBounds...)
    lo = vcat(map(x -> x.vol.lo, xs)...)
    hi = vcat(map(x -> x.vol.hi, xs)...)
    bt = vcat(map(x -> x.bt, xs)...)
    HyperRectBounds(lo, hi, bt)
end

spatialvolume(bounds::HyperRectBounds) = bounds.vol

function apply_bounds!(params::Union{AbstractVector,VectorOfSimilarVectors}, bounds::HyperRectBounds, setoob = true)
    params_flat = flatview(params)
    if setoob
        params_flat .= apply_bounds.(flatview(params), bounds.vol.lo, bounds.vol.hi, bounds.bt)
    else
        params_flat .= apply_bounds.(flatview(params), bounds.vol.lo, bounds.vol.hi, bounds.bt, params)
    end
    params
end


text:
using NeuroImages
using Base.Test
using AffineTransforms

arr = rand(8, 6, 5, 4)
img = NeuroImage(arr)

# reference space tests
@test voxel() != voxel()
@test mni() == mni()
@test coordsys(:MNI) == mni()
@test coordsys(:Talairach) == talairach()
@test mni() != talairach()
@test coordsys(img) == coordsys(img)
@test coordsys(NeuroImage(arr)) != coordsys(img)

# basic subsampling and physical, non-physical dimension tracking
@test ndims(img) == 4
@test phys_ndims(img) == 3
@test phys_size(img) == (8, 6, 5)
@test size(img) == (8, 6, 5, 4)
@test phys_ndims(img[2, 4:6, :, :]) == 3
@test ndims(img[2, 4:6, :, :]) == 4
@test phys_ndims(img[:, :, :, 4]) == 3
@test ndims(img[:, :, : 4]) == 3
@test phys_ndims(img[:, :, 2, 3]) == 3
@test ndims(img[:, :, 2, 3]) == 3

@test Array(img[2, 4:6, :, :]) == arr[2, 4:6, :, :]

# Voxel behaviour
@test img[2, 3, 5, 1] == arr[2, 3, 5, 1]
@test img[2, 3, 5, 1:3] == arr[2, 3, 5, 1:3]
@test location(img, 2, 3, 5, 1) == [2, 3, 5]
@test location(img, 2, 3, 5) == [2, 3, 5]
@test location(img, 2, 3) == [2, 3, 1]
@test location(img, img.coordsys, 2, 3, 5) == [2, 3, 5]
sub_img = img[4, 3:5, :, :]
@test location(sub_img, img.coordsys, 1, 1, 2) == [4, 3, 2]
@test sub_img[1, 1, 2] == arr[4, 3, 2]
@test location(sub_img, img.coordsys, 1, 2, 1) == [4, 4, 1]
@test sub_img[1, 2, 1] == arr[4, 4, 1]
@test location(sub_img, sub_img.coordsys, 1, 2, 1) == [1, 2, 1]

# Transformations
sub_img = img[4, 3:5, :, :]
@test transform_to(sub_img, sub_img) == sub_img
@test transform_to(img, sub_img) == sub_img
@test transform_to(img[3:end, :, :, :], sub_img) == sub_img
@test transform_to(img[3:end, :, :, 1], sub_img) == sub_img[:, :, :, 1]
sub_img2 = transform_to(img[:, :, 2:end, :], sub_img)
@test sub_img[:, :, 2:end, :] == sub_img2[:, :, 2:end, :]
@test all(isnan(sub_img2[:, :, 1:1, :, :])) # this slice is out of range

# Multiple transformations including standard space
push_transformation!(img, tformscale(2., 3), mni())
img2 = NeuroImage(rand(7, 7, 2))
push_transformation!(img2, tformtranslate([2, 0, 3]), mni(), img2.coordsys)
as_img2 = transform_to(img, img2)
@test as_img2[2, 2, 1] == img[2, 1, 2]
@test as_img2[4, 4, 1] == img[3, 2, 2]
img3 = NeuroImage(Array(img2))
push_transformation!(img, tformtranslate([-2, 0, -3]), img3.coordsys, mni())
@test all((Array(as_img2) .== Array(transform_to(img, img3))) | isnan(Array(as_img2)))
as_img = transform_to(img2, img)
@test all((Array(as_img) .== Array(transform_to(img3, img))) | isnan(Array(as_img)))

@test location(img, mni(), 2, 3) == [4, 6, 2]
@test location(img, img3.coordsys, 2, 3) == [2, 6, -1]

img_unz = read_nifti(joinpath(dirname(@__FILE__), "sample_image.nii"))
img_gz = read_nifti(joinpath(dirname(@__FILE__), "sample_image.nii.gz"))

@test Array(img_unz) == Array(img_gz)
@test_approx_eq(img_unz[13, 14, 16], 632.32965)
@test_approx_eq(location(img_unz, mni(), 13, 14, 16), [48.75, -47.25,  40.5])


text:
"""
    blended_conditional_gradient(f, grad!, lmo, x0)

Entry point for the Blended Conditional Gradient algorithm.
See Braun, Gábor, et al. "Blended conditonal gradients" ICML 2019.
The method works on an active set like [`FrankWolfe.away_frank_wolfe`](@ref),
performing gradient descent over the convex hull of active vertices,
removing vertices when their weight drops to 0 and adding new vertices
by calling the linear oracle in a lazy fashion.
"""
function blended_conditional_gradient(
    f,
    grad!,
    lmo,
    x0;
    line_search::LineSearchMethod=Adaptive(),
    line_search_inner::LineSearchMethod=Adaptive(),
    L=Inf,
    gamma0=0,
    hessian=nothing,
    step_lim=20,
    epsilon=1e-7,
    max_iteration=10000,
    print_iter=1000,
    trajectory=false,
    verbose=false,
    linesearch_tol=1e-7,
    emphasis=nothing,
    accelerated=false,
    K=2.0,
    weight_purge_threshold=1e-9,
    gradient=nothing,
    callback=nothing,
    timeout=Inf,
    print_callback=print_callback,
    lmo_kwargs...,
)

    # format string for output of the algorithm
    format_string = "%6s %13s %14e %14e %14e %14e %14e %14i %14i\n"

    t = 0
    primal = Inf
    dual_gap = Inf
    active_set = ActiveSet([(1.0, x0)])
    x = x0
    if gradient === nothing
        gradient = similar(x0)
    end
    primal = f(x)
    grad!(gradient, x)
    # initial gap estimate computation
    vmax = compute_extreme_point(lmo, gradient)
    phi = fast_dot(gradient, x0 - vmax) / 2
    dual_gap = phi
    traj_data = []
    if trajectory && callback === nothing
        callback = trajectory_callback(traj_data)
    end
    tt = regular
    time_start = time_ns()
    v = x0

    if line_search isa Shortstep && !isfinite(L)
        @error("Lipschitz constant not set to a finite value. Prepare to blow up spectacularly.")
    end

    if line_search isa Agnostic || line_search isa Nonconvex
        @error("Lazification is not known to converge with open-loop step size strategies.")
    end

    if line_search isa FixedStep && gamma0 == 0
        println("WARNING: gamma0 not set. We are not going to move a single bit.")
    end

    if verbose
        println("\nBlended Conditional Gradients Algorithm.")
        numType = eltype(x0)
        println(
            "EMPHASIS: $memory STEPSIZE: $line_search EPSILON: $epsilon MAXITERATION: $max_iteration TYPE: $numType",
        )
        grad_type = typeof(gradient)
        println("GRADIENTTYPE: $grad_type K: $K")
        println("WARNING: In memory emphasis mode iterates are written back into x0!")
        headers = (
            "Type",
            "Iteration",
            "Primal",
            "Dual",
            "Dual Gap",
            "Time",
            "It/sec",
            "#ActiveSet",
            "#non-simplex",
        )
        print_callback(headers, format_string, print_header=true)
    end
    # ensure x is a mutable type
    if !isa(x, Union{Array,SparseArrays.AbstractSparseArray})
        x = copyto!(similar(x), x)
    end
    non_simplex_iter = 0
    force_fw_step = false

    while t <= max_iteration && (phi ≥ epsilon || t == 0) # do at least one iteration for consistency with other algos

        #####################
        # managing time and Ctrl-C
        #####################
        time_at_loop = time_ns()
        if t == 0
            time_start = time_at_loop
        end
        # time is measured at beginning of loop for consistency throughout all algorithms
        tot_time = (time_at_loop - time_start) / 1e9

        if timeout < Inf
            if tot_time ≥ timeout
                if verbose
                    @info "Time limit reached"
                end
                break
            end
        end

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

        # TODO replace with single call interface from function_gradient.jl
        #Mininize over the convex hull until strong Wolfe gap is below a given tolerance.
        num_simplex_descent_steps = minimize_over_convex_hull!(
            f,
            grad!,
            gradient,
            active_set::ActiveSet,
            phi,
            t,
            time_start,
            non_simplex_iter,
            line_search_inner=line_search_inner,
            verbose=verbose,
            print_iter=print_iter,
            hessian=hessian,
            accelerated=accelerated,
            max_iteration=max_iteration,
            callback=callback,
            timeout=timeout,
            print_callback=print_callback,
            format_string=format_string,
        )
        t += num_simplex_descent_steps
        #Take a FW step.
        x = compute_active_set_iterate(active_set)
        primal = f(x)
        grad!(gradient, x)
        # compute new atom
        (v, value) = lp_separation_oracle(
            lmo,
            active_set,
            gradient,
            phi,
            K;
            inplace_loop=(emphasis == memory),
            force_fw_step=force_fw_step,
            lmo_kwargs...,
        )
        force_fw_step = false
        xval = fast_dot(x, gradient)
        if value > xval - phi / K
            tt = dualstep
            # setting gap estimate as ∇f(x) (x - v_FW) / 2
            phi = (xval - value) / 2
        else
            tt = regular
            gamma, L = line_search_wrapper(
                line_search,
                t,
                f,
                grad!,
                x,
                x - v,
                gradient,
                dual_gap,
                L,
                gamma0,
                linesearch_tol,
                step_lim,
                1.0,
            )

            if gamma == 1.0
                active_set_initialize!(active_set, v)
            else
                active_set_update!(active_set, gamma, v)
            end
        end

        x = compute_active_set_iterate(active_set)
        dual_gap = phi
        if callback !== nothing
            state = (
                t=t,
                primal=primal,
                dual=primal - dual_gap,
                dual_gap=dual_gap,
                time=tot_time,
                x=x,
                v=v,
                active_set_length=length(active_set),
                non_simplex_iter=non_simplex_iter,
            )
            callback(state)
        end

        if verbose && mod(t, print_iter) == 0
            if t == 0
                tt = initial
            end
            rep = (
                st[Symbol(tt)],
                string(t),
                Float64(primal),
                Float64(primal - dual_gap),
                Float64(dual_gap),
                tot_time,
                t / tot_time,
                length(active_set),
                non_simplex_iter,
            )
            print_callback(rep, format_string)
            flush(stdout)
        end

        t = t + 1
        non_simplex_iter += 1

    end

    ## post-processing and cleanup after loop

    # report last iteration
    if verbose
        x = compute_active_set_iterate(active_set)
        grad!(gradient, x)
        v = compute_extreme_point(lmo, gradient)
        primal = f(x)
        dual_gap = fast_dot(x, gradient) - fast_dot(v, gradient)
        tot_time = (time_ns() - time_start) / 1e9
        rep = (
            st[Symbol(last)],
            string(t - 1),
            Float64(primal),
            Float64(primal - dual_gap),
            Float64(dual_gap),
            tot_time,
            t / tot_time,
            length(active_set),
            non_simplex_iter,
        )
        print_callback(rep, format_string)
        flush(stdout)
    end

    # cleanup the active set, renormalize, and recompute values
    active_set_cleanup!(active_set, weight_purge_threshold=weight_purge_threshold)
    active_set_renormalize!(active_set)
    x = compute_active_set_iterate(active_set)
    grad!(gradient, x)
    v = compute_extreme_point(lmo, gradient)
    primal = f(x)
    #dual_gap = 2phi
    dual_gap = fast_dot(x, gradient) - fast_dot(v, gradient)

    # report post-processed iteration
    if verbose
        tot_time = (time_ns() - time_start) / 1e9
        rep = (
            st[Symbol(pp)],
            string(t - 1),
            Float64(primal),
            Float64(primal - dual_gap),
            Float64(dual_gap),
            tot_time,
            t / tot_time,
            length(active_set),
            non_simplex_iter,
        )
        print_callback(rep, format_string)
        print_callback(nothing, format_string, print_footer=true)
        flush(stdout)
    end
    return x, v, primal, dual_gap, traj_data
end

"""
    minimize_over_convex_hull!

Given a function f with gradient grad! and an active set
active_set this function will minimize the function over
the convex hull of the active set until the strong-wolfe
gap over the active set is below tolerance.

It will either directly minimize over the convex hull using
simplex gradient descent, or it will transform the problem
to barycentric coordinates and minimize over the unit
probability simplex using gradient descent or Nesterov's
accelerated gradient descent.
"""
function minimize_over_convex_hull!(
    f,
    grad!,
    gradient,
    active_set::ActiveSet,
    tolerance,
    t,
    time_start,
    non_simplex_iter;
    line_search_inner=Adaptive(),
    verbose=true,
    print_iter=1000,
    hessian=nothing,
    linesearch_tol=10e-10,
    step_lim=100,
    weight_purge_threshold=1e-12,
    storage=nothing,
    accelerated=false,
    max_iteration,
    callback,
    timeout=Inf,
    print_callback=nothing,
    format_string=nothing,
)
    #No hessian is known, use simplex gradient descent.
    if hessian === nothing
        number_of_steps = simplex_gradient_descent_over_convex_hull(
            f,
            grad!,
            gradient,
            active_set::ActiveSet,
            tolerance,
            t,
            time_start,
            non_simplex_iter,
            line_search_inner=line_search_inner,
            verbose=verbose,
            print_iter=print_iter,
            linesearch_tol=linesearch_tol,
            step_lim=step_lim,
            weight_purge_threshold=weight_purge_threshold,
            max_iteration=max_iteration,
            callback=callback,
            timeout=timeout,
            print_callback=print_callback,
            format_string=format_string,
        )
    else
        x = compute_active_set_iterate(active_set)
        grad!(gradient, x)
        #Rewrite as problem over the simplex
        M, b = build_reduced_problem(
            active_set.atoms,
            hessian,
            active_set.weights,
            gradient,
            tolerance,
        )
        #Early exit if we have detected that the strong-Wolfe gap is below the desired tolerance while building the reduced problem.
        if isnothing(M)
            return 0
        end
        #In case the matrices are DoubleFloats we need to cast them as Float64, because LinearAlgebra does not work with them.
        if eltype(M) === Double64
            converted_matrix = convert(Array{Float64}, M)
            L_reduced = eigmax(converted_matrix)
        else
            L_reduced = eigmax(M)
            #L_reduced = Arpack.eigs(M, nev=1, which=:LM)
        end
        reduced_f(y) =
            f(x) - fast_dot(gradient, x) +
            0.5 * transpose(x) * hessian * x +
            fast_dot(b, y) +
            0.5 * dot(y, M, y)
        function reduced_grad!(storage, x)
            return storage .= b + M * x
        end
        #Solve using Nesterov's AGD
        if accelerated
            if eltype(M) === Double64
                mu_reduced = eigmin(converted_matrix)
            else
                mu_reduced = eigmin(M)
            end
            if L_reduced / mu_reduced > 1.0
                new_weights, number_of_steps =
                    accelerated_simplex_gradient_descent_over_probability_simplex(
                        active_set.weights,
                        reduced_f,
                        reduced_grad!,
                        tolerance,
                        t,
                        time_start,
                        non_simplex_iter,
                        verbose=verbose,
                        print_iter=print_iter,
                        L=L_reduced,
                        mu=mu_reduced,
                        max_iteration=max_iteration,
                        callback=callback,
                        timeout=timeout,
                        print_callback=print_callback,
                        format_string=format_string,
                    )
                @. active_set.weights = new_weights
            end
        end
        if !accelerated || L_reduced / mu_reduced == 1.0
            #Solve using gradient descent.
            new_weights, number_of_steps = simplex_gradient_descent_over_probability_simplex(
                active_set.weights,
                reduced_f,
                reduced_grad!,
                tolerance,
                t,
                time_start,
                non_simplex_iter,
                verbose=verbose,
                print_iter=print_iter,
                L=L_reduced,
                max_iteration=max_iteration,
                callback=callback,
                timeout=timeout,
                print_callback=print_callback,
                format_string=format_string,
            )
            @. active_set.weights = new_weights
        end
    end
    active_set_cleanup!(active_set, weight_purge_threshold=weight_purge_threshold)
    return number_of_steps
end

"""
    build_reduced_problem(atoms::AbstractVector{<:AbstractVector}, hessian, weights, gradient, tolerance)

Given an active set formed by vectors , a (constant)
Hessian and a gradient constructs a quadratic problem
over the unit probability simplex that is equivalent to
minimizing the original function over the convex hull of the
active set. If λ are the barycentric coordinates of dimension
equal to the cardinality of the active set, the objective
function is:
    f(λ) = reduced_linear^T λ + 0.5 * λ^T reduced_hessian λ

In the case where we find that the current iterate has a strong-Wolfe
gap over the convex hull of the active set that is below the tolerance
we return nothing (as there is nothing to do).

"""
function build_reduced_problem(
    atoms::AbstractVector{<:ScaledHotVector},
    hessian,
    weights,
    gradient,
    tolerance,
)
    n = atoms[1].len
    k = length(atoms)
    reduced_linear = [fast_dot(gradient, a) for a in atoms]
    if strong_frankwolfe_gap(reduced_linear) <= tolerance
        return nothing, nothing
    end
    aux_matrix = zeros(eltype(atoms[1].active_val), n, k)
    #Compute the intermediate matrix.
    for i in 1:k
        aux_matrix[:, i] .= atoms[i].active_val * hessian[atoms[i].val_idx, :]
    end
    #Compute the final matrix.
    reduced_hessian = zeros(eltype(atoms[1].active_val), k, k)
    for i in 1:k
        reduced_hessian[:, i] .= atoms[i].active_val * aux_matrix[atoms[i].val_idx, :]
    end
    reduced_linear .-= reduced_hessian * weights
    return reduced_hessian, reduced_linear
end

function build_reduced_problem(
    atoms::AbstractVector{<:SparseArrays.AbstractSparseArray},
    hessian,
    weights,
    gradient,
    tolerance,
)
    n = length(atoms[1])
    k = length(atoms)

    reduced_linear = [fast_dot(gradient, a) for a in atoms]
    if strong_frankwolfe_gap(reduced_linear) <= tolerance
        return nothing, nothing
    end

    #Construct the matrix of vertices.
    vertex_matrix = spzeros(n, k)
    for i in 1:k
        vertex_matrix[:, i] .= atoms[i]
    end
    reduced_hessian = transpose(vertex_matrix) * hessian * vertex_matrix
    reduced_linear .-= reduced_hessian * weights
    return reduced_hessian, reduced_linear
end

function build_reduced_problem(
    atoms::AbstractVector{<:AbstractVector},
    hessian,
    weights,
    gradient,
    tolerance,
)
    n = length(atoms[1])
    k = length(atoms)

    reduced_linear = [fast_dot(gradient, a) for a in atoms]
    if strong_frankwolfe_gap(reduced_linear) <= tolerance
        return nothing, nothing
    end

    #Construct the matrix of vertices.
    vertex_matrix = zeros(n, k)
    for i in 1:k
        vertex_matrix[:, i] .= atoms[i]
    end
    reduced_hessian = transpose(vertex_matrix) * hessian * vertex_matrix
    reduced_linear .-= reduced_hessian * weights
    return reduced_hessian, reduced_linear
end

"""
Checks the strong Frank-Wolfe gap for the reduced problem.
"""
function strong_frankwolfe_gap(gradient)
    val_min = Inf
    val_max = -Inf
    for i in 1:length(gradient)
        temp_val = gradient[i]
        if temp_val < val_min
            val_min = temp_val
        end
        if temp_val > val_max
            val_max = temp_val
        end
    end
    return val_max - val_min
end

"""
    accelerated_simplex_gradient_descent_over_probability_simplex

Minimizes an objective function over the unit probability simplex
until the Strong-Wolfe gap is below tolerance using Nesterov's
accelerated gradient descent.
"""
function accelerated_simplex_gradient_descent_over_probability_simplex(
    initial_point,
    reduced_f,
    reduced_grad!,
    tolerance,
    t,
    time_start,
    non_simplex_iter;
    verbose=verbose,
    print_iter=print_iter,
    L=1.0,
    mu=1.0,
    max_iteration,
    callback,
    timeout=Inf,
    print_callback=nothing,
    format_string=nothing,
)
    number_of_steps = 0
    x = deepcopy(initial_point)
    x_old = deepcopy(initial_point)
    y = deepcopy(initial_point)
    gradient_x = similar(x)
    gradient_y = similar(x)
    d = similar(x)
    reduced_grad!(gradient_x, x)
    reduced_grad!(gradient_y, x)
    strong_wolfe_gap = strong_frankwolfe_gap_probability_simplex(gradient_x, x)
    q = mu / L
    # If the problem is close to convex, simply use the accelerated algorithm for convex objective functions.
    if mu < 1.0e-3
        alpha = 0.0
        alpha_old = 0.0
    else
        gamma = (1 - sqrt(q)) / (1 + sqrt(q))
    end
    while strong_wolfe_gap > tolerance && t + number_of_steps <= max_iteration
        @. x_old = x
        reduced_grad!(gradient_y, y)
        x = projection_simplex_sort(y .- gradient_y / L)
        if mu < 1.0e-3
            alpha_old = alpha
            alpha = 0.5 * (1 + sqrt(1 + 4 * alpha^2))
            gamma = (alpha_old - 1.0) / alpha
        end
        @. y = x + gamma * (x - x_old)
        number_of_steps += 1
        primal = reduced_f(x)
        reduced_grad!(gradient_x, x)
        strong_wolfe_gap = strong_frankwolfe_gap_probability_simplex(gradient_x, x)

        if callback !== nothing
            state = (
                t=t + number_of_steps,
                primal=primal,
                dual=primal - tolerance,
                dual_gap=tolerance,
                time=(time_ns() - time_start) / 1e9,
                x=x,
            )
            callback(state)
        end
        tt = simplex_descent
        if verbose && mod(t + number_of_steps, print_iter) == 0
            if t == 0
                tt = initial
            end
            tot_time = (time_ns() - time_start) / 1.0e9
            rep = (
                st[Symbol(tt)],
                string(t + number_of_steps),
                Float64(primal),
                Float64(primal - tolerance),
                Float64(tolerance),
                tot_time,
                t / tot_time,
                length(initial_point),
                non_simplex_iter,
            )
            print_callback(rep, format_string)
            flush(stdout)
        end
        if timeout < Inf
            tot_time = (time_ns() - time_start) / 1e9
            if tot_time ≥ timeout
                if verbose
                    @info "Time limit reached"
                end
                break
            end
        end
    end
    return x, number_of_steps
end

"""
    simplex_gradient_descent_over_probability_simplex

Minimizes an objective function over the unit probability simplex
until the Strong-Wolfe gap is below tolerance using gradient descent.
"""
function simplex_gradient_descent_over_probability_simplex(
    initial_point,
    reduced_f,
    reduced_grad!,
    tolerance,
    t,
    time_start,
    non_simplex_iter;
    verbose=verbose,
    print_iter=print_iter,
    L=1.0,
    max_iteration,
    callback,
    timeout=Inf,
    print_callback=nothing,
    format_string=nothing,
)
    number_of_steps = 0
    x = deepcopy(initial_point)
    gradient = similar(x)
    d = similar(x)
    reduced_grad!(gradient, x)
    strong_wolfe_gap = strong_frankwolfe_gap_probability_simplex(gradient, x)
    while strong_wolfe_gap > tolerance && t + number_of_steps <= max_iteration
        x = projection_simplex_sort(x .- gradient / L)
        number_of_steps = number_of_steps + 1
        primal = reduced_f(x)
        reduced_grad!(gradient, x)
        strong_wolfe_gap = strong_frankwolfe_gap_probability_simplex(gradient, x)

        if callback !== nothing
            state = (
                t=t + number_of_steps,
                primal=primal,
                dual=primal - tolerance,
                dual_gap=tolerance,
                time=(time_ns() - time_start) / 1e9,
                x=x,
            )
            callback(state)
        end
        tt = simplex_descent
        if verbose && mod(t + number_of_steps, print_iter) == 0
            if t == 0
                tt = initial
            end
            tot_time = (time_ns() - time_start) / 1.0e9
            rep = (
                st[Symbol(tt)],
                string(t + number_of_steps),
                Float64(primal),
                Float64(primal - tolerance),
                Float64(tolerance),
                tot_time,
                t / tot_time,
                length(initial_point),
                non_simplex_iter,
            )
            print_callback(rep, format_string)
            flush(stdout)
        end
        if timeout < Inf
            tot_time = (time_ns() - time_start) / 1e9
            if tot_time ≥ timeout
                if verbose
                    @info "Time limit reached"
                end
                break
            end
        end
    end
    return x, number_of_steps
end

"""
    projection_simplex_sort(x; s=1.0)

Perform a projection onto the probability simplex of radius `s`
using a sorting algorithm.
"""
function projection_simplex_sort(x; s=1.0)
    n = length(x)
    if sum(x) == s && all(>=(0.0), x)
        return x
    end
    v = x .- maximum(x)
    u = sort(v, rev=true)
    cssv = cumsum(u)
    rho = sum(u .* collect(1:1:n) .> (cssv .- s)) - 1
    theta = (cssv[rho+1] - s) / (rho + 1)
    w = clamp.(v .- theta, 0.0, Inf)
    return w
end

"""
    strong_frankwolfe_gap_probability_simplex

Compute the Strong-Wolfe gap over the unit probability simplex
given a gradient.
"""
function strong_frankwolfe_gap_probability_simplex(gradient, x)
    val_min = Inf
    val_max = -Inf
    for i in 1:length(gradient)
        if x[i] > 0
            temp_val = gradient[i]
            if temp_val < val_min
                val_min = temp_val
            end
            if temp_val > val_max
                val_max = temp_val
            end
        end
    end
    return val_max - val_min
end

"""
    simplex_gradient_descent_over_convex_hull(f, grad!, gradient, active_set, tolerance, t, time_start, non_simplex_iter)

Minimizes an objective function over the convex hull of the active set
until the Strong-Wolfe gap is below tolerance using simplex gradient
descent.
"""
function simplex_gradient_descent_over_convex_hull(
    f,
    grad!,
    gradient,
    active_set::ActiveSet,
    tolerance,
    t,
    time_start,
    non_simplex_iter;
    line_search_inner=Adaptive(),
    verbose=true,
    print_iter=1000,
    hessian=nothing,
    linesearch_tol=10e-10,
    step_lim=100,
    weight_purge_threshold=1e-12,
    max_iteration,
    callback,
    timeout=Inf,
    print_callback=nothing,
    format_string=nothing,
)
    number_of_steps = 0
    L_inner = nothing
    upgrade_accuracy_flag = false
    x = compute_active_set_iterate(active_set)
    while t + number_of_steps ≤ max_iteration
        grad!(gradient, x)
        #Check if strong Wolfe gap over the convex hull is small enough.
        c = [fast_dot(gradient, a) for a in active_set.atoms]
        if maximum(c) - minimum(c) <= tolerance || t + number_of_steps ≥ max_iteration
            return number_of_steps
        end
        #Otherwise perform simplex steps until we get there.
        k = length(active_set)
        csum = sum(c)
        c .-= (csum / k)
        # name change to stay consistent with the paper, c is actually updated in-place
        d = c
        # NOTE: sometimes the direction is non-improving
        # usual suspects are floating-point errors when multiplying atoms with near-zero weights
        # in that case, inverting the sense of d
        # Computing the quantity below is the same as computing the <-\nabla f(x), direction>.
        # If <-\nabla f(x), direction>  >= 0 the direction is a descent direction.
        descent_direction_product = fast_dot(d, d) + (csum / k) * sum(d)
        @inbounds if descent_direction_product < 0
            current_iteration = t + number_of_steps
            @warn "Non-improving d ($descent_direction_product) due to numerical instability in iteration $current_iteration. Temporarily upgrading precision to BigFloat for the current iteration."
            # extended warning - we can discuss what to integrate
            # If higher accuracy is required, consider using Double64 (still quite fast) and if that does not help BigFloat (slower) as type for the numbers.
            # Alternatively, consider using AFW (with lazy = true) instead."
            bdir = big.(gradient)
            c = [fast_dot(bdir, a) for a in active_set.atoms]
            csum = sum(c)
            c .-= csum / k
            d = c
            descent_direction_product_inner = fast_dot(d, d) + (csum / k) * sum(d)
            @inbounds if descent_direction_product_inner < 0
                @warn "d non-improving in large precision, forcing FW"
                @warn "dot value: $descent_direction_product_inner"
                return number_of_steps
            end
        end

        η = eltype(d)(Inf)
        rem_idx = -1
        @inbounds for idx in eachindex(d)
            if d[idx] > 0
                max_val = active_set.weights[idx] / d[idx]
                if η > max_val
                    η = max_val
                    rem_idx = idx
                end
            end
        end
        # TODO at some point avoid materializing both x and y
        x = copy(active_set.x)
        η = max(0, η)
        @. active_set.weights -= η * d
        y = copy(update_active_set_iterate!(active_set))
        number_of_steps += 1
        if f(x) ≥ f(y)
            active_set_cleanup!(active_set, weight_purge_threshold=weight_purge_threshold)
        else
            if line_search_inner isa Adaptive
                gamma, L_inner = adaptive_step_size(
                    f,
                    grad!,
                    gradient,
                    x,
                    x - y,
                    L_inner,
                    gamma_max=1.0,
                    upgrade_accuracy=upgrade_accuracy_flag,
                )
                #If the stepsize is that small we probably need to increase the accuracy of
                #the types we are using.
                if gamma < eps()
                    #@warn "Upgrading the accuracy of the adaptive line search."
                    L_inner = nothing
                    gamma, L_inner = adaptive_step_size(
                        f,
                        grad!,
                        gradient,
                        x,
                        x - y,
                        L_inner,
                        gamma_max=1.0,
                        upgrade_accuracy=true,
                    )
                end
            else
                gamma, _ = backtrackingLS(
                    f,
                    gradient,
                    x,
                    x - y,
                    1.0,
                    linesearch_tol=linesearch_tol,
                    step_lim=step_lim,
                )
            end
            gamma = min(1.0, gamma)
            # step back from y to x by (1 - γ) η d
            # new point is x - γ η d
            if gamma == 1.0
                active_set_cleanup!(active_set, weight_purge_threshold=weight_purge_threshold)
            else
                @. active_set.weights += η * (1 - gamma) * d
                @. active_set.x = x + gamma * (y - x)
            end
        end
        x = compute_active_set_iterate(active_set)
        primal = f(x)
        dual_gap = tolerance

        if callback !== nothing
            state = (
                t=t,
                primal=primal,
                dual=primal - dual_gap,
                dual_gap=dual_gap,
                time=(time_ns() - time_start) / 1e9,
                x=x,
                active_set_length=length(active_set),
                non_simplex_iter=non_simplex_iter,
            )
            callback(state)
        end
        tt = simplex_descent
        if verbose && mod(t + number_of_steps, print_iter) == 0
            if t == 0
                tt = initial
            end
            tot_time = (time_ns() - time_start) / 1.0e9
            rep = (
                st[Symbol(tt)],
                string(t + number_of_steps),
                Float64(primal),
                Float64(primal - dual_gap),
                Float64(dual_gap),
                tot_time,
                t / tot_time,
                length(active_set),
                non_simplex_iter,
            )
            print_callback(rep, format_string)
            flush(stdout)
        end
        if timeout < Inf
            tot_time = (time_ns() - time_start) / 1e9
            if tot_time ≥ timeout
                if verbose
                    @info "Time limit reached"
                end
                break
            end
        end

    end
    return number_of_steps
end

"""
Returns either a tuple `(y, val)` with `y` an atom from the active set satisfying
the progress criterion and `val` the corresponding gap `dot(y, direction)`
or the same tuple with `y` from the LMO.

`inplace_loop` controls whether the iterate type allows in-place writes.
`kwargs` are passed on to the LMO oracle.
"""
function lp_separation_oracle(
    lmo::LinearMinimizationOracle,
    active_set::ActiveSet,
    direction,
    min_gap,
    K;
    inplace_loop=false,
    force_fw_step::Bool=false,
    kwargs...,
)
    # if FW step forced, ignore active set
    if !force_fw_step
        ybest = active_set.atoms[1]
        x = active_set.weights[1] * active_set.atoms[1]
        if inplace_loop
            if !isa(x, Union{Array,SparseArrays.AbstractSparseArray})
                if x isa AbstractVector
                    x = convert(SparseVector{eltype(x)}, x)
                else
                    x = convert(SparseArrays.SparseMatrixCSC{eltype(x)}, x)
                end
            end
        end
        val_best = fast_dot(direction, ybest)
        for idx in 2:length(active_set)
            y = active_set.atoms[idx]
            if inplace_loop
                x .+= active_set.weights[idx] * y
            else
                x += active_set.weights[idx] * y
            end
            val = fast_dot(direction, y)
            if val < val_best
                val_best = val
                ybest = y
            end
        end
        xval = fast_dot(direction, x)
        if xval - val_best ≥ min_gap / K
            return (ybest, val_best)
        end
    end
    # otherwise, call the LMO
    y = compute_extreme_point(lmo, direction; kwargs...)
    # don't return nothing but y, fast_dot(direction, y) / use y for step outside / and update phi as in LCG (lines 402 - 406)
    return (y, fast_dot(direction, y))
end


text:
const PWGRID_VERSION = 2

mutable struct GVectorsW
    Ngwx::Int
    idx_gw2r::Array{Int}
end

mutable struct GVectors
    Ng::Int
    G::Array{Float64,2}
    G2::Array{Float64}
end

mutable struct PWGrid
    Ns::Array{Int64}
    LatVecs::Array{Float64,2}
    RecVecs::Array{Float64,2}
    Ω::Float64
    r::Array{Float64,2}
    gvec::GVectors
    gvecw::GVectorsW
end

function PWGrid( Ns, LatVecs::Array{Float64,2} )

    if any( Ns .% 2 .== 1 )
        @printf("Error: Ns must be even numbers\n")
        exit()
    end

    RecVecs = 2*pi*inv(LatVecs')
    Ω = det(LatVecs)
    #
    LatVecsLen = Array{Float64}(undef,3)
    LatVecsLen[1] = norm(LatVecs[:,1])
    LatVecsLen[2] = norm(LatVecs[:,2])
    LatVecsLen[3] = norm(LatVecs[:,3])

    Npoints = prod(Ns)
    r = init_grid_R( Ns, LatVecs )

    gvec = init_grid_G( Ns, RecVecs )
    gvecw = init_gvecw( Ns, gvec.G2 )

    return PWGrid( Ns, LatVecs, RecVecs, Ω, r, gvec, gvecw )
end

function mm_to_nn(mm::Int,S::Int)
    if mm > S/2
        return mm - S
    else
        return mm
    end
end

function init_grid_G( Ns, RecVecs )

    Ng = prod(Ns)

    G  = Array{Float64}(undef,3,Ng)
    G2 = Array{Float64}(undef,Ng)

    ig = 0
    for k in 0:Ns[3]-1
    for j in 0:Ns[2]-1
    for i in 0:Ns[1]-1
        ig = ig + 1
        gi = mm_to_nn( i, Ns[1] )
        gj = mm_to_nn( j, Ns[2] )
        gk = mm_to_nn( k, Ns[3] )
        G[1,ig] = RecVecs[1,1]*gi + RecVecs[1,2]*gj + RecVecs[1,3]*gk
        G[2,ig] = RecVecs[2,1]*gi + RecVecs[2,2]*gj + RecVecs[2,3]*gk
        G[3,ig] = RecVecs[3,1]*gi + RecVecs[3,2]*gj + RecVecs[3,3]*gk
        G2[ig] = G[1,ig]^2 + G[2,ig]^2 + G[3,ig]^2
    end
    end
    end

    return GVectors( Ng, G, G2 )
end

function init_gvecw( Ns, G2 )
    edges = find_edges( Ns )
    G2mx = minimum( G2[edges] )
    #@printf("G2mx = %f\n", G2mx)
    @printf("ecutwfc_Ry = %f\n", G2mx/4)
    idx_gw2r = findall( G2 .< G2mx/4 )
    Ngwx = length(idx_gw2r)
    return GVectorsW( Ngwx, idx_gw2r )
end

function find_edges( Ns )
    eS = Ns/2 .+ 0.5
    edges = []
    ip = 1
    for k in 0:Ns[3]-1
    for j in 0:Ns[2]-1
    for i in 0:Ns[1]-1
        ei = abs(i - eS[1])
        ej = abs(j - eS[2])
        ek = abs(k - eS[3])
        # if any of i, j, or k is equal to Ns/2 or Ns/2+1
        if ei < 1.0 || ej < 1.0 || ek < 1.0
            push!(edges,ip)
        end
        ip = ip + 1
    end
    end
    end
    return edges
end

function init_grid_R( Ns, LatVecs )
    #
    Npoints = prod(Ns)
    #
    R = Array{Float64}(undef,3,Npoints)
    ip = 0
    for k in 0:Ns[3]-1
    for j in 0:Ns[2]-1
    for i in 0:Ns[1]-1
        ip = ip + 1
        R[1,ip] = LatVecs[1,1]*i/Ns[1] + LatVecs[1,2]*j/Ns[2] + LatVecs[1,3]*k/Ns[3]
        R[2,ip] = LatVecs[2,1]*i/Ns[1] + LatVecs[2,2]*j/Ns[2] + LatVecs[2,3]*k/Ns[3]
        R[3,ip] = LatVecs[3,1]*i/Ns[1] + LatVecs[3,2]*j/Ns[2] + LatVecs[3,3]*k/Ns[3]
    end
    end
    end
    #
    return R
end


text:
using FDM

export check_Dv, check_Dv_update, check_errs,
       assert_approx_equal, domain1, domain2, points, in_domain

"""
    approximate_Dv(
        f,
        ȳ::∇ArrayOrScalar,
        x::Tuple{Vararg{∇ArrayOrScalar}},
        v::Tuple{Vararg{∇ArrayOrScalar}}
    )
    approximate_Dv(f::Function, ȳ::∇ArrayOrScalar, x::∇ArrayOrScalar, v::∇ArrayOrScalar)

Estimate the directional derivative of `f` at `x` in direction `v`. If the function has
multiple arguments, `x` and `v` should be `Tuple`s of inputs and directions respectively.
"""
function approximate_Dv(
    f,
    ȳ::∇ArrayOrScalar,
    x::Tuple{Vararg{∇ArrayOrScalar}},
    v::Tuple{Vararg{∇ArrayOrScalar}}
)
    return FDM.Central(5, 1)(ε -> sum(ȳ .* f((x .+ ε .* v)...)))
end

approximate_Dv(f, ȳ::∇ArrayOrScalar, x::∇ArrayOrScalar, v::∇ArrayOrScalar) =
    approximate_Dv(f, ȳ, (x,), (v,))

"""
    compute_Dv(
        f,
        ȳ::∇ArrayOrScalar,
        x::Tuple{Vararg{∇ArrayOrScalar}},
        v::Tuple{Vararg{∇ArrayOrScalar}}
    )

Compute the directional derivative of `f` at `x` in direction `v` using AD. Use this
result to back-propagate the sensitivity ȳ. If ȳ, x and v are column vectors, then this is
equivalent to computing `ȳ'(J f)(x) v`, where `(J f)(x)` denotes the Jacobian of `f`
evaluated at `x`. Analogous operations happen for scalars and N-dimensional arrays.
"""
function compute_Dv(
    f,
    ȳ::∇ArrayOrScalar,
    x::Tuple{Vararg{∇ArrayOrScalar}},
    v::Tuple{Vararg{∇ArrayOrScalar}}
)
    x_ = Leaf.(Tape(), x)
    ∇f = ∇(f(x_...), ȳ)
    return sum(map((x, v)->sum(∇f[x] .* v), x_, v))
end
compute_Dv(f, ȳ::∇ArrayOrScalar, x::∇ArrayOrScalar, v::∇ArrayOrScalar) =
    compute_Dv(f, ȳ, (x,), (v,))

function compute_Dv_update(
    f,
    ȳ::∇ArrayOrScalar,
    x::Tuple{Vararg{∇ArrayOrScalar}},
    v::Tuple{Vararg{∇ArrayOrScalar}}
)
    x_ = Leaf.(Tape(), x)
    y = f(x_...)
    rtape = reverse_tape(y, ȳ)

    # Randomly initialise `Leaf`s.
    inits = Vector(undef, length(rtape))
    for i = 1:length(rtape)
        if isleaf(tape(y)[i])
            inits[i] = randned_container(unbox(tape(y)[i]))
            rtape[i] = copy(inits[i])
        end
    end

    # Perform the reverse pass.
    ∇f = propagate(tape(y), rtape)

    # Substract the random initialisations.
    for i = 1:length(rtape)
        isleaf(tape(y)[i]) && (∇f[i] -= inits[i])
    end

    return sum(map((x, v)->sum(∇f[x] .* v), x_, v))
end
compute_Dv_update(f, ȳ::∇ArrayOrScalar, x::∇ArrayOrScalar, v::∇ArrayOrScalar) =
    compute_Dv_update(f, ȳ, (x,), (v,))
isleaf(::Leaf) = true
isleaf(::Any) = false

"""
    check_errs(
        f,
        ȳ::∇ArrayOrScalar,
        x::T,
        v::T,
        ε_abs::∇Scalar=1e-10,
        ε_rel::∇Scalar=1e-7
    )::Bool where T

Check that the difference between finite differencing directional derivative estimation and
RMAD directional derivative computation for function `f` at `x` in direction `v`, for both
allocating and in-place modes, has absolute and relative errors of `ε_abs` and `ε_rel`
respectively, when scaled by reverse-mode sensitivity `ȳ`.
"""
function check_errs(
    f,
    ȳ::∇ArrayOrScalar,
    x::T,
    v::T,
    ε_abs::∇Scalar=1e-10,
    ε_rel::∇Scalar=1e-7
)::Bool where T
    ∇x_alloc = compute_Dv(f, ȳ, x, v)
    ∇x_inplace = compute_Dv_update(f, ȳ, x, v)
    ∇x_fin_diff = approximate_Dv(f, ȳ, x, v)
    assert_approx_equal(∇x_alloc, ∇x_fin_diff, ε_abs, ε_rel, "<$f> allocated")
    assert_approx_equal(∇x_inplace, ∇x_fin_diff, ε_abs, ε_rel, "<$f> in-place")
    return true
end

"""
    in_domain(f::Function, x::Float64...)

Check whether an input `x` is in a scalar, real function `f`'s domain.
"""
function in_domain(f::Function, x::Float64...)
    try
        y = f(x...)
        return isa(y, Real) && !isnan(y)
    catch err
        return isa(err, DomainError) ? false : throw(err)
    end
end

# Test points that are used to determine functions's domains.
points = [-π + .1, -.5π + .1, -.9, -.1, .1, .9, .5π - .1, π - .1]

"""
    domain1{T}(in_domain::Function, measure::Function, points::Vector{T})
    domain1(f::Function)

Attempt to find a domain for a unary, scalar function `f`.

# Arguments
- `in_domain::Function`: Function that takes a single argument `x` and returns whether `x`
    argument is in `f`'s domain.
- `measure::Function`: Function that measures the size of a set of points for `f`.
- `points::Vector{T}`: Ordered set of test points to construct the domain from.
"""
function domain1(in_domain::Function, measure::Function, points::Vector{T}) where T
   # Find the connected sets of points that are in f's domain.
   connected_sets, set = Vector{Vector{T}}(), Vector{T}()
   for x in points
       if in_domain(x)
           push!(set, x)
       else
           if length(set) > 0
               push!(connected_sets, set)
               set = Vector{T}()
           end
       end
   end

   # Add the possibly yet unadded set.
   length(set) > 0 && push!(connected_sets, set)

   # Return nothing if no domain could be found.
   length(connected_sets) == 0 && return

   # Pick the largest domain.
   return connected_sets[argmax(measure.(connected_sets))]
end

function domain1(f::Function)
    set = domain1(x -> in_domain(f, x), x -> maximum(x) - minimum(x), points)
    set === nothing && return
    return (minimum(set), maximum(set))
end

"""
    Slice2

Slice of a Float64 x Float64 domain.
"""
mutable struct Slice2
    x::Float64
    y_range::Union{Tuple{Float64, Float64}, Nothing}
end

"""
    domain2(f::Function)

Attempt to find a rectangular domain for a binary, scalar function `f`.
"""
function domain2(f::Function)
    # Construct slices for all x in points.
    slices = Slice2.(points, [domain1(y -> f(x, y)) for x in points])

    # Extract a set of in-domain slices.
    measure = x -> maximum(getfield.(x, :x)) - minimum(getfield.(x, :x))
    in_domain_slices = domain1(x -> x.y_range !== nothing, measure, slices)
    in_domain_slices === nothing && return

    # Extract the x range of the domain.
    xs = getfield.(in_domain_slices, :x)
    x_range = (minimum(xs), maximum(xs))

    # Extract the y range of the domain.
    y_ranges = getfield.(in_domain_slices, :y_range)
    y_lower, y_upper = maximum(getindex.(y_ranges, 1)), minimum(getindex.(y_ranges, 2))
    y_lower >= y_upper && return
    y_range = (y_lower, y_upper)

    return (x_range, y_range)
end

# `beta`s domain cannot be determined correctly, since `beta(-.2, -.2)` doesn't throw an
# error, strangely enough.
domain2(::typeof(beta)) = ((minimum(points[points .> 0]), maximum(points)),
                           (minimum(points[points .> 0]), maximum(points)))

# Both of these functions are technically defined on the entire real line, but the left
# half is troublesome due to the large number of points at which it isn't defined. As such
# we restrict unit testing to the right-half.
domain1(::typeof(gamma)) = (minimum(points[points .> 0]), maximum(points[points .> 0]))
domain1(::typeof(trigamma)) = (minimum(points[points .> 0]), maximum(points[points .> 0]))


text:
module SpinGlassNetworks
    using LabelledGraphs
    using LightGraphs
    using MetaGraphs # TODO: remove that
    using CSV
    using DocStringExtensions
    using LinearAlgebra
    using Base.Cartesian

    import Base.Prehashed

    export unique_neighbors

    unique_neighbors(ig::LabelledGraph, i::Int) = filter(j -> j > i, neighbors(ig, i))

    @generated function unique_dims(A::AbstractArray{T,N}, dim::Integer) where {T,N}
        quote
            1 <= dim <= $N || return copy(A)
            hashes = zeros(UInt, axes(A, dim))

            # Compute hash for each row
            k = 0
            @nloops $N i A d->(if d == dim; k = i_d; end) begin
                @inbounds hashes[k] = hash(hashes[k], hash((@nref $N A i)))
            end

            # Collect index of first row for each hash
            uniquerow = similar(Array{Int}, axes(A, dim))
            firstrow = Dict{Prehashed,Int}()
            for k = axes(A, dim)
                uniquerow[k] = get!(firstrow, Prehashed(hashes[k]), k)
            end
            uniquerows = collect(values(firstrow))

            # Check for collisions
            collided = falses(axes(A, dim))
            @inbounds begin
                @nloops $N i A d->(if d == dim
                    k = i_d
                    j_d = uniquerow[k]
                else
                    j_d = i_d
                end) begin
                    if (@nref $N A j) != (@nref $N A i)
                        collided[k] = true
                    end
                end
            end

            if any(collided)
                nowcollided = similar(BitArray, axes(A, dim))
                while any(collided)
                    # Collect index of first row for each collided hash
                    empty!(firstrow)
                    for j = axes(A, dim)
                        collided[j] || continue
                        uniquerow[j] = get!(firstrow, Prehashed(hashes[j]), j)
                    end
                    for v ∈ values(firstrow)
                        push!(uniquerows, v)
                    end

                    # Check for collisions
                    fill!(nowcollided, false)
                    @nloops $N i A d->begin
                        if d == dim
                            k = i_d
                            j_d = uniquerow[k]
                            (!collided[k] || j_d == k) && continue
                        else
                            j_d = i_d
                        end
                    end begin
                        if (@nref $N A j) != (@nref $N A i)
                            nowcollided[k] = true
                        end
                    end
                    (collided, nowcollided) = (nowcollided, collided)
                end
            end

            (@nref $N A d->d == dim ? sort!(uniquerows) : (axes(A, d))), indexin(uniquerow, uniquerows)
        end
    end

    include("states.jl")
    include("ising.jl")
    include("spectrum.jl")
    include("lattice.jl")
    include("factor.jl")
end # module


text:
n = 0
while n < 10
    n += 1
    println(n)
end

myfriends = ["Ted", "Robyn", "Barney", "Lily", "Marshall"]

i = 1
while i <= length(myfriends)
    friend = myfriends[i]
    println("Hola $friend, ¡gusto en verte!")
    i += 1
end

for n in 1:10
    println(n)
end

myfriends = ["Ted", "Robyn", "Barney", "Lily", "Marshall"]

for friend in myfriends
    println("Hi $friend, it's great to see you!")
end

for n = 1:10
    println(n)
end

for n ∈ 1:10
    println(n)
end

m, n = 5, 5
A = fill(0, (m, n))

for i in 1:m
    for j in 1:n
        A[i, j] = i + j
    end
end
A

B = fill(0, (m, n))

for i in 1:m, j in 1:n
    B[i, j] = i + j
end
B

C = [i + j for i in 1:m, j in 1:n]

for n in 1:10
   A = [i + j for i in 1:n, j in 1:n]
   display(A)
end

max_stars_repo_name: miguelraz/JuliaTutorials

max_issues_repo_name: miguelraz/JuliaTutorials

max_forks_repo_name: miguelraz/JuliaTutorials


text:
module AIControl
    using Distributions
    using JLD2
    using FileIO
    using CSV
    using DataFrames
    using GZip
    using Statistics
    using LinearAlgebra

    include("./ReferenceContigs.jl")
    include("./BamReader.jl")
    include("./BinningMap.jl")
    include("./BinnedReader.jl")
    include("./DenseBlockIterator.jl")
    include("./MatrixReader.jl")
    include("./Utils.jl")
    include("./PeakWriter.jl")
    include("./PeakCaller.jl")
    include("./EvaluationUtil.jl")
end


text:
showmarathon(; kwargs...) = printtable(marathon(;kwargs...))

function head2head(teamA, teamB, folder, seasons=1:100000;
                   turf=:both)
    @assert turf in [:both, :home, :away]

    G = allgamesbyseason(folder)
    allTeams = unique(vcat( map(x->x[:HomeTeam],G)..., map(x->x[:AwayTeam],G)... ))

    filter!(x-> Dates.year(Date(x[1,:Date])) in seasons, G)

    homeTeams = String[]
    awayTeams = String[]
    if turf==:both || turf==:home
        push!(homeTeams, teamA)
        push!(awayTeams, teamB)
    end
    if turf==:both || turf==:away
        push!(homeTeams, teamB)
        push!(awayTeams, teamA)
    end
    filterbyteam!(G, allTeams, homeTeams, [:HomeTeam])
    filterbyteam!(G, allTeams, awayTeams, [:AwayTeam])

    T = mergetables(table.(G)...)

    TA = T[T[:Team].==teamA, :]
    TB = T[T[:Team].==teamB, :]

    (size(TA,1)==0 || size(TB,1)==0) && return ""

    # (Spelade, Vunna, Oavgjorda, Förlorade, Målskillnad, Poäng för teamA – poäng för teamB)
    string(TA[1,:GamesPlayed], ' ', TA[1,:Wins], ' ', TA[1,:Draws], ' ', TA[1,:Losses], ' ', TA[1,:GoalsFor], '-', TA[1,:GoalsAgainst], ' ', TA[1,:Points], '-', TB[1,:Points])
end

function showhead2head(teamA, teamB;
                       folder=defaultresultsfolder(),
                       seasons=1:100000)
    println("(Spelade, Vunna, Oavgjorda, Förlorade, Målskillnad, Poäng för $teamA – poäng för $teamB)")
    println("Alla matcher: ", head2head(teamA, teamB, folder, seasons, turf=:both))
    println("Hemmamatcher för $teamA: ", head2head(teamA, teamB, folder, seasons, turf=:home))
    println("Bortamatcher för $teamA ", head2head(teamA, teamB, folder, seasons, turf=:away))
end

function showgames(; folder=defaultresultsfolder(), seasons=1:100000,
                     teams=String[], home=String[], away=String[])
    G = allgamesbyseason(folder)
    allTeams = unique(vcat( map(x->x[:HomeTeam],G)..., map(x->x[:AwayTeam],G)... ))

    filter!(x-> Dates.year(Date(x[1,:Date])) in seasons, G)

    # put single strings in array
    typeof(teams)<:AbstractString && (teams=[teams])
    typeof(home) <:AbstractString && (home =[home])
    typeof(away) <:AbstractString && (away =[away])

    # add teams to both home/away
    home = unique(vcat(home, teams))
    away = unique(vcat(away, teams))

    filterbyteam!(G, allTeams, home, [:HomeTeam])
    filterbyteam!(G, allTeams, away, [:AwayTeam])

    G = _vcat(G...)
    @assert size(G,1)>0 "No games to show."
    printgames(G)
end


text:
function readvalue(stream, position)
    seek(stream, position)
    return read(stream, Int32)
end

function readvalues(filename::String, indices)
    stream = open(filename, "r")
    try
        return Int32[readvalue(stream, index*4) for index in indices]
    finally
        close(stream)
    end
end

function getindices(n)
    [10000+((i-1)*3) for i=1:n]
end

function smallmarspec(outer, inner)
  for i=1:outer
    r = readvalues("D:\\temp\\bathy_10m.sbg", getindices(inner))
  end
end

function allmarspec(outer, inner)
    paths = ["bathy_10m.sbg", "biogeo01_aspect_EW_10m.sbg", "biogeo02_aspect_NS_10m.sbg", "biogeo03_plan_curvature_10m.sbg", "biogeo04_profile_curvature_10m.sbg", "biogeo05_dist_shore_10m.sbg", "biogeo06_bathy_slope_10m.sbg", "biogeo07_concavity_10m.sbg", "biogeo08_sss_mean_10m.sbg", "biogeo09_sss_min_10m.sbg",
            "biogeo10_sss_max_10m.sbg", "biogeo11_sss_range_10m.sbg", "biogeo12_sss_variance_10m.sbg", "biogeo13_sst_mean_10m.sbg", "biogeo14_sst_min_10m.sbg", "biogeo15_sst_max_10m.sbg", "biogeo16_sst_range_10m.sbg", "biogeo17_sst_variance_10m.sbg", "sss01_10m.sbg", "sss02_10m.sbg", "sss03_10m.sbg", "sss04_10m.sbg", "sss05_10m.sbg",
            "sss06_10m.sbg", "sss07_10m.sbg", "sss08_10m.sbg", "sss09_10m.sbg", "sss10_10m.sbg", "sss11_10m.sbg", "sss12_10m.sbg", "sst01_10m.sbg", "sst02_10m.sbg", "sst03_10m.sbg", "sst04_10m.sbg", "sst05_10m.sbg", "sst06_10m.sbg", "sst07_10m.sbg", "sst08_10m.sbg", "sst09_10m.sbg"]
    indices = getindices(inner)
    r = []
    for i=1:outer
        r = [readvalues(string("D:\\temp\\",path), indices) for path in paths]
    end
    r
end

@elapsed allmarspec(10,10) # <0.09s
@elapsed allmarspec(100,100) # <5s
@elapsed allmarspec(1000,100) # 45s
@elapsed allmarspec(10,10000) # 41s
@elapsed allmarspec(1,100000) # 46s

max_stars_repo_name: samuelbosch/blogbits

max_issues_repo_name: samuelbosch/blogbits

max_forks_repo_name: samuelbosch/blogbits


text:
# Combined 1D/2D plots across multiple frames.
#
# Hongyang Zhou, hyzhou@umich.edu

using Vlasiator, PyPlot, Glob, Printf, LaTeXStrings
using Vlasiator: set_args, prep2d, set_colorbar

@assert VERSION ≥ v"1.7.0" "Compatible with Julia v1.7+!"
@assert matplotlib.__version__ >= "3.4" "Require Matplotlib version 3.4+ to use subfigure!"

struct Varminmax{T}
   "Density, [amu/cc]"
   ρmin::T
   ρmax::T
   "Velocity, [km/s]"
   vmin::T
   vmax::T
   "Pressure, [nPa]"
   pmin::T
   pmax::T
   "Magnetic field, [nT]"
   bmin::T
   bmax::T
   "Electric field, [nT]"
   emin::T
   emax::T
   "Alfven speed, [km/s]"
   vamin::T
   vamax::T
   "Sonic speed, [km/s]"
   vsmin::T
   vsmax::T
end

function init_figure(varminmax, loc, pArgs)
   (; ρmin, ρmax, vmin, vmax, pmin, pmax, bmin, bmax, emin, emax,
      vamin, vamax, vsmin, vsmax) = varminmax

   fig = plt.figure(constrained_layout=true, figsize=(12, 7.2))
   subfigs = fig.subfigures(1, 2, wspace=0.01, width_ratios=[2,1])

   axsL = subfigs[1].subplots(5, 1, sharex=true)
   axsR = subfigs[2].subplots(2, 1, sharex=true)

   # Set line plots' axes
   axsL[end].set_xlim(loc[1], loc[end])

   axsL[1].set_ylim(ρmin, ρmax)
   axsL[2].set_ylim(vmin, vmax)
   axsL[3].set_ylim(pmin, pmax)
   axsL[4].set_ylim(bmin, bmax)
   axsL[5].set_ylim(emin, emax)
   for ax in axsL
      ax.xaxis.set_minor_locator(matplotlib.ticker.AutoMinorLocator())
      ax.yaxis.set_minor_locator(matplotlib.ticker.AutoMinorLocator())
      ax.grid(true)
   end

   axsL[end].set_xlabel(L"x [$R_E$]"; fontsize=14)
   axsL[1].set_ylabel("n [amu/cc]";   fontsize=14)
   axsL[2].set_ylabel("V [km/s]";     fontsize=14)
   axsL[3].set_ylabel("P [nPa]";      fontsize=14)
   axsL[4].set_ylabel("B [nT]";       fontsize=14)
   axsL[5].set_ylabel("E [mV/m]";     fontsize=14)

   fakeline = loc
   l1 = axsL[1].plot(loc, fakeline, label="Proton density", color="#1f77b4")
   l2 = axsL[2].plot(loc, fakeline, label="Vx",             color="#1f77b4")
   l3 = axsL[2].plot(loc, fakeline, label="Vy",             color="#ff7f0e")
   l4 = axsL[2].plot(loc, fakeline, label="Vz",             color="#2ca02c")
   l5 = axsL[3].plot(loc, fakeline, label="Ram",            color="#1f77b4")
   l6 = axsL[3].plot(loc, fakeline, label="Thermal",        color="#ff7f0e")
   l7 = axsL[4].plot(loc, fakeline, label="Bx",             color="#1f77b4")
   l8 = axsL[4].plot(loc, fakeline, label="By",             color="#ff7f0e")
   l9 = axsL[4].plot(loc, fakeline, label="Bz",             color="#2ca02c")
   l10= axsL[5].plot(loc, fakeline, label="Ex",             color="#1f77b4")
   l11= axsL[5].plot(loc, fakeline, label="Ey",             color="#ff7f0e")
   l12= axsL[5].plot(loc, fakeline, label="Ez",             color="#2ca02c")

   ls = (l1, l2, l3, l4, l5, l6, l7, l8, l9, l10, l11, l12)

   axsL[2].legend(;loc="lower left",  ncol=3, frameon=false, fontsize=12)
   axsL[3].legend(;loc="upper left",  ncol=2, frameon=false, fontsize=12)
   axsL[4].legend(;loc="upper right", ncol=3, frameon=false, fontsize=12)
   axsL[5].legend(;loc="lower right", ncol=3, frameon=false, fontsize=12)

   vl1 = axsL[1].vlines(loc[1], ρmin, ρmax; colors="r", linestyle="dashed", alpha=0.5)
   vl2 = axsL[2].vlines(loc[1], vmin, vmax; colors="r", linestyle="dashed", alpha=0.5)
   vl3 = axsL[3].vlines(loc[1], pmin, pmax; colors="r", linestyle="dashed", alpha=0.5)
   vl4 = axsL[4].vlines(loc[1], bmin, bmax; colors="r", linestyle="dashed", alpha=0.5)
   vl5 = axsL[5].vlines(loc[1], emin, emax; colors="r", linestyle="dashed", alpha=0.5)

   hl2 = axsL[2].hlines(0.0, loc[1], loc[end]; colors="k", linestyle="dashed", alpha=0.2)
   hl4 = axsL[4].hlines(0.0, loc[1], loc[end]; colors="k", linestyle="dashed", alpha=0.2)
   hl5 = axsL[5].hlines(0.0, loc[1], loc[end]; colors="k", linestyle="dashed", alpha=0.2)

   vlines = (vl1, vl2, vl3, vl4, vl5)

   for ax in axsR
      ax.set_aspect("equal")

      # Set border line widths
      for loc in ("left", "bottom", "right", "top")
         edge = get(ax.spines, loc, nothing)
         edge.set_linewidth(2.0)
      end
      ax.xaxis.set_tick_params(width=2.0, length=3)
      ax.yaxis.set_tick_params(width=2.0, length=3)
      ax.xaxis.set_minor_locator(matplotlib.ticker.AutoMinorLocator())
      ax.yaxis.set_minor_locator(matplotlib.ticker.AutoMinorLocator())

      ax.set_ylabel(L"Y [$R_E$]"; fontsize=14)
   end

   axsR[2].set_xlabel(L"X [$R_E$]"; fontsize=14)

   axsR[1].set_title("Alfven speed", fontsize=14)
   axsR[2].set_title("Sound speed", fontsize=14)

   x, y = Vlasiator.get_axis(pArgs)
   fakedata = fill(NaN32, length(y), length(x))

   cnorm1, cticks1 = set_colorbar(Linear, vamin, vamax)
   cnorm2, cticks2 = set_colorbar(Linear, vsmin, vsmax)
   cmap = matplotlib.cm.turbo

   c1 = axsR[1].pcolormesh(x, y, fakedata; norm=cnorm1, cmap)
   c2 = axsR[2].pcolormesh(x, y, fakedata; norm=cnorm2, cmap)

   rInner = 31.8e6 # [m]
   circle1 = plt.Circle((0, 0), rInner/Vlasiator.RE, color="w")
   circle2 = plt.Circle((0, 0), rInner/Vlasiator.RE, color="w")
   axsR[1].add_patch(circle1)
   axsR[2].add_patch(circle2)

   cb1 = colorbar(c1; ax=axsR[1], ticks=cticks1, fraction=0.046, pad=0.04, extend="max")
   cb1.ax.set_ylabel("[km/s]"; fontsize=14)

   cb2 = colorbar(c2; ax=axsR[2], ticks=cticks2, fraction=0.046, pad=0.04)
   cb2.ax.set_ylabel("[km/s]"; fontsize=14)

   #fig.suptitle("Density Pulse Run", fontsize="xx-large")

   cs = (c1, c2)

   return fig, subfigs, ls, vlines, cs
end

"Update frame."
function update_plot!(plotargs, outdir, file)
   isfile(outdir*file[end-8:end-5]*".png") && return

   fig, subfigs, ls, vlines, cs = plotargs

   meta = load(file)

   p_extract = readvariable(meta, "vg_pressure", cellids) .* 1f9 |> vec # [nPa]
   rho_extract = readvariable(meta, "proton/vg_rho", cellids) |> vec
   v_extract = readvariable(meta, "proton/vg_v", cellids)
   vmag2_extract = sum(x -> x*x, v_extract, dims=1) |> vec
   pram_extract = rho_extract .* Vlasiator.mᵢ .* vmag2_extract .* 1f9 # [nPa]

   bz = readvariable(meta, "vg_b_vol", cellids)[3,:] .* 1f9 #[nT]

   ls[1][1].set_ydata(rho_extract ./ 1f6)
   ls[2][1].set_ydata(v_extract[1,:] ./ 1f3)
   ls[3][1].set_ydata(pram_extract)
   ls[4][1].set_ydata(p_extract)
   ls[5][1].set_ydata(bz)

   imagnetopause_ = findfirst(<(0.0), bz)
   for vline in vlines
      update_vline(vline, loc[imagnetopause_])
   end

   str_title = @sprintf "Sun-Earth line, t= %4.1fs" meta.time
   subfigs[1].suptitle(str_title, fontsize=14)

   data = prep2d(meta, "VA", :z)'
   cs[1].update(Dict("array" => data ./ 1e3))

   data = prep2d(meta, "VS", :z)'
   cs[2].update(Dict("array" => data ./ 1e3))

   savefig(outdir*file[end-8:end-5]*".png", bbox_inches="tight", dpi=100)
   return
end

function update_vline(h, x)
   seg_old = h.get_segments()
   ymin = seg_old[1][1, 2]
   ymax = seg_old[1][2, 2]

   seg_new = [[x ymin; x ymax]]

   h.set_segments(seg_new)
end

####### Main

files = glob("bulk*.vlsv", ".")
nfile = length(files)
# Set output directory
outdir = "out/"

meta = load(files[1])

x1, x2 = 7.0, 18.0 # Earth radii
point1 = [x1, 0, 0] .* Vlasiator.RE
point2 = [x2, 0, 0] .* Vlasiator.RE

cellids, distances, coords = getcellinline(meta, point1, point2)

loc = range(x1, x2, length=length(cellids))

pArgs = set_args(meta, "fakename", RE; normal=:none)

# Upper/lower limits for each variable
ρmin, ρmax = 0.0, 10.0     # [amu/cc]
vmin, vmax = -640.0, 100.0 # [km/s]
pmin, pmax = 0.0, 1.82     # [nPa]
bmin, bmax = -25.0, 60.0   # [nT]
emin, emax = -5.0, 5.0     # [mV/m]
vamin, vamax = 0.0, 250.0  # [km/s]
vsmin, vsmax = 0.0, 400.0  # [km/s]

varminmax =
   Varminmax(ρmin, ρmax, vmin, vmax, pmin, pmax, bmin, bmax, emin, emax,
   vamin, vamax, vsmin, vsmax)

plotargs = init_figure(varminmax, loc, pArgs)

# Loop over snapshots
for (i, file) in enumerate(files)
   println("i = $i/$nfile, file = $(basename(file))")
   update_plot!(plotargs, outdir, file)
end

close(plotargs[1])
println("Finished!")


text:
using DevEnvConfig
using Documenter

DocMeta.setdocmeta!(DevEnvConfig, :DocTestSetup, :(using DevEnvConfig); recursive=true)

# Note: even if this package is configured to use a GitHub repo name
# of the form PACKAGE (and *not* PACKAGE.jl), we still define the
# sitename to be PACKAGE.jl in the template below.
# Its recommended to keep this convention as this is what users may
# google for... (likewise for the title of the main README.md)

makedocs(;
    modules=[DevEnvConfig],
    authors="ylvain <ylvain.dev@ethics-gradient.org> and contributors",
    repo="https://github.com/ylvain/DevEnvConfig.jl/blob/{commit}{path}#{line}",
    sitename="DevEnvConfig.jl",
    format=Documenter.HTML(;
        prettyurls=get(ENV, "CI", "false") == "true",
        canonical="https://ylvain.github.io/DevEnvConfig.jl/stable",
        assets=String[],
    ),
    pages=[
        "Home" => "index.md",
    ],
)

let
    DEPLY_REPO = "ylvain/DevEnvConfig.jl" # we may deploy to another repo
    EVENT_NAME = get(ENV,"GITHUB_EVENT_NAME", "")
    if EVENT_NAME == "workflow_dispatch" || EVENT_NAME == "release"
        EVENT_NAME = "push" # allows deploy on manual invokation
    end
    withenv("GITHUB_REPOSITORY" => DEPLY_REPO, "GITHUB_EVENT_NAME" => EVENT_NAME) do
        deploydocs(;
            repo="github.com/"*DEPLY_REPO, # default is "github.com/ylvain/Pk1.jl"
            devurl="dev",         # default, subdir for push without tag
            forcepush=true,       # default:false, changes will be combined with the previous commit and force pushed, erasing the Git history on the deployment branch.
            devbranch="main",     # which branch to access for the doc build and content
            branch="gh-pages",        # on which branch of the deploy-repo do we commit the generated doc
        )
    end
end


text:
"""
"""
pad

pad(n::NTuple{N}) where {N} =
    pad(ntuple(i -> dirichlet, Val(N)), n)

pad((bc,), (n,)) = pad(bc, n)

pad(::Dirichlet, n::Int) =
    Diagonal(BitVector(i == n for i in 1:n))

pad(::Periodic, n::Int) = Eye{Bool}(n)

function pad(bc::NTuple{2}, n::NTuple{2})
    opn = pad.(bc, n)
    eye = Eye{Bool}.(n)
    ApplyArray(kron, eye[2], opn[1]) .|
    ApplyArray(kron, opn[2], eye[1])
end

function pad(bc::NTuple{3}, n::NTuple{3})
    opn = pad.(bc, n)
    eye = Eye{Bool}.(n)
    ApplyArray(kron, eye[3], eye[2], opn[1]) .|
    ApplyArray(kron, eye[3], opn[2], eye[1]) .|
    ApplyArray(kron, opn[3], eye[2], eye[1])
end


text:
steady_position(i,ls) = (i-1)*ls

function initial_position(i,ls,λ,shift,pos)
    xs=steady_position(i,ls)
    dx=xs-pos
    ds=(-1/λ^2)*exp(-(dx/λ)^2/2)*shift
    xs+λ*dx*ds
end

function update_force!(fx,xc,ks)
    ns=length(xc)
    for i ∈ 2:ns-1 # Julia's loops are fast
        fx[i] = -ks*(2xc[i]-xc[i-1]-xc[i+1])
    end
end

function update_position!(xt,xc,xp,fx,δt,ms)
    coef=δt^2/ms
    @. xt = 2xc - xp + fx*coef #Broadcast : eltwise ops
end

function advance_nδtpf(xc,xp,xt,fx,sp,ap)
    ls,ms,ks,ns=getvalues(sp)
    δt,nδt,nδtperframe=getvalues(ap)

    for _ ∈ 1:nδtperframe # Anonymous iter variable (_)
        update_force!(fx,xc,ks)
        update_position!(xt,xc,xp,fx,δt,ms)
        xc,xp,xt=xt,xc,xp # Julia's swap
    end
    xc,xp,xt
end

function animate_spring(sp,ip,ap)
    ls,ms,ks,ns=getvalues(sp)
    λ,shift,pos=getvalues(ip)
    δt,nδt,nδtperframe=getvalues(ap)
    #1D Array comprehensions
    xs=[steady_position(i,ls) for i ∈ 1:ns]
    xc=[initial_position(i,ls,λ,shift,pos) for i ∈ 1:ns]

    dc,fx,xt=zero(xc),zero(xc),zero(xc)
    xp=copy(xc) # xp=xc <=> zero initial velocities
    @. dc = xc - xs
    mdc=maximum(dc)

    nf=nδt÷nδtperframe
    t=0.0               # Simulation time
    anim = @animate for i ∈ 1:nf
        xc,xp,xt=advance_nδtpf(xc,xp,xt,fx,sp,ap)
        @. dc = xc - xs
        t+=nδtperframe*δt
        plot(xs,dc,ylims=(-mdc,mdc),
            xlabel=L"x_s",ylabel=L"x_c(t) - x_s",
            thickness_scaling = 1.6,size=(600,400),legend=false,
            right_margin = 10Plots.PlotMeasures.mm,title="t=$(Int(round(t)))")
    end

    gif(anim,"toto.gif",fps=15)
end


text:
using LinearAlgebra, DiffEqOperators, Random, Test, BandedMatrices, SparseArrays

function fourth_deriv_approx_stencil(N)
    A = zeros(N,N+2)
    A[1,1:8] = [3.5 -56/3 42.5 -54.0 251/6 -20.0 5.5 -2/3]
    A[2,1:8] = [2/3 -11/6 0.0 31/6 -22/3 4.5 -4/3 1/6]
    A[N-1,N-5:end] = reverse([2/3 -11/6 0.0 31/6 -22/3 4.5 -4/3 1/6], dims=2)
    A[N,N-5:end] = reverse([3.5 -56/3 42.5 -54.0 251/6 -20.0 5.5 -2/3], dims=2)
    for i in 3:N-2
        A[i,i-2:i+4] = [-1/6 2.0 -13/2 28/3 -13/2 2.0 -1/6]
    end
    return A
end

function second_derivative_stencil(N)
  A = zeros(N,N+2)
  for i in 1:N, j in 1:N+2
      (j-i==0 || j-i==2) && (A[i,j]=1)
      j-i==1 && (A[i,j]=-2)
  end
  A
end

@testset "Differentiation on 2D array" begin
    M = zeros(22,22)
    M_temp = zeros(20,22)
    indices = Iterators.drop(CartesianIndices((22,22)), 0)
    for idx in indices
        M[idx] = sin(idx[1]*0.1)
    end
    L = CenteredDifference(4,4,0.1,20)

    #test mul!
    mul!(M_temp, L, M)

    correct_row = 10000.0*fourth_deriv_approx_stencil(20)*M[:,1]

    for i in 1:22
        @test M_temp[:,i] ≈ correct_row
    end

    # Test that * agrees will mul!
    @test M_temp == L*M

    # Differentiation along second dimension
    L = CenteredDifference{2}(4,4,0.1,20)
    M_temp_2 = zeros(22,20)
    indices = Iterators.drop(CartesianIndices((22,22)), 0)
    for idx in indices
        M[idx] = sin(idx[2]*0.1)
    end

    #test mul!
    mul!(M_temp_2, L, M)
    for i in 1:22
        @test M_temp_2[i,:] ≈ correct_row
    end

    # Test that * agrees will mul!
    @test M_temp_2 == L*M
end

@testset "Differenting on 3D array with L2" begin
    M = zeros(22,22,22)
    M_temp = zeros(20,22,22)
    indices = Iterators.drop(CartesianIndices((22,22,22)), 0)
    for idx in indices
        M[idx] = sin(idx[1]*0.1)
    end
    L = CenteredDifference(2,2,0.1,20)

    #test mul!
    mul!(M_temp, L, M)

    correct_row = 100.0*second_derivative_stencil(20)*M[:,1,1]

    for i in 1:22
        for j in 1:22
            @test M_temp[:,i,j] ≈ correct_row
        end
    end

    # Test that * agrees will mul!
    @test M_temp == L*M

    # Differentiation along second dimension
    L = CenteredDifference{2}(2,2,0.1,20)
    M_temp_2 = zeros(22,20,22)
    indices = Iterators.drop(CartesianIndices((22,22,22)), 0)
    for idx in indices
        M[idx] = sin(idx[2]*0.1)
    end

    #test mul!
    mul!(M_temp_2, L, M)
    for i in 1:22
        for j in 1:22
            @test M_temp_2[i,:,j] ≈ correct_row
        end
    end

    # Test that * agrees will mul!
    @test M_temp_2 == L*M

    # Differentiation along third dimension
    L = CenteredDifference{3}(2,2,0.1,20)
    M_temp_3 = zeros(22,22,20)
    indices = Iterators.drop(CartesianIndices((22,22,22)), 0)
    for idx in indices
        M[idx] = sin(idx[3]*0.1)
    end

    #test mul!
    mul!(M_temp_3, L, M)
    for i in 1:22
        for j in 1:22
            @test M_temp_3[i,j,:] ≈ correct_row
        end
    end

    # Test that * agrees will mul!
    @test M_temp_3 == L*M
end

@testset "Differentiation on 3D array with L4" begin
    M = zeros(22,22,22)
    M_temp = zeros(20,22,22)
    indices = Iterators.drop(CartesianIndices((22,22,22)), 0)
    for idx in indices
        M[idx] = sin(idx[1]*0.1)
    end
    L = CenteredDifference(4,4,0.1,20)

    #test mul!
    mul!(M_temp, L, M)

    correct_row = 10000.0*fourth_deriv_approx_stencil(20)*M[:,1,1]

    for i in 1:22
        for j in 1:22
            @test M_temp[:,i,j] ≈ correct_row
        end
    end

    # Test that * agrees will mul!
    @test M_temp == L*M

    # Differentiation along second dimension
    L = CenteredDifference{2}(4,4,0.1,20)
    M_temp_2 = zeros(22,20,22)
    for idx in indices
        M[idx] = sin(idx[2]*0.1)
    end

    #test mul!
    mul!(M_temp_2, L, M)
    for i in 1:22
        for j in 1:22
            @test M_temp_2[i,:,j] ≈ correct_row
        end
    end

    # Test that * agrees will mul!
    @test M_temp_2 == L*M

    # Differentiation along third dimension
    L = CenteredDifference{3}(4,4,0.1,20)
    M_temp_3 = zeros(22,22,20)
    for idx in indices
        M[idx] = sin(idx[3]*0.1)
    end

    #test mul!
    mul!(M_temp_3, L, M)
    for i in 1:22
        for j in 1:22
            @test M_temp_3[i,j,:] ≈ correct_row
        end
    end

    # Test that * agrees will mul!
    @test M_temp_3 == L*M
end

@testset "Differentiating an arbitrary higher dimension" begin
    N = 6
    L = CenteredDifference{N}(4,4,0.1,30)
    M = zeros(5,5,5,5,5,32,5);
    M_temp = zeros(5,5,5,5,5,30,5);
    indices = Iterators.drop(CartesianIndices((5,5,5,5,5,32,5)), 0);
    for idx in indices
        M[idx] = cos(idx[N]*0.1)
    end

    correct_row = (10.0^4)*fourth_deriv_approx_stencil(30)*M[1,1,1,1,1,:,1]

    #test mul!
    mul!(M_temp, L, M)
    indices = Iterators.drop(CartesianIndices((5,5,5,5,5,30,5)), 0);
    for idx in indices
        @test M_temp[idx] ≈ correct_row[idx[N]]
    end

    # Test that * agrees will mul!
    @test M_temp == L*M
end

@testset "Differentiating with non-symmetric interior stencil" begin

    # The following tests check that multiplication of an operator with a
    # non-symmetric interior stencil is consistent with what we expect

    N = 1
    L = CenteredDifference{N}(3,4,0.1,30)
    M = zeros(32,32)
    for i in 1:32
        for j in 1:32
            M[i,j] = cos(0.1i)
        end
    end

    M_temp = zeros(30,32)
    mul!(M_temp, L, M)

    @test M_temp ≈ Array(L)*M

    N = 2
    L = CenteredDifference{N}(3,4,0.1,30)
    M = zeros(32,32)
    for i in 1:32
        for j in 1:32
            M[i,j] = cos(0.1j)
        end
    end

    M_temp = zeros(32,30)
    mul!(M_temp, L, M)

    @test M_temp ≈ transpose(Array(L)*transpose(M))

    # Three dimensions

    N = 1
    L = CenteredDifference{N}(3,4,0.1,30)
    M = zeros(32,32,32)
    for i in 1:32
        for j in 1:32
            for k in 1:32
                M[i,j,k] = cos(0.1i)
            end
        end
    end

    M_temp = zeros(30,32,32)
    mul!(M_temp, L, M)
    for i in 1:32
        @test M_temp[:,:,i] ≈ Array(L)*M[:,:,i]
    end

    correct_row = L*M[:,1,1]

    N = 2
    L = CenteredDifference{N}(3,4,0.1,30)
    M = zeros(32,32,32)
    for i in 1:32
        for j in 1:32
            for k in 1:32
                M[i,j,k] = cos(0.1j)
            end
        end
    end

    M_temp = zeros(32,30,32)
    mul!(M_temp, L, M)

    for i in 1:32
        for j in 1:32
            @test M_temp[i,:,j] ≈ correct_row
        end
    end

    N = 3
    L = CenteredDifference{N}(3,4,0.1,30)
    M = zeros(32,32,32)
    for i in 1:32
        for j in 1:32
            for k in 1:32
                M[i,j,k] = cos(0.1k)
            end
        end
    end

    M_temp = zeros(32,32,30)
    mul!(M_temp, L, M)

    for i in 1:32
        for j in 1:32
            @test M_temp[i,j,:] ≈ correct_row
        end
    end
end

@testset "Differentiating with coefficients" begin

    # Three dimensions
    N = 1
    µ = 5.5
    L = CenteredDifference{N}(3,4,0.1,30)

    µL = µ*L
    M = zeros(32,32,32)
    for i in 1:32
        for j in 1:32
            for k in 1:32
                M[i,j,k] = cos(0.1i)
            end
        end
    end

    correct_slice = 5.5*(L*M[:,1,1])
    M_temp = zeros(30,32,32)
    mul!(M_temp, µL, M)

    for i in 1:32
        for j in 1:32
            @test M_temp[:,i,j] ≈ correct_slice
        end
    end

    N = 2
    µL = µ*CenteredDifference{N}(3,4,0.1,30)
    M = zeros(32,32,32)
    for i in 1:32
        for j in 1:32
            for k in 1:32
                M[i,j,k] = cos(0.1j)
            end
        end
    end

    M_temp = zeros(32,30,32)
    mul!(M_temp, µL, M)

    for i in 1:32
        for j in 1:32
            @test M_temp[i,:,j] ≈ correct_slice
        end
    end

    N = 3
    µL = µ*CenteredDifference{N}(3,4,0.1,30)
    M = zeros(32,32,32)
    for i in 1:32
        for j in 1:32
            for k in 1:32
                M[i,j,k] = cos(0.1k)
            end
        end
    end

    M_temp = zeros(32,32,30)
    mul!(M_temp, µL, M)

    for i in 1:32
        for j in 1:32
            @test M_temp[i,j,:] ≈ correct_slice
        end
    end
end