text:
cd(@__DIR__)

using Pkg; Pkg.activate(".."); Pkg.instantiate()

const nthreads = 50
using Distributed
using SharedArrays

addprocs(nthreads)

@everywhere mean(a) = sum(a)/length(a)

@everywhere geomean(a) = exp(mean(log.(a)))

@everywhere include("SequenceAlignment/SequenceAlignment.jl")

using CSV, DataFrames, LinearAlgebra
using SharedArrays
@everywhere using .SequenceAlignment
#@everywhere using StatsBase

@everywhere function ldnStar(w1::Union{Missing,String},w2::Union{Missing,String})
    if ismissing(w1) || ismissing(w2)
        return missing
    end
    v1 = split(w1,"-")
    v2 = split(w2,"-")
    scores = Vector{Float64}(undef, length(v1)*length(v2))
    counter = 1
    for x in v1
        for y in v2
            @inbounds scores[counter] = ldn(x,y)
            counter += 1
        end
    end
    minimum(scores)
end

@everywhere begin
    using CSV
    using DataFrames
    d = CSV.File("../data/asjp19wide.csv") |> DataFrame
    minSim = -sqrt(40)
    maxSim = (log(40*(39)+1)-1)*sqrt(40)
end;

languages = string.(d.longname);

# concepts = names(d)[2:end]

@everywhere asjpMtx = Matrix(d[:,2:end]);

@everywhere function dercLDN(
    i::Int,
    j::Int,
    asjpMtx::Matrix{Union{Missing, String}} = asjpMtx,
    maxSim::Float64 = maxSim,
    minSim::Float64 = minSim,
)
    defined1 = findall(.!ismissing.(asjpMtx[i, :]))
    defined2 = findall(.!ismissing.(asjpMtx[j, :]))
    definedBoth = intersect(defined1, defined2)
    nBoth = length(definedBoth)
    dg = Vector{Float64}(undef, nBoth)
    for (k, c) in enumerate(definedBoth)
        dg[k] = ldnStar(asjpMtx[i, c], asjpMtx[j, c])
    end
    nOffD = length(defined1) * length(defined2) - nBoth
    offDg = Vector{Float64}(undef, nOffD)
    counter = 1
    for k1 in defined1
        w1 = asjpMtx[i, k1]
        for k2 in defined2
            if k1 != k2
                w2 = asjpMtx[j, k2]
                @inbounds offDg[counter] = ldnStar(w1, w2)
                counter += 1
            end
        end
    end
    ranks = Vector{Float64}(undef, nBoth)
    for k = 1:nBoth
        @inbounds x = dg[k]
        @inbounds ranks[k] = geomean(1 .+ (sum(offDg .< x):sum(offDg .<= x)))
    end
    stc = mean(-log.(ranks ./ (1 + nOffD)))
    sim = (stc - 1) * sqrt(nBoth)
    (maxSim - sim) / (maxSim - minSim)
end

# ger = "IE.GERMANIC.STANDARD_GERMAN"
#
# eng = "IE.GERMANIC.ENGLISH"
#
# @show dercLDN(ger, eng)
#
# @time gerDsts = [dercLDN(ger, l) for l in languages]
#
# @time begin
#     gerDistsT = Vector{Union{Missing, Float64}}(undef, length(languages))
#     Threads.@threads for i in 1:length(gerDistsT)
#         gerDistsT[i] = dercLDN(ger, languages[i])
#     end
# end

lpairs = [
    [i,j] for (i, l1) in enumerate(languages)
    for (j, l2) in enumerate(languages) if i < j
];

@everywhere function f(p)
    i,j = p
    dercLDN(i,j)
end

#---

ldists = pmap(f, lpairs);

levDistsLong = DataFrame(l1=languages[hcat(lpairs...)[1,:]],
                         l2=languages[hcat(lpairs...)[2,:]], ldist=ldists)

levDists = unstack(levDistsLong, :l1, :l2, :ldist)

CSV.write("../data/levenshteinLanguageDistances.csv", levDists)


text:
using LinearAlgebra

_normalize(P, normalized) = normalized ? P : normalize([
    P[1, 1] P[1, 2]
    P[2, 1] P[2, 2]
], 1)

_papb(P) = P[1, 1] + P[1, 2], P[1, 1] + P[2, 1]

_D_max(P, D) = let (pa, pb) = _papb(P)
    D < 0 ?
    max(-pa * pb, -(1 - pa) * (1 - pb)) :
    min(pa * (1 - pb), (1 - pa) * pb)
end

"""
Coefficient of Linkage Disequilibrium

`P` is a matrix for the (not necessary normalized) frequency of haplotypes for two loci A and B.
"""
function D(P; normalized = false)
    P = _normalize(P, normalized)
    pa, pb = _papb(P)
    P[1, 1] - pa * pb
end

"""
A normalized measure of LD.
"""
function D_prime(P; normalized = false)
    P = _normalize(P, normalized)
    vD = D(P; normalized = true)
    vD / _D_max(P, D)
end

"""
The correlation coefficient
"""
function r²(P; normalized = false)
    P = _normalize(P, normalized)
    pa, pb = _papb(P)
    vD = D(P; normalized = true)
    vD ^ 2 / (pa * (1 - pa) * pb * (1 - pb))
end

"""
`snp1` and `snp2` is n-element boolean arrays indicating alleles at two SNPs within n haplotypes.
"""
function r²(snp1::AbstractVector{Bool}, snp2::AbstractVector{Bool})
    P = zeros(2, 2)
    n = length(snp1)
    for i in 1:n
        P[2 - snp1[i], 2 - snp2[i]] += 1
    end
    # P includes numbers of SNP pairs that has following values
    # [(1,1) (1,0)
    #  (0,1) (0,0)]
    r²(P)
end

"""
`snps` is a nxm boolean matrix indicating alleles at m SNPs within n haplotypes.

Return:

`R²`: A matrix of `r²` values for each pair of SNPs.
"""
function r²(snps::Matrix{Bool})
    n, m = size(snps)
    R² = zeros(m, m)
    for i in 1:m-1
        R²[i, i] = 1.0
        for j in i + 1:m
            R²[i, j] = R²[j, i] = r²(snps[:, i], snps[:, j])
        end
    end
    R²
end

ext: jl


text:
type dpcf
    bfull::Array{Float64,1}
    bhalf::Array{Float64,1}
    afull::Array{Float64,1}
    ahalf::Array{Float64,1}
end

function DampCoeff(medium::Union{elastic2d,acoustic2d,elastic3d,acoustic3d})
    ext = medium.ext
    m = 0.25
    n = 0.75
    ll = 4

    # R is a parameter depend on the thickness of boundaries
    if ext == 5
       R = 0.01
    elseif ext == 10
       R = 0.001
    elseif ext == 20
       R = 0.0001
    else
       error("unsupported damping layers, choose from 5, 10 or 20")
    end
    a_max = 10*pi
    tmp_a = linspace(0,a_max,ext)
    dWFfull = zeros(ext)
    dWFhalf = zeros(ext)
    maxv = maximum(medium.pvel)
    for i = 1 : ext
        dWFfull[i] = -maxv/ext * log(R) * (m*(ext+1-i)/ext + n*((ext+1-i )/ext)^ll)
        dWFhalf[i] = -maxv/ext * log(R) * (m*(ext+0.5-i)/ext + n*((ext+0.5-i)/ext)^ll)
    end
    bhalf = Float64.(exp.(-(dWFhalf.+tmp_a).*medium.dt))
    bfull = Float64.(exp.(-(dWFfull.+tmp_a).*medium.dt))
    ahalf = Float64.(dWFhalf./(dWFhalf.+tmp_a).*(bhalf.-1))
    afull = Float64.(dWFfull./(dWFfull.+tmp_a).*(bfull.-1))
    return dpcf(bfull,bhalf,afull,ahalf)
end

            #==========================#
            #=== Obsorbing Boundary ===#
            #==========================#
#=== elastic 2d ===#
function BD(nwf::nelwf2d, medium::elastic2d)

    ext = medium.ext
    iflag = medium.iflag
    dpcf = DampCoeff(medium)

    # Vx
    indexZ = nwf.BDnvx[1]
    indexX = nwf.BDnvx[2] - nwf.nvx[2]
    PVxBTxx = zeros(Float64,indexZ,indexX)

    indexZ = nwf.BDnvx[1] - nwf.nvx[1]
    indexX = nwf.BDnvx[2]
    PVxBTxz = zeros(Float64,indexZ,indexX)

    # Vz
    indexZ = nwf.BDnvz[1] - nwf.nvz[1]
    indexX = nwf.BDnvz[2]
    PVzBTzz = zeros(Float64,indexZ,indexX)

    indexZ = nwf.BDnvz[1]
    indexX = nwf.BDnvz[2] - nwf.nvz[2]
    PVzBTxz = zeros(Float64,indexZ,indexX)

    # Tzz & Txx
    indexZ = nwf.BDntpp[1] - nwf.ntpp[1]
    indexX = nwf.BDntpp[2]
    PTzzBVz = zeros(Float64,indexZ,indexX)
    PTxxBVz = zeros(Float64,indexZ,indexX)

    indexZ = nwf.BDntpp[1]
    indexX = nwf.BDntpp[2] - nwf.ntpp[2]
    PTzzBVx = zeros(Float64,indexZ,indexX)
    PTxxBVx = zeros(Float64,indexZ,indexX)

    # Txz
    indexZ = nwf.BDntxz[1] - nwf.ntxz[1]
    indexX = nwf.BDntxz[2]
    PTxzBVx = zeros(Float64,indexZ,indexX)

    indexZ = nwf.BDntxz[1]
    indexX = nwf.BDntxz[2] - nwf.ntxz[2]
    PTxzBVz = zeros(Float64,indexZ,indexX)

    return elbd2d(Float64.(dpcf.bfull), Float64.(dpcf.bhalf), Float64.(dpcf.afull), Float64.(dpcf.ahalf),
                      PVxBTxx, PVxBTxz,
                      PVzBTzz, PVzBTxz,
                      PTxxBVx, PTxxBVz,
                      PTzzBVx, PTzzBVz,
                      PTxzBVx, PTxzBVz)
end

#=== acoustic 2d ===#
function BD(nwf::nacwf2d, medium::acoustic2d)
    ext = medium.ext
    iflag = medium.iflag
    dpcf = DampCoeff(medium)

    # Vx
    indexZ = nwf.BDnvx[1]
    indexX = nwf.BDnvx[2] - nwf.nvx[2]
    PVxBTpp = zeros(Float64,indexZ,indexX)

    # Vz
    indexZ = nwf.BDnvz[1] - nwf.nvz[1]
    indexX = nwf.BDnvz[2]
    PVzBTpp = zeros(Float64,indexZ,indexX)

    # Tzz & Txx
    indexZ = nwf.BDntpp[1] - nwf.ntpp[1]
    indexX = nwf.BDntpp[2]
    PTppBVz = zeros(Float64,indexZ,indexX)

    indexZ = nwf.BDntpp[1]
    indexX = nwf.BDntpp[2] - nwf.ntpp[2]
    PTppBVx = zeros(Float64,indexZ,indexX)

    return acbd2d(Float64.(dpcf.bfull), Float64.(dpcf.bhalf), Float64.(dpcf.afull), Float64.(dpcf.ahalf),
                      PVxBTpp, PVzBTpp,
                      PTppBVx, PTppBVz)
end

#=== elastic 3d ===#
function BD(nwf::nelwf3d, medium::elastic3d)

        ext = medium.ext
        iflag = medium.iflag
        dpcf = DampCoeff(medium)

        # Vx
        indexZ = nwf.BDnvx[1]
        indexX = nwf.BDnvx[2] - nwf.nvx[2]
        indexY = nwf.BDnvx[3]
        PVxBTxx = zeros(Float64,indexZ,indexX,indexY)

        indexZ = nwf.BDnvx[1] - nwf.nvx[1]
        indexX = nwf.BDnvx[2]
        indexY = nwf.BDnvx[3]
        PVxBTxz = zeros(Float64,indexZ,indexX,indexY)

        indexZ = nwf.BDnvx[1]
        indexX = nwf.BDnvx[2]
        indexY = nwf.BDnvx[3] - nwf.nvx[3]
        PVxBTxy = zeros(Float64,indexZ,indexX,indexY)

        # Vy
        indexZ = nwf.BDnvy[1]
        indexX = nwf.BDnvy[2] - nwf.nvy[2]
        indexY = nwf.BDnvy[3]
        PVyBTxy = zeros(Float64,indexZ,indexX,indexY)

        indexZ = nwf.BDnvy[1] - nwf.nvy[1]
        indexX = nwf.BDnvy[2]
        indexY = nwf.BDnvy[3]
        PVyBTyz = zeros(Float64,indexZ,indexX,indexY)

        indexZ = nwf.BDnvy[1]
        indexX = nwf.BDnvy[2]
        indexY = nwf.BDnvy[3] - nwf.nvy[3]
        PVyBTyy = zeros(Float64,indexZ,indexX,indexY)

        # Vz
        indexZ = nwf.BDnvz[1] - nwf.nvz[1]
        indexX = nwf.BDnvz[2]
        indexY = nwf.BDnvz[3]
        PVzBTzz = zeros(Float64,indexZ,indexX,indexY)

        indexZ = nwf.BDnvz[1]
        indexX = nwf.BDnvz[2] - nwf.nvz[2]
        indexY = nwf.BDnvz[3]
        PVzBTxz = zeros(Float64,indexZ,indexX,indexY)

        indexZ = nwf.BDnvz[1]
        indexX = nwf.BDnvz[2]
        indexY = nwf.BDnvz[3] - nwf.nvz[3]
        PVzBTyz = zeros(Float64,indexZ,indexX,indexY)

        # Tzz & Txx & Tyy
        indexZ = nwf.BDntpp[1] - nwf.ntpp[1]
        indexX = nwf.BDntpp[2]
        indexY = nwf.BDntpp[3]
        PTzzBVz = zeros(Float64,indexZ,indexX,indexY)
        PTxxBVz = zeros(Float64,indexZ,indexX,indexY)
        PTyyBVz = zeros(Float64,indexZ,indexX,indexY)

        indexZ = nwf.BDntpp[1]
        indexX = nwf.BDntpp[2] - nwf.ntpp[2]
        indexY = nwf.BDntpp[3]
        PTzzBVx = zeros(Float64,indexZ,indexX,indexY)
        PTxxBVx = zeros(Float64,indexZ,indexX,indexY)
        PTyyBVx = zeros(Float64,indexZ,indexX,indexY)

        indexZ = nwf.BDntpp[1]
        indexX = nwf.BDntpp[2]
        indexY = nwf.BDntpp[3] - nwf.ntpp[3]
        PTzzBVy = zeros(Float64,indexZ,indexX,indexY)
        PTxxBVy = zeros(Float64,indexZ,indexX,indexY)
        PTyyBVy = zeros(Float64,indexZ,indexX,indexY)

        # Txz
        indexZ = nwf.BDntxz[1] - nwf.ntxz[1]
        indexX = nwf.BDntxz[2]
        indexY = nwf.BDntxz[3]
        PTxzBVx = zeros(Float64,indexZ,indexX,indexY)

        indexZ = nwf.BDntxz[1]
        indexX = nwf.BDntxz[2] - nwf.ntxz[2]
        indexY = nwf.BDntxz[3]
        PTxzBVz = zeros(Float64,indexZ,indexX,indexY)

        # Tyz
        indexZ = nwf.BDntyz[1] - nwf.ntyz[1]
        indexX = nwf.BDntyz[2]
        indexY = nwf.BDntyz[3]
        PTyzBVy = zeros(Float64,indexZ,indexX,indexY)

        indexZ = nwf.BDntyz[1]
        indexX = nwf.BDntyz[2]
        indexY = nwf.BDntyz[3] - nwf.ntyz[3]
        PTyzBVz = zeros(Float64,indexZ,indexX,indexY)

        # Txy
        indexZ = nwf.BDntxy[1]
        indexX = nwf.BDntxy[2] - nwf.ntxy[2]
        indexY = nwf.BDntxy[3]
        PTxyBVy = zeros(Float64,indexZ,indexX,indexY)

        indexZ = nwf.BDntxy[1]
        indexX = nwf.BDntxy[2]
        indexY = nwf.BDntxy[3] - nwf.ntxy[3]
        PTxyBVx = zeros(Float64,indexZ,indexX,indexY)

    return elbd3d(Float64.(dpcf.bfull),Float64.(dpcf.bhalf),
    Float64.(dpcf.afull),Float64.(dpcf.ahalf),
                  PVzBTzz, PVzBTxz, PVzBTyz,
                  PVxBTxx, PVxBTxz, PVxBTxy,
                  PVyBTyy, PVyBTyz, PVyBTxy,
                  PTzzBVz, PTzzBVx, PTzzBVy,
                  PTxxBVz, PTxxBVx, PTxxBVy,
                  PTyyBVz, PTyyBVx, PTyyBVy,
                  PTxzBVx, PTxzBVz,
                  PTyzBVz, PTyzBVy,
                  PTxyBVx, PTxyBVy)
end

#=== acoustic ===#
function BD(nwf::nacwf3d, medium::acoustic3d)

    ext = medium.ext
    iflag = medium.iflag
    dpcf = DampCoeff(medium)

    # Vx
    indexZ = nwf.BDnvx[1]
    indexX = nwf.BDnvx[2] - nwf.nvx[2]
    indexY = nwf.BDnvx[3]
    PVxBTpp = zeros(Float64,indexZ,indexX,indexY)

    # Vy
    indexZ = nwf.BDnvy[1]
    indexX = nwf.BDnvy[2]
    indexY = nwf.BDnvy[3] - nwf.nvy[3]
    PVyBTpp = zeros(Float64,indexZ,indexX,indexY)

    # Vz
    indexZ = nwf.BDnvz[1] - nwf.nvz[1]
    indexX = nwf.BDnvz[2]
    indexY = nwf.BDnvz[3]
    PVzBTpp = zeros(Float64,indexZ,indexX,indexY)

    # Tzz & Txx & Tyy
    indexZ = nwf.BDntpp[1] - nwf.ntpp[1]
    indexX = nwf.BDntpp[2]
    indexY = nwf.BDntpp[3]
    PTppBVz = zeros(Float64,indexZ,indexX,indexY)

    indexZ = nwf.BDntpp[1]
    indexX = nwf.BDntpp[2] - nwf.ntpp[2]
    indexY = nwf.BDntpp[3]
    PTppBVx = zeros(Float64,indexZ,indexX,indexY)

    indexZ = nwf.BDntpp[1]
    indexX = nwf.BDntpp[2]
    indexY = nwf.BDntpp[3] - nwf.ntpp[3]
    PTppBVy = zeros(Float64,indexZ,indexX,indexY)

    return acbd3d(dpcf.bfull,dpcf.bhalf,dpcf.afull,dpcf.ahalf,
                  PVzBTpp, PVxBTpp, PVyBTpp,
                  PTppBVz, PTppBVx, PTppBVy)
end


text:
# estimation of the space in bytes
function sizeof(v::AbstractIndexableBitVector)
    size = 0
    for name in names(v)
        size += sizeof(v.(name))
        if !isbits(typeof(v.(name)))
            # word size in bytes
            size += sizeof(Int)
        end
    end
    return size
end

# return the i-th bit
bitat(chunk::Uint64, i::Int) = (chunk >>> (64 - i)) & 0x01 == 0x01
bitat{T<:Unsigned}(::Type{T}, chunk::T, i::Int) = (chunk >>> (sizeof(T) * 8 - i)) & 1 == 1

# assume 1 byte = 8 bits
bitsof{T<:Unsigned}(::Type{T}) = sizeof(T) * 8

# make a bit mask
lmask{T<:Unsigned}(typ::Type{T}, n::Int) = typemax(typ) << (bitsof(typ) - n)
rmask{T<:Unsigned}(typ::Type{T}, n::Int) = typemax(typ) >> (bitsof(typ) - n)


text:
#=
# 2D heat equation, Dirichlet bc, CG
=#

### If the Finch package has already been added, use this line #########
using Finch # Note: to add the package, first do: ]add "https://github.com/paralab/Finch.git"

### If not, use these four lines (working from the examples directory) ###
# if !@isdefined(Finch)
#     include("../Finch.jl");
#     using .Finch
# end
##########################################################################

init_finch("heat2d");

useLog("heat2dlog")

# Set up the configuration
domain(2)                   # dimension
functionSpace(order=2)      # basis polynomial order
timeStepper(RK4)            # time stepper (optional second arg is CFL#)

# Specify the problem
mesh(QUADMESH, elsperdim=10) # 10x10 elements

u = variable("u")            # make a scalar variable with symbol u
testSymbol("v")              # sets the symbol for a test function

T = 1;
timeInterval(T)              # The time interval is 0 to T
initial(u, "abs(x-0.5)+abs(y-0.5) < 0.2 ? 1 : 0") # initial condition

boundary(u, 1, DIRICHLET, 0)  # boundary condition for BID 1 is Dirichlet with value 0

# Write the weak form
coefficient("f", "0.5*sin(6*pi*x)*sin(6*pi*y)")
weakForm(u, "Dt(u*v) + 0.01 * dot(grad(u),grad(v)) - f*v")

solve(u);

## Uncomment below to plot ##
# # solution is stored in the variable's "values"
# using Plots
# pyplot();
# display(plot(Finch.grid_data.allnodes[1,:], Finch.grid_data.allnodes[2,:], u.values[:], st = :surface))

finalize_finch() # Finish writing and close any files

max_stars_repo_name: paralab/Finch

max_issues_repo_name: paralab/Finch

max_forks_repo_name: paralab/Finch


text:
@inline function always_assert(cond::Bool, msg::String)
    cond || throw(AlwaysAssertionError(msg))
    return nothing
end


text:
using GenericInstruments
GI = GenericInstruments
####################################
# Instantiate obj
niScope1 = GI.SCOPE.INSTR(:NI5122,"PXI1Slot3")

# Init
GI.SCOPE.connect!(niScope1)
# Auto setup
GI.SCOPE.niScope_AutoSetup(niScope1)
# Note: channel names start from 1, to ensure compatibility with Lecroy scopes
# Channel coupling
GI.SCOPE.set_coupling(niScope1;ch="1",cpl="gnd")
GI.SCOPE.niScope_GetAttributeViInt32(niScope1, "0", 1250003) # OK
# Channel state on/off
GI.SCOPE.set_state(niScope1;ch="1",st="on")
GI.SCOPE.niScope_GetAttributeViBoolean(niScope1, "0", 1250005) #OK
# Probe attenuation
GI.SCOPE.set_atten(niScope1;ch="1",att="10x")
GI.SCOPE.niScope_GetAttributeViReal64(niScope1, "0", 1250004) # always returns 1.0!!!
# BW limit
GI.SCOPE.set_bw_lim(niScope1;ch="1",bw="100MHz")
GI.SCOPE.niScope_GetAttributeViReal64(niScope1, "0", 1250006) #OK

## VERTICAL
# Vrange
GI.SCOPE.set_vrange(niScope1;ch="1",vrang=2.2)
GI.SCOPE.niScope_GetAttributeViReal64(niScope1, "0", 1250001) #OK, it approximates to next value
# Vertical offset
GI.SCOPE.set_voffs(niScope1;ch="1",voffs=0.12)
GI.SCOPE.niScope_GetAttributeViReal64(niScope1, "0", 1250002) #OK

## HORIZONTAL
# Nr of acquisition points
GI.SCOPE.set_nr_pts(niScope1;npts=1.3e6)
GI.SCOPE.niScope_GetAttributeViInt32(niScope1, "", 1250009) #OK
# Sampling rate
GI.SCOPE.set_srate(niScope1;sr=1.6e6) # take care, this is min sample rate, not actual sample rate
GI.SCOPE.niScope_GetAttributeViReal64(niScope1, "", 1150009) #OK, min sample rate
GI.SCOPE.niScope_GetAttributeViReal64(niScope1, "", 1250010) #OK, actual sample rate:1.6129032258064516e6
# Horizontal offset origin in #divisions
GI.SCOPE.set_hoffs_div(niScope1;href=1) # actually sends 1*10 = 10%
GI.SCOPE.niScope_GetAttributeViReal64(niScope1, "", 1250011) #OK, returns 1*10 = 10%

## TRIGGER
# Set trigger type
GI.SCOPE.set_trig_type(niScope1;tp="edge")
GI.SCOPE.niScope_GetAttributeViInt32(niScope1, "", 1250012) #OK, find triggertype enum!
# Set trigger source
GI.SCOPE.set_trig_src(niScope1;src="1") #can be channel:"0" or "ext", etc. check enum
GI.SCOPE.niScope_GetAttributeViString(niScope1, "", 1250013) #OK
# Set trigger slope
GI.SCOPE.set_trig_slope(niScope1;slp="pos")
GI.SCOPE.niScope_GetAttributeViInt32(niScope1, "", 1250018) # OK
# Set trigger level
GI.SCOPE.set_trig_lev(niScope1;lev=1.15)
GI.SCOPE.niScope_GetAttributeViReal64(niScope1, "", 1250017) #OK? 1.1485451761102603
# Set trigger holdoff
GI.SCOPE.set_trig_holdoff(niScope1;hf=120e-9)
GI.SCOPE.niScope_GetAttributeViReal64(niScope1, "", 1250016) #OK
# Trigger delay time
GI.SCOPE.set_hoffs_t(niScope1;href=1.2e-3)
GI.SCOPE.niScope_GetAttributeViReal64(niScope1, "", 1250015) #OK
# Set trigger coupling
GI.SCOPE.set_trig_cpl(niScope1;ch="1",cpl="dc")
GI.SCOPE.niScope_GetAttributeViInt32(niScope1, "", 1250014) #always returns 0

# Built-in high level functions
GI.SCOPE.niScope_ConfigureVertical(niScope1;
	ch="1",vrang=5.0,voffs=0.5,cpl="dc",att="1x",st="on")

GI.SCOPE.niScope_ConfigureHorizontalTiming(niScope1;
	minSampleRate=2e5,minNumPts=1000,refPosition=0.1,numRecords=1,enforceRealtime=1)

# Fetch waveform
GI.SCOPE.niScope_ConfigureTriggerImmediate(niScope1)
GI.SCOPE.niScope_InitiateAcquisition(niScope1)
GI.SCOPE.niScope_ActualRecordLength(niScope1)
y,initialX, Xincrem = GI.SCOPE.fetch_wfm(niScope1,ch="1")
plot(y)

# Close
GI.SCOPE.disconnect!(niScope1)


text:
using MeshCatMechanisms
using LinearAlgebra
using RigidBodyDynamics
using Plots
using GeometryTypes
import TrajectoryOptimization: hold_trajectory, Trajectory, total_time
import RigidBodyDynamics: transform
include("N_plots.jl")
include("../kuka_visualizer.jl")
model, obj = Dynamics.kuka
n,m = model.n, model.m
nn = m   # Number of positions

function plot_sphere(frame::CartesianFrame3D,center,radius,mat,name="")
    geom = HyperSphere(Point3f0(center), convert(Float32,radius))
    setelement!(vis,frame,geom,mat,name)
end

function plot_cylinder(frame::CartesianFrame3D,c1,c2,radius,mat,name="")
    geom = Cylinder(Point3f0(c1),Point3f0(c2),convert(Float32,radius))
    setelement!(vis,frame,geom,mat,name)
end

# Create Mechanism
kuka = parse_urdf(Dynamics.urdf_kuka,remove_fixed_tree_joints=false)
world = root_frame(kuka)

# Create Visualizer
visuals = URDFVisuals(Dynamics.urdf_kuka)
vis = MechanismVisualizer(kuka, visuals)
open(vis)

# Default solver options
opts = SolverOptions()
opts.cost_tolerance = 1e-6
opts.constraint_tolerance = 1e-5
dircol_options = Dict("tol"=>opts.cost_tolerance,"constr_viol_tol"=>opts.constraint_tolerance)

############################################
#    Unconstrained Joint Space Goal        #
############################################

# Define objective
x0 = zeros(n)
x0[2] = -pi/2
xf = copy(x0)
xf[1] = pi/4
Q = 1.0*Diagonal(I,n)
Q = Diagonal([ones(7); ones(7)*20])
Qf = 250.0*Diagonal(I,n)
R = 1e-3*Diagonal(I,m)
# Rd = 1e-2
# R = Diagonal([1e-2,1e-2,Rd,Rd,Rd,Rd,Rd])
tf = 5.0
obj_uncon = LQRObjective(Q, R, Qf, tf, x0, xf)

# Define solver
N = 41
solver = Solver(model,obj_uncon,N=N, opts=opts)

# Generate initial control trajectory
U0_hold = hold_trajectory(solver, kuka, x0[1:7])
U0_hold[:,end] .= 0
X0_hold = TrajectoryOptimization.rollout(solver,U0_hold)

# Solve
solver.opts.verbose = true
solver.opts.live_plotting = false
solver.opts.iterations_innerloop = 200
solver.state.infeasible
solver.opts.bp_reg_initial = 0
res, stats = solve(solver,U0_hold)
stats["iterations"]
J = stats["cost"][end]
norm(res.X[N]-xf)
plot(res.U)

res_d, stats_d = solve_dircol(solver,X0_hold,U0_hold,options=dircol_options)
eval_f = gen_usrfun_ipopt(solver::Solver,:hermite_simpson)[1]
J - cost(solver,res_d.X,res_d.U)

set_configuration!(vis, x0[1:7])
animate_trajectory(vis, res.X)
set_configuration!(vis, xf[1:7])
animate_trajectory(vis, res_d.X)

plot(stats["cost"],yscale=:log10)
plot!(stats_d["cost"])

u_bnd = [50,100,30,50,30,30,30]
obj_con = ConstrainedObjective(obj_uncon, u_min=-u_bnd, u_max=u_bnd)
solver_con = Solver(model, obj_con, N=N, opts=opts)
res_con, stats_con = solve(solver_con, U0_hold)

opts = SolverOptions()
opts.square_root = true
opts.R_minimum_time = 40
opts.minimum_time_tf_estimate = 0.6
opts.penalty_initial = 1
opts.penalty_scaling = 20
opts.penalty_initial_minimum_time_equality = 10
opts.penalty_initial_minimum_time_inequality = 10
opts.outer_loop_update_type = :default
opts.cost_tolerance = 1e-4
opts.constraint_tolerance = 1e-3
opts.cost_tolerance_intermediate = 1e-2
opts.use_nesterov = false

obj_min = update_objective(obj_con, tf=:min)
solver_min = Solver(model, obj_min, N=N, opts=opts)
solver_min.opts.verbose = true
res_min, stats_min = solve(solver_min,to_array(res_con.U))
stats_min["iterations"]
TrajectoryOptimization.total_time(solver,res)
TrajectoryOptimization.total_time(solver_min,res_min)
plot(res_min.X,legend=:none)
plot(res_min.U)
plot(res_min.X, 1:6)
plot(res.U)
plot!(res_min.U)

dt = [u[end] for u in res_min.U]
set_configuration!(vis, x0[1:7])
animate_trajectory(vis, res.X, solver.dt)
animate_trajectory(vis, res_min.X, mean(dt)^2)

plot(dt)

#####################################
#        WITH TORQUE LIMITS         #
#####################################

# Limit Torques
obj_con = ConstrainedObjective(obj_uncon, u_min=-75,u_max=75)
U_uncon = to_array(res.U)

solver = Solver(model,obj_con,N=N, opts=opts)
solver.opts.verbose = false
solver.opts.penalty_scaling = 50
solver.opts.penalty_initial = 0.0001
solver.opts.cost_tolerance_intermediate = 1e-3
solver.opts.outer_loop_update_type = :feedback
solver.opts.use_nesterov = false
solver.opts.iterations = 500
solver.opts.bp_reg_initial = 0

# iLQR
res_con, stats_con = solve(solver,U0_hold)
stats_con["iterations"]
plot(res_con.U)
cost(solver,res_con)

# DIRCOL
res_con_d, stats_con_d = solve_dircol(solver,X0_hold,U0_hold,options=dircol_options)
cost(solver,res_con_d)

p = convergence_plot(stats_con,stats_con_d,xscale=:log10)
TrajectoryOptimization.plot_vertical_lines!(p,stats_con["outer_updates"],title="Kuka Arm with Torque Limits")

# Visualize
set_configuration!(vis, x0[1:7])
animate_trajectory(vis, res_con.X)

# Dircol Truth
group = "kuka/armswing/constrained"
solver_truth = Solver(model,obj_con,N=101)
run_dircol_truth(solver_truth, Array(res_con_d.X), Array(res_con_d.U), group::String)
X_truth, U_truth = get_dircol_truth(solver_truth,X0_hold,U0_hold,group)[2:3]
interp(t) = TrajectoryOptimization.interpolate_trajectory(solver_truth, X_truth, U_truth, t)

Ns = [41,51,75,101]
disable_logging(Logging.Debug)
run_step_size_comparison(model, obj_con, U0_hold, group, Ns, integrations=[:midpoint,:rk3],dt_truth=solver_truth.dt,opts=solver.opts,X0=X0_hold)
plot_stat("runtime",group,legend=:bottomright,title="Kuka Arm with Torque Limits")
plot_stat("iterations",group,legend=:bottom,title="Kuka Arm with Torque Limits")
plot_stat("error",group,yscale=:log10,legend=:left,title="Kuka Arm with Torque Limits")
plot_stat("c_max",group,yscale=:log10,legend=:bottom,title="Kuka Arm with Torque Limits")

####################################
#       END EFFECTOR GOAL          #
####################################

# Run ik to get final configuration to achieve goal
goal = [.5,.24,.9]
ik_res = Dynamics.kuka_ee_ik(kuka,goal)
xf[1:nn] = configuration(ik_res)

# Define objective with new final configuration
obj_ik = LQRObjective(Q, R, Qf, tf, x0, xf)
obj_ik = ConstrainedObjective(obj_ik)

# Solve
solver_ik = Solver(model,obj_ik,N=N)
res_ik, stats_ik = solve(solver_ik,U0_hold)
stats_ik["iterations"]
cost(solver,res_ik)
norm(res_ik.X[N] - xf)
ee_ik = Dynamics.calc_ee_position(kuka,res_ik.X)
norm(ee_ik[N] - goal)

X0 = rollout(solver,U0_hold)
options = Dict("max_iter"=>10000,"tol"=>1e-6)
res_ik_d, stats_ik_d = solve_dircol(solver_ik,X0_hold,U0_hold,options=options)
cost(solver,res_ik_d)

# Plot the goal as a sphere
plot_sphere(world,goal,0.02,green_,"goal")
set_configuration!(vis, x0[1:7])
animate_trajectory(vis, res_ik.X)

##########################################################
#          End-effector obstacle avoidance               #
##########################################################

# Define obstacles
r = 0.2
pos = Int.(floor.([0.12N, 0.28N]))
circles = [ee_ik[p] for p in pos]
for circle in circles; push!(circle,r) end
n_obstacles = length(circles)

import TrajectoryOptimization.sphere_constraint
state_cache = StateCache(kuka)
ee_body, ee_point = Dynamics.get_kuka_ee(kuka)
world = root_frame(kuka)

function cI(c,x::AbstractVector{T},u) where T
    state = state_cache[T]
    set_configuration!(state,x[1:nn])
    ee = transform(state,ee_point,world).v
    for i = 1:n_obstacles
        c[i] = sphere_constraint(ee,circles[i][1],circles[i][2],circles[i][3],r)
    end
end

function addcircles!(vis,circles)
    world = root_frame(kuka)
    for (i,circle) in enumerate(circles)
        p = Point3D(world,collect(circle[1:3]))
        setelement!(vis,p,circle[4],"obs$i")
    end
end

# Plot Obstacles in Visualizer
addcircles!(vis,circles)

# Formulate and solve problem
obj_obs = ConstrainedObjective(obj_ik,cI=cI)
solver = Solver(model, obj_obs, N=N)
solver.opts.verbose = false
solver.opts.penalty_scaling = 100
solver.opts.penalty_initial = 0.0001
# solver.opts.cost_tolerance = 1e-6
solver.opts.cost_tolerance_intermediate = 1e-3
# solver.opts.iterations = 200
solver.opts.bp_reg_initial = 10
U0 = to_array(res_ik.U)
U0_hold = hold_trajectory(solver,kuka,x0[1:nn])
res_obs, stats_obs = solve(solver,U0_hold)
cost(solver,res_obs)
X = res_obs.X
U = res_obs.U
ee_obs = Dynamics.calc_ee_position(kuka,X)

X0 = rollout(solver,U0_hold)
res_obs_d, stat_obs_d = solve_dircol(solver,X0,U0,options=options)
cost(solver,res_obs_d)

# Visualize
set_configuration!(vis, x0[1:7])
# animate_trajectory(vis, res_ik.X)
animate_trajectory(vis, res_obs.X, 0.2)

# Plot Constraints
C0 = zero.(res_obs.C)
for k = 1:N
    cI(C0[k],res_ik.X[k],res_ik.U[k])
end
cI(res_obs.C[N],X[N],U[N])
plot(to_array(C0)',legend=:bottom, width=2, color=[:blue :red :green :yellow])
plot!(to_array(res_obs.C)',legend=:bottom, color=[:blue :red :green :yellow])

# Compare EE trajectories
plot(to_array(ee_ik)',width=2)
plot!(to_array(ee_obs)')

##########################################################
#            Full body Obstacle Avoidance                #
##########################################################

# Collision Bubbles
function kuka_points(kuka,plot=false)
    bodies = [3,4,5,6]
    radii = [0.1,0.12,0.09,0.09]
    ee_body,ee_point = Dynamics.get_kuka_ee(kuka)
    ee_radii = 0.05

    points = Point3D[]
    frames = CartesianFrame3D[]
    for (idx,radius) in zip(bodies,radii)
        body = findbody(kuka,"iiwa_link_$idx")
        frame = default_frame(body)
        point = Point3D(frame,0.,0.,0.)
        plot ? plot_sphere(frame,0,radius,body_collision,"body_$idx") : nothing
        # plot ? setelement!(vis,point,radius,"body_$idx") : nothing
        push!(points,point)
        push!(frames,frame)
    end
    frame = default_frame(ee_body)
    plot ? plot_sphere(frame,0,ee_radii,body_collision,"end_effector") : nothing
    # plot ? setelement!(vis,ee_point,ee_radii,"end_effector") : nothing
    push!(points,ee_point)
    push!(radii,ee_radii)
    return points, radii, frames
end

function calc_kuka_points(x::Vector{T},points) where T
    state = state_cache[T]
    set_configuration!(state,x[1:nn])
    [transform(state,point,world).v for point in points]
end

function collision_constraint(c,obstacle,kuka_points,radii) where T

end

function generate_collision_constraint(kuka::Mechanism, circles, cylinders=[])
    # Specify points along the arm
    points, radii = kuka_points(kuka)
    num_points = length(points)
    nCircle = length(circles)
    nCylinder = length(cylinders)
    num_obstacles = nCircle + nCylinder

    function cI_obstacles(c,x,u)
        nn = length(u)

        # Get current world location of points along arm
        arm_points = calc_kuka_points(x[1:nn],points)

        C = reshape(view(c,1:num_points*num_obstacles),num_points,num_obstacles)
        for i = 1:nCircle
            c_obstacle = view(C,1:num_points,i)
            for (p,kuka_point) in enumerate(arm_points)
                c_obstacle[p] = sphere_constraint(circles[i],kuka_point,radii[p]+circles[i][4])
            end
        end
        for j = 1:nCylinder
            i = j + nCircle
            c_obstacle = view(C,1:num_points,i)
            for (p,kuka_point) in enumerate(arm_points)
                c_obstacle[p] = circle_constraint(cylinders[j],kuka_point,radii[p]+cylinders[j][3])
            end
        end
    end
end

function addcylinders!(vis,cylinders,height=1.5)
    for (i,cyl) in enumerate(cylinders)
        plot_cylinder(world,[cyl[1],cyl[2],0],[0,0,height],cyl[3],blue_,"cyl_$i")
    end
end

# Define objective
x0 = zeros(n)
x0[2] = pi/2
x0[3] = pi/2
x0[4] = pi/2
xf = zeros(n)
xf[1] = pi/2
xf[4] = pi/2
set_configuration!(vis, xf[1:7])
Q = 1e-4*Diagonal(I,n)*10
Qf = 250.0*Diagonal(I,n)
R = 1e-5*Diagonal(I,m)/100
Rd = 1e-6
R = Diagonal([1e-8,1e-8,Rd,Rd,Rd,Rd,Rd])
tf = 5.0
obj_uncon = LQRObjective(Q, R, Qf, tf, x0, xf)

set_configuration!(vis, x0[1:7])

# Add more obstacles
d = 0.25
circles2 = copy(circles)
circles2 = Vector{Float64}[]
push!(circles2,[d,0.0,1.2,0.2])
push!(circles2,[0,-d,0.4,0.2])
push!(circles2,[0,-d,1.2,0.2])
addcircles!(vis,circles2)

cylinders = [[d,-d,0.1],[d,d,0.1],[-d,-d,0.1]]
addcylinders!(vis,cylinders)

points,radii,frames = kuka_points(kuka,true)

# Generate constraint function
num_obstacles = length(circles2)+length(cylinders)
c = zeros(length(points)*num_obstacles)
cI_arm_obstacles = generate_collision_constraint(kuka,circles2,cylinders)
cI_arm_obstacles(c,x0,zeros(m))
c

# Formulate and solve problem
costfun = LQRCost(Q,R,Qf,xf)
obj_obs_arm = ConstrainedObjective(costfun,tf,x0,xf,cI=cI_arm_obstacles,u_min=-80,u_max=80)
obj_obs_arm.cost.Q = Q
obj_obs_arm.cost.R = R*1e-8

solver = Solver(model, obj_obs_arm, N=41)
solver.opts.verbose = true
solver.opts.penalty_scaling = 10
solver.opts.penalty_initial = 0.05
# solver.opts.cost_tolerance = 1e-6
solver.opts.cost_tolerance_intermediate = 1e-2
solver.opts.cost_tolerance = 1e-7
# solver.opts.iterations = 200
solver.opts.bp_reg_initial = 0
solver.opts.outer_loop_update_type = :default
U0_hold = hold_trajectory(solver, kuka, x0[1:7])
res_obs, stats_obs = solve(solver,U0_hold)
X = res_obs.X
U = res_obs.U
ee_obs_arm = Dynamics.calc_ee_position(kuka,X)

U0_warm = to_array(U)

# Visualize
set_configuration!(vis, x0[1:7])
animate_trajectory(vis, res_ik.X)
animate_trajectory(vis, res_obs.X, 0.2)

X_interp, U_interp = TrajectoryOptimization.interp_traj(201,5.,res_obs.X,res_obs.U)
animate_trajectory(vis, X_interp, 0.05)

interpolate_trajectory

plot(to_array(res_obs.U)',legend=:none)

plot(to_array(res_obs.C)')

point_positions = [[zeros(3) for i = 1:length(points)] for k = 1:N]
for k = 1:N
    point_pos = calc_kuka_points(res_obs.X[k][1:nn],points)
    for i = 1:length(points)
        point_positions[k][i] = point_pos[i]
    end
end

dist = zeros(N)
for k = 1:N
    dist[k[]] = sphere_constraint(point_positions[k][2],circles2[3],circles2[3][4]+radii[2])
end
point_positions[2][1]

plot(dist)

c = zeros(5)
collision_constraint(c,circles2[3],point_positions[5],radii)
c

c = zeros(5*5)
cI_arm_obstacles(c,res_obs.X[5],res_obs.U[5])

obj_min = update_objective(obj_obs_arm,tf=0.6)
solver_min = Solver(model,obj_min,N=N,opts=opts)
solver_min.opts.penalty_scaling = 150
solver_min.opts.penalty_initial = 0.0005
# solver.opts.cost_tolerance = 1e-6
solver_min.opts.cost_tolerance_intermediate = 1e-2
solver_min.opts.cost_tolerance = 1e-4
solver_min.opts.constraint_tolerance = 1e-3
# solver.opts.iterations = 200
solver.opts.bp_reg_initial = 0

solver_min.opts.verbose = true
res_min0,stats_min0 = solve(solver_min,U0_hold)

X_interp, U_interp = TrajectoryOptimization.interp_traj(201,5.,res_min.X,res_min.U)
animate_trajectory(vis, X_interp, solver_min.dt)

obj_min = update_objective(obj_obs_arm,tf=:min)
obj_min.cost.Q .= Diagonal([ones(nn)*1e-4; ones(nn)*1e-1])
obj_min.cost.R .= Diagonal(I,m)*1e-6

solver_min = Solver(model,obj_min,N=N,opts=opts)
solver_min.opts.penalty_scaling = 10
solver_min.opts.penalty_initial = 0.0005
# solver.opts.cost_tolerance = 1e-6
solver_min.opts.cost_tolerance_intermediate = 1e-3
solver_min.opts.cost_tolerance = 1e-4
solver_min.opts.constraint_tolerance = 1e-3
solver_min.opts.minimum_time_tf_estimate = 0.6
solver_min.opts.penalty_initial_minimum_time_equality = 10
solver_min.opts.penalty_initial_minimum_time_inequality = 10
solver_min.opts.penalty_scaling_minimum_time_equality = 100
solver_min.opts.R_minimum_time = 1
solver_min.opts.square_root = true
solver_min.opts.use_nesterov = false
solver_min.opts.iterations = 1000

solver_min.opts.verbose = true
U0 = hold_trajectory(solver_min, kuka, x0[1:7])
U_warm = to_array(res_min0.U)
res_min,stats_min = solve(solver_min, U_warm)
total_time(solver_min,res_min)

U_guess = to_array(res_min.U)
res_min,stats_min = solve(solver_min, U_guess)

dt = [u[end] for u in res_min.U]
plot(dt)
plot(res_min.U)
plot(res_min.X,1:6,legend=:bottom)

X_interp, U_interp = TrajectoryOptimization.interp_traj(201,5.,res_min.X,res_min.U)
animate_trajectory(vis, X_interp, 0.01)
solver_min.dt

##########################################################
#          End-effector frame Cost Funtion               #
##########################################################

function get_ee_costfun(mech,Qee,Q,R,Qf,ee_goal)
    statecache = StateCache(mech)
    state = MechanismState(mech)
    world = root_frame(mech)
    ee_pos = Dynamics.get_kuka_ee_postition_fun(mech,statecache)
    nn = num_positions(mech)
    n,m = 2nn,nn

    ee_body, ee_point = Dynamics.get_kuka_ee(kuka)
    p = path(mech, root_body(mech), ee_body)
    Jp = point_jacobian(state, p, transform(state, ee_point, world))

    # Hessian of the ee term wrt full state
    hess_ee = zeros(n,n)
    grad_ee = zeros(n)
    linds = LinearIndices(hess_ee)
    qq = linds[1:nn,1:nn]
    q_inds = 1:nn

    H = zeros(m,n)

    function costfun(x,u)
        ee = ee_pos(x) - ee_goal
        return (ee'Qee*ee + x'Q*x + u'R*u)/2
    end
    function costfun(xN::Vector{Float64})
        ee = ee_pos(xN) - ee_goal
        return 0.5*ee'Qf*ee + xN'Q*xN
    end

    function expansion(x::Vector{T},u) where T
        state = statecache[T]
        set_configuration!(state,x[1:nn])
        ee = ee_pos(x)

        point_in_world = transform(state, ee_point, world)
        point_jacobian!(Jp, state, p, point_in_world)
        Jv = Array(Jp)

        hess_ee[qq] = Jv'Qee*Jv
        Pxx = hess_ee + Q
        Puu = R
        Pux = H

        grad_ee[q_inds] = Jv'Qee*(ee-ee_goal)
        Px = grad_ee + Q*x
        Pu = R*u
        return Pxx,Puu,Pux,Px,Pu
    end
    function expansion(xN::AbstractVector{T}) where T
        state = statecache[T]
        set_configuration!(state,xN[1:nn])
        ee = ee_pos(xN)

        point_in_world = transform(state, ee_point, world)
        point_jacobian!(Jp, state, p, point_in_world)
        Jv = Array(Jp)

        hess_ee[qq] = Jv'Qf*Jv
        Pxx = hess_ee + Q

        grad_ee[q_inds] = Jv'Qf*(ee-ee_goal)
        Px = grad_ee + Q*xN
        return Pxx,Px
    end
    return costfun, expansion
end

Q2 = Diagonal([zeros(nn); ones(nn)*1e-5])
Qee = Diagonal(I,3)*1e-3
Qfee = Diagonal(I,3)*200
R2 = Diagonal(I,m)*1e-12

u0 = zeros(m)
eecostfun, expansion = get_ee_costfun(kuka,Qee,Q2,R2,Qfee,goal)
ee_costfun = GenericCost(eecostfun,eecostfun,expansion,n,m)
stage_cost(ee_costfun,x0,u0)
expansion(x0,u0)
expansion(x0)
TrajectoryOptimization.taylor_expansion(ee_costfun,x0)

ee_obj = UnconstrainedObjective(ee_costfun,tf,x0)
u_bnd = ones(m)*20
u_bnd[2] = 100
u_bnd[4] = 30
x_bnd = ones(n)*Inf
x_bnd[nn+1:end] .= 15
ee_obj_con = ConstrainedObjective(ee_obj,u_min=-u_bnd,u_max=u_bnd, x_min=-x_bnd, x_max=x_bnd,use_xf_equality_constraint=false)

U0_ik = to_array(res_ik.U)
solver_ee = Solver(model,ee_obj_con,N=61)
solver_ee.opts.cost_tolerance = 1e-6
solver_ee.opts.verbose = true
res,stats = solve(solver_ee,U0_hold)

set_configuration!(vis, x0[1:7])
animate_trajectory(vis, res_ik.X)
animate_trajectory(vis, res.X)

plot(res.U,legend=:bottom)
qdot = to_array(res.X)[8:end,:]
plot(qdot')

res.C[N]
to_array(res.X)
res.X[end]


text:
const cₚ = 10.0 #penalty term for unwanted frequency

function optimize_rs_mf(rs0, r_min, r_max, points, point_flags, ids, P, ui, k0,
                        kin, centers, fmmopts, optimopts::Optim.Options;
                        method = BFGS(linesearch = LineSearches.BackTracking()))

    Ns = size(centers,1)
    φs = zeros(Ns)
    Nk = length(k0); @assert Nk == length(kin)
    J = length(rs0)
    @assert maximum(ids) <= J
    if length(r_min) == 1
        r_min = r_min*ones(Float64,J)
    else
        @assert J == length(r_min)
    end
    if length(r_max) == 1
        r_max = r_max*ones(Float64,J)
    else
        @assert J == length(r_max)
    end
    verify_min_distance(CircleParams.(r_max), centers, ids,
        points) || error("Particles are too close or r_max are too large.")

    groups,boxSize = divideSpace(centers, fmmopts)
    P2,Q = FMMtruncation(fmmopts.acc, boxSize, maximum(k0))
    mFMM = [FMMbuildMatrices(k0[ik], P, P2, Q, groups, centers, boxSize) for ik = 1:Nk]
    scatteringMatrices = [[sparse(one(Complex{Float64})I, 2*P+1, 2*P+1) for i = 1:J] for ik = 1:Nk]
    dS_S = [[sparse(one(Complex{Float64})I, 2*P+1, 2*P+1) for i = 1:J] for ik = 1:Nk]

    #calculate nd expand incident field
    α = [u2α(k0[ik], ui, centers, P) for ik = 1:Nk]
    uinc_ = [uinc(k0[ik], points, ui) for ik = 1:Nk]
    H = [optimizationHmatrix(points, centers, Ns, P, k0[ik]) for ik = 1:Nk]

    # Allocate buffers
    buf = FMMbuffer(Ns,P,Q,length(groups))
    shared_var = [OptimBuffer(Ns, P, size(points,1), J) for ik = 1:Nk]
    initial_rs = copy(rs0)
    last_rs = similar(initial_rs); last_rs[1] = NaN; @assert last_rs != initial_rs #initial_rs, last_rs must be different before first iteration
    df = OnceDifferentiable(
        rs -> optimize_rs_mf_f(rs, last_rs, shared_var, φs, α, H, points, point_flags,
                            P, uinc_, Ns, k0, kin, centers, scatteringMatrices, dS_S,
                            ids, mFMM, fmmopts, buf),
        (grad_stor, rs) -> optimize_rs_mf_g!(grad_stor, rs, last_rs, shared_var, φs,
                            α, H, points, point_flags, P, uinc_, Ns, k0, kin, centers,
                            scatteringMatrices, dS_S, ids, mFMM, fmmopts, buf),
        initial_rs)

    optimize(df, r_min, r_max, initial_rs, Fminbox(method), optimopts)
end

function optimize_rs_mf_common!(rs, last_rs, shared_var, φs, α,
            H, points, P, uinc_, Ns, k0, kin, centers, scatteringMatrices,
            dS_S, ids, mFMM, fmmopts, buf)
    if rs != last_rs
        copyto!(last_rs, rs)
        #do whatever common calculations and save to shared_var
        #construct rhs
        for ik in eachindex(k0)
            for id in unique(ids)
                updateCircleScatteringDerivative!(scatteringMatrices[ik][id],
                    dS_S[ik][id], k0[ik], kin[ik], rs[id], P)
            end
            for ic = 1:Ns
                rng = (ic-1)*(2*P+1) .+ (1:2*P+1)
                buf.rhs[rng] = scatteringMatrices[ik][ids[ic]]*α[ik][rng]
            end

            MVP = LinearMap{eltype(buf.rhs)}(
                        (output_, x_) -> FMM_mainMVP_pre!(output_, x_,
                                            scatteringMatrices[ik], φs, ids, P,
                                            mFMM[ik], buf.pre_agg, buf.trans),
                        Ns*(2*P+1), Ns*(2*P+1), ismutating = true)

            shared_var[ik].β[:] = 0
            shared_var[ik].β,ch = gmres!(shared_var[ik].β, MVP, buf.rhs,
                                    restart = Ns*(2*P+1), tol = fmmopts.tol,
                                    log = true, initially_zero = true)
            !ch.isconverged && error("""FMM process did not converge, normalized
                residual: $(norm(MVP*shared_var[ik].β - buf.rhs)/norm(buf.rhs))""")

            shared_var[ik].f[:] = transpose(H[ik])*shared_var[ik].β
            shared_var[ik].f[:] .+= uinc_[ik]
        end
    end
end

function optimize_rs_mf_f(rs, last_rs, shared_var, φs, α, H, points, point_flags,
                        P, uinc_, Ns, k0, kin, centers, scatteringMatrices, dS_S,
                        ids, mFMM, fmmopts, buf)
    optimize_rs_mf_common!(rs, last_rs, shared_var, φs, α, H, points, P, uinc_,
        Ns, k0, kin, centers, scatteringMatrices, dS_S, ids, mFMM, fmmopts, buf)

    fobj_rs_mf(shared_var, point_flags)
end

function optimize_rs_mf_g!(grad_stor, rs, last_rs, shared_var, φs, α, H, points,
            point_flags, P, uinc_, Ns, k0, kin, centers, scatteringMatrices, dS_S,
            ids, mFMM, opt, buf)
    optimize_rs_mf_common!(rs, last_rs, shared_var, φs, α, H, points, P, uinc_,
        Ns, k0, kin, centers, scatteringMatrices, dS_S, ids, mFMM, opt, buf)

    fill!(grad_stor, 0)
    for ik in eachindex(k0)
        MVP = LinearMap{eltype(buf.rhs)}(
                    (output_, x_) -> FMM_mainMVP_transpose!(output_, x_,
                                        scatteringMatrices[ik], φs, ids, P,
                                        mFMM[ik], buf.pre_agg, buf.trans),
                    Ns*(2*P+1), Ns*(2*P+1), ismutating = true)

        #build rhs of adjoint problem
        gobj_rs_mf!(shared_var, ik, H[ik], point_flags)
        #solve adjoint problem
        λadj, ch = gmres(MVP, shared_var[ik].rhs_grad, restart = Ns*(2*P+1),
                    tol = opt.tol, log = true)
        # λadj, ch = gmres!(λadj, MVP, shared_var.rhs_grad, restart = Ns*(2*P+1),
        #                 tol = opt.tol, log = true, initially_zero = true)

        for n = 1:length(rs)
            #compute n-th gradient - here we must pay the price for symmetry
            #as more than one beta is affected. Overwrites rhs_grad.
            for ic = 1:Ns
                rng = (ic-1)*(2*P+1) .+ (1:2*P+1)
                if ids[ic] == n
                    shared_var[ik].rhs_grad[rng] = dS_S[ik][n]*shared_var[ik].β[rng]
                else
                    shared_var[ik].rhs_grad[rng] = 0.0
                end
            end
            grad_stor[n] += -2*real(transpose(λadj)*shared_var[ik].rhs_grad)
        end
    end
end

function fobj_rs_mf(sv, point_flags)
    #emphasize field maximization
    fobj = 0.0
    for i in eachindex(point_flags)
        ik = point_flags[i]
        #add and subtract for easy sum
        sumu = sum(abs2, sv[jk].f[i] for jk in eachindex(sv)) - abs2(sv[ik].f[i])
        fobj += (1 + cₚ*sumu)/abs2(sv[ik].f[i])
    end
    fobj
end

function gobj_rs_mf!(sv, ik, H, point_flags)
    #calculate -(∂f/∂β)ᵀ for adjoint method
    fill!(sv[ik].rhs_grad, 0)
    for i in eachindex(point_flags)
        ik′ = point_flags[i]
        if ik == ik′
            sumu = sum(abs2, sv[jk].f[i] for jk in eachindex(sv))
            num = 1 + cₚ*(sumu - abs2(sv[ik].f[i]))
            denom = abs2(sv[ik].f[i])*sv[ik].f[i]
        else
            num = (-cₚ)*conj(sv[ik].f[i])
            denom = abs2(sv[ik′].f[i])
        end
        sv[ik].rhs_grad .+= view(H,:,i)*(num/denom)
    end
end


text:
function initialize_pgfplots()

    pgfplots_pre = "\\definecolor{airforceblue}{rgb}{0.36, 0.54, 0.66}
    \\definecolor{amaranth}{rgb}{0.9, 0.17, 0.31}
    \\definecolor{asparagus}{rgb}{0.53, 0.66, 0.42}
    \\definecolor{cadmiumorange}{rgb}{0.93, 0.53, 0.18}
    \\pgfplotscreateplotcyclelist{var2}{%
    no marks,very thick,color = airforceblue\\\\%1
    no marks,very thick,color = cadmiumorange\\\\%2
    }
    \\pgfplotscreateplotcyclelist{var3}{%
    no marks,very thick,color = airforceblue\\\\%1
    no marks,very thick,color = cadmiumorange\\\\%2
    no marks,very thick,color = asparagus\\\\%3
    }
    \\pgfplotscreateplotcyclelist{var4}{%
    no marks,very thick,color = airforceblue\\\\%1
    no marks,very thick,color = cadmiumorange\\\\%2
    no marks,very thick,color = asparagus\\\\%3
    no marks,very thick,color = amaranth\\\\%4
    }"
    push!(PGFPlotsX.CUSTOM_PREAMBLE, pgfplots_pre)

end

function pgfplot(y; Legend = "y1", Label =("x","y"), Title = "Figure", Path = "NA", Width = "0.6*\\textwidth", Legend_pos = "outer north east", PDF = false, Xtick = false, Xticklabels = false )
    x = 1:size(y,1)
    if !Xtick
        p = @pgf Axis({
            xlabel = Label[1],
            ylabel = Label[2],
            title = Title,
            width = Width,
            legend_pos = Legend_pos,
            cycle_list_name = "var2",
            xmin = extrema(x)[1],
            xmax = extrema(x)[2],
            },
                PlotInc(Table([:x => x, :y => y])),
                LegendEntry(Legend),
            )
    else
            p = @pgf Axis({
            xlabel = Label[1],
            ylabel = Label[2],
            title = Title,
            width = Width,
            xtick = Xtick,
            xticklabels = Xticklabels,
            legend_pos = Legend_pos,
            cycle_list_name = "var2",
            xmin = extrema(x)[1],
            xmax = extrema(x)[2],
            },
                PlotInc(Table([:x => x, :y => y])),
                LegendEntry(Legend),
            )
    end
        if Path != "NA"
            pgfsave(Path*".tex", p, include_preamble = false)
            if PDF
                pgfsave(Path*".pdf", p)
            end
        end
        display("image/png",p)
end

function pgfplot(y1,y2; Legend = ("y1","y2"), Label =("x","y"),Title = "Figure", Path = "NA", Width = "0.6*\\textwidth", Legend_pos = "outer north east", PDF = false, Xtick = false, Xticklabels = false)
    x1 = 1:size(y1,1)
    x2 = 1:size(y2,1)
    if !Xtick
        p = @pgf Axis({
            xlabel = Label[1],
            ylabel = Label[2],
            title = Title,
            width = Width,
            legend_pos = Legend_pos,
            cycle_list_name = "var2",
            xmin = extrema(x1)[1],
            xmax = extrema(x1)[2],

            },
                Plot(Table([:x => x1, :y => y1])),
                LegendEntry(Legend[1]),
                Plot(Table([:x => x2, :y => y2])),
                LegendEntry(Legend[2])
            )
    else
        p = @pgf Axis({
            xlabel = Label[1],
            ylabel = Label[2],
            title = Title,
            width = Width,
            xtick = Xtick,
            xticklabels = Xticklabels,
            legend_pos = Legend_pos,
            cycle_list_name = "var2",
            xmin = extrema(x1)[1],
            xmax = extrema(x1)[2],

            },
                Plot(Table([:x => x1, :y => y1])),
                LegendEntry(Legend[1]),
                Plot(Table([:x => x2, :y => y2])),
                LegendEntry(Legend[2])
            )
    end

        if Path != "NA"
            pgfsave(Path*".tex", p, include_preamble = false)
            if PDF
                pgfsave(Path*".pdf", p)
            end
        end
        display("image/png",p)
end

function pgfplot(y1,y2,y3; Legend = ("y1","y2","y3"), Label = ("x","y"), Title = "Figure", Path = "NA", Width = "0.6*\\textwidth", Legend_pos = "outer north east", PDF = false, Xtick = false, Xticklabels = false)
    x1 = 1:size(y1,1)
    x2 = 1:size(y2,1)
    x3 = 1:size(y3,1)
    if !Xtick
        p = @pgf Axis({
            xlabel = Label[1],
            ylabel = Label[2],
            title = Title,
            width = Width,
            legend_pos = Legend_pos,
            cycle_list_name = "var3",
            xmin = extrema(x1)[1],
            xmax = extrema(x1)[2],

            },
                PlotInc(Table([:x => x1, :y => y1])),
                LegendEntry(Legend[1]),
                PlotInc(Table([:x => x2, :y => y2])),
                LegendEntry(Legend[2]),
                PlotInc(Table([:x => x3, :y => y3])),
                LegendEntry(Legend[3])
            )
    else
        p = @pgf Axis({
            xlabel = Label[1],
            ylabel = Label[2],
            title = Title,
            width = Width,
            legend_pos = Legend_pos,
            xtick = Xtick,
            xticklabels = Xticklabels,
            cycle_list_name = "var3",
            xmin = extrema(x1)[1],
            xmax = extrema(x1)[2],

            },
                PlotInc(Table([:x => x1, :y => y1])),
                LegendEntry(Legend[1]),
                PlotInc(Table([:x => x2, :y => y2])),
                LegendEntry(Legend[2]),
                PlotInc(Table([:x => x3, :y => y3])),
                LegendEntry(Legend[3])
            )
    end
        if Path != "NA"
            pgfsave(Path*".tex", p, include_preamble = false)
            if PDF
                pgfsave(Path*".pdf", p)
            end
        end
        display("image/png",p)
end

function pgfplot(y1,y2,y3,y4; Legend = ("y1","y2","y3","y4"), Label =("x","y"), Title = "Figure", Path = "NA", Width = "0.6*\\textwidth", Legend_pos = "outer north east", PDF = false, Xtick = false, Xticklabels = false)
    x1 = 1:size(y1,1)
    x2 = 1:size(y2,1)
    x3 = 1:size(y3,1)
    x4 = 1:size(y4,1)
    if !Xtick
        p = @pgf Axis({
            xlabel = Label[1],
            ylabel = Label[2],
            title = Title,
            width = Width,
            legend_pos = Legend_pos,
            cycle_list_name = "var4",
            xmin = extrema(x1)[1],
            xmax = extrema(x1)[2],
            },
                Plot(Table([:x => x1, :y => y1])),
                LegendEntry(Legend[1]),
                Plot(Table([:x => x2, :y => y2])),
                LegendEntry(Legend[2]),
                Plot(Table([:x => x3, :y => y3])),
                LegendEntry(Legend[3]),
                Plot(Table([:x => x4, :y => y4])),
                LegendEntry(Legend[4])
            )
    else
        p = @pgf Axis({
            xlabel = Label[1],
            ylabel = Label[2],
            title = Title,
            width = Width,
            legend_pos = Legend_pos,
            xtick = Xtick,
            xticklabels = Xticklabels,
            cycle_list_name = "var4",
            xmin = extrema(x1)[1],
            xmax = extrema(x1)[2],
            },
                Plot(Table([:x => x1, :y => y1])),
                LegendEntry(Legend[1]),
                Plot(Table([:x => x2, :y => y2])),
                LegendEntry(Legend[2]),
                Plot(Table([:x => x3, :y => y3])),
                LegendEntry(Legend[3]),
                Plot(Table([:x => x4, :y => y4])),
                LegendEntry(Legend[4])
            )
    end
        if Path != "NA"
            pgfsave(Path*".tex", p, include_preamble = false)
            if PDF
                pgfsave(Path*".pdf", p)
            end
        end
        display("image/png",p)
end


text:
# syntax: proto3
using ProtoBuf
import ProtoBuf.meta

mutable struct NodeDef_AttrEntry <: ProtoType
    __protobuf_jl_internal_meta::ProtoMeta
    __protobuf_jl_internal_values::Dict{Symbol,Any}
    __protobuf_jl_internal_defaultset::Set{Symbol}

    function NodeDef_AttrEntry(; kwargs...)
        obj = new(meta(NodeDef_AttrEntry), Dict{Symbol,Any}(), Set{Symbol}())
        values = obj.__protobuf_jl_internal_values
        symdict = obj.__protobuf_jl_internal_meta.symdict
        for nv in kwargs
            fldname, fldval = nv
            fldtype = symdict[fldname].jtyp
            (fldname in keys(symdict)) || error(string(typeof(obj), " has no field with name ", fldname))
            values[fldname] = isa(fldval, fldtype) ? fldval : convert(fldtype, fldval)
        end
        obj
    end
end # mutable struct NodeDef_AttrEntry (mapentry)
const __meta_NodeDef_AttrEntry = Ref{ProtoMeta}()
function meta(::Type{NodeDef_AttrEntry})
    ProtoBuf.metalock() do
        if !isassigned(__meta_NodeDef_AttrEntry)
            __meta_NodeDef_AttrEntry[] = target = ProtoMeta(NodeDef_AttrEntry)
            allflds = Pair{Symbol,Union{Type,String}}[:key => AbstractString, :value => AttrValue]
            meta(target, NodeDef_AttrEntry, allflds, ProtoBuf.DEF_REQ, ProtoBuf.DEF_FNUM, ProtoBuf.DEF_VAL, ProtoBuf.DEF_PACK, ProtoBuf.DEF_WTYPES, ProtoBuf.DEF_ONEOFS, ProtoBuf.DEF_ONEOF_NAMES)
        end
        __meta_NodeDef_AttrEntry[]
    end
end
function Base.getproperty(obj::NodeDef_AttrEntry, name::Symbol)
    if name === :key
        return (obj.__protobuf_jl_internal_values[name])::AbstractString
    elseif name === :value
        return (obj.__protobuf_jl_internal_values[name])::AttrValue
    else
        getfield(obj, name)
    end
end

mutable struct NodeDef_ExperimentalDebugInfo <: ProtoType
    __protobuf_jl_internal_meta::ProtoMeta
    __protobuf_jl_internal_values::Dict{Symbol,Any}
    __protobuf_jl_internal_defaultset::Set{Symbol}

    function NodeDef_ExperimentalDebugInfo(; kwargs...)
        obj = new(meta(NodeDef_ExperimentalDebugInfo), Dict{Symbol,Any}(), Set{Symbol}())
        values = obj.__protobuf_jl_internal_values
        symdict = obj.__protobuf_jl_internal_meta.symdict
        for nv in kwargs
            fldname, fldval = nv
            fldtype = symdict[fldname].jtyp
            (fldname in keys(symdict)) || error(string(typeof(obj), " has no field with name ", fldname))
            values[fldname] = isa(fldval, fldtype) ? fldval : convert(fldtype, fldval)
        end
        obj
    end
end # mutable struct NodeDef_ExperimentalDebugInfo
const __meta_NodeDef_ExperimentalDebugInfo = Ref{ProtoMeta}()
function meta(::Type{NodeDef_ExperimentalDebugInfo})
    ProtoBuf.metalock() do
        if !isassigned(__meta_NodeDef_ExperimentalDebugInfo)
            __meta_NodeDef_ExperimentalDebugInfo[] = target = ProtoMeta(NodeDef_ExperimentalDebugInfo)
            allflds = Pair{Symbol,Union{Type,String}}[:original_node_names => Base.Vector{AbstractString}, :original_func_names => Base.Vector{AbstractString}]
            meta(target, NodeDef_ExperimentalDebugInfo, allflds, ProtoBuf.DEF_REQ, ProtoBuf.DEF_FNUM, ProtoBuf.DEF_VAL, ProtoBuf.DEF_PACK, ProtoBuf.DEF_WTYPES, ProtoBuf.DEF_ONEOFS, ProtoBuf.DEF_ONEOF_NAMES)
        end
        __meta_NodeDef_ExperimentalDebugInfo[]
    end
end
function Base.getproperty(obj::NodeDef_ExperimentalDebugInfo, name::Symbol)
    if name === :original_node_names
        return (obj.__protobuf_jl_internal_values[name])::Base.Vector{AbstractString}
    elseif name === :original_func_names
        return (obj.__protobuf_jl_internal_values[name])::Base.Vector{AbstractString}
    else
        getfield(obj, name)
    end
end

mutable struct NodeDef <: ProtoType
    __protobuf_jl_internal_meta::ProtoMeta
    __protobuf_jl_internal_values::Dict{Symbol,Any}
    __protobuf_jl_internal_defaultset::Set{Symbol}

    function NodeDef(; kwargs...)
        obj = new(meta(NodeDef), Dict{Symbol,Any}(), Set{Symbol}())
        values = obj.__protobuf_jl_internal_values
        symdict = obj.__protobuf_jl_internal_meta.symdict
        for nv in kwargs
            fldname, fldval = nv
            fldtype = symdict[fldname].jtyp
            (fldname in keys(symdict)) || error(string(typeof(obj), " has no field with name ", fldname))
            values[fldname] = isa(fldval, fldtype) ? fldval : convert(fldtype, fldval)
        end
        obj
    end
end # mutable struct NodeDef
const __meta_NodeDef = Ref{ProtoMeta}()
function meta(::Type{NodeDef})
    ProtoBuf.metalock() do
        if !isassigned(__meta_NodeDef)
            __meta_NodeDef[] = target = ProtoMeta(NodeDef)
            allflds = Pair{Symbol,Union{Type,String}}[:name => AbstractString, :op => AbstractString, :input => Base.Vector{AbstractString}, :device => AbstractString, :attr => Base.Dict{AbstractString,AttrValue}, :experimental_debug_info => NodeDef_ExperimentalDebugInfo]
            meta(target, NodeDef, allflds, ProtoBuf.DEF_REQ, ProtoBuf.DEF_FNUM, ProtoBuf.DEF_VAL, ProtoBuf.DEF_PACK, ProtoBuf.DEF_WTYPES, ProtoBuf.DEF_ONEOFS, ProtoBuf.DEF_ONEOF_NAMES)
        end
        __meta_NodeDef[]
    end
end
function Base.getproperty(obj::NodeDef, name::Symbol)
    if name === :name
        return (obj.__protobuf_jl_internal_values[name])::AbstractString
    elseif name === :op
        return (obj.__protobuf_jl_internal_values[name])::AbstractString
    elseif name === :input
        return (obj.__protobuf_jl_internal_values[name])::Base.Vector{AbstractString}
    elseif name === :device
        return (obj.__protobuf_jl_internal_values[name])::AbstractString
    elseif name === :attr
        return (obj.__protobuf_jl_internal_values[name])::Base.Dict{AbstractString,AttrValue}
    elseif name === :experimental_debug_info
        return (obj.__protobuf_jl_internal_values[name])::NodeDef_ExperimentalDebugInfo
    else
        getfield(obj, name)
    end
end

export NodeDef_AttrEntry, NodeDef_ExperimentalDebugInfo, NodeDef
# mapentries: "NodeDef_AttrEntry" => ("AbstractString", "AttrValue")


text:
α = 0.025

@testset "nested archimedean copulas helpers 4 higher correlations" begin
  Random.seed!(43)
  rng = StableRNG(123)
  u = nestedcopulag("clayton", [[1,2],[3,4]], [2., 3.], 1.1, [0.1 0.2 0.3 0.4 0.5; 0.2 0.3 0.4 0.5 0.6]; rng = rng)
  @test u ≈ [0.2496 0.2946 0.71634 0.75654; 0.32992 0.37465 0.31906 0.34895] atol=1.0e-3

  Random.seed!(42)
  rng = StableRNG(123)
  x = nestedcopulag("gumbel", [[1,2],[3,4]], [2., 3.], 1.1, [0.1 0.2 0.3 0.4 0.5; 0.2 0.3 0.4 0.5 0.6]; rng = rng)
  @test x ≈ [0.0940 0.13852 0.17391 0.20250; 0.31450 0.3676 0.45506 0.4880] atol=1.0e-3

end

@testset "nested Clayton copula" begin
  @testset "exceptions" begin
    a = Clayton_cop(2, 1.)
    b = Clayton_cop(2, 2.)
    c = Clayton_cop(2, 10.)
    d = Clayton_cop(2, 20.)

    @test_throws DomainError Nested_Clayton_cop([a,b], 0, 1.5)
    @test_throws DomainError Nested_Clayton_cop([a,b], -1, 0.5)

    u = zeros(3,5)
    cp = Nested_Clayton_cop([a,b], 0, 0.5)
    @test_throws AssertionError simulate_copula!(u, cp)
    #@test_warn "θ << ϕ, marginals may not be uniform" Nested_Clayton_cop([c,d], 0, 0.05)
  end
  @testset "small example" begin
      c1 = Clayton_cop(2, 2.)
      c2 = Clayton_cop(2, 3.)
      cp = Nested_Clayton_cop([c1, c2], 1, 1.1)

      Random.seed!(43)
      rng = StableRNG(123)
      @test simulate_copula(1, cp; rng = rng) ≈ [0.274511 0.349494 0.8443515 0.5447064 0.44514597] atol=1.0e-5

      u = zeros(1,5)
      Random.seed!(43)
      rng = StableRNG(123)
      simulate_copula!(u, cp; rng = rng)
      @test u ≈ [0.274511 0.349494 0.8443515 0.5447064 0.44514597] atol=1.0e-5

  end
  @testset "large example on data" begin
      c1 = Clayton_cop(2, 3.)
      c2 = Clayton_cop(3, 4.)
      cp = Nested_Clayton_cop([c1, c2], 2, 1.5)

      Random.seed!(42)
      x = simulate_copula(20_000, cp)
      @test pvalue(ExactOneSampleKSTest(x[:,1], Uniform(0,1))) > α
      @test pvalue(ExactOneSampleKSTest(x[:,2], Uniform(0,1))) > α
      @test pvalue(ExactOneSampleKSTest(x[:,3], Uniform(0,1))) > α
      @test pvalue(ExactOneSampleKSTest(x[:,4], Uniform(0,1))) > α
      @test pvalue(ExactOneSampleKSTest(x[:,7], Uniform(0,1))) > α
      cc = corkendall(x)
      @test cc[:,1] ≈ [1.0, 3/5, 1.5/3.5, 1.5/3.5, 1.5/3.5, 1.5/3.5, 1.5/3.5] atol=1.0e-1
      @test cc[:,5] ≈ [1.5/3.5, 1.5/3.5, 2/3, 2/3, 1.0, 1.5/3.5, 1.5/3.5] atol=1.0e-1
      @test cc[6,7] ≈ 1.5/3.5 atol=1.0e-2
      @test tail(x[:,4], x[:,5], "r", 0.0001) ≈ 0
      @test tail(x[:,1], x[:,5], "r", 0.0001) ≈ 0
      @test tail(x[:,1], x[:,2], "r", 0.0001) ≈ 0
      @test tail(x[:,1], x[:,5], "l", 0.01) ≈ 2^(-1/(1.5)) atol=1.0e-1
      @test tail(x[:,1], x[:,2], "l", 0.01) ≈ 2^(-1/3) atol=1.0e-1
      @test tail(x[:,4], x[:,5], "l", 0.01) ≈ 2^(-1/4) atol=1.0e-1
      @test tail(x[:,6], x[:,7], "l", 0.01) ≈ 2^(-1/(1.5)) atol=1.0e-1

      #test on correlations
      c1 = Clayton_cop(2, .7, KendallCorrelation)
      cp = Nested_Clayton_cop([c1], 1, 0.3, KendallCorrelation)
      x = simulate_copula(20_000, cp)
      @test corkendall(x)[:,1] ≈ [1, 0.7, 0.3] atol=1.0e-2
    end
end

@testset "nested Ali-Mikhail-Haq copula" begin
  @testset "exceptions" begin
    a = AMH_cop(2, .8)
    b = AMH_cop(2, .3)
    @test_throws DomainError Nested_AMH_cop([a,b], 0, 0.5)
    @test_throws DomainError Nested_AMH_cop([a,b], -1, 0.1)

    u = zeros(5, 3)
    cp = Nested_AMH_cop([a,b], 0, 0.1)
    @test_throws AssertionError simulate_copula!(u, cp)
  end
  @testset "small example" begin
      c1 = AMH_cop(2, .8)
      c2 = AMH_cop(2, .9)
      cp = Nested_AMH_cop([c1, c2], 1, 0.5)
      Random.seed!(43)
      rng = StableRNG(123)
      @test simulate_copula(1, cp; rng = rng) ≈ [0.2066 0.3355 0.7459 0.2496 0.2804] atol=1.0e-3

      u = zeros(1,5)
      Random.seed!(43)
      rng = StableRNG(123)
      simulate_copula!(u, cp; rng = rng)
      @test u ≈[0.2066 0.3355 0.7459 0.2496 0.2804] atol=1.0e-3

  end
  @testset "large example" begin
      c1 = AMH_cop(3, .8)
      c2 = AMH_cop(2, .7)
      cp = Nested_AMH_cop([c1, c2], 2, 0.5)

      Random.seed!(44)
      x = simulate_copula(5_000, cp)
      @test pvalue(ExactOneSampleKSTest(x[:,1], Uniform(0,1))) > α
      @test pvalue(ExactOneSampleKSTest(x[:,2], Uniform(0,1))) > α
      @test pvalue(ExactOneSampleKSTest(x[:,3], Uniform(0,1))) > α
      @test pvalue(ExactOneSampleKSTest(x[:,4], Uniform(0,1))) > α
      @test pvalue(ExactOneSampleKSTest(x[:,5], Uniform(0,1))) > α
      @test pvalue(ExactOneSampleKSTest(x[:,6], Uniform(0,1))) > α
      c = corkendall(x)
      @test c[1:3,1] ≈ [1., 0.2337, 0.2337] atol=1.0e-2
      @test c[3:6,4] ≈ [0.130, 1., 0.195, 0.129] atol=1.0e-1
      @test tail(x[:,4], x[:,5], "r", 0.0001) ≈ 0
      @test tail(x[:,1], x[:,5], "r", 0.0001) ≈ 0
      @test tail(x[:,4], x[:,5], "l", 0.0001) ≈ 0
      @test tail(x[:,1], x[:,5], "l", 0.0001) ≈ 0
      @test tail(x[:,6], x[:,7], "l", 0.0001) ≈ 0
      @test tail(x[:,6], x[:,7], "r", 0.0001) ≈ 0
      @test tail(x[:,1], x[:,2], "l", 0.0001) ≈ 0
      @test tail(x[:,1], x[:,2], "r", 0.0001) ≈ 0

      #test on correlations
      c1 = AMH_cop(2, .2, KendallCorrelation)
      cp = Nested_AMH_cop([c1], 1, 0.1, KendallCorrelation)
      x = simulate_copula(10_000, cp)
      @test corkendall(x)[:,1] ≈ [1, 0.2, 0.1] atol=1.0e-2
  end
end

@testset "nested Frank copula" begin
  @testset "exceptions" begin
    a = Frank_cop(2, 2.)
    b = Frank_cop(2, -1.)
    c = Frank_cop(2, 1.)
    @test_throws DomainError Nested_Frank_cop([a,b], 0, 0.5)
    @test_throws DomainError Nested_Frank_cop([a,c], -1, 0.1)

    u = zeros(5, 7)
    cp = Nested_Frank_cop([a,c], 0, 0.1)
    @test_throws AssertionError simulate_copula!(u, cp)
  end
  @testset "small data set" begin
    a = Frank_cop(2, 2.)
    b = Frank_cop(2, 3.)
    cp = Nested_Frank_cop([a,b], 1, 1.1)

    Random.seed!(43)
    rng = StableRNG(123)
    @test simulate_copula(1, cp; rng = rng) ≈ [0.0851 0.1911 0.5927 0.13458 0.31417] atol=1.0e-3

    Random.seed!(43)
    rng = StableRNG(123)
    u = zeros(1,5)
    simulate_copula!(u, cp; rng = rng)
    @test u ≈ [0.0851 0.1911 0.5927 0.13458 0.31417] atol=1.0e-3
  end
  @testset "large data set" begin

    a = Frank_cop(3, 8.)
    b = Frank_cop(2, 10.)
    cp = Nested_Frank_cop([a,b], 2, 2.)

    Random.seed!(43)
    x = simulate_copula(5_000, cp)
    @test pvalue(ExactOneSampleKSTest(x[:,1], Uniform(0,1))) > α
    @test pvalue(ExactOneSampleKSTest(x[:,2], Uniform(0,1))) > α
    @test pvalue(ExactOneSampleKSTest(x[:,3], Uniform(0,1))) > α
    @test pvalue(ExactOneSampleKSTest(x[:,4], Uniform(0,1))) > α

    @test corkendall(x)[1:4,1] ≈ [1.0, 0.60262, 0.60262, 0.2139] atol=1.0e-1
    @test corkendall(x)[3:5,4] ≈ [0.2139, 1.0, 0.6658] atol=1.0e-2
    @test corkendall(x)[6:7,6] ≈ [1.0, 0.2139] atol=1.0e-2
    @test tail(x[:,4], x[:,5], "r", 0.0001) ≈ 0
    @test tail(x[:,1], x[:,5], "r", 0.0001) ≈ 0
    @test tail(x[:,4], x[:,5], "l", 0.0001) ≈ 0
    @test tail(x[:,1], x[:,5], "l", 0.0001) ≈ 0
    @test tail(x[:,6], x[:,7], "r", 0.0001) ≈ 0
    @test tail(x[:,6], x[:,7], "l", 0.0001) ≈ 0
    @test tail(x[:,1], x[:,2], "l", 0.0001) ≈ 0
    @test tail(x[:,1], x[:,2], "r", 0.0001) ≈ 0

    # correlation tests
    c1 = Frank_cop(2, .6, KendallCorrelation)
    cp = Nested_Frank_cop([c1], 1, 0.2, KendallCorrelation)
    x = simulate_copula(10_000, cp)
    @test corkendall(x)[:,1] ≈ [1, 0.6, 0.2] atol=1.0e-1
  end
end

@testset "single nested Gumbel" begin
  @testset "exceptios" begin
    a = Gumbel_cop(2, 2.)
    b = Gumbel_cop(2, 1.5)
    @test_throws DomainError Nested_Gumbel_cop([a,b], 0, 1.7)
    @test_throws DomainError Nested_Gumbel_cop([a,b], -1, 1.1)

    u = zeros(5, 7)
    cp = Nested_Gumbel_cop([a,b], 1, 1.1)
    @test_throws AssertionError simulate_copula!(u, cp)
  end
  @testset "test on small data" begin
    a = Gumbel_cop(2, 2.)
    b = Gumbel_cop(2, 3.)
    cp = Nested_Gumbel_cop([a,b], 1, 1.1)
    Random.seed!(43)
    rng = StableRNG(123)
    @test simulate_copula(1, cp; rng = rng) ≈ [0.67589 0.740974 0.243834 0.06055 0.34778] atol=1.0e-5

    u = zeros(1,5)
    Random.seed!(43)
    rng = StableRNG(123)
    simulate_copula!(u, cp; rng = rng)
    @test u ≈ [0.67589 0.740974 0.243834 0.06055 0.34778] atol=1.0e-5

    Random.seed!(43)
    rng = StableRNG(123)
    n = nestedcopulag("gumbel", [[1,2], [3,4]], [2., 3.], 1.1,  [0.1 0.2 0.3 0.4 0.5; 0.1 0.2 0.3 0.4 0.5]; rng = rng)
    @test n ≈ [0.09401 0.13852 0.17391 0.20250; 0.25067 0.3145 0.42217 0.45506] atol=1.0e-3

  end
  @testset "test on larger data" begin
    a = Gumbel_cop(2, 4.2)
    b = Gumbel_cop(2, 6.1)
    cp = Nested_Gumbel_cop([a,b], 1, 2.1)

    Random.seed!(44)
    x = simulate_copula(1_00_000, cp)
    @test pvalue(ExactOneSampleKSTest(x[:,1], Uniform(0,1))) > α
    @test pvalue(ExactOneSampleKSTest(x[:,2], Uniform(0,1))) > α
    @test pvalue(ExactOneSampleKSTest(x[:,3], Uniform(0,1))) > α
    @test pvalue(ExactOneSampleKSTest(x[:,4], Uniform(0,1))) > α
    @test pvalue(ExactOneSampleKSTest(x[:,5], Uniform(0,1))) > α
    M = [1. 0.7619 0.52380 0.52380 0.52380; 0.7619 1. 0.52380 0.52380 0.52380; 0.52380 0.52380 1. 0.83606 0.52380]
    @test corkendall(x)[1:3,:] ≈ M atol=1.0e-2
    @test tail(x[:,1], x[:,2], "r", 0.01) ≈ 2-2^(1/4.2) atol=1.0e-1
    @test tail(x[:,2], x[:,3], "r", 0.01) ≈ 2-2^(1/2.1) atol=1.0e-1
    @test tail(x[:,1], x[:,5], "r", 0.01) ≈ 2-2^(1/2.1) atol=1.0e-1
    @test tail(x[:,3], x[:,4], "r", 0.01) ≈ 2-2^(1/6.1) atol=1.0e-1
    @test tail(x[:,1], x[:,2], "l", 0.000001) ≈ 0
    @test tail(x[:,3], x[:,4], "l", 0.000001) ≈ 0
    @test tail(x[:,1], x[:,3], "l", 0.00001) ≈ 0
    @test tail(x[:,2], x[:,3], "l", 0.00001) ≈ 0
    @test tail(x[:,1], x[:,5], "l", 0.00001) ≈ 0

    # correlation tests
    c1 = Gumbel_cop(2, .8, KendallCorrelation)
    cp = Nested_Gumbel_cop([c1], 1, 0.2, KendallCorrelation)
    x = simulate_copula(10_000, cp)
    @test corkendall(x)[:,1] ≈ [1, 0.8, 0.2] atol=1.0e-2
  end
end

@testset "double nested Gumbel copula" begin
  @testset "exceptions" begin
    a = Gumbel_cop(2, 4.2)
    b = Gumbel_cop(2, 6.1)
    cp = Nested_Gumbel_cop([a,b], 1, 2.1)

    a1 = Gumbel_cop(2, 4.2)
    b1 = Gumbel_cop(2, 5.1)
    cp1 = Nested_Gumbel_cop([a1,b1], 0, 3.1)

    @test_throws DomainError Double_Nested_Gumbel_cop([cp, cp1], 2.2)
    @test_throws DomainError Double_Nested_Gumbel_cop([cp, cp1], 0.9, KendallCorrelation)
    copula = Double_Nested_Gumbel_cop([cp, cp1], 1.2)
    u = rand(3,20)
    @test_throws AssertionError simulate_copula!(u, copula)
  end
  @testset "small data" begin
    Random.seed!(43)
    a = Gumbel_cop(2, 5.)
    b = Gumbel_cop(2, 6.)
    p1 = Nested_Gumbel_cop([a,b], 1, 2.)

    c = Gumbel_cop(2, 5.5)
    p2 = Nested_Gumbel_cop([c], 2, 2.1)

    copula = Double_Nested_Gumbel_cop([p1, p2], 1.5)
    Random.seed!(43)
    rng = StableRNG(123)
    @test simulate_copula(1, copula; rng = rng) ≈ [0.4058 0.43187 0.13949 0.08405 0.20398 0.41659 0.46121 0.68961 0.44060]  atol=1.0e-3

    u = zeros(1,9)
    Random.seed!(43)
    rng = StableRNG(123)
    simulate_copula!(u, copula; rng = rng)
    #println(u)
    @test u ≈ [0.4058 0.43187 0.13949 0.08405 0.20398 0.41659 0.46121 0.68961 0.44060] atol=1.0e-3

  end
  @testset "large data" begin
    a = Gumbel_cop(2, 4.1)
    b = Gumbel_cop(2, 3.8)
    cp = Nested_Gumbel_cop([a,b], 0, 1.9)

    a1 = Gumbel_cop(2, 5.1)
    b1 = Gumbel_cop(2, 6.1)
    cp1 = Nested_Gumbel_cop([a1, b1], 0, 2.4)
    cgp = Double_Nested_Gumbel_cop([cp, cp1], 1.2)

    Random.seed!(43)
    x = simulate_copula(10_000, cgp)
    @test pvalue(ExactOneSampleKSTest(x[:,1], Uniform(0,1))) > α
    @test pvalue(ExactOneSampleKSTest(x[:,2], Uniform(0,1))) > α
    @test pvalue(ExactOneSampleKSTest(x[:,3], Uniform(0,1))) > α
    @test pvalue(ExactOneSampleKSTest(x[:,4], Uniform(0,1))) > α
    @test corkendall(x)[1,:] ≈ [1., 0.7560, 0.47368, 0.47368, 1/6, 1/6, 1/6, 1/6] atol=1.0e-1
    @test tail(x[:,1], x[:,2], "r", 0.01) ≈ 2-2^(1/4.1) atol=1.0e-1
    @test tail(x[:,1], x[:,3], "r", 0.01) ≈ 2-2^(1/1.9) atol=1.0e-1
    @test tail(x[:,1], x[:,5], "r", 0.01) ≈ 2-2^(1/1.2) atol=1.0e-1
    @test tail(x[:,1], x[:,2], "l", 0.00001) ≈ 0
    @test tail(x[:,1], x[:,3], "l", 0.00001) ≈ 0

    # correlation tests
    a = Gumbel_cop(2, 0.8, KendallCorrelation)
    cp = Nested_Gumbel_cop([a], 1, 0.5, KendallCorrelation)

    a1 = Gumbel_cop(2, 0.7, KendallCorrelation)
    cp1 = Nested_Gumbel_cop([a1], 1, 0.4, KendallCorrelation)
    cgp = Double_Nested_Gumbel_cop([cp, cp1], 0.2, KendallCorrelation)

    x = simulate_copula(10_000, cgp)
    @test pvalue(ExactOneSampleKSTest(x[:,1], Uniform(0,1))) > α
    @test pvalue(ExactOneSampleKSTest(x[:,2], Uniform(0,1))) > α
    @test pvalue(ExactOneSampleKSTest(x[:,3], Uniform(0,1))) > α
    @test pvalue(ExactOneSampleKSTest(x[:,4], Uniform(0,1))) > α
    @test pvalue(ExactOneSampleKSTest(x[:,5], Uniform(0,1))) > α
    @test pvalue(ExactOneSampleKSTest(x[:,6], Uniform(0,1))) > α
    c1 = [1, 0.8, 0.5, 0.2, 0.2, 0.2]
    c2 = [0.8, 1, 0.5, 0.2, 0.2, 0.2]
    c3 = [0.5, 0.5, 1, 0.2, 0.2, 0.2]
    c4 = [0.2, 0.2, 0.2, 1., 0.7, 0.4]
    c5 = [0.2, 0.2, 0.2, 0.7, 1., 0.4]
    c6 = [0.2, 0.2, 0.2, .4, 0.4, 1.]
    C = corkendall(x)
    @test C[:,1] ≈ c1 atol=1.0e-1
    @test C[:,2] ≈ c2 atol=1.0e-1
    @test C[:,3] ≈ c3 atol=1.0e-1
    @test C[:,4] ≈ c4 atol=1.0e-1
    @test C[:,5] ≈ c5 atol=1.0e-1
    @test C[:,6] ≈ c6 atol=1.0e-1
  end
end

@testset "Hierarchical Gumbel copula" begin
  @testset "exceptions" begin
    @test_throws DomainError Hierarchical_Gumbel_cop([5., 6., 7.])
    @test_throws DomainError Hierarchical_Gumbel_cop([1.5, 1., 0.5])
    @test_throws DomainError Hierarchical_Gumbel_cop([0.6, 0.4, 0.6], KendallCorrelation)
    @test_throws DomainError Hierarchical_Gumbel_cop([0.6, 0.4, -0.6], KendallCorrelation)

    u = zeros(3,10)
    c = Hierarchical_Gumbel_cop([4., 3., 2.])
    @test_throws AssertionError simulate_copula!(u, c)
  end
  @testset "simple example" begin
    Random.seed!(43)
    rng = StableRNG(123)
    @test simulate_copula(1, Hierarchical_Gumbel_cop([2., 1.8, 1.7]); rng = rng) ≈ [0.23064 0.32542 0.3123 0.76877] atol=1.0e-3

    u = zeros(1,4)
    Random.seed!(43)
    rng = StableRNG(123)
    simulate_copula!(u, Hierarchical_Gumbel_cop([2., 1.8, 1.7]); rng = rng)
    @test u ≈  [0.23064 0.32542 0.3123 0.76877] atol=1.0e-3

  end
  @testset "larger example" begin

    Random.seed!(42)
    x = simulate_copula(25_000, Hierarchical_Gumbel_cop([4.2, 3.6, 1.1]))
    @test pvalue(ExactOneSampleKSTest(x[:,1], Uniform(0,1))) > α
    @test pvalue(ExactOneSampleKSTest(x[:,2], Uniform(0,1))) > α
    @test pvalue(ExactOneSampleKSTest(x[:,3], Uniform(0,1))) > α
    @test pvalue(ExactOneSampleKSTest(x[:,4], Uniform(0,1))) > α
    @test corkendall(x)[1:2,:] ≈ [1. 0.7619 0.72222 0.0909; 0.7619 1. 0.72222 0.0909] atol=1.0e-2
    @test tail(x[:,2], x[:,3], "r", 0.01) ≈ 2-2^(1/3.6) atol=1.0e-1
    @test tail(x[:,3], x[:,4], "r", 0.01) ≈ 2-2^(1/1.1) atol=1.0e-1
    @test tail(x[:,1], x[:,2], "l", 0.00001) ≈ 0

    # correlations
    Random.seed!(42)
    x = simulate_copula(10_000, Hierarchical_Gumbel_cop([0.9, 0.2], KendallCorrelation))
    @test pvalue(ExactOneSampleKSTest(x[:,1], Uniform(0,1))) > α
    @test pvalue(ExactOneSampleKSTest(x[:,2], Uniform(0,1))) > α
    @test pvalue(ExactOneSampleKSTest(x[:,3], Uniform(0,1))) > α
    c = corkendall(x)
    @test c[:,1] ≈ [1., 0.9, 0.2] atol=1.0e-2
    @test c[:,3] ≈ [0.2, 0.2, 1.] atol=1.0e-2
  end
end

@testset "test on Big Float" begin

    c1 = Gumbel_cop(2, BigFloat(3.))
    c2 = Gumbel_cop(3, BigFloat(4.))
    cp = Nested_Gumbel_cop([c1, c2], 2, BigFloat(1.5))

    Random.seed!(42)
    x = simulate_copula(100, cp)
    @test typeof(x) == Array{BigFloat,2}
    @test pvalue(ExactOneSampleKSTest(x[:,1], Uniform(0,1))) > α
    @test pvalue(ExactOneSampleKSTest(x[:,2], Uniform(0,1))) > α
    @test pvalue(ExactOneSampleKSTest(x[:,3], Uniform(0,1))) > α

    copula = Double_Nested_Gumbel_cop([cp, cp], BigFloat(1.2))
    Random.seed!(42)
    x = simulate_copula(100, cp)
    @test typeof(x) == Array{BigFloat,2}
    @test pvalue(ExactOneSampleKSTest(x[:,1], Uniform(0,1))) > α
    @test pvalue(ExactOneSampleKSTest(x[:,2], Uniform(0,1))) > α
    @test pvalue(ExactOneSampleKSTest(x[:,3], Uniform(0,1))) > α

    ch = Hierarchical_Gumbel_cop(BigFloat.([2., 1.8, 1.7]))
    x = simulate_copula(100, ch)
    @test typeof(x) == Array{BigFloat,2}
    @test pvalue(ExactOneSampleKSTest(x[:,1], Uniform(0,1))) > α
    @test pvalue(ExactOneSampleKSTest(x[:,2], Uniform(0,1))) > α
    @test pvalue(ExactOneSampleKSTest(x[:,3], Uniform(0,1))) > α

end


text:
##
import Optim: minimizer, optimize, minimum

mutable struct OptFun
    opt
    fun_idx::Int
end

string(opt::OptFun) = string(string(opt.opt),"\t", enumerate(BBOBFunction)[opt.fun_idx])

function benchmark(op::OptFun, run_lengths, Ntrials, dimensions, Δf)

    f = enumerate(BBOBFunction)[op.fun_idx]
    optimizer = op.opt
    @info("$(string(optimizer))\t $f")

    reached_minium = zeros(Bool,Ntrials,length(run_lengths),length(dimensions))
    distance_to_xopt = zeros(Ntrials,length(run_lengths),length(dimensions))
    fmin = zeros(Ntrials,length(run_lengths),length(dimensions))

    t = 0.0
    for j in 1:length(run_lengths), i in 1:Ntrials, k in 1:length(dimensions)
        try
            D = dimensions[k]
            t += @elapsed mfit = optimize(optimizer,f.f,D,run_lengths[j])

            reached_minium[i,j,k] = minimum(mfit) < Δf + f.f_opt
            fmin[i,j,k] = minimum(mfit) - f.f_opt
            distance_to_xopt[i,j,k] =  √sum(abs2.(minimizer(mfit) - f.x_opt[1:D]))

        catch err
            @warn(err)

            reached_minium[i,j,k] = false
            fmin[i,j,k] = NaN
            distance_to_xopt[i,j,k] = NaN
            @warn(string(optimizer, " failed :", err))
        end

    end
    t /= Ntrials*length(run_lengths)*length(dimensions)

    mean(reached_minium,dims=1), mean(distance_to_xopt,dims=1), mean(fmin,dims=1), t
end

benchmark(optimizer::Any, f::BBOBFunction, run_lengths, Ntrials, dimensions, Δf) = benchmark(
    OptFun(optimizer,findfirst(enumerate(BBOBFunction) .== f)), run_lengths, Ntrials, dimensions, Δf
)

benchmark(optimizer::Any, fun_idx::Int, run_lengths, Ntrials, dimensions, Δf) = benchmark(
    OptFun(optimizer,fun_idx), run_lengths, Ntrials, dimensions, Δf
)

#

function benchmark(optimizers::Vector{T}, funcs, run_lengths, Ntrials, dimensions, Δf) where T

    mean_succ = zeros(length(optimizers),length(funcs),length(run_lengths),length(dimensions))
    mean_dist, mean_fmin = similar(mean_succ), similar(mean_succ)
    runtime = zeros(length(optimizers),length(funcs))

    ops = [OptFun(optimizers[i],funcs[j]) for i=1:length(optimizers), j=1:length(funcs)]

    out = pmap(op->benchmark(op, run_lengths, Ntrials, dimensions, Δf), ops)

    for i=1:length(optimizers), j=1:length(funcs)
        mean_succ[i,j,:,:], mean_dist[i,j,:,:], mean_fmin[i,j,:,:], runtime[i,j] = out[i,j]
    end
    mean_succ, mean_dist, mean_fmin, runtime
end

##


text:
module TestFunctions

struct Rosenbrock
    f
    ∇f
    argmin
    Rosenbrock(; a = 1, b = 5) =
    new(x->(a - x[1])^2 + b * (x[2] - x[1]^2)^2,
        x->[
            -2 * (a - x[1]) - 4 * b * x[1] * (x[2] - x[1]^2),
            2 * b * (x[2] - x[1]^2)
        ],
        [a, a^2])
end

end # module


text:
# Apply dimensionless periodic boundary conditions
# Alex Tam, 12/10/2020

"Apply dimensionless periodic boundary conditions to segments and motors"
function periodic(pos1, pos2)
    translations = Vector{Vector{Int}}(undef, 5); # Pre-allocate vector of translation vectors
    translations[1] = [0,0]; # Ensure that the identity translation occurs first
    translations[2] = [floor(pos1[1]), floor(pos1[2])]; # Store 1st possible combination
    translations[3] = [floor(pos1[1]), floor(pos2[2])]; # Store 2nd possible combination
    translations[4] = [floor(pos2[1]), floor(pos1[2])]; # Store 3rd possible combination
    translations[5] = [floor(pos2[1]), floor(pos2[2])]; # Store 4th possible combination
    translations = unique(translations); # Delete repeated translations
    return translations
end

"Update all filament segment translations"
function segment_translations(s, af)
    for f in af
        for seg in f.segments
            seg.t = periodic(s.an[f.index][seg.index+1], s.an[f.index][seg.index]); # Apply periodic BCs
        end
    end
    return af
end


text:
export CircularCompactPSARTSATrajectory

using MacroTools: @forward

struct CircularCompactPSARTSATrajectory{T<:CircularCompactSARTSATrajectory,P,names,types} <:
       AbstractTrajectory{names,types}
    trajectory::T
    priority::P
end

"""
    CircularCompactPSARTSATrajectory(;kwargs)

Similar to [`CircularCompactSARTSATrajectory`](@ref), except that another trace named `priority` is added.

# Key word arguments

- `capacity::Int`, the maximum length of each trace.
- `state_type = Int`
- `state_size = ()`
- `action_type = Int`
- `action_size = ()`
- `reward_type = Float32`
- `reward_size = ()`
- `terminal_type = Bool`
- `terminal_size = ()`
- `priority_type = Float32`
"""
function CircularCompactPSARTSATrajectory(; priority_type = Float32, kw...)
    t = CircularCompactSARTSATrajectory(; kw...)
    p = SumTree(priority_type, kw.data.capacity)
    names = typeof(t).parameters[1]
    types = typeof(t).parameters[2]
    CircularCompactPSARTSATrajectory{
        typeof(t),
        typeof(p),
        (names..., :priority),
        Tuple{types.parameters...,eltype(p)},
    }(
        t,
        p,
    )
end

@forward CircularCompactPSARTSATrajectory.trajectory Base.length, Base.isempty

RLBase.get_trace(t::CircularCompactPSARTSATrajectory, s::Symbol) =
    s == :priority ? t.priority : get_trace(t.trajectory, s)

function Base.getindex(b::CircularCompactPSARTSATrajectory, i::Int)
    (
        state = select_last_dim(b.trajectory[:state], i),
        action = select_last_dim(b.trajectory[:action], i),
        reward = select_last_dim(b.trajectory[:reward], i),
        terminal = select_last_dim(b.trajectory[:terminal], i),
        next_state = select_last_dim(b.trajectory[:state], i + 1),
        next_action = select_last_dim(b.trajectory[:action], i + 1),
        priority = select_last_dim(b.priority, i),
    )
end

function Base.empty!(b::CircularCompactPSARTSATrajectory)
    empty!(b.priority)
    empty!(b.trajectory)
end

function Base.push!(b::CircularCompactPSARTSATrajectory, kv::Pair{Symbol})
    k, v = kv
    if k == :priority
        push!(b.priority, v)
    else
        push!(b.trajectory, kv)
    end
end

function Base.pop!(t::CircularCompactPSARTSATrajectory, s::Symbol)
    if s == :priority
        pop!(t.priority)
    else
        pop!(t.trajectory, s)
    end
end

function Base.pop!(t::CircularCompactPSARTSATrajectory)
    (pop!(t.trajectory)..., priority = pop!(t.priority))
end


text:
__precompile__()
module SeisIO
using Blosc, Dates, DSP, FFTW, LightXML, LinearAlgebra, Markdown, Mmap, Printf, Sockets
using DelimitedFiles: readdlm
using Glob: glob
using HTTP: request, Messages.statustext
using Statistics: mean
Blosc.set_compressor("lz4")
Blosc.set_num_threads(Sys.CPU_THREADS)
path = Base.source_dir()

# DO NOT CHANGE IMPORT ORDER
include("imports.jl")
include("constants.jl")

# =========================================================
# CoreUtils: SeisIO needs these for core functions
# DO NOT CHANGE ORDER OF INCLUSIONS
include("CoreUtils/IO/FastIO.jl")
using .FastIO

include("CoreUtils/ls.jl")
include("CoreUtils/time.jl")
include("CoreUtils/namestrip.jl")
include("CoreUtils/typ2code.jl")
include("CoreUtils/poly.jl")
include("CoreUtils/calculus.jl")
include("CoreUtils/svn.jl")
include("CoreUtils/IO/read_utils.jl")
include("CoreUtils/IO/string_vec_and_misc.jl")

# =========================================================
# Types and methods
# DO NOT CHANGE ORDER OF INCLUSIONS

# Back-end types
include("Types/KWDefs.jl")

# Prereqs for custom types
include("Types/InstPosition.jl")
include("Types/InstResp.jl")

# IO buffer including location and instrument response buffers
include("Types/SeisIOBuf.jl")

# Abstract types
include("Types/GphysData.jl")
include("Types/GphysChannel.jl")

# Custom types
include("Types/SeisData.jl")
include("Types/SeisChannel.jl")

for i in ls(path*"/Types/Methods/")
  if endswith(i, ".jl")
    include(i)
  end
end

# =========================================================
# Logging
for i in ls(path*"/Logging/*")
  if endswith(i, ".jl")
    include(i)
  end
end

# =========================================================
# Utilities that may require SeisIO types to work
for i in ls(path*"/Utils/")
  if endswith(i, ".jl")
    include(i)
  end
end
include("Utils/Parsing/streams.jl")
include("Utils/Parsing/strings.jl")
include("Utils/Parsing/buffers.jl")

# =========================================================
# Data processing operations
for i in ls(path*"/Processing/*")
  if endswith(i, ".jl")
    include(i)
  end
end

# =========================================================
# Data formats
for i in ls(path*"/Formats/")
  if endswith(i, ".jl")
    include(i)
  end
end

# =========================================================
# Web clients
for i in ls(path*"/Web/")
  if endswith(i, ".jl")
    include(i)
  end
end

# =========================================================
# Submodules
#= Dependencies
.FastIO
    └---------------> Types/
            ╭-----------┴-----------------------┐
         .Quake                                 |
    ╭-------┴---┬--------┬--------┐         ╭---┴---┐
.RandSeis   .SeisHDF    .UW     .SUDS    .ASCII   .SEED
                └--------+--------┘         └---┬---┘
                         |                      |
                         v                      |
                      Wrappers/ <---------------┘
=#

include("Submodules/FormatGuide.jl")
using .Formats

include("Submodules/ASCII.jl")
using .ASCII

include("Submodules/Nodal.jl")
using .Nodal
import .Nodal: convert

include("Submodules/SEED.jl")
using .SEED
using .SEED: parserec!, read_seed_resp!, seed_cleanup!
export mseed_support, read_dataless, read_seed_resp!, read_seed_resp, RESP_wont_read, seed_support

# We need these types for the native file format
include("Submodules/Quake.jl")
using .Quake
import .Quake: convert, fwrite_note_quake!, merge_ext!
export read_qml, write_qml

include("Submodules/RandSeis.jl")

include("Submodules/SeisHDF.jl")
using .SeisHDF: read_hdf5, read_hdf5!, scan_hdf5, write_hdf5
export read_hdf5, read_hdf5!, scan_hdf5, write_hdf5

include("Submodules/SUDS.jl")

include("Submodules/UW.jl")

# =========================================================
# Wrappers
for i in ls(path*"/Wrappers/")
  if endswith(i, ".jl")
    include(i)
  end
end

formats["list"] = collect(keys(formats))

# Last steps
include("Last/splat.jl")
include("Last/native_file_io.jl")
include("Last/read_legacy.jl")
include("Last/set_file_ver.jl")

# Module ends
end


text:
######################################################################
# smc_trajectory.jl
#
# Here I try to apply the original way of doing it.
######################################################################

export smc_trajectory

function smc_trajectory(em::ErgodicManagerR2, tm::TrajectoryManager; verbose::Bool=true, umax::Float64=1.0)

    ud = VVF(tm.N)
    xd = VVF(tm.N+1)
    xd[1] = deepcopy(tm.x0)
    ck = zeros(em.K+1, em.K+1)
    for n = 1:tm.N
        # compute B
        Bx, By = compute_B(em, xd, n, tm.h, ck)

        # normalize
        den = sqrt(Bx*Bx + By*By)
        ux = -umax * Bx / den
        uy = -umax * By / den

        ud[n] = [ux, uy]
        xd[n+1] = integrate(tm, xd[n], ud[n])
    end

    return xd, ud
end

# TODO: this can be done a lot quicker
function compute_B(em::ErgodicManagerR2, xd::VVF, n::Int, h::Float64, ck::Matrix{Float64})
    Lx = em.domain.lengths[1]
    Ly = em.domain.lengths[2]
    dxn = xd[n][1] - x_min(em)
    dyn = xd[n][2] - y_min(em)
    Bx = By = 0.0
    for k1 = 0:em.K, k2 = 0:em.K
        hk = em.hk[k1+1, k2+1]
        cx = cos(k1*pi*dxn/Lx)
        cy = cos(k2*pi*dyn/Ly)

        # compute ck
        fk = ( (n-1)*ck[k1+1,k2+1] + cx*cy/hk ) / n
        ck[k1+1, k2+1] = fk

        # t * Lambda_k * (c_k - phi_k)
        LS = em.Lambda[k1+1,k2+1] * h * n * (fk - em.phik[k1+1, k2+1])

        # multiplying by gradient
        Bx += -LS * k1*pi * sin(k1*pi*dxn/Lx) * cy / (hk*Lx)
        By += -LS * k2*pi * cx * sin(k2*pi*dyn/Ly) / (hk*Ly)
    end
    return Bx, By
end

function smc_trajectory(em::ErgodicManagerR2T, tm::TrajectoryManager; verbose::Bool=true, umax::Float64=1.0)

    ud = VVF(tm.N)
    xd = VVF(tm.N+1)
    xd[1] = deepcopy(tm.x0)
    ck = zeros(em.K+1, em.K+1)
    for n = 1:tm.N
        # compute B
        Bx, By = compute_B(em, xd, n, tm.h, ck)

        # normalize
        den = sqrt(Bx*Bx + By*By)
        ux = -umax * Bx / den
        uy = -umax * By / den

        ud[n] = [ux, uy]
        xd[n+1] = integrate(tm, xd[n], ud[n])
    end

    return xd, ud
end

# TODO: this can be done a lot quicker
function compute_B(em::ErgodicManagerR2T, xd::VVF, n::Int, h::Float64, ck::Matrix{Float64})
    Lx = em.domain.lengths[1]
    Ly = em.domain.lengths[2]
    dxn = xd[n][1] - x_min(em)
    dyn = xd[n][2] - y_min(em)
    Bx = By = 0.0
    for k1 = 0:em.K, k2 = 0:em.K
        hk = em.hk[k1+1, k2+1]
        cx = cos(k1*pi*dxn/Lx)
        cy = cos(k2*pi*dyn/Ly)

        # compute ck
        fk = ( (n-1)*ck[k1+1,k2+1] + cx*cy/hk ) / n
        ck[k1+1, k2+1] = fk

        # t * Lambda_k * (c_k - phi_k)
        LS = em.Lambda[k1+1,k2+1] * h * n * (fk - em.phik[k1+1, k2+1])

        # multiplying by gradient
        Bx += -LS * k1*pi * sin(k1*pi*dxn/Lx) * cy / (hk*Lx)
        By += -LS * k2*pi * cx * sin(k2*pi*dyn/Ly) / (hk*Ly)
    end
    return Bx, By
end

function smc_trajectory(em::ErgodicManagerR3, tm::TrajectoryManager; verbose::Bool=true, umax::Float64=1.0)

    ud = VVF(tm.N)
    xd = VVF(tm.N+1)
    xd[1] = deepcopy(tm.x0)
    ck = zeros(em.K+1, em.K+1, em.K+3)
    for n = 1:tm.N
        # compute B
        Bx, By, Bz = compute_B(em, xd, n, tm.h, ck)

        # normalize
        den = sqrt(Bx*Bx + By*By + Bz*Bz)
        ux = -umax * Bx / den
        uy = -umax * By / den
        uz = -umax * Bz / den

        ud[n] = [ux, uy, uz]
        xd[n+1] = integrate(tm, xd[n], ud[n])
    end

    return xd, ud
end

function compute_B(em::ErgodicManagerR3, xd::VVF, n::Int, h::Float64, ck::Array{Float64,3})
    Lx = em.domain.lengths[1]
    Ly = em.domain.lengths[2]
    Lz = em.domain.lengths[3]

    dxn = xd[n][1] - x_min(em)
    dyn = xd[n][2] - y_min(em)
    dzn = xd[n][3] - z_min(em)

    Bx = By = Bz = 0.0

    for k1 = 0:em.K, k2 = 0:em.K, k3 = 0:em.K
        hk = em.hk[k1+1, k2+1, k3+1]
        cx = cos(k1*pi*dxn/Lx)
        cy = cos(k2*pi*dyn/Ly)
        cz = cos(k3*pi*dzn/Lz)

        # compute ck
        fk = ( (n-1)*ck[k1+1,k2+1,k3+1] + cx*cy*cz/hk ) / n
        ck[k1+1, k2+1, k3+1] = fk

        # t * Lambda_k * (c_k - phi_k)
        LS = em.Lambda[k1+1,k2+1,k3+1]*h*n*(fk - em.phik[k1+1, k2+1, k3+1])

        # multiplying by gradient
        Bx += -LS * k1*pi * sin(k1*pi*dxn/Lx) * cy * cz / (hk*Lx)
        By += -LS * k2*pi * cx * sin(k2*pi*dyn/Ly) * cz / (hk*Ly)
        Bz += -LS * k3*pi * cx * cy * sin(k3*pi*dzn/Lz) / (hk*Lz)
    end
    return Bx, By, Bz
end


text:
# ------------------------------------------------------------------
# Licensed under the MIT License. See LICENCE in the project root.
# ------------------------------------------------------------------

function imfilter_cpu(img::AbstractArray{T,N},
                      kern::AbstractArray{K,N}) where {T<:Real,K<:Real,N}
  imfilter(img, centered(kern), Inner(), Algorithm.FFT())
end


text:
"""
    ParaxialLens{T} <: Surface{T}

`surfacenormal` is the **output** direction of the lens.
Paraxial lens cannot act as the interface between two materials, hence only a single outside material is specified, by default Air.

Create with the following functions
```julia
ParaxialLensEllipse(focaldistance, halfsizeu, halfsizev, surfacenormal, centrepoint; rotationvec = [0.0, 1.0, 0.0], outsidematerial = OpticSim.GlassCat.Air, decenteruv = (0.0, 0.0))
ParaxialLensRect(focaldistance, halfsizeu, halfsizev, surfacenormal, centrepoint; rotationvec = [0.0, 1.0, 0.0], outsidematerial = OpticSim.GlassCat.Air, decenteruv = (0.0, 0.0))
ParaxialLensHex(focaldistance, side_length, surfacenormal, centrepoint; rotationvec = [0.0, 1.0, 0.0], outsidematerial = OpticSim.GlassCat.Air, decenteruv = (0.0, 0.0))
```
"""
struct ParaxialLens{T} <: Surface{T}
    shape::Union{Rectangle{T},Ellipse{T},Hexagon{T}}
    interface::ParaxialInterface{T}

    function ParaxialLens(shape::Union{Rectangle{T},Ellipse{T},Hexagon{T}}, interface::ParaxialInterface{T}) where {T<:Real}
        new{T}(shape, interface)
    end
end

function ParaxialLensRect(focaldistance::T, halfsizeu::T, halfsizev::T, surfacenormal::AbstractArray{T,1}, centrepoint::AbstractArray{T,1}; rotationvec::AbstractArray{T,1} = SVector{3,T}(0.0, 1.0, 0.0), outsidematerial::OpticSim.GlassCat.AbstractGlass = OpticSim.GlassCat.Air, decenteruv::Tuple{T,T} = (zero(T), zero(T))) where {T<:Real}
    @assert length(surfacenormal) == 3 && length(centrepoint) == 3
    return ParaxialLensRect(focaldistance, halfsizeu, halfsizev, SVector{3,T}(surfacenormal), SVector{3,T}(centrepoint), rotationvec = SVector{3,T}(rotationvec), outsidematerial = outsidematerial, decenteruv = decenteruv)
end

function ParaxialLensRect(focaldistance::T, halfsizeu::T, halfsizev::T, surfacenormal::SVector{3,T}, centrepoint::SVector{3,T}; rotationvec::SVector{3,T} = SVector{3,T}(0.0, 1.0, 0.0), outsidematerial::OpticSim.GlassCat.AbstractGlass = OpticSim.GlassCat.Air, decenteruv::Tuple{T,T} = (zero(T), zero(T))) where {T<:Real}
    r = Rectangle(halfsizeu, halfsizev, surfacenormal, centrepoint)
    centrepoint = centrepoint + decenteruv[1] * r.uvec + decenteruv[2] * r.vvec
    return ParaxialLens(r, ParaxialInterface(focaldistance, centrepoint, outsidematerial))
end

function ParaxialLensHex(focaldistance::T, side_length::T, surfacenormal::AbstractArray{T,1}, centrepoint::AbstractArray{T,1}; rotationvec::AbstractArray{T,1} = SVector{3,T}(0.0, 1.0, 0.0), outsidematerial::OpticSim.GlassCat.AbstractGlass = OpticSim.GlassCat.Air, decenteruv::Tuple{T,T} = (zero(T), zero(T))) where {T<:Real}
    @assert length(surfacenormal) == 3 && length(centrepoint) == 3
    return ParaxialLensHex(focaldistance, side_length, SVector{3,T}(surfacenormal), SVector{3,T}(centrepoint), rotationvec = SVector{3,T}(rotationvec), outsidematerial = outsidematerial, decenteruv = decenteruv)
end

function ParaxialLensHex(focaldistance::T, side_length::T, surfacenormal::SVector{3,T}, centrepoint::SVector{3,T}; rotationvec::SVector{3,T} = SVector{3,T}(0.0, 1.0, 0.0), outsidematerial::OpticSim.GlassCat.AbstractGlass = OpticSim.GlassCat.Air, decenteruv::Tuple{T,T} = (zero(T), zero(T))) where {T<:Real}
    h = Hexagon(side_length, surfacenormal, centrepoint)
    centrepoint = centrepoint + decenteruv[1] * h.uvec + decenteruv[2] * h.vvec
    return ParaxialLens(h, ParaxialInterface(focaldistance, centrepoint, outsidematerial))
end

function ParaxialLensEllipse(focaldistance::T, halfsizeu::T, halfsizev::T, surfacenormal::AbstractArray{T,1}, centrepoint::AbstractArray{T,1}; rotationvec::AbstractArray{T,1} = SVector{3,T}(0.0, 1.0, 0.0), outsidematerial::OpticSim.GlassCat.AbstractGlass = OpticSim.GlassCat.Air, decenteruv::Tuple{T,T} = (zero(T), zero(T))) where {T<:Real}
    @assert length(surfacenormal) == 3 && length(centrepoint) == 3
    return ParaxialLensEllipse(focaldistance, halfsizeu, halfsizev, SVector{3,T}(surfacenormal), SVector{3,T}(centrepoint), rotationvec = SVector{3,T}(rotationvec), outsidematerial = outsidematerial, decenteruv = decenteruv)
end

function ParaxialLensEllipse(focaldistance::T, halfsizeu::T, halfsizev::T, surfacenormal::SVector{3,T}, centrepoint::SVector{3,T}; rotationvec::SVector{3,T} = SVector{3,T}(0.0, 1.0, 0.0), outsidematerial::OpticSim.GlassCat.AbstractGlass = OpticSim.GlassCat.Air, decenteruv::Tuple{T,T} = (zero(T), zero(T))) where {T<:Real}
    e = Ellipse(halfsizeu, halfsizev, surfacenormal, centrepoint)
    centrepoint = centrepoint + decenteruv[1] * e.uvec + decenteruv[2] * e.vvec
    return ParaxialLens(e, ParaxialInterface(focaldistance, centrepoint, outsidematerial))
end

export ParaxialLens, ParaxialLensRect, ParaxialLensEllipse, ParaxialLensHex

interface(r::ParaxialLens{T}) where {T<:Real} = r.interface
centroid(r::ParaxialLens{T}) where {T<:Real} = centroid(r.shape)
normal(r::ParaxialLens{T}) where {T<:Real} = normal(r.shape)
normal(r::ParaxialLens{T}, ::T, ::T) where {T<:Real} = normal(r)
point(r::ParaxialLens{T}, u::T, v::T) where {T<:Real} = point(r.shape, u, v)
uv(r::ParaxialLens{T}, x::T, y::T, z::T) where {T<:Real} = uv(r, SVector{3,T}(x, y, z))
uv(r::ParaxialLens{T}, p::SVector{3,T}) where {T<:Real} = uv(r.shape, p)

function surfaceintersection(l::ParaxialLens{T}, r::AbstractRay{T,3}) where {T<:Real}
    itvl = surfaceintersection(l.shape, r)
    if itvl isa EmptyInterval{T}
        return EmptyInterval(T)
    else
        intsct = halfspaceintersection(itvl)
        u, v = uv(intsct)
        intsct = Intersection(α(intsct), point(intsct), normal(intsct), u, v, interface(l), flippednormal = flippednormal(intsct))
        return positivehalfspace(intsct)
    end
end

makemesh(l::ParaxialLens{T}, ::Int = 0) where {T<:Real} = makemesh(l.shape)

function processintersection(opticalinterface::ParaxialInterface{T}, point::SVector{N,T}, normal::SVector{N,T}, incidentray::OpticalRay{T,N}, temperature::T, pressure::T, ::Bool, firstray::Bool = false) where {T<:Real,N}
    raypower = power(incidentray)
    m = outsidematerialid(opticalinterface) # necessarily both the same
    n = one(T)
    if !isair(m)
        mat = glassforid(m)::OpticSim.GlassCat.Glass
        n = index(mat, wavelength(incidentray), temperature = temperature, pressure = pressure)::T
    end
    geometricpathlength = norm(point - origin(incidentray)) + (firstray ? zero(T) : T(RAY_OFFSET))
    thisraypathlength = n * geometricpathlength
    raypathlength = pathlength(incidentray) + thisraypathlength
    # refraction calculated directly for paraxial lens
    raydirection = normalize((opticalinterface.centroid - point) + direction(incidentray) * opticalinterface.focallength / dot(normal, direction(incidentray)))
    if opticalinterface.focallength < 0
        # flip for negative focal lengths
        raydirection = -raydirection
    end
    return raydirection, raypower, raypathlength
end


text:
function checkControlAvailability(basedir, binsize=100, excludes=[])
    controlFiles = collect(Set([i[1:end-8] for i in readdir(basedir)]))
    availableControls = String[]
    mask = Bool[]
    for c in controlFiles
        if isfile("$(basedir)$(c).fbin$(binsize)") && isfile("$(basedir)$(c).rbin$(binsize)")
            if !(c[1:end-4] in excludes)
                push!(availableControls, c)
                push!(mask, true)
            else
                push!(mask, false)
            end
        end
    end
    availableControls, mask
end

function loadControls(controlFiles, basedir, direction, suffix)
    readers = BinnedReader[]
    control_names = Any[]
    for i in 1:length(controlFiles)
        filename = "$(basedir)$(controlFiles[i])$(direction)$(suffix)"
        push!(readers, BinnedReader(filename))
        push!(control_names, filename)
    end
    readers, control_names
end