text:
"""
Defines different one-dimensional FFT-based transforms.

The transforms are all subtypes of an [`AbstractTransform`](@ref) type.

When possible, the names of the transforms are kept consistent with the
functions exported by
[`AbstractFFTs.jl`](https://juliamath.github.io/AbstractFFTs.jl/stable/api/)
and [`FFTW.jl`](https://juliamath.github.io/FFTW.jl/stable/fft/).
"""
module Transforms

using FFTW

# Operations defined for custom plans (currently IdentityPlan).
using LinearAlgebra

export binv, scale_factor, is_inplace
export eltype_input, eltype_output, length_output, plan, expand_dims

const FFTReal = FFTW.fftwReal  # = Union{Float32, Float64}
const RealArray{T} = AbstractArray{T} where T <: FFTReal
const ComplexArray{T} = AbstractArray{T} where T <: Complex

"""
    AbstractTransform

Specifies a one-dimensional FFT-based transform.
"""
abstract type AbstractTransform end

"""
    AbstractCustomPlan

Abstract type defining a custom plan, to be used as an alternative to FFTW
plans (`FFTW.FFTWPlan`).

The only custom plan defined in this module is [`IdentityPlan`](@ref).
The user can define other custom plans that are also subtypes of
`AbstractCustomPlan`.

Note that [`plan`](@ref) returns a subtype of either `FFTW.FFTWPlan` or
`AbstractCustomPlan`.
"""
abstract type AbstractCustomPlan end

"""
    Plan = Union{FFTW.FFTWPlan, AbstractCustomPlan}

Union type representing any plan returned by [`plan`](@ref).

See also [`AbstractCustomPlan`](@ref).
"""
const Plan = Union{FFTW.FFTWPlan, AbstractCustomPlan}

"""
    plan(transform::AbstractTransform, A, [dims];
         flags=FFTW.ESTIMATE, timelimit=Inf)

Create plan to transform array `A` along dimensions `dims`.

If `dims` is not specified, all dimensions of `A` are transformed.

For FFT plans, this function wraps the `AbstractFFTs.jl` and `FFTW.jl` plan
creation functions.
For more details on the function arguments, see
[`AbstractFFTs.plan_fft`](https://juliamath.github.io/AbstractFFTs.jl/stable/api/#AbstractFFTs.plan_fft).

In particular, note that for [`BRFFT`](@ref) plans, this function also requires
the length `d` of the transform output along the first transformed dimension.
This is described in the
[`AbstractFFTs.irfft`](https://juliamath.github.io/AbstractFFTs.jl/stable/api/#AbstractFFTs.irfft)
docs.
"""
function plan end

function plan(t::AbstractTransform, A; kwargs...)
    # Instead of passing dims = 1:N, we pass a tuple (1, 2, ..., N) to make sure
    # that the length of dims is known at compile time. This is important for
    # guessing the return type of r2r plans, which in principle are type
    # unstable (see comments in r2r.jl).
    N = ndims(A)
    dims = ntuple(identity, Val(N))  # (1, 2, ..., N)
    plan(t, A, dims; kwargs...)
end

"""
    binv(transform::AbstractTransform)

Returns the backwards transform associated to the given transform.

The backwards transform returned by this function is not normalised. The
normalisation factor for a given array can be obtained by calling
[`scale_factor`](@ref).

# Example

```jldoctest
julia> binv(Transforms.FFT())
BFFT

julia> binv(Transforms.BRFFT())
RFFT
```
"""
function binv end

"""
    is_inplace(transform::AbstractTransform)         -> Bool
    is_inplace(transforms::Vararg{AbtractTransform}) -> Union{Bool, Nothing}

Check whether a transform or a list of transforms is performed in-place.

If the list of transforms has a combination of in-place and out-of-place
transforms, `nothing` is returned.

# Example

```jldoctest; setup = :(import FFTW)
julia> is_inplace(Transforms.RFFT())
false

julia> is_inplace(Transforms.NoTransform!())
true

julia> is_inplace(Transforms.FFT!(), Transforms.R2R!(FFTW.REDFT01))
true

julia> is_inplace(Transforms.FFT(), Transforms.R2R(FFTW.REDFT01))
false

julia> is_inplace(Transforms.FFT(), Transforms.R2R!(FFTW.REDFT01)) === nothing
true

```
"""
function is_inplace end

@inline function is_inplace(tr::AbstractTransform, tr2::AbstractTransform,
                            next::Vararg{AbstractTransform})
    b = is_inplace(tr2, next...)
    b === nothing && return nothing
    a = is_inplace(tr)
    a === b ? a : nothing
end

"""
    scale_factor(transform::AbstractTransform, A, [dims])

Get factor required to normalise the given array after a transformation along
dimensions `dims` (all dimensions by default).

The array `A` must have the dimensions of the `transform` output.

**Important**: the dimensions `dims` must be the same that were passed to
[`plan`](@ref).

# Examples

```jldoctest scale_factor
julia> C = zeros(ComplexF32, 3, 4, 5);

julia> scale_factor(Transforms.FFT(), C)
60

julia> scale_factor(Transforms.BFFT(), C)
60

julia> scale_factor(Transforms.BFFT(), C, 2:3)
20

julia> R = zeros(Float64, 3, 4, 5);

julia> scale_factor(Transforms.BRFFT(), R, 2)
4

julia> scale_factor(Transforms.BRFFT(), R, 2:3)
20
```

This will fail because the output of `RFFT` is complex, and `R` is a real array:
```jldoctest scale_factor
julia> scale_factor(Transforms.RFFT(), R, 2:3)
ERROR: MethodError: no method matching scale_factor(::PencilFFTs.Transforms.RFFT, ::Array{Float64, 3}, ::UnitRange{Int64})
```
"""
function scale_factor end

scale_factor(t::AbstractTransform, A) = scale_factor(t, A, 1:ndims(A))

"""
    length_output(transform::AbstractTransform, length_in::Integer)

Returns the length of the transform output, given the length of its input.

The input and output lengths are specified in terms of the respective input
and output datatypes.
For instance, for real-to-complex transforms, these are respectively the
length of input *real* data and of output *complex* data.

Also note that for backward real-to-complex transforms ([`BRFFT`](@ref)), it is
assumed that the real data length is even. See also
the [`AbstractFFTs.irfft`
docs](https://juliamath.github.io/AbstractFFTs.jl/stable/api/#AbstractFFTs.irfft).
"""
function length_output end

"""
    eltype_input(transform::AbstractTransform, real_type<:AbstractFloat)

Determine input data type for a given transform given the floating point
precision of the input data.

Some transforms, such as [`R2R`](@ref) and [`NoTransform`](@ref), can take both
real and complex data. For those kinds of transforms, `Nothing` is returned.

# Example

```jldoctest; setup = :(import FFTW)
julia> eltype_input(Transforms.FFT(), Float32)
ComplexF32 (alias for Complex{Float32})

julia> eltype_input(Transforms.RFFT(), Float64)
Float64

julia> eltype_input(Transforms.R2R(FFTW.REDFT01), Float64)
Nothing

julia> eltype_input(Transforms.NoTransform(), Float64)
Nothing

```
"""
function eltype_input end

"""
    eltype_output(transform::AbstractTransform, eltype_input)

Returns the output data type for a given transform given the input type.

Throws `ArgumentError` if the input data type is incompatible with the transform
type.

# Example

```jldoctest
julia> eltype_output(Transforms.NoTransform(), Float32)
Float32

julia> eltype_output(Transforms.RFFT(), Float64)
ComplexF64 (alias for Complex{Float64})

julia> eltype_output(Transforms.BRFFT(), ComplexF32)
Float32

julia> eltype_output(Transforms.FFT(), Float64)
ERROR: ArgumentError: invalid input data type for PencilFFTs.Transforms.FFT: Float64
```
"""
function eltype_output end

eltype_output(::F, ::Type{T}) where {F <: AbstractTransform, T} =
    throw(ArgumentError("invalid input data type for $F: $T"))

"""
    expand_dims(transform::AbstractTransform, Val(N))

Expand a single multidimensional transform into one transform per dimension.

# Example

```jldoctest
# Expand a real-to-complex transform in 3 dimensions.
julia> expand_dims(Transforms.RFFT(), Val(3))
(RFFT, FFT, FFT)

julia> expand_dims(Transforms.BRFFT(), Val(3))
(BRFFT, BFFT, BFFT)

julia> expand_dims(Transforms.NoTransform(), Val(2))
(NoTransform, NoTransform)
```
"""
function expand_dims end

expand_dims(::F, ::Val) where {F <: AbstractTransform} =
    throw(ArgumentError("I don't know how to expand transform $F"))

Base.show(io::IO, ::F) where F <: AbstractTransform = print(io, nameof(F))

include("c2c.jl")
include("r2c.jl")
include("r2r.jl")
include("no_transform.jl")

end


text:
include("../src/MAPElites.jl")
using Test
import YAML
using BlackBoxOptimizationBenchmarking

function test_struct(map::MapElites)
    @test typeof(map.feature_dimension) == Int64
    @test typeof(map.grid_mesh) == Int64
    @test typeof(map.solutions) == SparseArray{Union{Missing,Individual},map.feature_dimension}
    @test typeof(map.performances) == SparseArray{Union{Missing,Array{Float64}},map.feature_dimension}
    # test logics between dimension
    @test length(size(map.solutions)) == map.feature_dimension
    @test length(size(map.performances)) == map.feature_dimension
    for i in 1:map.feature_dimension
        @test size(map.solutions)[i] == map.grid_mesh
        @test size(map.performances)[i] == map.grid_mesh
    end
end

function test_donotchange_struct(map1::MapElites,map2::MapElites)
    @test map1.feature_dimension == map2.feature_dimension
    @test map1.grid_mesh == map2.grid_mesh
    @test typeof(map1.solutions) == typeof(map2.solutions)
    @test typeof(map1.performances) == typeof(map2.performances)
    @test size(map1.solutions) == size(map2.solutions)
    @test size(map1.performances) == size(map2.performances)
end

function map_ind_toyproblem(ind::Individual)
    x = ind.genes[1]
    y = ind.genes[2]
    grid_mesh = 100

    x_ind = Int(trunc(grid_mesh*x)) + 1
    y_ind = Int(trunc(grid_mesh*y)) + 1
    [x_ind,y_ind]
end

function rosenbrock(i::Individual)
    x = i.genes
    y = -(sum([(1.0 - x[i])^2 + 100.0 * (x[i+1] - x[i]^2)^2
              for i in 1:(length(x)-1)]))
    [y]
end

@testset "MAPElites.jl" begin
    @testset "mapelites_struct.jl" begin
        # a first test simple enough fill with missing value
        map_el = MapElites(1,1)
        test_struct(map_el)
        @test isequal(map_el.solutions[1],missing)
        @test isequal(map_el.performances[1],missing)
        # a more difficult test fill with missing value
        map_el = MapElites(2,2)
        test_struct(map_el)
        @test isequal(map_el.solutions[1,1],missing)
        @test isequal(map_el.performances[1,1],missing)
        @test isequal(map_el.solutions[1,2],missing)
        @test isequal(map_el.performances[1,2],missing)
        @test isequal(map_el.solutions[2,1],missing)
        @test isequal(map_el.performances[2,1],missing)
        @test isequal(map_el.solutions[2,2],missing)
        @test isequal(map_el.performances[2,2],missing)
        # try a 3-D array with a mesh of 5 fill with missing value
        map_el = MapElites(3,5)
        test_struct(map_el)
        @test size(map_el.solutions) == (5,5,5)
        @test size(map_el.performances) == (5,5,5)
        @test isequal(map_el.solutions[3,2,1],missing)
        @test isequal(map_el.performances[3,2,1],missing)
        # test of the function test_donotchange_struct
        test_donotchange_struct(map_el,map_el)
        # map_el2 = MapElites(3,4)
        # test_donotchange_struct(map_el,map_el2)
    end

    @testset "mapelites_functions.jl" begin
        @testset "count element" begin
            map_el = MapElites(2,2)
            @test count_elt(map_el) == 0
            map_el.solutions[1,1] = Individual([0,1,0],[10.0])
            map_el.performances[1,1] = [10.0]
            @test count_elt(map_el) == 1
            map_el.solutions[1,2] = Individual([1,1,0],[15.0])
            map_el.performances[1,2] = [15.0]
            @test count_elt(map_el) == 2
            map_el = MapElites(3,4)
            @test count_elt(map_el) == 0
            map_el.solutions[1,1,1] = Individual([0,1,0],[10.0])
            map_el.performances[1,1,1] = [10.0]
            @test count_elt(map_el) == 1
            map_el.solutions[2,2,3] = Individual([1,1,0],[15.0])
            map_el.performances[2,2,3] = [15.0]
            @test count_elt(map_el) == 2
            test_struct(map_el)
        end

        @testset "add_to_map function" begin
            map_el = MapElites(3,10)
            map_el_old = deepcopy(map_el)
            coordinate = [5,4,3]
            solution = Individual([1,1,0],[5.0])
            performance = [5.0]
            add_to_map(map_el,coordinate,solution,performance)
            # check if we do not alterate structure by adding information
            test_struct(map_el)
            test_donotchange_struct(map_el_old,map_el)
            # check if we actually add at the right place our Individual
            @test count_elt(map_el)==count_elt(map_el_old)+1
            @test map_el.solutions[5,4,3] == solution
            @test map_el.performances[5,4,3] == performance
            # check if we add an object at the same place if it is still working
            map_el_old = deepcopy(map_el)
            new_solution = Individual([1,1,1],[10.0])
            new_perf = [10.0]
            add_to_map(map_el,coordinate,new_solution,new_perf)
            @test count_elt(map_el) == count_elt(map_el_old)
            @test map_el.solutions[5,4,3] == new_solution
            @test map_el.performances[5,4,3] == new_perf
            test_struct(map_el)
            # check if we add an individual with a lower performance (theorically no)
            worse_ind = Individual([1,0,0],[8.0])
            worse_perf = [8.0]
            add_to_map(map_el,coordinate,worse_ind,worse_perf)
            @test map_el.solutions[5,4,3] == new_solution
            @test map_el.performances[5,4,3] == new_perf
            # check if a better ind replace a worse one
            better_ind = Individual([1,0,0],[12.0])
            better_perf = [12.0]
            add_to_map(map_el,coordinate,better_ind,better_perf)
            @test map_el.solutions[5,4,3] == better_ind
            @test map_el.performances[5,4,3] == better_perf
        end

        @testset "select_random function" begin
            map_el = MapElites(3,4)
            ind1 = Individual([0,0,0],[1.0])
            ind2 = Individual([0,1,0],[2.0])
            add_to_map(map_el,[1,1,1],ind1,[1.0])
            add_to_map(map_el,[1,2,1],ind2,[2.0])
            ind = select_random(map_el)
            @test typeof(ind) <: Individual
            @test ((ind == ind1) || (ind == ind2))
            test_struct(map_el)
        end

        @testset "select_most_promising function" begin
            map_el = MapElites(3,4)
            ind1 = Individual([0,0,0],[10.0])
            perf1 = [10.0]
            coord1 = [1,2,3]
            ind2 = Individual([0,1,0],[18.0])
            perf2 = [18.0]
            coord2 = [2,2,1]
            ind3 = Individual([0,0,0],[15.0])
            perf3 = [15.0]
            coord3 = [3,4,1]
            add_to_map(map_el,coord1,ind1,perf1)
            add_to_map(map_el,coord2,ind2,perf2)
            add_to_map(map_el,coord3,ind3,perf3)
            @test count_elt(map_el) == 3
            most_promising = select_most_promising(map_el)
            @test most_promising == ind2
            @test most_promising.fitness == perf2
            @test typeof(most_promising) <: Individual
            ind4 = Individual([1,0,0],[20.0])
            perf4 = [20.0]
            coord4 = [3,2,2]
            add_to_map(map_el,coord4,ind4,perf4)
            most_promising = select_most_promising(map_el)
            @test most_promising == ind4
            @test most_promising.fitness == perf4
            @test typeof(most_promising) <: Individual
        end
    end

    @testset "populate_function.jl" begin
        @testset "mapelites_step!" begin
            map_el = MapElites(2,20)
            cfg = YAML.load_file("cfg/mapelites.yaml")
            e = Cambrian.Evolution(Cambrian.FloatIndividual,cfg)
            fit = i::Individual->[Random.rand()]
            Cambrian.fitness_evaluate!(e; fitness=fit)
            best = sort(e.population)[end]
            e_old = deepcopy(e)
            map_ind_to_b(ind::Individual) = [Random.rand(1:map_el.grid_mesh) for dim in 1:map_el.feature_dimension]
            mapelites_step!(e,map_el,map_ind_to_b)
            @test length(e.population) == cfg["n_population"]
            @test e_old.population != e.population
            @test count_elt(map_el) >= length(e_old.population)
            new_best = sort(e.population)[end]
            @test new_best.fitness >= best.fitness
        end

        @testset "mapelites_run!" begin
            cfg = YAML.load_file("cfg/mapelites.yaml")
            e = Cambrian.Evolution(Cambrian.FloatIndividual,cfg)
            features_dim = e.cfg["features_dim"]
            grid_mesh = e.cfg["grid_mesh"]
            mapel = MapElites(features_dim,grid_mesh)
            map_ind_to_b(ind::Individual) = [Random.rand(1:grid_mesh) for dim in 1:features_dim]
            mapelites_run!(e,mapel,map_ind_to_b)
            test_struct(mapel)
            @test count_elt(mapel) > 0
            @test length(e.population) == cfg["n_population"]
            @test e.gen == cfg["n_gen"]
            best = sort(e.population)[end]
            best_map = select_most_promising(mapel)
            @test best_map.fitness == best.fitness
        end
    end

    @testset "toyproblem" begin
        cfg = YAML.load_file("cfg/mapelites.yaml")
        cfg["n_genes"] = 2
        features_dim = 2
        grid_mesh = 100
        cfg["n_gen"] = 10
        cfg["n_population"] = 1000
        rast = BlackBoxOptimizationBenchmarking.F15
        evaluate1(ind::Individual) = rosenbrock(ind)
        e = Cambrian.Evolution(Cambrian.FloatIndividual,cfg)
        mapel = MapElites(features_dim,grid_mesh)
        mapelites_run!(e,mapel,map_ind_toyproblem;evaluate=evaluate1)
        test_struct(mapel)
        @test count_elt(mapel) > 0
        @test length(e.population) == cfg["n_population"]
        @test e.gen == cfg["n_gen"]
        best = sort(e.population)[end]
        best_map = select_most_promising(mapel)
        @test best_map.fitness == best.fitness
        # println(best_map)
        # println(best)
        # println(mapel.solutions)
    end
end

ext: jl


text:
"""
    onClose!(behavior::Symbol)

Set behavior on disconnect from device.
Options:
- `:run` - Allows state to continue after disconnect
- `:stop` - Default
- `:shutdown`
"""
function onClose!(behavior::Symbol)
    if behavior == :run
        val = Cint(0)
    elseif behavior == :stop
        val = Cint(1)
    elseif behavior == :shutdown
        val = Cint(2)
    else
        error("Behavior symbol `$behavior` not recognised")
    end
    FDwfParamSet(DwfParamOnClose, val)
    return nothing
end


text:
using QMDP
using POMDPs
using POMDPModels
using POMDPModelTools
using DiscreteValueIteration
using BeliefUpdaters
using POMDPTesting
using POMDPLinter: show_requirements, get_requirements
using Test
using Random

@testset "Standard QMDP" begin
    pomdp = TigerPOMDP()
    solver = QMDPSolver(verbose=true)

    @test_skip @requirements_info solver pomdp
    show_requirements(get_requirements(POMDPs.solve, (solver, pomdp)))

    policy = solve(solver, pomdp)

    solver = QMDPSolver(verbose=false)
    rng = MersenneTwister(11)

    bu = updater(policy)
    sd = initialstate(pomdp)
    b = initialize_belief(bu, sd)

    a = action(policy, b)
    v = value(policy, b)

    s = rand(rng, initialstate(pomdp))
    sp, o = @gen(:sp, :o)(pomdp, s, a, rng)
    bp = update(bu, b, a, o)
    @test isa(bp, DiscreteBelief)

    r = test_solver(solver, pomdp)
    @test isapprox(r, 17.711, atol=1e-2)
end

@testset "Sparse QMDP" begin
    pomdp = TigerPOMDP()
    solver = QMDPSolver(SparseValueIterationSolver())

    r = test_solver(solver, pomdp)
    @test isapprox(r, 17.711, atol=1e-2)
end


text:
# link lengths
const l0 = [0.1; 0; 0]
const l1 = [0.3; 0; 0]
const l2 = [0.3; 0; 0]
const l3 = 0.1
const l4 = 0.2
const l5 = 0.0205
const lc = [l3; 0; 0]
const lee = [l3 + l4; 0; l5] # sqrt((l3 + l4)^2 + l5^2)

# CoM locations
const l_cb = [0; 0.004; 0]
const l_c0 = [0.0125108364230515; 0.00117191218927888; 0]
const l_c1 = [0.149359714867044; 0; 0]
const l_c2 = [0.0469412900551914; 0; 0]
const l_c3 = [0.113177000131857; 0; -0.015332867880069]

# link masses
const mb = 7  # kg
const m0 = 0.24644240965487
const m1 = 0.0707939028219395
const m2 = 0.276735496985514
const m3 = 0.130824780046739
# const m = Diagonal([m0, m1, m2, m3])

# gravity, obviously
const g = 9.807

# mass moment of inertia in axis of rotation
const Ib = Array([0.0024241 5.252E-06 2.0733E-19;
                  5.252E-06 0.0044176 -3.1153E-19;
                  2.0733E-19 -3.1153E-19 0.0022481])

const I0 = Array([3.83120149546952E-05 1.46925714738609E-05 -8.60106401672571E-06;
                  1.46925714738609E-05 0.000172067745507247 1.0427260925207E-06;
                  -8.60106401672571E-06 1.0427260925207E-06 0.00014745218068435])

const I1 = Array([3.06999775886187E-06 7.91090301514898E-12 -1.43705963146176E-12;
                  7.91090301514898E-12 0.000147960574744097 1.30742394049546E-11;
                  -1.43705963146176E-12 1.30742394049546E-11 0.000147884231885009])

const I2 = Array([3.43038397803592E-05 -2.90339844227483E-07 6.18680397558952E-06;
                  -2.90339844227483E-07 0.000302324068012293 2.25016327583562E-08;
                  6.18680397558952E-06 2.25016327583562E-08 0.00028292376778719])

const I3 = Array([1.76996970020568E-05 -5.3695427116208E-07 7.62350214406387E-07;
                  -5.3695427116208E-07 0.000164188445564489 -2.77843753828047E-07;
                  7.62350214406387E-07 -2.77843753828047E-07 0.000160656046697151])

M̄ = [mb*I(3) zeros(3, 27);
    zeros(3,3) Ib zeros(3, 24);
    zeros(3,6) m0*I(3) zeros(3,21);
    zeros(3,9) I0 zeros(3,18);
    zeros(3,12) m1*I(3) zeros(3,15);
    zeros(3,15) I1 zeros(3, 12);
    zeros(3,18) m2*I(3) zeros(3, 9);
    zeros(3, 21) I2 zeros(3, 6);
    zeros(3, 24) m3*I(3) zeros(3, 3);
    zeros(3, 27) I3]

# Kinematics
function kin_ee(q)
    # fast forward kinematics of the end effector
    #rb = q[1:3]
    #Qb = q[4:7]
    r3 = q[29:31]
    #Q2 = q[25:28]
    Q3 = q[32:35]
    #pb = rb + rotate(Qb, l_cb)  # position vector from world frame to *JOINTS* 0 and 2
    #ree = pb + rotate(Q2, l2) + rotate(Q3, lee)
    return r3 + rotate(Q3, lee-l_c3)  # 3x1
end

function kinematics(q)
    # differentiable kinematics
    # actuator-to-end-effector forward kinematics
    Qb = q[4:7]
    # end-to-end link length magnitudes
    l_0 = 0.1
    l_1 = 0.3
    l_2 = 0.3
    l_3 = 0.1
    l_4 = 0.2
    l_5 = 0.0205

    a = a_act(q)
    q0 = a[1]
    q2 = a[2]

    d = 0
    x0a = l_0 * cos(q0)
    z0a = l_0 * sin(q0)
    rho = smoothsqrt((x0a + d)^2 + z0a^2)
    x1a = l_2 * cos(q2)
    z1a = l_2 * sin(q2)
    h = smoothsqrt((x0a - x1a)^2 + (z0a - z1a)^2)
    mu = acos((l_3^2 + h^2 - l_1^2) / (2 * l_3 * h + 1e-8))
    eta = acos((h^2 + l_2^2 - rho^2) / (2 * h * l_2 + 1e-8))
    alpha = pi - (eta + mu) + q2
    xa = l_2 * cos(q2) + (l_3 + l_4) * cos(alpha) - d + l_5 * cos(alpha - pi / 2)
    ya = 0
    za = l_2 * sin(q2) + (l_3 + l_4) * sin(alpha) + l_5 * cos(alpha - pi / 2)
    k_base = [xa; ya; za]

    return L(Qb)*R(Qb)'*H*k_base
end

# End effector Jacobian
function Jac(q)
    jac = ForwardDiff.jacobian(dq->kinematics(dq), q)
    return jac
end

max_stars_repo_name: bbokser/hopper-sim

max_issues_repo_name: bbokser/hopper-sim

max_forks_repo_name: bbokser/hopper-sim


text:
# This file was formerly a part of Julia. License is MIT: https://julialang.org/license

include("ZMQCM.jl")

start_worker(parse(Int,ARGS[1]), ARGS[2])

max_stars_repo_name: vtjnash/Examples

max_issues_repo_name: JuliaArchive/Examples

max_forks_repo_name: JuliaArchive/Examples


text:
export ConformalMap, differentiate

#
# A ConformalMap stores the data for h(t) = u₀sinh(t) + u₁ + u₂t + u₃ t^2 + ⋯ + uₙt^(n-1).
#
# u0 and u are the parameters of the map h(t) in Eq. (3.14),
# x are the x-coordinates of the pre-images x +/- i pi/2γ of the singularities, and
# O is the order of derivative.
#

type ConformalMap{T}
    u0::T
    u::Vector{T}
    x::Vector{T}
    O::Int
end

ConformalMap{T}(u0::T,u::Vector{T},x::Vector{T}) = ConformalMap(u0,u,x,0)
ConformalMap{T}(u0::T,u::Vector{T}) = ConformalMap(u0,u,T[])

function differentiate{T}(h::ConformalMap{T})
    n = length(h.u)-1
    if n > 0
        u = Array{T}(n)
        for j=1:n
            u[j] = j*h.u[j+1]
        end
        return ConformalMap(h.u0,u,h.x,h.O+1)
    else
        return ConformalMap(h.u0,T[],h.x,h.O+1)
    end
end
differentiate{T}(h::ConformalMap{T},n::Int) = n > 1 ? differentiate(differentiate(h),n-1) : n == 1 ? differentiate(h) : n == 0 ? h : throw("Cannot differentiate with order "*string(n)*".")

Base.transpose{T}(h::ConformalMap{T}) = differentiate(h)

function Base.inv{S,T<:Number}(h::ConformalMap{S},x::T)
    hp = h'
    t0 = convert(promote_type(S,T),Inf)
    t1 = asinh.((x-h.u[1])/h.u0)
    while norm(t1/t0-1,Inf) > 10eps(T)
        t0 = t1
        t1 = t0 - (h[t0]-x)/hp[t0]
    end
    return t1
end

function Base.inv{S,T<:Number,N}(h::ConformalMap{S},x::AbstractArray{T,N})
    hp = h'
    t0 = convert(Array{promote_type(S,T),N},fill(Inf,size(x)))
    t1 = asinh.((x-h.u[1])./h.u0)
    while norm(t1./t0-1,Inf) > 10eps(T)
        t0 = t1
        t1 = t0 - (h[t0]-x)./hp[t0]
    end
    return t1
end

function evaluate{T}(h::ConformalMap{T},t::Number)
    n = length(h.u)
    ret = n > 0 ? convert(promote_type(T,typeof(t)),h.u[n]) : zero(promote_type(T,typeof(t)))
    for j = n-1:-1:1
        ret = h.u[j] + t*ret
    end
    ret += iseven(h.O) ? h.u0*sinh(t) : h.u0*cosh(t)
end

Base.getindex{T}(h::ConformalMap{T},t::Number) = evaluate(h,t)
Base.getindex{S,T<:Number}(h::ConformalMap{S},t::Array{T}) = map(t->evaluate(h,t),t)


text:
"""
`hilbert(n)` creates an `n`-by-`n` Hilbert matrix.

## Example
```
julia> hilbert(4)
4×4 Array{Rational{Int64},2}:
 1//1  1//2  1//3  1//4
 1//2  1//3  1//4  1//5
 1//3  1//4  1//5  1//6
 1//4  1//5  1//6  1//7
```
"""
function hilbert(n::Int)
    A = zeros(Rational{Int}, n, n)
    for i = 1:n
        for j = 1:n
            A[i, j] = 1 // (i + j - 1)
        end
    end
    return A
end


text:
# module Bukdu.Plug

import ..Bukdu

immutable Logger
end

"""
plug `Plug.Logger` to write the event logs.

```julia
Endpoint() do
    plug(Plug.Logger, level= :info)
end
```
"""
function plug(::Type{Plug.Logger}; kw...)
    # level::Union{Symbol,Bool}
    opts = Dict(kw)
    level = haskey(opts, :level) ? opts[:level] : :debug
    Bukdu.Logger.set_level(level)
end


text:
using IntervalWavelets
using Test

@testset "Dyadic Rationals" begin
    @testset "Construct Dyadic Rationals" begin
        x = DyadicRational(1, 2)

        @test IntervalWavelets.numerator(x) == 1
        @test resolution(x) == 2

        @test DyadicRational(4, 1) == DyadicRational(2, 0)
        @test DyadicRational(-4, 1) == DyadicRational(-2, 0)
        @test DyadicRational(5, 1) == DyadicRational(5, 1)

        @test_throws DomainError DyadicRational(1, -2)
    end

    @testset "Arithmetic with Dyadic Rationals" begin
        @test DyadicRational(2, 0) + 1 == DyadicRational(3, 0)
        @test DyadicRational(2, 1) + 1 == DyadicRational(4, 1)

        @test DyadicRational(1, 0) + DyadicRational(1, 1) == DyadicRational(3, 1)
        @test DyadicRational(1, 0) - DyadicRational(1, 1) == DyadicRational(1, 1)
        @test DyadicRational(1, 1) - DyadicRational(-1, 1) == DyadicRational(1, 0)

        @test 2 * DyadicRational(2, 0) == DyadicRational(4, 0)
        @test 3 * DyadicRational(1, 2) == DyadicRational(3, 2)
        @test DyadicRational(-3, 1) * DyadicRational(-1, 2) == DyadicRational(3, 3)
    end

    @testset "All Dyadic Rationals" begin
        @test IntervalWavelets.all_dyadic_rationals(0, 1, 0) == DyadicRational.([0 ; 1], 0)
        @test IntervalWavelets.all_dyadic_rationals(0, 1, 1) == DyadicRational.(0:2, 1)

        left = DyadicRational(-1, 1)
        right = DyadicRational(1, 2)
        @test all_dyadic_rationals(left, right, 2) == DyadicRational.(-2:1, 2)
        @test length(all_dyadic_rationals(right, left, 2)) == 0

        @test_throws DomainError IntervalWavelets.all_dyadic_rationals(left, right, 1)
    end

    @testset "Convert Dyadic Rational" begin
        @test Integer(DyadicRational(3, 0)) == 3
        @test_throws InexactError Integer(DyadicRational(3, 2))

        @test float(DyadicRational(3, 0)) == 3.0
        @test float(DyadicRational(3, 2)) == 0.75
    end

    @testset "Compare Dyadic Rationals" begin
        @test DyadicRational(1, 2) < DyadicRational(2, 2)
        @test DyadicRational(1, 2) > DyadicRational(-1, 2)

        @test DyadicRational(1, 2) >= DyadicRational(1, 2)
        @test DyadicRational(1, 2) <= DyadicRational(1, 2)
    end
end


text:
function chobac!(kb::Matrix{Float64}, loads::Vector{Float64})
  #local l, m
  n = size(kb, 1)
  iw = size(kb, 2) - 1
  loads[1] = loads[1] / kb[1, iw+1]
  for i in 2:n
    x = 0.0
    k = 1
    if i <= (iw + 1)
      k = iw - i + 2
    end
    for j in k:iw
      x += kb[i, j]*loads[i+j-iw-1]
    end
    loads[i] = (loads[i] - x) / kb[i, iw+1]
  end
  loads[n] = loads[n] / kb[n, iw+1]
  for i in n-1:-1:1
    x = 0.0
    l = i + iw
    m = l - 1
    if i > (n - iw)
      l = n
      m = i + 1
    end
    for j in m:l
      x += kb[j, iw+i-j+1] * loads[j]
    end
    loads[i] = (loads[i] - x) / kb[i, iw+1]
  end
end


text:
"""
    optblocklength(data, bi::BootInput)::Float64
    optblocklength(data ; kwargs...)::Float64

Provides an estimate of the optimal block-length to use with a dependent bootstrap.

For multivariate datasets, optimal block length is estimated for each column of data, and then
bi.fblocklengthcombine, which is a function that maps Vector{Float64} to Float64, is called
to reduce the multiple estimates to a single estimates. The default value for fblocklengthcombine
is median.

Block length methods currently implemented include: \n
     - Patton, Politis, White (2009) "Correction to Automatic Block Length Selection For the Dependent Bootstrap" \n

For all methods discussed above, bandwidth is estimated following Politis (2003) "Adaptive Bandwidth Choice", using the
flat-top kernel suggested in that paper.
"""
function optblocklength(x::AbstractVector{<:Number}, blm::BLPPW2009{P2003}, bootmethod::Tbm)::Float64 where {Tbm<:BootMethod}
	length(x) < 3 && error("You must have at least 3 observations to estimate block length")
    (M, xVar, covVec) = blocklength_ma_and_cor(x) #Bandwidth method currently forced to politis (2003)
	kernelCovVec = blocklength_kernel_cov(covVec, M)
	gHat = 0.0
	for k = 1:M
		gHat += 2 * k * kernelCovVec[k] #note, "2*" is because sum is from -M and M, but is symmetric about 0. Note, 0 term in sum vanishes since k=0 -> |k|=0
	end
    dHat = optblocklength_blppw2009_dhat(bootmethod, xVar, kernelCovVec)
    #Equation 9 and 14 from Politis, White (2004)
	blocklength = (2 * gHat^2 * length(x) / dHat)^(1/3)
	blocklength = min(blocklength, ceil(min(3*sqrt(length(x)), length(x) / 3))) #Enforce upper bound on block length suggested by Patton
	blocklength = max(blocklength, 1.0)
	return blocklength
end
(optblocklength_blppw2009_dhat(a::BootStationary, xvar::Float64, kernelcovvec::Vector{Float64})::Float64) = 2 * (xvar + 2*sum(kernelcovvec))^2 #note, in expression (1 + 2*sum(kernelCovVec)), "2*" is because sum is from -M to M, but is symmetric around 0. "1+" is the zero term of the sum which is simply equal to unity.
(optblocklength_blppw2009_dhat(a::T, xvar::Float64, kernelcovvec::Vector{Float64})::Float64) where {T<:Union{BootCircular,BootMoving,BootIID,BootNoOverlap}} = (4/3) * (xvar + 2*sum(kernelcovvec))^2 #note, in expression (1 + 2*sum(kernelCovVec)), "2*" is because sum is from -M to M, but is symmetric around 0. "1+" is the zero term of the sum which is simply equal to unity.

#Block length selection method of Paparoditis, Politis (2002) "The Tapered Block Bootstrap for General Statistics From Stationary Sequences"
# function optblocklength(x::AbstractVector{<:Number}, blm::BLPP2002{P2003,Tkf}, bootmethod::Tbm)::Float64 where {Tbm<:BootMethod,Tkf<:KernelFunctionMethod}
# 	length(x) < 3 && error("You must have at least 3 observations to estimate block length")
#     Tbm != BootTapered && println("WARNING: Optimal parameter values in pp2002 block length procedure are unknown for bootstrap method $(bootmethod).")
# 	(M, xVar, covVec) = blocklength_ma_and_cor(x)
# 	kernelCovVec = blocklength_kernel_cov(covVec, M)
# 	deltaUnknown = (xVar + 2*sum(kernelCovVec))^2 #Unknown parameter in Delta expression, section 4, Paparoditis, Politis (2002). (note, "1+" is for k=0 term in summation, "2*" is because summation is from -M to M, but is symmetric about 0)
# 	gammaUnknown = 0.0 #Unknown parameter in Gamma expression, start of section 4
# 	for k = 1:M
# 		gammaUnknown += 2 * k^2 * kernelCovVec[k] #note, "2*" is because sum is from -M and M, but is symmetric about 0. Note, 0 term in sum vanishes since k=0 -> k^2=0
# 	end
#     (gammaHat, deltaHat) = optblocklength_blpp2002_param(blm.kernelfunction, gammaUnknown, deltaUnknown)
#     #Equation 20 from Paraproditis, Politis (2002)
# 	blocklength = (4 * gammaHat^2 * length(x) / deltaHat)^(1/5)
# 	blocklength = min(blocklength, ceil(min(3*sqrt(length(x)), length(x) / 3))) #Enforce upper bound on block length suggested by Patton
# 	blocklength = max(blocklength, 1.0)
# 	return blocklength
# end
# (optblocklength_blpp2002_param(kf::KernelTrap, gu::Float64, du::Float64)::Float64) = (-5.45*gu, 1.099*du) #-5.45 = (1/2) * -10.9 = (1/2) * (w*w)''(0) / (w*w)(0) [OPTIMAL TRAP VALUES], see Paparoditis, Politis (2002), 1.099 = 2 * 0.5495 = 2 * int_{-1}^{1} ((w*w)^2(x) / (w*w)^2(0)) dx [OPTIMAL TRAP VALUES], see Paparoditis, Politis (2002)
# (optblocklength_blpp2002_param(kf::KernelSmooth, gu::Float64, du::Float64)::Float64) = (-5.175*gu, 1.1312*deltaUnknown) #-5.175 = (1/2) * -10.35 = (1/2) * (w*w)''(0) / (w*w)(0) [OPTIMAL SMOOTH VALUES], see Paparoditis, Politis (2002), 1.1312 = 2 * 0.5656 = 2 * int_{-1}^{1} ((w*w)^2(x) / (w*w)^2(0)) dx [OPTIMAL SMOOTH VALUES], see Paparoditis, Politis (2002)
# (optblocklength_blpp2002_param(kf::T, gu::Float64, du::Float64)::Float64) where {T} = error("Optimal parameters not known for input kernel function: $(kf)")
(optblocklength(x::AbstractVector{<:Number}, bi::BootInput{Tbm,BLDummy})::Float64) where {Tbm<:BootMethod} = error("Logic fail. It should not have been possible to call this method. Please file an issue with full stacktrace.")
(optblocklength(x::AbstractVector{<:Number}, bi::BootInput)::Float64) = optblocklength(x, bi.blocklengthmethod, bi.bootmethod)
#Multivariate dataset wrapper
(optblocklength(data, bl::Tbl, bm::Tbm, f::Tf)::Float64) where {Tbl<:BlockLengthMethod,Tbm<:BootMethod,Tf<:Function} = f([ optblocklength(local_get_var(data, i), bl, bm) for i = 1:num_var(data) ])
(optblocklength(data, bi::BootInput)::Float64) = bi.fblocklengthcombine([ optblocklength(local_get_var(data, i), bi) for i = 1:num_var(data) ])
#Keyword input wrapper
(optblocklength(data ; kwargs...)::Float64) = optblocklength(data, BootInput(data, kwargs...))

#These two functions are used by several of the block-length selection procedures
function blocklength_ma_and_cor(x::AbstractVector{T})::Tuple{Int, Float64, Vector{Float64}} where {T<:Number}
	(M, xVar, covVec) = bandwidth_politis_2003(x)
	if M > 0.5 * length(x)
		println("WARNING: Bandwidth in parameter estimation section of blocklength forced to half total number of observations. Data may contain excessive dependence.")
		M = Int(round(0.5 * length(x)))
	end
	M < 2 && (M = 2) #Even though M output of bandwidth function is always greater than 2, the above check for excessively large M can result in M being reduced below 2 (admittedly only in very unusual circumstances)
	length(covVec) < M && append!(covVec, autocov(x, length(covVec)+1:M)) #Get any additional autocovariances that we might need
	return(M, xVar, covVec)
end
function blocklength_kernel_cov(covVec::AbstractVector{Float64}, M::Int)::Vector{Float64}
	length(covVec) < M && error("Error in blocklength_kernel_cov likely caused by logic fail in blocklength_ma_and_cor. Please lodge a github issue, preferably with reproducible example.")
	kernelCovVec = Float64[ kernel_politis_2003_flat_top(k / M) * covVec[k] for k = 1:M ]
	return(kernelCovVec)
end

#Note, the following non-exported function is called in the ForecastEval package, so if you alter the function name you will
#need to adjust that package too.
"""
    bandwidth_politis_2003(x::AbstractVector{T})::Tuple{Int, Float64, Vector{Float64}} where {T<:Number}

Implements the methodology from Politis (2003) "Adaptive Bandwidth Choice" to obtain a data-driven bandwidth estimate.

Return tuple is, in order, the bandwidth estimate, the variance of x, and the autocorrelations used to get the bandwidth estimate.

Note, most users won't be interested in the second and third output, but sometimes this routine will be called by other
functions that need these terms, so they are returned to avoid duplicate computation.
"""
function bandwidth_politis_2003(x::AbstractVector{T})::Tuple{Int, Float64, Vector{Float64}} where {T<:Number}
    length(x) < 2 && error("Input data must have at least two observations")
    adjustmentTerm = 1.0 #This term is used by me for debugging. It serves no statistical purpose.
    politis_c = 2.0 #This value is again recommended in Politis (2003)
    K = max(5, Int(ceil(sqrt(log(10, length(x)))))) #This value is recommended in Politis (2003)
	mHat = 1
	corVec = Float64[]
	append!(corVec, autocor(x, 1:min(20, length(x)-1))) #I add autocorrelations in blocks as this is more efficient than doing it one at a time
	corBound = politis_c * sqrt(log(10, length(x)) / length(x)) #Note, use of log base 10 is deliberate and recommended in Politis (2003)
	KCounter = 0
	mHatFound = false
    for k = 1:length(x)-2
        #Add one to counter if bound is satisfied, otherwise reset counter to 0
        abs(corVec[k]) < corBound ? (KCounter += 1) : (KCounter = 0)
		if KCounter >= K #We found K autocorrelations in a row that satisfy the bound, so break.
			mHat = k - K + 1
			mHatFound = true
			break
		end
        #If we run out of autocorrelations to check, add another block of them to corVec
        k == length(corVec) && append!(corVec, autocor(x, length(corVec)+1:min(length(corVec)+20, length(x)-1)))
	end
    mHatFound == false && (mHat = length(x) - 1) #Bound mHat in the case where we fail to hit the break condition
	M = Int(ceil(adjustmentTerm * 2 * mHat)) #"2*" is a standard rule, see e.g. Politis (2003).
    M > length(x) - 1 && (M = length(x) - 1) #Apply upper bound to M
    M < 2 && (M = 2) #Apply lower bound to M
	xVar = ((length(x)-1) /  length(x)) * var(x) #Used to scale autocorrelations to autocovariances for return argument
	return (M, xVar, xVar * corVec)
end

"""
    kernel_politis_2003_flat_top(x::Float64)::Float64

Implements the flat-top kernel function discussed in Politis (2003) "Adaptive Bandwidth Choice"
"""
function kernel_politis_2003_flat_top(x::Float64)::Float64
    x_abs = abs(x)
    x_abs <= 0.5 && return 1.0
    x_abs <= 1.0 && return (2.0 * (1 - x_abs))
    return 0.0
end


text:
module SeaweedFSClient

using HTTP, JSON, Pipe

export create, save, load, filedata, delete

const Host = NamedTuple{(:hostname, :port), Tuple{String, Int64}}

function decode_response(h, r)
    if 200 <= r.status < 300
        json = String(r.body)
        if length(json) > 0
            return (host=h, status=r.status, Dict(Symbol(k)=>v for (k,v) in JSON.parse(json))...)
        end
    end
    return (host=h, status=r.status,)
end

volume(f::NamedTuple) = volume(f.fid)
volume(fid::AbstractString) = split(fid, ",")[1]

server(f::NamedTuple) = server(f.host, volume(f))
server(h::Host, v::AbstractString) = @pipe HTTP.request("GET", "http://$(h.hostname):$(h.port)/dir/lookup?volumeId=$(v)") |> decode_response(h,_)

"""
    create(h::Host)
# Arguments
- `h` an instance of NamedTuple{(:hostname, :port), Tuple{String, Int64}}
# Example
```
julia> f = create((hostname="localhost", port=9333))
(host = (hostname = "localhost", port = 9333), status = 200, url = "192.168.1.9:8080", publicUrl = "192.168.1.9:8080", count = 1, fid = "3,09b8e10215"
```
"""
create(h::Host) = @pipe HTTP.request("GET", "http://$(h.hostname):$(h.port)/dir/assign") |> decode_response(h,_)

"""
    save(f::NamedTuple, file_data)
    save(h::Host, url, fid, file_data)
# Arguments
- `f` is the response returned from `create` (or any tuple with (host=(hostname=String, port=Int), fid="V,FID", url="http..."))
Send the `file_data` to the fid. `file_data` is created using `filedata()`
# Example

```
# using a created f
julia> save(f, filedata("readme.txt", "This is some text"))
(host = (hostname = "localhost", port = 9333), status = 201, eTag = "0d79b2b0", size = 17)

```
"""
save(f::NamedTuple, form::HTTP.Form) = save(f.host, f.url, f.fid, form)
save(h::Host, url, fid, form::HTTP.Form) = @pipe HTTP.post("http://$url/$fid", [], form) |> decode_response(h,_)

"""
    load(f::NamedTuple, options=Dict{String,String}())::Union{Nothing, Vector{UInt8}}
    load(f::NamedTuple, options=Dict{String,String}(); range=0:0)::Union{Nothing, Vector{UInt8}}
load a file from the server
# Arguments
- `f` is the response returned from `create` (or any tuple with (host=(hostname=String, port=Int), fid="V,FID", url="http..."))
- `range` optional byte range instead of the whole file
For some files the server will do actions. The documented one is width=W, height=H and `mode in ["fit", "fill"]` - perhaps more will follow
# Example
```
# using a created f
julia> load(f)
17-element Vector{UInt8}:
 0x54
 0x68
    ⋮
 0x20
 0x74
```
"""
function load(f::NamedTuple, options=Dict{String,String}(); range=0:0)
    srv = server(f)
    if srv.status == 200
        headers = Dict{String, String}()
        url = srv.locations[1]["url"]
        qs = ""
        if haskey(options, "width") && haskey(options, "height")
            qs = """?width=$(options["width"])&height=$(options["height"])&mode=$(get(options, "mode", ""))"""
        end
        if range != 0:0
            headers["Range"] = "bytes=$(range[1])-$(range[end])"
        end
        r = HTTP.request("GET", "http://$(url)/$(f.fid)$qs", headers)
        if 200 <= r.status < 300
            return r.body
        end
    end
end

"""
    delete(f::NamedTuple)
    delete(url, fid)
# Arguments
- `f` is the response returned from `create` (or any tuple with (host=(hostname=String, port=Int), fid="V,FID", url="http..."))
Delete a file from the server
# Example
```
# using a created f
julia> delete(f)
(host = nothing, status = 202, size = 29)
```
"""
delete(f::NamedTuple) = delete(f.url, f.fid)
delete(url, fid) = @pipe HTTP.request("DELETE", "http://$url/$fid") |> decode_response(nothing, _)

"""
    filedata(text)
    filedata(io::IO)
Create a file data object to send to the server.
# Example
```
# using a created f
julia> open("/home/matt/some_readme.txt") do io save(f, filedata(io, "readme.txt")) end
(host = (hostname = "localhost", port = 9333), status = 201, eTag = "04fd3fe2", name = "readme.txt", size = 29)
```
"""
filedata(text) = HTTP.Form(Dict("text"=>text))
filedata(io::IO, filename="filename") = HTTP.Form(Dict("key" => HTTP.Multipart(filename, io, HTTP.sniff(io)),))

###
end


text:
global const ATPNittoFinalsPoints = Dict(
    "Winner" => 1500,
    "Finals" => 0,
    "Semi-Finals" => 0,
    "Quarter-Finals" => 0,
    "Round-16" => 0,
    "Round-32" => 0,
    "Round-64" => 0,
    "Round-128" => 0,
    "Qualified" => 0
)

global const ATPGrandSlamDoublesPoints = Dict(
    "Winner" => 2000,
    "Finals" => 1200,
    "Semi-Finals" => 720,
    "Quarter-Finals" => 360,
    "Round-16" => 180,
    "Round-32" => 90,
    "Round-64" => 45,
    "Round-128" => 10,
    "Qualified" => 25
)

global const ATP1000DoublesPoints = Dict(
    "Winner" => 1000,
    "Finals" => 600,
    "Semi-Finals" => 360,
    "Quarter-Finals" => 180,
    "Round-16" => 90,
    "Round-32" => 0,
    "Round-64" => 0,
    "Round-128" => 0,
    "Qualified" => 0,
)

global const ATP500DoublesPoints = Dict(
    "Winner" => 500,
    "Finals" => 300,
    "Semi-Finals" => 180,
    "Quarter-Finals" => 90,
    "Round-16" => 45,
    "Round-32" => 0,
    "Round-64" => 0,
    "Round-128" => 0,
    "Qualified" => 0
)

global const ATP250DoublesPoints = Dict(
    "Winner" => 250,
    "Finals" => 150,
    "Semi-Finals" => 90,
    "Quarter-Finals" => 45,
    "Round-16" => 0,
    "Round-32" => 0,
    "Round-64" => 0,
    "Round-128" => 0,
    "Qualified" => 0
)

global const ATPChallengerTour125DoublesPoints = Dict(
    "Winner" => 125,
    "Finals" => 75,
    "Semi-Finals" => 45,
    "Quarter-Finals" => 25,
    "Round-16" => 0,
    "Round-32" => 0,
    "Round-64" => 0,
    "Round-128" => 0,
    "Qualified" => 0
)

global const ATPChallengerTour110DoublesPoints = Dict(
    "Winner" => 110,
    "Finals" => 65,
    "Semi-Finals" => 40,
    "Quarter-Finals" => 20,
    "Round-16" => 0,
    "Round-32" => 0,
    "Round-64" => 0,
    "Round-128" => 0,
    "Qualified" => 0
)

global const ATPChallengerTour100DoublesPoints = Dict(
    "Winner" => 110,
    "Finals" => 60,
    "Semi-Finals" => 35,
    "Quarter-Finals" => 18,
    "Round-16" => 0,
    "Round-32" => 0,
    "Round-64" => 0,
    "Round-128" => 0,
    "Qualified" => 0
)

global const ATPChallengerTour90DoublesPoints = Dict(
    "Winner" => 90,
    "Finals" => 55,
    "Semi-Finals" => 33,
    "Quarter-Finals" => 17,
    "Round-16" => 0,
    "Round-32" => 0,
    "Round-64" => 0,
    "Round-128" => 0,
    "Qualified" => 0
)

global const ATPChallengerTour80DoublesPoints = Dict(
    "Winner" => 80,
    "Finals" => 48,
    "Semi-Finals" => 29,
    "Quarter-Finals" => 15,
    "Round-16" => 0,
    "Round-32" => 0,
    "Round-64" => 0,
    "Round-128" => 0,
    "Qualified" => 0
)

global const ITFWorldTennisTour25kPoints = Dict(
    "Winner" => 20,
    "Finals" => 12,
    "Semi-Finals" => 6,
    "Quarter-Finals" => 3,
    "Round-16" => 1,
    "Round-32" => 0,
    "Round-64" => 0,
    "Round-128" => 0,
    "Qualified" => 0
)

global const ITFWorldTennisTour15kPoints = Dict(
    "Winner" => 10,
    "Finals" => 6,
    "Semi-Finals" => 4,
    "Quarter-Finals" => 2,
    "Round-16" => 1,
    "Round-32" => 0,
    "Round-64" => 0,
    "Round-128" => 0,
    "Qualified" => 0
)

max_stars_repo_name: Danial-Hussain/Tennis-Ranking

max_issues_repo_name: Danial-Hussain/Tennis-Ranking

max_forks_repo_name: Danial-Hussain/Tennis-Ranking


text:
using StaticArrays

"""
    clusters::Vector{Cluster}
    data::OrderedDict{FockConfig{N}, OrderedDict{ClusterConfig{N}, Vector{T}}}

This represents an arbitrarily sparse state. E.g., used in TPSCI
"""
struct ClusteredState{T,N,R} <: AbstractState
    clusters::Vector{Cluster}
    data::OrderedDict{FockConfig{N}, OrderedDict{ClusterConfig{N}, MVector{R,T}}}
end
Base.haskey(ts::ClusteredState, i) = return haskey(ts.data,i)
#Base.iterate(ts::ClusteredState, state=1) = iterate(ts.data, state)
#Base.eltype(::Type{ClusteredState{T,N,R}}) where {T,N,R} = OrderedDict{ClusterConfig{N}, MVector{R,T}}

"""
    ClusteredState(clusters; T=Float64, R=1)

Constructor creating an empty vector
# Arguments
- `clusters::Vector{Cluster}`
- `T`:  Type of data for coefficients
- `R`:  Number of roots
# Returns
- `ClusteredState`
"""
function ClusteredState(clusters; T=Float64, R=1)
    N = length(clusters)
    return ClusteredState{T,N,R}(clusters,OrderedDict{FockConfig{N}, OrderedDict{ClusterConfig{N}, MVector{R,T}}}())
end

"""
    function ClusteredState(v::ClusteredState{T,N,R}; T=T, R=R) where {T,N,R}

Constructor creating a `ClusteredState` with the same basis as `v`, but with potentially different `R` and `T`.
Coefficients of new vector are 0.0

# Arguments
- `T`:  Type of data for coefficients
- `R`:  Number of roots
# Returns
- `ClusteredState`
"""
function ClusteredState(v::ClusteredState{TT,NN,RR}; T=TT, R=RR) where {TT,NN,RR}
    out = ClusteredState(v.clusters,T=T,R=R)
    for (fock, configs) in v.data
        add_fockconfig!(out,fock)
        for (config, coeffs) in configs
            out[fock][config] = zeros(T,R)
        end
    end
    return out
end

"""
    ClusteredState(clusters::Vector{Cluster}, fconfig::FockConfig{N}; T=Float64, R=1) where {N}

Constructor using only a single FockConfig. This allows us to turn the CMF state into a ClusteredState.
# Arguments
- `clusters`: vector of clusters types
- `fconfig`: starting FockConfig
- `T`:  Type of data for coefficients
- `R`:  Number of roots
# Returns
- `ClusteredState`
"""
function ClusteredState(clusters::Vector{Cluster}, fconfig::FockConfig{N}; T=Float64, R=1) where {N}
    #={{{=#

    state = ClusteredState(clusters, T=T, R=R)
    add_fockconfig!(state, fconfig)
    conf = ClusterConfig([1 for i in 1:length(clusters)])
    state[fconfig][conf] = zeros(T,R)
    return state
#=}}}=#
end

"""
    add_fockconfig!(s::ClusteredState, fock::FockConfig)
"""
function add_fockconfig!(s::ClusteredState{T,N,R}, fock::FockConfig{N}) where {T<:Number,N,R}
    s.data[fock] = OrderedDict{ClusterConfig{N}, MVector{R,T}}()
    #s.data[fock] = OrderedDict{ClusterConfig{N}, MVector{R,T}}(ClusterConfig([1 for i in 1:N]) => zeros(MVector{R,T}))
end

"""
    getindex(s::ClusteredState, fock::Vector{Tuple{T,T}}) where T<:Integer
"""
#Base.getindex(s::ClusteredState, fock::Vector{Tuple{T,T}}) where T<:Integer = s.data[fock]
@inline Base.getindex(s::ClusteredState, fock) = s.data[fock]
@inline Base.setindex!(s::ClusteredState, a, b) = s.data[b] = a

function Base.size(s::ClusteredState{T,N,R}) where {T,N,R}
    return length(s),R
end
function Base.length(s::ClusteredState)
    l = 0
    for (fock,configs) in s.data
        l += length(keys(configs))
    end
    return l
end
"""
    get_vector(s::ClusteredState; root=1)
"""
function get_vector(s::ClusteredState; root=1)
    v = zeros(length(s))
    idx = 1
    for (fock, configs) in s.data
        for (config, coeff) in configs
            v[idx] = coeff[root]
            idx += 1
        end
    end
    return v
end
"""
    get_vectors(s::ClusteredState)
"""
function get_vectors(s::ClusteredState{T,N,R}) where {T,N,R}
    v = zeros(T,length(s), R)
    idx = 1
    for (fock, configs) in s.data
        for (config, coeff) in configs
            v[idx,:] .= coeff[:]
            idx += 1
        end
    end
    return v
end

"""
    get_vectors!(v, s::ClusteredState)
"""
function get_vectors!(v, s::ClusteredState{T,N,R}) where {T,N,R}
    idx = 1
    for (fock, configs) in s.data
        for (config, coeff) in configs
            v[idx,:] .= coeff[:]
            idx += 1
        end
    end
    return
end

"""
    function set_vector!(ts::ClusteredState{T,N,R}, v::Matrix{T}) where {T,N,R}

Fill the coefficients of `ts` with the values in `v`
"""
function set_vector!(ts::ClusteredState{T,N,R}, v::Matrix{T}) where {T,N,R}

    nbasis = size(v,1)
    nroots = size(v,2)

    length(ts) == nbasis || throw(DimensionMismatch)
    R == nroots || throw(DimensionMismatch)

    idx = 1
    for (fock, tconfigs) in ts.data
        for (tconfig, coeffs) in tconfigs
            #ts[fock][tconfig] = MVector{R}(v[idx,:])
            @views coeffs .= v[idx,:]
            idx += 1
        end
    end
    nbasis == idx-1 || error("huh?", nbasis, " ", idx)
    return
end

#"""
#    function set_vector!(ts::ClusteredState{T,N,R}, v) where {T,N,R}
#
#Fill the coefficients of `ts` with the values in `v`
#"""
#function set_vector!(ts::ClusteredState{T,N,R}, v) where {T,N,R}
#
#    nbasis = size(v,1)
#    nroots = size(v,2)
#
#    length(ts) == nbasis || throw(DimensionMismatch)
#    R == nroots || throw(DimensionMismatch)
#
#    idx = 1
#    for (fock, tconfigs) in ts.data
#        for (tconfig, coeffs) in tconfigs
#            #ts[fock][tconfig] = MVector{R}(v[idx,:])
#            @views coeffs .= v[idx,:]
#            idx += 1
#        end
#    end
#    nbasis == idx-1 || error("huh?", nbasis, " ", idx)
#    return
#end

"""
    Base.display(s::ClusteredState; thresh=1e-3, root=1)

Pretty print
"""
function Base.display(s::ClusteredState; thresh=1e-3, root=1)
    @printf(" --------------------------------------------------\n")
    @printf(" ---------- Fockspaces in state ------: Dim = %5i  \n",length(s))
    @printf(" ----------                root ------:     = %5i  \n",root)
    @printf(" --------------------------------------------------\n")
    @printf(" Printing contributions greater than: %f", thresh)
    @printf("\n")
    @printf(" %-20s%-20s%-20s\n", "Weight", "# Configs", "Fock space(α,β)...")
    @printf(" %-20s%-20s%-20s\n", "-------", "---------", "----------")
    for (fock,configs) in s.data
        prob = 0
        for (config, coeff) in configs
            prob += coeff[root]*coeff[root]
        end
        if prob > thresh
            @printf(" %-20.3f%-20i", prob,length(s.data[fock]))
            for sector in fock
                @printf("(%2i,%-2i)", sector[1],sector[2])
            end
            println()
        end
    end
    print(" --------------------------------------------------\n")
end

"""
    print_configs(s::ClusterState; thresh=1e-3)

Pretty print
"""
function print_configs(s::ClusteredState; thresh=1e-3, root=1)
    #display(keys(s.data))
    idx = 1
    for (fock,configs) in s.data
        length(s.clusters) == length(fock) || throw(Exception)
        length(s.data[fock]) > 0 || continue
        @printf(" Dim %4i fock_space: ",length(s.data[fock]))
        [@printf(" %-2i(%i:%i) ",fii,fi[1],fi[2]) for (fii,fi) in enumerate(fock)]
        println()
        for (config, value) in s.data[fock]
            @printf(" %5i",idx)
            for c in config
                @printf("%3i",c)
            end
            @printf(":%12.8f\n",value[1])
            idx += 1
        end
    end
end

"""
    norm(s::ClusteredState, root)
"""
function LinearAlgebra.norm(s::ClusteredState, root)
    norm = 0
    for (fock,configs) in s.data
        for (config,coeff) in configs
            norm += coeff[root]*coeff[root]
        end
    end
    return sqrt(norm)
end

"""
    norm(s::ClusteredState{T,N,R}) where {T,N,R}
"""
function LinearAlgebra.norm(s::ClusteredState{T,N,R}) where {T,N,R}
    norms = zeros(T,R)
    for (fock,configs) in s.data
        for (config,coeff) in configs
            for r in 1:R
                norms[r] += coeff[r]*coeff[r]
            end
        end
    end
    for r in 1:R
        norms[r] = sqrt(norms[r])
    end
    return norms
end

"""
    normalize!(s::AbstractState)
"""
function normalize!(s::AbstractState)
    scale!(s,1/sqrt(dot(s,s)))
end

"""
    scale!(s::ClusteredState,c)
"""
function scale!(s::ClusteredState{T,N,R},c;root=nothing) where {T,N,R}
    if root == nothing
        for (fock,configs) in s.data
            for (config,coeff) in configs
                s[fock][config] .= coeff.*c
            end
        end
    else
        root <= R || error("root>R")
        for (fock,configs) in s.data
            for (config,coeff) in configs
                s[fock][config][root] = coeff[root]*c
            end
        end
    end
end

"""
    dot(v1::ClusteredState,v2::ClusteredState; r1=1, r2=1)
"""
function dot(v1::ClusteredState{T,N,1},v2::ClusteredState{T,N,1}) where {T,N}
    d = T(0)
    for (fock,configs) in v1.data
        haskey(v2.data, fock) || continue
        for (config,coeff) in configs
            haskey(v2.data[fock], config) || continue
            d += coeff[1] * v2.data[fock][config][1]
        end
    end
    return d
end

"""
    dot(v1::ClusteredState,v2::ClusteredState; r1=1, r2=1)
"""
function dot(v1::ClusteredState{T,N,R}, v2::ClusteredState{T,N,R}, r1, r2) where {T,N,R}
    d = T(0)
    for (fock,configs) in v1.data
        haskey(v2.data, fock) || continue
        for (config,coeff) in configs
            haskey(v2.data[fock], config) || continue
            d += coeff[r1] * v2.data[fock][config][r2]
        end
    end
    return d
end

"""
    dot(v1::ClusteredState,v2::ClusteredState; r1=1, r2=1)
"""
function orth!(v1::ClusteredState{T,N,R}) where {T,N,R}
    d = T(0)
    F = svd(get_vectors(v1))

    set_vector!(v1, F.U*F.Vt)
    return
end

"""
    prune_empty_fock_spaces!(s::ClusteredState)

remove fock_spaces that don't have any configurations
"""
function prune_empty_fock_spaces!(s::ClusteredState)
    keylist = [keys(s.data)...]
    for fock in keylist
        if length(s[fock]) == 0
            delete!(s.data, fock)
        end
    end
#    # I'm not sure why this is necessary
#    idx = 0
#    for (fock,configs) in s.data
#        for (config, coeffs) in s.data[fock]
#            idx += 1
#        end
#    end
    return
end

"""
    zero!(s::ClusteredState)

set all elements to zero
"""
function zero!(s::ClusteredState{T,N,R}) where {T,N,R}
    for (fock,configs) in s.data
        for (config,coeffs) in configs
            s.data[fock][config] = zeros(size(MVector{R,T}))
            #s.data[fock][config] = zeros(MVector{R,T})
        end
    end
end

"""
    function rand!(s::ClusteredState{T,N,R}) where {T,N,R}

set all elements to random values, and orthogonalize
"""
function rand!(s::ClusteredState{T,N,R}) where {T,N,R}
    #={{{=#
    v0 = rand(T,size(s)) .- .5
    set_vector!(s,v0)
    orthonormalize!(s)
end
#=}}}=#

"""
    function orthonormalize!(s::ClusteredState{T,N,R}) where {T,N,R}

orthonormalize
"""
function orthonormalize!(s::ClusteredState{T,N,R}) where {T,N,R}
    #={{{=#
    v0 = get_vectors(s)
    v0[:,1] .= v0[:,1]./norm(v0[:,1])
    for r in 2:R
        #|vr> = |vr> - |v1><v1|vr> - |v2><v2|vr> - ...
        for r0 in 1:r-1
            v0[:,r] .-= v0[:,r0] .* (v0[:,r0]'*v0[:,r])
        end
        v0[:,r] .= v0[:,r]./norm(v0[:,r])
    end
    isapprox(det(v0'*v0), 1.0, atol=1e-14) || @warn "initial guess det(v0'v0) = ", det(v0'v0)
    set_vector!(s,v0)
end
#=}}}=#

"""
    clip!(s::ClusteredState; thresh=1e-5)
"""
function clip!(s::ClusteredState; thresh=1e-5)
#={{{=#
    for (fock,configs) in s.data
        for (config,coeff) in configs
            if all(abs(c) < thresh for c in coeff)
                delete!(s.data[fock], config)
            end
        end
    end
    prune_empty_fock_spaces!(s)
end
#=}}}=#

"""
    add!(s1::ClusteredState, s2::ClusteredState)

Add coeffs in `s2` to `s1`
"""
function add!(s1::ClusteredState, s2::ClusteredState)
    #={{{=#
    for (fock,configs) in s2.data
        if haskey(s1, fock)
            for (config,coeffs) in configs
                if haskey(s1[fock], config)
                    s1[fock][config] .+= s2[fock][config]
                else
                    s1[fock][config] = deepcopy(s2[fock][config])
                end
            end
        else
            s1[fock] = deepcopy(s2[fock])
        end
    end
    #=}}}=#
end

"""
    function extract_roots(v::ClusteredState{T,N,R}, roots)

Extract roots to give new `ClusteredState`
"""
function extract_roots(v::ClusteredState{T,N,R}, roots) where {T,N,R}
    vecs = get_vectors(v)[:,roots]

    out = ClusteredState(v.clusters, T=T, R=length(roots))
    for (fock,configs) in v.data
        add_fockconfig!(out,fock)
        for (config,coeffs) in configs
            out[fock][config] = deepcopy(v[fock][config][roots])
        end
    end

    return out
end

max_stars_repo_name: nmayhall-vt/FermiCG

max_issues_repo_name: nmayhall-vt/FermiCG

max_forks_repo_name: nmayhall-vt/FermiCG


text:
# file stabilization.jl
# this is the primary file for adding stabilization to the weak form.
# Stabilization functions may be put in separate files if they are included
# here

include("GLS.jl")
include("filtering.jl")
include("artificial_dissipation.jl")

@doc """
### EulerEquationMod.edgestabilize!

Adds edge-stabilization (see Burman and Hansbo, doi:10.1016/j.cma.2003.12.032)
to a given residual.  Different methods are available depending on the rank of
`u`:

* For *scalar* fields, it is assumed that `u` is a rank-2 array, with the first
dimension for the local-node index, and the second dimension for the element
index.
* For *vector* fields, `u` is a rank-3 array, with the first dimension for the
index of the vector field, the second dimension for the local-node index, and
the third dimension for the element index.

Naturally, the number of entries in the dimension of `u` (and `res`)
corresponding to the nodes must be equal to the number of nodes in the SBP
operator `sbp`.

**Inputs**

* `sbp`: an SBP operator type
* `ifaces`: list of element interfaces stored as an array of `Interface`s
* `u`: the array of solution data, numDofPerNode x numNodesPerElement x numEl.
       for scalar fields (ie. numDofPerNode), this can be a 2D array.
* `x`: Cartesian coordinates stored in (coord,node,element) format
* `dxidx`: scaled Jacobian of the mapping (as output from `mappingjacobian!`)
* `jac`: determinant of the Jacobian array, numNodesPerElement x numEl
* `alpha`: array of transformation terms (see below)
* `stabscale`: numfaces x numNodes per face array of scaling parameter

**In/Outs**

* `res`: where the result of the integration is stored, same shape as u

**Details**

The array `alpha` is used to compute the directional derivative normal to the faces.
For a 2-dimensional problem, it can be computed as follows:

      for k = 1:mesh.numelem
        for i = 1:sbp.numnodes
          for di1 = 1:2
            for di2 = 1:2
              alpha[di1,di2,i,k] = (dxidx[di1,1,i,k].*dxidx[di2,1,i,k] +
                                dxidx[di1,2,i,k].*dxidx[di2,2,i,k])*jac[i,k]
            end
          end
        end
      end

"""->
function edgestabilize!(sbp::AbstractOperator{T}, ifaces::Array{Interface},
                  mesh::AbstractMesh,
                  u::AbstractArray{T,2}, x::AbstractArray{T,3},
                  dxidx::AbstractArray{T,4}, jac::AbstractArray{T,2},
                  alpha::AbstractArray{T,4},
                  stabscale::AbstractArray{Tres, 2},
                  res::AbstractArray{T,2}) where {T, Tres}
# for scalar equations only!
  @assert( sbp.numnodes == size(u,1) == size(res,1) == size(dxidx,3) == size(x,2)
          == size(alpha,3) )
  @assert( size(dxidx,4) == size(alpha,4) == size(u,2) == size(res,2) == size(x,3) )
  @assert( length(u) == length(res) )
  dim = size(sbp.Q, 3)

  numNodesPerFace = size(stabscale, 2)
  # JEH: temporary, until nbrnodeindex is part of sbp type
  nbrnodeindex = collect(numNodesPerFace:-1:1)

  Dn = zero(T)
  dirL = zeros(T, (dim))
  dirR = zeros(T, (dim))
  workvec = zeros(T, (dim))
  tmpL = zero(T)
  tmpR = zero(T)
  EDn = zeros(T, (numNodesPerFace) )
  for (facenum, face) in enumerate(ifaces)
    fill!(EDn, zero(T))
    for i = 1:numNodesPerFace
      # iL = element-local index for ith node on left element face
      # iR = element-local index for ith node on right element face
      iL = mesh.facenodes[i, face.faceL]::Int
      #iR = mesh.facenodes[getnbrnodeindex(sbp, face, i)::Int, face.faceR]::Int
      iR = mesh.facenodes[nbrnodeindex[i], face.faceR]::Int
      # apply the normal-derivative difference operator along the face
      smallmatvec!(sview(alpha,:,:,iL,face.elementL),
                   sview(mesh.sbpface.normal,:,face.faceL), dirL)
      Dn = zero(T)
      Dn = directionalDifferentiateElement!(sbp, dirL, sview(u,:,face.elementL), iL)
      smallmatvec!(sview(alpha,:,:,iR,face.elementR),
                   sview(mesh.sbpface.normal,:,face.faceR), dirR)
      Dn += directionalDifferentiateElement!(sbp, dirR, sview(u,:,face.elementR), iR)
      # get differential area element: need 1/ds for each Dn term (here and loop
      # below)to get unit normal, and then need ds for integration, so net
      # result is 1/ds
      ds = calcDiffElementArea(sview(mesh.sbpface.normal,:,face.faceL),
                              sview(dxidx,:,:,iL,face.elementL), workvec)::T
      # apply the scaling function

      scale = stabscale[i, facenum]
#      Dn *= stabscale(u[iL,face.elementL], sview(dxidx,:,:,iL,face.elementL),
#                      sview(mesh.sbpface.normal,:,face.faceL))::T/ds # note that u[iL] = u[iR]
      Dn *= scale/ds
      # add the face-mass matrix contribution
      for j = 1:numNodesPerFace
        EDn[j] += sbp.wface[j,i]*Dn
      end
    end
    # here we use hand-coded reverse-mode to apply the transposed
    # normal-derivative difference operator
    for i = 1:numNodesPerFace
      iL = mesh.facenodes[i, face.faceL]::Int
      #iR = mesh.facenodes[getnbrnodeindex(sbp, face, i), face.faceR]::Int
      iR = mesh.facenodes[nbrnodeindex[i], face.faceR]::Int
      smallmatvec!(sview(alpha,:,:,iL,face.elementL),
                   sview(mesh.sbpface.normal,:,face.faceL), dirL)
      smallmatvec!(sview(alpha,:,:,iR,face.elementR),
                   sview(mesh.sbpface.normal,:,face.faceR), dirR)
      for di = 1:dim
        tmpL = dirL[di]*EDn[i]/sbp.w[iL]
        tmpR = dirR[di]*EDn[i]/sbp.w[iR]
        for j = 1:sbp.numnodes
          res[j,face.elementL] += sbp.Q[iL,j,di]*tmpL
          res[j,face.elementR] += sbp.Q[iR,j,di]*tmpR
        end
      end
    end
  end
end

# for vector equations

function edgestabilize!(sbp::AbstractOperator,
     mesh::AbstractMesh,
     ifaces::Array{Interface}, u::AbstractArray{Tsol,3},
     x::AbstractArray{Tmsh,3}, dxidx::AbstractArray{Tmsh,4},
     jac::AbstractArray{Tmsh,2},
     alpha::AbstractArray{Tmsh,4},
     stabscale::AbstractArray{Tres,2},
     res::AbstractArray{Tres,3}) where {Tmsh,  Tsol, Tres}

  @assert( sbp.numnodes == size(u,2) == size(res,2) == size(dxidx,3) == size(x,2)
          == size(alpha,3) )
  @assert( size(dxidx,4) == size(alpha,4) == size(u,3) == size(res,3) == size(x,3) )
  @assert( length(u) == length(res) )
  dim = size(sbp.Q, 3)

  numNodesPerFace = size(stabscale, 2)
  # JEH: temporary, until nbrnodeindex is part of sbp type
  nbrnodeindex = collect(numNodesPerFace:-1:1)

  Dn = zeros(Tsol, size(u,1))
  dirL = zeros(Tmsh, (dim))
  dirR = zeros(Tmsh, (dim))
  workvec = zeros(Tmsh, (dim))
  tmpL = zero(Dn)
  tmpR = zero(Dn)
  EDn = zeros(Tres, (size(u,1),numNodesPerFace) )
  for (facenum, face) in enumerate(ifaces)
    fill!(EDn, zero(Tres))
    for i = 1:numNodesPerFace
      # iL = element-local index for ith node on left element face
      # iR = element-local index for ith node on right element face
      iL = mesh.facenodes[i, face.faceL]
      #iR = mesh.facenodes[getnbrnodeindex(sbp, face, i), face.faceR]
      iR = mesh.facenodes[nbrnodeindex[i], face.faceR]

      # apply the normal-derivative difference operator along the face
      smallmatvec!(sview(alpha,:,:,iL,face.elementL),
                   sview(mesh.sbpface.normal,:,face.faceL), dirL)

      fill!(Dn, zero(Tres))
      directionalDifferentiateElement!(sbp, dirL, sview(u,:,:,face.elementL), iL, Dn)
      smallmatvec!(sview(alpha,:,:,iR,face.elementR),
                   sview(mesh.sbpface.normal,:,face.faceR), dirR)
      directionalDifferentiateElement!(sbp, dirR, sview(u,:,:,face.elementR), iR, Dn)
      # get differential area element: need 1/ds for each Dn term (here and loop
      # below) to get unit normals, and then need ds for integration, so net
      # result is 1/ds
      ds = calcDiffElementArea(sview(mesh.sbpface.normal,:,face.faceL),
                              sview(dxidx,:,:,iL,face.elementL), workvec)

      # apply the scaling function
      scale = stabscale[i, facenum]
      for field = 1:size(u,1)
        Dn[field] *= scale
      end

      # add the face-mass matrix contribution
      for j = 1:numNodesPerFace
        for field = 1:size(u,1)
          EDn[field,j] += sbp.wface[j,i]*Dn[field]
        end
      end
    end

    for i = 1:numNodesPerFace
      iL = mesh.facenodes[i, face.faceL]
      iR = mesh.facenodes[nbrnodeindex[i], face.faceR]
      smallmatvec!(sview(alpha,:,:,iL,face.elementL),
                   sview(mesh.sbpface.normal,:,face.faceL), dirL)
      smallmatvec!(sview(alpha,:,:,iR,face.elementR),
                   sview(mesh.sbpface.normal,:,face.faceR), dirR)

      # here we use hand-coded reverse-mode to apply the transposed
      # normal-derivative difference operator
      for di = 1:size(sbp.Q, 3)
        for field = 1:size(u,1)
          tmpL[field] = dirL[di]*EDn[field,i]/sbp.w[iL]
          tmpR[field] = dirR[di]*EDn[field,i]/sbp.w[iR]
        end
          elementL = face.elementL
          elementR = face.elementR
        for j = 1:sbp.numnodes
          for field = 1:size(u,1)
            tmp2 =  sbp.Q[iL,j,di]*tmpL[field]
            res[field,j,face.elementL] += sbp.Q[iL,j,di]*tmpL[field]
            tmp2 = sbp.Q[iR,j,di]*tmpR[field]
            res[field,j,face.elementR] += sbp.Q[iR,j,di]*tmpR[field]
          end
        end
      end
    end
  end

end

# for vector equations
# WIP: uses res_edge
#TODO: cleanup function signature
function edgestabilize!(mesh, sbp::AbstractOperator, eqn,
     ifaces::Array{Interface}, u::AbstractArray{Tsol,3},
     x::AbstractArray{Tmsh,3}, dxidx::AbstractArray{Tmsh,4},
     jac::AbstractArray{Tmsh,2},
     alpha::AbstractArray{Tmsh,4},
     stabscale::AbstractArray{Tres,2},
     res::AbstractArray{Tres, 3}, res_edge::AbstractArray{Tres,4}) where {Tmsh,  Tsol, Tres}

  @assert( sbp.numnodes == size(u,2) == size(res,2) == size(dxidx,3) == size(x,2)
          == size(alpha,3) )
  @assert( size(dxidx,4) == size(alpha,4) == size(u,3) == size(res,3) == size(x,3) )
#  @assert( length(u) == length(res) )
# res is the residual array for element affecting themselves
# res_edge is the residual array of how elements affect each other
# this won't work for finite differences
# should there be a way to detect if this is a residual evaluation or a
# differentiation and use the 2x faster version of this function?
  numNodesPerFace = size(stabscale, 2)
  dim = size(sbp.Q, 3)

#  println("initially:")
#  println("res = \n", res)
#  println("res_edge = \n", res_edge)

  # JEH: temporary, until nbrnodeindex is part of sbp type
  nbrnodeindex = collect(numNodesPerFace:-1:1)

  Dn = zeros(Tsol, size(u,1))
  dirL = zeros(Tmsh, (dim))
  dirR = zeros(Tmsh, (dim))
  workvec = zeros(Tmsh, (dim))
  tmpL = zero(Dn)
  tmpR = zero(Dn)
  EDn = zeros(Tres, (size(u,1),numNodesPerFace) )

  # consider how elementR affects elementL and itself
  for (facenum, face) in enumerate(ifaces)
  # for facenum = 1:length(ifaces)
    # face = ifaces[facenum]
    fill!(EDn, zero(Tres))
    uL = real(sview(u, :, :, face.elementL))
    uR = sview(u, :, :, face.elementR)
    # println("elementL = ", face.elementL, ", elementR = ", face.elementR)
    # println("uL = ", uL, ", uR = ", uR)
    for i = 1:numNodesPerFace  # consider making this its own function
      # iL = element-local index for ith node on left element face
      # iR = element-local index for ith node on right element face
      iL = mesh.facenodes[i, face.faceL]
      #iR = mesh.facenodes[getnbrnodeindex(sbp, face, i), face.faceR]
      iR = mesh.facenodes[nbrnodeindex[i], face.faceR]
      # apply the normal-derivative difference operator along the face
      smallmatvec!(sview(alpha,:,:,iL,face.elementL),
                   sview(mesh.sbpface.normal,:,face.faceL), dirL)
      fill!(Dn, zero(Tres))
      directionalDifferentiateElement!(sbp, dirL, uL, iL, Dn)
      smallmatvec!(sview(alpha,:,:,iR,face.elementR),
                   sview(mesh.sbpface.normal,:,face.faceR), dirR)
      directionalDifferentiateElement!(sbp, dirR, uR, iR, Dn)
      # get differential area element: need 1/ds for each Dn term (here and loop
      # below) to get unit normals, and then need ds for integration, so net
      # result is 1/ds
      ds = calcDiffElementArea(sview(mesh.sbpface.normal,:,face.faceL),
                              sview(dxidx,:,:,iL,face.elementL), workvec)  # this assumes Tsbp is a lower type than the other
      # apply the scaling function
      # calculate scale
      #=
      iL = mesh.facenodes[j, face_i.faceL]
      iR = mesh.facenodes[nbrnodeindex[j], face_i.faceR]
      q = sview(eqn.q, :, iL, face_i.elementL)
      dxidx = sview(mesh.dxidx, :, :, iL, face_i.elementL)
      nrm = sview(mesh.sbpface.normal, :, face_i.faceL)
      =#
      uL_node = real(sview(eqn.q, :, iL, face.elementL)) # take real part here?
      dxidx_node = sview(mesh.dxidx, :, :, iL, face.elementL)
      nrm_node = sview(mesh.sbpface.normal, :, face.faceL)

      scale = stabscale(uL_node, dxidx_node, nrm_node, eqn.params)
      # scale = stabscale[i, facenum]
      # scale = stabscale(sview(u,:,iL,face.elementL), sview(dxidx,:,:,iL,face.elementL),
                         # sview(mesh.sbpface.normal,:,face.faceL))::T./ds # note that u[iL] = u[iR]
      # println("before scaling, Dn = ", Dn)
      # println("scale = ", scale)
      for field = 1:size(u,1)
        Dn[field] *= scale
      end

      # println("after scaling Dn = ", Dn)
      # add the face-mass matrix contribution
      for j = 1:numNodesPerFace
        for field = 1:size(u,1)
          EDn[field,j] += sbp.wface[j,i]*Dn[field]
        end
      end
    end  # end loop over face nodes

    # println("EDn = ", EDn)

    for i = 1:numNodesPerFace
      # println("mesh.facenodes = ", mesh.facenodes)

      iL = mesh.facenodes[i, face.faceL]
      #iR = mesh.facenodes[getnbrnodeindex(sbp, face, i), face.faceR]
      iR = mesh.facenodes[nbrnodeindex[i], face.faceR]
      smallmatvec!(sview(alpha,:,:,iL,face.elementL),
                   sview(mesh.sbpface.normal,:,face.faceL), dirL)
      smallmatvec!(sview(alpha,:,:,iR,face.elementR),
                   sview(mesh.sbpface.normal,:,face.faceR), dirR)
      # println("dirL = ", dirL)
      # println("dirR = ", dirR)
      # here we use hand-coded reverse-mode to apply the transposed
      # normal-derivative difference operator
      for di = 1:size(sbp.Q, 3)
        for field = 1:size(u,1)
          tmpL[field] = dirL[di]*EDn[field,i]/sbp.w[iL]
          tmpR[field] = dirR[di]*EDn[field,i]/sbp.w[iR]
        end

        # println("tmpL = ", tmpL)
        # println("tmpR = ", tmpR)
        elementL = face.elementL
        elementR = face.elementR
        faceL = face.faceL
        faceR = face.faceR
        for j = 1:sbp.numnodes
          for field = 1:size(u,1)
            # this is elementR affecting elementL
            tmp2 = sbp.Q[iL,j,di]*tmpL[field]
            # println("res_edge[$field, $j, $elementL, $faceL] += ", tmp2)

            res_edge[field,j,face.elementL, face.faceL] += sbp.Q[iL,j,di]*tmpL[field]
            # this is elementR affecting itself
            tmp2 = sbp.Q[iR,j,di]*tmpR[field]
            # println("res[$field, $j, $elementR] += ", tmp2)

            res[field,j,face.elementR] += sbp.Q[iR,j,di]*tmpR[field]
          end  # end loop over fields
        end  # end loop j=1:sbp.numnodes
      end  # end loop over directions di
    end  # end loop over i = 1:numNodesPerFace
  end  # end loop over interfaces

  # println("res = \n", res)
  # println("res_edge = \n", res_edge)
  # println("edgestabilization second calculation")
  # now consider how elementL affects elementR, and itself
  for (facenum, face) in enumerate(ifaces)
  # for facenum = 1:length(ifaces)
    # face = ifaces[facenum]
    fill!(EDn, zero(Tres))
    uL = sview(u, :, :, face.elementL)
    uR = real(sview(u, :, :, face.elementR))
    # println("elementL = ", face.elementL, ", elementR = ", face.elementR)
    # println("uL = ", uL, ", uR = ", uR)

    for i = 1:numNodesPerFace  # consider making this its own function
      # iL = element-local index for ith node on left element face
      # iR = element-local index for ith node on right element face
      iL = mesh.facenodes[i, face.faceL]
      #iR = mesh.facenodes[getnbrnodeindex(sbp, face, i), face.faceR]
      iR = mesh.facenodes[nbrnodeindex[i], face.faceR]
      # apply the normal-derivative difference operator along the face
      smallmatvec!(sview(alpha,:,:,iL,face.elementL),
                   sview(mesh.sbpface.normal,:,face.faceL), dirL)
      fill!(Dn, zero(Tres))
      directionalDifferentiateElement!(sbp, dirL, uL, iL, Dn)
      smallmatvec!(sview(alpha,:,:,iR,face.elementR),
                   sview(mesh.sbpface.normal,:,face.faceR), dirR)
      directionalDifferentiateElement!(sbp, dirR, uR, iR, Dn)
      # get differential area element: need 1/ds for each Dn term (here and loop
      # below) to get unit normals, and then need ds for integration, so net
      # result is 1/ds
      ds = calcDiffElementArea(sview(mesh.sbpface.normal,:,face.faceL),
                              sview(dxidx,:,:,iL,face.elementL), workvec)
      # apply the scaling function
      uL_node = sview(eqn.q, :, iL, face.elementL)
      dxidx_node = sview(mesh.dxidx, :, :, iL, face.elementL)
      nrm_node = sview(mesh.sbpface.normal, :, face.faceL)

      scale = stabscale(uL_node, dxidx_node, nrm_node, eqn.params)

      # scale = stabscale[i, facenum]
      # scale = stabscale(sview(u,:,iL,face.elementL), sview(dxidx,:,:,iL,face.elementL),
                         # sview(mesh.sbpface.normal,:,face.faceL))::T./ds # note that u[iL] = u[iR]

      for field = 1:size(u,1)
        Dn[field] *= scale
      end
      # add the face-mass matrix contribution
      for j = 1:numNodesPerFace
        for field = 1:size(u,1)
          EDn[field,j] += sbp.wface[j,i]*Dn[field]
        end
      end
    end  # end loop over face nodes

    for i = 1:numNodesPerFace
      # println("mesh.facenodes = ", mesh.facenodes)

      iL = mesh.facenodes[i, face.faceL]
      #iR = mesh.facenodes[getnbrnodeindex(sbp, face, i), face.faceR]
      iR = mesh.facenodes[nbrnodeindex[i], face.faceR]
      smallmatvec!(sview(alpha,:,:,iL,face.elementL),
                   sview(mesh.sbpface.normal,:,face.faceL), dirL)
      smallmatvec!(sview(alpha,:,:,iR,face.elementR),
                   sview(mesh.sbpface.normal,:,face.faceR), dirR)
      # here we use hand-coded reverse-mode to apply the transposed
      # normal-derivative difference operator
      for di = 1:size(sbp.Q, 3)
        for field = 1:size(u,1)
          tmpL[field] = dirL[di]*EDn[field,i]/sbp.w[iL]
          tmpR[field] = dirR[di]*EDn[field,i]/sbp.w[iR]
        end

        elementL = face.elementL
        elementR = face.elementR
        faceL = face.faceL
        faceR = face.faceR
        for j = 1:sbp.numnodes
          for field = 1:size(u,1)
            # this is elementL affecting itself
            tmp2 = sbp.Q[iL,j,di]*tmpL[field]

            # println("res[$field, $j, $elementL] += ", tmp2)

            res[field,j,face.elementL] += sbp.Q[iL,j,di]*tmpL[field]
            tmp2 = sbp.Q[iR,j,di]*tmpR[field]
            # println("res_edge[$field, $j, $elementR, $faceR] += ", tmp2)
            # this is elementL affecting elementR
            res_edge[field,j,face.elementR, face.faceR] += sbp.Q[iR,j,di]*tmpR[field]
          end  # end loop over fields
        end  # end loop j=1:sbp.numnodes
      end  # end loop over directions di
    end  # end loop over i = 1:numNodesPerFace
  end  # end loop over interfaces

#  println("finished second edge stabilization calculation")
#  println("res = \n", res)
#  println("res_edge = \n", res_edge)

end  # end function

@doc """
### EulerEquationMod.stabscale

  This function calculates the edge stabilization scalaing parameter at a node
  and returns it.

   Inputs:
    * q : vector of conservative variables
    * dxidx : jacobian of xi wrt x coordinates at the node
    * nrm : normal vector in xi space
    * params : ParamType{2}

    This is a low level function
"""->
# low level function
function stabscale(q::AbstractArray{Tsol,1},
       dxidx::AbstractArray{Tmsh,2}, nrm::AbstractArray{Tmsh,1},
       params::ParamType{2} ) where {Tmsh, Tsol}
# calculate stabscale for a single node

#     println("==== entering stabscale ====")

    gamma = params.gamma
    edge_stab_gamma = params.edgestab_gamma
    q_vals = zeros(Tsol, length(q))

    # u holds whatever the variable type that we are solving the equation in
    # q_vals holds the conservative variables
    # convert to conservative variables if not already using them
    convertToConservative(params, q, q_vals)

    # calc pressure using the variables u and the params object
    # of type ParamType{2, :the_variable_type_of_u}
    pressure = calcPressure(params, q)
    speed_sound = calcSpeedofSound(params, q)

    # extract conserved variables
    rho = q_vals[1]
    vel_x = q_vals[2]/rho
    vel_y = q_vals[3]/rho
    Energy = q_vals[4]

    vel_squared = vel_x^2 + vel_y^2
    energy = Energy/rho - (1/2)*vel_squared

    # edge lengths component wise
    h_x = dxidx[1,1]*nrm[1] + dxidx[2,1]*nrm[2]
    h_y = dxidx[1,2]*nrm[1] + dxidx[2,2]*nrm[2]
    h = sqrt(h_x^2 + h_y^2)  # edge length

    U = vel_x*(h_x/h) + vel_y*(h_y/h)

    return (abs(U) + speed_sound)*edge_stab_gamma*h^(2)

  end

@doc """
### EulerEquationMod.stabscale

  This function calculate the stabilization scaling parameter across the
  entire mesh by calling the low level method.  This populates eqn.stabscale.

  This is a mid level function
"""->
# mid level function
function stabscale(mesh::AbstractMesh{Tmsh}, sbp::AbstractOperator,
                   eqn::EulerData{Tsol}) where {Tmsh,  Tsol}
# calculate stabscale for entire mesh

  nbrnodeindex = collect(mesh.numNodesPerFace:-1:1)

  for i=1:mesh.numInterfaces
    face_i = mesh.interfaces[i]
    for j=1:mesh.numNodesPerFace
      #TODO: iL and iR need more descriptive names
      iL = mesh.facenodes[j, face_i.faceL]
      iR = mesh.facenodes[nbrnodeindex[j], face_i.faceR]
      q = sview(eqn.q, :, iL, face_i.elementL)
      dxidx = sview(mesh.dxidx, :, :, iL, face_i.elementL)
      nrm = sview(mesh.sbpface.normal, :, face_i.faceL)

      eqn.stabscale[j,i] = stabscale(q, dxidx, nrm, eqn.params)
    end
  end

  return nothing
end

@doc """
### EulerEquationMod.calcEdgeStabAlpha

  This function calculates the edge stabilization parameter alpha across the
  entire mesh.

  This is a mid level function.
"""
# used by EulerData Constructor - not that that matters for any reason
# mid level function
function calcEdgeStabAlpha(mesh::AbstractMesh{Tmsh},
  sbp::AbstractOperator, eqn::EulerData{Tsol, Tres, Tdim}) where {Tmsh,  Tsol, Tres, Tdim}
# calculate alpha, needed by edge stabilization

  numEl = mesh.numEl
  dxidx = mesh.dxidx
  jac = mesh.jac

  # calculating alpha, required by edgestabilize!
  # this can be computed a priori
  for k = 1:numEl
    for i = 1:mesh.numNodesPerElement
      for di1 = 1:Tdim
        for di2 = 1:Tdim
          eqn.edgestab_alpha[di1,di2,i,k] = (dxidx[di1,1,i,k].*dxidx[di2,1,i,k] + dxidx[di1,2,i,k].*dxidx[di2,2,i,k])*jac[i,k]
        end
      end
    end
  end

  return nothing
end


text:
include("error.jl")
include("ast_def.jl")
include("ctx_def.jl")
include("alphalib.jl")
include("ast.jl")
include("ctx.jl")
include("semantics.jl")
include("codegen.jl")
include("cli.jl")

# Generated by parser.syntax
include("parser.yy.jl")

alphamain() = climain(ARGS)


text:
# Inspired from https://github.com/jump-dev/Clp.jl/blob/master/scripts/clang.jl written by Oscar Downson (@odow).
#
# This script can be used to build the C interface to CUDD. However, it requires
# you to manually do the following steps first:
#
# 1) Copy `cudd/cudd.h` from CUDD into this /scripts directory
# 2) Run this script

import Clang

LIBCUDD_HEADERS = [
    joinpath(@__DIR__, "cudd.h"),
]

wc = Clang.init(
    headers = LIBCUDD_HEADERS,
    output_file = joinpath(dirname(@__DIR__), "src", "gen", "libcudd_api.jl"),
    common_file = joinpath(dirname(@__DIR__), "src", "gen", "libcudd_common.jl"),
    header_wrapped = (root, current) -> root == current,
    header_library = x -> "libcudd",
    clang_diagnostics = true,
)

run(wc)


text:
import Random: rand, rand!, randn!, MersenneTwister
import RandomNumbers: AbstractRNG
import AQFED.Math: norminvAS241, norminvBSMS
#import AQFED.Random: randz

export AbstractRNGSeq, ZRNGSeq, next!, nextn!, skipTo

mutable struct AbstractRNGSeq{T,N} <: AbstractSeq{N} #where {T <: AbstractRNG}
    rng::T
    startIndex::Int
    currentIndex::Int
end

AbstractRNGSeq(rng::T, ndims::Int) where {T} = AbstractRNGSeq{T,ndims}(rng, 0, 0)

@inline function next!(s::AbstractRNGSeq, points::AbstractVector{<:AbstractFloat})
    rand!(s.rng, points)
    s.currentIndex += length(points)
end

@inline function nextn!(s::AbstractRNGSeq, points::AbstractVector{<:AbstractFloat})
    rand!(s.rng, points)
    @. points = norminv(points) #deterministic number of random numbers
    s.currentIndex += length(points)
end

@inline function nextn!(s::AbstractRNGSeq{T, N},  dim::Int, points::AbstractVector{<:AbstractFloat}) where {T,N}
    rand!(s.rng, points)
    @. points = norminv(points) #deterministic number of random numbers
     # @. points = norminvBSMS(points)
    s.currentIndex += length(points)

end
@inline function next!(s::AbstractRNGSeq{T, N}, dim::Int, points::AbstractVector{<:AbstractFloat}) where {T,N}
    rand!(s.rng, points)
    s.currentIndex += length(points)
end

function skipTo(s::AbstractRNGSeq, dim::Int, startIndex::Int)
    #the following allows to not skip at every dim. Breaks BB use in some ways,
    #but is supported by most RNGs. Counter-based RNGs may have a specific impl.
    #presupposes that RNG is not used
    n = ndims(s)
    if s.startIndex != startIndex
        skip(s.rng, startIndex*n - s.currentIndex)
        s.currentIndex = startIndex*n
        s.startIndex = startIndex
    end
end

mutable struct ZRNGSeq{T,N} <: AbstractSeq{N} #where {T <: AbstractRNG}
    rng::T
    startIndex::Int
end

ZRNGSeq(rng::T, ndims::Int) where {T} = ZRNGSeq{T,ndims}(rng, 0)

@inline function next!(s::ZRNGSeq, points::AbstractVector{<:AbstractFloat})
    rand!(s.rng, points)
end

@inline function nextn!(s::ZRNGSeq, points::AbstractVector{<:AbstractFloat})
    randn!(s.rng, points)
end

@inline function nextn!(s::ZRNGSeq,  dim::Int, points::AbstractVector{<:AbstractFloat})
    randn!(s.rng, points)
end

@inline function next!(s::ZRNGSeq, dim::Int, points::AbstractVector{<:AbstractFloat})
    rand!(s.rng, points)
end

function skipTo(s::ZRNGSeq, dim::Int, startIndex::Int)
    #the following allows to not skip at every dim. Breaks BB use in some ways,
    #but is supported by most RNGs. Counter-based RNGs may have a specific impl.
    #presupposes that RNG is not used
    n = ndims(s)
    if s.startIndex != startIndex
        skipTo(s.rng, startIndex*n)
        s.startIndex = startIndex
    end
end


text:
#  Unit "Gajd.jl" of the Diagonalization.jl Package for Julia language
#
#  MIT License
#  Copyright (c) 2019,
#  Marco Congedo, CNRS, Grenoble, France:
#  https://sites.google.com/site/marcocongedo/home

# ? CONTENTS :
#  This unit implements the Gauss AJD algorithm of Congedo (unpublished).

#  The algorithm handles the AJD diagonalization procedure, corresponding
#  to the case m=1, k>1 according to the taxonomy adopted in this package.

# update input matrices and AJD matrix for Gauss Algorithms
# bj <- bj +  θbi (update the jth column with respect to the ith one)
# Cpj <- bp' C (bj +  θbi) for p=1:n (jth row of C)
# Cjq <- (bj +  θbi)'' C bq  for q=1:n (jth column of C)
# the update of C is done only on the lower triangular part
# 𝐋 is a lower triangular matrix of k-length vectors : 𝐋[i, j][k]=C[k][i, j]
@inline function _update1!(j, i, n, θ, θ², 𝐋, B) # i>j
   for p = 1:j-1 𝐋[j, p] += θ*𝐋[i, p] end     # update 𝐂 :
   𝐋[j, j] += θ²*𝐋[i, i] + 2θ*𝐋[i, j]         # write jth row and column
   for p = j+1:i 𝐋[p, j] += θ*𝐋[i, p] end     # only on the lower
   for p = i+1:n 𝐋[p, j] += θ*𝐋[p, i] end     # triangular part.
   B[:, j] += θ*B[:, i]                       # update B
end

# update1! takes care of the udpate if i>j, update2! if j≥i
@inline function _update2!(j, i, n, θ, θ², 𝐋, B) # j>i
   for p = 1:i-1 𝐋[j, p] += θ*𝐋[i, p] end     # update 𝐂 :
   𝐋[j, j] += θ²*𝐋[i, i] + 2θ*𝐋[j, i]         # write jth row and column
   for p = i:j-1 𝐋[j, p] += θ*𝐋[p, i] end     # only on the lower
   for p = j+1:n 𝐋[p, j] += θ*𝐋[p, i] end     # triangular part.
   B[:, j] += θ*B[:, i]                       # update B
end

#  PRIMITIVE GAJD algorithm:
#  It takes as input a lower triangular matrix
#  holding in its elements vectors of k real numbers.
#  From data in matrix form given as k symmetric C_κ matrices of size nxn,
#  we have 𝐋[i, j][κ] = C_κ[i, j], for κ=1:k, i>j=1:n.
#  It finds a non-singular matrix B such that the
#  congruences B'*C_κ*B are as diagonal as possible for all κ=1:k.
#  `tol` is the convergence to be attained.
#  `maxiter` is the maximum number of iterations allowed.
#  if `verbose`=true, the convergence attained at each iteration and other
#  information will be printed.
#  RETURN: B, the number of iterations and the convergence attained (a 3-tuple)
function gajd(𝐋::AbstractArray;
               tol = 0.,
               maxiter = 1000,
               verbose = false)

   # find optimal theta and update convergence (∡)
   function _gauss!(𝑖, 𝑗, i) # 𝑖 must be < 𝑗
      θ  = h*sum(𝐋[𝑗, 𝑖].*𝐋[i, i])
      θ² = θ^2
      ∡ += θ²
   end

   @inline function congedoSweep!()
      ∡ = T(0.)
      for i = 1:n
         h = -inv(sum(𝐋[i, i].^2)) # h is invariant in the inner loop

         # transform all j≠i columns of B with respect to its ith column:
         for j = 1:i-1
            _gauss!(j, i, i) # find θ, θ² and update ∡
            _update1!(j, i, n, θ, θ², 𝐋, B) # update 𝐋 and B given θ and θ²
         end
         for j = i+1:n
            _gauss!(i, j, i) # find θ, θ² and update ∡
            _update2!(j, i, n, θ, θ², 𝐋, B) # update 𝐋 and B given θ and θ²
         end
      end
      return ∡ * e # convergence: average squared theta over all n(n-1) pairs
   end

   # declare variables
   T, n = eltype(𝐋[1, 1]), size(𝐋, 1)
   h, θ, θ², ∡, e = T(0), T(0), T(0), T(0), T(inv(n*(n-1)))
   B = Matrix{T}(I, n, n) # initialization of the AJD matrix

   iter, conv = _iterate!("GOFF", congedoSweep!, maxiter, T, tol, verbose)

   return B, iter, conv
end

#  ADVANCED GAJD algorithm:
#  It takes as input a vector of k real symmetric matrices 𝐂 and finds a
#  non-singular matrix B such that the congruences B'*𝐂_κ*B are as diagonal
#  as possible for all κ=1:k.

#  if `trace1` is true (false by default), all input matrices are normalized
#  so as to have unit trace. Note that the diagonal elements
#  of the input matrices must be all positive.
#
#  `w` is an optional vector of k positive weights for each matrix in 𝐂.
#  if `w` is different from `nothing` (default), the input matrices are
#  weighted with these weights (after trace normalization if `trace1` is true).
#  A function can be passed as the `w` argument, in which case the kth weight
#  is found as the output of the function applied to the kth matrix in 𝐂.
#  A good choice in general is the `nonD` function declared in tools.jl unit.
#
#  if `whitening` = true is passed, the Arithmetic mean of the matrices in 𝐂 is
#  computed (using the PosDefManifold.jl package) and the matrices in 𝐂
#  are pre-transformed using the whitening matrix of the mean.
#  Dimensionality reduction can be obtained at this stage using optional
#  arguments `eVar` and `eVarMeth` (see documentation of the AJD constructors).
#
#  if sort=true (default) the column vectors of the B matrix are normalized
#  to unit norm and permuted so as to sort in descending order the mean over
#  κ=1:k of the diagonal elements of B'*𝐂_κ*B.
#  Note that if `whitening` is true the output B will not have unit norm columns
#  as it is multiplied by the whitener after being scaled and sorted
#  and before being returned.
#
#  if  `whitening` = false (default), a matrix can be provided with the `init`
#  argument in order to initialize B. In this case the actual AJD
#  will be given by init*B, where B is the output of the algorithm.
#
#  `tol` is the convergence to be attained.
#
#  `maxiter` is the maximum number of iterations allowed.
#
#  if `verbose`=true, the convergence attained at each iteration and other
#  information will be printed.
#
#  return: B, its pseudo-inverse, the mean diagonal elements of B'*mean(𝐂)*B,
#          the number of iterations and the convergence attained
#
#  NB: Differently from other AJD algorithms, this algorithm proceeds
#  by transformations of one vector at a time given a pair of vectors of B.
#  A sweep goes over all n*(n+1) ij pairs, with i,j ∈ 1:n, j≠i.
#  The update of the input matrices is RECURSIVE, thus it is not suitable
#  for multi-threading. This algorithm has the lowest complexity per iteration
#  among all algorithms here implemented and scale extremely well over k, i.e.,
#  for n small and k large it offers the best performance.
function gajd( 𝐂::Union{Vector{Hermitian}, Vector{Symmetric}};
               trace1   :: Bool  = false,
               w        :: Twf   = ○,
               preWhite :: Bool  = false,
               sort     :: Bool  = true,
               init     :: Union{Matrix, Nothing} = ○,
               tol      :: Real  = 0.,
               maxiter  :: Int   = 1000,
               verbose  :: Bool  = false,
            eVar     :: TeVaro = ○,
            eVarMeth :: Function = searchsortedfirst)

   # trace normalization and weighting
   𝐆 = _normalizeAndWeight(trace1, w, 𝐂)

   # pre-whiten or initialize or nothing
   W = _preWhiteOrInit!(𝐆, preWhite, Euclidean, eVar, eVarMeth, init)

   T, n = eltype(𝐆[1]), size(𝐆[1], 1)

   # arrange data in a LowerTriangular matrix of k-vectors
   𝐋 = _arrangeData!(T, n, 𝐆)

   B, iter, conv = gajd(𝐋; tol=tol, maxiter=maxiter, verbose=verbose)

   # scale and permute the vectors of B
   D=Diagonal([mean(𝐋[i, i]) for i=1:n])
   λ = sort ? _permute!(_scale!(B, D, n)...) : diag(D)

   return preWhite ? (W.F*B, pinv(B)*W.iF, λ, iter, conv) :
                     (B, pinv(B), λ, iter, conv)
end

function gLogLike(𝐋::AbstractArray;
                  tol = 0.,
                  maxiter = 1000,
                  verbose = false)

   # find optimal theta and update convergence ∡
   function _gauss!(𝑖, 𝑗, i, j) # 𝑖 must be < 𝑗
      #=
      fill!(Π, T(1))
      for l=1:𝑖-1 Π.*=𝐋[l, l] end # much faster without the if!
      for l=𝑖+1:𝑗-1 Π.*=𝐋[l, l] end
      for l=𝑗+1:n Π.*=𝐋[l, l] end
      =#

      Π_=Π./𝐋[j, j]  # 𝐋[𝑗, 𝑖] here below picks from lower triangular part
      θ = -sum(@. 𝐋[𝑗, 𝑖]*𝐋[i, i]*Π_) / sum(@. 𝐋[i, i]^2 *Π_)
      ####θ = -sum(@. lᵢⱼ*lᵢᵢ*Π) / sum(lᵢᵢ² .*Π)
      θ² = θ^2
      ∡ += θ²
   end

   @inline function congedoSweep!()
      ∡ = T(0.)
      for i = 1:n
         # product of the diagonal elements excluding the ith one (for each k)
         fill!(Π, T(1))
         for l = 1:i-1 Π .*= 𝐋[l, l] end
         for l = i+1:n Π .*= 𝐋[l, l] end

         # transform all j≠i columns of B with respect to its ith column:
         for j = 1:i-1
            _gauss!(j, i, i, j) # find θ, θ² and update ∡
            _update1!(j, i, n, θ, θ², 𝐋, B) # update 𝐋 and B given θ and θ²
         end
         for j = i+1:n
            _gauss!(i, j, i, j) # find θ, θ² and update ∡
            _update2!(j, i, n, θ, θ², 𝐋, B) # update 𝐋 and B given θ and θ²
         end
      end
      return ∡ * e # convergence: average squared theta over all n(n-1) pairs
   end

   # declare variables
   T, n = eltype(𝐋[1, 1]), size(𝐋, 1)
   Π = Vector{T}(undef,  length(𝐋[1, 1])); Π_= similar(Π)
   θ, θ², ∡, e = T(0), T(0), T(0), inv(n*(n-1))
   B = Matrix{T}(I, n, n) # initialization of the AJD matrix

   iter, conv = _iterate!("GLogLike", congedoSweep!, maxiter, T, tol, verbose)
   return B, iter, conv
end

function gLogLike( 𝐂::Union{Vector{Hermitian}, Vector{Symmetric}};
               w        :: Twf   = ○,
               preWhite :: Bool  = false,
               sort     :: Bool  = true,
               init     :: Union{Matrix, Nothing} = ○,
               tol      :: Real  = 0.,
               maxiter  :: Int   = 1000,
               verbose  :: Bool  = false,
            eVar     :: TeVaro = ○,
            eVarMeth :: Function = searchsortedfirst)

   # pre-whiten or initialize or nothing
   W, 𝐆 = _preWhiteOrInit(𝐂, preWhite, Jeffrey, eVar, eVarMeth, init, :Hvector)

   T, n = eltype(𝐆[1]), size(𝐆[1], 1)

   # arrange data in a LowerTriangular matrix of k-vectors
   𝐋 = _arrangeData!(T, n, 𝐆)

   B, iter, conv = gLogLike(𝐋; tol=tol, maxiter=maxiter, verbose=verbose)

   # scale and permute the vectors of B
   D=Diagonal([mean(𝐋[i, i]) for i=1:n])
   λ = sort ? _permute!(_scale!(B, D, n)...) : diag(D)

   return preWhite ? (W.F*B, pinv(B)*W.iF, λ, iter, conv) :
                     (B, pinv(B), λ, iter, conv)
end


text:
const Size = Int

# manually-released RemoteRef alternative
struct MemToken
    where::Int
    key::Int
    size::Size # size in bytes
end

const MAX_MEMORY = Ref{Float64}((Sys.total_memory() / nprocs()) / 2) # half the process's share
const _mymem = Dict{Int,Tuple{Size, Any}}()
const freeable_lru = MemToken[]

let token_count = 0
    global next_token_id
    next_token_id() = (token_count+=1)
end

function approx_size(d)
    Base.summarysize(d) # note: this is accurate but expensive
end

function approx_size(d::Array{T}) where T
    if isbits(eltype(T))
        sizeof(d)
    else
        Base.summarysize(d)
    end
end

function approx_size(xs::AbstractArray{String})
    # doesn't check for redundant references, but
    # really super fast in comparison to summarysize
    sum(map(sizeof, xs))
end

function make_token(data)
    sz = approx_size(data)
    tok = MemToken(myid(), next_token_id(), sz)

    if !isempty(freeable_lru)
        # take this opportunity to purge cached data if necessary
        total_size = sum(map(first, values(_mymem))) + sz
        deleted = Int[]
        i = 1
        while total_size > MAX_MEMORY[] && i <= length(freeable_lru)
            # we need to weed out some old data here
            # if everything that can be has been cleaned up
            t = freeable_lru[i]
            push!(deleted, i)
            pop!(_mymem, t.key)
            total_size -= t.size
            @logmsg("cached & released $t - $(t.size)B dropped")
            i += 1
        end
        if !isempty(deleted)
            deleteat!(freeable_lru, deleted)
        end
    end

    _mymem[tok.key] = (sz,data)
    tok
end

function release_token(tok, keeparound=false)
    if tok.where == myid()
        if keeparound
            # move token to the lru cache
            # XXX: this doesn't check for duplicates
            # duplicates are removed in unrelease_token
            push!(freeable_lru, tok)
            @logmsg("soft-released $tok - $(tok.size)B freed")
        else
            # XXX: this doesn't check to see if it needs to be removed
            # from freeable_lru - however the data is released.
            pop!(_mymem, tok.key)
            @logmsg("removed $tok - $(tok.size)B freed")
        end
    else
        remotecall_fetch(()->release_token(tok, keeparound), tok.where)
    end
    nothing
end

function Base.fetch(t::MemToken)
    if t.where == myid()
        if haskey(_mymem, t.key)
            return Nullable{Any}(last(_mymem[t.key]))
        else
            return Nullable{Any}()
        end
    else
        remotecall_fetch(()->fetch(t), t.where)
    end
end

function unrelease_token(tok)
    # first we need to check if the token was removed due cache pruning

    if tok.where == myid()
        # set released to true, but don't remove it yet.
        if !haskey(_mymem, tok.key)
            return false
        end
        idx = find(x->x.key == tok.key, freeable_lru)
        deleteat!(freeable_lru, idx)
        true
    else
        remotecall_fetch(()->unrelease_token(tok), tok.where)
    end
end


text:
import TrajectoryOptimization
urdf_folder = joinpath(@__DIR__, "urdfs")
urdf_triplecartpole = joinpath(urdf_folder, "triplecartpole.urdf")

triplecartpole_model = TrajectoryOptimization.Model(urdf_triplecartpole, [1., 0., 0., 0.])
triplecartpole_model_full = TrajectoryOptimization.Model(urdf_triplecartpole, [1., 1., 1., 1.])