text:
using PairwiseListMatrices
using DataFrames
using NamedArrays

using Test
using DelimitedFiles
using LinearAlgebra
using Statistics

@testset "PairwiseListMatrices" begin

    list = [1,-2,3]
    mat_diag_triu =  [ 1 -2
                        0 3 ]
    mat_diag_tril =  [ 1 0
                       -2 3 ]
    mat_triu = [ 0 1 -2
                  0 0 3
                  0 0 0 ]
    mat_tril = [ 0 0 0
                  1 0 0
                 -2 3 0 ]
    mat_diag_sym = [ 1 -2
                     -2 3 ]
    mat_sym = [ 0 1 -2
                 1 0 3
                -2 3 0 ]

    plmd_triu = UpperTriangular(PairwiseListMatrix(list, true))
    plmd_tril = LowerTriangular(PairwiseListMatrix(list, true))
    plm_triu  = UpperTriangular(PairwiseListMatrix(list))
    plm_tril  = LowerTriangular(PairwiseListMatrix(list))
    plmd_sym  = PairwiseListMatrix(list, true)
    plm_sym   = PairwiseListMatrix(list)

    @testset "Creation from list" begin

        @test plmd_triu == mat_diag_triu
        @test plmd_tril == mat_diag_tril
        @test plm_triu  == mat_triu
        @test plm_tril  == mat_tril
        @test plmd_sym  == mat_diag_sym
        @test plm_sym   == mat_sym
    end

    @testset "Matrix" begin

        @test Matrix(plmd_triu) == mat_diag_triu
        @test Matrix(plmd_tril) == mat_diag_tril
        @test Matrix(plm_triu ) == mat_triu
        @test Matrix(plm_tril ) == mat_tril
        @test Matrix(plmd_sym ) == mat_diag_sym
        @test Matrix(plm_sym  ) == mat_sym
    end

    @testset "Getters" begin

        @test getlist(plmd_sym) == plmd_sym.list
        @test getlist(plm_sym)  ==  plm_sym.list

        @test getdiag(plmd_sym) == plmd_sym.diag
        @test getdiag(plm_sym)  ==  plm_sym.diag
    end

    @testset "Length" begin

        @test lengthlist(2, true)  == lengthlist(plmd_sym)
        @test lengthlist(3, false) == lengthlist(plm_sym)

        @test length(plmd_sym) == 4
        @test length(plm_sym) == 9
    end

    @testset "Linear indexing" begin

        for i in 1:length(plm_sym)
            @test plm_sym[i] == mat_sym[i]
            plm_sym[i] = 100
            @test plm_sym[i] == 100
            plm_sym[i] = mat_sym[i]
        end
        for i in 1:length(plmd_sym)
            @test plmd_sym[i] == mat_diag_sym[i]
            plmd_sym[i] = 100
            @test plmd_sym[i] == 100
            plmd_sym[i] = mat_diag_sym[i]
        end

    end

    @testset "Convert" begin

        @test mat_diag_sym ==
                convert(PairwiseListMatrix{Int, true, Vector{Int}},  mat_diag_sym)
        @test mat_diag_sym ==
                convert(PairwiseListMatrix{Int, false, Vector{Int}}, mat_diag_sym)

        @test convert(Matrix{Float64}, mat_sym) ==
                convert(PairwiseListMatrix{Float64, true, Vector{Float64}},  mat_sym)
        @test convert(Matrix{Float64}, mat_sym) ==
                convert(PairwiseListMatrix{Float64, false, Vector{Float64}}, mat_sym)

        @test convert(Matrix{Int}, plm_sym)  == mat_sym
        @test convert(Matrix{Int}, plmd_sym) == mat_diag_sym

        @test convert(PairwiseListMatrix{Int, false, Vector{Int}}, plmd_sym) == plmd_sym
        @test convert(PairwiseListMatrix{Int, true, Vector{Int}}, plm_sym)   == plm_sym
    end

    @testset "Unary operations" begin

        for f in [ -, abs, x -> sqrt(abs(x))]
            @test broadcast(f, plmd_triu) == broadcast(f, mat_diag_triu)
            @test broadcast(f, plmd_tril) == broadcast(f, mat_diag_tril)
            @test broadcast(f, plm_triu ) == broadcast(f, mat_triu)
            @test broadcast(f, plm_tril ) == broadcast(f, mat_tril)
            @test broadcast(f, plmd_sym ) == broadcast(f, mat_diag_sym)
            @test broadcast(f, plm_sym  ) == broadcast(f, mat_sym)
        end
    end

    @testset "Binary operations" begin

        for f in [ -, +, * ]
            @test broadcast(f, plmd_triu, plmd_triu) == broadcast(f, mat_diag_triu, mat_diag_triu)
            @test broadcast(f, plmd_tril, plmd_tril) == broadcast(f, mat_diag_tril, mat_diag_tril)
            @test broadcast(f, plm_triu , plm_triu ) == broadcast(f, mat_triu,      mat_triu     )
            @test broadcast(f, plm_tril , plm_tril ) == broadcast(f, mat_tril,      mat_tril     )
            @test broadcast(f, plmd_sym , plmd_sym ) == broadcast(f, mat_diag_sym,  mat_diag_sym )
            @test broadcast(f, plm_sym  , plm_sym  ) == broadcast(f, mat_sym,       mat_sym      )
        end

        @test plmd_sym ./ plmd_sym == mat_diag_sym ./ mat_diag_sym

        # https://github.com/JuliaLang/julia/issues/19615
        for f in [ *, / ]
            @test broadcast(f, plmd_triu, 2) == broadcast(f, mat_diag_triu, 2)
            @test broadcast(f, plmd_tril, 2) == broadcast(f, mat_diag_tril, 2)
            @test broadcast(f, plm_triu , 2) == broadcast(f, mat_triu,      2)
            @test broadcast(f, plm_tril , 2) == broadcast(f, mat_tril,      2)
        end

        for f in [ -, +, *, / ]
            @test broadcast(f, plmd_sym , 2) == broadcast(f, mat_diag_sym,  2)
            @test broadcast(f, plm_sym  , 2) == broadcast(f, mat_sym,       2)
        end

        @testset "./ with Float64 & Int" begin

            plm_t = PairwiseListMatrix([.5, .4, .3], true)
            plm_f = PairwiseListMatrix([1., 1., 1.], false)
            fill!(plm_f, 1.0)
            mat_t = Matrix(plm_t)
            mat_f = Matrix(plm_f)

            for value in  (4, 4.0)
                result =  plm_t ./ value
                @test isa(result, PairwiseListMatrix{Float64,true,Vector{Float64}})
                @test result == (mat_t ./ value)

                result =  value ./ plm_t
                @test isa(result, PairwiseListMatrix{Float64,true,Vector{Float64}})
                @test result == (value ./ mat_t)

                result =  plm_f ./ value
                @test isa(result, PairwiseListMatrix{Float64,false,Vector{Float64}})
                @test result == (mat_f ./ value)

                result =  value ./ plm_f
                @test isa(result, PairwiseListMatrix{Float64,false,Vector{Float64}})
                @test result == (value ./ mat_f)
            end
        end

        # plm = PairwiseListMatrix(rand(500500), true)
        # @btime broadcast(Base.:/::typeof(/), $plm, 10.0);
        # @btime broadcast(Base.:/::typeof(/), $plm.list, 10.0);
        # @btime 0.5 .* $plm ./ 10.0;
        # @btime 0.5 .* $plm.list ./ 10.0;

    end

    @testset "Inplace Broadcast" begin

        for (p,m) in [(plmd_sym, mat_diag_sym), (plm_sym, mat_sym)]
            cp = deepcopy(p)
            cm = deepcopy(m)
            cp .+= 1.0
            cm .+= 1.0
            @test cp == cm
            cp .+= cp
            cm .+= cm
            @test cp == cm
        end
    end

    @testset "Transpose" begin

        @test transpose(plmd_triu) == plmd_tril == transpose(mat_diag_triu)
        @test transpose(plmd_tril) == plmd_triu == transpose(mat_diag_tril)
        @test transpose(plm_triu) == plm_tril == transpose(mat_triu)
        @test transpose(plm_tril) == plm_triu == transpose(mat_tril)

        @test adjoint(plmd_triu) == plmd_tril == adjoint(mat_diag_triu)
        @test adjoint(plmd_tril) == plmd_triu == adjoint(mat_diag_tril)
        @test adjoint(plm_triu) == plm_tril == adjoint(mat_triu)
        @test adjoint(plm_tril) == plm_triu == adjoint(mat_tril)

        @test transpose(plmd_sym) == plmd_sym == transpose(mat_diag_sym)
        @test transpose(plm_sym) == plm_sym == transpose(mat_sym)

        @test adjoint(plmd_sym) == plmd_sym == adjoint(mat_diag_sym)
        @test adjoint(plm_sym) == plm_sym == adjoint(mat_sym)

        @test transpose!(plm_sym) == plm_sym
        @test adjoint!(plm_sym) == plm_sym
    end

    @testset "Linear algebra" begin

        @test plmd_triu * plmd_triu == mat_diag_triu * mat_diag_triu
        @test plmd_tril * plmd_tril == mat_diag_tril * mat_diag_tril
        @test plm_triu  * plm_triu  == mat_triu      * mat_triu
        @test plm_tril  * plm_tril  == mat_tril      * mat_tril
        @test plmd_sym  * plmd_sym  == mat_diag_sym  * mat_diag_sym
        @test plm_sym   * plm_sym   == mat_sym       * mat_sym
        @test Symmetric(plm_sym) * Symmetric(plm_sym) == Symmetric(mat_sym) * Symmetric(mat_sym)

        @test plmd_triu / plmd_triu == mat_diag_triu / mat_diag_triu
        @test plmd_tril / plmd_tril == mat_diag_tril / mat_diag_tril
        @test plmd_sym  / plmd_sym  == mat_diag_sym  / mat_diag_sym
        @test plm_sym   / plm_sym   == mat_sym       / mat_sym

        for p in [:U, :S, :Vt]
            @test getproperty(svd(plmd_triu), p) ≈ getproperty(svd(mat_diag_triu), p)
            @test getproperty(svd(plmd_tril), p) ≈ getproperty(svd(mat_diag_tril), p)
            @test getproperty(svd(plm_triu ), p) ≈ getproperty(svd(mat_triu), p)
            @test getproperty(svd(plm_tril ), p) ≈ getproperty(svd(mat_tril), p)
            @test getproperty(svd(plmd_sym ), p) ≈ getproperty(svd(mat_diag_sym), p)
            @test getproperty(svd(plm_sym  ), p) ≈ getproperty(svd(mat_sym), p)
        end
    end

    @testset "Stats" begin

        for f in [ mean, std ]
            @test f(plmd_triu) == f(mat_diag_triu)
            @test f(plmd_tril) == f(mat_diag_tril)
            @test f(plm_triu ) == f(mat_triu)
            @test f(plm_tril ) == f(mat_tril)
            @test f(plmd_sym ) == f(mat_diag_sym)
            @test f(plm_sym  ) == f(mat_sym)

            @test f(plmd_sym, dims=1) == f(mat_diag_sym, dims=1)
            @test f(plm_sym , dims=1) == f(mat_sym, dims=1)
            @test f(plmd_sym, dims=2) == f(mat_diag_sym, dims=2)
            @test f(plm_sym , dims=2) == f(mat_sym, dims=2)
        end

        @test cor(plmd_sym) ≈ cor(mat_diag_sym)
        @test cor(plm_sym ) ≈ cor(mat_sym)

        # broadcast used in cor(...)
        @test plmd_sym .- mean(plmd_sym,dims=1) == mat_diag_sym .- mean(mat_diag_sym,dims=1)
        @test plmd_sym .- mean(plmd_sym,dims=2) == mat_diag_sym .- mean(mat_diag_sym,dims=2)
    end

    @testset "triu" begin

        @test triu(plmd_sym ) == triu(mat_diag_sym)
        @test triu(plm_sym  ) == triu(mat_sym)

        @test triu(plmd_sym,1) == triu(mat_diag_sym, 1)
        @test triu(plm_sym, 1) == triu(mat_sym, 1)

        @test_throws ErrorException triu!(plm_sym)
        @test_throws ErrorException triu!(plm_sym, 1)
    end

    # + and - definitions are ambiguous with:
    # +(A::AbstractArray{Bool,N<:Any}, x::Bool)
    #
    # @testset "Boolean binary op" begin
    #
    #     list = PairwiseListMatrix([true, true, true])
    #
    #     @test list - true == [ -1  0  0
    #                                  0 -1  0
    #                                  0  0 -1 ]
    #
    #     @test list + true == [ 1  2  2
    #                                 2  1  2
    #                                 2  2  1 ]
    # end

    @testset "Mean without diagonal" begin

        diag_false = PairwiseListMatrix([10,20,30], false)
        diag_true  = PairwiseListMatrix([0,10,20,0,30,0], true)

        for list in [diag_false, diag_true]
            @test vec( mean_nodiag(list, dims=1) ) == [15., 20., 25.]
            @test vec( mean_nodiag(list, dims=2) ) == [15., 20., 25.]
            @test mean_nodiag(list) == 20.
        end
    end
end

@testset "Named PairwiseListMatrix" begin

    list =  [1,2,3]
    labels= ["a", "b", "c"]
    labels_diag = ["A", "B"]

    plm_diag = PairwiseListMatrix(list, true)
    plm = PairwiseListMatrix(list)
    nplm_diag = setlabels(plm_diag, labels_diag)
    nplm = setlabels(plm, labels)

    @testset "set and get labels" begin

        @test isa(nplm_diag, NamedArray)
        @test isa(nplm, NamedArray)
        @test isa(nplm_diag.array, PairwiseListMatrix)
        @test isa(nplm.array, PairwiseListMatrix)

        @test_throws AssertionError setlabels!(nplm_diag, labels)
        @test_throws AssertionError setlabels!(nplm, labels_diag)

        @test getlabels(nplm_diag) == labels_diag
        @test getlabels(nplm) == labels

        setlabels!(nplm, ["A","B","C"])
        @test getlabels(nplm) == ["A","B","C"]
        setlabels!(nplm, ["a","b","c"])
    end

    @testset "get/setindex" begin

        for i in 1:2
            for j in 2:3
                @test nplm[labels[i], labels[j]] == nplm[labels[j], labels[i]]
            end
        end

        @test nplm_diag["A", "B"] == nplm_diag["B", "A"]

        nplm_diag["A", "B"] = 20
        @test nplm_diag["B", "A"] == 20
        nplm_diag["B", "A"] = 2
        @test nplm_diag["A", "B"] == 2
    end

    @testset "join" begin

        left = setlabels(PairwiseListMatrix(collect(1.:15.), true),
            String["a","b","c","d","e"])
        right = setlabels(PairwiseListMatrix(collect(1.:10.), false),
            String["a","e","i","o","u"])

        a, b = join(left, right)
        @test (a, b) == join(left, right, kind=:inner)
        @test getlabels(a) == getlabels(b)
        @test getlabels(a) == ["a", "e"]
        @test size(a) == (2,2)
        @test size(b) == (2,2)

        a, b = join(left, right, kind=:left)
        @test getlabels(a) == getlabels(b)
        @test getlabels(a) == getlabels(left)
        @test a == left
        @test b != right

        a, b = join(left, right, kind=:right)
        @test getlabels(a) == getlabels(b)
        @test getlabels(b) == getlabels(right)
        @test a != left
        @test b == right

        a, b = join(left, right, kind=:outer)
        @test getlabels(a) == getlabels(b)
        @test getlabels(a) == ["a", "b", "c", "d", "e", "i", "o", "u"]
        @test size(a) == (8,8)
        @test size(b) == (8,8)
        @test a != left
        @test b != right
    end

    @testset "Named and not named arrays" begin

        @testset "Symmetric" begin

            @test issymmetric(plm)
            @test issymmetric(nplm)
            @test issymmetric(plm_diag)
            @test issymmetric(nplm_diag)
        end

        @testset "Diagonal" begin

            @test hasdiagonal(plm_diag)
            @test hasdiagonal(nplm_diag)
            @test !hasdiagonal(plm)
            @test !hasdiagonal(nplm)

            @test diagonal(plm_diag) == [1,3]
            @test diagonal(nplm_diag) == [1,3]
            @test diagonal(plm) == [0,0,0]
            @test diagonal(nplm) == [0,0,0]
        end

        @testset "eltype" begin

            @test eltype(plm) == Int
            @test eltype(nplm) == Int
            @test eltype(plm_diag) == Int
            @test eltype(nplm_diag) == Int
        end

        @testset "delegated functions" begin

            for F in (getlist, getdiag, Matrix, lengthlist, sum_nodiag, mean_nodiag, diagonal)
                    @test F(nplm) == F(plm)
                    @test F(nplm_diag) == F(plm_diag)
            end
        end

        @testset "map and broadcast" begin

            mat = Matrix(nplm)
            mat_diag = Matrix(nplm_diag)

            @test map(sqrt,plm) == map(sqrt,mat)
            @test map(sqrt,nplm) == map(sqrt,mat)
            @test map(sqrt,plm_diag) == map(sqrt,mat_diag)
            @test map(sqrt,nplm_diag) == map(sqrt,mat_diag)

            @test broadcast(sqrt,plm) == broadcast(sqrt,mat)
            @test broadcast(sqrt,nplm) == broadcast(sqrt,mat)
            @test broadcast(sqrt,plm_diag) == broadcast(sqrt,mat_diag)
            @test broadcast(sqrt,nplm_diag) == broadcast(sqrt,mat_diag)
        end
    end
end

@testset "Vector{PairwiseListMatrix}" begin

    list_samples = [ PairwiseListMatrix(rand(10), true) for i in 1:100 ]
    list_diag_samples = [ PairwiseListMatrix(rand(6), false) for i in 1:100 ]
    full_samples = Matrix{Float64}[ Matrix(mat) for mat in list_samples ]
    full_diag_samples = Matrix{Float64}[ Matrix(mat) for mat in list_diag_samples ]

    for f in [sum, mean]
        @test f(list_samples) == f(full_samples)
        @test f(list_diag_samples) == f(full_diag_samples)
    end

    @test sum(list_samples[1:1]) == list_samples[1]
    @test sum(list_diag_samples[1:1]) == list_diag_samples[1]

    @test_throws ErrorException sum(PairwiseListMatrix{Int, true, Array{Int,1}}[])
    @test_throws ErrorException sum(PairwiseListMatrix{Int, true, Array{Int,1}}[
                                        PairwiseListMatrix([1, 1, 1], true),
                                        PairwiseListMatrix([1, 1, 1, 1, 1, 1], true) ])
    @test_throws ErrorException std(PairwiseListMatrix{Int, true, Array{Int,1}}[
                                        PairwiseListMatrix([1, 1, 1], true) ])

    full_samples = reshape(hcat(full_samples...), (4,4,100))
    full_diag_samples = reshape(hcat(full_diag_samples...), (4,4,100))
    # @test std(list_samples) ≈ std(full_samples,3)[:,:,1] # It uses n - 1
    @test std(list_samples) ≈ sqrt.(mean(abs2.(full_samples .- mean(full_samples, dims=3)), dims=3))
    # @test std(list_diag_samples) ≈ std(full_diag_samples,3)[:,:,1] # It uses n - 1
    @test std(list_diag_samples) ≈ sqrt.(mean(abs2.(full_diag_samples .- mean(full_diag_samples, dims=3)), dims=3))

    @testset "Z score" begin

        list = PairwiseListMatrix{Float64, false, Vector{Float64}}[
            PairwiseListMatrix([1., 1., 1.]),
            PairwiseListMatrix([3., 3., 3.]) ]
        mat = PairwiseListMatrix([2., 2., 2.])

        @test std(list) == PairwiseListMatrix([1., 1., 1.])
        @test mean(list) == mat
        @test PairwiseListMatrices.zscore(list, mat) == PairwiseListMatrix([0., 0., 0.])

        @test_throws MethodError PairwiseListMatrices.zscore(list, PairwiseListMatrix([2.,2.,2.], true))
        @test_throws ErrorException PairwiseListMatrices.zscore(list, PairwiseListMatrix([2.,2.,2.,2.,2.,2.]))

        list = PairwiseListMatrix{Float64, true, Vector{Float64}}[
            PairwiseListMatrix([1., 1., 1.], true),
            PairwiseListMatrix([3., 3., 3.], true) ]
        mat = PairwiseListMatrix([2., 2., 2.], true)

        @test std(list) == PairwiseListMatrix([1., 1., 1.], true)
        @test mean(list) == mat
        @test PairwiseListMatrices.zscore(list, mat) == PairwiseListMatrix([0., 0., 0.], true)

        @test_throws MethodError PairwiseListMatrices.zscore(list,PairwiseListMatrix([2.], false))
        @test_throws ErrorException PairwiseListMatrices.zscore(list,PairwiseListMatrix([2.,2.,2.,2.,2.,2.],true))
    end
end

@testset "Apply" begin

    PLM = PairwiseListMatrix([1,2,3], false)

    apply2list(PLM) do list, k
        Main.@test(list[k] == k)
    end

    list_values = [1,2,3,4,5,6]
    PLMtrue  = PairwiseListMatrix(list_values, true)
    PLMfalse = PairwiseListMatrix(list_values, false)
    full_t   = Matrix(PLMtrue)
    full_f   = Matrix(PLMfalse)

    apply2list(PLMtrue) do list, k
        @test(list[k] == list_values[k])
    end
    apply2list(PLMfalse) do list, k
        @test(list[k] == list_values[k])
    end

    @iteratelist PLMtrue  Main.@test(list[k] == list_values[k])
    @iteratelist PLMfalse Main.@test(list[k] == list_values[k])

    apply2diag(PLMfalse) do diag, k
        @test(diag[k] == 0)
    end

    apply2upper(PLMtrue, use_diag=true) do list, k, i, j
        list[k] = list_values[k]
    end
    apply2upper(PLMfalse, use_diag=false) do list, k, i, j
        list[k] = list_values[k]
    end

    apply2upper(PLMtrue, use_diag=true) do list, k, i, j
        list[k] = full_t[i, j]
    end
    apply2upper(PLMtrue, use_diag=false) do list, k, i, j
        list[k] = full_t[i, j]
    end

    apply2upper(PLMfalse, use_diag=true) do list, k, i, j
        list[k] = full_f[i, j]
    end
    apply2upper(PLMfalse, use_diag=false) do list, k, i, j
        list[k] = full_f[i, j]
    end

    @iterateupper PLMtrue  true  list[k] = list_values[k]
    @iterateupper PLMfalse false list[k] = list_values[k]

    @iterateupper PLMtrue  true  list[k] = full_t[i,j]
    @iterateupper PLMtrue  false list[k] = full_t[i,j]

    @iterateupper PLMtrue  true  list[k] = full_f[i,j]
    @iterateupper PLMfalse false list[k] = full_f[i,j]
end

@testset "IO" begin

    @testset "to/from table" begin

        table = [ "A" "B" 10
                  "A" "C" 20
                  "B" "C" 30 ]

        list = setlabels(PairwiseListMatrix([10,20,30]), ["A", "B", "C"])
        list_diag = setlabels(PairwiseListMatrix([0,10,20,0,30,0], true), ["A", "B", "C"])

        @test to_table(list, diagonal=false) == table
        @test from_table(table, false, diagonalvalue=0) == list

        @test to_table(list) == [ "A" "A" 0
                                  "A" "B" 10
                                  "A" "C" 20
                                  "B" "B" 0
                                  "B" "C" 30
                                  "C" "C" 0]

        @test to_table(list) == to_table(list_diag)
        @test to_table(list, diagonal=false) == to_table(list_diag, diagonal=false)

        @testset "README" begin
            # using Any type instead of Int to allow @test
            plm  = PairwiseListMatrix(Any[1,2,3], false)
            nplm  = setlabels(plm, ["a","b","c"])
            table = to_table(nplm)
            new_nplm = from_table(table, true)
            @test new_nplm == plm
        end
    end

    @testset "DataFrames" begin

        values   = [1,2,3,4,5,6]
        PLMtrue  = PairwiseListMatrix(values, true)
        PLMfalse = PairwiseListMatrix(values, false)

        df = DataFrame(to_dict(PLMtrue))
        @test PLMtrue == from_table(df, true)

        df = DataFrame(to_dict(PLMtrue, diagonal=false))
        @test triu(PLMtrue,1) == triu(from_table(df, false).array,1)

        df = DataFrame(to_dict(PLMfalse))
        @test PLMfalse == from_table(df, true)

        df = DataFrame(to_dict(PLMfalse, diagonal=false))
        @test PLMfalse == from_table(df, false)
    end

    @testset "Write" begin

        values   = [1,2,3,4,5,6]
        PLMtrue  = PairwiseListMatrix(values, true)
        PLMfalse = PairwiseListMatrix(values, false)
        filename = joinpath(tempdir(), "pairwiselistmatrices_test.temp")

        try
          writedlm(filename, PLMtrue, diagonal=true)
          @test map(string,readdlm(filename,Int)) == map(string,to_table(PLMtrue,diagonal=true))
        finally
          rm(filename)
        end

        try
          writedlm(filename,PLMtrue,diagonal=false)
          @test map(string,readdlm(filename,Int)) == map(string,to_table(PLMtrue,diagonal=false))
        finally
          rm(filename)
        end

        try
          writedlm(filename,PLMfalse,diagonal=true)
          @test map(string,readdlm(filename,Int)) == map(string,to_table(PLMfalse,diagonal=true))
        finally
          rm(filename)
        end

        try
          writedlm(filename,PLMfalse,diagonal=false)
          @test map(string,readdlm(filename,Int)) == map(string,to_table(PLMfalse,diagonal=false))
        finally
          rm(filename)
        end

    end
end


text:
include("PRM.jl")
#using Distributions

#### HELPER FXN

    function areaRect(r::HyperRectangle)
        w, h = widths(r)
        area = w*h
    end

    function unionAreaRects(r1::HyperRectangle, r2::HyperRectangle)
        area1 = areaRect(r1)
        area2 = areaRect(r2)
        overlap = areaRect(intersect(r1, r2))
        area = area1 + area2 - overlap
        return area
    end
### END

####################################
## PARAMETERS
####################################
numSamples = 40
connectRadius = 2
param = algT.AlgParameters(numSamples, connectRadius)

targetNumObs = 3
clutterPercentage = 0.15
roomWidth,roomHeight  = 20,20

startstate = Point(4.,4)
goalstate = Point(15.,15)

####################################
## INIT
####################################
obstacles = Vector{HyperRectangle}()
perimeter = HyperRectangle(Vec(0.,0), Vec(roomWidth,roomHeight))
wallsPerimeter = algfxn.decompRect(perimeter)

walls = Vector{LineSegment}()
for l in wallsPerimeter
    push!(walls, l)
end

####################################
# Randomly Generate Clutter (obstacles)
####################################

# implement function to create clutter
# Let's say on average we want to create 4 obstacles... (otherwise, *most* of
# the time we will product one large rectangle that is falling out of the room

#targetNumObs = 2

roomArea = roomWidth * roomHeight
targetSumObsArea= roomArea * clutterPercentage
sumObsArea = 0

while sumObsArea < targetSumObsArea
    #x,y = rand(Uniform(1, roomWidth),2)
    x,y = rand(1.0:roomWidth,2)

    randWidth, randHeight = rand(Uniform(1, roomWidth/targetNumObs),2)
    #randWidth, randHeight = rand(1:roomWidth/targetNumObs,2)
    protoObstacle = HyperRectangle(Vec(x, y), Vec(randWidth, randHeight)) #Todo
    if contains(perimeter, protoObstacle)
        push!(obstacles, protoObstacle)
        sumObsArea += randWidth*randHeight
    end
end
print("obstacles generated")

#plot!(walls, color =:black)
#plot!(obstacles, fillalpha=0.5)

@show targetSumObsArea
@show sumObsArea

####################################
# Run PRM once and display config space plot
####################################
plot()

r = algT.Room(roomWidth,roomHeight,walls,obstacles)
roomPlot = plotfxn.plotRoom(r)

## Run preprocessing
nodeslist, edgeslist = preprocessPRM(r, param)
print("\n ---- pre-processed --- \n")

## Query created RM

pathcost, isPathFound, solPath = queryPRM(startstate, goalstate, nodeslist, edgeslist, obstacles)
print("\n ---- queried ---- \n")

## Plot path found
timestamp = Base.Dates.now()
title = "PRM with # samples=$numSamples, maxDist=$connectRadius, \npathcost = $pathcost,
        timestamp=$timestamp)\n\n"
roadmap = algT.roadmap(startstate, goalstate, nodeslist, edgeslist)

fooplot = plotfxn.plotPRM(roomPlot , roadmap, solPath, title::String)
gui(fooplot)

print("\n --- Time --- \n")
@show timestamp
#print("\n --- Obstacles --- \n")
#@show obstacles
#print("\n --- Nodes --- \n")
#@show nodeslist
#print("\n --- Edges --- \n")
#@show edgeslist
print("\n -------- \n")

# todo: save one "representative" figure from each run

####################################
# Run multiple trials and scatterplot all cost vs area for all runs
# due to varying absolute area
####################################

####################################
## Parrameters
####################################
nTrials = 30
nSamples_list = [10 20 30 80 150 200 300]

####################################
## Init
####################################
cost = 0
listCosts = Vector{Float32}()
listpSucc= Vector{Float32}()

####################################
# Run Trials
####################################

#@show listpSucc

tic()
for nSamples in nSamples_list
    totalcost = 0.
    idx = 0.
    nSuccess = 0.
    pathcost = 0.
    param = algT.AlgParameters(nSamples, connectRadius)
    while idx < nTrials
        nodeslist, edgeslist = preprocessPRM(r, param)
        pathcost, isPathFound, solPath = queryPRM(startstate, goalstate, nodeslist, edgeslist, obstacles)
        if isPathFound
            nSuccess += 1
        end
        if !(pathcost == Void)
            totalcost += pathcost
        end
        idx += 1
    end

    avgCost = totalcost /  nSuccess
    pSucc = nSuccess / nTrials
    #@printf("For the iter of %d the avg cost was %d across %d trials", maxIter, avgCost, nTrials)
    push!(listCosts, avgCost)
    push!(listpSucc, pSucc)
end

timeExperiment = toc();
print("Time to run experiment: $timeExperiment\n")
timestamp = Base.Dates.now()

####################################
# Plot Results
####################################
# clutter (area) on X
# pathcost on Y
sizeplot = (800, 800)

supTitle="\nPRM with maxDist=$connectRadius, nTrials=$nTrials.
        (time to run:$timeExperiment, timestamp=$timestamp)\n\n"
costTitle= "numsamples vs pathcost\n"

pPRMcost = scatter(nSamples_list, listCosts',
    color=:black,
    title = supTitle * costTitle, ylabel = "euclidean path cost", xlabel = "numsamples",
    yaxis=((0,100), 0:20:100))

successTitle = "\nnumsamples vs pSuccess\n"
pPRMsuccess = scatter(nSamples_list, listpSucc',
    color = :orange, markersize= 6,
    title = successTitle, ylabel = ("P(success)=numSucc/$nTrials trials"), xlabel = "numsamples",
    yaxis=((0, 1.2), 0:0.1:1))

plot(pPRMcost, pPRMsuccess, layout=(2,1), legend=false,
    xaxis=((0, 320), 0:50:300),
    size = sizeplot)

ext: jl


text:
# # Basic linear algebra in Julia
# Author: Andreas Noack Jensen (MIT) (http://www.econ.ku.dk/phdstudent/noack/)
# (with edits from Jane Herriman)

#-

# First let's define a random matrix

A = rand(1:4,3,3)

# Define a vector of ones

x = fill(1.0, (3,)) # = fill(1.0, 3)

# Notice that $A$ has type Array{Int64,2} but $x$ has type Array{Float64,1}. Julia defines the aliases Vector{Type}=Array{Type,1} and Matrix{Type}=Array{Type,2}.
#
# Many of the basic operations are the same as in other languages
# #### Multiplication

b = A*x

# #### Transposition
# As in other languages `A'` is the conjugate transpose, or adjoint

A'

# and we can get the transpose with

transpose(A)

# #### Transposed multiplication
# Julia allows us to write this without *

A'A

# #### Solving linear systems
# The problem $Ax=b$ for ***square*** $A$ is solved by the \ function.

A\b

# `A\b` gives us the *least squares solution* if we have an overdetermined linear system (a "tall" matrix)

Atall = rand(3, 2)

#-

Atall\b

# and the *minimum norm least squares solution* if we have a rank-deficient least squares problem

v = rand(3)
rankdef = hcat(v, v)

#-

rankdef\b

# Julia also gives us the minimum norm solution when we have an underdetermined solution (a "short" matrix)

bshort = rand(2)
Ashort = rand(2, 3)

#-

Ashort\bshort

# # The LinearAlgebra library
#
# While much of linear algebra is available in Julia by default (as shown above), there's a standard library named `LinearAlgebra` that brings in many more relevant names and functions. In particular, it provides factorizations and some structured matrix types.  As with all packages, you can bring these additional features into your session with a `using LinearAlgebra`.

#-

# ### Exercises
#
# #### 10.1
# Take the inner product (or "dot" product) of a vector `v` with itself and assign it to variable `dot_v`.

v = [1,2,3]

#-

@assert dot_v == 14

# #### 10.2
# Take the outer product of a vector v with itself and assign it to variable `cross_v`

@assert cross_v == [0, 0, 0]

max_stars_repo_name: fercarozzi/JuliaAcademyMaterials

max_issues_repo_name: fercarozzi/JuliaAcademyMaterials

max_forks_repo_name: fercarozzi/JuliaAcademyMaterials


text:
const julia_version = v"1.7.0"

include("../common.jl")

max_stars_repo_name: singularitti/Yggdrasil

max_issues_repo_name: singularitti/Yggdrasil

max_forks_repo_name: singularitti/Yggdrasil


text:
@enum TagFamilies tag36h11 tag25h9 tag16h5

struct AprilTag
    family::String
    id::Int
    hamming::Int
    # goodness::Float32
    decision_margin::Float32
    H::Matrix{Float64}
    c::Vector{Float64}
    p::Vector{Vector{Float64}}
end

mutable struct AprilTagDetector
    #pointers to c managed memmory
    td::Ptr{apriltag_detector_t}
    tf::Ptr{apriltag_family_t}

end

# Base.propertynames(x::AprilTagDetector, private::Bool=false) =
#     (:nThreads, :quad_decimate, :quad_sigma, :refine_edges, :refine_decode, :refine_pose,
#         (private ? fieldnames(typeof(x)) : ())...)

Base.propertynames(x::AprilTagDetector, private::Bool=false) =
    (:nThreads, :quad_decimate, :quad_sigma, :refine_edges, :decode_sharpening,
        (private ? fieldnames(typeof(x)) : ())...)

Base.getproperty(x::AprilTagDetector,f::Symbol) = begin
    if f == :nThreads
        getnThreads(x)
    elseif f == :quad_decimate
        getquad_decimate(x)
    elseif f == :quad_sigma
        getquad_sigma(x)
    elseif f == :refine_edges
        getrefine_edges(x)
    elseif f == :decode_sharpening
        getdecode_sharpening(x)
    # elseif f == :refine_decode
    #     getrefine_decode(x)
    # elseif f == :refine_pose
    #     getrefine_pose(x)
    else
        getfield(x,f)
    end
end

Base.setproperty!(x::AprilTagDetector,f::Symbol, v) = begin
    if f == :nThreads
        setnThreads(x,v)
    elseif f == :quad_decimate
        setquad_decimate(x,v)
    elseif f == :quad_sigma
        setquad_sigma(x,v)
    elseif f == :refine_edges
        setrefine_edges(x,v)
    elseif f == :decode_sharpening
        setdecode_sharpening(x,v)
    # elseif f == :refine_decode
    #     setrefine_decode(x,v)
    # elseif f == :refine_pose
    #     setrefine_pose(x,v)
    else
        # Base.setfield!(x,f,v)
        Base.setfield!(x, f, convert(fieldtype(typeof(x), f), v))
    end
end

function Base.show(io::IO, mime::MIME{Symbol("text/plain")}, F::AprilTagDetector)
    println(io, summary(F))
    println(io, "nThreads: ", F.nThreads)
    println(io, "quad_decimate: ", F.quad_decimate)
    println(io, "quad_sigma: ", F.quad_sigma)
    println(io, "refine_edges: ", F.refine_edges)
    println(io, "decode_sharpening: ", F.decode_sharpening)
    # println(io, "refine_decode: ", F.refine_decode)
    # println(io, "refine_pose: ", F.refine_pose)
end

"""
	AprilTagDetector(tagfamily=tag36h11)
Create a default AprilTag detector with the 36h11 tag family
Create an AprilTag detector with tag family in `tagfamily::TagFamilies
@enum TagFamilies tag36h11 tag25h9 tag16h5`
"""
function AprilTagDetector(tagfamily::TagFamilies = tag36h11)
    #create tag detector
    td = apriltag_detector_create()
    #create tag family
    if tagfamily == tag36h11
        tf = tag36h11_create()
    # elseif tagfamily == tag36h10
    #     tf = tag36h10_create()
    elseif tagfamily == tag25h9
        tf = tag25h9_create()
    elseif tagfamily == tag16h5
        tf = tag16h5_create()
    end

    #add family to detector
    apriltag_detector_add_family(td, tf)

    #Register finalizer and return detector
    return finalizer(d->freeDetector!(d, false), AprilTagDetector(td,tf))
end

const U8Types = Union{UInt8, N0f8, Gray{N0f8}}
"""
	AprilTagDetector(img)
Run the april tag detector on a image
"""
function (detector::AprilTagDetector)(image::Array{T, 2}) where T <: U8Types

    if detector.td == C_NULL
        error("AprilTags Detector does not exist")
    end

    if detector.tf == C_NULL
        error("AprilTags family does not exist")
    end
    #create image8 object for april tags
    image8 = convert(AprilTags.image_u8_t, image)

    # run detector on image
    detections =  apriltag_detector_detect(detector.td, image8)

    # copy and return detections julia struct
    tags = AprilTags.copyAprilTagDetections(detections)
    # copy and return detections c struct
    # tags = getTagDetections(detections)

    #distroy detections memmory
    apriltag_detections_destroy(detections)

    return tags

end

function (detector::AprilTagDetector)(image::Array{ColorTypes.RGB{T}, 2}) where T
    # Converting to greyscale
    image = Gray.(image)
    # Call internal
    return detector(image)
end

"""
	threadcalldetect(detector, image)
Run the april tag detector on a image
"""
function threadcalldetect(detector::AprilTagDetector, image::Array{T, 2}) where T <: U8Types

    if detector.td == C_NULL
        error("AprilTags Detector does not exist")
    end

    if detector.tf == C_NULL
        error("AprilTags family does not exist")
    end
    #create image8 object for april tags
    image8 = convert(AprilTags.image_u8_t, image)

    # run detector on image
    detections =  threadcall_apriltag_detector_detect(detector.td, image8)

    # copy and return detections julia struct
    tags = AprilTags.copyAprilTagDetections(detections)
    # copy and return detections c struct
    # tags = getTagDetections(detections)

    #distroy detections memmory
    apriltag_detections_destroy(detections)

    return tags

end

"""
	freeDetector!(apriltagdetector)
Free the allocated memmory
"""
function freeDetector!(detector::AprilTagDetector, verbose::Bool=true)

    if detector.td == C_NULL
        verbose && @warn("AprilTags Detector does not exist")
    else
        apriltag_detector_destroy(detector.td)
    end

    if detector.tf == C_NULL
        verbose && @warn("AprilTags family does not exist")
    else
        # use this one for now, c code is similar
        tag36h11_destroy(detector.tf)
    end

    #TODO: how do I destroy the detector itself, for now just nulls
    # eg. somethin like detector = nothing but modif_height input
    detector.td = C_NULL
    detector.tf = C_NULL
    return nothing
end

# # TODO overload convert
# function convert2image_u8(image)::image_u8_t
# #create image8 opject for april tags
#     (rows,cols) = size(image)
#     imbuf = reinterpret(UInt8, image'[:])
#     return AprilTags.image_u8_t(Int32(cols), Int32(rows), Int32(cols), Base.unsafe_convert(Ptr{UInt8}, imbuf))
# end
function convert(::Type{image_u8_t}, image::Array{UInt8, 2})
#create image8 opject for april tags
    (rows,cols) = size(image)
    imbuf = image'[:]
    return AprilTags.image_u8_t(Int32(cols), Int32(rows), Int32(cols), Base.unsafe_convert(Ptr{UInt8}, imbuf))
end
# TODO maybe add for AbstractArray types such as ReinterpretArray
# function convert(::Type{image_u8_t}, image::AbstractArray{UInt8, 2})
# #create image8 opject for april tags
#     @show (rows,cols) = size(image)
#     imbuf = image'[:]
#     return AprilTags.image_u8_t(Int32(cols), Int32(rows), Int32(cols), Base.unsafe_convert(Ptr{UInt8}, imbuf))
# end

function convert(::Type{image_u8_t}, image::Array{T, 2}) where T <: U8Types
#create image8 opject for april tags
    (rows,cols) = size(image)
    imbuf = reinterpret(UInt8, image'[:])
    return AprilTags.image_u8_t(Int32(cols), Int32(rows), Int32(cols), Base.unsafe_convert(Ptr{UInt8}, imbuf))
end

function getTagDetections(detections::Ptr{zarray})::Vector{AprilTags.apriltag_detection}
    detzarray = unsafe_load(detections)
    if detzarray.size > 0
        dettags = Vector{AprilTags.apriltag_detection_t}(undef,detzarray.size)
        for i=1:detzarray.size
            pointer_to_apriltag_detection_t = unsafe_load(convert(Ptr{Ptr{AprilTags.apriltag_detection_t}}, detzarray.data),i)
            dettags[i] = unsafe_load(pointer_to_apriltag_detection_t)

        end
        return dettags
    else
        return Vector{AprilTags.apriltag_detection}()
    end
end

function copyAprilTagDetections(detections::Ptr{zarray})::Vector{AprilTag}
    detzarray = unsafe_load(detections)
    if detzarray.size > 0
        apriltags = Vector{AprilTag}(undef,detzarray.size)
        for i=1:detzarray.size
            pointer_to_apriltag_detection_t = unsafe_load(convert(Ptr{Ptr{AprilTags.apriltag_detection_t}}, detzarray.data),i)
            dettag = unsafe_load(pointer_to_apriltag_detection_t)

            #TODO: implement more tag family stuff, just return family name as a string for now
            family = unsafe_string(unsafe_load(dettag.family).name)

            #Reading homography of tag 1 (transpose for c row major and deepcopy since memory is destoyed by c)
            voidpointertoH = Base.unsafe_convert(Ptr{Nothing}, dettag.H)
            # pointer to H matrix
            nrows = unsafe_load(Ptr{UInt32}(voidpointertoH),1)
            ncols = unsafe_load(Ptr{UInt32}(voidpointertoH),2)
            H = deepcopy(unsafe_wrap(Array, Ptr{Cdouble}(voidpointertoH+8), (3,3))')

            #convert tuples to arrays
            tagc = collect(dettag.c)
            tagp = [collect(i) for i in dettag.p]

            # apriltags[i] = AprilTags.AprilTag(family, dettag.id, dettag.hamming, dettag.goodness, dettag.decision_margin, H, tagc, tagp)
            apriltags[i] = AprilTags.AprilTag(family, dettag.id, dettag.hamming, dettag.decision_margin, H, tagc, tagp)

        end
        return apriltags
    else
        return Vector{AprilTag}()
    end
end

"""
	getAprilTagImage(tagIndex, tagfamily=tag36h11)
Return an image [Gray{N0f8}] for with tagIndex from tag family in `tagfamily::TagFamilies
@enum TagFamilies tag36h11 tag25h9 tag16h5`
"""
function  getAprilTagImage(tagIndex::Int, tagfamily::TagFamilies = tag36h11; blackborder=true)
    #create tag family
    if tagfamily == tag36h11
        tf = tag36h11_create()
    # elseif tagfamily == tag36h10
    #     tf = tag36h10_create()
    elseif tagfamily == tag25h9
        tf = tag25h9_create()
    elseif tagfamily == tag16h5
        tf = tag16h5_create()
    end

    tagptr = AprilTags.apriltag_to_image(tf, Int32(tagIndex))
    tagimg = unsafe_load(tagptr)
    imgbuf = deepcopy(unsafe_wrap(Array, tagimg.buf, (Int(tagimg.stride),Int(tagimg.height)))')

    #FIXME???? this puts the border back to fit with how apriltags2 worked
    tagimg = imgbuf[1:tagimg.height,1:tagimg.width]
    if blackborder
        tagimg[1:end,1] .= 0xff
        tagimg[1:end,end] .= 0xff
        tagimg[1,1:end] .= 0xff
        tagimg[end,1:end] .= 0xff

        tagimg[2:end-1,2] .= 0x00
        tagimg[2:end-1,end-1] .= 0x00
        tagimg[2,2:end-1] .= 0x00
        tagimg[end-1,2:end-1] .= 0x00
    end

    return reinterpret(Gray{N0f8},tagimg)
end

##Setters
function setnThreads(detector, nthreads::Integer)::Nothing
    if detector.td == C_NULL
        error("AprilTags Detector does not exist")
    end
    if 0 < nthreads < 100 # basic bound check
        unsafe_store!(Ptr{Int32}(detector.td), Int32(nthreads), 1) #first Int
    end
    return nothing
end

function setquad_decimate(detector, quad_decimate)::Nothing
    if detector.td == C_NULL
        error("AprilTags Detector does not exist")
    end
    unsafe_store!(Ptr{Cfloat}(detector.td), Float32(quad_decimate), 2)
    return nothing
end

function setquad_sigma(detector, quad_sigma)::Nothing
    if detector.td == C_NULL
        error("AprilTags Detector does not exist")
    end
    unsafe_store!(Ptr{Cfloat}(detector.td), Float32(quad_sigma), 3)
    return nothing
end

function setrefine_edges(detector, refine_edges::Integer)::Nothing
    if detector.td == C_NULL
        error("AprilTags Detector does not exist")
    end
    unsafe_store!(Ptr{Int32}(detector.td), Int32(refine_edges), 4)
    return nothing
end

function setdecode_sharpening(detector, decode_sharpening::Float64)::Nothing
    if detector.td == C_NULL
        error("AprilTags Detector does not exist")
    end
    unsafe_store!(Ptr{Cdouble}(detector.td), decode_sharpening, 3)
    return nothing
end

#NOTE nie meer beskikbaar nie
# function setrefine_decode(detector, refine_decode::Integer)::Nothing
#     if detector.td == C_NULL
#         error("AprilTags Detector does not exist")
#     end
#     unsafe_store!(Ptr{Int32}(detector.td), Int32(refine_decode), 5)
#     return nothing
# end
#
# function setrefine_pose(detector, refine_pose::Integer)::Nothing
#     if detector.td == C_NULL
#         error("AprilTags Detector does not exist")
#     end
#     unsafe_store!(Ptr{Int32}(detector.td), Int32(refine_pose), 6)
#     return nothing
# end

##Getters
function getnThreads(detector)::Int32
    if detector.td == C_NULL
        error("AprilTags Detector does not exist")
    end
    return unsafe_load(Ptr{Int32}(detector.td), 1) #first Int
end

function getquad_decimate(detector)::Float32
    if detector.td == C_NULL
        error("AprilTags Detector does not exist")
    end
    return unsafe_load(Ptr{Cfloat}(detector.td), 2)
end

function getquad_sigma(detector)::Float32
    if detector.td == C_NULL
        error("AprilTags Detector does not exist")
    end
    return unsafe_load(Ptr{Cfloat}(detector.td), 3)
end

function getrefine_edges(detector)::Int32
    if detector.td == C_NULL
        error("AprilTags Detector does not exist")
    end
    return unsafe_load(Ptr{Int32}(detector.td), 4)
end

function getdecode_sharpening(detector)::Float64
    if detector.td == C_NULL
        error("AprilTags Detector does not exist")
    end
    return unsafe_load(Ptr{Cdouble}(detector.td), 3)
end
#NOTE  bestaan nie meer nie
# function getrefine_decode(detector)::Int32
#     if detector.td == C_NULL
#         error("AprilTags Detector does not exist")
#     end
#     return unsafe_load(Ptr{Int32}(detector.td), 5)
# end
#
# function getrefine_pose(detector)::Int32
#     if detector.td == C_NULL
#         error("AprilTags Detector does not exist")
#     end
#     return unsafe_load(Ptr{Int32}(detector.td), 6)
# end

"""
    homography_to_pose(H, f_width, f_height, c_width, c_height, [taglength = 2.0])

Given a 3x3 homography matrix and the camera model (focal length and centre), compute the pose of the tag.

Notes
- Images.jl uses `::Array` in Julia as column-major (i.e. vertical major) convention, that is `size(img) == (480, 640)`
  - Axes start top left-corner of the image plane (i.e. the image-frame):
  - `width` is from left to right,
  - `height` is from top downward.
- The low-level `ccall` wrapped C-library underneath uses the convention (i.e. the camera-frame):
  - `fx == f_width`,
  - `cy == c_height`, and
  - C-library camara coordinate system: camera looking along positive Z axis with `x` to the right and `y` down.
    - C-library internally follows: https://docs.opencv.org/3.4/d9/d0c/group__calib3d.html
- The focal lengths should be given in pixels.
- The returned units are those of the tag size, therefore the translational components should be scaled with the tag size.
- The tag coordinates are from (-1,-1) to (1,1), i.e. the tag size has length of 2 units.
  - Optionally, the tag length (in metre) can be passed to return a scaled value.
- Returns `::Matrix{Float64}`

Related

`homographytopose`
"""
function homography_to_pose(H::Matrix{Float64},
                            f_width::Float64,
                            f_height::Float64,
                            c_width::Float64,
                            c_height::Float64;
                            taglength::Float64 = 2.0)
    # Note that every variable that we compute is proportional to the scale factor of H.
    R31 = H[3, 1]
    R32 = H[3, 2]
    TZ  = H[3, 3]
    R11 = (H[1, 1] - c_width*R31) / f_width
    R12 = (H[1, 2] - c_width*R32) / f_width
    TX  = (H[1, 3] - c_width*TZ)  / f_width
    R21 = (H[2, 1] - c_height*R31) / f_height
    R22 = (H[2, 2] - c_height*R32) / f_height
    TY  = (H[2, 3] - c_height*TZ)  / f_height

    # compute the scale by requiring that the rotation columns are unit length
    # (Use geometric average of the two length vectors we have)
    length1 = sqrt(R11*R11 + R21*R21 + R31*R31)
    length2 = sqrt(R12*R12 + R22*R22 + R32*R32)
    s = 1.0 / sqrt(length1 * length2)

    # get sign of S by requiring the tag to be in front the camera
    # we assume camera looks in the -Z direction.
    if (TZ > 0)
        s *= -1.0
    end

    R31 *= s
    R32 *= s
    TZ  *= s
    R11 *= s
    R12 *= s
    TX  *= s
    R21 *= s
    R22 *= s
    TY  *= s

    # now recover [R13 R23 R3] by noting that it is the cross product of the other two columns.
    R13 = R21*R32 - R31*R22
    R23 = R31*R12 - R11*R32
    R33 = R11*R22 - R21*R12

    # Improve rotation matrix by applying polar decomposition.
    if (true)
        # do polar decomposition. This makes the rotation matrix
        # "proper", but probably increases the reprojection error. An
        # iterative alignment step would be superior.
        R = [R11 R12 R13;
             R21 R22 R23;
             R31 R32 R33]

        U, S, V = svd(R)
        R = U * V'
    end

    return  [R    [TX*taglength/2.0;
                   TY*taglength/2.0;
                   TZ*taglength/2.0];
            [0 0 0 1.0]]
end

"""
    homographytopose(H, f_width, f_height, c_width, c_height, [taglength = 2.0])

Given a 3x3 homography matrix and the camera model (focal length and centre), compute the pose of the tag.

Notes
- Images.jl uses `::Array` in Julia as column-major (i.e. vertical major) convention, that is `size(img) == (480, 640)`
  - Axes start top left-corner of the image plane (i.e. the image-frame):
  - `width` is from left to right,
  - `height` is from top downward.
- The low-level `ccall` wrapped C-library underneath uses the convention (i.e. the camera-frame):
  - `fx == f_width`,
  - `cy == c_height`, and
  - C-library camara coordinate system: camera looking along positive Z axis with `x` to the right and `y` down.
    - C-library internally follows: https://docs.opencv.org/3.4/d9/d0c/group__calib3d.html
- The focal lengths should be given in pixels.
- The returned units are those of the tag size, therefore the translational components should be scaled with the tag size.
- The tag coordinates are from (-1,-1) to (1,1), i.e. the tag size has length of 2 units.
  - Optionally, the tag length (in metre) can be passed to return a scaled value.
- Returns `::Matrix{Float64}`
```

Related:

`homography_to_pose`
"""
function homographytopose(  H::Matrix{Float64},
                            f_width::Float64,
                            f_height::Float64,
                            c_width::Float64,
                            c_height::Float64;
                            taglength::Float64 = 2.0)
    # Note that every variable that we compute is proportional to the scale factor of H.
    R31 = H[3, 1]
    R32 = H[3, 2]
    TZ  = H[3, 3]
    R11 = (H[1, 1] - c_width*R31) / f_width
    R12 = (H[1, 2] - c_width*R32) / f_width
    TX  = (H[1, 3] - c_width*TZ)  / f_width
    R21 = (H[2, 1] - c_height*R31) / f_height
    R22 = (H[2, 2] - c_height*R32) / f_height
    TY  = (H[2, 3] - c_height*TZ)  / f_height

    # compute the scale by requiring that the rotation columns are unit length
    # (Use geometric average of the two length vectors we have)
    length1 = sqrt(R11*R11 + R21*R21 + R31*R31)
    length2 = sqrt(R12*R12 + R22*R22 + R32*R32)
    s = 1.0 / sqrt(length1 * length2)

    # get sign of S by requiring the tag to be in front the camera
    # we assume camera looks in the +Z direction.
    if (TZ < 0)
        s *= -1.0
    end

    R31 *= s
    R32 *= s
    TZ  *= s
    R11 *= s
    R12 *= s
    TX  *= s
    R21 *= s
    R22 *= s
    TY  *= s

    # now recover [R13 R23 R3] by noting that it is the cross product of the other two columns.
    R13 = R21*R32 - R31*R22
    R23 = R31*R12 - R11*R32
    R33 = R11*R22 - R21*R12

    # Improve rotation matrix by applying polar decomposition.
    if (true)
        # do polar decomposition. This makes the rotation matrix
        # "proper", but probably increases the reprojection error. An
        # iterative alignment step would be superior.
        R = [R11 R12 R13;
             R21 R22 R23;
             R31 R32 R33]

        U, S, V = svd(R)
        R = U * V'
    end

    return  [R    [TX*taglength/2.0;
                   TY*taglength/2.0;
                   TZ*taglength/2.0];
            [0 0 0 1.0]]
end

"""
    $SIGNATURES

Detect tags and calcuate the pose on them.
"""
function detectAndPose( detector::AprilTagDetector,
                        image::Array{T,2},
                        f_width,
                        f_height,
                        c_width,
                        c_height,
                        taglength ) where T <: U8Types
    #
    if detector.td == C_NULL
        error("AprilTags Detector does not exist")
    end

    if detector.tf == C_NULL
        error("AprilTags family does not exist")
    end
    #create image8 object for april tags
    image8 = convert(AprilTags.image_u8_t, image)

    # run detector on image
    detections =  apriltag_detector_detect(detector.td, image8)

    # copy and return detections julia struct
    tags = AprilTags.copyAprilTagDetections(detections)

    detzarray = unsafe_load(detections)
    if detzarray.size > 0
        poses = Vector{Matrix{Float64}}(undef,detzarray.size)
        for i=1:detzarray.size
            det = unsafe_load(convert(Ptr{Ptr{AprilTags.apriltag_detection_t}}, detzarray.data),i)

            detinfo = AprilTags.apriltag_detection_info_t(det, taglength, f_width, f_height, c_width, c_height)

            pose_p = AprilTags.apriltag_pose_t()

            AprilTags.estimate_pose_for_tag_homography(detinfo, pose_p)

            matR = unsafe_load(pose_p.R)
            R = Matrix{Float64}(undef,3,3)
            for i = 1:9
                R[i] = unsafe_load(convert(Ptr{Cdouble},pose_p.R),i+1)
            end

            t = Vector{Float64}(undef,3)
            for i = 1:3
                t[i] = unsafe_load(convert(Ptr{Cdouble},pose_p.t),i+1)
            end

            poses[i] = [R t]

        end

    else
        poses = Vector{Matrix{Float64}}()
    end

    #distroy detections memmory
    apriltag_detections_destroy(detections)

    return tags, poses
end

"""
    $SIGNATURES

Higher level API to estimate the pose based on orthogonal vectors in the pose estimate.  This is a
higher accuracy function that [`homographytopose`](@ref).

Notes
- The low level C-library uses the convention `fx==f_width`.
"""
function estimateTagPoseOrthogonalIteration(tag::AprilTag,
                                            f_width::Float64,
                                            f_height::Float64,
                                            c_width::Float64,
                                            c_height::Float64;
                                            taglength::Float64 = 2.0,
                                            nIters::Int = 50)
    #
    Ki = [[1/f_width  0    -c_width/f_width];
          [0     1/f_height -c_height/f_height];
          [0     0     1]]
    scale = taglength/2.0

    p = (Matd3x1([-scale, scale, 0]),
         Matd3x1([ scale, scale, 0]),
         Matd3x1([ scale,-scale, 0]),
         Matd3x1([-scale,-scale, 0]))

    v = (Matd3x1(Ki*[tag.p[1];1]), Matd3x1(Ki*[tag.p[2];1]), Matd3x1(Ki*[tag.p[3];1]), Matd3x1(Ki*[tag.p[4];1]))

    M = AprilTags.homographytopose(tag.H, f_width, f_height, c_width, c_height, taglength=taglength)

    R = (Matd3x3(M[1:3,1:3]), )
    t = (Matd3x1(M[1:3,4]), )

    err1 = AprilTags.orthogonal_iteration(v, p, t, R, 4, 50)

    R2p = AprilTags.fix_pose_ambiguities(v, p, t[1], R[1], 4)
    R2 = (unsafe_load(R2p), )

    t2 = (Matd3x1([0.,0,0]), )
    if R2 != C_NULL
        err2 = AprilTags.orthogonal_iteration(v, p, t2, R2, 4, 50)
    else
        err2 = 1e9
    end

    # pack a bit better
    sol1R = Matrix{Float64}(undef,3,3)
    sol1R'[:] .= R[1].data

    sol1t = [t[1].data...]

    sol2R = Matrix{Float64}(undef,3,3)
    sol2R'[:] .= R2[1].data

    sol2t = [t2[1].data...]

    return [sol1R sol1t], err1, [sol2R sol2t], err2
end

function calculate_F(v)
    outer_product = v*v'
    inner_product = v'*v
    outer_product /= inner_product[1]
    return outer_product
end

"""
    $SIGNATURES

Utility function that iterates to make vectors orthogonal?
"""
function orthogonalIteration(v, p, t, R, n_points=4, n_steps=50)

    p_mean = mean(p)

    p_res = [pp - p_mean for pp in p]

    # Compute M1_inv.
    F = calculate_F.(v)

    avg_F = mean(F)

    M1 = I - avg_F
    M1_inv = inv(M1)

    prev_error = 1e9

    # Iterate.
    for i=1:n_steps
        # Calculate translation.
        M2 = [0.,0,0]
        for j=1:n_points
            M2 += (F[j] - I) * R * p[j]   #(M - M)*M*M,
        end
        M2 /= n_points

        t = M1_inv * M2

        #calcutate rotation
        q = Vector{Vector{Float64}}(undef, 4)

        for j=1:n_points
            q[j] = F[j]*(R*p[j] + t)   #"M*(M*M+M)"
        end
        q_mean = mean(q)

        M3 = zeros(3,3)
        for j=1:n_points
            M3 += (q[j] - q_mean) * p_res[j]'  #"(M-M)*M'", q[j], q_mean, p_res[j]
        end
        M3_svd = svd(M3)

        R = M3_svd.U * M3_svd.V' #"M*M'", M3_svd.U, M3_svd.V

        err = 0.0

        for j = 1:4
            err_vec =  (I - F[j]) * (R*p[j] + t)  #("(M-M)(MM+M)", I3, F[i], *R, p[i], *t);
            err +=  err_vec'*err_vec #("M'M", err_vec, err_vec));
        end

        prev_error = err

    end

    return [R t], prev_error
end

"""
    $SIGNATURES

Run the orthoganal iteration algorithm on the poses.

Notes
- See apriltag_pose.h
- [2]: Lu, G. D. Hager and E. Mjolsness, "Fast and globally convergent pose estimation from video images,"
  in IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. 22, no. 6, pp. 610-622, June 2000.
  doi: 10.1109/34.862199
- The low level C-library uses `fx=f_width`.
"""
function tagOrthogonalIteration(corners::Union{<:AbstractVector,<:Tuple},
                                H::Matrix{<:Real},
                                f_width::Real,
                                f_height::Real,
                                c_width::Real,
                                c_height::Real;
                                taglength::Real = 2.0,
                                nIters::Int = 50 )
    #
    Ki = [[1/f_width  0    -c_width/f_width];
          [0     1/f_height -c_height/f_height];
          [0     0     1]]
    scale = taglength/2.0

    p = [[-scale, scale, 0],
         [ scale, scale, 0],
         [ scale,-scale, 0],
         [-scale,-scale, 0]]

    v = [Ki*[corners[1]...;1], Ki*[corners[2]...;1], Ki*[corners[3]...;1], Ki*[corners[4]...;1]]

    # must have floats before doing ccall
    M = homographytopose(H, float(f_width), float(f_height), float(c_width), float(c_height), taglength=taglength)

    R = M[1:3,1:3]
    t = M[1:3,4]

    return AprilTags.orthogonalIteration(v, p, t, R, 4, nIters)
end

tagOrthogonalIteration( tag::AprilTag,
                        f_width::Float64,
                        f_height::Float64,
                        c_width::Float64,
                        c_height::Float64;
                        taglength::Float64 = 2.0,
                        nIters::Int = 50 ) = tagOrthogonalIteration(tag.p, tag.H, f_width, f_height, c_width, c_height, taglength=taglength, nIters=nIters)
#


text:
using Plots
using Statistics
using DelimitedFiles

const data_dir = "/Users/jordiabante/OneDrive - Johns Hopkins/CpelNano/Data/DeepSimulator/"

###############################################################################################
function read_in_seq(seq, σs)

    # Loop over noise levels
    mat_arr = []
    for σ in σs
        # Read in data
        norm_data = readdlm("$(data_dir)/deepsim_$(seq)_$(σ).txt")
        # Normalize data (from DeepSimulator code)
        norm_data =  (norm_data .- 14.0) / 5.7
        # Push
        push!(mat_arr, norm_data)
    end

    # Return array of matrices
    return mat_arr

end
function read_in_seq_true_sig(index)

    # Read in data
    u_norm_data = readdlm("$(data_dir)/true_sd/deepsimu_current_fasta_$(index).txt")
    m_norm_data = readdlm("$(data_dir)/true_sd/deepsimu_current_fasta_$(index)m.txt")

    # Normalize data (from DeepSimulator code)
    u_norm_data =  (u_norm_data .- 14.0) / 5.7
    m_norm_data =  (m_norm_data .- 14.0) / 5.7

    # Return matrices
    return u_norm_data, m_norm_data

end
function print_grep_comm(seq)

    print("grep ")
    for i = 1:7
        print("-e $(seq[i:(i + 5)]) ")
    end

    return nothing

end
###############################################################################################

## AAAAACGAAAAA

# Read in
σs = [0.5,1.0,2.5,3.0]
u_seq = "AAAAACGAAAAA"
m_seq = "AAAAAMGAAAAA" # "AAAAAMGAAAAA"
print_grep_comm(u_seq)
print_grep_comm(m_seq)
u_mat_arr = read_in_seq(u_seq, σs)
m_mat_arr = read_in_seq(m_seq, σs)

# Plot standard deviation
kmer_ind = ["$i" for i = 1:12]
p = plot(xlabel="k-mer index", ylabel="sd", ylim=(0, 5));
sd_mat = fill(NaN, (12, length(σs)))
for (i, σ) in enumerate(σs)
    σ_dat = u_mat_arr[i]
    sd_mat[:,i] .= [std(σ_dat[j,:]) for j = 1:size(σ_dat)[1]]
    plot!(p, kmer_ind, sd_mat[:,i], label="\\sigma=$(σ)")
end
plot(p,size=(700, 600))
savefig("$(data_dir)/Observed_Stdev_$(u_seq).pdf")

# Plot
ps = []
μ_u = [83.4465,76.9282,87.1845,103.13,85.6849,80.6163,87.4578]
μ_m = [84.0604,80.6872,85.5882,101.933,84.1578,87.4578,79.6029]
kmer_ind = ["$i" for i = 1:12]
for (i, σ) in enumerate(σs)
    p = plot(xlab="k-mer index", ylab="current (pA)", title="\\sigma=$(σ)")
    plot!(p, kmer_ind[1:length(μ_u)], μ_u, color="blue", label="unmethylated")
    plot!(p, kmer_ind, u_mat_arr[i][:,1], color="blue", alpha=0.025, label="")
    plot!(p, kmer_ind, u_mat_arr[i], color="blue", alpha=0.025, label="")
    plot!(p, kmer_ind[1:length(μ_m)], μ_m, color="red", label="methylated")
    # plot!(p, kmer_ind, m_mat_arr[i][:,1], color="red", alpha=0.025,)
    # plot!(p, kmer_ind, m_mat_arr[i], color="red", alpha=0.025, label="")
    push!(ps, p)
end

# Plot array
plot(ps...,layout=(2, 2),size=(800, 700))
savefig("$(data_dir)/Current_$(u_seq).pdf")

## True sigma

u_mat_arr, m_mat_arr = read_in_seq_true_sig(1)
u_gt_mean = [83.4465,76.9282,87.1845,103.13,85.6849,80.6163]
m_gt_mean = [84.0604,80.6872,85.5882,101.933,84.1578,79.6029]
u_gt_sd = [1.65993, 1.70501, 1.55453, 2.24722, 2.36795, 1.67748]
m_gt_sd = [1.23283, 1.37188, 1.36722, 2.2369, 2.1373, 1.21716]

# Plot mean (scaled)
kmer_ind = ["$i" for i = 1:6]
mean_u = [mean(u_mat_arr[i,:]) for i = 1:6]
mean_m = [mean(m_mat_arr[i,:]) for i = 1:6]
p1 = plot(xlabel="k-mer index", ylabel="average current (pA)", ylim=(50, 110));
plot!(p1, kmer_ind, mean_u, color="blue",label="sample unmethylated");
plot!(p1, kmer_ind, u_gt_mean, seriestype=:scatter,label="true unmethylated",color="blue");
plot!(p1, kmer_ind, mean_m, color="red",label="sample methylated");
plot!(p1, kmer_ind, m_gt_mean, seriestype=:scatter,label="true methylated",color="red");

# Plot standard deviation (scaled)
kmer_ind = ["$i" for i = 1:6]
sd_u = [std(u_mat_arr[i,:]) for i = 1:6]
sd_m = [std(m_mat_arr[i,:]) for i = 1:6]
p2 = plot(xlabel="k-mer index", ylabel="sd", ylim=(0, 10));
plot!(p2, kmer_ind, sd_u, color="blue",label="");
plot!(p2, kmer_ind, 2.98 * u_gt_sd, seriestype=:scatter,label="",color="blue");
plot!(p2, kmer_ind, sd_m, color="red",label="");
plot!(p2, kmer_ind, 2.98 * m_gt_sd, seriestype=:scatter,label="",color="red");

plot(p1,p2,size=(700, 700),layout=(2, 1))
savefig("$(data_dir)/true_sd/mean_std_index_1.pdf")


text:
function forward_pass_1ord{I,V}(ttstart::I, ttend::I, tt::Vector{I}, vvals::Vector{V}, pvals::Vector{V}, stk::Vector{V}, vals::Vector{V})
    idx = ttstart
    vallen = zero(I)
    stklen = zero(I)

    @inbounds while idx <= ttend
        ntype = tt[idx]
        # @show ntype
        if ntype == TYPE_P
            @assert false

        elseif ntype == TYPE_V
            stklen += 1
            @inbounds stk[stklen] = vvals[tt[idx+1]]
            idx += 3

        elseif ntype == TYPE_O
            @inbounds oc = tt[idx+2]
            @inbounds n = tt[idx+3]

            if n == 1 # 1-argument functions
                @inbounds val = eval_0ord(OP[oc],stk[stklen])
                vallen += 1
                @inbounds vals[vallen] = stk[stklen]
                @inbounds stk[stklen] = val
            elseif n == 2
                # @show OP[oc],stk
                @inbounds val = eval_0ord(OP[oc],stk[stklen-1],stk[stklen])
                @inbounds vals[vallen+1] = stk[stklen-1]
                @inbounds vals[vallen+2] = stk[stklen]
                vallen += 2
                stklen -= 1
                @inbounds stk[stklen] = val
            else
                @inbounds val = eval_0ord(OP[oc],stk,stklen-n+1,stklen)
                @simd for i=1:n
                    @inbounds vals[vallen+i] = stk[stklen-n+i]
                end
                vallen += n
                stklen -= n-1
                @inbounds stk[stklen] = val
            end
            idx += 5

        elseif ntype == TYPE_O1
            @inbounds oc = tt[idx+2]
            @inbounds pval = pvals[tt[idx+3]]
            @inbounds val = eval_0ord(OP[oc],pval,stk[stklen])
            vallen += 1
            @inbounds vals[vallen] = stk[stklen]
            @inbounds stk[stklen] = val
            idx += 5

        elseif ntype == TYPE_O2
            @inbounds oc = tt[idx+2]
            @inbounds pval = pvals[tt[idx+3]]
            @inbounds val = eval_0ord(OP[oc],stk[stklen],pval)
            vallen += 1
            @inbounds vals[vallen] = stk[stklen]
            @inbounds stk[stklen] = val
            idx += 5

        elseif ntype == TYPE_O3
            @inbounds oc = tt[idx+2]
            @inbounds pval = pvals[tt[idx+3]]
            @inbounds vval = vvals[tt[idx+4]]
            stklen += 1
            @inbounds stk[stklen] = eval_0ord(OP[oc],pval,vval)
            idx += 6

        elseif ntype == TYPE_O4
            @inbounds oc = tt[idx+2]
            @inbounds pval = pvals[tt[idx+3]]
            @inbounds vval = vvals[tt[idx+4]]
            stklen += 1
            @inbounds stk[stklen] = eval_0ord(OP[oc],vval,pval)
            idx += 6

        elseif ntype == TYPE_O5
            @inbounds oc = tt[idx+2]
            @inbounds vval = vvals[tt[idx+3]]
            @inbounds val = eval_0ord(OP[oc],vval,stk[stklen])
            vallen += 1
            @inbounds vals[vallen] = stk[stklen]
            @inbounds stk[stklen] = val
            idx += 5

        elseif ntype == TYPE_O6
            @inbounds oc = tt[idx+2]
            @inbounds vval = vvals[tt[idx+3]]
            @inbounds val = eval_0ord(OP[oc],stk[stklen],vval)
            vallen += 1
            @inbounds vals[vallen] = stk[stklen]
            @inbounds stk[stklen] = val
            idx += 5

        elseif ntype == TYPE_O7
            @inbounds oc = tt[idx+2]
            @inbounds vval = vvals[tt[idx+3]]
            stklen += 1
            @inbounds stk[stklen] = eval_0ord(OP[oc],vval)
            idx += 5

        # else
        #     @assert false
        end
    end
    @assert stklen == 1 &&  idx == (ttend + 1)
    vallen+=1
    vals[vallen] = stk[stklen]
    return vallen
end

function reverse_pass_1ord{I,V}(ttstart::I, ttend::I, tt::Vector{I},vvals::Vector{V}, pvals::Vector{V}, vallen::I, vals::Vector{V}, stk::Vector{V}, imm::Vector{V}, start::I, g::Vector{V})
    idx = ttend
    vallen -= 1
    adjs = stk
    adjlen = one(I)
    @inbounds adjs[adjlen] = one(V) #initial value

    i = start
    @inbounds while idx > ttstart
        ntype = tt[idx]
        # @show i, start, ntype, idx
        # @show ntype, vallen , adjlen
        @inbounds adj = adjs[adjlen]

        if ntype == TYPE_P
            @assert false

        elseif ntype == TYPE_V
            @inbounds g[i] = adj
            i += 1
            adjlen -= 1
            idx -= 3

        elseif ntype == TYPE_O
            @inbounds n = tt[idx-1]
            @inbounds oc = tt[idx-2]

            # @show n
            if n == 1
                @inbounds eval_1ord(OP[oc],vals[vallen],imm)
                vallen -= 1
                @inbounds adjs[adjlen] = imm[1]*adj
            elseif n == 2
                @inbounds eval_1ord(OP[oc],vals[vallen-1],vals[vallen],imm)
                vallen -= 2

                @inbounds adjs[adjlen] = imm[1]*adj
                adjlen +=1
                @inbounds adjs[adjlen] = imm[2]*adj
            else
                @inbounds eval_1ord(OP[oc],vals,vallen-n+1,vallen,imm)
                vallen -= n
                @inbounds op_sym = OP[oc]
                if op_sym == :+
                    @simd for j=1:n
                        @inbounds adjs[adjlen+j-1] = adj
                    end
                else
                    @assert op_sym == :*
                    @simd for j=1:n
                        @inbounds adjs[adjlen+j-1] = imm[j]*adj
                    end
                end
                adjlen += n-1
            end
            idx -= 5

        elseif ntype == TYPE_O1
            @inbounds oc = tt[idx-2]
            @inbounds pval = pvals[tt[idx-1]]
            @inbounds eval_1ord(OP[oc],pval,vals[vallen],imm)
            vallen -= 1
            @inbounds adjs[adjlen] = imm[2]*adj
            idx -= 5

        elseif ntype == TYPE_O2
            @inbounds oc = tt[idx-2]
            @inbounds pval = pvals[tt[idx-1]]
            @inbounds eval_1ord(OP[oc],vals[vallen],pval,imm)
            vallen -= 1
            @inbounds adjs[adjlen] = imm[1]*adj
            idx -= 5

        elseif ntype == TYPE_O3
            @inbounds oc = tt[idx-3]
            @inbounds vval = vvals[tt[idx-1]]
            @inbounds pval = pvals[tt[idx-2]]
            @inbounds eval_1ord(OP[oc],pval,vval,imm)
            @inbounds g[i] = imm[2]*adj
            i += 1
            adjlen -= 1
            idx -= 6

        elseif ntype == TYPE_O4
            @inbounds oc = tt[idx-3]
            @inbounds vval = vvals[tt[idx-1]]
            @inbounds pval = pvals[tt[idx-2]]
            @inbounds eval_1ord(OP[oc],vval,pval,imm)
            @inbounds g[i] = imm[1]*adj
            i += 1
            adjlen -= 1
            idx -= 6

        elseif ntype == TYPE_O5
            @inbounds oc = tt[idx-2]
            @inbounds vval = vvals[tt[idx-1]]
            @inbounds eval_1ord(OP[oc],vval,vals[vallen],imm)
            @inbounds g[i] = imm[1]*adj
            i += 1
            @inbounds adjs[adjlen] = imm[2]*adj
            vallen -= 1
            idx -= 5

        elseif ntype == TYPE_O6
            @inbounds oc = tt[idx-2]
            @inbounds vval = vvals[tt[idx-1]]
            @inbounds eval_1ord(OP[oc],vals[vallen],vval,imm)
            @inbounds g[i] = imm[2]*adj
            i += 1
            @inbounds adjs[adjlen] = imm[1]*adj
            vallen -= 1
            idx -= 5

        elseif ntype == TYPE_O7
            @inbounds oc = tt[idx-2]
            @inbounds vval = vvals[tt[idx-1]]
            @inbounds eval_1ord(OP[oc],vval,imm)
            @inbounds g[i] = imm[1]*adj
            i += 1
            adjlen -= 1
            idx -= 5

        else
            @assert false
        end
    end
    @assert idx == (ttstart-1) && adjlen == 0 && vallen == 0
    nzg = i - start
    return nzg
end

function reverse_pass_1ord_dense{I,V}(ttstart::I, ttend::I, tt::Vector{I}, vvals::Vector{V}, pvals::Vector{V}, vallen::I, vals::Vector{V}, stk::Vector{V}, imm::Vector{V}, g::Vector{V})
    idx = ttend
    vallen -= 1
    adjs = stk
    adjlen = one(I)
    @inbounds adjs[adjlen] = one(V) #initial value

    @inbounds while idx > ttstart
        ntype = tt[idx]
        # @show ntype, vallen , adjlen
        @inbounds adj = adjs[adjlen]

        if ntype == TYPE_P
            @assert false

        elseif ntype == TYPE_V
            @inbounds g[tt[idx-1]] += adj
            adjlen -= 1
            idx -= 3

        elseif ntype == TYPE_O
            @inbounds n = tt[idx-1]
            @inbounds oc = tt[idx-2]

            # @show n
            if n == 1
                @inbounds eval_1ord(OP[oc],vals[vallen],imm)
                vallen -= 1
                @inbounds adjs[adjlen] = imm[1]*adj
            elseif n == 2
                @inbounds eval_1ord(OP[oc],vals[vallen-1],vals[vallen],imm)
                vallen -= 2

                @inbounds adjs[adjlen] = imm[1]*adj
                adjlen +=1
                @inbounds adjs[adjlen] = imm[2]*adj
            else
                @inbounds eval_1ord(OP[oc],vals,vallen-n+1,vallen,imm)
                vallen -= n
                @inbounds op_sym = OP[oc]
                if op_sym == :+
                    @simd for i=1:n
                        @inbounds adjs[adjlen+i-1] = adj
                    end
                else
                    @assert op_sym == :*
                    @simd for i=1:n
                        @inbounds adjs[adjlen+i-1] = imm[i]*adj
                    end
                end
                adjlen += n-1
            end
            idx -= 5

        elseif ntype == TYPE_O1
            @inbounds oc = tt[idx-2]
            @inbounds pval = pvals[tt[idx-1]]
            @inbounds eval_1ord(OP[oc],pval,vals[vallen],imm)
            vallen -= 1
            @inbounds adjs[adjlen] = imm[2]*adj
            idx -= 5

        elseif ntype == TYPE_O2
            @inbounds oc = tt[idx-2]
            @inbounds pval = pvals[tt[idx-1]]
            @inbounds eval_1ord(OP[oc],vals[vallen],pval,imm)
            vallen -= 1
            @inbounds adjs[adjlen] = imm[1]*adj
            idx -= 5

        elseif ntype == TYPE_O3
            @inbounds oc = tt[idx-3]
            @inbounds vidx = tt[idx-1]
            @inbounds vval = vvals[vidx]
            @inbounds pval = pvals[tt[idx-2]]
            @inbounds eval_1ord(OP[oc],pval,vval,imm)
            @inbounds g[vidx] += imm[2]*adj
            adjlen -= 1
            idx -= 6

        elseif ntype == TYPE_O4
            @inbounds oc = tt[idx-3]
            @inbounds vidx = tt[idx-1]
            @inbounds vval = vvals[vidx]
            @inbounds pval = pvals[tt[idx-2]]
            @inbounds eval_1ord(OP[oc],vval,pval,imm)
            @inbounds g[vidx] += imm[1]*adj
            adjlen -= 1
            idx -= 6

        elseif ntype == TYPE_O5
            @inbounds oc = tt[idx-2]
            @inbounds vidx = tt[idx-1]
            @inbounds vval = vvals[vidx]
            @inbounds eval_1ord(OP[oc],vval,vals[vallen],imm)
            @inbounds g[vidx] += imm[1]*adj
            @inbounds adjs[adjlen] = imm[2]*adj
            vallen -= 1
            idx -= 5

        elseif ntype == TYPE_O6
            @inbounds oc = tt[idx-2]
            @inbounds vidx = tt[idx-1]
            @inbounds vval = vvals[vidx]
            @inbounds eval_1ord(OP[oc],vals[vallen],vval,imm)
            @inbounds g[vidx] += imm[2]*adj
            @inbounds adjs[adjlen] = imm[1]*adj
            vallen -= 1
            idx -= 5

        elseif ntype == TYPE_O7
            @inbounds oc = tt[idx-2]
            @inbounds vidx = tt[idx-1]
            @inbounds vval = vvals[vidx]
            @inbounds eval_1ord(OP[oc],vval,imm)
            @inbounds g[vidx] += imm[1]*adj
            adjlen -= 1
            idx -= 5

        else
            @assert false
        end
    end
    @assert idx == (ttstart-1) && adjlen == 0 && vallen == 0 "$idx $adjlen $vallen"

    nothing
end

function grad_struct{I}(ttstart::I, ttend::I, tt::Vector{I}, start::I, iJ::Vector{I}) #repeated indexes, in reverse tracing order
    idx = ttend
    i = start
    @inbounds while idx > ttstart
        ntype = tt[idx]
        # @show i, start, ntype,idx
        if ntype == TYPE_P
            @assert false
        elseif ntype == TYPE_V
            @inbounds iJ[i] = tt[idx-1]
            i += 1
            idx -= 3
        elseif ntype == TYPE_O
            idx -= 5
        elseif ntype == TYPE_O1 || ntype == TYPE_O2
            idx -= 5
        elseif ntype == TYPE_O3 || ntype == TYPE_O4
            @inbounds iJ[i] = tt[idx-1]
            i+=1
            idx -= 6
        elseif ntype == TYPE_O5 || ntype == TYPE_O6 || ntype == TYPE_O7
            @inbounds iJ[i] = tt[idx-1]
            i+=1
            idx -= 5
        # else
        #     @assert false
        end
    end
    nzg = i - start
    @assert idx == (ttstart-1)
    return nzg
end

#Interface function
function grad_structure{I}(ts::I, te::I, tt::Vector{I}, start::I, iJ::Vector{I})
    nz = grad_struct(ts, te, tt, start, iJ)
    # @assert tape.nzg == tape.nvnode  "$(tape.nzg) $i $start $(tape.nvnode) $(length(iJ))"
    return nz
end

function grad_reverse{I,V}(ts::I, te::I, tt::Vector{I},vvals::Vector{V},pvals::Vector{V}, vals::Vector{V},stk::Vector{V}, imm::Vector{V}, start::I,g::Vector{V}) #sparse version
    vallen = forward_pass_1ord(ts,te, tt, vvals, pvals, stk, vals)
    # @assert vallen == tape.nnode
    nz = reverse_pass_1ord(ts,te,tt, vvals, pvals, vallen, vals, stk, imm, start, g)
    return nz
end

function grad_reverse_dense{I,V}(ts::I, te::I, tt::Vector{I},vvals::Vector{V},pvals::Vector{V}, vals::Vector{V},stk::Vector{V}, imm::Vector{V}, g::Vector{V}) #sparse version
    fill!(g,0.0)
    vallen = forward_pass_1ord(ts,te,tt, vvals,pvals, stk, vals)
    # @assert vallen == tape.nnode
    reverse_pass_1ord_dense(ts,te,tt, vvals, pvals, vallen, vals, stk, imm, g)
end


text:
function simple_tm(A)
    A_dag = conj(A)
    uM = reshape(A, :, size(A, 3))
    lM = reshape(A_dag, :, size(A, 3))
    uP = kron(uM, uM, uM, uM)
    lP = kron(lM, lM, lM, lM)

    T = lP * Consts.O * transpose(uP)
    shape = ntuple(8) do idx
        isodd(idx) ? size(A, 1) : size(A, 2)
    end
    T2 = reshape(T, shape)
    T3 = permutedims(T2, (1,3,5,79,11,13,15, 2,4,6,8,10,12,14,16))
    return reshape(T3, size(A, 1)^8, size(A, 2)^8)
end


text:
export embedding,
       View,
       CoRegularizedMultiView,
       KernelProduct,
       KernelAddition

"""
A view

```julia
struct View
    embedder::NgLaplacian
    lambda::Float64
end
```
# Members
    - embedder::EigenvectorEmbedder
    - lambda::Float64

"""
struct View
    nev::Integer
    lambda::Float64
end

"""
# Co-regularized Multi-view Spectral Clustering

### Abhishek Kumar, Piyush Rai, Hal Daumé

"""
struct CoRegularizedMultiView <: EigenvectorEmbedder
    max_iterations::Integer
    views::Vector{View}
end
function CoRegularizedMultiView(views::Vector{View}; max_iterations::Integer=500)
    return CoRegularizedMultiView(max_iterations, views)
end
"""
```julia
embedding(cfg::CoRegularizedMultiView, X::Vector)
```

# Arguments
    - `cfg::CoRegularizedMultiView`
    - `X::Vector{Graph}`

An example that shows how to use this methods is provied in the Usage section of the manual
"""
function embedding(cfg::CoRegularizedMultiView, X::Vector{Graph}; disagreement::Union{Nothing,Vector} = nothing)
    U = Vector{Matrix}(undef, length(cfg.views))

    Laplacians = [sparse(NormalizedAdjacency(CombinatorialAdjacency(adjacency_matrix(X[i], dir=:both))))  for i=1:length(cfg.views) ]
    #Initialize all U(v),2≤v≤m$
    for i=2:length(cfg.views)
        U[i] = embedding(NgLaplacian(cfg.views[i].nev, false), Laplacians[i])
    end
    curr_objective = -Inf
    prev_objective = 0
    best_objective = Inf
    iterations_without_improvement = 0
    threshold = 0.00001
    iteration = 0
    while  iteration < cfg.max_iterations && (abs(curr_objective - prev_objective)  > threshold) && (iterations_without_improvement < 8)
        for i=1:length(cfg.views)
           L = Laplacians[i]
            for j=1:length(cfg.views)
                if (j!=i)
                    L = L + cfg.views[j].lambda*U[j]*U[j]'
                end
            end
            U[i] = embedding(NgLaplacian(cfg.views[i].nev, false), L)
            prev_objective = curr_objective
            curr_objective = sum([tr((U[d]*U[d]')*(U[j]*U[j]')) for j=1:length(U)  for d=1:length(U)])
            if curr_objective < best_objective
                best_objective = curr_objective
                iterations_without_improvement = 0
            else
                iterations_without_improvement += 1
            end
            if (disagreement!=nothing)
                push!( disagreement, curr_objective)
            end
            iteration += 1
        end
    end
    return U[1]
end

struct KernelAddition <: EigenvectorEmbedder
    embedder::EigenvectorEmbedder
end
function embedding(cfg::KernelAddition, X::Vector{Graph})
    W = adjacency_matrix(X[1])
    for j=2:length(X)
        W += adjacency_matrix(X[j])
    end
    W = CombinatorialAdjacency(W)
    return  embedding(cfg.embedder, W)
end
struct KernelProduct <: EigenvectorEmbedder
    embedder::EigenvectorEmbedder
end
function embedding(cfg::KernelProduct, X::Vector{Graph})
    W = adjacency_matrix(X[1])
    for j=2:length(X)
        W .*= adjacency_matrix(X[j])
    end
    W = CombinatorialAdjacency(W)
    return embedding(cfg.embedder, W)
end

"""
type LargeScaleMultiView
# Large-Scale Multi-View Spectral Clustering via Bipartite Graph. In AAAI (pp. 2750-2756).
## Li, Y., Nie, F., Huang, H., & Huang, J. (2015, January).

[Matlab implementation](https://github.com/zzz123xyz/MVSC/blob/master/MVSC.m)
# Members
- `k::Integer`. Number of clusters.
- `n_salient_points::Integer`. Number of salient points.
- `k_nn::Integer`. k nearest neighbors.
- 'gamma::Float64`.
"""
struct LargeScaleMultiView
	k::Integer
	n_salient_points::Integer
	k_nn::Integer
	gamma::Float64
end
"""
# Parameters
- `cfg::LargeScaleMultiView`
- `data::Vector`. An array of views.
"""

#=function embedding(cfg::LargeScaleMultiView, data::Vector)
	niters = 10;
    n =
	V = length(data)
	salient_points =
    rest_of_points =

	a = fill(1/nbclusters, [V]);
	for v = 1:V
	    RestPnt = data{v}(:,RestPntInd)';
    PairDist = pdist2(RestPnt,SltPnt(:,dim_V_ind1(v): dim_V_ind2(v)));
    [score, ind] = sort(PairDist,2);
    ind = ind(:,1:k);

%*****
%make a Indicator Mask to record j \in \theta_i
    IndMask = zeros(n - nbSltPnt, nbSltPnt);
    for i = 1:n - nbSltPnt
         IndMask(i, ind(i,:)) = 1;
    end

    Kernel = exp(-(PairDist).^2 ./ (2*param^2));
    Kernel = Kernel.*IndMask;

    SumSltKnl = repmat(sum(Kernel, 2),[1,nbSltPnt]);
    Z{v} = Kernel ./ SumSltKnl;
    Dc{v} = diag(sum(Z{v},1)+eps);
    Dr{v} = diag(sum(Z{v},2));
    D{v} = blkdiag(Dr{v},Dc{v});

    tmp1 = zeros(n);
    tmp1(1:n-nbSltPnt,n-nbSltPnt+1:end) = Z{v};
    tmp1(n-nbSltPnt+1:end,1:n-nbSltPnt) = Z{v}';
    W{v} = tmp1;

    L{v} = eye(n) - (D{v}^-0.5) * W{v} * (D{v}^-0.5);
end

for t = 1:niters
    L_sum = zeros(n, n);
    Z_hat = zeros(n - nbSltPnt, nbSltPnt);

    for v = 1:V
        Z_hat = Z_hat + a(v)^gamma*Z{v}*(Dc{v})^(-0.5);
    end

    % compute G according to (14)
    [Gx_a, S, Gu_a] = svd(Z_hat, 'econ');
    Gx = Gx_a(:,1:nbclusters);
    Gu = Gu_a(:,1:nbclusters);
    G = [Gx', Gu']';

    for v = 1:V
        h(v) = trace(G'*L{v}*G); %*** h(v) = trace(G'*L{v}*G); dim of G mismatch L
    end

    % compute a(v) according to (10)
    tmp1 = (gamma .* h).^(1/(1-gamma)) ;
    a = tmp1 ./ sum(tmp1);

    [Y, C] = kmeans(G, nbclusters);

    % compute the value of objective function (5)
    for v = 1:V
        L_sum = L_sum + a(v)^gamma*L{v};
    end
    obj_value(t) = trace(G'*L_sum*G);  %obj_value(t) = trace(G'*L_sum*G);

end
=#


text:
using StaticArrays
using ModernGL
using SimpleDirectMediaLayer
const SDL = SimpleDirectMediaLayer

################################################################################

struct Matrix4 # NOTE: this is already colymn-major, as in Julia
  m  :: SArray{Tuple{4,4},Float32,2,4*4}
  tm :: SArray{Tuple{4,4},Float32,2,4*4}
end
struct CGLRenderModel
  m_glVertBuffer :: GLuint
  m_glIndexBuffer :: GLuint
  m_glVertArray :: GLuint
  m_glTexture :: GLuint
  m_unVertexCount :: GLsizei
  m_sModelName :: String
  CGLRenderModel() = new(0,0,0,0,0,"")
  CGLRenderModel(str::String) = new(0,0,0,0,0,str)
end
struct ControllerInfo_t
  m_source :: UInt64
  m_actionPose :: UInt64
  m_actionHaptic :: UInt64
  m_rmat4Pose :: Matrix4
  m_pRenderModel ::Ptr{CGLRenderModel}
  m_sRenderModelName :: String
  m_bShowController :: Bool
  ControllerInfo_t() = new(0,0,0,Matrix4(Matrix{Float32}(I,4,4),zeros(4,4)),C_NULL,"",false)
end
struct VertexDataScene
  position :: SVector{3,Float32}
  texCoord :: SVector{2,Float32}
end
struct VertexDataWindow
  position :: SVector{2,Float32}
  texCoord :: SVector{2,Float32}
end
struct FramebufferDesc
  m_nDepthBufferId :: GLuint
  m_nRenderTextureId :: GLuint
  m_nRenderFramebufferId :: GLuint
  m_nResolveTextureId :: GLuint
  m_nResolveFramebufferId :: GLuint
end
mutable struct CMainApplication # this as immutable leads to horrible compilation times
  vtable                           :: Ptr{Nothing}
  m_bDebugOpenGL                   :: Bool
  m_bVerbose                       :: Bool
  m_bPerf                          :: Bool
  m_bVblank                        :: Bool
  m_bGlFinishHack                  :: Bool
  m_pHMD                           :: Ptr{Nothing} # vr::IVRSystem
  m_strDriver                      :: String # std::string
  m_strDisplay                     :: String # std::string
  m_rTrackedDevicePose             :: SVector{64,OpenVR.TrackedDevicePose_t}
  m_rmat4DevicePose                :: SVector{64,Matrix4}
  m_rHand                          :: SVector{2,ControllerInfo_t}
  m_pCompanionWindow               :: Ptr{Nothing} # SDL_Window *
  m_nCompanionWindowWidth          :: Int32
  m_nCompanionWindowHeight         :: Int32
  m_pContext                       :: SDL.GLContext
  m_iTrackedControllerCount        :: Int32
  m_iTrackedControllerCount_Last   :: Int32
  m_iValidPoseCount                :: Int32
  m_iValidPoseCount_Last           :: Int32
  m_bShowCubes                     :: Bool
  m_vAnalogValue                   :: SVector{2,Float32}
  m_strPoseClasses                 :: String # std::string
  m_rDevClassChar                  :: SVector{64,Cchar} # k_unMaxTrackedDeviceCount
  m_iSceneVolumeWidth              :: Int32
  m_iSceneVolumeHeight             :: Int32
  m_iSceneVolumeDepth              :: Int32
  m_fScaleSpacing                  :: Float32
  m_fScale                         :: Float32
  m_iSceneVolumeInit               :: Int32
  m_fNearClip                      :: Float32
  m_fFarClip                       :: Float32
  m_iTexture                       :: GLuint
  m_uiVertcount                    :: UInt32
  m_glSceneVertBuffer              :: GLuint
  m_unSceneVAO                     :: GLuint
  m_unCompanionWindowVAO           :: GLuint
  m_glCompanionWindowIDVertBuffer  :: GLuint
  m_glCompanionWindowIDIndexBuffer :: GLuint
  m_uiCompanionWindowIndexSize     :: UInt32
  m_glControllerVertBuffer         :: GLuint
  m_unControllerVAO                :: GLuint
  m_uiControllerVertcount          :: UInt32
  m_mat4HMDPose                    :: Matrix4
  m_mat4eyePosLeft                 :: Matrix4
  m_mat4eyePosRight                :: Matrix4
  m_mat4ProjectionCenter           :: Matrix4
  m_mat4ProjectionLeft             :: Matrix4
  m_mat4ProjectionRight            :: Matrix4
  m_unSceneProgramID               :: GLuint
  m_unCompanionWindowProgramID     :: GLuint
  m_unControllerTransformProgramID :: GLuint
  m_unRenderModelProgramID         :: GLuint
  m_nSceneMatrixLocation           :: GLint
  m_nControllerMatrixLocation      :: GLint
  m_nRenderModelMatrixLocation     :: GLint
  leftEyeDesc                      :: FramebufferDesc
  rightEyeDesc                     :: FramebufferDesc
  m_nRenderWidth                   :: Int32
  m_nRenderHeight                  :: Int32
  m_vecRenderModels                :: Array{Ptr{CGLRenderModel},1} # std::vector< CGLRenderModel * >
  m_vecRenderModelsr               :: Array{Ref{CGLRenderModel},1} # std::vector< CGLRenderModel * >
  m_actionHideCubes                :: UInt64
  m_actionHideThisController       :: UInt64
  m_actionTriggerHaptic            :: UInt64
  m_actionAnalongInput             :: UInt64
  m_actionsetDemo                  :: UInt64
  CMainApplication() = new()
end


text:
@testset "https://github.com/aviatesk/JET.jl/issues/71" begin
    stats = @benchmark_freshexec ntimes = 1 @report_call println(QuoteNode(nothing))
    @test stats.time ≤ 10
end


text:
import ReverseDiff
import ReverseDiff: TrackedReal, InstructionTape, seed!, unseed!, reverse_pass!, deriv
using LinearAlgebra: logabsdet

##########################
# injective function DSL #
##########################

struct InjectiveFunction
    julia_function::Function
end

macro inj(ast)
    if ast.head != :function
        error("syntax error at $(ast) in $(ast.head)")
    end
    if length(ast.args) != 2
        error("syntax error at $(ast) in $(ast.args)")
    end
    signature = ast.args[1]
    body = ast.args[2]
    if signature.head != :call
        error("syntax error at $(ast) in $(signature)")
    end
    function_name = signature.args[1]
    args = signature.args[2:end]
    escaped_args = map(esc, args)
    fn_args = [esc(state), escaped_args...]
    Expr(:block,
        Expr(:(=),
            esc(function_name),
            Expr(:call, :InjectiveFunction,
                Expr(:function, Expr(:tuple, fn_args...), esc(body)))))
end

macro write(value, addr)
    Expr(:call, :write_to_addr, esc(state), esc(value), esc(addr))
end

macro copy(input_addr, output_addr)
    Expr(:call, :copy_addr, esc(state), esc(input_addr), esc(output_addr))
end

macro swap(value, input_addr, output_addr)
    Expr(:call, :swap, esc(state), esc(input_addr), esc(output_addr))
end

macro copyall(input_addr, output_addr)
    Expr(:call, :copyall, esc(state), esc(input_addr), esc(output_addr))
end

macro swapall(input_addr, output_addr)
    Expr(:call, :swapall, esc(state), esc(input_addr), esc(output_addr))
end

mutable struct InjectiveApplyState
    input::Any
    output::HomogenousTrie{Any,Any}
    tape::InstructionTape
    tracked_reads::HomogenousTrie{Any,TrackedReal}
    tracked_writes::HomogenousTrie{Any,TrackedReal}
    copied::AddressSet
    visitor::AddressVisitor
end

function InjectiveApplyState(input)
    output = HomogenousTrie{Any,Any}()
    tape = InstructionTape()
    tracked_reads = HomogenousTrie{Any,TrackedReal}()
    tracked_writes = HomogenousTrie{Any,TrackedReal}()
    copied = DynamicAddressSet()
    visitor = AddressVisitor()
    InjectiveApplyState(input, output, tape, tracked_reads, tracked_writes, copied, visitor)
end

function exec(fn::InjectiveFunction, state::InjectiveApplyState, args::Tuple)
    fn.julia_function(state, args...)
end

function maybe_track_value!(state::InjectiveApplyState, addr, value)
    value
end

function maybe_track_value!(state::InjectiveApplyState, addr, value::Float64)
    tracked_value = ReverseDiff.track(value, state.tape)
    set_leaf_node!(state.tracked_reads, addr, tracked_value)
    tracked_value
end

function read(state::InjectiveApplyState, addr)
    if has_leaf_node(state.tracked_reads, addr)
        # use the existing tracked read value
        get_leaf_node(state.tracked_reads, addr)::TrackedReal
    else
        value = get_leaf_node(state.input, addr)
        maybe_track_value!(state, addr, value)
    end
end

function write_to_addr(state::InjectiveApplyState, value, addr)
    visit!(state.visitor, addr)
    set_leaf_node!(state.output, addr, value)
    nothing
end

function write_to_addr(state::InjectiveApplyState, tracked_value::TrackedReal, addr)
    visit!(state.visitor, addr)
    # TODO what if user tries to write same tracked value to two diff addrs?
    set_leaf_node!(state.tracked_writes, addr, tracked_value)
    set_leaf_node!(state.output, addr, ReverseDiff.value(tracked_value))
    nothing
end

function copy_addr(state::InjectiveApplyState, input_addr, output_addr)
    visit!(state.visitor, output_addr)
    if !has_leaf_node(state.input, input_addr)
        error("Value at $input_addr not found in input ($(state.input))")
    end
    value = get_leaf_node(state.input, input_addr)
    set_leaf_node!(state.output, output_addr, value)
    push_leaf_node!(state.copied, input_addr)
    nothing
end

function swap(state::InjectiveApplyState, input_addr, output_addr)
    copy(state, input_addr, output_addr)
    copy(state, output_addr, input_addr)
end

function copyall(state::InjectiveApplyState, input_addr, output_addr)
    visit!(state.visitor, output_addr)
    if !has_internal_node(state.input, input_addr)
        error("Namespace at $input_addr not found in input")
    end
    node = get_internal_node(state.input, input_addr)
    set_internal_node!(state.output, output_addr, node)
    push_leaf_node!(state.copied, input_addr)
    nothing
end

function swapall(state::InjectiveApplyState, input_addr, output_addr)
    copyall(state, input_addr, output_addr)
    copyall(state, output_addr, input_addr)
end

function addr(state::InjectiveApplyState, fn::InjectiveFunction, args, output_addr)
    visit!(state.visitor, output_addr)
    sub_assignment = HomogenousTrie{Any,Any}()
    sub_tracked_writes = HomogenousTrie{Any,TrackedReal}()
    visitor = AddressVisitor()
    sub_state = InjectiveApplyState(state.input, sub_assignment,
        state.tape, state.tracked_reads, sub_tracked_writes, state.copied, visitor)
    value = exec(fn, sub_state, args)
    set_internal_node!(state.output, output_addr, sub_assignment)
    set_internal_node!(state.tracked_writes, output_addr, sub_tracked_writes)
    value
end

function splice(state::InjectiveApplyState, fn::InjectiveFunction, args::Tuple)
    exec(fn, state, args)
end

function apply(fn::InjectiveFunction, args::Tuple, input)
    state = InjectiveApplyState(input)
    val = exec(fn, state, args)

    # addresses that were copied are excluded from inputs
    # they can be ignored when computing Jacobian determinant
    # this is a performance optimization
    delete!(state.tracked_reads, state.copied)
    tracked_inputs = collect(values(state.tracked_reads))
    tracked_outputs = collect(values(state.tracked_writes))
    n = length(tracked_inputs)
    if n != length(tracked_outputs)
        error("input and output dimensions differ")
    end
    jacobian = zeros(n, n)
    for (i, tracked_output) in enumerate(tracked_outputs)
        seed!(tracked_output)
        reverse_pass!(state.tape)
        for (j, tracked_input) in enumerate(tracked_inputs)
            jacobian[i, j] = deriv(tracked_input)
            unseed!(tracked_input)
        end
        unseed!(tracked_output)
    end
    (log_abs_det, det_sign) = logabsdet(jacobian)
    (state.output, log_abs_det, val)
end

export @inj
export @write, @copy, @swap, @copyall, @swapall, apply

max_stars_repo_name: BoltzmannBrain/Gen

max_issues_repo_name: BoltzmannBrain/Gen

max_forks_repo_name: BoltzmannBrain/Gen


text:
struct FundamentalConstants{C,Q,M,E,Mu,U}
    c::C
    q::Q
    mₑ::M
    ε₀::E
    μ₀::Mu
    unit_system::U
end

fundamental_constants(::Val{:SI_unitful}) = c_0, -e, m_e, ε_0, μ_0
fundamental_constants(::Val{:SI}) = ustrip.(fundamental_constants(Val(:SI_unitful)))
fundamental_constants(::Val{:atomic_unitful}) = auconvert.(fundamental_constants(Val(:SI_unitful)))
fundamental_constants(::Val{:atomic}) = ustrip.(fundamental_constants(Val(:atomic_unitful)))

function FundamentalConstants(units::Symbol)
    constants = fundamental_constants(Val(units))
    FundamentalConstants(constants..., units)
end