text:
"""
@(SIGNATURES)
    Generate an approximately continuous LQR controller from a discrete controller for use in simulation
        -requires X and U nominal trajectories and gains K, in matrix form; number of knot points and time step size from solve
        -option for first-order hold interpolation (cubic state, linear gains and control)
        -option for controller saturation with max/min limits
"""
function generate_controller(X::Matrix,U::Matrix,K::Array{Float64,3},N::Int64,dt::Float64,integration::Symbol=:zoh,u_min=-Inf,u_max=Inf)
    """
    X - (n,N)
    U - zoh: (m,N-1)
    K - zoh: (m,n,N-1)

    """
    m, n, _ = size(K)

    # Get matrices in correct format for Interpolations.jl
    K_zoh = [K[:,:,k] for k = 1:N-1]
    X_zoh = [X[:,k] for k = 1:N-1]
    U_zoh = [U[:,k] for k = 1:N-1]

    # For zoh and Interpolations.jl we repeate the final K,X,U
    push!(K_zoh,K[:,:,N-1])
    push!(X_zoh,X[:,N-1])
    push!(U_zoh,U[:,N-1])

    # zero-order hold interpolation on gains, and trajectories, NOTE: cubic on states
    K_interp_zoh = interpolate(K_zoh,BSpline(Constant()))
    X_interp_zoh = interpolate(X_zoh,BSpline(Constant()))
    X_zoh = []
    for i = 1:n
        push!(X_zoh,[X[i,k] for k = 1:N])
    end
    function X_interp_zoh(j)
        x = zeros(n)
        for i = 1:n
            x[i] = interpolate(X_zoh[i],BSpline(Cubic(Line(OnGrid()))))(j)
        end
        return x
    end
    U_interp_zoh = interpolate(U_zoh,BSpline(Constant()))

    function controller_zoh(x,t)
        j = t/dt + 1
        return max.(min.(K_interp_zoh(floor(Int64,j))*(x - X_interp_zoh(j)) + U_interp_zoh(floor(Int64,j)),u_max),u_min)
    end
    return controller_zoh
end

function interpolate_trajectory(solver::Solver, X, U, t)
    X = copy(X)
    U = copy(U)
    integration = solver.integration
    n,m,N = get_sizes(solver)
    dt = solver.dt
    time = collect(get_time(solver))
    if integration == :midpoint
        interp_X = interpolate_rows(time, X, :midpoint)
    else  # :rk3, rk4
        interp_X = interpolate_rows(time, X, :cubic)
    end

    interp_U = interpolate_rows(time, U, :zoh)
    # else  # :zoh
    #     interp_U = interpolate_rows(time, U, :linear)
    # end
    Xnew = interp_X(t)
    Unew = interp_U(t)
    return Xnew, Unew
end

"""
$(SIGNATURES)
Generates a function that interpolates the rows of a matrix
# Arguments
* interpolation: interpolation scheme. Options are [:zoh, :midpoint, :linear, :cubic]
"""
function interpolate_rows(t::Vector{T}, X::Matrix{T}, interpolation=:cubic) where T
    n,N = size(X)
    if interpolation == :zoh
        bc = Constant()
    elseif interpolation == :midpoint
        bc = Constant()
        Xm = copy(X)
        for k = 1:N-1
            Xm[:,k] = (Xm[:,k] + Xm[:,k+1])/2
        end
        Xm[:,end] = X[:,end-1]
        X = Xm
    elseif interpolation == :linear
        bc = Linear()
    elseif interpolation == :cubic
        bc = Cubic(Line(OnGrid()))
    end
    # Use zero order hold for :zoh and :midpoint
    interpolation in [:linear, :cubic] ? zoh = false : zoh = true

    # Create interpolator
    itr = interpolate(X, (NoInterp(), BSpline(bc) ))
    dt = t[2] - t[1]

    # Interpolation function
    function interp(t_)
        i = t_./dt .+ 1
        if zoh
            i = floor.(i)
        end
        itr(1:n,i)
    end
end

"""
$(SIGNATURES)
    Time Varying Discrete Linear Quadratic Regulator (TVLQR)
"""
function lqr(A::Array{Float64,3}, B::Array{Float64,3}, Q::AbstractArray{Float64,2}, R::AbstractArray{Float64,2}, Qf::AbstractArray{Float64,2})::Array{Float64,3}
    n,m,N = size(B)
    N += 1
    K = zeros(m,n,N-1)
    S = zeros(n,n)

    # Boundary conditions
    S .= Qf
    for k = 1:N-1
        # Compute control gains
        K[:,:,k] = (R + B[:,:,k]'*S*B[:,:,k])\(B[:,:,k]'*S*A[:,:,k])
        # Calculate cost-to-go for backward propagation
        S .= Q + A[:,:,k]'*S*A[:,:,k] - A[:,:,k]'*S*B[:,:,k]*K[:,:,k]
    end
    K
end

function lqr(results::SolverResults, solver::Solver)
    n, m, N = get_sizes(solver)
    Q, R, Qf, xf = get_cost_matrices(solver)
    A, B = results.fx, results.fu
    lqr(A,B,Q,R,Qf)
end

"""
@(SIGNATURES)
    Simulate LQR controller
"""
function simulate_controller(f!::Function,integration::Symbol,controller::Function,n::Int64,m::Int64,dt::Float64,x0::AbstractVector,tf::Float64,u_min=-Inf,u_max=Inf)
    t_sim = 0:dt:tf
    N_sim = length(t_sim)
    if integration == :ode45
        function closed_loop_dynamics(t,x)
            xdot = zeros(n)
            f(xdot,x,controller(x,t))
            xdot
        end

        t_sim, X_sim = ode45(closed_loop_dynamics, x0, 0:dt:tf)
        X_sim = to_array(X_sim)
        U_sim = zeros(m,N_sim)
        for k = 1:N_sim-1
            U_sim[:,k] = controller(X_sim[:,k],t_sim[k])
        end
        return X_sim, U_sim
    else
        # Discrete dynamics
        discretizer = eval(integration)
        fd! = discretizer(f!, dt)

        # determine number of knot points for simulation
        N_sim = convert(Int64,floor(tf/dt))+1

        # Allocate memory for simulated state and control trajectories
        X_sim = zeros(n,N_sim)
        X_sim[:,1] = x0
        U_sim = zeros(m,N_sim)

        # simulate
        for (k,t) in enumerate(range(0,length=N_sim-1,stop=tf-dt))
            U_sim[:,k] = controller(X_sim[:,k],t)
            fd(view(X_sim,:,k+1), X_sim[:,k],U_sim[:,k])
        end
        return X_sim, U_sim
    end
end


text:
# test functions for the 5taxon networks
# functions used in tests_5taxon_readTopology.jl
# fixit: it would be better that within each function, it would look
# for the needed edges/nodes starting from the hybrid node
# so that it would be a general function (not bounded to the specific indeces)
# Claudia November 2014

# Case C bad triangle II
function testCaseC(net::HybridNetwork)
    n=searchHybridNode(net);
    node = n[1];
    node.k != 3 ? error("k diff than 3") : nothing
    node.isVeryBadTriangle ? nothing : error("does not know it is very bad triangle")
    node.isExtBadTriangle ? error("thinks it is extremely bad triangle") : nothing
    net.hasVeryBadTriangle ? nothing : error("net does not know it has very bad triangle")
    net.numBad == 0 ? nothing : error("net.numBad should be 0")
    net.numHybrids != 1 ? error("should have 1 hybrid, but net.numHybrids is $(net.numHybrids): $([n.number for n in net.hybrid])") : nothing
end

# Case F bad diamond I
function testCaseF(net::HybridNetwork)
    net.visited = [e.istIdentifiable for e in net.edge];
    net.numHybrids == 1 ? nothing : error("networks should have one hybrid node and it has $(net.numHybrids)")
    node=net.hybrid[1];
    node.k != 4 ? error("k diff than 4") : nothing
    (net.edge[3].inCycle != node.number || net.edge[4].inCycle != node.number || net.edge[5].inCycle != node.number || net.edge[7].inCycle != node.number ) ? error("edges not correctly in cycle") : nothing
    (net.node[3].inCycle  != node.number || net.node[4].inCycle  != node.number || net.node[6].inCycle  != node.number || net.node[7].inCycle  != node.number) ? error("nodes not correctly in cycle") : nothing
    !node.isBadDiamondI ? error("does not know it is bad diamond I") : nothing
    node.isBadDiamondII ? error("thinks it is bad diamond II") : nothing
    net.edge[2].containRoot ? error("edge can contain root") : nothing
    (!net.edge[3].hybrid || !net.edge[3].isMajor) ? error("edge 3 is not hybrid or major") : nothing
    (!net.edge[5].hybrid || net.edge[5].isMajor) ? error("edge 5 is not hybrid or is major") : nothing
    net.node[4].gammaz != net.edge[3].gamma*net.edge[4].z ? error("node 4 gammaz not correctly calculated") : nothing
    net.node[6].gammaz != net.edge[5].gamma*net.edge[7].z ? error("node 6 gammaz not correctly calculated") : nothing
    (net.edge[4].istIdentifiable || net.edge[7].istIdentifiable) ? error("edges identifiable that should not") : nothing
    !net.edge[8].istIdentifiable ? error("edge 8 not identifiable") : nothing
    net.visited[8] = false;
    !all([!id for id in net.visited]) ? error("edges not identifiable as identifiable") : nothing
    [n.number for n in net.leaf] == [1,2,5,7,8] ? nothing : error("net.leaf is wrong")
end

# Case G
function testCaseG(net::HybridNetwork)
    net.visited = [e.istIdentifiable for e in net.edge];
    net.numHybrids == 1 ? nothing : error("networks should have one hybrid node and it has $(net.numHybrids)")
    node=net.hybrid[1];
    node.k != 4 ? error("k diff than 4") : nothing
    (net.edge[5].inCycle != node.number || net.edge[6].inCycle != node.number || net.edge[7].inCycle != node.number || net.edge[9].inCycle != node.number ) ? error("edges not correctly in cycle") : nothing
    (net.node[5].inCycle  != node.number || net.node[6].inCycle  != node.number || net.node[8].inCycle  != node.number || net.node[9].inCycle  != node.number) ? error("nodes not correctly in cycle") : nothing
    (node.isBadDiamondI || node.isBadDiamondII ) ? error("thinks it is bad diamond") : nothing
    net.edge[4].containRoot ? error("edge can contain root") : nothing
    (!net.edge[5].hybrid || !net.edge[5].isMajor) ? error("edge 5 is not hybrid or major") : nothing
    (!net.edge[7].hybrid || net.edge[7].isMajor) ? error("edge 7 is not hybrid or is major") : nothing
    net.node[5].gammaz != -1 ? error("node 5 gammaz should be -1") : nothing
    (!net.edge[6].istIdentifiable || !net.edge[3].istIdentifiable || !net.edge[9].istIdentifiable) ? error("edges identifiable as not identifiable") : nothing
    net.visited[6] = false;
    net.visited[3] = false;
    net.visited[9] = false;
    !all([!id for id in net.visited]) ? error("edges not identifiable as identifiable") : nothing
    [n.number for n in net.leaf] == [1,2,4,7,8] ? nothing : error("net.leaf is wrong")
end

# Case H
function testCaseH(net::HybridNetwork)
    net.visited = [e.istIdentifiable for e in net.edge];
    net.numHybrids == 1 ? nothing : error("networks should have one hybrid node and it has $(net.numHybrids)")
    node=net.hybrid[1];
    node.k != 4 ? error("k diff than 4") : nothing
    (net.edge[4].inCycle != node.number || net.edge[5].inCycle != node.number || net.edge[7].inCycle != node.number || net.edge[9].inCycle != node.number ) ? error("edges not correctly in cycle") : nothing
    (net.node[4].inCycle  != node.number || net.node[6].inCycle  != node.number || net.node[8].inCycle  != node.number || net.node[10].inCycle  != node.number) ? error("nodes not correctly in cycle") : nothing
    (node.isBadDiamondI || node.isBadDiamondII) ? error("thinks it is bad diamond") : nothing
    net.edge[8].containRoot ? error("8 can contain root") : nothing
    (!net.edge[9].hybrid || !net.edge[9].isMajor) ? error("edge 9 is not hybrid or major") : nothing
    (!net.edge[4].hybrid || net.edge[4].isMajor) ? error("edge 4 is not hybrid or is major") : nothing
    node.gammaz != -1 ? error("hybrid node gammaz should be -1") : nothing
    (!net.edge[3].istIdentifiable || !net.edge[5].istIdentifiable || !net.edge[7].istIdentifiable) ? error("edge9,5,13not identifiable") : nothing
    net.visited[3] = false;
    net.visited[5] = false;
    net.visited[7] = false;
    !all([!id for id in net.visited]) ? error("edges not identifiable as identifiable") : nothing
    [n.number for n in net.leaf] == [1,2,4,5,6] ? nothing : error("net.leaf is wrong")
end

# Case J
function testCaseJ(net::HybridNetwork)
    net.visited = [e.istIdentifiable for e in net.edge];
    net.numHybrids == 1 ? nothing : error("networks should have one hybrid node and it has $(net.numHybrids)")
    node=net.hybrid[1];
    node.k != 5 ? error("k diff than 5") : nothing
    (net.edge[2].inCycle != node.number || net.edge[4].inCycle != node.number || net.edge[6].inCycle != node.number || net.edge[10].inCycle != node.number || net.edge[8].inCycle != node.number) ? error("edges not correctly in cycle") : nothing
    (net.node[2].inCycle  != node.number || net.node[4].inCycle  != node.number || net.node[6].inCycle  != node.number || net.node[10].inCycle  != node.number || net.node[9].inCycle) != node.number ? error("nodes not correctly in cycle") : nothing
    net.edge[1].containRoot ? error("edge can contain root") : nothing
    (!net.edge[2].hybrid || !net.edge[2].isMajor) ? error("edge 2 is not hybrid or major") : nothing
    node.gammaz != -1 ? error("hybrid node gammaz should be -1") : nothing
    (!net.edge[4].istIdentifiable || !net.edge[6].istIdentifiable || !net.edge[10].istIdentifiable) ? error("edge9,5,10not identifiable") : nothing
    net.visited[4] = false;
    net.visited[6] = false;
    net.visited[10] = false;
    !all([!id for id in net.visited]) ? error("edges not identifiable as identifiable") : nothing
    [n.number for n in net.leaf] == [1,3,4,5,6] ? nothing : error("net.leaf is wrong")
end

# Case D bad triangle I
function testCaseD(net::HybridNetwork)
 net.visited = [e.istIdentifiable for e in net.edge];
    n = searchHybridNode(net);
    node = n[1];
    node.k != 3 ? error("k diff than 3") : nothing
    node.isVeryBadTriangle ? nothing : error("does not know it is very bad triangle")
    node.isExtBadTriangle ? error("thinks it is extremely bad triangle") : nothing
    net.hasVeryBadTriangle ? nothing : error("net does not know it has very bad triangle")
    net.numBad == 0 ? nothing : error("net.numBad should be 0")
    net.numHybrids != 1 ? error("should have 1 hybrid, but net.numHybrids is $(net.numHybrids): $([n.number for n in net.hybrid])") : nothing
end

# Case E bad triangle I
function testCaseE(net::HybridNetwork)
    net.visited = [e.istIdentifiable for e in net.edge];
    n = searchHybridNode(net);
    node = n[1];
    node.k != 3 ? error("k diff than 3") : nothing
    node.isVeryBadTriangle ? nothing : error("does not know it is very bad triangle")
    node.isExtBadTriangle ? error("thinks it is extremely bad triangle") : nothing
    net.hasVeryBadTriangle ? nothing : error("net does not know it has very bad triangle")
    net.numBad == 0 ? nothing : error("net.numBad should be 0")
    net.numHybrids != 1 ? error("should have 1 hybrid, but net.numHybrids is $(net.numHybrids): $([n.number for n in net.hybrid])") : nothing
end

# Case I bad diamond II
function testCaseI(net::HybridNetwork)
    net.numHybrids == 1 ? nothing : error("networks should have one hybrid node and it has $(net.numHybrids)")
    node = net.hybrid[1];
    node.isBadDiamondII ? nothing : error("does not know it is bad diamond II")
    node.isBadDiamondI ? error("thinks it is bad diamond I") : nothing
    node.k == 4 ? nothing : error("k should be 4")
    net.visited = [e.istIdentifiable for e in net.edge];
    edge4 = getIndexEdge(4,net);
    edge1 = getIndexEdge(1,net);
    edge2 = getIndexEdge(2,net);
    edge3 = getIndexEdge(3,net);
    edge9 = getIndexEdge(9,net);
    edge10 = getIndexEdge(10,net);
    edge6 = getIndexEdge(6,net);
    node1 = getIndexNode(-2,net);
    node2 = getIndexNode(-3,net);
    node5 = getIndexNode(-6,net);
    node3 = getIndexNode(3,net);
    (net.edge[edge4].inCycle != node.number || net.edge[edge9].inCycle != node.number || net.edge[edge6].inCycle != node.number || net.edge[edge10].inCycle != node.number ) ? error("edges not correctly in cycle") : nothing
    (net.node[node1].inCycle  != node.number || net.node[node2].inCycle  != node.number || net.node[node5].inCycle  != node.number || net.node[node3].inCycle  != node.number) ? error("nodes 1,5,11,12 not correctly in cycle") : nothing
    (net.edge[edge1].containRoot || net.edge[edge2].containRoot || net.edge[edge3].containRoot) ? error("edges can contain root and shouldn't") : nothing
    (!net.edge[edge4].hybrid || !net.edge[edge4].isMajor) ? error("edge 4 is not hybrid or major") : nothing
    net.edge[edge3].length != 0 ? error("edges should have length 0") : nothing
    net.edge[edge3].istIdentifiable ? error("edge9,4 identifiable and should not") : nothing
    (net.edge[edge4].istIdentifiable && net.edge[edge9].istIdentifiable && net.edge[edge6].istIdentifiable && net.edge[edge10].istIdentifiable) || error("edges that should be identifiable, are not")
    net.visited[edge4] = false;
    net.visited[edge9] = false;
    net.visited[edge6] = false;
    net.visited[edge10] = false;
    !all([!id for id in net.visited]) ? error("edges not identifiable as identifiable") : nothing
    [n.number for n in net.leaf] == [1,2,4,5,6] ? nothing : error("net.leaf is wrong")
end

# tree example
function testTree(net::HybridNetwork)
    !all([!e.hybrid for e in net.edge]) ? error("some edge is still hybrid") : nothing
    !all([!e.hybrid for e in net.node]) ? error("some node is still hybrid") : nothing
    !all([!e.hasHybEdge for e in net.node]) ? error("some node has hybrid edge") : nothing
    !all([e.isMajor for e in net.edge]) ? error("some edge is not major") : nothing
    !all([e.containRoot for e in net.edge]) ? error("some edge cannot contain root") : nothing
    edge9 = getIndexNumEdge(9,net);
    edge5 = getIndexNumEdge(5,net);
    (!net.edge[5].istIdentifiable || !net.edge[3].istIdentifiable) ? error("edge3,5 not identifiable") : nothing
    net.visited = [e.istIdentifiable for e in net.edge];
    net.visited[3] = false;
    net.visited[5] = false;
    !all([!id for id in net.visited]) ? error("edges not identifiable as identifiable") : nothing
    net.edge[2].length != 1.5 ? error("edge length for 2 is wrong") : nothing
    net.edge[4].length != 0.2 ? error("edge length for 4 is wrong") : nothing
    [n.number for n in net.leaf] == [1,2,4,6,7] ? nothing : error("net.leaf is wrong")
end

ext: jl


text:
using DataStructures

const MIN_AREA = 10
const MAX_COST = 2.0
const RADIUS = 80

function match!(objects::Vector{Object}, blobs::Vector{Blob}, num_objects::Int;
                radius=RADIUS, min_area=MIN_AREA)
    possible_matches = limit_to_radius(objects, blobs, radius)
    matches = naive_match(objects, blobs, possible_matches)
    finished_merging = false
    remove_duplicate_matches!(matches, objects)
    while !finished_merging
        remove_duplicate_matches!(matches, objects)
        finished_merging = evaluate_object_merges(matches, objects, blobs)
    end
    # parition_merged_blobs!(merges, objects, blobs)
    merge_blob_fragments!(matches, objects, blobs, possible_matches)
    update_with_new_objects!(matches, objects, blobs, num_objects, min_area)
end

function limit_to_radius(objects, blobs, radius)
    possible_matches = zeros(Int, length(objects), length(blobs))
    for (n, object) in enumerate(objects)
        for (m, blob) in enumerate(blobs)
            if distance(object, blob) < radius
                possible_matches[n, m] = 1
            end
        end
    end
    return possible_matches
end

function naive_match(objects, blobs, possible_matches)
    matches = zeros(possible_matches)
    for (n, object) in enumerate(objects)
        min_cost = Inf
        min_blob = -1
        for (m, blob) in enumerate(blobs)
            if possible_matches[n, m] == 0
                continue
            end
            c = cost(object, blob)
            if c < min_cost
                min_cost = c
                min_blob = m
            end
        end
        if min_blob > 0
            matches[n, min_blob] = 1
        end
    end
    return matches
end

function remove_duplicate_matches!(matches, objects)
    (n, m) = size(matches)
    for j in 1:m
        number_matched = 0
        transients = Queue(Int)
        for i in 1:n
            if matches[i, j] != 0
                number_matched += 1
                if is_transient(objects[i])
                    enqueue!(transients, i)
                end
            end
        end
        if number_matched > length(transients)
            for t in transients
                matches[t, j] = 0
            end
        end
    end
    return matches
end

function evaluate_object_merges(matches, objects, blobs)
    finished_checking_merges = true
    (n, m) = size(matches)
    for j in 1:m
        if sum(view(matches, :, j)) > 1
            blob_matches = Vector{Object}()
            match_indices = Vector{Int}()
            for i in 1:n
                if matches[i, j] != 0
                    push!(blob_matches, objects[i])
                    push!(match_indices, i)
                end
            end
            best_match = composite_cost_comparison(blob_matches, blobs[j])
            if best_match == -1  # indicates best match is to composite object
                for i in 1:n
                    if matches[i, j] != 0
                        if best_match < 0
                            best_match = i
                        else
                            matches[i, j] = 0
                        end
                    end
                end
                objects[best_match] = combine(blob_matches)
            else
                best_match = match_indices[best_match]
                finished_checking_merges = false
                for i in 1:n
                    if best_match != i
                        matches[i, j] = 0
                    end
                end
            end
        end
    end
    return finished_checking_merges
end

function merge_blob_fragments!(matches, objects, blobs, possible_matches)
    (n, m) = size(matches)
    unsecure_matches = Vector{Int}()
    secure_matches = Vector{Int}()
    unallocated_blobs = Vector{Int}()
    for j in 1:m
        if sum(view(matches, :, j)) == 0
            push!(unallocated_blobs, j)
        else
            for i in 1:m
                if matches[i, j] != 0
                    if is_transient(objects[i])
                        push!(unsecure_matches, j)
                    else
                        push!(secure_matches, j)
                    end
                    break
                end
            end
        end
    end
    attempt_merges!(secure_matches, unallocated_blobs, matches, objects, blobs, possible_matches)
    attempt_merges!(secure_matches, unsecure_matches, matches, objects, blobs, possible_matches)
    attempt_merges!(unsecure_matches, unallocated_blobs, matches, objects, blobs, possible_matches)
end

function attempt_merges!(sources, targets, matches, objects, blobs, possible_matches)
    (n, m) = size(matches)
    finished = false
    while !finished
        finished = true
        for idx in sources
            source_blob = blobs[idx]
            object_idx = 0
            for object_idx in 1:n
                if matches[object_idx, idx] != 0
                    break
                end
            end
            object = objects[object_idx]
            for target_idx in length(targets):-1:1
                blob_idx = targets[target_idx]
                if possible_matches[object_idx, blob_idx] == 0
                    continue
                end
                merged = merge_if_match_improves!(object, source_blob, blobs[blob_idx])
                if merged
                    matches[:, blob_idx] = 0
                    blobs[idx] = combine(source_blob, blobs[blob_idx])
                    deleteat!(targets, target_idx)
                    finished = false
                end
            end
        end
    end
end

function merge_if_match_improves!(object, source_blob, target_blob)
    original_cost = cost(object, source_blob)
    merged_blob = combine(source_blob, target_blob)
    merged_cost = cost(object, merged_blob)
    return merged_cost < original_cost
end

function update_with_new_objects!(matches, objects, blobs, num_objects, min_area)
    (n, m) = size(matches)
    for i in n:-1:1
        if sum(view(matches, i, :)) == 0
            deleteat!(objects, i)
        else
            j = 0
            for j in 1:m
                if matches[i, j] != 0
                    break
                end
            end
            objects[i] = update(objects[i], blobs[j])
        end
    end
    for j in 1:m
        if sum(view(matches, :, j)) == 0 && blobs[j].area > min_area
            num_objects += 1
            push!(objects, Object(blobs[j], num_objects))
        end
    end
    return num_objects
end


text:
using Compat, Compat.LinearAlgebra
using FillArrays
using MathematicalSystems
using HybridSystems
using Polyhedra

function horizontal_jump_example(lib::Polyhedra.Library, shift::Bool=false)
    A = LightAutomaton(2)
    add_transition!(A, 1, 2, 1)
    add_transition!(A, 2, 1, 2)

    if shift
        H1 = polyhedron(hrep([Matrix(1.0I, 2, 2); -Matrix(1.0I, 2, 2)], [ones(2); ones(2)]), lib)
    else
        H1 = polyhedron(hrep([Matrix(1.0I, 2, 2); -Matrix(1.0I, 2, 2)], [1., 3, 1, -1]), lib)
    end
    H2 = polyhedron(hrep([Matrix(1.0I, 2, 2); -Matrix(1.0I, 2, 2)], [4, 1 + shift, -2, 1]), lib)
    U = polyhedron(hrep(reshape([1., -1.], 2, 1), [2., -2.]), lib)

    S = ConstrainedDiscreteIdentitySystem.(2, [H1, H2])

    s1 = ConstrainedLinearControlDiscreteSystem([1. 0; 0 1], reshape([1.; 0], 2, 1), H1, U)
    s2 = ConstrainedLinearControlDiscreteSystem([1. 0; 0 1], reshape([1.; 0], 2, 1), H2, U)
    Re = [s1, s2]

    sw = AutonomousSwitching()
    Sw = Fill(sw, 2)

    HybridSystem(A, S, Re, Sw)
end


text:
# This implements viscous creep laws and routines to compute with them
#
# Note that various simple creep laws are defined in this file;
# more complex ones (such as DislocationCreep) are in separate files
# in the same directory
#
# In case you want to add new creep laws, have a look at how the ones
# here are implemented. Please add tests as well!

using ..MaterialParameters: MaterialParamsInfo
import GeoParams.param_info

export  DislocationCreep,
        SetDislocationCreep

const AxialCompression, SimpleShear, Invariant = 1,2,3

# Dislocation Creep ------------------------------------------------
"""
    DislocationCreep(n = 1.0NoUnits, r = 0.00.0NoUnits, A = 1.5MPa/s, E = 476.0kJ/mol, V = 6e-6m^3/mol, apparatus = AxialCompression )

Defines the flow law parameter of a dislocation creep law
The (isotropic) dislocation creep law, as used by experimtalists, is given by
```math
     \\dot{\\gamma} = A \\sigma_\\mathrm{d}^n f_\\mathrm{H2O}^r \\exp(-\\frac{E+PV}{RT})
```
where ``n`` is the power law exponent,
``r`` is the exponent of fugacity dependence,
``A`` is a pre-exponential factor [MPa^(n+r)] (if manually defined, n and r must be either pre-defined or substituted),
``E`` is the activation energy [kJ/mol], ``V`` is the activation volume [m^3/mol]. ``\\dot{\\gamma}`` is the ordinary strain rate [1/s],
and ``\\sigma_\\mathrm{d}`` is the differential stress which are converted into second invariants using the apparatus type that can be
either AxialCompression, SimpleShear or Invariant.
If the flow law paramters are already given as a function of second invariants, choose apparatus = "Invariant"
```julia-repl
julia> x2      =   DislocationCreep(n=3)
DislocationCreep: n=3, r=0.0, A=1.5 MPa^-3 s^-1, E=476.0 kJ mol^-1, V=6.0e-6 m^3 mol^-1, Apparatus=AxialCompression
```
"""
@with_kw_noshow struct DislocationCreep{T,N,U1,U2,U3,U4,U5} <: AbstractCreepLaw{T}
    Name::NTuple{N,Char}    =   ""               # The name is encoded as a NTuple{Char} to make it isbits
    n::GeoUnit{T,U1}        = 1.0NoUnits         # power-law exponent
    r::GeoUnit{T,U1}        = 0.0NoUnits         # exponent of water-fugacity dependence
    A::GeoUnit{T,U2}        = 1.5MPa^(-n-r)/s    # pre-exponential factor
    E::GeoUnit{T,U3}        = 476.0kJ/mol        # activation energy
    V::GeoUnit{T,U4}        = 6e-6m^3/mol        # activation volume
    R::GeoUnit{T,U5}        = 8.314J/mol/K       # Universal gas constant
    Apparatus::Int32        = AxialCompression   # type of experimental apparatus, either AxialCompression, SimpleShear or Invariant
end
DislocationCreep(args...) = DislocationCreep(NTuple{length(args[1]), Char}(collect.(args[1])), convert.(GeoUnit,args[2:end-1])..., args[end])

function param_info(s::DislocationCreep)
    name = String(collect(s.Name))
    eq = L"\tau_{ij} = 2 \eta  \dot{\varepsilon}_{ij}"
    if name == ""
        return MaterialParamsInfo(Equation=eq)
    end
    inf = DislocationCreep_info[name][2]
    return MaterialParamsInfo(Equation=eq, Comment=inf.Comment, BibTex_Reference=inf.BibTex_Reference)
end

# Calculation routines for linear viscous rheologies
# All inputs must be non-dimensionalized (or converted to consitent units) GeoUnits
function computeCreepLaw_EpsII(TauII, a::DislocationCreep, p::CreepLawVariables)
    @unpack_val n,r,A,E,V,R = a
    @unpack_val P,T,f       = p

    FT, FE = CorrectionFactor(a)

    return A*(TauII*FT)^n*f^r*exp(-(E + P*V)/(R*T))/FE
end

function computeCreepLaw_EpsII(TauII, a::DislocationCreep; P::_R, T::_R, f::_R, args...) where _R<:Real
    @unpack_val n,r,A,E,V,R = a

    FT, FE = CorrectionFactor(a);

    return A*(TauII*FT)^n*f^r*exp(-(E + P*V)/(R*T))/FE;
end

# EpsII .= A.*(TauII.*FT).^n.*f.^r.*exp.(-(E.+P.*V)./(R.*T))./FE; Once we have a
# All inputs must be non-dimensionalized (or converted to consistent units) GeoUnits
function computeCreepLaw_TauII(EpsII, a::DislocationCreep, p::CreepLawVariables)
    @unpack_val n,r,A,E,V,R = a
    @unpack_val P,T,f       = p

    FT, FE = CorrectionFactor(a)

    return A^(-1/n)*(EpsII*FE)^(1/n)*f^(-r/n)*exp((E + P*V)/(n * R*T))/FT;
end

# EpsII .= A.*(TauII.*FT).^n.*f.^r.*exp.(-(E.+P.*V)./(R.*T))./FE; Once we have a
# All inputs must be non-dimensionalized (or converted to consistent units) GeoUnits
function computeCreepLaw_TauII(EpsII, a::DislocationCreep; P::_R, T::_R, f::_R, args...) where _R<:Real
    @unpack_val n,r,A,E,V,R = a

    FT, FE = CorrectionFactor(a);

    return A^(-1/n)*(EpsII*FE)^(1/n)*f^(-r/n)*exp((E + P*V)/(n * R*T))/FT
end

# Print info
function show(io::IO, g::DislocationCreep)
    print(io, "DislocationCreep: Name = $(String(collect(g.Name))), n=$(g.n.val), r=$(g.r.val), A=$(g.A.val), E=$(g.E.val), V=$(g.V.val), Apparatus=$(g.Apparatus)" )
end
#-------------------------------------------------------------------------

# This computes correction factors to go from experimental data to tensor format
# A nice discussion
function CorrectionFactor(a::DislocationCreep{_T}) where {_T}
    if a.Apparatus == AxialCompression
        FT = sqrt(one(_T)*3)               # relation between differential stress recorded by apparatus and TauII
        FE = one(_T)*2/sqrt(one(_T)*3)     # relation between gamma recorded by apparatus and EpsII
    elseif a.Apparatus == SimpleShear
        FT = one(_T)*2                     # it is assumed that the flow law parameters were derived as a function of differential stress, not the shear stress. Must be modidified if it is not the case
        FE = one(_T)*2
    end
    return FT,FE
end

# Add pre-defined creep laws
"""
    SetDislocationCreep["Name of Dislocation Creep"]
This is a dictionary with pre-defined creep laws
"""
SetDislocationCreep(name::String) = DislocationCreep_info[name][1]

DislocationCreep_info = Dict([

# Olivine rheology
("Dry Olivine | Hirth & Kohlstedt (2003)",
# after Hirth, G. & Kohlstedt (2003), D. Rheology of the upper mantle and the mantle wedge: A view from the experimentalists.
# Inside the subduction Factory 83?105. Table 1, "dry dislocation" parameters
    (DislocationCreep(
        Name = "Dry Olivine | Hirth & Kohlstedt (2003)",
        n = 3.5NoUnits,
        A = 1.1e5MPa^(-3.5)/s,
        E = 530.0kJ/mol,
        V = 15e-6m^3/mol,
        Apparatus = AxialCompression,
        r = 0.0NoUnits),
    MaterialParamsInfo(Comment = "Still to be verified with the original publication (BK). Values checked, plots are not reproduced (DK).",
        BibTex_Reference = parse_bibtex("""
            @incollection{eiler_rheology_2003,
            address = {Washington, D. C.},
            title = {Rheology of the upper mantle and the mantle wedge: {A} view from the experimentalists},
            volume = {138},
            isbn = {978-0-87590-997-4},
            shorttitle = {Rheology of the upper mantle and the mantle wedge},
            url = {http://www.agu.org/books/gm/v138/138GM06/138GM06.shtml},
            language = {en},
            urldate = {2019-10-09},
            booktitle = {Geophysical {Monograph} {Series}},
            publisher = {American Geophysical Union},
            author = {Hirth, Greg and Kohlstedt, David},
            editor = {Eiler, John},
            year = {2003},
            doi = {10.1029/138GM06},
            pages = {83--105},
            }
        """))
    )
)

# Olivine rheology
("Wet Olivine | Hirth & Kohlstedt (2003)",
    # After Hirth, G. & Kohlstedt (2003), D. Rheology of the upper mantle and the mantle wedge: A view from the experimentalists.
    #   Inside the subduction Factory 83?105. Table 1, "wet dislocation" parameters
    #  Note that this assumes C_OH=1000
    (DislocationCreep(
        Name = "Wet Olivine | Hirth & Kohlstedt (2003)",
        n = 3.5NoUnits,
        A = 90MPa^(-3.5)/s,
        E = 480kJ/mol,
        V = 11e-6m^3/mol,
        r   = 1.2NoUnits,
        Apparatus = AxialCompression),
        MaterialParamsInfo(Comment = "Still to be verified with the original publication (BK). Values checked, plots are not reproduced (DK).",
        BibTex_Reference = parse_bibtex("""
            @incollection{HirthKohlstedt_OlivineRheology_2003,
            address = {Washington, D. C.},
            title = {Rheology of the upper mantle and the mantle wedge: {A} view from the experimentalists},
            volume = {138},
            isbn = {978-0-87590-997-4},
            shorttitle = {Rheology of the upper mantle and the mantle wedge},
            url = {http://www.agu.org/books/gm/v138/138GM06/138GM06.shtml},
            language = {en},
            urldate = {2019-10-09},
            booktitle = {Geophysical {Monograph} {Series}},
            publisher = {American Geophysical Union},
            author = {Hirth, Greg and Kohlstedt, David},
            editor = {Eiler, John},
            year = {2003},
            doi = {10.1029/138GM06},
            pages = {83--105},
            }
        """))
    )
)

# Quartz Diorite rheology
("Quartz Diorite | Hansen & Carter (1982)",
    #  After Hansen (1982), 'Semibrittle creep of selected crustal rocks at 1000 MPa.' and, Hansen & Carter (1982),
    #  'Flow properties of continental lithosphere.'
    #  Hansen (1982), Fig. 53, page 184 in PDF viewer and table 18, page 224
    #  Carter & Tsenn (1986), table 4, page 18 in PDF viewer
    (DislocationCreep(
        Name = "Quartz Diorite | Hansen & Carter (1982)",
        n = 2.25NoUnits,
        A = 3.5e-2MPa^(-2.25)/s,
        E = 212kJ/mol,
        V = 0m^3/mol,
        r   = 0NoUnits,
        Apparatus =   AxialCompression),
        MaterialParamsInfo(Comment = "Verified with the original publication Hansen(1982)(NM). Values checked, plots are not reproduced (NM).",
        BibTex_Reference = parse_bibtex("""
             @article{carter1982stress,
             title={Stress magnitudes in natural rock salt},
             author={Carter, Neville L and Hansen, Francis D and Senseny, Paul E},
             journal={Journal of Geophysical Research: Solid Earth},
             volume={87},
             number={B11},
             pages={9289--9300},
             year={1982},
             publisher={Wiley Online Library}
             }
        """))
    )
)

# Diabase rheology
("Diabase | Caristan (1982)",
    #  After Caristan (1982), 'The transition from high temperature creep to fracture in Maryland diabase.'
    #  and, Bremond (1999),
    #  'Hydrothermalism and diapirism in the Archean: gravitational instability constraints'
    #  Bremond (1999), page 5 in text
    (DislocationCreep(
        Name = "Diabase | Caristan (1982)",
        n = 3.05NoUnits,
        A = 6.0e-2MPa^(-3.05)/s,
        E = 276kJ/mol,
        V = 0m^3/mol,
        r   = 0NoUnits,
        Apparatus =   AxialCompression),
        MaterialParamsInfo(Comment = "Values checked (Bremond (1999)), plots are not reproduced (NM).",
        BibTex_Reference = parse_bibtex("""
             @article{caristan1982transition,
             title={The transition from high temperature creep to fracture in Maryland diabase},
             author={Caristan, Y},
             journal={Journal of Geophysical Research: Solid Earth},
             volume={87},
             number={B8},
             pages={6781--6790},
             year={1982},
             publisher={Wiley Online Library}
             }
        """))
    )
)

# Tumut Pond Serpentinite rheology
("Tumut Pond Serpentinite | Raleigh and Paterson (1965)",
    #  After Raleigh and Paterson (1965), 'Experimental deformation of serpentinite and its tectonic implications'
    #  and, Bremond (1999),
    #  'Hydrothermalism and diapirism in the Archean: gravitational instability constraints'
    #  Bremond (1999), page 5 in text
    (DislocationCreep(
        Name = "Tumut Pond Serpentinite | Raleigh and Paterson (1965)",
        n = 2.8NoUnits,
        A = 6.3e-7MPa^(-2.8)/s,
        E = 66kJ/mol,
        V = 0m^3/mol,
        r   = 0NoUnits,
        Apparatus =   AxialCompression),
        MaterialParamsInfo(Comment = "Values checked (Bremond (1999)), plots are not reproduced (NM).",
        BibTex_Reference = parse_bibtex("""
             @article{raleigh1965experimental,
             title={Experimental deformation of serpentinite and its tectonic implications},
             author={Raleigh, CB and Paterson, MS},
             journal={Journal of Geophysical Research},
             volume={70},
             number={16},
             pages={3965--3985},
             year={1965},
             publisher={Wiley Online Library}
             }
        """))
    )
)

# Maryland strong diabase rheology
("Maryland strong diabse | Mackwell et al. (1998)",
    #  After Mackwell et al. (1998), 'High-temperatur deformation of dry diabase with application to tectonics on Venus'
    #  Mackwell et al. (1998), page 980, equation in text
    (DislocationCreep(
        Name = "Maryland strong diabse | Mackwell et al. (1998)",
        n = 4.7NoUnits,
        A = 8MPa^(-4.7)/s,
        E = 485kJ/mol,
        V = 0m^3/mol,
        r = 0NoUnits,
        Apparatus = AxialCompression),
        MaterialParamsInfo(Comment = "Values checked (Mackwell et al. (1998))(NM), plots are not reproduced (NM).",
        BibTex_Reference = parse_bibtex("""
            @article{mackwell1998high,
            title={High-temperature deformation of dry diabase with application to tectonics on Venus},
            author={Mackwell, SJ and Zimmerman, ME and Kohlstedt, DL},
            journal={Journal of Geophysical Research: Solid Earth},
            volume={103},
            number={B1},
            pages={975--984},
            year={1998},
            publisher={Wiley Online Library}
            }
        """))
    )
)

# Mafic Granulite rheology
("Mafic Granulite | Wilks and Carter (1990)",
    #  After Li, Gerya and Burg (2010), table 2
    #  referring to Ranalli (1995), 'Rheology of the Earth' (Book), page 334, table 10.3
    #  referring to Wilks and Carter (1990), 'Rheology of some continental lower crustal rocks', Fig. 6, Pikwitonei Granulite
    (DislocationCreep(
        Name = "Mafic Granulite | Wilks and Carter (1990)",
        n = 4.2NoUnits,
        A = 1.4e4MPa^(-4.2)/s,
        E = 445kJ/mol,
        V = 0m^3/mol,
        r = 0NoUnits,
        Apparatus = AxialCompression),
        MaterialParamsInfo(Comment = "Values checked (Wilks and Carter (1990))(NM), plots are not reproduced (NM).",
        BibTex_Reference = parse_bibtex("""
            @article{wilks1990rheology,
            title={Rheology of some continental lower crustal rocks},
            author={Wilks, Kenneth R and Carter, Neville L},
            journal={Tectonophysics},
            volume={182},
            number={1-2},
            pages={57--77},
            year={1990},
            publisher={Elsevier}
            }
        """))
    )
)

# Wet Quartzite rheology
("Wet Quartzite | Ueda et al. (2008)",
    #  Ueda et al. (2008), table 1
    (DislocationCreep(
        Name = "Wet Quartzite | Ueda et al. (2008)",
        n = 2.3NoUnits,
        A = 1*exp10(-3.5)MPa^(-2.3)/s,
        E = 154kJ/mol,
        V = 0m^3/mol,
        r = 0NoUnits,
        Apparatus = AxialCompression),
        MaterialParamsInfo(Comment = "Values checked (Ueda et al. (2008))(NM), plots are not reproduced (NM).",
        BibTex_Reference = parse_bibtex("""
            @article{ueda2008subduction,
            title={Subduction initiation by thermal--chemical plumes: numerical studies},
            author={Ueda, Kosuke and Gerya, Taras and Sobolev, Stephan V},
            journal={Physics of the Earth and Planetary Interiors},
            volume={171},
            number={1-4},
            pages={296--312},
            year={2008},
            publisher={Elsevier}
            }
        """))
    )
)

# Granite rheology
("Granite | Carter and Tsenn (1987)",
    #  Huismans et al. (2001), table 2
    #  referring to Carter and Tsenn (1987), 'Flow properties of continental lithosphere', table 4, Westerly Granite (dry)
    #  referring to Hansen and Carter (1983), 'Semibrittle Creep Of Dry And Wet Westerly Granite At 1000 MPa', not accessable
    (DislocationCreep(
        Name = "Granite | Carter and Tsenn (1987)",
        n = 3.3NoUnits,
        A = 1.0*exp10(-5.7)MPa^(-3.3)/s,
        E = 186.5kJ/mol,
        V = 0m^3/mol,
        r = 0NoUnits,
        Apparatus = AxialCompression),
        MaterialParamsInfo(Comment = "Values checked (Carter and Tsenn (1987))(NM), plots are not reproduced (NM).",
        BibTex_Reference = parse_bibtex("""
            @article{carter1987flow,
            title={Flow properties of continental lithosphere},
            author={Carter, Neville L and Tsenn, Michael C},
            journal={Tectonophysics},
            volume={136},
            number={1-2},
            pages={27--63},
            year={1987},
            publisher={Elsevier}
            }
        """))
    )
)

# Plagioclase An75 rheology
("Plagioclase An75 | Ji and Zhao (1993)",
    #  Ranalli (1995), page 334, table 10.3
    #  referring to Ji and Zhao (1993), 'Flow laws of multiphase rocks calculated from experimental data on the constituent phases', table 2 , plagioclase (Ab25An75)
    #  referring to Shelton and Tullis (1981), 'Experimental flow laws for crustal rocks', not accessable
   (DislocationCreep(
        Name = "Plagioclase An75 | Ji and Zhao (1993)",
        n = 3.2NoUnits,
        A = 3.27e-4MPa^(-3.2)/s,
        E = 238kJ/mol,
        V = 0m^3/mol,
        r = 0NoUnits,
        Apparatus = AxialCompression),
        MaterialParamsInfo(Comment = "Values checked (Ji and Zhao (1993))(NM), plots are not reproduced (NM).",
        BibTex_Reference = parse_bibtex("""
            @article{ji1993flow,
            title={Flow laws of multiphase rocks calculated from experimental data on the constituent phases},
            author={Ji, Shaocheng and Zhao, Pinglao},
            journal={Earth and Planetary Science Letters},
            volume={117},
            number={1-2},
            pages={181--187},
            year={1993},
            publisher={Elsevier}
            }
        """))
    )
)

# Dry Anorthite rheology
("Dry Anorthite | Rybecki and Dresen (2000)",
    #  Rybecki and Dresen (2000), table 2 + table 3
    (DislocationCreep(
        Name = "Dry Anorthite | Rybecki and Dresen (2000)",
        n = 3.0NoUnits,
        A = 1.0*exp10(-12.7)MPa^(-3.0)/s,
        E = 648kJ/mol,
        V = 0m^3/mol,
        r = 0NoUnits,
        Apparatus = AxialCompression),
        MaterialParamsInfo(Comment = "Values checked (Rybecki and Dresen (2000))(NM), plots are not reproduced (NM).",
        BibTex_Reference = parse_bibtex("""
            @article{rybacki2000dislocation,
            title={Dislocation and diffusion creep of synthetic anorthite aggregates},
            author={Rybacki, Erik and Dresen, Georg},
            journal={Journal of Geophysical Research: Solid Earth},
            volume={105},
            number={B11},
            pages={26017--26036},
            year={2000},
            publisher={Wiley Online Library}
            }
        """))
    )
)

# Wet Anorthite rheology
("Wet Anorthite | Rybecki and Dresen (2000)",
    #  Rybecki and Dresen (2000), table 2 + table 3
    (DislocationCreep(
        Name = "Wet Anorthite | Rybecki and Dresen (2000)",
        n = 3.0NoUnits,
        A = 1.0*exp10(-2.6)MPa^(-3.0)/s,
        E = 356kJ/mol,
        V = 0m^3/mol,
        r = 0NoUnits,
        Apparatus = AxialCompression),
        MaterialParamsInfo(Comment = "Values checked (Rybecki and Dresen (2000))(NM), plots are not reproduced (NM).",
        BibTex_Reference = parse_bibtex("""
            @article{rybacki2000dislocation,
            title={Dislocation and diffusion creep of synthetic anorthite aggregates},
            author={Rybacki, Erik and Dresen, Georg},
            journal={Journal of Geophysical Research: Solid Earth},
            volume={105},
            number={B11},
            pages={26017--26036},
            year={2000},
            publisher={Wiley Online Library}
            }
        """))
    )
)

# Wet Olivine rheology
("Wet Olivine | Hirth and Kohlstedt (2003)",
    #  Hirth and Kohlstedt (2003), table 1, no constant C_OH
    (DislocationCreep(
        Name = "Wet Olivine | Hirth and Kohlstedt (2003)",
        n = 3.5NoUnits,
        A = 1600.0MPa^(-3.5)/s,
        E = 520kJ/mol,
        V = 11.0e-6m^3/mol,
        r = 1.2NoUnits,
        Apparatus = AxialCompression),
        MaterialParamsInfo(Comment = "Values checked (Hirth and Kohlstedt (2003))(NM), plots are not reproduced (NM).",
        BibTex_Reference = parse_bibtex("""
            @article{hirth2003rheology,
            title={Rheology of the upper mantle and the mantle wedge: A view from the experimentalists},
            author={Hirth, Greg and Kohlstedf, D},
            journal={Geophysical monograph-american geophysical union},
            volume={138},
            pages={83--106},
            year={2003},
            publisher={AGU AMERICAN GEOPHYSICAL UNION}
            }
        """))
    )
)

# Wet Quarzite rheology
("Wet Quarzite | Kirby (1983)",
    #  Li, Gerya and Burg (2010), table 2
    #  Ranalli (1995), 'Rheology of the Earth' (Book)
    #  referring to Kirby (1983), table 2, first quartzite (wet)
    #  referring to Koch et al. (1981), unpublished manuscript...
    (DislocationCreep(
        Name = "Wet Quarzite | Kirby (1983)",
        n = 2.3NoUnits,
        A = 3.2e-4MPa^(-2.3)/s,
        E = 154kJ/mol,
        V = 0m^3/mol,
        r = 0NoUnits,
        Apparatus = AxialCompression),
        MaterialParamsInfo(Comment = "Values checked (Kirby (1983))(NM), plots are not reproduced (NM).",
        BibTex_Reference = parse_bibtex("""
            @article{kirby1983rheology,
            title={Rheology of the lithosphere},
            author={Kirby, Stephen H},
            journal={Reviews of Geophysics},
            volume={21},
            number={6},
            pages={1458--1487},
            year={1983},
            publisher={Wiley Online Library}
            }
        """))
    )
)

# Wet Upper Mantle Olivine rheology
("Wet Upper Mantle Olivine | Afonso and Ranalli (2004)",
    #  Schmalholz, Kaus, Burg (2009), table 1
    #  referring to Afonso and Ranalli (2004), table 1, wet peridotite
    #  referring to Chopra and Paterson papers, but values dont fit the Afonso and Ranalli (2004) ones
    (DislocationCreep(
        Name = "Wet Upper Mantle Olivine | Afonso and Ranalli (2004)",
        n = 4.0NoUnits,
        A = 2.0e3MPa^(-4.0)/s,
        E = 471kJ/mol,
        V = 0.0m^3/mol,
        r = 0.0NoUnits,
        Apparatus = AxialCompression),
        MaterialParamsInfo(Comment = "Values checked (Afonso and Ranalli (2004))(NM), plots are not reproduced (NM).",
        BibTex_Reference = parse_bibtex("""
            @article{afonso2004crustal,
            title={Crustal and mantle strengths in continental lithosphere: is the jelly sandwich model obsolete?},
            author={Afonso, Juan Carlos and Ranalli, Giorgio},
            journal={Tectonophysics},
            volume={394},
            number={3-4},
            pages={221--232},
            year={2004},
            publisher={Elsevier}
            }
        """))
    )
)

# Granite rheology
("Granite | Tirel et al. (2008)",
    #  Tirel et al. (2008), table 1
    #  referring to Kirby and Kronenberg (1987), table 3
    #  different values for n and A in Kirby and Kronenberg (1987) compared with Tirel et al. (2008)
    (DislocationCreep(
        Name = "Granite | Tirel et al. (2008)",
        n = 3.2NoUnits,
        A = 1.25e-9MPa^(-3.2)/s,
        E = 123kJ/mol,
        V = 0.0m^3/mol,
        r = 0.0NoUnits,
        Apparatus = AxialCompression),
        MaterialParamsInfo(Comment = "Values checked (Tirel et al. (2008))(NM), plots are not reproduced (NM).",
        BibTex_Reference = parse_bibtex("""
            @article{tirel2008dynamics,
            title={Dynamics and structural development of metamorphic core complexes},
            author={Tirel, C{\'e}line and Brun, Jean-Pierre and Burov, Evgueni},
            journal={Journal of Geophysical Research: Solid Earth},
            volume={113},
            number={B4},
            year={2008},
            publisher={Wiley Online Library}
            }
        """))
    )
)

]); # end of setting pre-defined creep laws


text:
function gaussian_elimination(A::Array{Float64,2})

    rows = size(A,1)
    cols = size(A,2)

    # Main loop going through all columns
    for k = 1:min(rows,cols)

        # Step 1: finding the maximum element for each column
        max_index = indmax(abs.(A[k:end,k])) + k-1

        # Check to make sure matrix is good!
        if (A[max_index, k] == 0)
            println("matrix is singular! End!")
            exit(0)
        end

        # Step 2: swap row with highest value for that column to the top
        temp_vector = A[max_index, :]
        A[max_index, :] = A[k, :]
        A[k, :] = temp_vector
        #println(A)

        # Loop for all remaining rows
        for i = (k+1):rows

            # Step 3: finding fraction
            fraction = A[i,k]/A[k,k]

            # loop through all columns for that row
            for j = (k+1):cols

                 # Step 4: re-evaluate each element
                 A[i,j] -= A[k,j]*fraction

            end

            # Step 5: Set lower elements to 0
            A[i,k] = 0
        end
    end
end

function back_substitution(A::Array{Float64,2})

    rows = size(A,1)
    cols = size(A,2)

    # Creating the solution Vector
    soln = Vector{Float64}(rows)

    # initialize the final element
    soln[rows] = A[rows, cols] / A[rows, cols-1]

    for i = (rows - 1):-1:1
        sum = 0.0
        for j = rows:-1:i
            sum += soln[j]*A[i,j]
        end
        soln[i] = (A[i, cols] - sum) / A[i, i]
    end

    return soln
end

function main()
    A = [2. 3 4 6;
         1 2 3 4;
         3 -4 0 10]

    gaussian_elimination(A)
    println(A)
    soln = back_substitution(A)

    for element in soln
        println(element)
    end
end

main()

max_stars_repo_name: sanstorik/algorithm-archive

max_issues_repo_name: sanstorik/algorithm-archive

max_forks_repo_name: sanstorik/algorithm-archive


text:
# # Working with GBIF data

# **Justification for this use case:** building species distribution models
# requires information of where species are. In this document, we will see how
# the `SimpleSDMLayers` and `GBIF` packages interact, as a first step to get in
# a usable state. Specifically, we will work on the occurences of the
# relationship between temperature and precipitation for a few occurrences of
# the fungus *Hypomyces lactifluorum*, which will be the taxon used for all
# SDM-related vignettes.

using SimpleSDMLayers
using GBIF
using Plots
using Statistics

# We will focus on showing where on the temperature/precipitation space the
# occurrences are, so we will download these layers:

temperature, precipitation = SimpleSDMPredictor(WorldClim, BioClim, [1, 12])

# And now, we can follow the GBIF documentation to get some occurrences

observations = occurrences(
    GBIF.taxon("Hypomyces lactifluorum"; strict=true),
    "hasCoordinate" => "true",
    "country" => "CA",
    "country" => "US",
    "limit" => 300,
)
while length(observations) < size(observations)
    occurrences!(observations)
end

@info observations

# We can then extract the temperature for the first occurrence:

temperature[observations[1]]

# Of course, it would be unwieldy to do this for every occurrence in our dataset,
# and so we will see a way do it much faster. But first, we do not need the entire
# surface of the planet to perform our analysis, and so we will instead clip the
# layers:

temperature = clip(temperature, observations)
precipitation = clip(precipitation, observations)

# This will make the future queries a little faster. By default, the `clip`
# function will ad a 5% margin on every side. To get the values of a layer at
# every occurrence in a `GBIFRecord`, we simply pass the records as a position:

histogram2d(temperature, precipitation; c=:devon, frame=:zerolines, leg=false, lab="")
scatter!(
    temperature[observations],
    precipitation[observations];
    lab="",
    c=:white,
    msc=:orange,
    alpha=0.2,
)
xaxis!("Temperature (°C)")
yaxis!("Precipitation (mm)")

# This will return a record of all data for all geo-localized occurrences
# (*i.e.* neither the latitude nor the longitude is `missing`) in a
# `GBIFRecords` collection, as an array of the `eltype` of the layer. Note that
# the layer values can be `nothing`, in which case you might need to run
# `filter(!isnothing, temperature_clip[kf_occurrences]` for it to work with the
# plotting functions.

# We can also plot the records over space, using the overloads of the `latitudes`
# and `longitudes` functions:

contour(temperature; c=:cork, frame=:box, fill=true, clim=(-20, 20), levels=6)
scatter!(
    longitudes(observations), latitudes(observations); lab="", c=:white, msc=:orange, ms=2
)

# We can finally make a layer with the number of observations per cells:

presabs = mask(temperature, observations, Float32)
plot(log1p(presabs); c=:tokyo)

# Because the cells are rather small, and there are few observations, this is not
# necessarily going to be very informative - to get a better sense of the
# distribution of observations, we can get the average number of observations in a
# radius of 100km around each cell (we will do so for a zoomed-in part of the map
# to save time):

zoom = clip(presabs; left=-80.0, right=-65.0, top=50.0, bottom=40.0)
buffered = slidingwindow(zoom, Statistics.mean, 100.0)
plot(buffered; c=:tokyo, frame=:box)
scatter!(
    longitudes(observations),
    latitudes(observations);
    lab="",
    c=:white,
    msc=:orange,
    ms=2,
    alpha=0.5,
)


text:
module MyToolbox

using Reexport
export @reexport

@reexport using Pkg,
    Random,
    Printf,
    Logging,
    Profile,
    Base.Iterators,        # `partition`, `cycle`, `flatten` (= chain), …
    # ↑ stdlib,
    DataStructures,        # `DefaultDict`, `OrderedDict`, `counter`, queues, …
    StatsBase,             # `sample`, `describe`, …. Plus: reexports `Statistics` from stdlib
    PartialFunctions,      # Currying (`func $ a`, like `partial(func, a)` in Python)
    PyFormattedStrings,    # f-strings as in Python (but with C format spec)
    LaTeXStrings,          # `L"These strings can contain $ and \ without escaping"`
    FilePaths,             # `Path` type and `/` joins, as in Python
    Requires,              # `@require PkgName = "…" …` in `__init__`.
    IJulia,
    ProgressMeter,         # `@showprogress`
    ProfileView,           # `@profview`
    LoggingExtras,         # `TeeLogger`, `ActiveFilteredLogger`, …
    ComponentArrays,
    Parameters,            # `@unpack`, `@pack!`, `@with_kw` (> `Base.@kwdef`)
    JLD2                   # Saving Julia types to HDF5

@reexport using BenchmarkTools: @benchmark, @btime
    # This exports `params`, which conflicts with `Distributions` when that is loaded.

@reexport using Setfield: @set, @set!   # `@set! immut.some.field = new`.
    # `set` (the exported method, not the macro) clashes with Sciplotlib's.

include("macros.jl")
export @alias, @withfb

include("show.jl")
export dumps, showsome

include("cvec.jl")
export CVec, idvec

include("iter.jl")
export ziplongest, chain

include("misctools.jl")
export resetrng!, linspace

function __init__()

    # See included files below for additional imports and @reexports of MyToolbox, besides
    # those in this file.
    @require PyPlot = "d330b81b-6aea-500a-939a-2ce795aea3ee" begin
        include("figures.jl")
        export savefig
    end
    @require DataFrames = "a93c6f00-e57d-5684-b7b6-d8193f3e46c0" begin
        include("dataframes.jl")
        export printsimple
    end

    if isdefined(Main, :IJulia) && Main.IJulia.inited
        prettify_logging_in_IJulia()
    end
    # [a todo: eval `using FilePathsBase: /` in Main]
end

end # module


text:
module Gettext

using PyCall

const gt = PyNULL()

function __init__()
    copy!(gt, pyimport("gettext"))
end

bindtextdomain(domain, localedir=nothing) = gt[:bindtextdomain](domain, localedir)
textdomain(domain=nothing) = gt[:textdomain](domain)

gettext(message) = gt[:gettext](message)
dgettext(domain, message) = gt[:dgettext](domain, message)
ngettext(singular, plural, n) = gt[:ngettext](singular, plural, n)
dngettext(domain, singular, plural, n) = gt[:dngettext](domain, singular, plural, n)

macro __str(s)
    gettext(s)
end

macro N__str(s)
    s
end

export bindtextdomain, textdomain, gettext, lgettext, dgettext, dlgettext, ngettext, lngettext, dngettext, ldngettext, @__str, @N__str

end # module


text:
struct Fail
   message::String
end

function Fail(msg, origex)
   Fail(msg * "\nOriginal Julia error message:\n" *
         sprint(showerror, origex, backtrace()))
end

struct ElementList
   positionalelements::Vector{Any}
   names::Vector{Symbol}
   namedelements::Dict{Symbol, Any}
   attributes::Dict{String, Any}
   parsingfails::Vector{Fail}
end

function ElementList(unnamed::Vector{Any} = Vector{Any}(),
      names::Vector{Symbol} = Vector{Symbol}(),
      named::Dict{Symbol, Any} = Dict{Symbol, Any}(),
      attributes = Dict{String, Any}())
   ElementList(unnamed, names, named, attributes, Vector{Fail}())
end

function ElementList(unnamed::Vector{Any},
      named::Dict{Symbol, Any} = Dict{Symbol, Any}(),
      attributes = Dict{String, Any}())

   ElementList(unnamed, collect(keys(named)), named, attributes, Vector{Fail}())
end

struct Call
   fun::Union{Function, Type, Module}
   args::ElementList
   parsingfails::Vector{Fail}

   function Call(fun::Union{Function, Type} = () -> nothing,
      args::ElementList = ElementList(),
      parsingfails::Vector{Fail} = Vector{Fail}())

      new(fun, args, parsingfails)
   end
end

function isbitsequal(x)
   y -> x .=== y
end

# replace `value` in `x` with `replacement`
# but use === comparison instead of == as the normal replace does
function replace_bitsequal(x, val, replacement)
   [(x[i] === val ? replacement : x[i]) for i in eachindex(x)]
end

# represents_na shall be equivalent to the R_IsNA function.
# See https://github.com/wch/r-source/blob/502561df523f1dc0430dfe5862c6a174c7067b4f/src/main/arithmetic.c#L126-L137
# and https://cran.r-project.org/doc/manuals/r-devel/R-data.html#Special-values
# " [...] NA is represented by the NaN value with low-word 0x7a2 (1954 in decimal)."
const magicnumber1954shifted = (UInt64(1954) << 32)
function represents_na(x::Float64)
   isnan(x) && (reinterpret(UInt64, x) << 32) == magicnumber1954shifted
end

# Returns true if there is a value in the array x that is a R NA value,
# as is determined by the R_IsNA function.
# Replaces all "dirty" NA values (i.e., values that are considered NA by R,
# but are not bitwise equal to NA_real_ in R) with the exact value of NA_real_
function any_na_normalize!(x::AbstractVector{Float64})
   ret = false
   for i in eachindex(x)
      @inbounds if represents_na(x[i])
         x[i] = R_NA_REAL
         ret = true
      end
   end
   ret
end

function read_attributes(communicator)
   nattributes = read_nattributes(communicator)
   attributes = Dict{String, Any}()
   for i in 1:nattributes
      name = read_string(communicator)
      attributes[name] = read_element(communicator)
   end
   attributes
end

function read_complexs(communicator, n::Int)
   doublepairs = reinterpret(Float64, read_bin(communicator, 16*n))
   map(i -> Complex{Float64}(doublepairs[2*i - 1], doublepairs[2*i]), 1:n)
end

function read_complexs_with_missings(communicator, n::Int)
   doublepairs = reinterpret(Float64, read_bin(communicator, 16*n))
   any_nas_found = any_na_normalize!(doublepairs)
   ret = map(i -> Complex{Float64}(doublepairs[2*i - 1], doublepairs[2*i]), 1:n)
   if any_nas_found
      return replace_bitsequal(ret, R_NA_COMPLEX, missing)
   else
      return ret
   end
end

function read_float64s(communicator, n::Int)
   reinterpret(Float64, read_bin(communicator, 8*n))
end

function read_float64s_with_missings(communicator, n::Int)
   ret = read_float64s(communicator, n)
   if any_na_normalize!(ret)
      return replace_bitsequal(ret, R_NA_REAL, missing)
   else
      return ret
   end
end

function read_ints(communicator, n::Int)
   map(Int, reinterpret(Int32, read_bin(communicator, 4*n)))
end

read_int(communicator) = read_ints(communicator, 1)[1]

function read_ints_with_missings(communicator, n::Int)
   ret = read_ints(communicator, n)
   if any(isequal(R_NA_INTEGER), ret)
      return replace(ret, R_NA_INTEGER => missing)
   else
      return ret
   end
end

function read_nattributes(communicator)
   ret = read_bin(communicator, 1)[1]
end

function read_string(communicator)
   nbytes = read_int(communicator)
   if nbytes > 0
      return String(read_bin(communicator, nbytes))
   else
      return ""
   end
end

function read_strings(communicator, n::Int)
   ret = Vector{String}(undef, n)
   for i = 1:n
      ret[i] = read_string(communicator)
   end
   ret
end

function read_bools(communicator, n::Int)
   reinterpret(Bool, read_bin(communicator, n))
end

function read_dimensions(communicator)
   ndims = read_int(communicator)
   if ndims == 0
      return 0
   else
      return read_ints(communicator, ndims)
   end
end

function read_element(communicator)
   typeid = read_byte(communicator)

   if typeid == TYPE_ID_LIST
      return read_list(communicator)
   elseif typeid == TYPE_ID_NOTHING
      return nothing
   elseif typeid == TYPE_ID_EXPRESSION
      return read_expression(communicator)
   elseif typeid == TYPE_ID_OBJECT_REFERENCE
      # not needed in Julia, ignore for now
      object_class = read_bin(communicator, 1)
      ref = parseheapref(read_bin(communicator, 8))
      return ObjectReference(ref)
   elseif typeid == TYPE_ID_CALLBACK
      callbackid = read_string(communicator)
      callback = callbackfun(callbackid, communicator)
      return callback
   elseif typeid == TYPE_ID_SYMBOL
      return Symbol(read_string(communicator))
   else
      dimensions = read_dimensions(communicator)
      nelements = dimensions == 0 ? 1 : reduce(*, dimensions)

      attributes = NO_ATTRIBUTES
      if typeid == TYPE_ID_FLOAT64
         ret = read_float64s_with_missings(communicator, nelements)
         attributes = read_attributes(communicator)
         return convert_reshape_element(ret, attributes, dimensions)
      elseif typeid == TYPE_ID_INT
         ret = read_ints_with_missings(communicator, nelements)
         attributes = read_attributes(communicator)
         return convert_reshape_element(ret, attributes, dimensions)
      elseif typeid == TYPE_ID_BOOL
         ret = read_bools(communicator, nelements)
         return reshape_element(ret, dimensions)
      elseif typeid == TYPE_ID_STRING
         ret = read_strings(communicator, nelements)
         attributes = read_attributes(communicator)
         return convert_reshape_string(ret, attributes, dimensions)
      elseif typeid == TYPE_ID_COMPLEX
         ret = read_complexs_with_missings(communicator, nelements)
         attributes = read_attributes(communicator)
         return convert_reshape_element(ret, attributes, dimensions)
      elseif typeid == TYPE_ID_RAW
         ret = read_bin(communicator, nelements)
         attributes = read_attributes(communicator)
         return convert_reshape_raw(ret, attributes, dimensions)
      else
         return Fail("Invalid type id $typeid of element")
      end
   end
end

function reshape_element(element, dimensions)
   if dimensions == 0
      return element[1]
   else # dimensions > 0
      return Array(reshape(element, dimensions...))
   end
end

function convert_reshape_element(element, attributes, dimensions)
   typestr = get(attributes, "JLTYPE", "")
   if typestr != ""
      try
         type = maineval(typestr)
         element = convert.(type, element)
      catch ex
         return Fail("Conversion to type \"$typestr\" failed", ex)
      end
   end
   return reshape_element(element, dimensions)
end

function convert_reshape_raw(element, attributes, dimensions)
   typestr = get(attributes, "JLTYPE", "")
   if typestr != ""
      try
         type = maineval(typestr)
         if type <: SEND_AS_RAW_TYPES
            element = reinterpret(type, element)
            # adjust dimensions, as the original dimensions are for a UInt8 array
            if length(element) == 1 && isempty(get(attributes, "JLDIM", Int[]))
               dimensions = 0
            else
               dimensions[1] /= sizeof(type)
            end
            element = convert.(type, element)
         elseif type <: String
            # It may be the case that a Julia String can not be handled in R,
            # e.g. if it contains NUL characters. Then it will be encoded in bytes.
            return String(element)
         end
      catch ex
         return Fail("Conversion to type \"$typestr\" failed", ex)
      end
   end
   return reshape_element(element, dimensions)
end

function convert_reshape_string(ret::Array{String}, attributes, dimensions)
   if haskey(attributes, "NA")
      ret = convert(Array{Union{String, Missing}}, ret)
      try
         for i in attributes["NA"]
            ret[i] = missing
         end
      catch ex
         return Fail("Translating NAs failed", ex)
      end
   end
   return convert_reshape_element(ret, attributes, dimensions)
end

function read_expression(communicator)
   exprstr = read_string(communicator)
   try
      return maineval(exprstr)
   catch ex
      return Fail("Evaluation of \"$exprstr\" failed", ex)
   end
end

function read_list(communicator)

   npositional = read_int(communicator)
   positionalelements = Vector{Any}(undef, npositional)
   for i in 1:npositional
      positionalelements[i] = read_element(communicator)
   end

   fails = Vector{Fail}(
         positionalelements[isa.(positionalelements, Fail)])

   nnamed = read_int(communicator)
   names = Vector{Symbol}(undef, nnamed)
   namedelements = Dict{Symbol, Any}()
   for i in 1:nnamed
      name = read_string(communicator)
      namedelement = read_element(communicator)
      try
         sym = Symbol(name)
         namedelements[sym] = namedelement
         names[i] = sym
      catch ex
         push!(fails, Fail("Ignored element with name $name", ex))
      end
   end

   attributes = read_attributes(communicator)

   ElementList(positionalelements, names, namedelements, attributes, fails)
end

function findfield(name::AbstractString)
   # The use of eval here brings a potential security risk if one wants
   # to restrict the functions that can be called somehow
   maineval(name)::Union{Function, Type, Module}
end

# return a function that is the broadcasted version of f,
# corresponding to adding a dot in the call, as
# f.(args...) is actually equivalent to broadcast(f, args...)
# (see Julia documentation of dot syntax for vectorizing functions)
function	broadcasted(f::Function)
   (args...; kwargs...) -> broadcast((x) -> f(x; kwargs...), args...)
end

function broadcasted(call::Call)
   Call(broadcasted(call.fun), call.args, call.parsingfails)
end

function read_call(communicator)
   name = read_string(communicator)
   if endswith(name, ".")
      funname = name[1:(end-1)]
      return broadcasted(read_call(communicator, funname))
   else
      return read_call(communicator, name)
   end
end

function read_call(communicator, name::AbstractString)
   fails = Vector{Fail}()
   fun = () -> nothing
   try
      fun::Union{Function, Type} = findfield(name)
   catch ex
      push!(fails, Fail("Unable to identify function", ex))
   end
   args = read_list(communicator)
   Call(fun, args, fails)
end


text:
include("ewald.jl")

using ForwardDiff
using ReverseDiff
using FiniteDiff

using LinearAlgebra
using Test
using BenchmarkTools

#= helpers =#

function compute_forces_manual(positions, lattice, charges)
    forces_manual = zeros(Vec3{Float64}, length(positions))
    γ1 = energy_ewald(lattice, charges, positions, forces=forces_manual)
    forces_manual = reduce(hcat, forces_manual)
    return forces_manual
end

function compute_forces_autodiff(positions, lattice, charges; autodiff_backend=:ForwardDiff)
    _positions = reduce(hcat, positions)
    if autodiff_backend == :FiniteDiff
        gradient = FiniteDiff.finite_difference_gradient
    else
        gradient = eval(autodiff_backend).gradient
    end
    forces = gradient(_positions -> begin
            positions = collect(eachcol(_positions))
            -energy_ewald(lattice, charges, positions)
        end,
        _positions
    )
    return forces
end

#==========#
# Forces   #
#==========#

@testset "Forces" begin
    lattice = [0.0  5.131570667152971 5.131570667152971;
               5.131570667152971 0.0 5.131570667152971;
               5.131570667152971 5.131570667152971  0.0]
    # perturb positions away from equilibrium to get nonzero force
    positions = [ones(3)/8+rand(3)/20, -ones(3)/8]
    charges = [14, 14]

    forces = zeros(Vec3{Float64}, 2)
    γ1 = energy_ewald(lattice, charges, positions, forces=forces)

    # Compare forces to finite differences
    disp = [rand(3)/20, rand(3)/20]
    ε = 1e-8
    γ2 = energy_ewald(lattice, charges, positions .+ ε .* disp)
    @test (γ2-γ1)/ε ≈ -dot(disp, forces) atol=abs(γ1*1e-6)
end

begin
    lattice = [0.0  5.131570667152971 5.131570667152971;
               5.131570667152971 0.0 5.131570667152971;
               5.131570667152971 5.131570667152971  0.0]
    # perturb positions away from equilibrium to get nonzero force
    positions = [ones(3)/8+rand(3)/20, -ones(3)/8]
    charges = [14, 14]

    # compute forces manually
    forces_manual = compute_forces_manual(positions, lattice, charges)

    # compute forces via ForwardDiff
    forces_forwarddiff = compute_forces_autodiff(positions, lattice, charges; autodiff_backend=:ForwardDiff)

    display(forces_manual)
    display(forces_forwarddiff)

    @test forces_manual ≈ forces_forwarddiff atol=1e-6
end

# Ewald needed for ForwardDiff: No annotation of eltype T to allow overloading.

@time compute_forces_manual(positions, lattice, charges)
@time compute_forces_autodiff(positions, lattice, charges; autodiff_backend=:FiniteDiff)
@time compute_forces_autodiff(positions, lattice, charges; autodiff_backend=:ForwardDiff)
@time compute_forces_autodiff(positions, lattice, charges; autodiff_backend=:ReverseDiff)
# compute_forces_autodiff(positions, lattice, charges; autodiff_backend=:Zygote) # Error

#=====================#
# Compile ReverseDiff # https://github.com/JuliaDiff/ReverseDiff.jl/blob/master/examples/gradient.jl
#=====================#
# While not strictly required, ReverseDiff recommends
# compiling an explicit tape that pre-records operations

@time let
    _positions = reduce(hcat, positions)
    compiled_energy_tape = ReverseDiff.compile(
        ReverseDiff.GradientTape(
            _positions -> begin
                positions = collect(eachcol(_positions))
                -energy_ewald(lattice, charges, positions)
            end,
            rand(3,2)
        )
    )
    res = similar(_positions)
    ReverseDiff.gradient!(res, compiled_energy_tape, _positions)
end

#=====================================#
# Forward vs Reverse on larger system # TODO: ask if system is still physically meaningful
#=====================================#

function benchmark_ewald(num_atoms=20)
    lattice = [0.0  5.131570667152971 5.131570667152971;
    5.131570667152971 0.0 5.131570667152971;
    5.131570667152971 5.131570667152971  0.0]
    # perturb positions away from equilibrium to get nonzero force
    positions = [ones(3)/8+rand(3)/20 for _ in 1:num_atoms]
    charges = [14 for _ in 1:num_atoms]

    # advanced setup for ReverseDiff
    compiled_energy_tape = ReverseDiff.compile(
        ReverseDiff.GradientTape(
            _positions -> begin
                pos = collect(eachcol(_positions))
                -energy_ewald(lattice, charges, pos)
            end,
            rand(3, num_atoms)
        )
    )

    forces_manual = compute_forces_manual(positions, lattice, charges)
    forces_forwarddiff = compute_forces_autodiff(positions, lattice, charges; autodiff_backend=:ForwardDiff)

    _positions = reduce(hcat, positions)
    forces_reversediff = similar(_positions)
    ReverseDiff.gradient!(forces_reversediff, compiled_energy_tape, _positions)

    # check consistency of forces
    @test forces_manual ≈ forces_forwarddiff atol=1e-6
    @test forces_manual ≈ forces_reversediff atol=1e-6

    println("Timings:")
    println("Energy without forces")
    @btime energy_ewald($lattice, $charges, $positions)
    println("Manual forces")
    @btime compute_forces_manual($positions, $lattice, $charges)
    println("FiniteDiff forces")
    @btime compute_forces_autodiff($positions, $lattice, $charges; autodiff_backend=:FiniteDiff)
    println("ForwardDiff forces")
    @btime compute_forces_autodiff($positions, $lattice, $charges; autodiff_backend=:ForwardDiff)
    println("ReverseDiff forces")
    @btime compute_forces_autodiff($positions, $lattice, $charges; autodiff_backend=:ReverseDiff)
    println("ReverseDiff forces (compiled tape)")
    @btime ReverseDiff.gradient!($forces_reversediff, $compiled_energy_tape, $_positions)

    return nothing
end

benchmark_ewald(2)
    # Timings:
    # Energy without forces
    #   2.505 ms (75160 allocations: 3.69 MiB)
    # Manual forces
    #   9.217 ms (145079 allocations: 8.21 MiB)
    # FiniteDiff forces
    #   33.084 ms (901977 allocations: 44.26 MiB)
    # ForwardDiff forces
    #   3.780 ms (75174 allocations: 9.40 MiB)
    # ReverseDiff forces
    #   167.857 ms (3525296 allocations: 133.12 MiB)
    # ReverseDiff forces (compiled tape)
    #   65.120 ms (0 allocations: 0 bytes)

# benchmark_ewald(10) # ReverseDiff seems to crash Julia here (due to loop unrolling maybe? TODO)

benchmark_ewald(4)
    # Timings:
    # Energy without forces
    #   10.412 ms (280481 allocations: 14.24 MiB)
    # Manual forces
    #   41.158 ms (548468 allocations: 31.73 MiB)
    # FiniteDiff forces
    #   392.245 ms (6731643 allocations: 341.68 MiB)
    # ForwardDiff forces
    #   36.874 ms (280495 allocations: 59.24 MiB)
    # ReverseDiff forces
    #   758.308 ms (14019343 allocations: 526.59 MiB)
    # ReverseDiff forces (compiled tape)
    #   238.758 ms (0 allocations: 0 bytes)

#==========#
# Stresses # probably can be sped up further, but they work :)
#==========#

@time energy_ewald(lattice, charges, positions)
@time ForwardDiff.gradient(lattice -> energy_ewald(lattice, charges, positions), lattice)
@time FiniteDiff.finite_difference_gradient(lattice -> energy_ewald(lattice, charges, positions), lattice)

#==========#
# Misc     #
#==========#

# TODO
# 1. Remove ForwardDiff from manual forces for better speed comparison
# 2. Try rewrite for more efficient reverse mode (ReverseDiff & Zygote)
#      - Use Distances.jl and its ChainRules ?
#      - Can we do something against the unrolling while-loop?
# x. (periodically check AbstractDifferentiation.jl)

max_stars_repo_name: niklasschmitz/DifferentiableDFTK

max_issues_repo_name: niklasschmitz/DifferentiableDFTK

max_forks_repo_name: niklasschmitz/DifferentiableDFTK


text:
function setup!(ws::COSMO.Workspace)
	# scale problem data
	if ws.settings.scaling != 0
		scale_ruiz!(ws)
	end

	set_rho_vec!(ws)

	# factor the KKT condition matrix
	ws.flags.FACTOR_LHS && factor_KKT!(ws)
end


text:
using Makie, Observables, StaticArrays, AbstractPlotting

include("simulation.jl")
include("plotting.jl")
include("util.jl")

r(θ) = (sin(θ), -cos(θ))
p_θ₀(γ) = sqrt(2*9.8*γ)
energy(θ, p_θ) = 0.5*p_θ^2 - 9.8*cos(θ)

θ = Node(0.0)
p_θ = Node(0.0)
E = Node(0.0)

γ = Node(0.0)
on(γ) do val
    θ[] = 0.0
    p_θ[] = p_θ₀(val)
    E[] = 9.8(val - 1)
end

γ[] = 0.95

scene = instantiate_plot(θ, p_θ, E, γ);
update_cam!(scene.children[1], FRect3D(Vec3f0(0), Vec3f0(1)))
@async sim!(θ, p_θ, scene=scene)

max_stars_repo_name: tlqmj/phasespace

max_issues_repo_name: tlqmj/phasespace

max_forks_repo_name: tlqmj/phasespace


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

using Test
using ParallelProcessingTools

using Base.Threads

@testset "onthreads" begin
    if nthreads() < 2
        @warn "JULIA multithreading not enabled"
    end

    function do_work(n)
        if n < 0
            throw(ArgumentError("n must be >= 0"))
        end
        s::Float64 = 0
        for i in 1:n
            if n % 1000 == 0
                yield()
            end
            s += log(abs(asin(sin(Complex(log(i), log(i))))) + 1)
        end
        s
    end

    @testset "macro onthreads" begin
        @test (begin
            tl = ThreadLocal(0)
            @onthreads allthreads() tl[] = threadid()
            getallvalues(tl)
        end) == 1:nthreads()
    end

    @testset "macro mt_async" begin
        @test begin
            n = 128
            A = zeros(n)
            @sync for i in eachindex(A)
                @mt_async begin
                    do_work(10^3)
                    A[i] = log(i)
                end
            end
            A == log.(1:n)
        end
    end

    @testset "Examples" begin
        @testset "Example 1" begin
            tlsum = ThreadLocal(0.0)
            data = rand(100)
            @onthreads allthreads() begin
                tlsum[] = sum(workpart(data, allthreads(), Base.Threads.threadid()))
            end
            @test sum(getallvalues(tlsum)) ≈ sum(data)
        end

        if nthreads() >= 4
            @testset "Example 2" begin
                # Assuming 4 threads:
                tl = ThreadLocal(42)
                threadsel = 2:3
                @onthreads threadsel begin
                    tl[] = Base.Threads.threadid()
                end
                @test getallvalues(tl)[threadsel] == [2, 3]
                @test getallvalues(tl)[[1,4]] == [42, 42]
            end
        end
    end
end


text:
# Formulation K2: (if regul==:classic, adds additional regularization parmeters -ρ (top left) and δ (bottom right))
# [-Q - D     A' ] [x] = rhs
# [ A         0  ] [y]
export K2LDLParams

"""
Type to use the K2 formulation with a LDLᵀ factorization, using the package
[`LDLFactorizations.jl`](https://github.com/JuliaSmoothOptimizers/LDLFactorizations.jl).
The outer constructor

    sp = K2LDLParams(; regul = :classic, ρ0 = sqrt(eps()) * 1e5, δ0 = sqrt(eps()) * 1e5)

creates a [`RipQP.SolverParams`](@ref) that should be used to create a [`RipQP.InputConfig`](@ref).
`regul = :dynamic` uses a dynamic regularization (the regularization is only added if the LDLᵀ factorization
encounters a pivot that has a small magnitude).
`regul = :none` uses no regularization (not recommended).
When `regul = :classic`, the parameters `ρ0` and `δ0` are used to choose the initial regularization values.
"""
mutable struct K2LDLParams <: AugmentedParams
  uplo::Symbol
  regul::Symbol
  ρ0::Float64
  δ0::Float64
end

function K2LDLParams(;
  regul::Symbol = :classic,
  ρ0::Float64 = sqrt(eps()) * 1e5,
  δ0::Float64 = sqrt(eps()) * 1e5,
)
  regul == :classic ||
    regul == :dynamic ||
    regul == :none ||
    error("regul should be :classic or :dynamic or :none")
  uplo = :U # mandatory for LDLFactorizations
  return K2LDLParams(uplo, regul, ρ0, δ0)
end

mutable struct PreallocatedDataK2LDL{T <: Real, S} <: PreallocatedDataAugmentedLDL{T, S}
  D::S # temporary top-left diagonal
  regu::Regularization{T}
  diag_Q::SparseVector{T, Int} # Q diagonal
  K::SparseMatrixCSC{T, Int} # augmented matrix
  K_fact::LDLFactorizations.LDLFactorization{T, Int, Int, Int} # factorized matrix
  fact_fail::Bool # true if factorization failed
  diagind_K::Vector{Int} # diagonal indices of J
end

# outer constructor
function PreallocatedData(
  sp::K2LDLParams,
  fd::QM_FloatData{T},
  id::QM_IntData,
  itd::IterData{T},
  pt::Point{T},
  iconf::InputConfig{Tconf},
) where {T <: Real, Tconf <: Real}

  # init Regularization values
  D = similar(fd.c, id.nvar)
  if iconf.mode == :mono && T == Float64
    regu = Regularization(T(sp.ρ0), T(sp.δ0), 1e-5 * sqrt(eps(T)), 1e0 * sqrt(eps(T)), sp.regul)
    D .= -T(1.0e0) / 2
  else
    regu =
      Regularization(T(sp.ρ0), T(sp.δ0), T(sqrt(eps(T)) * 1e0), T(sqrt(eps(T)) * 1e0), sp.regul)
    D .= -T(1.0e-2)
  end
  diag_Q = get_diag_Q(fd.Q.data.colptr, fd.Q.data.rowval, fd.Q.data.nzval, id.nvar)
  K = create_K2(id, D, fd.Q.data, fd.A, diag_Q, regu)

  diagind_K = get_diag_sparseCSC(K.colptr, id.ncon + id.nvar)
  K_fact = ldl_analyze(Symmetric(K, :U))
  if regu.regul == :dynamic
    Amax = @views norm(K.nzval[diagind_K], Inf)
    regu.ρ, regu.δ = -T(eps(T)^(3 / 4)), T(eps(T)^(0.45))
    K_fact.r1, K_fact.r2 = regu.ρ, regu.δ
    K_fact.tol = Amax * T(eps(T))
    K_fact.n_d = id.nvar
  elseif regu.regul == :none
    regu.ρ, regu.δ = zero(T), zero(T)
  end
  K_fact = ldl_factorize!(Symmetric(K, :U), K_fact)
  K_fact.__factorized = true

  return PreallocatedDataK2LDL(
    D,
    regu,
    diag_Q, #diag_Q
    K, #K
    K_fact, #K_fact
    false,
    diagind_K, #diagind_K
  )
end

function convertpad(
  ::Type{<:PreallocatedData{T}},
  pad::PreallocatedDataK2LDL{T_old},
  T0::DataType,
) where {T <: Real, T_old <: Real}
  pad = PreallocatedDataK2LDL(
    convert(Array{T}, pad.D),
    convert(Regularization{T}, pad.regu),
    convert(SparseVector{T, Int}, pad.diag_Q),
    convert(SparseMatrixCSC{T, Int}, pad.K),
    convertldl(T, pad.K_fact),
    pad.fact_fail,
    pad.diagind_K,
  )

  if pad.regu.regul == :classic
    if T == Float64 && T0 == Float64
      pad.regu.ρ_min, pad.regu.δ_min = T(sqrt(eps()) * 1e-5), T(sqrt(eps()) * 1e0)
    else
      pad.regu.ρ_min, pad.regu.δ_min = T(sqrt(eps(T)) * 1e1), T(sqrt(eps(T)) * 1e1)
    end
  elseif pad.regu.regul == :dynamic
    pad.regu.ρ, pad.regu.δ = -T(eps(T)^(3 / 4)), T(eps(T)^(0.45))
    pad.K_fact.r1, pad.K_fact.r2 = pad.regu.ρ, pad.regu.δ
  end

  return pad
end

# function used to solve problems
# solver LDLFactorization
function solver!(
  dd::AbstractVector{T},
  pad::PreallocatedDataK2LDL{T},
  dda::DescentDirectionAllocs{T},
  pt::Point{T},
  itd::IterData{T},
  fd::Abstract_QM_FloatData{T},
  id::QM_IntData,
  res::AbstractResiduals{T},
  cnts::Counters,
  T0::DataType,
  step::Symbol,
) where {T <: Real}
  ldiv!(pad.K_fact, dd)
  return 0
end

function update_pad!(
  pad::PreallocatedDataK2LDL{T},
  dda::DescentDirectionAllocs{T},
  pt::Point{T},
  itd::IterData{T},
  fd::Abstract_QM_FloatData{T},
  id::QM_IntData,
  res::AbstractResiduals{T},
  cnts::Counters,
  T0::DataType,
) where {T <: Real}
  if pad.regu.regul == :classic && cnts.k != 0 # update ρ and δ values, check K diag magnitude
    out = update_regu_diagK2!(
      pad.regu,
      pad.K.nzval,
      pad.diagind_K,
      id.nvar,
      itd.pdd,
      itd.l_pdd,
      itd.mean_pdd,
      cnts,
      T,
      T0,
    )
    out == 1 && return out
  end

  out = factorize_K2!(
    pad.K,
    pad.K_fact,
    pad.D,
    pad.diag_Q,
    pad.diagind_K,
    pad.regu,
    pt.s_l,
    pt.s_u,
    itd.x_m_lvar,
    itd.uvar_m_x,
    id.ilow,
    id.iupp,
    id.ncon,
    id.nvar,
    cnts,
    itd.qp,
    T,
    T0,
  ) # update D and factorize K

  if out == 1
    pad.fact_fail = true
    return out
  end

  return 0
end

# Init functions for the K2 system
function fill_K2!(
  K_colptr,
  K_rowval,
  K_nzval,
  D,
  Q_colptr,
  Q_rowval,
  Q_nzval,
  A_colptr,
  A_rowval,
  A_nzval,
  diag_Q_nzind,
  δ,
  ncon,
  nvar,
  regul,
)
  added_coeffs_diag = 0 # we add coefficients that do not appear in Q in position i,i if Q[i,i] = 0
  n_nz = length(diag_Q_nzind)
  c_nz = n_nz > 0 ? 1 : 0
  @inbounds for j = 1:nvar  # Q coeffs, tmp diag coefs.
    K_colptr[j + 1] = Q_colptr[j + 1] + added_coeffs_diag
    for k = Q_colptr[j]:(Q_colptr[j + 1] - 2)
      nz_idx = k + added_coeffs_diag
      K_rowval[nz_idx] = Q_rowval[k]
      K_nzval[nz_idx] = -Q_nzval[k]
    end
    if c_nz == 0 || c_nz > n_nz || diag_Q_nzind[c_nz] != j
      added_coeffs_diag += 1
      K_colptr[j + 1] += 1
      nz_idx = K_colptr[j + 1] - 1
      K_rowval[nz_idx] = j
      K_nzval[nz_idx] = D[j]
    else
      if c_nz != 0
        c_nz += 1
      end
      nz_idx = K_colptr[j + 1] - 1
      K_rowval[nz_idx] = j
      K_nzval[nz_idx] = D[j] - Q_nzval[Q_colptr[j + 1] - 1]
    end
  end

  countsum = K_colptr[nvar + 1] # current value of K_colptr[Q.n+j+1]
  nnz_top_left = countsum # number of coefficients + 1 already added
  @inbounds for j = 1:ncon
    countsum += A_colptr[j + 1] - A_colptr[j]
    if regul == :classic && δ > 0
      countsum += 1
    end
    K_colptr[nvar + j + 1] = countsum
    for k = A_colptr[j]:(A_colptr[j + 1] - 1)
      nz_idx = (regul == :classic && δ > 0) ? k + nnz_top_left + j - 2 : k + nnz_top_left - 1
      K_rowval[nz_idx] = A_rowval[k]
      K_nzval[nz_idx] = A_nzval[k]
    end
    if regul == :classic && δ > 0
      nz_idx = K_colptr[nvar + j + 1] - 1
      K_rowval[nz_idx] = nvar + j
      K_nzval[nz_idx] = δ
    end
  end
end

function create_K2(id, D, Q, A, diag_Q, regu)
  # for classic regul only
  n_nz = length(D) - length(diag_Q.nzind) + length(A.nzval) + length(Q.nzval)
  T = eltype(D)
  if regu.regul == :classic && regu.δ > zero(T)
    n_nz += id.ncon
  end
  K_colptr = Vector{Int}(undef, id.ncon + id.nvar + 1)
  K_colptr[1] = 1
  K_rowval = Vector{Int}(undef, n_nz)
  K_nzval = Vector{T}(undef, n_nz)
  # [-Q -D    A]
  # [0       δI]

  fill_K2!(
    K_colptr,
    K_rowval,
    K_nzval,
    D,
    Q.colptr,
    Q.rowval,
    Q.nzval,
    A.colptr,
    A.rowval,
    A.nzval,
    diag_Q.nzind,
    regu.δ,
    id.ncon,
    id.nvar,
    regu.regul,
  )

  return SparseMatrixCSC(id.ncon + id.nvar, id.ncon + id.nvar, K_colptr, K_rowval, K_nzval)
end

# iteration functions for the K2 system
function factorize_K2!(
  K,
  K_fact,
  D,
  diag_Q,
  diagind_K,
  regu,
  s_l,
  s_u,
  x_m_lvar,
  uvar_m_x,
  ilow,
  iupp,
  ncon,
  nvar,
  cnts,
  qp,
  T,
  T0,
)
  if regu.regul == :classic
    D .= -regu.ρ
    if regu.δ > zero(T)
      K.nzval[view(diagind_K, (nvar + 1):(ncon + nvar))] .= regu.δ
    end
  else
    D .= zero(T)
  end
  D[ilow] .-= s_l ./ x_m_lvar
  D[iupp] .-= s_u ./ uvar_m_x
  D[diag_Q.nzind] .-= diag_Q.nzval
  K.nzval[view(diagind_K, 1:nvar)] = D

  if regu.regul == :dynamic
    Amax = @views norm(K.nzval[diagind_K], Inf)
    if Amax > T(1e6) / K_fact.r2 && cnts.c_pdd < 8
      if T == Float32
        return one(Int) # update to Float64
      elseif qp || cnts.c_pdd < 4
        cnts.c_pdd += 1
        regu.δ /= 10
        K_fact.r2 = regu.δ
      end
    end
    K_fact.tol = Amax * T(eps(T))
    ldl_factorize!(Symmetric(K, :U), K_fact)

  elseif regu.regul == :classic
    ldl_factorize!(Symmetric(K, :U), K_fact)
    while !factorized(K_fact)
      out = update_regu_trycatch!(regu, cnts, T, T0)
      out == 1 && return out
      cnts.c_catch += 1
      cnts.c_catch >= 4 && return 1
      D .= -regu.ρ
      D[ilow] .-= s_l ./ x_m_lvar
      D[iupp] .-= s_u ./ uvar_m_x
      D[diag_Q.nzind] .-= diag_Q.nzval
      K.nzval[view(diagind_K, 1:nvar)] = D
      if regu.δ > zero(T)
        K.nzval[view(diagind_K, (nvar + 1):(ncon + nvar))] .= regu.δ
      end
      ldl_factorize!(Symmetric(K, :U), K_fact)
    end

  else # no Regularization
    ldl_factorize!(Symmetric(K, :U), K_fact)
  end

  return 0 # factorization succeeded
end


text:
module EquationOfStateWorkflow

using Crystallography: cellvolume
using Express.EquationOfStateWorkflow: Scf, VcOptim
using QuantumESPRESSO.Inputs.PWscf: CellParametersCard
using QuantumESPRESSO.Outputs.PWscf:
    Preamble, parse_electrons_energies, parsefinal, isjobdone, tryparsefinal
using Unitful: @u_str
using UnitfulAtomic

import Express.EquationOfStateWorkflow: parseoutput

include("Config.jl")
include("actions.jl")

function parseoutput(::Scf)
    function (file)
        str = read(file, String)
        preamble = tryparse(Preamble, str)
        e = try
            parse_electrons_energies(str, :converged)
        catch
        end
        if preamble !== nothing && !isempty(e)
            return preamble.omega * u"bohr^3" => e.ε[end] * u"Ry"  # volume, energy
        else
            return
        end
    end
end
function parseoutput(::VcOptim)
    function (file)
        str = read(file, String)
        if !isjobdone(str)
            @warn "Job is not finished!"
        end
        x = tryparsefinal(CellParametersCard, str)
        if x !== nothing
            return cellvolume(parsefinal(CellParametersCard, str)) * u"bohr^3" =>
                parse_electrons_energies(str, :converged).ε[end] * u"Ry"  # volume, energy
        else
            return
        end
    end
end

end