## %PREDICTMD_GENERATED_BY%

import PredictMDExtra
PredictMDExtra.import_all()

import PredictMD
PredictMD.import_all()

# PREDICTMD IF INCLUDE TEST STATEMENTS
import CSVFiles
import DataFrames
import FileIO
import RDatasets
import Random
import Test
# PREDICTMD ELSE
# PREDICTMD ENDIF INCLUDE TEST STATEMENTS

# PREDICTMD IF INCLUDE TEST STATEMENTS
logger = Base.CoreLogging.current_logger_for_env(Base.CoreLogging.Debug, Symbol(splitext(basename(something(@__FILE__, "nothing")))[1]), something(@__MODULE__, "nothing"))
if isnothing(logger)
    logger_stream = devnull
else
    logger_stream = logger.stream
end
# PREDICTMD ELSE
# PREDICTMD ENDIF INCLUDE TEST STATEMENTS

### Begin project-specific settings

DIRECTORY_CONTAINING_THIS_FILE = @__DIR__
PROJECT_DIRECTORY = dirname(
    joinpath(splitpath(DIRECTORY_CONTAINING_THIS_FILE)...)
    )
PROJECT_OUTPUT_DIRECTORY = joinpath(
    PROJECT_DIRECTORY,
    "output",
    )
mkpath(PROJECT_OUTPUT_DIRECTORY)
mkpath(joinpath(PROJECT_OUTPUT_DIRECTORY, "data"))
mkpath(joinpath(PROJECT_OUTPUT_DIRECTORY, "models"))
mkpath(joinpath(PROJECT_OUTPUT_DIRECTORY, "plots"))

# PREDICTMD IF INCLUDE TEST STATEMENTS
@debug("PROJECT_OUTPUT_DIRECTORY: ", PROJECT_OUTPUT_DIRECTORY,)
if PredictMD.is_travis_ci()
    PredictMD.cache_to_path!(
        ;
        from = ["cpu_examples", "breast_cancer_biopsy", "output",],
        to = [PROJECT_OUTPUT_DIRECTORY],
        )
end
# PREDICTMD ELSE
# PREDICTMD ENDIF INCLUDE TEST STATEMENTS

### End project-specific settings

### Begin Knet neural network classifier code

Random.seed!(999)

trainingandtuning_features_df_filename = joinpath(
    PROJECT_OUTPUT_DIRECTORY,
    "data",
    "trainingandtuning_features_df.csv",
    )
trainingandtuning_labels_df_filename = joinpath(
    PROJECT_OUTPUT_DIRECTORY,
    "data",
    "trainingandtuning_labels_df.csv",
    )
testing_features_df_filename = joinpath(
    PROJECT_OUTPUT_DIRECTORY,
    "data",
    "testing_features_df.csv",
    )
testing_labels_df_filename = joinpath(
    PROJECT_OUTPUT_DIRECTORY,
    "data",
    "testing_labels_df.csv",
    )
training_features_df_filename = joinpath(
    PROJECT_OUTPUT_DIRECTORY,
    "data",
    "training_features_df.csv",
    )
training_labels_df_filename = joinpath(
    PROJECT_OUTPUT_DIRECTORY,
    "data",
    "training_labels_df.csv",
    )
tuning_features_df_filename = joinpath(
    PROJECT_OUTPUT_DIRECTORY,
    "data",
    "tuning_features_df.csv",
    )
tuning_labels_df_filename = joinpath(
    PROJECT_OUTPUT_DIRECTORY,
    "data",
    "tuning_labels_df.csv",
    )
trainingandtuning_features_df = DataFrames.DataFrame(
    FileIO.load(
        trainingandtuning_features_df_filename;
        type_detect_rows = 100,
        )
    )
trainingandtuning_labels_df = DataFrames.DataFrame(
    FileIO.load(
        trainingandtuning_labels_df_filename;
        type_detect_rows = 100,
        )
    )
testing_features_df = DataFrames.DataFrame(
    FileIO.load(
        testing_features_df_filename;
        type_detect_rows = 100,
        )
    )
testing_labels_df = DataFrames.DataFrame(
    FileIO.load(
        testing_labels_df_filename;
        type_detect_rows = 100,
        )
    )
training_features_df = DataFrames.DataFrame(
    FileIO.load(
        training_features_df_filename;
        type_detect_rows = 100,
        )
    )
training_labels_df = DataFrames.DataFrame(
    FileIO.load(
        training_labels_df_filename;
        type_detect_rows = 100,
        )
    )
tuning_features_df = DataFrames.DataFrame(
    FileIO.load(
        tuning_features_df_filename;
        type_detect_rows = 100,
        )
    )
tuning_labels_df = DataFrames.DataFrame(
    FileIO.load(
        tuning_labels_df_filename;
        type_detect_rows = 100,
        )
    )

smoted_training_features_df_filename = joinpath(
    PROJECT_OUTPUT_DIRECTORY,
    "data",
    "smoted_training_features_df.csv",
    )
smoted_training_labels_df_filename = joinpath(
    PROJECT_OUTPUT_DIRECTORY,
    "data",
    "smoted_training_labels_df.csv",
    )
smoted_training_features_df = DataFrames.DataFrame(
    FileIO.load(
        smoted_training_features_df_filename;
        type_detect_rows = 100,
        )
    )
smoted_training_labels_df = DataFrames.DataFrame(
    FileIO.load(
        smoted_training_labels_df_filename;
        type_detect_rows = 100,
        )
    )

categorical_feature_names_filename = joinpath(
    PROJECT_OUTPUT_DIRECTORY,
    "data",
    "categorical_feature_names.jld2",
    )
continuous_feature_names_filename = joinpath(
    PROJECT_OUTPUT_DIRECTORY,
    "data",
    "continuous_feature_names.jld2",
    )
categorical_feature_names = FileIO.load(
    categorical_feature_names_filename,
    "categorical_feature_names",
    )
continuous_feature_names = FileIO.load(
    continuous_feature_names_filename,
    "continuous_feature_names",
    )
feature_names = vcat(categorical_feature_names, continuous_feature_names)

single_label_name = :Class
negative_class = "benign"
positive_class = "malignant"
single_label_levels = [negative_class, positive_class]

categorical_label_names = Symbol[single_label_name]
continuous_label_names = Symbol[]
label_names = vcat(categorical_label_names, continuous_label_names)

knet_mlp_predict_function_source = """
function knetmlp_predict(
        w,
        x0::AbstractArray;
        probabilities::Bool = true,
        )
    x1 = Knet.relu.( w[1]*x0 .+ w[2] )
    x2 = Knet.relu.( w[3]*x1 .+ w[4] )
    x3 = w[5]*x2 .+ w[6]
    unnormalizedlogprobs = x3
    if probabilities
        normalizedlogprobs = Knet.logp(unnormalizedlogprobs; dims = 1)
        normalizedprobs = exp.(normalizedlogprobs)
        return normalizedprobs
    else
        return unnormalizedlogprobs
    end
end
"""

knet_mlp_loss_function_source = """
function knetmlp_loss(
        predict::Function,
        modelweights,
        x::AbstractArray,
        ytrue::AbstractArray;
        L1::Real = Float64(0),
        L2::Real = Float64(0),
        )
    loss = Knet.nll(
        predict(
            modelweights,
            x;
            probabilities = false,
            ),
        ytrue;
        dims = 1,
        )
    if L1 != 0
        loss += L1 * sum(sum(abs, w_i) for w_i in modelweights[1:2:end])
    end
    if L2 != 0
        loss += L2 * sum(sum(abs2, w_i) for w_i in modelweights[1:2:end])
    end
    return loss
end
"""

feature_contrasts = PredictMD.generate_feature_contrasts(
    smoted_training_features_df,
    feature_names,
    )

knetmlp_modelweights = Any[
    Float64.(
        0.1f0*randn(Float64,64,feature_contrasts.num_array_columns_without_intercept)
        ),
    Float64.(
        fill(Float64(0),64,1)
        ),
    Float64.(
        0.1f0*randn(Float64,32,64)
        ),
    Float64.(
        fill(Float64(0),32,1)
        ),
    Float64.(
        0.1f0*randn(Float64,2,32)
        ),
    Float64.(
        fill(Float64(0),2,1)
        ),
    ]

knetmlp_losshyperparameters = Dict()
knetmlp_losshyperparameters[:L1] = Float64(0.0)
knetmlp_losshyperparameters[:L2] = Float64(0.0)

knetmlp_optimizationalgorithm = :Momentum
knetmlp_optimizerhyperparameters = Dict()
knetmlp_minibatchsize = 48

knet_mlp_classifier =
    PredictMD.single_labelmulticlassdataframeknetclassifier(
        feature_names,
        single_label_name,
        single_label_levels;
        package = :Knet,
        name = "Knet MLP",
        predict_function_source = knet_mlp_predict_function_source,
        loss_function_source = knet_mlp_loss_function_source,
        losshyperparameters = knetmlp_losshyperparameters,
        optimizationalgorithm = knetmlp_optimizationalgorithm,
        optimizerhyperparameters = knetmlp_optimizerhyperparameters,
        minibatchsize = knetmlp_minibatchsize,
        modelweights = knetmlp_modelweights,
        printlosseverynepochs = 1,
        maxepochs = 50,
        feature_contrasts = feature_contrasts,
        )

PredictMD.parse_functions!(knet_mlp_classifier)

PredictMD.fit!(
    knet_mlp_classifier,
    smoted_training_features_df,
    smoted_training_labels_df,
    tuning_features_df,
    tuning_labels_df,
    )

PredictMD.set_max_epochs!(knet_mlp_classifier, 100)

PredictMD.fit!(
    knet_mlp_classifier,
    smoted_training_features_df,
    smoted_training_labels_df,
    tuning_features_df,
    tuning_labels_df,
    )

knet_learningcurve_lossvsepoch = PredictMD.plotlearningcurve(
    knet_mlp_classifier,
    :loss_vs_epoch;
    );

# PREDICTMD IF INCLUDE TEST STATEMENTS
filename = string(
    tempname(),
    "_",
    "knet_learningcurve_lossvsepoch",
    ".pdf",
    )
rm(filename; force = true, recursive = true,)
@debug("Attempting to test that the file does not exist...", filename,)
Test.@test(!isfile(filename))
@debug("The file does not exist.", filename, isfile(filename),)
PredictMD.save_plot(filename, knet_learningcurve_lossvsepoch)
if PredictMD.is_force_test_plots()
    @debug("Attempting to test that the file exists...", filename,)
    Test.@test(isfile(filename))
    @debug("The file does exist.", filename, isfile(filename),)
end
# PREDICTMD ELSE
# PREDICTMD ENDIF INCLUDE TEST STATEMENTS

display(knet_learningcurve_lossvsepoch)
PredictMD.save_plot(
    joinpath(
        PROJECT_OUTPUT_DIRECTORY,
        "plots",
        "knet_learningcurve_lossvsepoch.pdf",
        ),
    knet_learningcurve_lossvsepoch,
    )

knet_learningcurve_lossvsepoch_skip10epochs = PredictMD.plotlearningcurve(
    knet_mlp_classifier,
    :loss_vs_epoch;
    startat = 10,
    endat = :end,
    );

# PREDICTMD IF INCLUDE TEST STATEMENTS
filename = string(
    tempname(),
    "_",
    "knet_learningcurve_lossvsepoch_skip10epochs",
    ".pdf",
    )
rm(filename; force = true, recursive = true,)
@debug("Attempting to test that the file does not exist...", filename,)
Test.@test(!isfile(filename))
@debug("The file does not exist.", filename, isfile(filename),)
PredictMD.save_plot(filename, knet_learningcurve_lossvsepoch_skip10epochs)
if PredictMD.is_force_test_plots()
    @debug("Attempting to test that the file exists...", filename,)
    Test.@test(isfile(filename))
    @debug("The file does exist.", filename, isfile(filename),)
end
# PREDICTMD ELSE
# PREDICTMD ENDIF INCLUDE TEST STATEMENTS

display(knet_learningcurve_lossvsepoch_skip10epochs)
PredictMD.save_plot(
    joinpath(
        PROJECT_OUTPUT_DIRECTORY,
        "plots",
        "knet_learningcurve_lossvsepoch_skip10epochs.pdf",
        ),
    knet_learningcurve_lossvsepoch_skip10epochs,
    )

knet_learningcurve_lossvsiteration = PredictMD.plotlearningcurve(
    knet_mlp_classifier,
    :loss_vs_iteration;
    window = 50,
    sampleevery = 10,
    );

# PREDICTMD IF INCLUDE TEST STATEMENTS
filename = string(
    tempname(),
    "_",
    "knet_learningcurve_lossvsiteration",
    ".pdf",
    )
rm(filename; force = true, recursive = true,)
@debug("Attempting to test that the file does not exist...", filename,)
Test.@test(!isfile(filename))
@debug("The file does not exist.", filename, isfile(filename),)
PredictMD.save_plot(filename, knet_learningcurve_lossvsiteration)
if PredictMD.is_force_test_plots()
    @debug("Attempting to test that the file exists...", filename,)
    Test.@test(isfile(filename))
    @debug("The file does exist.", filename, isfile(filename),)
end
# PREDICTMD ELSE
# PREDICTMD ENDIF INCLUDE TEST STATEMENTS

display(knet_learningcurve_lossvsiteration)
PredictMD.save_plot(
    joinpath(
        PROJECT_OUTPUT_DIRECTORY,
        "plots",
        "knet_learningcurve_lossvsiteration.pdf",
        ),
    knet_learningcurve_lossvsiteration,
    )

knet_learningcurve_lossvsiteration_skip100iterations =
    PredictMD.plotlearningcurve(
        knet_mlp_classifier,
        :loss_vs_iteration;
        window = 50,
        sampleevery = 10,
        startat = 100,
        endat = :end,
        );

# PREDICTMD IF INCLUDE TEST STATEMENTS
filename = string(
    tempname(),
    "_",
    "knet_learningcurve_lossvsiteration_skip100iterations",
    ".pdf",
    )
rm(filename; force = true, recursive = true,)
@debug("Attempting to test that the file does not exist...", filename,)
Test.@test(!isfile(filename))
@debug("The file does not exist.", filename, isfile(filename),)
PredictMD.save_plot(filename, knet_learningcurve_lossvsiteration_skip100iterations)
if PredictMD.is_force_test_plots()
    @debug("Attempting to test that the file exists...", filename,)
    Test.@test(isfile(filename))
    @debug("The file does exist.", filename, isfile(filename),)
end
# PREDICTMD ELSE
# PREDICTMD ENDIF INCLUDE TEST STATEMENTS

display(knet_learningcurve_lossvsiteration_skip100iterations)
PredictMD.save_plot(
    joinpath(
        PROJECT_OUTPUT_DIRECTORY,
        "plots",
        "knet_learningcurve_lossvsiteration_skip100iterations.pdf",
        ),
    knet_learningcurve_lossvsiteration_skip100iterations,
    )

knet_mlp_classifier_hist_training =
    PredictMD.plotsinglelabelbinaryclassifierhistogram(
        knet_mlp_classifier,
        smoted_training_features_df,
        smoted_training_labels_df,
        single_label_name,
        single_label_levels,
        );

# PREDICTMD IF INCLUDE TEST STATEMENTS
filename = string(
    tempname(),
    "_",
    "knet_mlp_classifier_hist_training",
    ".pdf",
    )
rm(filename; force = true, recursive = true,)
@debug("Attempting to test that the file does not exist...", filename,)
Test.@test(!isfile(filename))
@debug("The file does not exist.", filename, isfile(filename),)
PredictMD.save_plot(filename, knet_mlp_classifier_hist_training)
if PredictMD.is_force_test_plots()
    @debug("Attempting to test that the file exists...", filename,)
    Test.@test(isfile(filename))
    @debug("The file does exist.", filename, isfile(filename),)
end
# PREDICTMD ELSE
# PREDICTMD ENDIF INCLUDE TEST STATEMENTS

display(knet_mlp_classifier_hist_training)
PredictMD.save_plot(
    joinpath(
        PROJECT_OUTPUT_DIRECTORY,
        "plots",
        "knet_mlp_classifier_hist_training.pdf",
        ),
    knet_mlp_classifier_hist_training,
    )

knet_mlp_classifier_hist_testing =
        PredictMD.plotsinglelabelbinaryclassifierhistogram(
        knet_mlp_classifier,
        testing_features_df,
        testing_labels_df,
        single_label_name,
        single_label_levels,
        );

# PREDICTMD IF INCLUDE TEST STATEMENTS
filename = string(
    tempname(),
    "_",
    "knet_mlp_classifier_hist_testing",
    ".pdf",
    )
rm(filename; force = true, recursive = true,)
@debug("Attempting to test that the file does not exist...", filename,)
Test.@test(!isfile(filename))
@debug("The file does not exist.", filename, isfile(filename),)
PredictMD.save_plot(filename, knet_mlp_classifier_hist_testing)
if PredictMD.is_force_test_plots()
    @debug("Attempting to test that the file exists...", filename,)
    Test.@test(isfile(filename))
    @debug("The file does exist.", filename, isfile(filename),)
end
# PREDICTMD ELSE
# PREDICTMD ENDIF INCLUDE TEST STATEMENTS

display(knet_mlp_classifier_hist_testing)
PredictMD.save_plot(
    joinpath(
        PROJECT_OUTPUT_DIRECTORY,
        "plots",
        "knet_mlp_classifier_hist_testing.pdf",
        ),
    knet_mlp_classifier_hist_testing,
    )

show(
    logger_stream, PredictMD.singlelabelbinaryclassificationmetrics(
        knet_mlp_classifier,
        smoted_training_features_df,
        smoted_training_labels_df,
        single_label_name,
        positive_class;
        sensitivity = 0.95,
        );
    allrows = true,
    allcols = true,
    splitcols = false,
    )

show(
    logger_stream, PredictMD.singlelabelbinaryclassificationmetrics(
        knet_mlp_classifier,
        testing_features_df,
        testing_labels_df,
        single_label_name,
        positive_class;
        sensitivity = 0.95,
        );
    allrows = true,
    allcols = true,
    splitcols = false,
    )

knet_mlp_classifier_filename = joinpath(
    PROJECT_OUTPUT_DIRECTORY,
    "models",
    "knet_mlp_classifier.jld2",
    )

PredictMD.save_model(knet_mlp_classifier_filename, knet_mlp_classifier)

# PREDICTMD IF INCLUDE TEST STATEMENTS
knet_mlp_classifier = nothing
Test.@test isnothing(knet_mlp_classifier)
# PREDICTMD ELSE
# PREDICTMD ENDIF INCLUDE TEST STATEMENTS

### End Knet neural network classifier code

# PREDICTMD IF INCLUDE TEST STATEMENTS
if PredictMD.is_travis_ci()
    PredictMD.path_to_cache!(
        ;
        to = ["cpu_examples", "breast_cancer_biopsy", "output",],
        from = [PROJECT_OUTPUT_DIRECTORY],
        )
end
# PREDICTMD ELSE
# PREDICTMD ENDIF INCLUDE TEST STATEMENTS

## %PREDICTMD_GENERATED_BY%


export Pathfinding

"""
    Pathfinding
Submodule containing functionality for path-finding based on the A* algorithm.
Currently available for [`GridSpace`](@ref) and [`ContinuousSpace`](@ref).
Discretization of [`ContinuousSpace`](@ref) is taken care of internally.

You can enable path-finding and set its options by creating an instance of a
[`Pathfinding.AStar`](@ref) struct. This must be passed to the relevant pathfinding functions
during the simulation. Call [`plan_route!`](@ref) to set the destination for an agent. This
triggers the algorithm to calculate a path from the agent's current position to the one
specified. You can alternatively use [`plan_best_route!`](@ref) to choose the best target
from a list. Once a target has been set, you can move an agent one step along its
precalculated path using the [`move_along_route!`](@ref) function.

Refer to the [Maze Solver](@ref), [Mountain Runners](@ref) and [Rabbit, Fox, Hawk](@ref)
examples using path-finding and see the available functions below as well.
"""
module Pathfinding

using Agents
using DataStructures
using LinearAlgebra

abstract type GridPathfinder{D,P,M} end

include("metrics.jl")
include("pathfinding_utils.jl")
include("astar.jl")
include("astar_grid.jl")
include("astar_continuous.jl")

export CostMetric,
    DirectDistance,
    MaxDistance,
    PenaltyMap,
    AStar,
    delta_cost,
    penaltymap,
    nearby_walkable,
    random_walkable

end


export add_frames!, add_joint!, change_sequence!, change_sequences!, interpolate!, project!,
remove_frames!, remove_joint!, remove_joints!, rename!, replace_offset!, replace_offsets!,
scale!, zero!

"""
    add_frames!(g::BVHGraph, frames::Integer)

Extend the animation by a number of `frames`.

The positions and rotations for the additional frames are set to zero.
"""
function add_frames!(g::BVHGraph, f::Integer, to_end::Bool = true)
    to_end && positions!(g, [positions(g); zeros(Float64, f, 3)])
    to_end || positions!(g, [zeros(Float64, f, 3); positions(g)])

    for v in vertices(g)

        if outneighbors(g, v) != []
            to_end && rotations!(g, v, [rotations(g, v); zeros(Float64, f, 3)])
            to_end || rotations!(g, v, [zeros(Float64, f, 3); rotations(g, v)])
        end

    end

    frames!(g, frames(g) + f)
    return g
end

add_frames!(f::Integer, to_end::Bool = true) = g -> add_frames!(g, f, to_end)

"""
    add_joint!(g::BVHGraph, v₋₁::Integer, v₊₁::Integer, name::AbstractString; fraction::Float64 = 0.5)

Add a vertex on the straight line between `v₋₁` and `v₊₁` named `name`.
`fraction` refers to the fraction of the old offset that will be assigned to the new vertex.

"JOINT" is automatically added in front of `name`.
`v₋₁` and `v₊₁` can also be identified by their name.
"""
function add_joint!(g::BVHGraph, v₋₁::Integer, v₊₁::Integer, name::AbstractString; fraction::Float64 = 0.5)
    v = add_vertex!(g, name = "JOINT $(name)", sequence = sequence(g, v₋₁), rotations = zeros(Float64, frames(g), 3))
    o = offset(g, v₋₁, v₊₁)
    add_edge!(g, v₋₁, v, offset = fraction * o)
    add_edge!(g, v, v₊₁, offset = (1 - fraction) * o)
    rem_edge!(g, v₋₁, v₊₁)
    return g
end

add_joint!(v₋₁::Integer, v₊₁::Integer, name::AbstractString; fraction::Float64 = 0.5) = g -> add_joint!(g, v₋₁, v₊₁, name, fraction = fraction)
add_joint!(g::BVHGraph, v₋₁::AbstractString, v₊₁::AbstractString, name::AbstractString; fraction::Float64 = 0.5) = add_joint!(g, find(g, v₋₁), find(g, v₊₁), name, fraction = fraction)
add_joint!(v₋₁::AbstractString, v₊₁::AbstractString, name::AbstractString; fraction::Float64 = 0.5) = g -> add_joint!(g, find(g, v₋₁), find(g, v₊₁), name, fraction = fraction)

"""
    add_joint!(g::BVHGraph, v₋₁::Integer, name::AbstractString, off::Vector{Float64}, nb::Vector = outneighbors(g, v₋₁))

Add a vertex named `name` with offset `off` as an outneighbor to `v₋₁`.
The outneighbors of `v₋₁` that should be attached as outneighbors to the new vertex can be specified.

"JOINT" is automatically added in front of `name`.
`v₋₁` can also be identified by its name.
"""
function add_joint!(g::BVHGraph, v₋₁::Integer, name::AbstractString, o::Vector{Float64}, nb::Vector = outneighbors(g, v₋₁))
    v = add_vertex!(g, name = "JOINT $(name)", sequence = sequence(g, v₋₁), rotations = zeros(Float64, frames(g), 3))

    for n in nb

        if n in outneighbors(g, v₋₁)
            add_edge!(g, v, n, offset = offset(g, v₋₁, n) - o)
            rem_edge!(g, v₋₁, n)
        end

    end

    add_edge!(g, v₋₁, v, offset = o)
    return g
end

add_joint!(v₋₁::Integer, name::AbstractString, o::Vector{Float64}, nb::Vector = outneighbors(g, v₋₁)) = g -> add_joint!(g, v₋₁, name, o, nb)
add_joint!(v₋₁::AbstractString, name::AbstractString, o::Vector{Float64}) = g -> add_joint!(g, find(g, v₋₁), name, o)
add_joint!(g::BVHGraph, v₋₁::AbstractString, name::AbstractString, o::Vector{Float64}, nb::Vector = outneighbors(g, find(g, v₋₁))) = add_joint!(g, find(g, v₋₁), name, o, nb)

"""
    change_sequence!(g::BVHGraph, v::Integer, sym::Symbol)

Change the rotation order of vertex `v` to `sym`. The euler angles are adjusted accordingly.

Valid Symbols are `:XYZ`, `:XYX`, `:XZX`, `:XZY`, `:YXZ`, `:YZX`, `:YXY`, `:YZY`,
`:ZXY`, `:ZYX`, `:ZXZ`, `:ZYZ`.

See also: [`change_sequences!`](@ref)
"""
function change_sequence!(g::BVHGraph, v::Integer, sym::Symbol)
    r = constructor(sym)

    for f in 1:frames(g)
        R = rotation(g, v, f) |> r
        rotations(g, v)[f, :] = degrees(R)
    end

    sequence!(g, v, sym)
    return g
end

change_sequence!(v::Integer, sym::Symbol) = g -> change_sequence!(g, v, sym)

"""
    change_sequences!(g::BVHGraph, sym::Symbol)

Change the rotation order of all vertices to `sym`. The euler angles are adjusted accordingly.

Valid Symbols are `:XYZ`, `:XYX`, `:XZX`, `:XZY`, `:YXZ`, `:YZX`, `:YXY`, `:YZY`,
`:ZXY`, `:ZYX`, `:ZXZ`, `:ZYZ`.

See also: [`change_sequence!`](@ref)
"""
function change_sequences!(g::BVHGraph, sym::Symbol)
    for v in vertices(g)
        outneighbors(g, v) != [] && change_sequence!(g, v, sym)
    end

    return g
end

change_sequences!(sym::Symbol) = g -> change_sequences!(g, sym)

"""
    interpolate!(g::BVHGraph, h::Integer = 1)

Add a number of `h` frames between each pair of consecutive frames by interpolating between
them using LERP.
"""
function interpolate!(g::BVHGraph, h::Integer = 1)
    n = frames(g)
    frames!(g, (h + 1) * n - h)
    frametime!(g, frametime(g) / (h + 1))
    fraction = 1 / (h + 1)

    for v in vertices(g)

        if outneighbors(g, v) != []
            sym = sequence(g, v)
            constr = constructor(sym)
            M = zeros(Float64, (h + 1) * n - h, 3)
            M[1, :] = rotations(g, v, 1)

            for f in 1:n - 1
                q₀ = UnitQuaternion(rotation(g, v, sym, f))
                R₁ = rotation(g, v, sym, f + 1)
                q₁ = UnitQuaternion(R₁)

                for i in 1:h
                    R = q₀ * (1 - (fraction * i)) + q₁ * (fraction * i)
                    M[(h + 1) * f + (i - h), :] = R |> constr |> degrees
                end

                M[(h + 1) * f + 1, :] = R₁ |> degrees
            end

            rotations!(g, v, M)
        end
    end

    P = zeros(Float64, (h + 1) * n - h, 3)
    P[1, :] = positions(g)[1, :]

    for f in 1:n - 1
        p₀ = positions(g)[f, :]
        p₁ = positions(g)[f + 1, :]

        for i in 1:h
            P[(h + 1) * f + (i - h), :] = p₀ * (1 - (fraction * i)) + p₁ * (fraction * i)
        end

        P[(h + 1) * f + 1, :] = p₁
    end

    positions!(g, P)
    return g
end

interpolate!(h::Integer = 1) = g -> interpolate!(g, h)

"""
    project!(g::BVHGraph, h::BVHGraph, T::Matrix = Matrix(1.0I, 3, 3))

Transfer the rotations of each vertex in `h` to the corresponding vertex in `g`.
The corresponding vertices must have the same names.
`T` is a rotation matrix that should be provided if the global orientations of `g` and `h` differ.

See also: [`replace_offset!`](@ref), [`replace_offsets!`](@ref)
"""
function project!(g::BVHGraph, h::BVHGraph, T::Matrix = Matrix(1.0I, 3, 3))
    for hv in vertices(h)

        if outneighbors(h, hv) != []
            gv = find(g, name(h, hv))

            for f in 1:frames(h)
                R = rotation(h, hv, f)
                rotation!(g, gv, f, T * R * inv(T))
            end
        end
    end

    for f in 1:frames(h)
        positions(g)[f, :] = T * positions(h)[f, :]
    end

    frametime!(g, frametime(h))
    return g
end

project!(x...) = g -> project!(g, x...)

"""
    remove_frames!(g::BVHGraph, frames::Integer, from_end::Bool = true)

Shorten the animation by a number of `frames`.

By default the frames are removed from the end.
"""
function remove_frames!(g::BVHGraph, f::Integer, from_end::Bool = true)
    n = frames(g)
    from_end && positions!(g, positions(g)[1:n-f, :])
    from_end || positions!(g, positions(g)[1+f:n, :])

    for v in vertices(g)

        if outneighbors(g, v) != []
            from_end && rotations!(g, v, rotations(g, v)[1:n-f, :])
            from_end || rotations!(g, v, rotations(g, v)[1+f:n, :])
        end

    end

    frames!(g, n - f)
    return g
end

remove_frames!(f::Integer, from_end::Bool = true) = g -> remove_frames!(g, f, from_end)

"""
    remove_joint!(g::BVHGraph, v::Integer, v₊₁::Integer = outneighbors(g, v) != [] ? outneighbors(g, v)[1] : 0)

Remove a vertex `v` and adjust the offsets and rotations of the surrounding vertices in such a way
that deviations from their original positions are minimized.

In the case that `v` possesses multiple outneighbors, a neighbor `v₊₁` can be specified that
will be prioritized when adjusting rotations.
`v` and `v₊₁` can also be identified by their name.

See also: [`remove_joints!`](@ref), [`optimize_offsets!`](@ref)
"""
function remove_joint!(g::BVHGraph, v::Integer, v₊₁::Integer = outneighbors(g, v) != [] ? outneighbors(g, v)[1] : 0)
    v₋₁ = inneighbors(g, v)[1]

    if outneighbors(g, v) != []
        oᵥ = offset(g, v₋₁, v)
        oᵥ₊₁ = offset(g, v, v₊₁)
        o = oᵥ + oᵥ₊₁

        for f in 1:frames(g)
            Rᵥ = rotation(g, v, f)
            Rᵥ₋₁ = rotation(g, v₋₁, f)
            B = rotation_between(o, inv(Rᵥ) * oᵥ + oᵥ₊₁)
            rotation!(g, v₋₁, f, Rᵥ₋₁ * Rᵥ * B)

            for n in outneighbors(g, v)

                if outneighbors(g, n) != []
                    rotation!(g, n, f, inv(B) * rotation(g, n, f))
                end

            end

            for n in outneighbors(g, v₋₁)

                if outneighbors(g, n) != []
                    rotation!(g, n, f, inv(B) * inv(Rᵥ) * rotation(g, n, f))
                end

            end
        end

        for n in outneighbors(g, v)
            add_edge!(g, v₋₁, n)
            offset!(g, v₋₁, n, oᵥ + offset(g, v, n))
        end

    elseif outneighbors(g, v₋₁) |> length == 1
        name!(g, v₋₁, "End Site")
        rotations!(g, v₋₁, zeros(Float64, 1, 3))
    end

    rem_vertex!(g, v)
    return g
end

remove_joint!(v::Integer, x...) = g -> remove_joint!(g, v, x...)
remove_joint!(nameᵥ::AbstractString, nameᵥ₊₁::AbstractString) = g -> remove_joint!(g, find(g, nameᵥ), find(g, nameᵥ₊₁))

"""
    remove_joints!(g::BVHGraph, names::AbstractString...)

Remove every vertex in `names` and adjust the offsets and rotations of the surrounding vertices
in such a way that deviations from their original positions are minimized.

See also: [`remove_joint!`](@ref), [`optimize_offsets!`](@ref)
"""
function remove_joints!(g::BVHGraph, names::AbstractString...)
    for nameᵥ in names
        remove_joint!(g, find(g, nameᵥ))
    end

    return g
end

remove_joints!(names::AbstractString...) = g -> remove_joints!(g, names...)

"""
    rename!(g::BVHGraph, dict::Dict{String,String})

Change the names of all vertices in keys to their values.
"""
function rename!(g::BVHGraph, dict::Dict{String,String})
    for k in keys(dict)
        name!(g, find(g, k), "JOINT $(dict[k])")
    end

    return g
end

rename!(dict::Dict{String,String}) = g -> rename!(g, dict)

"""
    replace_offset!(g::BVHGraph, h::BVHGraph, gv₊₁::Integer, T::Matrix{Float64} = Matrix(1.0I, 3, 3); change_rotation::Bool = true)

Replace the offset of a vertex `gv₊₁` in `g` with the offset of the corresponding vertex in `h`.
The corresponding vertex and its inneighbor must have the same names as those in `g`.
The rotations of the surrounding vertices are adjusted.
`T` is a rotation matrix that should be provided if the global orientations of `g` and `h` differ.

See also: [`replace_offsets!`](@ref), [`project!`](@ref)
"""
function replace_offset!(g::BVHGraph, h::BVHGraph, gv₊₁::Integer, T::Matrix{Float64} = Matrix(1.0I, 3, 3); change_rotation::Bool = true)
    gv = inneighbors(g, gv₊₁)[1]
    hv = find(h, name(g, gv))
    hv₊₁ = find_outneighbor(h, hv, name(g, gv₊₁))
    goᵥ₊₁ = offset(g, gv, gv₊₁)
    hoᵥ₊₁ = T * offset(h, hv, hv₊₁)
    scale = norm(goᵥ₊₁) / norm(hoᵥ₊₁)
    offset!(g, gv, gv₊₁, scale * hoᵥ₊₁)

    if change_rotation
        B = rotation_between(hoᵥ₊₁, goᵥ₊₁)

        for f in 1:frames(g)
            Rᵥ = rotation(g, gv, f)
            rotation!(g, gv, f, Rᵥ * B)

            for n in outneighbors(g, gv)

                if outneighbors(g, n) != []
                    Rᵥ₊₁ = rotation(g, n, f)
                    rotation!(g, n, f, inv(B) * Rᵥ₊₁)
                end

            end
        end
    end

    return g
end

"""
    replace_offsets!(g::BVHGraph, h::BVHGraph, exclude::Vector, T::Matrix{Float64} = Matrix(1.0I, 3, 3))

Replace the offsets of all vertices in `g`, except those in `exclude`, with the offsets of
their corresponding vertices in `h`.
The corresponding vertex and its inneighbor must have the same names as those in `g`.
The rotations of the surrounding vertices are adjusted.
`T` is a rotation matrix that should be provided if the global orientations of `g` and `h` differ.

See also: [`replace_offset!`](@ref), [`project!`](@ref)
"""
function replace_offsets!(g::BVHGraph, h::BVHGraph, exclude::Vector, T::Matrix{Float64} = Matrix(1.0I, 3, 3))
    push!(exclude, 1)

    for n in outneighbors(g, 1)
        push!(exclude, n)
    end

    for v in vertices(g)
        v in exclude || replace_offset!(g, h, v, T)
    end

    return g
end

replace_offsets!(x...) = g -> replace_offsets!(g, x...)

"""
    scale!(g::BVHGraph, scale::Float64)

Multiply all offsets as well as the positions of ROOT by `scale`.
"""
function scale!(g::BVHGraph, scale::Float64)
    for e in edges(g)
        offset!(g, src(e), dst(e), offset(g, src(e), dst(e)) * scale)
    end

    offset!(g, offset(g) * scale)
    positions!(g, positions(g) * scale)
    return g
end

scale!(scale::Float64) = g -> scale!(g, scale)

"""
    zero!(g::BVHGraph)

Change all rotations as well as the positions of ROOT to zero.
"""
function zero!(g::BVHGraph)
    f = frames(g)
    positions!(g, zeros(Float64, f, 3))

    for v in vertices(g)
        outneighbors(g, v) != [] && rotations!(g, v, zeros(Float64, f, 3))
    end

    return g
end

zero!() = g -> zero!(g)


using PQEcolaPoint
using Dates

# build a PUT contract -
put_options = Dict{String,Any}()
put_options["ticker"] = "AMD"
put_options["expiration_date"] = Date(2022, 04, 14)
put_options["strike_price"] = 105.0
put_options["premium"] = 3.59
put_options["number_of_contracts"] = 1
put_options["direction"] = -1
put_options["current_price"] = 2.87
put_contract_model = build(PutContractModel, put_options)

# build a CALL contract -
call_options = Dict{String,Any}()
call_options["ticker"] = "AMD"
call_options["expiration_date"] = Date(2022, 04, 14)
call_options["strike_price"] = 130.0
call_options["premium"] = 302.0
call_options["number_of_contracts"] = 1
call_options["direction"] = -1
call_options["current_price"] = 6.28
call_contract_model = build(CallContractModel, call_options)

# build an EQUITY model -
equity_options = Dict{String,Any}()
equity_options["ticker"] = "AMD"
equity_options["purchase_price"] = 106.0
equity_options["current_price"] = 122.99
equity_options["direction"] = 1
equity_options["number_of_shares"] = 1
equity_contract_model = build(EquityModel, equity_options)


#------------------------------
# Convenience function for in benchmarking
#------------------------------

# define convenience function to generate data for simpler benchmark loop
function generate_all(n, p, q, q_info; seed::Int=12)
    # set seed for reproducibility
    Random.seed!(seed)
    # choose simulation setting for baseline variables
    baseline_simulation = "groupsonly" # alternatives: "trueparams" or "groupsonly"
    # generate time dependent variables
    xs, tvals, group1, group2 = generate_xs(n, p, true_u0, sol_group1, sol_group2, maxntps=1, σ_var=0.1, σ_ind=0.1); # default vals: t_start=1.5, t_end=10, maxntps=10, dt=0.1, σ_var=0.1, σ_ind=0.5
    # generate baseline variables
    if baseline_simulation == "trueparams"
        x_baseline = generate_baseline(n, q, q_info, group1, true_odeparams_group1, true_odeparams_group2); # defaul vals: σ_info=0.1, σ_noise=0.1
    elseif baseline_simulation == "groupsonly"
        x_baseline = generate_baseline(n, q, q_info, group1,  σ_info=0.5, σ_noise=0.5); # default vals: σ_info=1, σ_noise=1
    else
        error("Please select either 'trueparams' or 'groupsonly' as mode for simulation of baseline variables")
    end
    return xs, x_baseline, tvals, group1, group2
end

function run_benchmark(trainingdata, m)
    L = loss_wrapper(m)
    ps = getparams(m)
    opt = ADAM(0.0005)
    for epoch in 1:35
        Flux.train!(L, ps, trainingdata, opt)
    end
end

using Printf
function prettymemory(b)
    if b < 1024
        return string(b, " bytes")
    elseif b < 1024^2
        value, units = b / 1024, "KiB"
    elseif b < 1024^3
        value, units = b / 1024^2, "MiB"
    else
        value, units = b / 1024^3, "GiB"
    end
    return string(@sprintf("%.2f", value), " ", units)
end


"""
    @snapshotoutput(name)

A function that returns some instantaneous output about a time-varying solution should have
the signature `name(state,constraint,sys::ILMSystem,t[,kwargs...])`, where `state` is a solution vector
(of the same type as returned by `state(zeros_sol(sys))`), `constraint` is
of the same type as returned by `constraint(zeros_sol(sys))`),
`sys` is the system struct,
`t` is time, and `kwargs` are any additional keyword arguments. This macro
extends that function so that it will also operate on the integrator
e.g, `name(integrator)`, and the solution history array `sol`, e.g.,
`name(sol::ODESolution,sys,t)`, using interpolation for values of `t` that are not explicitly
in the solution array. It can also take a range of values of `t`, e.g,
`name(sol::ODESolution,sys,0.1:0.01:0.2)`.
"""
macro snapshotoutput(name)

  return esc(quote

      $name(u::ConstrainedSystems.ArrayPartition,sys::ILMSystem{S,P,N},t,args...;kwargs...) where {S,P,N} = $name(state(u),constraint(u),sys,t,args...;kwargs...)

      $name(u::ConstrainedSystems.ArrayPartition,sys::ILMSystem{S,P,0},t,args...;kwargs...) where {S,P} = $name(state(u),zeros_surface(sys),sys,t,args...;kwargs...)

      $name(integ::ConstrainedSystems.OrdinaryDiffEq.ODEIntegrator,args...;kwargs...) = $name(integ.u,integ.p,integ.t,args...;kwargs...)

      $name(sol::ConstrainedSystems.OrdinaryDiffEq.ODESolution,sys::ILMSystem,t::Real,args...;kwargs...) = $name(sol(t),sys,t,args...;kwargs...)

      $name(sol::ConstrainedSystems.OrdinaryDiffEq.ODESolution,sys::ILMSystem,t::AbstractArray,args...;kwargs...) =
          map(ti -> $name(sol(ti),sys,ti,args...;kwargs...),t)

      export $name

  end)

end

"""
    @scalarsurfacemetric(name)

A function that returns a time-varying scalar surface metric should have
the signature `name(state,constraint,sys::ILMSystem,t,bodyid,[,kwargs...])`, where `state` is a solution vector
(of the same type as returned by `state(zeros_sol(sys))`), `constraint` is
of the same type as returned by `constraint(zeros_sol(sys))`),
`sys` is the system struct,
`t` is time, `bodyid` is the body ID, and `kwargs` are any additional keyword arguments. This macro
extends that function so that it will also operate on the integrator
e.g, `name(integrator,bodyid)`, and the solution history array `sol`, e.g.,
`name(sol::ODESolution,sys,t,bodyid)`, using interpolation for values of `t` that are not explicitly
in the solution array. It can also take a range of values of `t`, e.g,
`name(sol::ODESolution,sys,0.1:0.01:0.2,bodyid)`. And finally, it
can simply take `name(sol::ODESolution,sys,bodyid)` to return
the entire history.
"""
macro scalarsurfacemetric(name)
    return esc(quote
        function $name(sol::ConstrainedSystems.OrdinaryDiffEq.ODESolution,sys::ILMSystem,bodyid::Int;kwargs...)
            mom = map((u,t) -> $name(u,sys,t,bodyid;kwargs...),sol.u,sol.t)
            mom
        end

        @snapshotoutput $name
    end)
end

"""
    @vectorsurfacemetric(name)

A function that returns a time-varying scalar surface metric should have
the signature `name(state,constraint,sys::ILMSystem,t,bodyid,[,kwargs...])`, where `state` is a solution vector
(of the same type as returned by `state(zeros_sol(sys))`), `constraint` is
of the same type as returned by `constraint(zeros_sol(sys))`),
`sys` is the system struct,
`t` is time, `bodyid` is the body ID, and `kwargs` are any additional keyword arguments. This macro
extends that function so that it will also operate on the integrator
e.g, `name(integrator,bodyid)`, and the solution history array `sol`, e.g.,
`name(sol::ODESolution,sys,t,bodyid)`, using interpolation for values of `t` that are not explicitly
in the solution array. It can also take a range of values of `t`, e.g,
`name(sol::ODESolution,sys,0.1:0.01:0.2,bodyid)`. And finally, it
can simply take `name(sol::ODESolution,sys,bodyid)` to return
the entire history.
"""
macro vectorsurfacemetric(name)
    return esc(quote
        function $name(sol::ConstrainedSystems.OrdinaryDiffEq.ODESolution,sys::ILMSystem,bodyid::Int;kwargs...)
            vx = map((u,t) -> $name(u,sys,t,bodyid;kwargs...)[1],sol.u,sol.t)
            vy = map((u,t) -> $name(u,sys,t,bodyid;kwargs...)[2],sol.u,sol.t)
            vx, vy
        end

        @snapshotoutput $name
    end)
end


#
#   UNFINISHED !!!
#
#

#
# Intersection of elements from left and right stream
#

#
# this could be done with an inner join,
# computation of intersection, and resorting
#
# Here we are trying to solve the problem using heaps for left and
# right stream. Each stream has a (bpEnd-sorted) heap that contains
# all currently open segments of that stream
#
# Whenever we encounter a new segment (on left or right stream),
# we take the (bpEnd-sorted) heap from the other stream,
# pop off any segments that have already ended,
# and then report the intersection of the new segment with all
# remaining elements on the heap.
# i.e. lelt.Chrom, max(lelt.bpStart, relt.bpStart),
#                  min(lelt.bpEnd, relt.bpEnd)
#
#
# Need to keep track of which stream produced new segment,
# (:left or :right) and which idx of other streams heap is
# to be reported next
#

export gorintersect

using Tables
using DataStructures: heappush!, heappop!

struct Intersection{L,R}
    left::L
    right::R
end

Base.IteratorEltype(::Type{Intersection{L,R}}) where {L,R} = Base.HasEltype()

#Base.eltype(::Type{Intersection{L,R}}) where {L,R} =
#    Base.return_types(mergex, (eltype(L), eltype(R)))[1]

Base.IteratorSize(::Type{Intersection{L,R}}) where {L,R} = Base.SizeUnknown()

Tables.istable(i::Intersection{L,R}) where {L,R} = true
Tables.rowaccess(i::Intersection{L,R}) where {L,R} = true
Tables.schema(i::Intersection{L,R}) where {L,R} = Tables.Schema(eltype(i))
Tables.rows(i::Intersection{L,R}) where {L,R} = i

gorintersect(left, right) = Intersection{typeof(left), typeof(right)}(left, right)

gorintersect(right) = left -> gorintersect(left, right)

function Base.iterate(i::Intersection{L,R}) where {L,R}

    lelt_s = iterate(i.left)
    lelt_s == nothing && return nothing

    relt_s = iterate(i.right)

    lh = Vector{Tuple{String, Int64}, eltype(i.left)}()
    rh = Vector{Tuple{String, Int64}, eltype(i.right)}()

    state = (lelt_s, lh, relt_s, rh, :left, 1)
    iterate(i, state)

end


"""
Front-End is responsible for tracking keypoints
and computing poses for the Frames.
It also decides when the system needs a new Keyframe in the map.

# Parameters

- `current_frame::Frame`: Current frame that is being processed.
    This is a shared Frame between FrontEnd, MapManager, Estimator
    and SlamManager.
- `motion_model::MotionModel`: Motion model that is used to predict
    pose for the Frame before the actual pose for it was computed.
- `map_manager::MapManager`: Map manager that is responsible for the
    creation of new Keyframes in the map.
- `params::Params`: Parameters of the system.
- `current_image::Matrix{Gray{Float64}}`: Current image that is processed.
- `previous_image::Matrix{Gray{Float64}}`: Previous processed image.
- `current_pyramid::LKPyramid`: Pre-computed pyramid
    that is used for optical flow tracking for the `current_image`.
- `previous_pyramid::LKPyramid`: Pre-computed pyramid
    that is used for optical flow tracking for the `previous_image`.
"""
mutable struct FrontEnd
    current_frame::Frame
    motion_model::MotionModel
    map_manager::MapManager
    params::Params

    current_image::Matrix{Gray{Float64}}
    previous_image::Matrix{Gray{Float64}}

    current_pyramid::LKPyramid
    previous_pyramid::LKPyramid
    geev_cache::GEEV4x4Cache
end

function FrontEnd(params::Params, frame::Frame, map_manager::MapManager)
    empty_pyr = LKPyramid(
        [Matrix{Gray{Float64}}(undef, 0, 0)],
        nothing, nothing, nothing, nothing, nothing, nothing)
    FrontEnd(
        frame, MotionModel(), map_manager, params,
        Matrix{Gray{Float64}}(undef, 0, 0), Matrix{Gray{Float64}}(undef, 0, 0),
        empty_pyr, empty_pyr, GEEV4x4Cache())
end

"""
    track!(fe::FrontEnd, image, time)

Given an image and time at which it was taken, track keypoints in it.
After tracking, decide if the system needs a new Keyframe added to the map.
If it is the first image to be tracked, then Keyframe is always needed.

# Returns

`true` if the system needs a new Keyframe, otherwise `false`.
"""
function track!(fe::FrontEnd, image, time, slam_io)
    is_kf_required = false
    lock(fe.map_manager.map_lock)
    try
        is_kf_required = track_mono!(fe, image, time, slam_io)
        is_kf_required && create_keyframe!(fe.map_manager, image)
    catch e
        showerror(stdout, e)
        display(stacktrace(catch_backtrace()))
    finally
        unlock(fe.map_manager.map_lock)
    end

    @debug "Pose $(fe.current_frame.id) WC: $(fe.current_frame.wc[1:3, 4])"
    is_kf_required
end

function track_mono!(fe::FrontEnd, image, image_time, slam_io)
    preprocess!(fe, image)
    fe.current_frame.id == 1 && return true
    # Apply motion model & update current Frame pose.
    new_pose = fe.motion_model(fe.current_frame.wc, image_time)
    set_wc!(fe.current_frame, new_pose, slam_io)

    t1 = time()
    klt_tracking!(fe)
    t2 = time()
    @debug "[FE] KLT Tracking ($(t2 - t1) sec)."

    if !fe.params.vision_initialized
        if fe.current_frame.nb_keypoints < 50
            @warn "[Front End] NB KP < 50. Reset required."
            fe.params.reset_required = true
            return false
        elseif check_ready_for_init!(fe, slam_io)
            @debug "[Front End] System ready for initialization."
            fe.params.vision_initialized = true
            return true
        else
            @debug "[Front End] System not ready for initialization."
            return false
        end
    end

    # Epipolar filtering to remove outliers.
    # In case P3P fails this pose will be used.
    t1 = time()
    pose_5pt = compute_pose_5pt!(fe; min_parallax=5.0, use_motion_model=true)
    fe.map_manager.nb_keyframes > 2 && pose_5pt ≢ nothing &&
        set_cw!(fe.current_frame, pose_5pt, slam_io)
    t2 = time()
    @debug "[FE] 5PT Pose ($(t2 - t1) sec)."

    t1 = time()
    compute_pose!(fe, slam_io)
    t2 = time()
    @debug "[FE] Compute Pose ($(t2 - t1) sec)."

    update!(fe.motion_model, fe.current_frame.wc, image_time)
    check_new_kf_required(fe)
end

"""
    compute_pose!(fe::FrontEnd)

Compute pose of a current Frame using P3P Ransac algorithm.
Pose is computed from the triangulated Keypoints (MapPoints) that are visible
in this frame.

# Returns

`true` if the pose was successfully computed and applied to current Frame,
otherwise `false`.
"""
function compute_pose!(fe::FrontEnd, slam_io)
    if fe.current_frame.nb_3d_kpts < 5
        @warn "[Front-End] Not enough 3D keypoints to compute P3P $(fe.current_frame.nb_3d_kpts)."
        return false
    end

    p3p_pixels = Vector{Point2f}(undef, fe.current_frame.nb_3d_kpts)
    p3p_pdn_positions = Vector{Point3f}(undef, fe.current_frame.nb_3d_kpts)
    p3p_3d_points = Vector{Point3f}(undef, fe.current_frame.nb_3d_kpts)
    p3p_kpids = Vector{Int64}(undef, fe.current_frame.nb_3d_kpts)
    i = 1

    for kp in values(fe.current_frame.keypoints)
        kp.is_3d || continue
        mp = get(fe.map_manager.map_points, kp.id, nothing)
        mp ≡ nothing && continue

        p3p_pdn_positions[i] = normalize(kp.position)
        # Convert pixel to `(x, y)` format, expected by P3P.
        p3p_pixels[i] = kp.undistorted_pixel[[2, 1]]
        p3p_3d_points[i] = mp.position
        p3p_kpids[i] = kp.id
        i += 1
    end

    i -= 1
    p3p_pdn_positions = @view(p3p_pdn_positions[1:i])
    p3p_pixels = @view(p3p_pixels[1:i])
    p3p_3d_points = @view(p3p_3d_points[1:i])
    p3p_kpids = @view(p3p_kpids[1:i])

    # P3P computes world → camera projection.
    res = p3p_ransac(
        p3p_3d_points, p3p_pixels, p3p_pdn_positions,
        fe.current_frame.camera.K; threshold=fe.params.max_reprojection_error,
    )
    if res ≡ nothing
        @warn "[FE] P3P Ransac returned Nothing."
        fe |> reset_frame!
        return false
    end

    n_inliers, model = res
    if n_inliers < 5 || model ≡ nothing
        @warn "[FE] P3P too few inliers - resetting!"
        fe |> reset_frame!
        return false
    end

    KP, inliers, error = model
    set_cw!(fe.current_frame, to_4x4(fe.current_frame.camera.iK * KP), slam_io)
    # Remove outliers after P3P.
    for (kpid, inlier) in zip(p3p_kpids, inliers)
        inlier || remove_obs_from_current_frame!(fe.map_manager, kpid)
    end

    # Prepare data for BA refinement.
    p3p_3d_points = @view(p3p_3d_points[inliers])
    p3p_kpids = @view(p3p_kpids[inliers])

    c = 1
    for i in 1:length(p3p_pixels)
        inliers[i] || continue
        p = p3p_pixels[i]
        p3p_pixels[c] = Point2f(p[2], p[1])
        c += 1
    end
    p3p_pixels = @view(p3p_pixels[1:n_inliers])

    # Refinement.
    new_T, init_error, res_error, outliers, n_outliers = pnp_bundle_adjustment(
        fe.current_frame.camera, fe.current_frame.cw,
        p3p_pixels, p3p_3d_points;
        iterations=10, show_trace=false,
        repr_ϵ=fe.params.max_reprojection_error)
    if length(p3p_3d_points) - n_outliers < 5 || res_error > init_error
        @warn "[FE] P3P BA too few inliers - resetting!"
        fe |> reset_frame!
        return false
    end

    for (kpid, outlier) in zip(p3p_kpids, outliers)
        outlier && remove_obs_from_current_frame!(fe.map_manager, kpid)
    end

    set_cw!(fe.current_frame, new_T, slam_io)
    true
end

"""
    compute_pose_5pt!(fe::FrontEnd; min_parallax, use_motion_model)

Copmute pose for pixel correspondences using 5-point algorithm
to recover essential matrix, then pose from it.

# Arguments

- `min_parallax`: Minimum parallax required between pixels in the current Frame
    and previous Keyframe to compute pose.
    Note, that parallax is rotation-compensated, meaning a rotation from
    current to previous frame is computed and applied to every pixel.
- `use_motion_model`: If `true`, then use constant-velocity motion model
    to predict next pose from previous frame.
    Otherwise, the computed pose will be "local".

# Returns

If successfull, 4x4 pose matrix, that transforms points
from previous Keyframe to current Frame.
Otherwise `nothing`.
"""
function compute_pose_5pt!(fe::FrontEnd; min_parallax, use_motion_model)
    if fe.current_frame.nb_keypoints < 8
        @debug "[FE] Not enough keypoints for initialization: " *
            "$(fe.current_frame.nb_keypoints)"
        return nothing
    end

    previous_keyframe = get(
        fe.map_manager.frames_map, fe.current_frame.kfid, nothing)
    previous_keyframe ≡ nothing && return nothing
    R_compensation = get_Rcw(previous_keyframe) * get_Rwc(fe.current_frame)

    n_parallax = 0
    avg_parallax = 0.0

    previous_points = Vector{Point2f}(undef, fe.current_frame.nb_keypoints)
    current_points = Vector{Point2f}(undef, fe.current_frame.nb_keypoints)
    previous_pd = Vector{Point2f}(undef, fe.current_frame.nb_keypoints)
    current_pd = Vector{Point2f}(undef, fe.current_frame.nb_keypoints)
    kp_ids = Vector{Int64}(undef, fe.current_frame.nb_keypoints)
    i = 1

    # Get all Keypoint's positions and compute rotation-compensated parallax.
    @inbounds for keypoint in values(fe.current_frame.keypoints)
        pkf_keypoint = get(previous_keyframe.keypoints, keypoint.id, nothing)
        pkf_keypoint ≡ nothing && continue

        # Convert points to `(x, y)` format as expected by five points.
        previous_points[i] = pkf_keypoint.undistorted_pixel[[2, 1]]
        current_points[i] = keypoint.undistorted_pixel[[2, 1]]
        previous_pd[i] = pkf_keypoint.position[[1, 2]]
        current_pd[i] = keypoint.position[[1, 2]]
        kp_ids[i] = keypoint.id
        i += 1

        # Compute rotation-compensated parallax.
        rot_position = R_compensation * keypoint.position
        rot_px = project(fe.current_frame.camera, rot_position)
        avg_parallax += norm(rot_px - pkf_keypoint.undistorted_pixel)
        n_parallax += 1
    end
    if n_parallax < 8
        @warn "[FE] Not enough keypoints in previous KF " *
            "to compute 5pt Essential Matrix."
        return nothing
    end

    avg_parallax /= n_parallax
    if avg_parallax < min_parallax
        @warn "[FE] Not enough parallax ($avg_parallax) " *
            "to compute 5pt Essential Matrix."
        return nothing
    end

    i -= 1
    previous_points = @view(previous_points[1:i])
    current_points = @view(current_points[1:i])
    previous_pd = @view(previous_pd[1:i])
    current_pd = @view(current_pd[1:i])
    kp_ids = @view(kp_ids[1:i])

    # `P` is `cw`: transforms from world (previous frame) to current frame.
    n_inliers, (_, P, inliers, _) = five_point_ransac(
        previous_points, current_points, previous_pd, current_pd,
        fe.current_frame.camera.K, fe.current_frame.camera.K, fe.geev_cache;
        max_repr_error=fe.params.max_reprojection_error)
    if n_inliers < 5
        @warn "[FE] Not enough inliers ($n_inliers) for the " *
            "5pt Essential Matrix."
        return nothing
    end

    if n_inliers != n_parallax
        for (i, inlier) in enumerate(inliers)
            inlier || remove_obs_from_current_frame!(fe.map_manager, kp_ids[i])
        end
    end

    if use_motion_model
        # Get motion-model translation scale from last KeyFrame.
        prev_cw = get_cw(previous_keyframe)
        current = prev_cw * get_wc(fe.current_frame)
        scale = norm(current[1:3, 4])

        R, t = P[1:3, 1:3], P[1:3, 4]
        t = scale .* normalize(t)
        return to_4x4(R, t) * prev_cw
    end
    to_4x4(P) # cw pose
end

"""
    check_ready_for_init!(fe::FrontEnd)

Check if there is enough average rotation compensated parallax
between current Frame and previous KeyFrame.

Additionally, compute Essential matrix using 5-point Ransac algorithm
to filter out outliers and check if there is enough inliers to proceed.
"""
function check_ready_for_init!(fe::FrontEnd, slam_io)
    avg_parallax = compute_parallax(
        fe, fe.current_frame.kfid;
        compensate_rotation=false, median_parallax=false)
    @debug "[FE] Initial parallax $avg_parallax vs $(fe.params.initial_parallax)."
    avg_parallax ≤ fe.params.initial_parallax && return false
    pose = compute_pose_5pt!(
        fe; min_parallax=fe.params.initial_parallax, use_motion_model=false)
    ready = pose ≢ nothing
    ready && set_cw!(fe.current_frame, pose, slam_io)
    ready
end

"""
    check_new_kf_required(fe::FrontEnd)

Check if we need to insert a new KeyFrame into the Map.
"""
function check_new_kf_required(fe::FrontEnd)
    prev_kf = get(fe.map_manager.frames_map, fe.current_frame.kfid, nothing)
    prev_kf ≡ nothing && return false

    # Id difference since last KeyFrame.
    frames_δ = fe.current_frame.id - prev_kf.id
    if fe.current_frame.nb_occupied_cells < 0.33 * fe.params.max_nb_keypoints &&
        frames_δ ≥ 5 && !fe.params.local_ba_on
        return true
    end
    if fe.current_frame.nb_3d_kpts < 20 && frames_δ ≥ 2
        return true
    end
    if fe.current_frame.nb_3d_kpts > 0.5 * fe.params.max_nb_keypoints &&
        (fe.params.local_ba_on || frames_δ < 2)
        return false
    end

    # Time difference since last KeyFrame.
    time_δ = fe.current_frame.time - prev_kf.time
    # TODO option for stereo
    median_parallax = compute_parallax(
        fe, prev_kf.kfid; compensate_rotation=true, only_2d=false,
    )
    cx = median_parallax ≥ fe.params.initial_parallax / 2.0 # TODO || stereo
    c0 = median_parallax ≥ fe.params.initial_parallax
    c1 = fe.current_frame.nb_3d_kpts < 0.75 * prev_kf.nb_3d_kpts
    c2 = fe.current_frame.nb_occupied_cells < 0.5 * fe.params.max_nb_keypoints &&
        fe.current_frame.nb_3d_kpts < 0.85 * prev_kf.nb_3d_kpts &&
        !fe.params.local_ba_on

    cx && (c0 || c1 || c2)
end

"""
    compute_parallax(
        fe::FrontEnd, current_frame_id;
        compensate_rotation = true, only_2d = true, median_parallax = true)

Compute parallax in pixels between current Frame
and the provided `current_frame_id` Frame.

# Arguments

- `compensate_rotation::Bool`:
    Compensate rotation by computing relative rotation between
    current Frame and previous Keyframe if `true`. Default is `true`.
- `only_2d::Bool`: Consider only 2d keypoints. Default is `true`.
- `median_parallax::Bool`:
    Instead of the average, compute median parallax. Default is `true`.
"""
function compute_parallax(
    fe::FrontEnd, current_frame_id;
    compensate_rotation = true, only_2d = true, median_parallax = true,
)
    frame = get(fe.map_manager.frames_map, current_frame_id, nothing)
    if frame ≡ nothing
        @warn "[Front-End] Error in `compute_parallax`! " *
            "Keyframe $current_frame_id does not exist."
        return 0.0
    end

    current_rotation = compensate_rotation ?
        get_Rcw(frame) * get_Rwc(fe.current_frame) : SMatrix{3, 3, Float64, 9}(I)

    avg_parallax = 0.0
    n_parallax = 0
    parallax_set = Float64[]

    for keypoint in values(fe.current_frame.keypoints)
        only_2d && keypoint.is_3d && continue
        keypoint.id in keys(frame.keypoints) || continue

        upx = keypoint.undistorted_pixel
        compensate_rotation && (upx =
            project(frame.camera, current_rotation * keypoint.position);)

        parallax = norm(upx - get_keypoint_unpx(frame, keypoint.id))
        avg_parallax += parallax
        n_parallax += 1

        median_parallax && push!(parallax_set, parallax)
    end
    n_parallax == 0 && return 0.0

    if median_parallax
        avg_parallax = parallax_set |> median
    else
        avg_parallax /= n_parallax
    end
    avg_parallax
end

function preprocess!(fe::FrontEnd, image)
    t1 = time()
    fe.previous_image = fe.current_image
    fe.current_image = image

    if has_gradients(fe.previous_pyramid) && has_gradients(fe.current_pyramid)
        copy!(fe.previous_pyramid, fe.current_pyramid)
        update!(fe.current_pyramid, fe.current_image)
    else
        fe.previous_pyramid = fe.current_pyramid
        fe.current_pyramid = LKPyramid(
            fe.current_image, fe.params.pyramid_levels;
            σ=fe.params.pyramid_σ, reusable=true)
    end
    t2 = time()
    @debug "[FE] Preprocess ($(t2 - t1) sec)."
end

"""
    klt_tracking!(fe::FrontEnd)

Track keypoints from previous frame to current frame.
"""
function klt_tracking!(fe::FrontEnd)
    optical_flow_matching!(
        fe.map_manager, fe.current_frame,
        fe.previous_pyramid, fe.current_pyramid, false)
end

"""
    reset_frame!(fe::FrontEnd)

Reset current Frame in Front-End.
"""
function reset_frame!(fe::FrontEnd)
    for kpid in keys(fe.current_frame.keypoints)
        remove_obs_from_current_frame!(fe.map_manager, kpid)
    end
    fe.current_frame.keypoints |> empty!
    fe.current_frame.keypoints_grid .|> empty!

    fe.current_frame.nb_2d_kpts = 0
    fe.current_frame.nb_3d_kpts = 0
    fe.current_frame.nb_keypoints = 0
    fe.current_frame.nb_occupied_cells = 0
end

"""
    reset!(fe::FrontEnd)

Reset Front-End.
"""
function reset!(fe::FrontEnd)
    empty_pyr = LKPyramid(
        [Matrix{Gray{Float64}}(undef, 0, 0)],
        nothing, nothing, nothing, nothing, nothing, nothing)
    fe.previous_pyramid = empty_pyr
    fe.current_pyramid = empty_pyr
end


module SDPA

# See http://plato.asu.edu/ftp/sdpa_format.txt

import ..FileFormats
import MathOptInterface

const MOI = MathOptInterface

MOI.Utilities.@model(Model,
    (),
    (),
    (MOI.Nonnegatives, MOI.PositiveSemidefiniteConeTriangle),
    (),
    (),
    (),
    (),
    (MOI.VectorAffineFunction,)
)

function MOI.supports_constraint(
    ::Model{T},
    ::Type{MOI.SingleVariable},
    ::Type{<:MOI.Utilities.SUPPORTED_VARIABLE_SCALAR_SETS{T}}
) where {T}

    return false
end

function MOI.supports(
    ::Model,
    ::MOI.ObjectiveFunction{MOI.SingleVariable}
)
    return false
end

function MOI.supports(
    ::Model{T},
    ::MOI.ObjectiveFunction{MOI.ScalarQuadraticFunction{T}}
) where {T}
    return false
end

struct Options end

get_options(m::Model) = get(m.ext, :SDPA_OPTIONS, Options())

"""
    Model(; number_type::Type = Float64)

Create an empty instance of `FileFormats.SDPA.Model{number_type}`.

Note that the model is in geometric form. That is, the SDP model is represented
as a minimization with free variables and affine constraints in either the
nonnegative orthant or the positive semidefinite cone.

If a model is in standard form, that is, nonnegative and positive semidefinite
variables with equality constraints, use `Dualization.jl` to transform it into
the geometric form.
"""
function Model(; number_type::Type = Float64)
    model = Model{number_type}()
    model.ext[:SDPA_OPTIONS] = Options()
    return model
end

function Base.show(io::IO, ::Model)
    print(io, "A SemiDefinite Programming Algorithm Format (SDPA) model")
end

# ==============================================================================
#
#   Base.write
#
# ==============================================================================

"""
    Base.write(io::IO, model::FileFormats.SDPA.Model)

Write `model` to `io` in the SemiDefinite Programming Application file format.
"""
function Base.write(io::IO, model::Model{T}) where {T}
    options = get_options(model)
    # Helper functions for MOI constraints.
    function model_cons(con_func, con_set)
        MOI.get(model, MOI.ListOfConstraintIndices{con_func, con_set}())
    end
    con_function(con_idx) = MOI.get(model, MOI.ConstraintFunction(), con_idx)
    con_set(con_idx) = MOI.get(model, MOI.ConstraintSet(), con_idx)

    # Model name
    model_name = MOI.get(model, MOI.Name())
    if !isempty(model_name)
        # Lines starting with `"` are comments that should be ignored by a reader.
        println(io, '"', model_name)
    end

    num_vars = MOI.get(model, MOI.NumberOfVariables())
    println(io, num_vars)
    function _check_variable_index(vi::MOI.VariableIndex)
        if vi.value > num_vars
            error(
                "Non-contiguous variable indices not supported. This might " *
                "be due to deleted variables."
            )
        end
    end

    nonneg = model_cons(MOI.VectorAffineFunction{T}, MOI.Nonnegatives)
    psd = model_cons(
        MOI.VectorAffineFunction{T},
        MOI.PositiveSemidefiniteConeTriangle
    )
    println(io, length(nonneg) + length(psd))

    for block in eachindex(nonneg)
        print(io, -MOI.dimension(con_set(nonneg[block])))
        if block != length(nonneg) || !isempty(psd)
            print(io, " ")
        end
    end
    max_dim = 0
    for i in eachindex(psd)
        dim = MOI.side_dimension(con_set(psd[i]))
        max_dim = max(max_dim, dim)
        print(io, dim)
        if i != length(psd)
            print(io, " ")
        end
    end
    println(io)

    c = zeros(T, num_vars)
    sense = MOI.get(model, MOI.ObjectiveSense())
    if sense != MOI.FEASIBILITY_SENSE
        obj = MOI.get(model, MOI.ObjectiveFunction{MOI.ScalarAffineFunction{T}}())
        if !iszero(MOI.constant(obj))
            error(
                "Nonzero constant in objective function not supported. Note " *
                "that the constant may be added by the substitution of a " *
                "bridged variable."
            )
        end
        for term in obj.terms
            _check_variable_index(term.variable_index)
            c[term.variable_index.value] = term.coefficient
        end
        if sense == MOI.MAX_SENSE
            for i in eachindex(c)
                c[i] = -c[i]
            end
        end
    end
    for i in eachindex(c)
        print(io, c[i])
        if i != length(c)
            print(io, " ")
        end
    end
    println(io)

    index_map = Vector{Tuple{Int, Int}}(
        undef, MOI.dimension(MOI.PositiveSemidefiniteConeTriangle(max_dim))
    )
    k = 0
    for col = 1:max_dim
        for row in 1:col
            k += 1
            index_map[k] = (row, col)
        end
    end

    function _print_entry(matrix, block, psd, k, value)
        if psd
            row, col = index_map[k]
            if row == col
                entry = value
            else
                entry = value / 2
            end
        else
            row = k
            col = k
            entry = value
        end
        println(io, matrix, ' ', block, ' ', row, ' ', col, ' ', entry)
    end
    function _print_constraint(block, psd, ci::MOI.ConstraintIndex)
        func = MOI.Utilities.canonical(con_function(ci))
        F0 = MOI.constant(func)
        for k in eachindex(F0)
            if !iszero(F0[k])
                _print_entry(0, block, psd, k, F0[k])
            end
        end
        for term in func.terms
            vi = term.scalar_term.variable_index
            _check_variable_index(vi)
            α = term.scalar_term.coefficient
            if !iszero(α)
                _print_entry(vi.value, block, psd, term.output_index, α)
            end
        end
    end
    for block in eachindex(nonneg)
        _print_constraint(block, false, nonneg[block])
    end
    for i in eachindex(psd)
        _print_constraint(length(nonneg) + i, true, psd[i])
    end
    return
end

# ==============================================================================
#
#   Base.read!
#
# ==============================================================================

# Convert a pair of row and column indices of a symmetric matrix into a vector
# index for the col-wise upper triangle
function mat_to_vec_idx(i::Int, j::Int)
    if i > j
        return mat_to_vec_idx(j, i)
    else
        return div((j - 1) * j, 2) + i
    end
end

"""
    Base.read!(io::IO, model::FileFormats.SDPA.Model)

Read `io` in the SDPA file format and store the result in `model`.
"""
function Base.read!(io::IO, model::Model{T}) where T
    if !MOI.is_empty(model)
        error("Cannot read in file because model is not empty.")
    end
    num_variables_read = false
    num_blocks = nothing
    block_sets = nothing
    function dim_to_set(s::AbstractString)
        block_dim = parse(Int, s)
        if block_dim > 0
            return MOI.PositiveSemidefiniteConeTriangle(block_dim)
        else
            return MOI.Nonnegatives(-block_dim)
        end
    end
    objective_read = false
    c = nothing
    funcs = nothing
    MOI.set(model, MOI.ObjectiveSense(), MOI.MIN_SENSE)
    while !eof(io)
        line = strip(readline(io))
        # Skip blank lines and comments (SDPA comments start with `"`).
        if startswith(line, '"')
            continue
        end
        if !num_variables_read
            if isempty(line)
                continue
            end
            num_variables_read = true
            MOI.add_variables(model, parse(Int, line))
        elseif num_blocks === nothing
            if isempty(line)
                continue
            end
            num_blocks = parse(Int, line)
        elseif block_sets === nothing
            if isempty(line) && !iszero(num_blocks)
                continue
            end
            block_sets = dim_to_set.(split(line))
            if length(block_sets) != num_blocks
                error("The number of blocks ($num_blocks) does not match the length of the list of blocks dimensions ($(length(block_sets))).")
            end
            funcs = [MOI.VectorAffineFunction(MOI.VectorAffineTerm{T}[], zeros(T, MOI.dimension(block_sets[i]))) for i in 1:num_blocks]
        elseif !objective_read
            num_vars = MOI.get(model, MOI.NumberOfVariables())
            if isempty(line) && !iszero(num_vars)
                continue
            end
            objective_read = true
            c = parse.(T, split(line))
            if length(c) != num_vars
                error("The number of variables ($num_vars) does not match the length of the list of coefficients for the objective function vector of coefficients ($(length(c))).")
            end
            obj = zero(MOI.ScalarAffineFunction{T})
            for i in eachindex(c)
                if !iszero(c[i])
                    push!(obj.terms, MOI.ScalarAffineTerm(c[i], MOI.VariableIndex(i)))
                end
            end
            MOI.set(model, MOI.ObjectiveFunction{typeof(obj)}(), obj)
        else
            if isempty(line)
                continue
            end
            values = split(line)
            if length(values) != 5
                error("Invalid line specifying entry: $line. There are $(length(values)) values instead of 5.")
            end
            matrix = parse(Int, values[1])
            block = parse(Int, values[2])
            row = parse(Int, values[3])
            col = parse(Int, values[4])
            if block_sets[block] isa MOI.PositiveSemidefiniteConeTriangle
                k = mat_to_vec_idx(row, col)
            else
                if row != col
                    error("Invalid line specifying entry: $line. `$row != $col` while block $block has dimension $(MOI.dimension(block_sets[block])) so it is a diagonal block.")
                end
                k = row
            end
            entry = parse(T, values[5])
            if col == row
                coef = entry
            else
                coef = entry * 2
            end
            if iszero(matrix)
                if !iszero(coef)
                    funcs[block].constants[k] += coef
                end
            else
                if !iszero(coef)
                    push!(funcs[block].terms, MOI.VectorAffineTerm(k,
                        MOI.ScalarAffineTerm(coef, MOI.VariableIndex(matrix))))
                end
            end
        end
    end
    for block in 1:num_blocks
        MOI.add_constraint(model, funcs[block], block_sets[block])
    end
    return
end

end


function mapreduce_index(f::Vector{<:Function}, op, itr, init)
    y = iterate(itr)
    y === nothing && return init
    v = op(init, (y[1]), f[y[2][2]])
    while true
        y = iterate(itr, y[2])
        y === nothing && break
        v = op(v, (y[1]), f[y[2][2]])
    end
    return v
end

# these functions are experimental and later must be moved to row_functions or hp_row_functions
function row_any_multi(ds::AbstractDataset, f::Vector{<:Function}, cols = :)
    colsidx = index(ds)[cols]
    @assert length(f) == length(colsidx) "number of provided functions must match the number of selected columns"
    _op_bool_add_multi(x::Bool,y::Bool) = x | y ? true : false
    op_for_any_multi!(x, y, f) = x .= _op_bool_add_multi.(x, f.(y))
    # mapreduce(identity, op_for_anymissing!, eachcol(ds)[colsidx[sel_colsidx]], init = zeros(Bool, size(ds,1)))
    mapreduce_index(f, op_for_any_multi!, view(_columns(ds),colsidx), zeros(Bool, size(ds,1)))
end

function row_all_multi(ds::AbstractDataset, f::Vector{<:Function}, cols = :)
    colsidx = index(ds)[cols]
    @assert length(f) == length(colsidx) "number of provided functions must match the number of selected columns"
    _op_bool_mult_multi(x::Bool,y::Bool) = x & y ? true : false
    op_for_all_multi!(x, y, f) = x .= _op_bool_mult_multi.(x, f.(y))
    # mapreduce(identity, op_for_anymissing!, eachcol(ds)[colsidx[sel_colsidx]], init = zeros(Bool, size(ds,1)))
    mapreduce_index(f, op_for_all_multi!, view(_columns(ds),colsidx), ones(Bool, size(ds,1)))
end

_op_bool_add_multi(x::Bool,y::Bool) = x || y ? true : false

function hp_bool_add_multi!(x, y, f)
    Threads.@threads for i in 1:length(x)
        @inbounds x[i] = _op_bool_add_multi(x[i], f(y[i]))
    end
    x
end

function hp_row_any_multi(ds::AbstractDataset, f::Vector{<:Function}, cols = :)
    colsidx = index(ds)[cols]
    @assert length(f) == length(colsidx) "number of provided functions must match the number of selected columns"
    _hp_op_for_any_multi!(x, y, f) = x .= hp_bool_add_multi!(x, y, f)
    # mapreduce(identity, op_for_anymissing!, eachcol(ds)[colsidx[sel_colsidx]], init = zeros(Bool, size(ds,1)))
    mapreduce_index(f, _hp_op_for_any_multi!, view(_columns(ds),colsidx), zeros(Bool, size(ds,1)))
end

_op_bool_mult_multi(x::Bool,y::Bool) = x && y ? true : false

function hp_bool_mult_multi!(x, y, f)
    Threads.@threads for i in 1:length(x)
        @inbounds x[i] = _op_bool_mult_multi(x[i], f(y[i]))
    end
    x
end

function hp_row_all_multi(ds::AbstractDataset, f::Vector{<:Function}, cols = :)
    colsidx = index(ds)[cols]
    @assert length(f) == length(colsidx) "number of provided functions must match the number of selected columns"
    _hp_op_for_all_multi!(x, y, f) = x .= hp_bool_mult_multi!(x, y, f)
    # mapreduce(identity, op_for_anymissing!, eachcol(ds)[colsidx[sel_colsidx]], init = zeros(Bool, size(ds,1)))
    mapreduce_index(f, _hp_op_for_all_multi!, view(_columns(ds),colsidx), ones(Bool, size(ds,1)))
end

__STRING(x) = string(x)
__STRING(x::AbstractString) = x
__STRING(::Missing) = ""
__STRING(x::Bool) = x ? "1" : "0"
__STRING(::Nothing) = ""

__CODEUNIT(x) = Base.CodeUnits(__STRING(x))

#
# function _op_for_row_join(x, y, f, delim, quotechar, idx, p)
#     idx[] += 1
#     if quotechar === nothing
#         if idx[] < p
#             x .*= STRING.(f[idx[]].(y))
#             x .*= delim
#         else
#             x .*= STRING.(f[idx[]].(y))
#             x .*= '\n'
#         end
#     else
#         if idx[] < p
#             x .*= quotechar
#             x .*= STRING.(f[idx[]].(y))
#             x .*= quotechar
#             x .*= delim
#         else
#             x .*= quotechar
#             x .*= STRING.(f[idx[]].(y))
#             x .*= quotechar
#             x .*= '\n'
#         end
#     end
#     x
# end
#
# function row_join(ds::AbstractDataset, f::Vector{<:Function}, cols  = :; delim = ',', quotechar = nothing)
#     colsidx = index(ds)[cols]
#     idx = Ref{Int}(0)
#     p = length(colsidx)
#     init0 = fill("", nrow(ds))
#     mapreduce(identity, (x,y)->_op_for_row_join(x,y,f, delim, quotechar, idx, p), view(_columns(ds), colsidx), init = init0)
# end

# some experimental ideas

function write_vals!(a, pos, x::Integer)
    n_positive, neg = Base.split_sign(x)
    #always we assume base = 10
    needed_space = neg + ndigits(x)
    available_space = length(a)-pos+1
    needed_space > available_space && throw(ArgumentError("not enough space in buffer to write value into it"))
    _base!(a, pos, 10, n_positive, 0, neg)
    pos+needed_space
end

#TODO this doesn't work - since in writeshortest() a must be a vector{UInt8} but we pass a view of a matrix in DLMReader
function write_vals!(a, pos, x::Union{Float16, Float32, Float64})
    needed_space = Base.Ryu.neededdigits(typeof(x))
    available_space = length(a)-pos+1
    # needed_space > available_space && throw(ArgumentError("not enough space in buffer to write value into it"))
    _writeshortest(a, pos, x)
end

function write_vals!(a, pos, x::Bool)
    needed_space = 1
    available_space = length(a)-pos+1
    needed_space > available_space && throw(ArgumentError("not enough space in buffer to write value into it"))
    x ? a[pos] = UInt8('1') : a[pos] = UInt8('0')
    pos+1
end

function write_vals!(a, pos, ::Missing)
    pos
end
function write_vals!(a, pos, ::Nothing)
    pos
end

function write_vals!(a, pos, x::AbstractString)
    needed_space = length(x)
    available_space = length(a)-pos+1
    needed_space > available_space && throw(ArgumentError("not enough space in buffer to write value into it"))
    z = Base.CodeUnits(x)
    for i in 1:length(x)
        a[i+pos-1] =z[i]
    end
    pos+length(x)
end

function write_vals!(a, pos, x)
    y = string(x)
    needed_space = length(y)
    available_space = length(a)-pos+1
    needed_space > available_space && throw(ArgumentError("not enough space in buffer to write value into it"))
    for i in 1:length(y)
        a[i+pos-1] = Base.CodeUnits(y)[i]
    end
    pos+length(y)
end

function write_delim!(a, pos, delim)
    needed_space = length(delim)
    available_space = length(a)-pos+1
    needed_space > available_space && throw(ArgumentError("not enough space in buffer to write value into it"))
    for i in 1:length(delim)
        a[i+pos-1] = delim[i]
    end
    pos+length(delim)
end

function write_eol!(a, pos)
    needed_space = 1
    available_space = length(a)-pos+1
    needed_space > available_space && throw(ArgumentError("not enough space in buffer to write value into it"))
    a[pos] = UInt8('\n')
    pos+1
end

function write_quotechar!(a, pos, quotechar)
    needed_space = 1
    available_space = length(a)-pos+1
    needed_space > available_space && throw(ArgumentError("not enough space in buffer to write value into it"))
    a[pos] = quotechar
    pos+1
end
function write_quotechar!(a, pos, ::Nothing)
    pos
end

function _base!(a, pos, base::Integer, x::Integer, pad::Int, neg::Bool)
    (x >= 0) | (base < 0) || throw(DomainError(x, "For negative `x`, `base` must be negative."))
    2 <= abs(base) <= 62 || throw(DomainError(base, "base must satisfy 2 ≤ abs(base) ≤ 62"))
    b = (base % Int)::Int
    digits = abs(b) <= 36 ? Base.base36digits : Base.base62digits
    # pad = 0 makes issue when x == 0 (n will be 0)
    n = neg + ndigits(x, base=b)
    i = n
    @inbounds while i > neg
        if b > 0
            a[i+pos-1] = digits[1 + (rem(x, b) % Int)::Int]
            x = div(x,b)
        else
            a[i+pos-1] = digits[1 + (mod(x, -b) % Int)::Int]
            x = cld(x,b)
        end
        i -= 1
    end
    if neg; @inbounds a[pos]=0x2d; end
    a
end

function _op_for_row_join!(buffer, currentpos, y, f, delim, quotechar, idx, p, lo, hi)
    idx[] += 1
    if quotechar === nothing
        if idx[] < p
            for i in lo:hi
                currentpos[i] = write_vals!(view(buffer, :, i), currentpos[i], f[idx[]](y[i]))
                currentpos[i] = write_delim!(view(buffer, :, i), currentpos[i], delim)
            end
        else
            for i in lo:hi
                currentpos[i] = write_vals!(view(buffer, :, i), currentpos[i], f[idx[]](y[i]))
                currentpos[i] = write_eol!(view(buffer, :, i), currentpos[i])
            end
        end
    else
        if nonmissingtype(eltype(y)) <: AbstractString
            quotecharval = UInt8(quotechar)
        else
            quotecharval = nothing
        end
        if idx[]<p
            for i in lo:hi
                currentpos[i] = write_quotechar!(view(buffer, :, i), currentpos[i], quotecharval)
                currentpos[i] = write_vals!(view(buffer, :, i), currentpos[i], f[idx[]](y[i]))
                currentpos[i] = write_quotechar!(view(buffer, :, i), currentpos[i], quotecharval)
                currentpos[i] = write_delim!(view(buffer, :, i), currentpos[i], delim)
            end
        else
            for i in lo:hi
                currentpos[i] = write_quotechar!(view(buffer, :, i), currentpos[i], quotecharval)
                currentpos[i] = write_vals!(view(buffer, :, i), currentpos[i], f[idx[]](y[i]))
                currentpos[i] = write_quotechar!(view(buffer, :, i), currentpos[i], quotecharval)
                currentpos[i] = write_eol!(view(buffer, :, i), currentpos[i])
            end
        end
    end
    currentpos
end

# buffer = Matrix{UInt8}(undef, lsize , nrow(ds))
# currentpos = ones(Int, nrow(ds))
function row_join!(buffer, currentpos, ds::AbstractDataset, f::Vector{<:Function}, cols = :; delim = ',', quotechar = nothing, threads = true)
    colsidx = multiple_getindex(index(ds), cols)
    p = length(colsidx)
    dlm = UInt8.(delim)
    if threads
        cz = div(nrow(ds), __NCORES)
        idx = [Ref{Int}(0) for _ in 1:__NCORES]
        Threads.@threads for i in 1:__NCORES
            lo = (i-1)*cz+1
            i == __NCORES ? hi = nrow(ds) : hi = i*cz
            mapreduce(identity, (x,y) -> _op_for_row_join!(buffer, x, y, f, dlm, quotechar, idx[i], p, lo, hi), view(_columns(ds),colsidx), init = currentpos)
        end
    else
        idx = Ref{Int64}(0)
        mapreduce(identity, (x,y) -> _op_for_row_join!(buffer, x, y, f, dlm, quotechar, idx, p, 1, nrow(ds)), view(_columns(ds),colsidx), init = currentpos)
    end
    currentpos, buffer
end

@inline function append_sign(x, plus, space, buf, pos)
    if signbit(x) && !isnan(x)  # suppress minus sign for signaling NaNs
        buf[pos] = UInt8('-')
        pos += 1
    elseif plus
        buf[pos] = UInt8('+')
        pos += 1
    elseif space
        buf[pos] = UInt8(' ')
        pos += 1
    end
    return pos
end

### From Base.Ryu, because we need buf to be View of an array not vector (maybe we should change it in Ryu?)
function _writeshortest(buf, pos, x::T,
                       plus=false, space=false, hash=true,
                       precision=-1, expchar=UInt8('e'), padexp=false, decchar=UInt8('.'),
                       typed=false, compact=false) where {T}
    @assert 0 < pos <= length(buf)
    # special cases
    if x == 0
        if typed && x isa Float16
            buf[pos] = UInt8('F')
            buf[pos + 1] = UInt8('l')
            buf[pos + 2] = UInt8('o')
            buf[pos + 3] = UInt8('a')
            buf[pos + 4] = UInt8('t')
            buf[pos + 5] = UInt8('1')
            buf[pos + 6] = UInt8('6')
            buf[pos + 7] = UInt8('(')
            pos += 8
        end
        pos = append_sign(x, plus, space, buf, pos)
        buf[pos] = UInt8('0')
        pos += 1
        if hash
            buf[pos] = decchar
            pos += 1
        end
        if precision == -1
            buf[pos] = UInt8('0')
            pos += 1
            if typed && x isa Float32
                buf[pos] = UInt8('f')
                buf[pos + 1] = UInt8('0')
                pos += 2
            end
            if typed && x isa Float16
                buf[pos] = UInt8(')')
                pos += 1
            end
            return pos
        end
        while hash && precision > 1
            buf[pos] = UInt8('0')
            pos += 1
            precision -= 1
        end
        if typed && x isa Float32
            buf[pos] = UInt8('f')
            buf[pos + 1] = UInt8('0')
            pos += 2
        end
        if typed && x isa Float16
            buf[pos] = UInt8(')')
            pos += 1
        end
        return pos
    elseif isnan(x)
        pos = append_sign(x, plus, space, buf, pos)
        buf[pos] = UInt8('N')
        buf[pos + 1] = UInt8('a')
        buf[pos + 2] = UInt8('N')
        if typed
            if x isa Float32
                buf[pos + 3] = UInt8('3')
                buf[pos + 4] = UInt8('2')
            elseif x isa Float16
                buf[pos + 3] = UInt8('1')
                buf[pos + 4] = UInt8('6')
            end
        end
        return pos + 3 + (typed && x isa Union{Float32, Float16} ? 2 : 0)
    elseif !isfinite(x)
        pos = append_sign(x, plus, space, buf, pos)
        buf[pos] = UInt8('I')
        buf[pos + 1] = UInt8('n')
        buf[pos + 2] = UInt8('f')
        if typed
            if x isa Float32
                buf[pos + 3] = UInt8('3')
                buf[pos + 4] = UInt8('2')
            elseif x isa Float16
                buf[pos + 3] = UInt8('1')
                buf[pos + 4] = UInt8('6')
            end
        end
        return pos + 3 + (typed && x isa Union{Float32, Float16} ? 2 : 0)
    end

    output, nexp = Base.Ryu.reduce_shortest(x, compact ? 999_999 : nothing)

    if typed && x isa Float16
        buf[pos] = UInt8('F')
        buf[pos + 1] = UInt8('l')
        buf[pos + 2] = UInt8('o')
        buf[pos + 3] = UInt8('a')
        buf[pos + 4] = UInt8('t')
        buf[pos + 5] = UInt8('1')
        buf[pos + 6] = UInt8('6')
        buf[pos + 7] = UInt8('(')
        pos += 8
    end
    pos = append_sign(x, plus, space, buf, pos)

    olength = Base.Ryu.decimallength(output)
    exp_form = true
    pt = nexp + olength
    if -4 < pt <= (precision == -1 ? (T == Float16 ? 3 : 6) : precision) &&
        !(pt >= olength && abs(mod(x + 0.05, 10^(pt - olength)) - 0.05) > 0.05)
        exp_form = false
        if pt <= 0
            buf[pos] = UInt8('0')
            pos += 1
            buf[pos] = decchar
            pos += 1
            for _ = 1:abs(pt)
                buf[pos] = UInt8('0')
                pos += 1
            end
            # elseif pt >= olength
            # nothing to do at this point
            # else
            # nothing to do at this point
        end
    else
        pos += 1
    end
    i = 0
    ptr = pointer(buf)
    ptr2 = pointer(Base.Ryu.DIGIT_TABLE)
    if (output >> 32) != 0
        q = output ÷ 100000000
        output2 = (output % UInt32) - UInt32(100000000) * (q % UInt32)
        output = q

        c = output2 % UInt32(10000)
        output2 = div(output2, UInt32(10000))
        d = output2 % UInt32(10000)
        c0 = (c % 100) << 1
        c1 = (c ÷ 100) << 1
        d0 = (d % 100) << 1
        d1 = (d ÷ 100) << 1
        Base.Ryu.memcpy(ptr, pos + olength - 2, ptr2, c0 + 1, 2)
        Base.Ryu.memcpy(ptr, pos + olength - 4, ptr2, c1 + 1, 2)
        Base.Ryu.memcpy(ptr, pos + olength - 6, ptr2, d0 + 1, 2)
        Base.Ryu.memcpy(ptr, pos + olength - 8, ptr2, d1 + 1, 2)
        i += 8
    end
    output2 = output % UInt32
    while output2 >= 10000
        c = output2 % UInt32(10000)
        output2 = div(output2, UInt32(10000))
        c0 = (c % 100) << 1
        c1 = (c ÷ 100) << 1
        Base.Ryu.memcpy(ptr, pos + olength - i - 2, ptr2, c0 + 1, 2)
        Base.Ryu.memcpy(ptr, pos + olength - i - 4, ptr2, c1 + 1, 2)
        i += 4
    end
    if output2 >= 100
        c = (output2 % UInt32(100)) << 1
        output2 = div(output2, UInt32(100))
        Base.Ryu.memcpy(ptr, pos + olength - i - 2, ptr2, c + 1, 2)
        i += 2
    end
    if output2 >= 10
        c = output2 << 1
        buf[pos + 1] = Base.Ryu.DIGIT_TABLE[c + 2]
        buf[pos - exp_form] = Base.Ryu.DIGIT_TABLE[c + 1]
    else
        buf[pos - exp_form] = UInt8('0') + (output2 % UInt8)
    end

    if !exp_form
        if pt <= 0
            pos += olength
            precision -= olength
            while hash && precision > 0
                buf[pos] = UInt8('0')
                pos += 1
                precision -= 1
            end
        elseif pt >= olength
            pos += olength
            precision -= olength
            for _ = 1:nexp
                buf[pos] = UInt8('0')
                pos += 1
                precision -= 1
            end
            if hash
                buf[pos] = decchar
                pos += 1
                if precision < 0
                    buf[pos] = UInt8('0')
                    pos += 1
                end
                while precision > 0
                    buf[pos] = UInt8('0')
                    pos += 1
                    precision -= 1
                end
            end
        else
            pointoff = olength - abs(nexp)
            Base.Ryu.memmove(ptr, pos + pointoff + 1, ptr, pos + pointoff, olength - pointoff + 1)
            buf[pos + pointoff] = decchar
            pos += olength + 1
            precision -= olength
            while hash && precision > 0
                buf[pos] = UInt8('0')
                pos += 1
                precision -= 1
            end
        end
        if typed && x isa Float32
            buf[pos] = UInt8('f')
            buf[pos + 1] = UInt8('0')
            pos += 2
        end
    else
        if olength > 1 || hash
            buf[pos] = decchar
            pos += olength
            precision -= olength
        end
        if hash && olength == 1
            buf[pos] = UInt8('0')
            pos += 1
        end
        while hash && precision > 0
            buf[pos] = UInt8('0')
            pos += 1
            precision -= 1
        end

        buf[pos] = expchar
        pos += 1
        exp2 = nexp + olength - 1
        if exp2 < 0
            buf[pos] = UInt8('-')
            pos += 1
            exp2 = -exp2
        elseif padexp
            buf[pos] = UInt8('+')
            pos += 1
        end

        if exp2 >= 100
            c = exp2 % 10
            Base.Ryu.memcpy(ptr, pos, ptr2, 2 * div(exp2, 10) + 1, 2)
            buf[pos + 2] = UInt8('0') + (c % UInt8)
            pos += 3
        elseif exp2 >= 10
            Base.Ryu.memcpy(ptr, pos, ptr2, 2 * exp2 + 1, 2)
            pos += 2
        else
            if padexp
                buf[pos] = UInt8('0')
                pos += 1
            end
            buf[pos] = UInt8('0') + (exp2 % UInt8)
            pos += 1
        end
    end
    if typed && x isa Float16
        buf[pos] = UInt8(')')
        pos += 1
    end

    return pos
end


@testset "Movement models" begin
    @test eqdist_ap isa PopState
    @test eqdist_ap0.P ≈ eqdist_eigvecs
    @test all(eqdist_ap.P .> 0)
    @test all(sum(move.M; dims = 1) .≈ 1)
    @test all(0 .≤ eqdist_ap0.P .≤ 1)
    @test sum(eqdist_ap0) ≈ 1.0
    @test sum(eqdist_ap) ≈ 100.0
end


using Test
using IPFitting, ProgressMeter, JuLIP, SHIPs
using JuLIP: read_dict
using JuLIP.Testing: test_fio
using JuLIP.MLIPs: IPSuperBasis
Fit = IPFitting
DB = IPFitting.DB
Data = Fit.Data

##

function rand_data(sym, N, configtype="rand")
   cubic = (N == 1)
   at = bulk(sym, cubic=cubic) * N
   rattle!(at, 0.1)
   F = (N == 1) ? nothing : rand(3, length(at))
   return Dat(at, configtype; E = rand(), F = F) # , V = rand(3,3))
end

##
println("Double-Check (de-)dictionisation of basis: ")
basis1 = rpi_basis(species = :Ti, N = 2, maxdeg = 8)
basis2 = rpi_basis(species = :Ti, N = 3, maxdeg = 6)
B = IPSuperBasis(basis1, basis2)
println(@test all(test_fio(basis1)))
println(@test all(test_fio(basis2)))
println(@test all(test_fio(B)))

println("Double-Check (de-)dictionisation of Dat: ")
data1 = [ rand_data(:Ti, 3, "md") for n = 1:10 ]
data2 = [ rand_data(:Ti, 1, "cell") for n = 1:10 ]
println(@test read_dict.(write_dict.(data1)) == data1)
println(@test read_dict.(write_dict.(data2)) == data2)

##
println("Create a temporary database.")
dbpath = tempname()
data = [data1; data2]
db = nothing

try
   global db = DB.LsqDB(dbpath, B, data)
   println(@test true)
catch
   @info("...something went wrong...")
   println(@test false)
end

println("checking consistency of db")
println(@test DB.dbpath(db) == dbpath)
println(@test DB.kronfile(dbpath) == dbpath * "_kron.h5")
println(@test DB.infofile(dbpath) == dbpath * "_info.json")
println(@test db.basis == B)
println(@test db.configs == data)

println("re-load the database")
db1 = DB.LsqDB(dbpath)
println(@test DB.dbpath(db1) == dbpath)
println(@test db1.basis == db.basis)
println(@test db1.configs == db.configs)
println(@test db1.Ψ == db.Ψ)

##
println("re-load the database with mmap=true")
db2 = DB.LsqDB(dbpath, mmap=true)
println(@test DB.dbpath(db2) == dbpath)
println(@test db2.basis == db.basis)
println(@test db2.configs == db.configs)
println(@test db2.Ψ == db.Ψ)


module DataScripts

using LsqFit
using Plots
using DataFrames
using SFGTools

export plotrecent, pump_probe_process, cal_spectrometer

const DATA_PATH = "/Users/lackner/Documents/DataSFG"

# f = figure()
# close(f)

function plotrecent(idx=1; pixel=false, sfwl=false, dograb=false)
    dograb && grab(DATA_PATH)
    df = list_spectra()
    sort!(df, cols=:id, rev=true)

    if maximum(idx) < 9999
        # Treat as DataFrame Index
        data = load_spectra(df[:id][idx])
    else
        # Treat as Spectrum ID
        data = load_spectra(idx)
    end

    if length(idx) == 1
        # Make data iterable
        data = [data]
    end

    data = average.(data)

    # Empty Plot
    plot()
    for i = 1:length(data)
        if pixel
            x_binning = get_attribute(data[i], "x_binning")
            npixel = 512 / x_binning
            x = 1:npixel
            xlabelstr = "Pixel Number"
        elseif sfwl
            x = get_wavelength(data[i])
            xlabelstr = "Wavelength in nm"
        else
            x = get_ir_wavelength(data[i])
            xlabelstr = "IR Wavelength in nm"
        end

        name = get_attribute(data[i], "name")
        plot!(x, data[i].s, label=name)
        xlabel!(xlabelstr)
        ylabel!("Counts per Second")
    end
    gui()
    return nothing
end

"""
Process pump probe spectra. The data is splitted in the middle of the camera.

Keyword Arguments:

* `eventwidth`: Max. width of events that are being removed (default 8)
* `savepath`:   Path were the processed data is stored (default is the folder where the raw data is loaded from)
* `wlrange`:    Figures are cropped to these xlims (defaults to full range of available values) eg.: `wlrange=(3300, 3600)`
"""
function pump_probe_process(idarray;
    eventwidth=8,
    savepath="default",
    wlrange="default")

    print("Loading Data...")
    data = SFGTools.load_spectra(idarray)
    SFGTools.rm_events!.(data)
    data = SFGTools.average.(data)

    if savepath == "default"
        savepath = splitdir(splitdir(SFGTools.getdir(data[1].id))[1])[1]
        savepath = joinpath(savepath, "processed")
    end
    !isdir(savepath) && mkdir(savepath)

    dltime = SFGTools.get_pump_delay.(data)
    λ = SFGTools.get_ir_wavelength(data[1])
    ν = SFGTools.get_ir_wavenumber(data[1])
    name = SFGTools.get_attribute(data[1], "name")

    # Sort with respect to delay times
    p = sortperm(dltime)
    dltime = dltime[p]
    data = data[p]
    println("\tDone.")

    # Check if all delays are unique
    length(unique(dltime)) != length(dltime) && warn("Not all delay times are unique!")

    # Get Wavelength range
    wlrange == "default" && (wlrange = (minimum(λ), maximum(λ)))

    # Make seperate spectra for upper and lower part of CCD camera for all time
    # delays.
    print("Generating Spectra...")
    npixels = size(data[1].s)
    spectra = zeros(npixels[1],2)
    split = npixels[2] ÷ 2
    for d in data
        # Upper part
        spectra[:,1] += squeeze(sum(d.s[:,1:split], 2), 2)
        # Lower part
        spectra[:,2] += squeeze(sum(d.s[:,split+1:end], 2), 2)
    end
    # Normalize
    spectra /= length(data)

    plotarray = Plots.Plot[]
    p1 = Plots.plot(λ, spectra, labels=["A", "B"])
    Plots.title!(name * "/nSpectra Total")
    Plots.ylabel!("Counts [1/s]")
    Plots.xlabel!("IR Wavelength [nm]")
    Plots.xlims!(wlrange)
    push!(plotarray, p1)
    Plots.savefig(joinpath(savepath, "spectra.png"))
    println("\tDone.")

    # Make Heatmap
    print("Generating Heatmaps...")
    hmap = zeros(length(data), npixels[1], 2)
    for i in eachindex(data)
        hmap[i,:,1] = data[i].s[:,1:split]
        hmap[i,:,2] = data[i].s[:,split+1:end]
    end
    p2 = Plots.heatmap(λ, dltime, hmap[:,:,1], clim=(minimum(hmap), maximum(hmap)))
    Plots.title!(name * "\nA")
    Plots.xlabel!("IR Wavelength [nm]")
    Plots.ylabel!("Pump Delay [ps]")
    Plots.xlims!(wlrange)
    push!(plotarray, p2)
    Plots.savefig(joinpath(savepath, "heatmap_A.png"))

    p3 = heatmap(λ, dltime, hmap[:,:,2], clim=(minimum(hmap), maximum(hmap)))
    Plots.title!(name * "\nB")
    Plots.xlabel!("IR Wavelength [nm]")
    Plots.ylabel!("Pump Delay [ps]")
    Plots.xlims!(wlrange)
    push!(plotarray, p3)
    Plots.savefig(joinpath(savepath, "heatmap_B.png"))
    println("\tDone.")

    # Animated Spectra
    print("Animating Spectra")
    labels=("A", "B")
    anim = Plots.@animate for i = 1:size(hmap, 1)
        print(".")

        p1 = Plots.plot(λ, hmap[i,:,:], labels=labels, ylim=(minimum(hmap), maximum(hmap)))
        Plots.title!("Delay = $(round(dltime[i], 1)) ps")
        Plots.ylabel!("Counts [1/s]")
        Plots.xlabel!("IR Wavelength [nm]")
        Plots.xlims!(wlrange)

        p2 = Plots.heatmap(λ, dltime, hmap[:,:,1], clim=(minimum(hmap), maximum(hmap)))
        Plots.title!(name * "\nA")
        Plots.xlabel!("IR Wavelength [nm]")
        Plots.ylabel!("Pump Delay [ps]")
        Plots.scatter!([wlrange[1], wlrange[end]], [dltime[i], dltime[i]], markersize=10, legend=false, color=:white)
        Plots.xlims!(wlrange)

        p3 = heatmap(λ, dltime, hmap[:,:,2], clim=(minimum(hmap), maximum(hmap)))
        Plots.title!(name * "\nB")
        Plots.xlabel!("IR Wavelength [nm]")
        Plots.ylabel!("Pump Delay [ps]")
        Plots.scatter!([wlrange[1], wlrange[end]], [dltime[i], dltime[i]], markersize=10, legend=false, color=:white)
        Plots.xlims!(wlrange)

        Plots.plot(p1, p2, p3, layout=(3,1), size=(600,1200))
    end
    Plots.gif(anim, joinpath(savepath, "spectra.gif"), fps=2)
    println("\tDone.")

    return spectra, plotarray
end

function cal_spectrometer(idarray)

    data = SFGTools.load_spectra(idarray)
    data = SFGTools.average.(data)
    names = get_attribute(data, "name")
    splitted_names = split.(names, "_")

    λcal = zeros(length(data))
    locs = fill("", length(data))
    for i in eachindex(splitted_names)
        λcal[i] = splitted_names[i][end-1] |> parse
        locs[i] = splitted_names[i][end]
    end

    # Check if the data is suitable
    for i = 1:length(data)
        length(data[i].s) != 512 && error("All spectra need to have 512 data points")
        !any(["center", "right", "left"] .== locs[i]) && error("Filenames have to have the form \"*_334.15_left\" etc. (see help)")
    end

    # Remove everything but the peaks
    for i in eachindex(data)
        if locs[i] == "center"
            data[i].s[[1:236..., 276:end...]] = NaN
        elseif locs[i] == "left"
            data[i].s[20:end] = NaN
        elseif locs[i] == "right"
            data[i].s[1:end-20] = NaN
        end
    end

    peakpositions = fill(0, length(data))
    for i in 1:length(data)
        peakpositions[i] = findmax(data[i].s)[2]
    end
    peakpositions = reshape(peakpositions, (3, length(data) ÷ 3))'
    peakpositions = hcat(peakpositions[:,2], peakpositions[:,1], peakpositions[:,3])

    λ_spectrometer = Float64[]
    for (i, d) in enumerate(data)
        λ = get_attribute(d, "spectrometer_wavelength")
        push!(λ_spectrometer, λ)
    end
    λ_spectrometer = reshape(λ_spectrometer, (3, 7))'
    λ_spectrometer = hcat(λ_spectrometer[:,2], λ_spectrometer[:,1], λ_spectrometer[:,3])

    Δp = abs.(diff(peakpositions, 2))
    Δλ = abs.(diff(λ_spectrometer, 2))
    nmperpixel = Δλ ./ Δp

    mean_nmperpixel = mean(nmperpixel, 2)
    mean_nmperpixel = squeeze(mean_nmperpixel, 2)

    model(x, p) = p[1] * x.^2 + p[2] * x + p[3]
    fit = curve_fit(model, λ_spectrometer[:,2], mean_nmperpixel, [-3e-8, 5e-6, 0.02])
    xfit = linspace(330, 600, 100)
    yfit = model(xfit, fit.param)

    p1 = scatter(λ_spectrometer[:,2], mean_nmperpixel)
    plot!(xfit, yfit)
    xlabel!("Central Spectrometer Wavelength in nm")
    ylabel!("Wavelength Density [nm/pixel]")

    # the real center is in between pixels 256 and 257!
    Δp0 = peakpositions[:,2] - 256.5

    offset_fit = curve_fit(model, λ_spectrometer[:,2], Δp0, [-3e-4, 0.25, -32.0])
    offset_yfit = model(xfit, offset_fit.param)

    p2 = scatter(λ_spectrometer[:,2], Δp0)
    plot!(xfit, offset_yfit)
    xlabel!("Central Spectrometer Wavelength in nm")
    ylabel!("Pixel Offset")

    plot(p1, p2)
    gui()

    info("Add the following code to the `get_wavelength` function at the appropriate location in the SFGTools package:")

    println("
    # Calibration parameters were determined on $(now())
    # Pixel offset
    Δp = $(offset_fit.param[1]) * λ0^2 + $(offset_fit.param[2]) * λ0 - $(offset_fit.param[3])
    # Wavelenths per pixel
    pλ = $(fit.param[1]) * λ0^2 + $(fit.param[2]) * λ0 + $(fit.param[3])
    ")

    info("The right location is inside the if-elseif-else statement. Change the last else to:")

    println("elseif date < DateTime(\"$(now())\")")

    info("and add a new final else statement with the above code that contains the calibration parameters.")
    return nothing
end

end #module


module NiLang

using Reexport
@reexport using NiLangCore
import NiLangCore: ⊕, ⊖

using FixedPointNumbers: Q20f43, Fixed
export Fixed43
const Fixed43 = Q20f43

include("utils.jl")
include("vars.jl")
include("instructs.jl")
include("ulog.jl")
include("stack.jl")
include("complex.jl")
include("autobcast.jl")

include("autodiff/autodiff.jl")

include("stdlib/stdlib.jl")

include("deprecations.jl")

export AD
end # module


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

using RadiationDetectorSignals
using Test

using ArraysOfArrays, FillArrays, StructArrays, Unitful

@testset "detector_waveforms" begin
    nwf = 50
    wfdata = nestedview(rand(128, nwf))
    timedata = Fill(0:0.1:12.7,nwf)

    @test @inferred(ArrayOfRDWaveforms((wfdata, timedata))) isa StructArray
    A = ArrayOfRDWaveforms((timedata, wfdata))

    @test A.time[1] == A[1].time == 0:0.1:12.7
    @test A.value isa ArrayOfSimilarArrays
    @test A.value[1] == A[1].value
end # testset

@testset "detector_waveforms with units" begin
    nwf = 50
    wfdata = nestedview(rand(128, nwf) * u"eV")
    timedata = Fill((0:0.1:12.7) * u"ns",nwf)

    @test @inferred(ArrayOfRDWaveforms((wfdata, timedata))) isa StructArray
    A = ArrayOfRDWaveforms((timedata, wfdata))

    @test A.time[1] == A[1].time == (0:0.1:12.7) * u"ns"
    @test A.value isa ArrayOfSimilarArrays
    @test A.value[1] == A[1].value
end # testset