text:
using JLD2, DataFrames
#, Distributions, Serialization, Dates, Statistics, DataFrames
import GEAS:breeding, sim_QTL, summarize
function t_2021_08_06()
    @load "dat/run/base.jld2" base
    nqtl = 100
    par = begin
        Parameters = Dict(
            :nSire=> 100,
            :nDam => 200,   # male:female = 1:2
            :nSib => 40,
            :nC7e => 30,    # number of sib to be challenged
            :nG8n => 10,    # number of generations
            :nQTL => [nqtl, nqtl],
            :h²   => [.5, .5],
            :p8e  => .5,    # percentage of death after challenge
            :e19e => 1.,    # edit_successsful_rate
            :w4t  => 1,     # weight on the binary trait EBV
            :nK3n => 10,    # number of known QTL on binary trait
            :dd   => 0.01,  # A small value to be added to diagonals
            #:log  => slog
        )
        (; Parameters...)       # named tuple.  contents as above
    end
    #=
    Below is to test selection on phenotypes. To achieve this, insert
    prd[(g8n=ig,)][:, :val] = prd[(g8n=ig,)][:, :p7e]
    below calc_idx in function breeding
    =#
    nrpt = 20
    rst = DataFrame()
    for i in 1:nrpt
        @info "====== Repeat $i of $nrpt ======"
        qtl = sim_QTL(base, nqtl, nqtl)
        ped, snp = breeding(base, par, qtl, edit=true)
        df = summarize(ped)
        df.rpt = ones(Int32, nrow(df)) .* i
        append!(rst, df)
    end
    rst
end


text:
# Basic surface operators
using LinearAlgebra

"""
    regularize!(s::Nodes{Primal},f::ScalarData,cache::BasicILMCache)
    regularize!(s::Nodes{Primal},f::ScalarData,sys::ILMSystem)

The operation ``s = R_c f``, which regularizes scalar surface data `f`
onto the grid in the form of scalar grid data `s`. This is the adjoint
to [`interpolate!`](@ref)
"""
@inline regularize!(s::Nodes{Primal},f::ScalarData,cache::BasicILMCache) = regularize!(s,f,cache.R)

"""
    regularize!(s::Nodes{Dual},f::ScalarData,cache::BasicILMCache)
    regularize!(s::Nodes{Dual},f::ScalarData,sys::ILMSystem)

The operation ``s = R_N f``, which regularizes scalar surface data `f`
onto the grid in the form of scalar grid (curl) data `s`. Only works if
the cache is built for vector data. This is the adjoint
to [`interpolate!`](@ref)
"""
@inline regularize!(s::Nodes{Dual},f::ScalarData,cache::BasicILMCache) = regularize!(s,f,cache.Rcurl)

"""
    regularize!(v::Edges{Primal},vb::VectorData,cache::BasicILMCache)
    regularize!(v::Edges{Primal},vb::VectorData,sys::ILMSystem)

The operation ``\\mathbf{v} = R_f \\mathbf{v}_b``, which regularizes vector surface data `vb`
onto the grid in the form of scalar grid data `v`. This is the adjoint
to [`interpolate!`](@ref)
"""
@inline regularize!(v::Edges{Primal},vb::VectorData,cache::BasicILMCache) = regularize!(v,vb,cache.R)

function regularize!(s::Nodes{C},f::ScalarData,Rc::RegularizationMatrix) where C
  fill!(s,0.0)
  mul!(s,Rc,f)
end

function regularize!(v::Edges{C},vb::VectorData,Rf::RegularizationMatrix) where C
  fill!(v,0.0)
  mul!(v,Rf,vb)
end

"""
    interpolate!(f::ScalarData,s::Nodes{Primal},cache::BasicILMCache)
    interpolate!(f::ScalarData,s::Nodes{Primal},sys::ILMSystem)

The operation ``f = R_c^T s``, which interpolates scalar grid data `s`
onto the surface points in the form of scalar point data `f`. This is the adjoint
to [`regularize!`](@ref)
"""
@inline interpolate!(f::ScalarData,s::Nodes{Primal},cache::BasicILMCache) = interpolate!(f,s,cache.E)

"""
    interpolate!(f::ScalarData,s::Nodes{Dual},cache::BasicILMCache)
    interpolate!(f::ScalarData,s::Nodes{Dual},sys::ILMSystem)

The operation ``f = R_N^T s``, which interpolates scalar grid (curl) data `s`
onto the surface points in the form of scalar point data `f`. Only works if the
cache is built for vector data. This is the adjoint
to [`regularize!`](@ref)
"""
@inline interpolate!(f::ScalarData,s::Nodes{Dual},cache::BasicILMCache) = interpolate!(f,s,cache.Ecurl)

"""
    interpolate!(vb::VectorData,v::Edges{Primal},cache::BasicILMCache)
    interpolate!(vb::VectorData,v::Edges{Primal},sys::ILMSystem)

The operation ``\\mathbf{v}_b = R_c^T \\mathbf{v}``, which interpolates vector grid data `v`
onto the surface points in the form of scalar point data `vb`. This is the adjoint
to [`regularize!`](@ref)
"""
@inline interpolate!(vb::VectorData,v::Edges{Primal},cache::BasicILMCache) = interpolate!(vb,v,cache.E)

function interpolate!(f::ScalarData,s::Nodes{C},Ec::InterpolationMatrix) where C
  fill!(f,0.0)
  mul!(f,Ec,s)
end

function interpolate!(vb::VectorData,v::Edges{C},Ef::InterpolationMatrix) where C
  fill!(vb,0.0)
  mul!(vb,Ef,v)
end

"""
    regularize_normal!(v::Edges{Primal},f::ScalarData,cache::BasicILMCache)
    regularize_normal!(v::Edges{Primal},f::ScalarData,sys::ILMSystem)

The operation ``\\mathbf{v} = R_f \\mathbf{n}\\circ f``, which maps scalar surface data `f` (like
a jump in scalar potential) to grid data `v` (like velocity). This is the adjoint
to [`normal_interpolate!`](@ref).
"""
@inline regularize_normal!(q::Edges{Primal},f::ScalarData,cache::BasicILMCache) = regularize_normal!(q,f,cache.nrm,cache.Rsn,cache.snorm_cache)

function regularize_normal!(q::Edges{Primal,NX,NY},f::ScalarData{N},nrm::VectorData{N},Rf::RegularizationMatrix,snorm_cache::VectorData{N}) where {NX,NY,N}
    product!(snorm_cache,nrm,f)
    mul!(q,Rf,snorm_cache)
end

"""
    regularize_normal!(qt::EdgeGradient{Primal},v::VectorData,cache::BasicILMCache)
    regularize_normal!(qt::EdgeGradient{Primal},v::VectorData,sys::ILMSystem)

The operation ``\\mathbf{q}_t = R_t \\mathbf{n}\\circ \\mathbf{v}``, which maps scalar vector data `v` (like
a jump in velocity) to grid data `qt` (like velocity-normal tensor). This is the adjoint
to [`normal_interpolate!`](@ref).
"""
@inline regularize_normal!(q::EdgeGradient{Primal},f::VectorData,cache::BasicILMCache) = regularize_normal!(q,f,cache.nrm,cache.Rsn,cache.snorm_cache)

"""
    regularize_normal_symm!(qt::EdgeGradient{Primal},v::VectorData,cache::BasicILMCache)
    regularize_normal_symm!(qt::EdgeGradient{Primal},v::VectorData,sys::ILMSystem)

The operation ``\\mathbf{q}_t = R_t (\\mathbf{n}\\circ \\mathbf{v}+\\mathbf{v}\\circ \\mathbf{n})``, which maps scalar vector data `v` (like
a jump in velocity) to grid data `qt` (like velocity-normal tensor). This is the adjoint
to [`normal_interpolate_symm!`](@ref).
"""
@inline regularize_normal_symm!(q::EdgeGradient{Primal},f::VectorData,cache::BasicILMCache) = regularize_normal_symm!(q,f,cache.nrm,cache.Rsn,cache.snorm_cache,cache.snorm2_cache)

function regularize_normal!(q::EdgeGradient{Primal,Dual,NX,NY},f::VectorData{N},nrm::VectorData{N},Rf::RegularizationMatrix,snorm_cache::TensorData{N}) where {NX,NY,N}
    pointwise_tensorproduct!(snorm_cache,nrm,f)
    mul!(q,Rf,snorm_cache)
end

function regularize_normal_symm!(q::EdgeGradient{Primal,Dual,NX,NY},f::VectorData{N},nrm::VectorData{N},Rf::RegularizationMatrix,snorm_cache::TensorData{N},snorm2_cache::TensorData{N}) where {NX,NY,N}
    pointwise_tensorproduct!(snorm_cache,nrm,f)
    transpose!(snorm2_cache,snorm_cache)
    snorm_cache .+= snorm2_cache
    # subtract the dot product of f and n from each diagonal entry
    snorm_cache.dudx .-= pointwise_dot(nrm,f)
    snorm_cache.dvdy .-= pointwise_dot(nrm,f)
    mul!(q,Rf,snorm_cache)
end

"""
    regularize_normal_cross!(v::Edges{Primal},f::ScalarData,cache::BasicILMCache)
    regularize_normal_cross!(v::Edges{Primal},f::ScalarData,sys::ILMSystem)

The operation ``\\mathbf{v} = R_f \\mathbf{n}\\times f \\mathbf{e}_z``, which maps scalar surface data `f` (like
a jump in streamfunction, endowed with the out-of-plane unit vector ``\\mathbf{e}_z``) to grid
data `v` (like velocity). This is the negative adjoint to [`normal_cross_interpolate!`](@ref).
"""
@inline regularize_normal_cross!(q::Edges{Primal},f::ScalarData,cache::BasicILMCache) =
           _regularize_normal_cross!(q,f,cache,Val(cache_datatype(cache)))

@inline _regularize_normal_cross!(q,f,cache,::Val{:scalar}) =
          regularize_normal_cross!(q,f,cache.nrm,cache.Rsn,cache.snorm_cache)

@inline _regularize_normal_cross!(q,f,cache,::Val{:vector}) =
          regularize_normal_cross!(q,f,cache.nrm,cache.R,cache.sdata_cache)

function regularize_normal_cross!(q::Edges{Primal,NX,NY},f::ScalarData{N},nrm::VectorData{N},Rf::RegularizationMatrix,snorm_cache::VectorData{N}) where {NX,NY,N}
    pointwise_cross!(snorm_cache,nrm,f)
    mul!(q,Rf,snorm_cache)
end

"""
    regularize_normal_cross!(w::Nodes{Dual},vs::VectorData,cache::BasicILMCache)
    regularize_normal_cross!(w::Nodes{Dual},vs::VectorData,sys::ILMSystem)

The operation ``\\omega = R_N \\mathbf{n}\\times \\mathbf{v}_s``, which maps surface vector data `vs` (like
a jump in velocity) to grid dual nodal data `w` (like vorticity). This is for use only
with vector-data caches. This is the negative adjoint to [`normal_cross_interpolate!`](@ref).
"""
@inline regularize_normal_cross!(w::Nodes{Dual},vs::VectorData,cache::BasicILMCache) =
           regularize_normal_cross!(w,vs,cache.nrm,cache.Rcurl,cache.sscalar_cache)

function regularize_normal_cross!(w::Nodes{Dual,NX,NY},vs::VectorData{N},nrm::VectorData{N},Rn::RegularizationMatrix,scache::ScalarData{N}) where {NX,NY,N}
    pointwise_cross!(scache,nrm,vs)
    mul!(w,Rn,scache)
end

"""
    regularize_normal_dot!(f::Nodes{Primal},vs::VectorData,cache::BasicILMCache)
    regularize_normal_dot!(f::Nodes{Primal},vs::VectorData,sys::ILMSystem)

The operation ``\\phi = R_c \\mathbf{n}\\cdot \\mathbf{v}_s``, which maps surface vector data `vs` (like
a jump in velocity) to grid nodal data `f` (like scalar potential). This is the negative adjoint to [`normal_dot_interpolate!`](@ref).
"""
@inline regularize_normal_dot!(f::Nodes{Primal},vs::VectorData,cache::BasicILMCache) =
           _regularize_normal_dot!(f,vs,cache,Val(cache_datatype(cache)))

@inline _regularize_normal_dot!(f,vs,cache,::Val{:scalar}) =
           regularize_normal_dot!(f,vs,cache.nrm,cache.R,cache.sdata_cache)

@inline _regularize_normal_dot!(f,vs,cache,::Val{:vector}) =
           regularize_normal_dot!(f,vs,cache.nrm,cache.Rdiv,cache.sscalar_cache)

function regularize_normal_dot!(f::Nodes{Primal,NX,NY},vs::VectorData{N},nrm::VectorData{N},Rn::RegularizationMatrix,scache::ScalarData{N}) where {NX,NY,N}
    pointwise_dot!(scache,nrm,vs)
    mul!(f,Rn,scache)
end

"""
    regularize_normal_dot!(v::Edges{Primal},taus::TensorData,cache::BasicILMCache)
    regularize_normal_dot!(v::Edges{Primal},taus::TensorData,sys::ILMSystem)

The operation ``\\mathbf{v} = R_f \\mathbf{n}\\cdot \\mathbf{\\tau}_s``, which maps surface tensor data `taus` (like
a stress) to grid edge data `v` (like velocity). This is the negative adjoint to [`normal_dot_interpolate!`](@ref).
"""
@inline regularize_normal_dot!(v::Edges{Primal},taus::TensorData,cache::BasicILMCache{N,SCA,GCT}) where {N,SCA,GCT<:Edges} =
           regularize_normal_dot!(v,taus,cache.nrm,cache.R,cache.sdata_cache)

function regularize_normal_dot!(v::Edges{Primal,NX,NY},taus::TensorData{N},nrm::VectorData{N},R::RegularizationMatrix,scache::VectorData{N}) where {NX,NY,N}
    pointwise_dot!(scache,nrm,taus)
    mul!(v,R,scache)
end

"""
    normal_interpolate!(vn::ScalarData,v::Edges{Primal},cache::BasicILMCache)
    normal_interpolate!(vn::ScalarData,v::Edges{Primal},sys::ILMSystem)

The operation ``v_n = \\mathbf{n} \\cdot R_f^T \\mathbf{v}``, which maps grid data `v` (like velocity) to scalar
surface data `vn` (like normal component of surface velocity). This is the
adjoint to [`regularize_normal!`](@ref).
"""
@inline normal_interpolate!(vn::ScalarData,q::Edges{Primal},cache::BasicILMCache) = normal_interpolate!(vn,q,cache.nrm,cache.Esn,cache.snorm_cache)

function normal_interpolate!(vn::ScalarData{N},q::Edges{Primal,NX,NY},nrm::VectorData{N},Ef::InterpolationMatrix,snorm_cache::VectorData{N}) where {NX,NY,N}
    mul!(snorm_cache,Ef,q)
    pointwise_dot!(vn,nrm,snorm_cache)
end

"""
    normal_interpolate!(τ::VectorData,A::EdgeGradient{Primal},cache::BasicILMCache)
    normal_interpolate!(τ::VectorData,A::EdgeGradient{Primal},sys::ILMSystem)

The operation ``\\mathbf{\\tau} = \\mathbf{n} \\cdot R_t^T \\mathbf{A}``, which maps grid tensor data `A` (like velocity gradient tensor) to vector
surface data `τ` (like traction). This is the adjoint to [`regularize_normal!`](@ref).
"""
@inline normal_interpolate!(vn::VectorData,q::EdgeGradient{Primal},cache::BasicILMCache) = normal_interpolate!(vn,q,cache.nrm,cache.Esn,cache.snorm_cache)

"""
    normal_interpolate_symm!(τ::VectorData,A::EdgeGradient{Primal},cache::BasicILMCache)
    normal_interpolate_symm!(τ::VectorData,A::EdgeGradient{Primal},sys::ILMSystem)

The operation ``\\mathbf{\\tau} = \\mathbf{n} \\cdot R_t^T (\\mathbf{A} +\\mathbf{A}^T - (\\mathrm{tr}\\mathbf{A})\\mathbf{I})``, which maps grid tensor data `A` (like velocity gradient tensor) to vector
surface data `τ` (like traction). This is the adjoint to [`regularize_normal_symm!`](@ref).
"""
@inline normal_interpolate_symm!(vn::VectorData,q::EdgeGradient{Primal},cache::BasicILMCache) = normal_interpolate_symm!(vn,q,cache.nrm,cache.Esn,cache.gsnorm2_cache,cache.snorm_cache)

function normal_interpolate!(vn::VectorData{N},q::EdgeGradient{Primal,Dual,NX,NY},nrm::VectorData{N},Ef::InterpolationMatrix,snorm_cache::TensorData{N}) where {NX,NY,N}
    mul!(snorm_cache,Ef,q)
    pointwise_dot!(vn,nrm,snorm_cache)
end

function normal_interpolate_symm!(vn::VectorData{N},q::EdgeGradient{Primal,Dual,NX,NY},nrm::VectorData{N},Ef::InterpolationMatrix,gsnorm_cache::EdgeGradient{Primal,Dual,NX,NY},snorm_cache::TensorData{N}) where {NX,NY,N}
    transpose!(gsnorm_cache,q)
    gsnorm_cache .+= q
    # subtract the trace of q from each diagonal element
    gsnorm_cache.dudx .-= q.dudx + q.dvdy
    gsnorm_cache.dvdy .-= q.dudx + q.dvdy
    mul!(snorm_cache,Ef,gsnorm_cache)
    pointwise_dot!(vn,nrm,snorm_cache)
end

"""
    normal_cross_interpolate!(wn::ScalarData,v::Edges{Primal},cache::BasicILMCache)
    normal_cross_interpolate!(wn::ScalarData,v::Edges{Primal},sys::ILMSystem)

The operation ``w_n = e_z\\cdot (\\mathbf{n} \\times R_f^T \\mathbf{v})``, which maps grid data `v` (like velocity) to scalar
surface data `wn` (like vorticity in the surface). This is the
negative adjoint to [`regularize_normal_cross!`](@ref).
"""
@inline normal_cross_interpolate!(vn::ScalarData,q::Edges{Primal},cache::BasicILMCache) =
          _normal_cross_interpolate!(vn,q,cache,Val(cache_datatype(cache)))

@inline _normal_cross_interpolate!(vn,q,cache,::Val{:scalar}) =
          normal_cross_interpolate!(vn,q,cache.nrm,cache.Esn,cache.snorm_cache)

@inline _normal_cross_interpolate!(vn,q,cache,::Val{:vector}) =
          normal_cross_interpolate!(vn,q,cache.nrm,cache.E,cache.sdata_cache)

function normal_cross_interpolate!(vn::ScalarData{N},q::Edges{Primal,NX,NY},nrm::VectorData{N},Ef::InterpolationMatrix,scache::VectorData{N}) where {NX,NY,N}
    mul!(scache,Ef,q)
    pointwise_cross!(vn,nrm,scache)
end

"""
    normal_cross_interpolate!(vs::VectorData,s::Nodes{Dual},cache::BasicILMCache)
    normal_cross_interpolate!(vs::VectorData,s::Nodes{Dual},sys::ILMSystem)

The operation ``\\mathbf{v}_s = \\mathbf{n}\\times R_N^T \\psi\\mathbf{e}_z)``, which maps grid data `s` (like streamfunction) to vector
surface data `vs` (like velocity in the surface). It only works for a vector-type cache. This is the
negative adjoint to [`regularize_normal_cross!`](@ref).
"""
@inline normal_cross_interpolate!(vs::VectorData,s::Nodes{Dual},cache::BasicILMCache{N,SCA,GCT}) where {N,SCA,GCT<:Edges} =
          normal_cross_interpolate!(vs,s,cache.nrm,cache.Ecurl,cache.sscalar_cache)

function normal_cross_interpolate!(vs::VectorData{N},s::Nodes{Dual,NX,NY},nrm::VectorData{N},E::InterpolationMatrix,scache::ScalarData{N}) where {NX,NY,N}
    mul!(scache,E,s)
    pointwise_cross!(vs,nrm,scache)
end

"""
    normal_dot_interpolate!(vs::VectorData,f::Nodes{Primal},cache::BasicILMCache)
    normal_dot_interpolate!(vs::VectorData,f::Nodes{Primal},sys::ILMSystem)

The operation ``\\mathbf{v}_s = \\mathbf{n} R_c^T \\phi``, which maps grid nodal data `f` (like scalar potential)
to surface vector data `vs` (like velocity). This is the negative adjoint to [`regularize_normal_dot!`](@ref).
"""
@inline normal_dot_interpolate!(vs::VectorData,f::Nodes{Primal},cache::BasicILMCache) =
           _normal_dot_interpolate!(vs,f,cache,Val(cache_datatype(cache)))

@inline _normal_dot_interpolate!(vs,f,cache,::Val{:scalar}) =
           normal_dot_interpolate!(vs,f,cache.nrm,cache.E,cache.sdata_cache)

@inline _normal_dot_interpolate!(vs,f,cache,::Val{:vector}) =
           normal_dot_interpolate!(vs,f,cache.nrm,cache.Ediv,cache.sscalar_cache)

function normal_dot_interpolate!(vs::VectorData{N},f::Nodes{Primal,NX,NY},nrm::VectorData{N},En::InterpolationMatrix,scache::ScalarData{N}) where {NX,NY,N}
    mul!(scache,En,f)
    product!(vs,nrm,scache)
end

"""
    normal_dot_interpolate!(taus::TensorData,v::Edges{Primal},cache::BasicILMCache)
    normal_dot_interpolate!(taus::TensorData,v::Edges{Primal},sys::ILMSystem)

The operation ``\\mathbf{\\tau}_s = \\mathbf{n}\\circ R_f^T\\mathbf{v}``, which maps grid edge data `v` (like velocity)
to surface tensor data `taus` (like stress). This is the negative adjoint to [`regularize_normal_dot!`](@ref).
"""
@inline normal_dot_interpolate!(taus::TensorData,v::Edges{Primal},cache::BasicILMCache{N,SCA,GCT}) where {N,SCA,GCT<:Edges} =
           normal_dot_interpolate!(taus,v,cache.nrm,cache.E,cache.sdata_cache)

function normal_dot_interpolate!(taus::TensorData{N},v::Edges{Primal,NX,NY},nrm::VectorData{N},E::InterpolationMatrix,scache::VectorData{N}) where {NX,NY,N}
    mul!(scache,E,v)
    pointwise_tensorproduct!(taus,nrm,scache)
end

"""
    surface_curl!(w::Nodes{Dual},f::ScalarData,cache::BasicILMCache)
    surface_curl!(w::Nodes{Dual},f::ScalarData,sys::ILMSystem)

The operation ``w = C_s^T f = C^T R_f \\mathbf{n}\\circ f``, which maps scalar surface data `f` (like
a jump in scalar potential) to grid data `w` (like vorticity). This is the adjoint
to ``C_s``, also given by `surface_curl!` (but with arguments switched). Note that
the differential operations are divided either by 1 or by the grid cell size,
depending on whether `sys` has been designated with `IndexScaling` or `GridScaling`,
respectively.
"""
function surface_curl!(w::Nodes{Dual},f::ScalarData,cache::BasicILMCache)
  _unscaled_surface_curl!(w,f,cache,Val(cache_datatype(cache)))
  _scale_derivative!(w,cache)
end

@inline _unscaled_surface_curl!(w::Nodes{Dual,NX,NY},f::ScalarData{N},cache::BasicILMCache,::Val{:scalar}) where {NX,NY,N} =
          _unscaled_surface_curl!(w,f,cache.nrm,cache.Rsn,cache.gsnorm_cache,cache.snorm_cache)

@inline _unscaled_surface_curl!(w::Nodes{Dual,NX,NY},f::ScalarData{N},cache::BasicILMCache,::Val{:vector}) where {NX,NY,N} =
          _unscaled_surface_curl!(w,f,cache.nrm,cache.R,cache.gdata_cache,cache.sdata_cache)

function _unscaled_surface_curl!(w::Nodes{Dual,NX,NY},f::ScalarData{N},nrm::VectorData{N},Rf::RegularizationMatrix,q_cache::Edges{Primal,NX,NY},snorm_cache::VectorData{N}) where {NX,NY,N}
    fill!(w,0.0)
    fill!(q_cache,0.0)
    regularize_normal!(q_cache,f,nrm,Rf,snorm_cache)
    curl!(w,q_cache)
end

"""
    surface_curl!(w::Nodes{Dual},v::VectorData,cache::BasicILMCache)
    surface_curl!(w::Nodes{Dual},v::VectorData,sys::ILMSystem)

The operation ``w = C_s^T \\mathbf{v} = C^T R_f \\mathbf{v}``, which maps vector surface data `v` (like
velocity) to grid data `w` (like vorticity). This is the adjoint
to ``C_s``, also given by `surface_curl!` (but with arguments switched). Note that
the differential operations are divided either by 1 or by the grid cell size,
depending on whether `sys` has been designated with `IndexScaling` or `GridScaling`,
respectively.
"""
function surface_curl!(w::Nodes{Dual},v::VectorData,cache::BasicILMCache)
  _unscaled_surface_curl!(w,v,cache.R,cache.gdata_cache)
  _scale_derivative!(w,cache)
end

function _unscaled_surface_curl!(w::Nodes{Dual,NX,NY},v::VectorData{N},Rf::RegularizationMatrix,q_cache::Edges{Primal,NX,NY}) where {NX,NY,N}
    fill!(w,0.0)
    fill!(q_cache,0.0)
    regularize!(q_cache,v,Rf)
    curl!(w,q_cache)
end

"""
    surface_curl!(vn::ScalarData,s::Nodes{Dual},cache::BasicILMCache)
    surface_curl!(vn::ScalarData,s::Nodes{Dual},sys::ILMSystem)

The operation ``v_n = C_s s = \\mathbf{n} \\cdot R_f^T C s``, which maps grid data `s` (like
streamfunction) to scalar surface data `vn` (like normal component of velocity).
This is the adjoint to ``C_s^T``, also given by `surface_curl!`, but with
arguments switched.  Note that
the differential operations are divided either by 1 or by the grid cell size,
depending on whether `sys` has been designated with `IndexScaling` or `GridScaling`,
respectively.
"""
function surface_curl!(vn::ScalarData,s::Nodes{Dual},cache::BasicILMCache)
  _unscaled_surface_curl!(vn,s,cache,Val(cache_datatype(cache)))
  _scale_derivative!(vn,cache)
end

@inline _unscaled_surface_curl!(vn::ScalarData{N},s::Nodes{Dual,NX,NY},cache::BasicILMCache,::Val{:scalar}) where {NX,NY,N} =
          _unscaled_surface_curl!(vn,s,cache.nrm,cache.Esn,cache.gsnorm_cache,cache.snorm_cache)

@inline _unscaled_surface_curl!(vn::ScalarData{N},s::Nodes{Dual,NX,NY},cache::BasicILMCache,::Val{:vector}) where {NX,NY,N} =
          _unscaled_surface_curl!(vn,s,cache.nrm,cache.E,cache.gdata_cache,cache.sdata_cache)

function _unscaled_surface_curl!(vn::ScalarData{N},s::Nodes{Dual,NX,NY},nrm::VectorData{N},Ef::InterpolationMatrix,q_cache::Edges{Primal,NX,NY},snorm_cache::VectorData{N}) where {NX,NY,N}
    fill!(vn,0.0)
    fill!(q_cache,0.0)
    curl!(q_cache,s)
    normal_interpolate!(vn,q_cache,nrm,Ef,snorm_cache)
end

"""
    surface_curl!(v::VectorData,s::Nodes{Dual},cache::BasicILMCache)
    surface_curl!(v::VectorData,s::Nodes{Dual},sys::ILMSystem)

The operation ``\\mathbf{v} = C_s s = R_f^T C s``, which maps grid data `s` (like
streamfunction) to vector surface data `v` (like velocity).
This is the adjoint to ``C_s^T``, also given by `surface_curl!`, but with
arguments switched.  Note that
the differential operations are divided either by 1 or by the grid cell size,
depending on whether `sys` has been designated with `IndexScaling` or `GridScaling`,
respectively.
"""
function surface_curl!(v::VectorData,s::Nodes{Dual},cache::BasicILMCache)
  _unscaled_surface_curl!(v,s,cache.E,cache.gdata_cache)
  _scale_derivative!(v,cache)
end

function _unscaled_surface_curl!(v::VectorData{N},s::Nodes{Dual,NX,NY},Ef::InterpolationMatrix,q_cache::Edges{Primal,NX,NY}) where {NX,NY,N}
    fill!(q_cache,0.0)
    curl!(q_cache,s)
    interpolate!(v,q_cache,Ef)
end

"""
    surface_curl_cross!(w::Nodes{Dual},f::ScalarData,cache::BasicILMCache)
    surface_curl_cross!(w::Nodes{Dual},f::ScalarData,sys::ILMSystem)

The operation ``w = \\hat{C}_s^T f = C^T R_f \\mathbf{n}\\times f \\mathbf{e}_z``, which maps scalar surface data `f` (like
a jump in streamfunction, multiplied by out-of-plane unit vector ``\\mathbf{e}_z``) to grid data `w` (like vorticity).
This is the adjoint to ``\\hat{C}_s``, also given by `surface_curl_cross!` (but with arguments switched). Note that
the differential operations are divided either by 1 or by the grid cell size,
depending on whether `sys` has been designated with `IndexScaling` or `GridScaling`,
respectively.
"""
function surface_curl_cross!(w::Nodes{Dual},f::ScalarData,cache::BasicILMCache)
  _unscaled_surface_curl_cross!(w,f,cache,Val(cache_datatype(cache)))
  _scale_derivative!(w,cache)
end

@inline _unscaled_surface_curl_cross!(w::Nodes{Dual,NX,NY},f::ScalarData{N},cache::BasicILMCache,::Val{:scalar}) where {NX,NY,N} =
          _unscaled_surface_curl_cross!(w,f,cache.nrm,cache.Rsn,cache.gsnorm_cache,cache.snorm_cache)

@inline _unscaled_surface_curl_cross!(w::Nodes{Dual,NX,NY},f::ScalarData{N},cache::BasicILMCache,::Val{:vector}) where {NX,NY,N} =
          _unscaled_surface_curl_cross!(w,f,cache.nrm,cache.R,cache.gdata_cache,cache.sdata_cache)

function _unscaled_surface_curl_cross!(w::Nodes{Dual,NX,NY},f::ScalarData{N},nrm::VectorData{N},Rf::RegularizationMatrix,q_cache::Edges{Primal,NX,NY},snorm_cache::VectorData{N}) where {NX,NY,N}
    fill!(w,0.0)
    fill!(q_cache,0.0)
    regularize_normal_cross!(q_cache,f,nrm,Rf,snorm_cache)
    curl!(w,q_cache)
end

"""
    surface_curl_cross!(γ::ScalarData,s::Nodes{Dual},cache::BasicILMCache)
    surface_curl_cross!(γ::ScalarData,s::Nodes{Dual},sys::ILMSystem)

The operation ``\\gamma = \\hat{C}_s s = \\mathbf{e}_z\\cdot (\\mathbf{n} \\times R_f^T C s)``, which maps grid data `s` (like
streamfunction) to scalar surface data `γ` (like vorticity in the surface).
This is the adjoint to ``\\hat{C}_s^T``, also given by `surface_curl_cross!`, but with
arguments switched.  Note that
the differential operations are divided either by 1 or by the grid cell size,
depending on whether `sys` has been designated with `IndexScaling` or `GridScaling`,
respectively.
"""
function surface_curl_cross!(vn::ScalarData,s::Nodes{Dual},cache::BasicILMCache)
  _unscaled_surface_curl_cross!(vn,s,cache,Val(cache_datatype(cache)))
  _scale_derivative!(vn,cache)
end

@inline _unscaled_surface_curl_cross!(vn::ScalarData{N},s::Nodes{Dual,NX,NY},cache::BasicILMCache,::Val{:scalar}) where {NX,NY,N} =
          _unscaled_surface_curl_cross!(vn,s,cache.nrm,cache.Esn,cache.gsnorm_cache,cache.snorm_cache)

@inline _unscaled_surface_curl_cross!(vn::ScalarData{N},s::Nodes{Dual,NX,NY},cache::BasicILMCache,::Val{:vector}) where {NX,NY,N} =
          _unscaled_surface_curl_cross!(vn,s,cache.nrm,cache.E,cache.gdata_cache,cache.sdata_cache)

function _unscaled_surface_curl_cross!(vn::ScalarData{N},s::Nodes{Dual,NX,NY},nrm::VectorData{N},Ef::InterpolationMatrix,q_cache::Edges{Primal,NX,NY},snorm_cache::VectorData{N}) where {NX,NY,N}
    fill!(vn,0.0)
    fill!(q_cache,0.0)
    curl!(q_cache,s)
    normal_cross_interpolate!(vn,q_cache,nrm,Ef,snorm_cache)
end

"""
    surface_divergence!(Θ::Nodes{Primal},f::ScalarData,cache::BasicILMCache)
    surface_divergence!(Θ::Nodes{Primal},f::ScalarData,sys::ILMSystem)

The operation ``\\theta = D_s f = D R_f \\mathbf{n} \\circ f``, which maps surface scalar data `f` (like
jump in scalar potential) to grid data `Θ` (like dilatation, i.e. divergence of velocity).
This is the negative adjoint of [`surface_grad!`](@ref).
Note that
the differential operations are divided either by 1 or by the grid cell size,
depending on whether `sys` has been designated with `IndexScaling` or `GridScaling`,
respectively.
"""
function surface_divergence!(θ::Nodes{Primal},f::ScalarData,cache::BasicILMCache)
  _unscaled_surface_divergence!(θ,f,cache,Val(cache_datatype(cache)))
  _scale_derivative!(θ,cache)
end

@inline _unscaled_surface_divergence!(Θ::Nodes{Primal,NX,NY},f::ScalarData{N},cache,::Val{:scalar}) where {NX,NY,N} =
          _unscaled_surface_divergence!(Θ,f,cache.nrm,cache.Rsn,cache.gsnorm_cache,cache.snorm_cache)

@inline _unscaled_surface_divergence!(Θ::Nodes{Primal,NX,NY},f::ScalarData{N},cache,::Val{:vector}) where {NX,NY,N} =
          _unscaled_surface_divergence!(Θ,f,cache.nrm,cache.R,cache.gdata_cache,cache.sdata_cache)

function _unscaled_surface_divergence!(θ::Nodes{Primal,NX,NY},f::ScalarData{N},nrm::VectorData{N},Rf::RegularizationMatrix,q_cache::Edges{Primal,NX,NY},snorm_cache::VectorData{N}) where {NX,NY,N}
    fill!(θ,0.0)
    fill!(q_cache,0.0)
    regularize_normal!(q_cache,f,nrm,Rf,snorm_cache)
    divergence!(θ,q_cache)
end

"""
    surface_divergence!(v::Edges{Primal},dv::VectorData,cache::BasicILMCache)
    surface_divergence!(v::Edges{Primal},dv::VectorData,sys::ILMSystem)

The operation ``\\mathbf{v} = D_s d\\mathbf{v} = D R_t (\\mathbf{n} \\circ d\\mathbf{v})``, which maps surface vector data `dv` (like
jump in velocity) to grid data `v` (like velocity). This is the negative adjoint of [`surface_grad!`](@ref). Note that
the differential operations are divided either by 1 or by the grid cell size,
depending on whether `sys` has been designated with `IndexScaling` or `GridScaling`,
respectively.
"""
function surface_divergence!(θ::Edges{Primal},f::VectorData,cache::BasicILMCache)
  _unscaled_surface_divergence!(θ,f,cache.nrm,cache.Rsn,cache.gsnorm_cache,cache.snorm_cache)
  _scale_derivative!(θ,cache)
end

"""
    surface_divergence_symm!(v::Edges{Primal},dv::VectorData,cache::BasicILMCache)
    surface_divergence_symm!(v::Edges{Primal},dv::VectorData,sys::ILMSystem)

The operation ``\\mathbf{v} = D_s d\\mathbf{v} = D R_t (\\mathbf{n} \\circ d\\mathbf{v} + d\\mathbf{v} \\circ \\mathbf{n})``, which maps surface vector data `dv` (like
jump in velocity) to grid data `v` (like velocity). This is the negative adjoint of [`surface_grad_symm!`](@ref). Note that
the differential operations are divided either by 1 or by the grid cell size,
depending on whether `sys` has been designated with `IndexScaling` or `GridScaling`,
respectively.
"""
function surface_divergence_symm!(θ::Edges{Primal},f::VectorData,cache::BasicILMCache)
  _unscaled_surface_divergence_symm!(θ,f,cache.nrm,cache.Rsn,cache.gsnorm_cache,cache.snorm_cache,cache.snorm2_cache)
  _scale_derivative!(θ,cache)
end

function _unscaled_surface_divergence!(θ::Edges{Primal,NX,NY},f::VectorData{N},nrm::VectorData{N},Rf::RegularizationMatrix,q_cache::EdgeGradient{Primal,Dual,NX,NY},snorm_cache::TensorData{N}) where {NX,NY,N}
    fill!(θ,0.0)
    fill!(q_cache,0.0)
    regularize_normal!(q_cache,f,nrm,Rf,snorm_cache)
    divergence!(θ,q_cache)
end

function _unscaled_surface_divergence_symm!(θ::Edges{Primal,NX,NY},f::VectorData{N},nrm::VectorData{N},Rf::RegularizationMatrix,q_cache::EdgeGradient{Primal,Dual,NX,NY},snorm_cache::TensorData{N},snorm2_cache::TensorData{N}) where {NX,NY,N}
    fill!(θ,0.0)
    fill!(q_cache,0.0)
    regularize_normal_symm!(q_cache,f,nrm,Rf,snorm_cache,snorm2_cache)
    divergence!(θ,q_cache)
end

"""
    surface_grad!(vn::ScalarData,ϕ::Nodes{Primal},cache::BasicILMCache)
    surface_grad!(vn::ScalarData,ϕ::Nodes{Primal},sys::ILMSystem)

The operation ``v_n = G_s\\phi = \\mathbf{n} \\cdot R_f^T G\\phi``, which maps grid data `ϕ` (like
scalar potential) to scalar surface data `vn` (like normal component of velocity). This is the negative adjoint of [`surface_divergence!`](@ref).
Note that
the differential operations are divided either by 1 or by the grid cell size,
depending on whether `sys` has been designated with `IndexScaling` or `GridScaling`,
respectively.
"""
function surface_grad!(vn::ScalarData,ϕ::Nodes{Primal},cache::BasicILMCache)
  _unscaled_surface_grad!(vn,ϕ,cache,Val(cache_datatype(cache)))
  _scale_derivative!(vn,cache)
end

@inline _unscaled_surface_grad!(vn::ScalarData{N},ϕ::Nodes{Primal,NX,NY},cache,::Val{:scalar}) where {NX,NY,N} =
          _unscaled_surface_grad!(vn,ϕ,cache.nrm,cache.Esn,cache.gsnorm_cache,cache.snorm_cache)

@inline _unscaled_surface_grad!(vn::ScalarData{N},ϕ::Nodes{Primal,NX,NY},cache,::Val{:vector}) where {NX,NY,N} =
          _unscaled_surface_grad!(vn,ϕ,cache.nrm,cache.E,cache.gdata_cache,cache.sdata_cache)

function _unscaled_surface_grad!(vn::ScalarData{N},ϕ::Nodes{Primal,NX,NY},nrm::VectorData{N},Ef::InterpolationMatrix,q_cache::Edges{Primal,NX,NY},snorm_cache::VectorData{N}) where {NX,NY,N}
    fill!(vn,0.0)
    fill!(q_cache,0.0)
    grad!(q_cache,ϕ)
    normal_interpolate!(vn,q_cache,nrm,Ef,snorm_cache)
end

"""
    surface_grad!(τ::VectorData,v::Edges{Primal},cache::BasicILMCache)
    surface_grad!(τ::VectorData,v::Edges{Primal},sys::ILMSystem)

The operation ``\\mathbf{\\tau} = G_s v = \\mathbf{n} \\cdot R_t^T G \\mathbf{v}``, which maps grid vector data `v` (like
velocity) to vector surface data `τ` (like traction). This is the negative adjoint of [`surface_divergence!`](@ref). Note that
the differential operations are divided either by 1 or by the grid cell size,
depending on whether `sys` has been designated with `IndexScaling` or `GridScaling`,
respectively.
"""
function surface_grad!(vn::VectorData,ϕ::Edges{Primal},cache::BasicILMCache)
  _unscaled_surface_grad!(vn,ϕ,cache.nrm,cache.Esn,cache.gsnorm_cache,cache.snorm_cache)
  _scale_derivative!(vn,cache)
end

"""
    surface_grad_symm!(τ::VectorData,v::Edges{Primal},cache::BasicILMCache)
    surface_grad_symm!(τ::VectorData,v::Edges{Primal},sys::ILMSystem)

The operation ``\\mathbf{\\tau} = G_s v = \\mathbf{n} \\cdot R_t^T (G \\mathbf{v} + (G \\mathbf{v})^T)``, which maps grid vector data `v` (like
velocity) to vector surface data `τ` (like traction). This is the negative adjoint of [`surface_divergence_symm!`](@ref). Note that
the differential operations are divided either by 1 or by the grid cell size,
depending on whether `sys` has been designated with `IndexScaling` or `GridScaling`,
respectively.
"""
function surface_grad_symm!(vn::VectorData,ϕ::Edges{Primal},cache::BasicILMCache)
  _unscaled_surface_grad_symm!(vn,ϕ,cache.nrm,cache.Esn,cache.gsnorm_cache,cache.gsnorm2_cache,cache.snorm_cache)
  _scale_derivative!(vn,cache)
end

function _unscaled_surface_grad!(vn::VectorData{N},ϕ::Edges{Primal,NX,NY},nrm::VectorData{N},Ef::InterpolationMatrix,gsnorm_cache::EdgeGradient{Primal,Dual,NX,NY},snorm_cache::TensorData{N}) where {NX,NY,N}
    fill!(vn,0.0)
    fill!(gsnorm_cache,0.0)
    grad!(gsnorm_cache,ϕ)
    normal_interpolate!(vn,gsnorm_cache,nrm,Ef,snorm_cache)
end

function _unscaled_surface_grad_symm!(vn::VectorData{N},ϕ::Edges{Primal,NX,NY},nrm::VectorData{N},Ef::InterpolationMatrix,gsnorm_cache::EdgeGradient{Primal,Dual,NX,NY},gsnorm2_cache::EdgeGradient{Primal,Dual,NX,NY},snorm_cache::TensorData{N}) where {NX,NY,N}
    fill!(vn,0.0)
    fill!(gsnorm_cache,0.0)
    grad!(gsnorm_cache,ϕ)
    normal_interpolate_symm!(vn,gsnorm_cache,nrm,Ef,gsnorm2_cache,snorm_cache)
end

"""
    surface_divergence_cross!(Θ::Nodes{Primal},f::ScalarData,cache::BasicILMCache)
    surface_divergence_cross!(Θ::Nodes{Primal},f::ScalarData,sys::ILMSystem)

The operation ``\\theta = \\hat{D}_s f = D R_f \\mathbf{n} \\times f \\mathbf{e}_z``, which maps surface scalar data `f` (like
jump in streamfunction) to grid data `Θ` (like dilatation, i.e. divergence of velocity).
This is the adjoint of [`surface_grad_cross!`](@ref).
Note that the differential operations are divided either by 1 or by the grid cell size,
depending on whether `sys` has been designated with `IndexScaling` or `GridScaling`,
respectively.
"""
function surface_divergence_cross!(θ::Nodes{Primal},f::ScalarData,cache::BasicILMCache)
  _unscaled_surface_divergence_cross!(Θ,f,cache,Val(cache_datatype(cache)))
  _scale_derivative!(θ,cache)
end

@inline _unscaled_surface_divergence_cross!(Θ,f,cache,::Val{:scalar}) =
          _unscaled_surface_divergence_cross!(Θ,f,cache.nrm,cache.Rsn,cache.gsnorm_cache,cache.snorm_cache)

@inline _unscaled_surface_divergence_cross!(Θ,f,cache,::Val{:vector}) =
          _unscaled_surface_divergence_cross!(Θ,f,cache.nrm,cache.R,cache.gdata_cache,cache.sdata_cache)

function _unscaled_surface_divergence_cross!(θ::Nodes{Primal,NX,NY},f::ScalarData{N},nrm::VectorData{N},Rf::RegularizationMatrix,q_cache::Edges{Primal,NX,NY},snorm_cache::VectorData{N}) where {NX,NY,N}
    fill!(θ,0.0)
    fill!(q_cache,0.0)
    regularize_normal_cross!(q_cache,f,nrm,Rf,snorm_cache)
    divergence!(θ,q_cache)
end

"""
    surface_grad_cross!(γ::ScalarData,ϕ::Nodes{Primal},cache::BasicILMCache)
    surface_grad_cross!(γ::ScalarData,ϕ::Nodes{Primal},sys::ILMSystem)

The operation ``\\gamma = \\hat{G}_s\\phi = \\mathbf{e}_z \\cdot (\\mathbf{n} \\times R_f^T G\\phi)``, which maps grid data `ϕ` (like
scalar potential) to scalar surface data `γ` (like surface vorticity). This is the adjoint of [`surface_divergence_cross!`](@ref).
Note that
the differential operations are divided either by 1 or by the grid cell size,
depending on whether `sys` has been designated with `IndexScaling` or `GridScaling`,
respectively.
"""
function surface_grad_cross!(vn::ScalarData,ϕ::Nodes{Primal},cache::BasicILMCache)
  _unscaled_surface_grad_cross!(vn,ϕ,cache,Val(cache_datatype(cache)))
  _scale_derivative!(vn,cache)
end

@inline _unscaled_surface_grad_cross!(vn,ϕ,cache,::Val{:scalar}) =
          _unscaled_surface_grad_cross!(vn,ϕ,cache.nrm,cache.Esn,cache.gsnorm_cache,cache.snorm_cache)

@inline _unscaled_surface_grad_cross!(vn,ϕ,cache,::Val{:vector}) =
          _unscaled_surface_grad_cross!(vn,ϕ,cache.nrm,cache.E,cache.gdata_cache,cache.sdata_cache)

function _unscaled_surface_grad_cross!(vn::ScalarData{N},ϕ::Nodes{Primal,NX,NY},nrm::VectorData{N},Ef::InterpolationMatrix,q_cache::Edges{Primal,NX,NY},snorm_cache::VectorData{N}) where {NX,NY,N}
    fill!(vn,0.0)
    fill!(q_cache,0.0)
    grad!(q_cache,ϕ)
    normal_cross_interpolate!(vn,q_cache,nrm,Ef,snorm_cache)
end

"""
    mask(cache::BasicILMCache) -> GridData
    mask(sys::ILMSystem) -> GridData

Create grid data that consist of 1s inside of a surface (i.e., on a side opposite
  the normal vectors) and 0s outside. The grid data are the same type as the
  output data type of `sys`.  Only allows `sys` to have `GridScaling`.
"""
@inline mask(cache::BasicILMCache{N,GridScaling}) where {N} = _mask!(zeros_grid(cache),cache)

"""
    complementary_mask(cache::BasicILMCache) -> GridData
    complementary_mask(sys::ILMSystem) -> GridData

Create grid data that consist of 0s inside of a surface (i.e., on a side opposite
  the normal vectors) and 1s outside. The grid data are the same type as the
  output data type of `cache`.  Only allows `cache` to have `GridScaling`.
"""
@inline complementary_mask(cache::BasicILMCache{N,GridScaling}) where {N} =
          _complementary_mask!(zeros_grid(cache),cache)

"""
    mask!(w::GridData,cache::BasicILMCache)
    mask!(w::GridData,sys::ILMSystem)

Mask the data `w` in place by multiplying it by 1s inside of a surface (i.e., on a side opposite
  the normal vectors) and 0s outside. The grid data `w` must be of the same type as the
  output data type of `cache`. Only allows `cache` to have `GridScaling`.
"""
function mask!(w::T,cache::BasicILMCache{N,GridScaling}) where {T <: GridData,N}
  @unpack gdata_cache = cache
  _mask!(gdata_cache,cache)
  product!(w,gdata_cache,w)
end

"""
    complementary_mask!(w::GridData,cache::BasicILMCache)
    complementary_mask!(w::GridData,sys::ILMSystem)

Mask the data `w` in place by multiplying it by 0s inside of a surface (i.e., on a side opposite
  the normal vectors) and 1s outside. The grid data `w` must be of the same type as the
  output data type of `cache`. Only allows `cache` to have `GridScaling`.
"""
function complementary_mask!(w::T,cache::BasicILMCache{N,GridScaling}) where {T <: GridData,N}
  @unpack gdata_cache = cache
  _complementary_mask!(gdata_cache,cache)
  product!(w,gdata_cache,w)
end

function _mask!(msk,cache)
  @unpack sdata_cache, gdata_cache, L = cache
  typeof(msk) == typeof(gdata_cache) || error("Wrong data type")
  fill!(sdata_cache,1.0)
  surface_divergence!(msk,sdata_cache,cache)
  inverse_laplacian!(msk,cache)
  msk .*= -1.0
end

function _complementary_mask!(msk,cache)
    _mask!(msk,cache)
    msk .= 1.0 - msk
end

ext: jl


text:
using MeshCat, MeshCatMechanisms, RigidBodyDynamics

include(joinpath(pwd(),"models/kuka/kuka_utils.jl"))
urdf_path = joinpath(pwd(), "models/kuka/temp/kuka.urdf")
kuka = MeshCatMechanisms.parse_urdf(urdf_path, remove_fixed_tree_joints = true)
kuka_visuals = MeshCatMechanisms.URDFVisuals(urdf_path)

state = MechanismState(kuka)
state_cache = StateCache(kuka)
result = DynamicsResult(kuka)
result_cache = DynamicsResultCache(kuka)

vis = Visualizer()
mvis = MechanismVisualizer(kuka, kuka_visuals, vis[:base])
open(vis)

# Kuka iiwa arm parsed from URDF using RigidBodyDynamics.jl
struct Kuka{I, T} <: Model{I, T}
	n::Int
	m::Int
	d::Int

    state_cache1
    state_cache2
    state_cache3

    result_cache1
    result_cache2
    result_cache3
end

n = 14
m = 7
d = 0

results_cache1 = DynamicsResultCache(kuka)
results_cache2 = DynamicsResultCache(kuka)
results_cache3 = DynamicsResultCache(kuka)

state_cache1 = StateCache(kuka)
state_cache2 = StateCache(kuka)
state_cache3 = StateCache(kuka)

function f(model::Kuka, x, u, w)
	@error "not implemented"
end

model = Kuka{Midpoint, FixedTime}(
	n, m, d,
    state_cache1, state_cache2, state_cache3,
    results_cache1, results_cache2, results_cache3)


text:
const USE_GPU = true  # Use GPU? If this is set false, then no GPU needs to be available
using ParallelStencil
using ParallelStencil.FiniteDifferences3D
@static if USE_GPU
    @init_parallel_stencil(CUDA, Float64, 3)
else
    @init_parallel_stencil(Threads, Float64, 3)
end
using ImplicitGlobalGrid, Plots, Printf, Statistics
import MPI

@parallel function compute_V!(Vx::Data.Array, Vy::Data.Array, Vz::Data.Array, P::Data.Array, dt::Data.Number, ρ::Data.Number, dx::Data.Number, dy::Data.Number, dz::Data.Number)
    @inn(Vx) = @inn(Vx) - dt/ρ*@d_xi(P)/dx
    @inn(Vy) = @inn(Vy) - dt/ρ*@d_yi(P)/dy
    @inn(Vz) = @inn(Vz) - dt/ρ*@d_zi(P)/dz
    return
end

@parallel function compute_P!(P::Data.Array, Vx::Data.Array, Vy::Data.Array, Vz::Data.Array, dt::Data.Number, k::Data.Number, dx::Data.Number, dy::Data.Number, dz::Data.Number)
    @all(P) = @all(P) - dt*k*(@d_xa(Vx)/dx + @d_ya(Vy)/dy + @d_za(Vz)/dz)
    return
end

##################################################
@views function acoustic3D()
    # Physics
    lx, ly, lz = 60.0, 60.0, 60.0  # domain extends
    k          = 1.0               # bulk modulus
    ρ          = 1.0               # density
    t          = 0.0               # physical time
    # Numerics
    nx, ny, nz = 127, 127, 127     # numerical grid resolution; should be a mulitple of 32-1 for optimal GPU perf
    nt         = 1000              # number of timesteps
    nout       = 20                # plotting frequency
    # Derived numerics
    me, dims, nprocs, coords, comm = init_global_grid(nx, ny, nz) # MPI initialisation
    select_device()                                               # select one GPU per MPI local rank (if >1 GPU per node)
    dx, dy, dz = lx/(nx_g()-1), ly/(ny_g()-1), lz/(nz_g()-1)      # cell sizes
    # Array allocations
    P          = @zeros(nx  ,ny  ,nz  )
    Vx         = @zeros(nx+1,ny  ,nz  )
    Vy         = @zeros(nx  ,ny+1,nz  )
    Vz         = @zeros(nx  ,ny  ,nz+1)
    # Initial conditions
    P         .= Data.Array([exp(-(x_g(ix,dx,P)-0.5*lx)^2 -(y_g(iy,dy,P)-0.5*ly)^2 -(z_g(iz,dz,P)-0.5*lz)^2) for ix=1:size(P,1), iy=1:size(P,2), iz=1:size(P,3)])
    dt         = min(dx,dy,dz)/sqrt(k/ρ)/6.1
     # Preparation of visualisation
    ENV["GKSwstype"]="nul"
    if (me==0)
        if isdir("viz3D_out")==false mkdir("viz3D_out") end; loadpath = "./viz3D_out/"; anim = Animation(loadpath,String[])
        println("Animation directory: $(anim.dir)")
    end
    nx_v, ny_v, nz_v = (nx-2)*dims[1], (ny-2)*dims[2], (nz-2)*dims[3]
    if (nx_v*ny_v*nz_v*sizeof(Data.Number) > 0.8*Sys.free_memory()) error("Not enough memory for visualization.") end
    P_v   = zeros(nx_v, ny_v, nz_v) # global array for visu
    P_inn = zeros(nx-2, ny-2, nz-2) # no halo local array for visu
    y_sl  = Int(ceil(ny_g()/2))
    Xi_g  = dx:dx:(lx-dx) # inner points only
    Zi_g  = dz:dz:(lz-dz)
    # Time loop
    for it = 1:nt
        if (it==11) tic() end
        @hide_communication (16, 8, 4) begin # communication/computation overlap
            @parallel compute_V!(Vx, Vy, Vz, P, dt, ρ, dx, dy, dz)
            update_halo!(Vx, Vy, Vz)
        end
        @parallel compute_P!(P, Vx, Vy, Vz, dt, k, dx, dy, dz)
        t = t + dt
        # Visualisation
        if mod(it,nout)==0
            P_inn .= P[2:end-1,2:end-1,2:end-1]; gather!(P_inn, P_v)
            if (me==0) heatmap(Xi_g, Zi_g, P_v[:,y_sl,:]', aspect_ratio=1, xlims=(Xi_g[1],Xi_g[end]), ylims=(Zi_g[1],Zi_g[end]), c=:viridis, title="Pressure"); frame(anim) end
        end
    end
    # Performance
    wtime    = toc()
    A_eff    = (4*2)/1e9*nx*ny*nz*sizeof(Data.Number)  # Effective main memory access per iteration [GB] (Lower bound of required memory access: H and dHdτ have to be read and written (dHdτ for damping): 4 whole-array memaccess; B has to be read: 1 whole-array memaccess)
    wtime_it = wtime/(nt-10)                           # Execution time per iteration [s]
    T_eff    = A_eff/wtime_it                          # Effective memory throughput [GB/s]
    if (me==0) @printf("Total steps=%d, time=%1.3e sec (@ T_eff = %1.2f GB/s) \n", nt, wtime, round(T_eff, sigdigits=2)) end
    if (me==0) gif(anim, "acoustic3D.gif", fps = 15) end
    finalize_global_grid()
    return
end

acoustic3D()


text:
# Note that this script can accept some limited command-line arguments, run
# `julia build_tarballs.jl --help` to see a usage message.
using BinaryBuilder, Pkg

name = "SLICOT"
version = v"5.8.0"

# Collection of sources required to complete build
# Note to maintainers: extracts from LAPACK are deprecated routines, so probably don't want
# to update the LAPACK version used here.
sources = [
    GitSource("https://github.com//SLICOT/SLICOT-Reference.git",
              "d8e12fe9787f9e7d32df992cc32840e01944abd6"),
    ArchiveSource("https://github.com/Reference-LAPACK/lapack/archive/refs/tags/v3.8.0.tar.gz",
              "deb22cc4a6120bff72621155a9917f485f96ef8319ac074a7afbc68aab88bcf6"),
]

# Bash recipe for building across all platforms
script = raw"""
cd $WORKSPACE/srcdir/SLICOT-Reference
echo "cmake_minimum_required(VERSION 3.17)" >>CMakeLists.txt
echo "project( cml )" >>CMakeLists.txt
echo "enable_language( Fortran )" >>CMakeLists.txt
echo "add_subdirectory( src )" >>CMakeLists.txt

cd $WORKSPACE/srcdir/SLICOT-Reference/src
cp ../../lapack-3.8.0/SRC/DEPRECATED/[dz]latzm.f .
cp ../../lapack-3.8.0/SRC/DEPRECATED/dgegs.f .

echo "set( SLICOT_SOURCE_FILES" >source_files.cmake
ls *.f >>source_files.cmake
echo ")" >>source_files.cmake

LAB_SYMBOLS=(
DASUM DAXPY DBDSQR DCABS1 DCOPY DDOT DGEBAK DGEBAL DGEBRD DGECON
DGEEQU DGEES DGEEV DGEGS DGEHRD DGELQ2 DGELQF DGELS DGELSS DGELSY
DGEMM DGEMV DGEQLF DGEQP3 DGEQR2 DGEQRF DGER DGERFS DGERQ2 DGERQF
DGESC2 DGESV DGESVD DGESVX DGETC2 DGETRF DGETRI DGETRS DGGBAK DGGBAL
DGGES DGGEV DHGEQZ DHSEQR DLABAD DLACN2 DLACPY DLADIV DLAEXC DLAG2
DLAGV2 DLAHQR DLAIC1 DLALN2 DLAMC3 DLAMCH DLANGE DLANHS DLANSY DLANTR
DLANV2 DLAPMT DLAPY2 DLAPY3 DLAQGE DLARF DLARFB DLARFG DLARFT DLARFX
DLARNV DLARTG DLAS2 DLASCL DLASET DLASR DLASRT DLASSQ DLASV2 DLASY2
DLATRS DLATZM DNRM2 DORG2R DORGBR DORGHR DORGQR DORGR2 DORGRQ DORM2R
DORMBR DORMHR DORMLQ DORMQL DORMQR DORMR2 DORMRQ DORMRZ DPOCON DPOSV
DPOTRF DPOTRS DPPTRF DPPTRI DPPTRS DPTTRF DPTTRS DROT DROTG DRSCL
DSCAL DSPMV DSPR DSWAP DSYCON DSYEV DSYEVX DSYMM DSYMV DSYR2 DSYR2K
DSYRK DSYSV DSYTRF DSYTRI DSYTRS DTGEX2 DTGEXC DTGSEN DTGSYL DTPMV
DTRCON DTREVC DTREXC DTRMM DTRMV DTRSEN DTRSM DTRSV DTRSYL DTRTRI
DTRTRS DTZRZF DZASUM DZNRM2 IDAMAX ILAENV IZAMAX LSAME XERBLA ZAXPY
ZCOPY ZDRSCL ZDSCAL ZGECON ZGEES ZGEMM ZGEMV ZGEQP3 ZGEQRF ZGESV
ZGESVD ZGETRF ZGETRI ZGETRS ZHGEQZ ZLACGV ZLACON ZLACP2 ZLACPY ZLADIV
ZLAHQR ZLAIC1 ZLANGE ZLANHS ZLANTR ZLAPMT ZLARF ZLARFG ZLARNV ZLARTG
ZLASCL ZLASET ZLASSQ ZLATRS ZLATZM ZROT ZSCAL ZSWAP ZTRSM ZTZRZF
ZUNGQR ZUNMQR ZUNMRQ ZUNMRZ ZGERC ZGERU DGGHRD
)

echo "include(source_files.cmake)" >>CMakeLists.txt
echo "add_library( slicot_shared SHARED \${SLICOT_SOURCE_FILES} )" >> CMakeLists.txt
echo "target_link_libraries( slicot_shared \${LAPACK_blas_LIBRARIES})" >> CMakeLists.txt
echo "set_target_properties( slicot_shared PROPERTIES OUTPUT_NAME slicot )" >> CMakeLists.txt
echo "install( TARGETS slicot_shared LIBRARY DESTINATION lib )" >> CMakeLists.txt

FFLAGS="${FFLAGS} -O2 -fPIC -ffixed-line-length-none -cpp"

SYMBOL_DEFS=()
if [[ ${nbits} == 64 ]]; then
  for sym in ${LAB_SYMBOLS[@]}; do
    SYMBOL_DEFS+=("-D${sym}=${sym}_64")
  done
  FFLAGS="${FFLAGS} -fdefault-integer-8 ${SYMBOL_DEFS[@]}"
fi

mkdir ../build
cd ../build/
# Above on the fly added CMake code builds shared library with specified LAPACK/BLAS
cmake -DCMAKE_INSTALL_PREFIX=${prefix} \
    -DCMAKE_TOOLCHAIN_FILE=${CMAKE_TARGET_TOOLCHAIN} \
    -DLAPACK_blas_LIBRARIES="-L${libdir} -lblastrampoline" \
    -DCMAKE_BUILD_TYPE=Release \
    -DCMAKE_Fortran_FLAGS="${FFLAGS}" \
    ..
make -j${nproc}
make install

echo "" >>../LICENCE
echo "DGEGS, DLATZM, and ZLATZM are extracted from LAPACK, with the following LICENSE:" >>../LICENCE
echo "" >>../LICENSE
cat ../../lapack-3.8.0/LICENSE >>../LICENSE

install_license ../LICENSE
"""

# These are the platforms we will build for by default, unless further
# platforms are passed in on the command line
platforms = expand_gfortran_versions(supported_platforms())

# The products that we will ensure are always built
products = [
    LibraryProduct("libslicot", :libslicot)
]

# Dependencies that must be installed before this package can be built
dependencies = [
    Dependency(PackageSpec(name="libblastrampoline_jll", uuid="8e850b90-86db-534c-a0d3-1478176c7d93"); compat="3.0.4"),
    Dependency(PackageSpec(name="CompilerSupportLibraries_jll", uuid="e66e0078-7015-5450-92f7-15fbd957f2ae"))
]

# Build the tarballs, and possibly a `build.jl` as well.
build_tarballs(ARGS, name, version, sources, script, platforms, products, dependencies; julia_compat="1.7")


text:
### A Pluto.jl notebook ###
# v0.12.10

using Markdown
using InteractiveUtils

# ╔═╡ e5bec8b4-2823-11eb-2cc8-45152db443b1
using Revise, NBodyLeapFrog, NiLang, BenchmarkTools, DoubleFloats

# ╔═╡ 66f740f0-2824-11eb-2722-bfb0d8f9a3ed
using Plots

# ╔═╡ fc600196-282d-11eb-2cbc-bbaefeb02467
# patch
for OP in [:PlusEq, :Mis]
    function (f::PlusEq{typeof(/)})(x::Double64, y::Double64, z::Double64)
        (x+y/z), y, z
    end
    function (f::MinusEq{typeof(/)})(x::Double64, y::Double64, z::Double64)
        (x-y/z), y, z
    end
end

# ╔═╡ cc384744-2823-11eb-2d52-41d7fd7cca29
planets = Bodies.chunit_day2year.(Bodies.set);

# ╔═╡ 0cf50074-2824-11eb-115e-f39e8ad092d2
function convertelems(::Type{T}, b::Body) where T
    Body(
        V3(T(b.r.x), T(b.r.y), T(b.r.z)),
        V3(T(b.v.x), T(b.v.y), T(b.v.z)),
        T(b.m)
    )
end

# ╔═╡ 2c6337d0-282e-11eb-1c64-259ac62c2cc0
false && let
	T = Double64
	nplanets = length(planets)
	ps = convertelems.(T1, planets)
	f(getfield.(ps, :r), getfield.(ps, :v), getfield.(ps, :m); n = n, dt = 0.01)
end

# ╔═╡ 3fc1ac08-2830-11eb-3f66-5bc4491ae8fe
function simulate(::Type{T1}, f, planets, k::Int) where T1
	nplanets = length(planets)
	n = 1<<k
	ps = convertelems.(T1, planets)
	r, v = f(getfield.(ps, :r), getfield.(ps, :v), getfield.(ps, :m); n = n, dt = 0.01)
	r[:,mod1(n+1,2)]
end

# ╔═╡ fd6c7b96-2823-11eb-1727-3b0cc6d50cf9
function errors(r, rref)
	sum(NBodyLeapFrog.distance.(r, rref))/length(r)
end

# ╔═╡ d37c5db4-2824-11eb-1e9c-9715ce7268eb
logns = 2:12

# ╔═╡ 88b3bb56-2824-11eb-2de1-e37dd1d015f4
let
	r_Double64 = simulate.(Double64, NBodyLeapFrog.i_leapfrog!, Ref(planets), logns)
	plt = plot([], [], yscale=:log10, xscale=:log10, label="")
	for T in [Float32, Float64]
		rt = simulate.(T, NBodyLeapFrog.i_leapfrog_reuse!, Ref(planets), logns)
		rt_clean = simulate.(T, NBodyLeapFrog.i_leapfrog!, Ref(planets), logns)
		errs1 = errors.(r_Double64, rt)
		errs2 = errors.(r_Double64, rt_clean)
		plot!(plt, 1 .<< logns, errs1; label="$T, reusing")
		plot!(plt, 1 .<< logns, errs2; label="$T")
		#errs1 = errors.(r_Double64, r_Double64_clean)
		#plot!(plt, 1 .<< logns, errs1; label="double")
	end
	plt
end

# ╔═╡ Cell order:
# ╠═e5bec8b4-2823-11eb-2cc8-45152db443b1
# ╠═fc600196-282d-11eb-2cbc-bbaefeb02467
# ╠═cc384744-2823-11eb-2d52-41d7fd7cca29
# ╠═0cf50074-2824-11eb-115e-f39e8ad092d2
# ╠═2c6337d0-282e-11eb-1c64-259ac62c2cc0
# ╠═3fc1ac08-2830-11eb-3f66-5bc4491ae8fe
# ╠═fd6c7b96-2823-11eb-1727-3b0cc6d50cf9
# ╠═d37c5db4-2824-11eb-1e9c-9715ce7268eb
# ╠═66f740f0-2824-11eb-2722-bfb0d8f9a3ed
# ╠═88b3bb56-2824-11eb-2de1-e37dd1d015f4


text:
using Metis
using Random
using Test
using SparseArrays
import LightGraphs
import SimpleWeightedGraphs

@testset "Metis.permutation" begin
    Random.seed!(0)
    S = sprand(10, 10, 0.5); S = S + S'; fill(S.nzval, 1)
    perm, iperm = Metis.permutation(S)
    @test isperm(perm)
    @test isperm(iperm)
    @test S == S[perm,perm][iperm,iperm]
end

@testset "Metis.partition" begin
    Random.seed!(0)
    S = sprand(10, 10, 0.5); S = S + S'; fill(S.nzval, 1)
    T = LightGraphs.smallgraph(:tutte)
    for G in (S, T), alg in (:RECURSIVE, :KWAY), nparts in (3, 4)
        partition = Metis.partition(G, nparts, alg = alg)
        @test extrema(partition) == (1, nparts)
        @test all(x -> findfirst(==(x), partition) !== nothing, 1:nparts)
    end
end

@testset "Metis.partition with edge weights" begin
    T = LightGraphs.smallgraph(:tutte)
    G = SimpleWeightedGraphs.SimpleWeightedGraph(LightGraphs.nv(T))
    for i in 1:LightGraphs.nv(T)
        for j in LightGraphs.neighbors(T, i)
            SimpleWeightedGraphs.add_edge!(G, i, j, 1)
        end
    end
    for alg in (:RECURSIVE, :KWAY), nparts in (3, 4)
        unwpartition = Metis.partition(T, nparts, alg = alg)
        partition = Metis.partition(G, nparts, alg = alg)
        @test partition == unwpartition
    end
end

@testset "Metis.separator" begin
    Random.seed!(0)
    S = sprand(10, 10, 0.5); S = S + S'; fill(S.nzval, 1)
    T = LightGraphs.smallgraph(:tutte)
    for G in (S, T)
        parts = Metis.separator(G)
        @test extrema(parts) == (1, 3)
    end
end


text:
using Check3DPackings

@test 1 == 1


text:
###################################################
function partition(X, r_threshold, seed = 1)
###################################################

  # Here we follow the algorithm as described under section 2.2.1
  # in the PhD thesis of Wang, Manifold aligned density estimation

  rg = MersenneTwister(seed)

  N = size(X, 1)
  D = size(X, 2)

  S_centre = spzeros(N)  # for storing indices of data items that become centres
  T_used   = BitArray(undef, N) # for marking data items as used
  fill!(T_used, false)   # mark all data items initially as unused

  # select first data point
  index           = ceil(Int, rand(rg)*N)
  T_used[index]   = 1  # mark it used
  S_centre[index] = 1  # mark it as centre

  while sum(T_used)<N # keep processing while there are unused items

    # randomly choose an unused data item
    unused_indices = findall(T_used .== 0)
    unused_index   = unused_indices[ceil(Int, rand(rg)*length(unused_indices))]

    # mark it immediately as used
    T_used[unused_index] = 1

    # check whether it falls inside the radius of existing centres
    INSIDE = 0
    for centre_index in S_centre.nzind

      # calculate Euclidean distance
      EuclDist = norm(X[centre_index,:] - X[unused_index,:])
      if EuclDist <= r_threshold
        INSIDE = 1
        break
      end

    end

    # if inside is still 0, mark it a centre
    if INSIDE == 0
      S_centre[unused_index] = 1
    end

  end

  return S_centre.nzind

end


text:
@testset "type layouts" begin
	rng = MersenneTwister(0)
	for _test in LAYOUT_TESTS
		for aggregate in ("struct", "union")
			for strategy in ("packed", nothing)
				for kind in (:random, :ones)
					val = kind === :ones ? Core.Intrinsics.sext_int(UInt128, 0xff) : rand(rng, UInt128)
					test = replace(_test, r"^\s*struct" => aggregate)
					test = isnothing(strategy) ? test : replace(test, r"};\s*$" => "} __attribute__(($(strategy)));")

					@debug "$(kind)\n$(test)"
					if VERSION >= v"1.6-" || !occursin("_Alignas", test)
						@test checkC(test, val) == checkJL(test, val)
					else
						@test_skip checkC(test, val) == checkJL(test, val)  # _Alignas is not yet supported using Clang 9 (Julia 1.5)
					end
				end
			end
		end
	end
end


text:
abstract type AbstractSimResult end

struct SimResult{num_ <: Number, num_2 <: Number, time_array <: AbstractArray{num_}, sim_matrix <: AbstractMatrix{num_2}} <: AbstractSimResult
    times::time_array
    sim_result::sim_matrix
    initial_value::num_2
end


text:
using DataFrames
using Resample
using StableRNGs: StableRNG
using Test


text:
using LinearAlgebra
using SparseArrays
using Test
using SmithNormalForm

@testset "Bezout" begin
    a = 12
    b = 42

    s,t,g = SmithNormalForm.bezout(a,b)
    @test s*a + t*b == g

    s,t,g = SmithNormalForm.bezout(b,a)
    @test s*b + t*a == g
end

@testset "Smith Normal Form" begin
    M=[ 1  0  0  0  0  0 ;
        0  0  0  0  0  0 ;
       -1  0  1  0  0  1 ;
        0 -1  0 -1  0  0 ;
        0  0  0  1  1 -1 ;
        0  1 -1  0 -1  0 ]

    P, Q, A, Pinv, Qinv = snf(M)
    @test !issparse(A)
    @test P*A*Q == M
    @test inv(P) == Pinv
    @test inv(Q) == Qinv
    @testset "dense diag" for i in 1:4
        @test A[i,i] == -1
    end

    P, Q, A, Pinv, Qinv = snf(dropzeros(sparse(M)))
    @test issparse(A)
    @test P*A*Q == M
    @test inv(collect(P)) == Pinv
    @test inv(collect(Q)) == Qinv
    @testset "sparse diag" for i in 1:4
        @test A[i,i] == -1
    end

    P, Q, A, Pinv, Qinv = snf(sparse(M), inverse=false)
    @test issparse(A)
    @test iszero(Pinv)
    @test iszero(Qinv)
    @test P*A*Q == M
    @testset "sparse diag" for i in 1:4
        @test A[i,i] == -1
    end

    # Factorization
    M =[22  30  64  93  36;
        45  42  22  11  67;
        21   1  35  45  42]
    n, m = size(M)
    F = smith(M)
    @test !issparse(F.SNF)
    @test size(F.S) == (n,n)
    @test size(F.T) == (m,m)
    @test size(F.Sinv) == (n,n)
    @test size(F.Tinv) == (m,m)
    @test F.S*diagm(F)*F.T == M
    @test F.S*F.Sinv == Matrix{eltype(F)}(I, n, n)
    @test F.T*F.Tinv == Matrix{eltype(F)}(I, m, m)

    F = smith(sparse(M), inverse=false)
    @test issparse(F.SNF)
    @test size(F.S) == (n,n)
    @test size(F.T) == (m,m)
    @test F.S*diagm(F)*F.T == M
    @test iszero(F.Sinv)
    @test iszero(F.Tinv)
end


text:
using Test
using Mimi

include("../src/main_model.jl")

updatetestdata = false

function test_page_model()
    m = Model()

    set_dimension!(m, :time, [2020, 2030, 2040, 2050, 2075, 2100, 2150, 2200, 2250, 2300])
    set_dimension!(m, :region, ["EU", "USA", "OECD", "USSR", "China", "SEAsia", "Africa", "LatAmerica"])

    return m
end

function get_scenario(ii)
    if ii == 1
        return "ndcs", "NDCs", true, false
    end
    if ii == 2
        return "2c-saf", "2 degC Target", true, true
    end
end

@testset "mimi-page-2020" begin

    include("test_climatemodel.jl")
    include("test_AbatementCosts.jl")
    include("test_AdaptationCosts.jl")
    include("test_CH4cycle.jl")
    include("test_CH4emissions.jl")
    include("test_CH4forcing.jl")
    include("test_ClimateTemperature.jl")
    include("test_CO2cycle.jl")
    include("test_CO2emissions.jl")
    include("test_CO2forcing.jl")
    include("test_Discontinuity.jl")
    include("test_EquityWeighting.jl")
    include("test_GDP.jl")
    include("test_LGcycle.jl")
    include("test_LGemissions.jl")
    include("test_LGforcing.jl")
    include("test_loadparameters.jl")
    include("test_mainmodel.jl")
    include("test_mainmodel_noperm.jl")
    ## include("test_MarketDamages.jl") # missing data
    include("test_MarketDamagesBurke.jl")
    include("test_N2Ocycle.jl")
    include("test_N2Oemissions.jl")
    include("test_N2Oforcing.jl")
    include("test_NonMarketDamages.jl")
    include("test_Population.jl")
    include("test_SeaLevelRise.jl")
    include("test_SLRDamages.jl")
    include("test_SulphateForcing.jl")
    include("test_TotalAbatementCosts.jl")
    include("test_TotalAdaptationCosts.jl")
    include("test_TotalCosts.jl")
    include("test_TotalForcing.jl")
    include("test_Permafrost.jl")
    ## include("test_mcs.jl") # missing data
    include("test_scenarios_mcs.jl")
    include("test_scenarios.jl")
    include("test_standard_api.jl")
    ## include("test_scc.jl") # Takes very long to run.
    include("contrib/test_taxeffect.jl")
end


text:
using ACE, BenchmarkTools

pool = ACE.ArrayCache{Float64}()

##

function runn(x, N, len)
   @assert length(x) >= len
   fill!(x, 0)
   for n = 1:N, i = 1:len
      x[i] += i/n
   end
   return nothing
end

x = rand(1000)
@btime runn($x, 1000, 1000)

##

function runn2(cache, N, len)
   x = ACE.acquire!(cache, len)
   fill!(x.A, 0)
   for n = 1:N, i = 1:len
      x[i] += i/n
   end
   ACE.release!(x)
   return nothing
end

@btime runn2($pool, 1000, 1000)

##

N = 1000

for len in [1000, 300, 100, 30, 10]
   N = 1 # (1000 ÷ len) * 2
   println("len = $len")
   x = zeros(len)
   print(" preallocate: ")
   @btime runn($x, $N, $len)
   print("      cached: ")
   @btime runn2($pool, $N, $len)
   print("garbage coll: ")
   @btime runn(zeros($len), $N, $len)
end


text:
struct MinorPieceTrap
    pawnmask::Bitboard
    minormask::Bitboard
end

function init_knight_traps()
    # Trap 1
    # White knight trapped on h8, with pawn on f7
    pawnmask = Bitboard(LABEL_TO_SQUARE["f7"])
    knightmask = Bitboard(LABEL_TO_SQUARE["h8"])
    trap_1 = MinorPieceTrap(pawnmask, knightmask)

    # Trap 2
    # White knight trapped on h8, with pawn on h7
    pawnmask = Bitboard(LABEL_TO_SQUARE["h7"])
    knightmask = Bitboard(LABEL_TO_SQUARE["h8"])
    trap_2 = MinorPieceTrap(pawnmask, knightmask)

    # Trap 3
    # White knight trapped on a8, with pawn on c7
    pawnmask = Bitboard(LABEL_TO_SQUARE["c7"])
    knightmask = Bitboard(LABEL_TO_SQUARE["a8"])
    trap_3 = MinorPieceTrap(pawnmask, knightmask)

    # Trap 4
    # White knight trapped on a8, with pawn on a7
    pawnmask = Bitboard(LABEL_TO_SQUARE["a7"])
    knightmask = Bitboard(LABEL_TO_SQUARE["a8"])
    trap_4 = MinorPieceTrap(pawnmask, knightmask)

    # Trap 5
    # Black knight trapped on h1, with pawn on f2
    pawnmask = Bitboard(LABEL_TO_SQUARE["f2"])
    knightmask = Bitboard(LABEL_TO_SQUARE["h1"])
    trap_5 = MinorPieceTrap(pawnmask, knightmask)

    # Trap 6
    # Black knight trapped on h1, with pawn on h2
    pawnmask = Bitboard(LABEL_TO_SQUARE["h2"])
    knightmask = Bitboard(LABEL_TO_SQUARE["h1"])
    trap_6 = MinorPieceTrap(pawnmask, knightmask)

    # Trap 7
    # Black knight trapped on a1, with pawn on c2
    pawnmask = Bitboard(LABEL_TO_SQUARE["c2"])
    knightmask = Bitboard(LABEL_TO_SQUARE["a1"])
    trap_7 = MinorPieceTrap(pawnmask, knightmask)

    # Trap 8
    # Black knight trapped on a1, with pawn on a2
    pawnmask = Bitboard(LABEL_TO_SQUARE["a2"])
    knightmask = Bitboard(LABEL_TO_SQUARE["a1"])
    trap_8 = MinorPieceTrap(pawnmask, knightmask)

    white_trapped = @SVector [trap_1, trap_2, trap_3, trap_4]
    black_trapped = @SVector [trap_5, trap_6, trap_7, trap_8]
    knight_trap_patterns= @SVector [white_trapped, black_trapped]
end

function init_bishop_traps()
    # Trap 1
    # White bishop trapped on A7 or B8 by pawns on B6 and C7
    pawnmask = Bitboard(LABEL_TO_SQUARE["b6"]) | Bitboard(LABEL_TO_SQUARE["c7"])
    bishopmask = Bitboard(LABEL_TO_SQUARE["a7"]) | Bitboard(LABEL_TO_SQUARE["b8"])
    trap_1 = MinorPieceTrap(pawnmask, bishopmask)

    # Trap 2
    # White bishop trapped on h7 or G8, by pawns on g6 and f7
    pawnmask = Bitboard(LABEL_TO_SQUARE["g6"]) | Bitboard(LABEL_TO_SQUARE["f7"])
    bishopmask = Bitboard(LABEL_TO_SQUARE["h7"]) | Bitboard(LABEL_TO_SQUARE["g8"])
    trap_2 = MinorPieceTrap(pawnmask, bishopmask)

    # Trap 3
    # White bishop trapped on a8, c8, A6, B7, by pawns on B5, c6, d7
    pawnmask = Bitboard(LABEL_TO_SQUARE["b5"]) | Bitboard(LABEL_TO_SQUARE["c6"]) | Bitboard(LABEL_TO_SQUARE["d7"])
    bishopmask = Bitboard(LABEL_TO_SQUARE["a8"]) | Bitboard(LABEL_TO_SQUARE["c8"]) | Bitboard(LABEL_TO_SQUARE["a6"]) | Bitboard(LABEL_TO_SQUARE["b7"])
    trap_3 = MinorPieceTrap(pawnmask, bishopmask)

    # Trap 4
    # White bishop trapped on h8, f8, H6, g7, by pawns on g5, F6, E7
    pawnmask = Bitboard(LABEL_TO_SQUARE["g5"]) | Bitboard(LABEL_TO_SQUARE["f6"]) | Bitboard(LABEL_TO_SQUARE["e7"])
    bishopmask = Bitboard(LABEL_TO_SQUARE["h8"]) | Bitboard(LABEL_TO_SQUARE["f8"]) | Bitboard(LABEL_TO_SQUARE["h6"]) | Bitboard(LABEL_TO_SQUARE["g7"])
    trap_4 = MinorPieceTrap(pawnmask, bishopmask)

    # Trap 5
    # Black bishop trapped on A2 or B1 by pawns on B3 and C2
    pawnmask = Bitboard(LABEL_TO_SQUARE["b3"]) | Bitboard(LABEL_TO_SQUARE["c2"])
    bishopmask = Bitboard(LABEL_TO_SQUARE["a2"]) | Bitboard(LABEL_TO_SQUARE["b1"])
    trap_5 = MinorPieceTrap(pawnmask, bishopmask)

    # Trap 6
    # White bishop trapped on H2 or g1, by pawns on g3 and F2
    pawnmask = Bitboard(LABEL_TO_SQUARE["g3"]) | Bitboard(LABEL_TO_SQUARE["f2"])
    bishopmask = Bitboard(LABEL_TO_SQUARE["h2"]) | Bitboard(LABEL_TO_SQUARE["g1"])
    trap_6 = MinorPieceTrap(pawnmask, bishopmask)

    # Trap 7
    # White bishop trapped on a1, C1, A3, B2, by pawns on B4, C3, D2
    pawnmask = Bitboard(LABEL_TO_SQUARE["b4"]) | Bitboard(LABEL_TO_SQUARE["c3"]) | Bitboard(LABEL_TO_SQUARE["d2"])
    bishopmask = Bitboard(LABEL_TO_SQUARE["a1"]) | Bitboard(LABEL_TO_SQUARE["c1"]) | Bitboard(LABEL_TO_SQUARE["a3"]) | Bitboard(LABEL_TO_SQUARE["b2"])
    trap_7 = MinorPieceTrap(pawnmask, bishopmask)

    # Trap 8
    # White bishop trapped on H1, F1, H3, g2, by pawns on g4, F3, E2
    pawnmask = Bitboard(LABEL_TO_SQUARE["g4"]) | Bitboard(LABEL_TO_SQUARE["f3"]) | Bitboard(LABEL_TO_SQUARE["e2"])
    bishopmask = Bitboard(LABEL_TO_SQUARE["h1"]) | Bitboard(LABEL_TO_SQUARE["f1"]) | Bitboard(LABEL_TO_SQUARE["h3"]) | Bitboard(LABEL_TO_SQUARE["g2"])
    trap_8 = MinorPieceTrap(pawnmask, bishopmask)

    white_trapped = @SVector [trap_1, trap_2, trap_3, trap_4]
    black_trapped = @SVector [trap_5, trap_6, trap_7, trap_8]
    bishop_trap_patterns= @SVector [white_trapped, black_trapped]
end

function init_neighbour_file_masks()
    mask = [EMPTY for i in 1:8]
    for file in 1:8
        if file == 1
            neighbour = FILE[1] | FILE[2]
        elseif file == 8
            neighbour = FILE[7] | FILE[8]
        else
            neighbour = FILE[file + 1] | FILE[file] | FILE[file - 1]
        end
        mask[file] |= neighbour
    end
    mask
end

const NEIGHBOUR_FILE_MASKS = SVector{8}(init_neighbour_file_masks())

function init_passed_pawn_masks()
    mask = [[EMPTY for i in 1:64] for j in 1:2]
    @inbounds for sqr in 1:64
        for colour in 1:2
            rank = rankof(sqr)
            file = fileof(sqr)
            if colour == 1
                @simd for i in (rank+1):8
                    mask[colour][sqr] |= RANK[i] & NEIGHBOUR_FILE_MASKS[file]
                end
            else
                @simd for i in 1:(rank-1)
                    mask[colour][sqr] |= RANK[i] & NEIGHBOUR_FILE_MASKS[file]
                end
            end
        end
    end
    mask
end

function init_connected_pawn_masks()
    mask = [[EMPTY for i in 1:64] for j in 1:2]
    for sqr in 9:56
        mask[1][sqr] = pawnAttacks(BLACK, sqr) | pawnAttacks(BLACK, sqr + 8)
        mask[2][sqr] = pawnAttacks(WHITE, sqr) | pawnAttacks(WHITE, sqr - 8)
    end
    mask
end

const KNIGHT_TRAP_PATTERNS = init_knight_traps()
const BISHOP_TRAP_PATTERNS = init_bishop_traps()
const PASSED_PAWN_MASKS = SArray{Tuple{2}, SArray{Tuple{64}, Bitboard, 1, 64}, 1, 2}(init_passed_pawn_masks())
const CONNECTED_PAWN_MASKS = SArray{Tuple{2}, SArray{Tuple{64}, Bitboard, 1, 64}, 1, 2}(init_connected_pawn_masks())

max_stars_repo_name: J-Revell/Astellarn

max_issues_repo_name: J-Revell/Astellarn

max_forks_repo_name: J-Revell/Astellarn


text:
##### KineticLaw / MassActionOrder* #####

abstract type KineticLaw end

struct MassAction  <: KineticLaw end

##### ReactionStruct #####

struct ReactionStruct{MA <: KineticLaw}
    reactants :: Vector{Tuple{Int,Int}}
    net_change :: Vector{Tuple{Int,Int}}
    paramidx :: Int

    function ReactionStruct(law::MA, reactants, net_change, paramidx) where MA <: KineticLaw
        num_reactants = length(reactants)
        order = num_reactants > 0 ? sum(c for (_, c) in reactants) : 0

        !is_compatible_law(law, order, num_reactants) && throw(ArgumentError("reaction is not compatible with $(law) law"))

        return new{MA}(reactants, net_change, paramidx)
    end
end

function Base.show(io::IO, r::ReactionStruct{law}) where law <: KineticLaw
    # formula = r.formula
    # lhs = formula.args[1]
    # rhs = formula.args[2]
    #
    # print(io, string("  ", r.name, ": ", lhs, " -> ", rhs))
    print(io, "ReactionStruct")
end

is_compatible_law(::MassAction, order, num_reactants) = true

function execute_jump!(x, r::ReactionStruct)
    net_change = r.net_change

    for v in net_change
        k, v_k = v
        @inbounds @fastmath x[k] += v_k
    end
end

##### rate functions for stochastic mass action kinetics #####

@inline @inbounds function rate(r::ReactionStruct{MassAction}, x, p)
    i = r.paramidx
    total_rate = p[i]   # accumulates terms of the form (x_k - (j-1))
    prefactor = one(total_rate) # accumulates constants 1 / m_k!

    for (k, m) in r.reactants
        for j in 1:m
            total_rate *= (x[k] - (j-1))
            prefactor *= j
        end
    end

    total_rate /= prefactor

    return total_rate
end

@inline @inbounds function rate_derivative(r::ReactionStruct{MassAction}, x, p, i)
    total_rate = p[r.paramidx]  # accumulates terms of the form (x_k - (j-1))
    prefactor = one(total_rate) # accumulates constants 1 / m_k!
    deriv_term = zero(total_rate)

    for (k, m) in r.reactants
        for j in 1:m
            total_rate *= (x[k] - (j-1))
            prefactor *= j

            if k == i
                deriv_term += 1 / (x[k] - (j-1))
            end
        end
    end

    total_rate = total_rate / prefactor * deriv_term

    return total_rate
end

##### ReactionSystem #####

struct ReactionSystem{R,DG<:DependencyGraph}
    reactions::Vector{ReactionStruct{MassAction}}
    rxn_rates::R
    dep_graph::DG
    spc_graph::DG
    rxn_graph::DG
end

function ReactionSystem(model::Network)
    num_reactions = number_reactions(model)

    reactions = Vector{ReactionStruct{MassAction}}(undef, num_reactions)
    rxn_rates = zeros(num_reactions)

    build_reactions!(reactions, rxn_rates, model)

    dep_graph = rxnrxn_depgraph(DGLazy(), model)
    spc_graph = spcrxn_depgraph(DGLazy(), model)
    rxn_graph = rxnspc_depgraph(DGLazy(), model)

    return ReactionSystem(reactions, rxn_rates, dep_graph, spc_graph, rxn_graph)
end

function Base.summary(io::IO, r::ReactionSystem)
    print(io, "Well-Mixed Reaction System")
end

function Base.show(io::IO, r::ReactionSystem)
    summary(io, r)
    for (j, reaction) in enumerate(r.reactions)
        print(io, "\n", reaction)
    end
end

##### convenience functions #####

@inline function execute_jump!(x, rxn::ReactionSystem, j)
    @inbounds execute_jump!(x, rxn.reactions[j])
end

@inline @inbounds rate(rxn::ReactionSystem, x, j) = rate(rxn.reactions[j], x, rxn.rxn_rates)

@inline @inbounds function rate_derivative(rxn, x, i, j)
    rate_derivative(rxn.reactions[j], x, rxn.rxn_rates, i)
end

function netstoichiometry(rxn::ReactionSystem, num_species, num_reactions)
    V = zeros(Int, num_species, num_reactions)
    reactions = rxn.reactions
    for j in eachindex(reactions)
        r = reactions[j]
        for (k, v) in r.net_change
            V[k, j] = v
        end
    end
    return V
end

##### helper functions for building a ReactionSystem #####

function build_reactions!(rxn_set, rxn_rates, model)
    species   = species_list(model)
    reactions = reaction_list(model)

    indexmap = OrderedDict(key => i for (i, key) in enumerate(keys(species)))

    for (j, r) in enumerate(values(reactions))
        name = r.identifier
        formula = r.origex
        reactants = r.reactants
        products  = r.products
        rxn_rate  = r.rate

        rtuples = Tuple{Int,Int}[(indexmap[s], c) for (s, c) in reactants]
        net_change = Tuple{Int,Int}[]

        for s in keys(species)
            change = get(products, s, 0) - get(reactants, s, 0)
            if change != 0
                push!(net_change, (indexmap[s], change))
            end
        end
        klaw = get_kinetic_law(rtuples)

        rxn_set[j] = ReactionStruct(klaw, rtuples, net_change, j)
        rxn_rates[j] = rxn_rate
    end

    return rxn_set
end

function get_kinetic_law(rtuples)
    # num_reactants = length(rtuples)
    # order = num_reactants > 0 ? sum(c for (_, c) in rtuples) : 0
    return MassAction()
end

##### extras #####

function execute_leap!(state, stoichiometry, number_jumps)
    state += stoichiometry * number_jumps
end


text:
@testset "Test printing of the system and components" begin
    sys = create_system_data(with_time_series=true, time_series_in_memory=true)
    io = IOBuffer()
    show(io, "text/plain", sys)
    text = String(take!(io))
    @test occursin("TestComponent", text)
    @test occursin("Time Series Summary", text)
end

@testset "Test show_component_tables" begin
    sys = create_system_data(with_time_series=true, time_series_in_memory=true)
    io = IOBuffer()
    IS.show_components(io, sys.components, IS.TestComponent)
    @test occursin("TestComponent", String(take!(io)))

    IS.show_components(io, sys.components, IS.TestComponent, [:val])
    text = String(take!(io))
    @test occursin("TestComponent", text)
    @test occursin("val", text)

    IS.show_components(io, sys.components, IS.TestComponent, Dict("val" => x -> x.val * 10))
    text = String(take!(io))
    @test occursin("TestComponent", text)
    @test occursin("val", text)
end