text:
# MIT License
# Copyright (c) 2017: Xavier Gandibleux, Anthony Przybylski, Gauthier Soleilhac, and contributors.
function solve_lexico(m::Model, verbose; args...)
    #Retrieve objectives and their senses from vOptData
    vd = getvOptData(m)
    empty!(vd.Y_N) ; empty!(vd.X_E)
    objs = vd.objs
    objSenses = vd.objSenses
    nbObj = length(objs)

    #Check that problem is feasible and that no objective is unbounded
    for i = 1:nbObj
        m.obj = objs[i]
        m.objSense = objSenses[i]
        status = @suppress solve(m, ignore_solve_hook=true ; args...)
        status != :Optimal && return status
    end

    #Create the constraints on the objectives
    #JuMP doesn't support changing constraint coefficients so we use >= and <= constraints,
    #and typemin/typemax to deactivate the constraints
    @constraintref cstr_obj[1:nbObj]
    for i = 1:nbObj
        if objSenses[i] == :Max
            cstr_obj[i] = @constraint(m, objs[i].aff >= -1e16)
        else
            cstr_obj[i] = @constraint(m, objs[i].aff <= 1e16)
        end
    end

    for p in permutations(1:nbObj, nbObj)
        verbose && println("solving for objectives $p")
        solve_permutation(m, p, cstr_obj ; args...)
    end

    return :Optimal
end

function solve_permutation(m::Model, p, cstr_obj ; args...)

    vd = getvOptData(m)
    objs = vd.objs
    objSenses = vd.objSenses

    #Set the first objective of the permutation as an objective in the JuMP model
    m.obj = objs[p[1]]
    m.objSense = objSenses[p[1]]

    #Solve with that objective
    status = @suppress solve(m, ignore_solve_hook=true; args...)
    status != :Optimal && return status

    for i = 2:length(p)
        fVal = m.objVal #get the value for the last objective solved
        slack = objSenses[p[i-1]] == :Max ? -1e-6 : 1e-6
        JuMP.setRHS(cstr_obj[p[i-1]], fVal - objs[p[i-1]].aff.constant + slack) #set the constraint for the last objective solved
        m.obj = objs[p[i]] #set the i-th objective of the permutation in the JuMP model
        m.objSense = objSenses[p[i]]
        @suppress solve(m, ignore_solve_hook=true ; args...) #and solve
    end

    varArray = [JuMP.Variable(m,i) for i in 1:MathProgBase.numvar(m)]
    #Store results in vOptData
    push!(vd.Y_N, map(JuMP.getvalue, objs))
    push!(vd.X_E, JuMP.getvalue.(varArray))

    for i = 1:length(p)
        if objSenses[i] == :Max
            JuMP.setRHS(cstr_obj[i], -1e16)
        else
            JuMP.setRHS(cstr_obj[i], 1e16)
        end
    end

    nothing
end

function solve_eps(m::Model, ϵ::Float64, round_results, verbose ; args...)
    #Retrieve objectives and their senses from vOptData
    vd = getvOptData(m)
    empty!(vd.Y_N) ; empty!(vd.X_E)
    f1,f2 = vd.objs[1],vd.objs[2]
    f1Sense,f2Sense = vd.objSenses[1],vd.objSenses[2]

    #Set the first objective as an objective in the JuMP model
    m.obj=f1
    m.objSense = f1Sense

    #Solve with that objective
    status = solve(m, ignore_solve_hook=true; args...)

    #If a solution exists
    if status == :Optimal

        #Declare the epsilon-constraint (RHS will be set later)
        if f2Sense == :Min
            eps = @constraint(m, f2.aff <= 0.0)
        else
            eps = @constraint(m, f2.aff >= 0.0)
        end

        varArray = [JuMP.Variable(m,i) for i in 1:MathProgBase.numvar(m)]

        #While a solution exists
        while status == :Optimal
            #Get the score on the objectives
            f1Val = round_results ? round(m.objVal) : m.objVal
            f2Val = round_results ? round(JuMP.getvalue(f2)) : JuMP.getvalue(f2)

            R1 = f1Sense==:Min ? (<=) : (>=)
            R2 = f2Sense==:Min ? (<=) : (>=)
            weak_dom(a, b) = R1(a[1], b[1]) && R2(a[2], b[2])

            #If last solution found is dominated by this one
            if !isempty(vd.Y_N)
                Y_m1 = vd.Y_N[end]
                if weak_dom((f1Val, f2Val), Y_m1)
                    #Remove last solution from Y_N and X_E
                    pop!(vd.Y_N) ; pop!(vd.X_E)
                end
            end

            #Store results in vOptData
            push!(vd.X_E, JuMP.getvalue.(varArray))
            push!(vd.Y_N, [f1Val, f2Val])

            verbose && print("z1 = ", f1Val, ", z2 = ", f2Val)
            #Set the RHS of the epsilon-constraint
            if f2Sense == :Min
                JuMP.setRHS(eps, f2Val - f2.aff.constant - ϵ)
                verbose && println(". Solving with f2 <= ", f2Val - ϵ)
            else
                JuMP.setRHS(eps, f2Val - f2.aff.constant + ϵ)
                verbose && println(". Solving with f2 >= ", f2Val + ϵ)
            end

            #And solve again
            status = solve(m, ignore_solve_hook=true, suppress_warnings=true ; args...)
        end

        #Sort X_E and Y_N
        s = sortperm(vd.Y_N, by = e -> e[1])
        vd.Y_N = vd.Y_N[s]
        vd.X_E = vd.X_E[s]

        ###
        #JuMP doesn't support removing constraints
        #To leave the model unaltered we change the value of the RHS
        ###
        if f2Sense == :Min
            JuMP.setRHS(eps, 1e16)
        else
            JuMP.setRHS(eps,-1e16)
        end
    else
        return status
    end
    return :Optimal
end

function solve_dicho(m::Model, round_results ; args...)
    vd = getvOptData(m)
    empty!(vd.Y_N) ; empty!(vd.X_E)
    f1,f2 = vd.objs[1],vd.objs[2]
    f1Sense,f2Sense = vd.objSenses[1],vd.objSenses[2]
    varArray = [JuMP.Variable(m,i) for i in 1:MathProgBase.numvar(m)]

    #Set the first objective as an objective in the JuMP model
    m.obj=f1
    m.objSense = f1Sense

    #Solve with that objective
    status = solve(m, ignore_solve_hook=true ;  args...)

    #If a solution exists
    if status == :Optimal

        yr_1 = m.objVal
        yr_2 = JuMP.getvalue(f2)

        #Store results in vOptData
        push!(vd.Y_N, round_results ? round.([yr_1, yr_2]) : [yr_1, yr_2])
        push!(vd.X_E, JuMP.getvalue.(varArray))

        #Set the second objective as an objective in the JuMP model
        m.obj = f2
        m.objSense = f2Sense

        #Solve with that objective
        status = solve(m, ignore_solve_hook=true ; args...)

        if status == :Optimal

            ys_1 = JuMP.getvalue(f1)
            ys_2 = m.objVal

            if !isapprox(yr_1, ys_1, atol=1e-3) || !isapprox(yr_2, ys_2, atol=1e-3)
                push!(vd.Y_N, round_results ? round.([ys_1, ys_2]) : [ys_1, ys_2])
                push!(vd.X_E, JuMP.getvalue.(varArray))
                dichoRecursion(m, yr_1, yr_2, ys_1, ys_2, varArray, round_results ; args...)
            end

            #Sort X_E and Y_N
            s = sortperm(vd.Y_N, by = first)
            vd.Y_N = vd.Y_N[s]
            vd.X_E = vd.X_E[s]

            R1 = f1Sense==:Min ? (<=) : (>=)
            R2 = f2Sense==:Min ? (<=) : (>=)
            weak_dom(a, b) = R1(a[1], b[1]) && R2(a[2], b[2])

            #Filter X_E and Y_N :
            inds = Int[]
            for i = 1:length(vd.Y_N)-1
                if weak_dom(vd.Y_N[i], vd.Y_N[i+1])
                    push!(inds, i+1)
                elseif weak_dom(vd.Y_N[i+1], vd.Y_N[i])
                    push!(inds, i)
                end
            end
            deleteat!(vd.Y_N, inds)
            deleteat!(vd.X_E, inds)
        end
    end

    status
end

function dichoRecursion(m::Model, yr_1, yr_2, ys_1, ys_2, varArray, round_results ; args...)

    vd = getvOptData(m)
    f1,f2 = vd.objs[1],vd.objs[2]
    f1Sense,f2Sense = vd.objSenses[1],vd.objSenses[2]

    λ1 = abs(yr_2 - ys_2)
    λ2 = abs(ys_1 - yr_1)

    m.objSense = f1Sense

    if f1Sense==f2Sense
        lb = λ1*yr_1 + λ2*yr_2
        m.obj = λ1*f1 + λ2*f2
    else
        lb = λ1*yr_1 - λ2*yr_2
        m.obj = λ1*f1 - λ2*f2
    end

    solve(m, ignore_solve_hook=true ; args...)

    yt_1 = JuMP.getvalue(f1)
    yt_2 = JuMP.getvalue(f2)

    if f1Sense==f2Sense
        val = λ1*yt_1 + λ2*yt_2
    else
        val = λ1*yt_1 - λ2*yt_2
    end

    if (f1Sense == :Min && val < lb - 1e-4) || val > lb + 1e-4
        push!(vd.Y_N, round_results ? round.([yt_1, yt_2]) : [yt_1, yt_2])
        push!(vd.X_E, JuMP.getvalue.(varArray))
        dichoRecursion(m, yr_1, yr_2, yt_1, yt_2, varArray, round_results ; args...)
        dichoRecursion(m, yt_1, yt_2, ys_1, ys_2, varArray, round_results ; args...)
    end

end

function solve_Chalmet(m::Model, step ; args...)
    vd = getvOptData(m)
    empty!(vd.Y_N) ; empty!(vd.X_E)
    f1,f2 = vd.objs[1],vd.objs[2]
    f1Sense,f2Sense = vd.objSenses[1],vd.objSenses[2]
    varArray = [JuMP.Variable(m,i) for i in 1:MathProgBase.numvar(m)]

    #Set the first objective as an objective in the JuMP model
    m.obj=f1
    m.objSense = f1Sense

    #Solve with that objective
    status = solve(m, ignore_solve_hook=true ; args...)

    #If a solution exists
    if status == :Optimal

        yr_1 = m.objVal
        yr_2 = JuMP.getvalue(f2)

        #Store results in vOptData
        push!(vd.Y_N, [yr_1, yr_2])
        push!(vd.X_E, JuMP.getvalue.(varArray))

        #Set the second objective as an objective in the JuMP model
        m.obj=f2
        m.objSense = f2Sense

        #Solve with that objective
        status = solve(m, ignore_solve_hook=true ; args...)

        if status == :Optimal

            ys_1 = JuMP.getvalue(f1)
            ys_2 = m.objVal

            if !isapprox(yr_1, ys_1, atol=1e-3) || !isapprox(yr_2, ys_2, atol=1e-3)
                push!(vd.Y_N, [ys_1, ys_2])
                push!(vd.X_E, JuMP.getvalue.(varArray))
                #Declare the constraints on z1 and z2 (RHS will be set later)
                if f1Sense == :Min
                    cstrz1 = @constraint(m, f1.aff <= 0.0)
                else
                    cstrz1 = @constraint(m, f1.aff >= 0.0)
                end
                if f2Sense == :Min
                    cstrz2 = @constraint(m, f2.aff <= 0.0)
                else
                    cstrz2 = @constraint(m, f2.aff >= 0.0)
                end
                ChalmetRecursion(m, yr_1, yr_2, ys_1, ys_2, varArray, cstrz1, cstrz2, step ; args...)

                #Sort X_E and Y_N
                s = sortperm(vd.Y_N, by = e -> e[1])
                vd.Y_N = vd.Y_N[s]
                vd.X_E = vd.X_E[s]

                #We can use weak dominance definition here
                dom_min_min(a,b) = a[1] <= b[1] && a[2] <= b[2]
                dom_max_max(a,b) = a[1] >= b[1] && a[2] >= b[2]
                dom_min_max(a,b) = a[1] <= b[1] && a[2] >= b[2]
                dom_max_min(a,b) = a[1] >= b[1] && a[2] <= b[2]

                if f1Sense==f2Sense==:Min
                    dominates = dom_min_min
                elseif f1Sense==f2Sense==:Max
                    dominates = dom_max_max
                elseif f1Sense==:Min
                    dominates = dom_min_max
                else
                    dominates = dom_max_min
                end

                #Filter X_E and Y_N :
                inds = Int[]
                for i = 1:length(vd.Y_N)-1
                    if dominates(vd.Y_N[i], vd.Y_N[i+1])
                        push!(inds, i+1)
                    elseif dominates(vd.Y_N[i+1], vd.Y_N[i])
                        push!(inds, i)
                    end
                end
                deleteat!(vd.Y_N, inds)
                deleteat!(vd.X_E, inds)

                if f1Sense == :Min
                    JuMP.setRHS(cstrz1, 1e16)
                else
                    JuMP.setRHS(cstrz1,-1e16)
                end

                if f2Sense == :Min
                    JuMP.setRHS(cstrz2, 1e16)
                else
                    JuMP.setRHS(cstrz2,-1e16)
                end
            end
        end
    end

    return status
end

function ChalmetRecursion(m::Model, yr_1, yr_2, ys_1, ys_2, varArray, cstrz1, cstrz2, ϵ ; args...)

    vd = getvOptData(m)
    f1,f2 = vd.objs[1],vd.objs[2]
    f1Sense,f2Sense = vd.objSenses[1],vd.objSenses[2]

    m.objSense = f1Sense
    if f1Sense==f2Sense
        m.obj = f1 + f2
    else
        m.obj = f1 - f2
    end

    lbz1 = f1Sense==:Max ? min(yr_1, ys_1) : max(yr_1, ys_1)
    lbz2 = f2Sense==:Max ? min(yr_2, ys_2) : max(yr_2, ys_2)

    if f1Sense == :Min
        JuMP.setRHS(cstrz1, lbz1 - f1.aff.constant - ϵ)
        print(". Solving with f1 <= ", lbz1 - ϵ)
    else
        JuMP.setRHS(cstrz1, lbz1 - f1.aff.constant + ϵ)
        print(". Solving with f1 >= ", lbz1 + ϵ)
    end

    if f2Sense == :Min
        JuMP.setRHS(cstrz2, lbz2 - f2.aff.constant - ϵ)
        println(" and f2 <= ", lbz2 - ϵ)
    else
        JuMP.setRHS(cstrz2, lbz2 - f2.aff.constant + ϵ)
        println(" and f2 >= ", lbz2 + ϵ)
    end

    status = @suppress solve(m, ignore_solve_hook=true; args...)

    if status == :Optimal

        yt_1 = JuMP.getvalue(f1)
        yt_2 = JuMP.getvalue(f2)

        push!(vd.Y_N, [yt_1, yt_2])
        push!(vd.X_E, JuMP.getvalue.(varArray))
        ChalmetRecursion(m, yr_1, yr_2, yt_1, yt_2, varArray, cstrz1, cstrz2, ϵ ; args...)
        ChalmetRecursion(m, yt_1, yt_2, ys_1, ys_2, varArray, cstrz1, cstrz2, ϵ ; args...)

    end
end


text:
module _Inplace

using Brainstorm._Math._GMP: set!, add!, sub!, mul!, div!, neg!, isqrt!, rsh!, lsh!

const guard_digits = 5
const eps_digits = 5

pi(digits::Integer) = begin
    o = big(10)^(digits + guard_digits + eps_digits)
    osqr = o^2
    t = big(0)

    # a = 1
    a = big(0)
    set!(a, o)

    # b = 1 / sqrt(2)
    set!(t, osqr)
    lsh!(t, 1)
    isqrt!(t)
    b = osqr ÷ t

    s = big(0)
    d = a - b

    n = 0
    ϵ = 10^eps_digits

    while d >= ϵ
        # s += 2^n * d^2
        mul!(d, d)
        lsh!(d, n)
        add!(s, d)

        # a, b = (a + b) / 2, sqrt(a * b)
        set!(t, a)
        add!(t, b)
        rsh!(t, 1)

        mul!(b, a)
        isqrt!(b)
        set!(a, t)

        # d = a - b
        set!(d, a)
        sub!(d, b)

        n += 1
    end

    # 4*a^2 / (1 - s)
    neg!(s)
    add!(s, osqr)
    mul!(s, big(10)^(guard_digits + eps_digits))
    mul!(a, a)
    mul!(a, o)
    lsh!(a, 2)
    div!(a, s)
end

end

ext: jl


text:
using Documenter, Distributions
import Random: AbstractRNG, rand!

makedocs(
    sitename = "Distributions.jl",
    modules  = [Distributions],
    doctest  = false,
    pages    = [
        "index.md",
        "starting.md",
        "types.md",
        "univariate.md",
        "truncate.md",
        "multivariate.md",
        "matrix.md",
        "cholesky.md",
        "mixture.md",
        "fit.md",
        "extends.md",
    ]
)

deploydocs(;
    repo = "github.com/JuliaStats/Distributions.jl.git",
    versions = ["stable" => "v^", "v#.#", "dev" => "master"],
    push_preview=true,
)


text:
cd(@__DIR__); include("setups/grid23x22.jl")
gr(dpi = 200)

## assemble frames
frame = sgwt_frame(W; nf = 6)
SGWT = reshape(frame, (N, :))
SGWT_dual = (SGWT * SGWT') \ SGWT

rNGWF, dic_l2x = rngwf_all_vectors(D, 𝚽; σ = 0.2 * maximum(D), thres = 0.15)
rNGWF_dual = (rNGWF * rNGWF') \ rNGWF
Γ = rngwf_lx(dic_l2x)

## (a)
f = digit_img[:]
plt = heatmap(reshape(f, (Nx, Ny))', c = :viridis, ratio = 1,
        frame = :none, xlim = [1, Nx], size = (600, 500))
savefig(plt, "../figs/grid23x22_fdigit3.png")

## (b)
rel_approx_sgwt, f_approx_sgwt = frame_approx(f, SGWT, SGWT_dual; num_kept = 3 * N)
rel_approx_rngwf, f_approx_rngwf = frame_approx(f, rNGWF, rNGWF_dual; num_kept = 3 * N)
plot(0:length(rel_approx_rngwf)-1, [rel_approx_sgwt rel_approx_rngwf],
    grid = false, lw = 2, c = [:blue :green],
    xlab = "Number of Coefficients Retained",
    ylab = "Relative Approximation Error", yaxis = :log,
    lab = ["SGWT" "rNGWF"])
plt = plot!(xguidefontsize = 14, yguidefontsize = 14, legendfontsize = 11, size = (600, 500))
savefig(plt, "../figs/grid23x22_approx_fdigit3.png")


text:
using GeoArrays
using Test

@testset "GeoArrays" begin
    cd(dirname(@__FILE__)) do
        include("get_testdata.jl")
        include("test_geoutils.jl")
        include("test_io.jl")
        include("test_interpolate.jl")
        include("test_crs.jl")
        include("test_operations.jl")
    end
end


text:
module Perf
using Test
using BenchmarkTools
using VoxelRayTracers

using BenchmarkTools

function _count(itr)
    ret = 0
    for x in itr
        ret += 1
    end
    return ret
end

position = [0.01,-100, -100]
velocity = [0.001, 1,1]
#edgs = (-2:100.0, -50:50.0, sort!(randn(100)))
edgs = (-2:100.0, -50:50.0, -100:1:100)
for dim in 1:3
    ray = (position=position[1:dim], velocity=velocity[1:dim])
    itr = @inferred eachtraversal(ray, edgs[1:dim])
    @show dim
    b = @benchmark _count($itr)
    display(b)
    @test b.allocs == 0
    @show _count(itr)
end

# position = [0.01,-100, -100]
# velocity = [0.001, 1,1]
# edgs = (-2:100.0, -50:50.0, sort!(randn(100)))
# ray = (position=position, velocity=velocity)
# itr = eachtraversal(ray, edgs)
# function doit(itr)
#     ret = 0.0
#     for hit in itr
#         ret += hit.entry_time
#     end
#     ret
# end
#
#
#

@testset "eachtraversal no allocs $dim" for dim in 1:3
    nwalls = 20
    edges = Tuple(range(-10, stop=10, length=nwalls) for _ in 1:dim)
    ray = (position=randn(dim), velocity=randn(dim))
    data = randn((nwalls - 1 for _ in 1:dim)...)

    function raysum(ray, edges, data)
        out = 0.0
        for i in 1:1000
            for hit in eachtraversal(ray, edges)
                out += data[hit.voxelindex]
            end
        end
        out
    end

    b = @benchmark $raysum($ray, $edges, $data) samples=2 evals=2
    display(b)
    @test b.allocs < 100
end

end#module


text:
using Compat.Test
import Roots.fzero

# test robustness of derivative free algorithms

## tests:http://people.sc.fsu.edu/~jburkardt/cpp_src/test_zero/test_zero.html
function newton_baffler(x)
    if ( x - 0.0 ) < -0.25
        0.75 * ( x - 0 ) - 0.3125
    elseif  ( x - 0 ) < 0.25
        2.0 * ( x - 0 )
    else
        0.75 * ( x - 0 ) + 0.3125
    end
end

pathological = [
                (x ->sin( x ) - x / 2, .1),
                (x-> 2 * x - exp( - x ), 1),
                (x -> x * exp( - x ), 1),
                (x -> exp( x ) - 1 / ( 10 * x )^2, 1),
                (x -> ( x + 3 ) * ( x - 1 )^2, 1),

                (x -> exp( x ) - 2 - 1 / ( 10 * x )^2 + 2 / ( 100 * x )^3, 1),
                (x -> cbrt(x), 1),
                (x -> cos( x ) - x, 1),
                (x-> newton_baffler(x), 8),
                (x -> 20.0 * x / ( 100.0 * x^2 + 1.0), 0.095),

                (x ->  ( 4.0 + x^2) * ( 2.0 + x ) * ( 2.0 - x )  / ( 16.0 * x * x * x * x + 0.00001 ), 1),
                (x -> (x == 1.0) ? float(0) : sign(x-1.0) * exp(log(1e4) + log(abs(x - 1.0)) - 1.0/(x-1.0)^2), 1),
                (x -> 0.00000000001 * (x - 100.0), 1),
                (x -> 1.0 / ( ( x - 0.3 ) * ( x - 0.3 ) + 0.01 ) + 1.0 / ( ( x - 0.9 ) * ( x - 0.9 ) + 0.04 ) + 2.0 * x - 5.2, -1),
                (x -> ( 1 - 6x^2) * cbrt(x) * exp(-x^2) / (3*x), -0.25),

                (x -> ( pi * ( x - 5.0 ) / 180.0 ) - 0.8 * sin( pi * x / 180.0 ), 1),
                (x -> x^3 - 2*x - 5, 2),
                (x -> 1e6 * (x^7 -7x^6 +21x^5 -35x^4 +35x^3-21x^2+7x-1),  0.990),
                (x -> cos(100*x)-4*erf(30*x-10), 0.0)
                ]

for (f1, x0) in pathological

      try
          x = fzero(f1, x0)
          @assert (f1(x) == 0.0 || f1(prevfloat(x)) * f1(nextfloat(x)) <= 0 || abs(f1(x)) <= eps(float(x0))^(1/2))
      catch err
#          rethrow(err)
#          if !isa(err, Roots.PossibleExtremaReached)
#              throw(err)
#          end
      end
end

## make a graphic comparing values
function run_robustness_test()
    orders = [0,1,2,5,8,16]
    N = length(orders)
    k,n = length(pathological), 50
    m = ["" for i=1:(N + 2)*k, j=1:n]
    ## how to mark values. If error we also use "x"
    function howgood(f1, xstar)
        if f1(xstar) == 0       # perfect
            "."
        elseif f1(prevfloat(xstar)) * f1(nextfloat(xstar)) < 0 # perfect
        "-"
        elseif -1e-16 < f1(xstar) < 1e-16 # approximate
            "~"
        elseif abs(xstar) > 5   # ran away yet converged
            "!"
        else                    # not close?
            "*"
        end
    end

    for i in 1:k
        f1,x = pathological[i]
        xs = linspace(-5, 5, n)
        for k in 1:n
            for j in 1:length(orders)
                try
                    xstar = fzero(f1, xs[k], order=orders[j], maxeval=100)
                    m[(N+1)*(i-1)+j, k] = howgood(f1, xstar)
                catch e
                    m[(N+1)*(i-1)+j, k] = "x"
                end
            end
        end
    end
    println("\n----")
    print(join(mapslices(join, m, 2),"\n"))
end


text:
function time_update(filter::SquareRootFilter, x, k)
    F = jacobian_dynamics_x(filter, x, k)
    v = get_process_noise(filter, k) # Zero mean RV
    G_v = jacobian_dynamics_v(filter, x, k)

    Q_sqrt = chol(v)
    P_sqrt = chol(x)

    x_pred = predict_x(filter, x, k)

    L = lq(hcat(F*P_sqrt, G_v*Q_sqrt)).L
    n_x = size(F, 1)
    P_pred_sqrt = L[1:n_x, 1:n_x]

    return MvNormal(x_pred, P_pred_sqrt)
end
function measurement_update(filter::SquareRootFilter, y, x_pred, k)

    H = get_H(filter, k)
    P_pred_sqrt = chol(x_pred)
    R_sqrt = chol(y)

    y_pred = H*mean(x_pred) # Include other input and shit
    e = mean(y) - y_pred # Innovation

    n_x = size(P_pred_sqrt, 1)
    n_y = size(R_sqrt, 1)
    z = zeros(n_x, n_y)
    M = vcat(
        hcat(R_sqrt, H*P_pred_sqrt),
        hcat(z,  P_pred_sqrt)
    )
    L = lq(M).L

    S_sqrt = L[1:n_y, 1:n_y]
    P_sqrt = L[1+n_y:n_y + n_x, 1+n_y : n_y + n_x]
    KS_sqrt = L[1 + n_y:n_x + ny, 1:n_y]

    K = KS_sqrt/S_sqrt # Should be fast since triangular

    x_filtered = mean(x_pred) + K*e

    return MvNormal(x_filtered, P_sqrt), MvNormal(e, S_sqrt)
end


text:
alphas = PROWIMData.props_off["lift_distribution_alpha"]
ploti = 1:length(alphas)
wing_b = PROWIMData.wing["span"]
wing_TR = PROWIMData.wing["TR"] # 1.0 # hershey bar
wing_c = PROWIMData.wing["chord"] # 240e-3
wing_AR = wing_b / wing_c
wing_θroot = PROWIMData.wing["theta_root"]
wing_θtip = PROWIMData.wing["theta_tip"]
wing_le_sweep = PROWIMData.wing["le_sweep"]
wing_ϕ = PROWIMData.wing["dihedral"]

plot_base_name = "PROWIM_props_off"
data_PROWIM_lift_distribution = AS.lift_distribution_template(ploti, alphas, wing_b, wing_TR, wing_AR, wing_θroot, wing_θtip, wing_le_sweep, wing_ϕ;
    plot_directory = joinpath(AS.topdirectory, "data","plots",AS.TODAY),
    plot_base_name = plot_base_name,
    plot_extension = ".pdf",
    step_symbol = LS.L"\alpha ",
    surfacenames = ["PROWIM, props off"]
)
objective = AS.runsimulation!(data_PROWIM_lift_distribution...)

prowim_lift_distribution_props_off = PROWIMData.props_off["lift_distribution"]

fig = plt.figure(plot_base_name * "_cf_distribution")
fig_lift = plt.figure(plot_base_name * "_lift_distribution")
axs = fig.get_axes()
ax_lift = fig_lift.get_axes()[1]
alpha_labels = Int.(round.(alphas .* 180/pi, digits=0))
for (idata, data) in enumerate(prowim_lift_distribution_props_off)
    cratio = idata / length(alpha_labels)
    axs[3].scatter(data[:,1], data[:,2], marker = "x", color = (0.05, 0.85-cratio*0.7, 0.15 + 0.75 * cratio), label=LS.L"PROWIM, \alpha = " * "$(alpha_labels[idata])" * LS.L"^\circ")
    ax_lift.scatter(data[:,1] ./ wing_b * 2, data[:,2], marker = "x", color = (0.05, 0.85-cratio*0.7, 0.15 + 0.75 * cratio), label=LS.L"PROWIM, \alpha = " * "$(alpha_labels[idata])" * LS.L"^\circ")
end
axs[3].legend(loc="upper left", bbox_to_anchor=(1.01,1))
ax_lift.legend(loc="upper left", bbox_to_anchor=(1.01,1))
fig.tight_layout()
fig.savefig(joinpath(plot_directory,"PROWIM_props_off_lift_distribution.pdf"), bbox_inches="tight")
fig.tight_layout()
fig.savefig(joinpath(plot_directory,"PROWIM_props_off_lift_distribution.pdf"), bbox_inches="tight")
# fig.savefig(joinpath(notebookdirectory,"PROWIM_propsoff_lift_distribution.pdf"), bbox_inches="tight")

# @test objective == 0

max_stars_repo_name: byuflowlab/AircraftSystems

max_issues_repo_name: byuflowlab/AircraftSystems

max_forks_repo_name: byuflowlab/AircraftSystems


text:
#cfg title = "Using the Visualizer"
#cfg weight = 13
#cfg active = true
#cfg deps = ["visualize.png"]

#md # ## Overview
# In this example, we walk through how to use LyceumMuJoCoViz.jl to playback saved
# trajectories and interact with a saved policy in real time using
# the policy we learned in the "Learning a control policy" example.

#md # ## The Code

# First, let's go head and grab all the dependencies:
using LyceumAI         # For `NaturalPolicyGradient` and `DiagGaussianPolicy`
using Shapes           # For the `allocate` function
using LyceumMuJoCo     # For the HopperV2 environment
using LyceumMuJoCoViz  # For the visualizer itself
using FastClosures     # For helping avoid performance issues with closures, discussed below
using JLSO             # For loading saved data

# Here we demonstrate two modes of visualizing results of an algorithm like
# `NaturalPolicyGradient` or `MPPI` from `LyceumAI`: playing back saved trajectories and
# interacting with a policy or controller in real time. For the former, we need only pass
# to the `visualize` function our saved trajectories as a vector of matricies, where each
# element of the vector is a matrix of size `(length(statespace(env)), T)`, where `T` is
# the length the trajectory. Note that each trajectory can be of a different length.
# For the latter, we pass a control callback to `visualize` that will be called each
# time `step!(env)` is called.
function viz_hopper_NPG()
    ## Load our experiment results
    x = JLSO.load("/tmp/hopper_example.jlso")

    env = LyceumMuJoCo.HopperV2()

    ## Load the states from our saved trajectory, as well as the learned policy.
    states = x["stocstates"].states
    pol = x["policy"]

    ## Allocate some buffers for our control callback.
    a = allocate(actionspace(env))
    o = allocate(obsspace(env))

    ## As discussed in the Julia performance tips, captured variables
    ## (e.g. in a closure) can sometimes hinder performance. To help with that,
    ## we use `let` blocks as suggested.
    ctrlfn = let o = o, a = a, pol = pol
        function (env)
            getobs!(o, env)
            a .= pol(o)
            setaction!(env, a)
        end
    end

    visualize(env, controller = ctrlfn, trajectories = states)
end

# Now just call the function to see the visualizer appear!
#md # ```julia
#md # viz_hopper_NPG()
#md # ```
#jl viz_hopper_NPG()
#nb viz_hopper_NPG()

#md # You should see the following:
#md # ![visualizer](visualize.png)


text:
@testset "NodeResult" begin end

@testset "CurrentState" begin
    fm = _build_dmip_formulation()

    nvars = Cerberus.num_variables(fm)
    pb_float = 12.4
    pb_int = 12

    cs1 = @inferred _CurrentState()
    cs2 = @inferred _CurrentState(primal_bound = pb_float)
    cs3 = @inferred _CurrentState(primal_bound = pb_int)

    @test length(cs1.tree) == 1
    @test length(cs2.tree) == 1
    @test length(cs3.tree) == 1

    @test Cerberus._is_root_node(Cerberus.pop_node!(cs1.tree))
    @test Cerberus._is_root_node(Cerberus.pop_node!(cs2.tree))
    @test Cerberus._is_root_node(Cerberus.pop_node!(cs3.tree))

    @test cs1.primal_bound == Inf
    @test cs2.primal_bound == pb_float
    @test cs3.primal_bound == pb_int

    @test cs1.dual_bound == -Inf
    @test cs2.dual_bound == -Inf
    @test cs3.dual_bound == -Inf

    @test isempty(cs1.best_solution)
    @test isempty(cs2.best_solution)
    @test isempty(cs3.best_solution)

    @test cs1.total_node_count == 0
    @test cs2.total_node_count == 0
    @test cs3.total_node_count == 0

    @test cs1.total_simplex_iters == 0
    @test cs2.total_simplex_iters == 0
    @test cs3.total_simplex_iters == 0
end

@testset "copy(::Basis)" begin
    src = Cerberus.Basis(
        [MOI.NONBASIC_AT_LOWER, MOI.BASIC, MOI.NONBASIC_AT_LOWER],
        [MOI.BASIC],
        MOI.BasisStatusCode[],
        [MOI.NONBASIC],
        MOI.BasisStatusCode[],
        [MOI.NONBASIC_AT_UPPER],
    )
    dest = copy(src)
    @test src.base_var_constrs == dest.base_var_constrs
    @test src.base_lt_constrs == dest.base_lt_constrs
    @test src.base_gt_constrs == dest.base_gt_constrs
    @test src.base_et_constrs == dest.base_et_constrs
    @test src.branch_lt_constrs == dest.branch_lt_constrs
    @test src.branch_gt_constrs == dest.branch_gt_constrs
    empty!(src.base_var_constrs)
    empty!(src.base_lt_constrs)
    empty!(src.base_gt_constrs)
    empty!(src.base_et_constrs)
    empty!(src.branch_lt_constrs)
    empty!(src.branch_gt_constrs)
    @test dest.base_var_constrs ==
          [MOI.NONBASIC_AT_LOWER, MOI.BASIC, MOI.NONBASIC_AT_LOWER]
    @test dest.base_lt_constrs == [MOI.BASIC]
    @test isempty(dest.base_gt_constrs)
    @test dest.base_et_constrs == [MOI.NONBASIC]
    @test isempty(dest.branch_lt_constrs)
    @test dest.branch_gt_constrs == [MOI.NONBASIC_AT_UPPER]
end

@testset "reset_formulation_state!" begin
    form = _build_formulation_with_single_disjunction()
    state = Cerberus.CurrentState()
    node = Cerberus.Node(
        [Cerberus.BoundUpdate(_CVI(5), _LT(0.0))],
        [Cerberus.BoundUpdate(_CVI(3), _GT(1.0))],
        [Cerberus.AffineConstraint(_CSAF([2.3], [_CVI(2)], 0.0), _LT(2.3))],
        [Cerberus.AffineConstraint(_CSAF([2.3], [_CVI(2)], 0.0), _GT(2.3))],
        4,
    )
    node_result = Cerberus.NodeResult(node)
    Cerberus.populate_lp_model!(state, form, node, node_result, CONFIG)
    Cerberus.apply_branchings!(state, node)
    Cerberus.formulate_disjunctions!(state, form, node, node_result, CONFIG)
    @test length(state._variable_indices) == 5
    @test length(state.constraint_state.base_state.var_constrs) == 5
    @test isempty(state.constraint_state.base_state.lt_constrs)
    @test length(state.constraint_state.base_state.gt_constrs) == 1
    @test isempty(state.constraint_state.base_state.et_constrs)
    @test state.constraint_state.branch_state.num_lt_branches == 1
    @test state.constraint_state.branch_state.num_gt_branches == 1
    @test length(state.constraint_state.branch_state.lt_general_constrs) == 1
    @test length(state.constraint_state.branch_state.lt_general_constrs) == 1
    @test length(state.disjunction_state) == 1

    Cerberus.reset_formulation_state!(state)
    @test isempty(state._variable_indices)
    @test isempty(state.constraint_state.base_state.var_constrs)
    @test isempty(state.constraint_state.base_state.lt_constrs)
    @test isempty(state.constraint_state.base_state.gt_constrs)
    @test isempty(state.constraint_state.base_state.et_constrs)
    @test state.constraint_state.branch_state.num_lt_branches == 0
    @test state.constraint_state.branch_state.num_gt_branches == 0
    @test isempty(state.constraint_state.branch_state.lt_general_constrs)
    @test isempty(state.constraint_state.branch_state.lt_general_constrs)
    @test isempty(state.disjunction_state)
end

@testset "instantiate" begin
    form = _build_dmip_formulation()
    state = Cerberus.CurrentState()
    vis = state._variable_indices
    @test isempty(vis)
    node = Cerberus.Node()
    node_result = Cerberus.NodeResult(node)
    Cerberus.populate_lp_model!(state, form, node, node_result, CONFIG)
    @test length(vis) == 3
    vi_1 = @inferred Cerberus.instantiate(_CVI(1), state)
    @test vi_1 == vis[1]
    @test Cerberus.instantiate(_CVI(2), state) == vis[2]
    @test Cerberus.instantiate(_CVI(3), state) == vis[3]
    svs = _SV.(vis)
    @test _is_equal(
        Cerberus.instantiate(
            _CSAF([1.2, 3.4, 5.6], [_CVI(3), _CVI(1), _CVI(2)], 7.8),
            state,
        ),
        1.2 * svs[3] + 3.4 * svs[1] + 5.6 * svs[2] + 7.8,
    )
    @test_throws BoundsError Cerberus.instantiate(_CVI(4), state)

    cvi = @inferred Cerberus.attach_index!(state, _VI(7))
    @test cvi == _CVI(4)
end


text:
module NBT
using GZip
using DataStructures
using StaticArrays
# Defines tags name and id

const   TAG_End = 0
const   TAG_Byte = 1
const   TAG_Short = 2
const   TAG_Int = 3
const   TAG_Long = 4
const   TAG_Float = 5
const   TAG_Double = 6
const   TAG_Byte_Array = 7
const   TAG_String = 8
const   TAG_List = 9
const   TAG_Compound = 10
const   TAG_Int_Array = 11
const   TAG_Long_Array = 12

mutable struct TAG_Node
    id::UInt8
    name::String
    payload::Vector{TAG_Node}
    value::Union{String, UInt8, Int16, Int32, Int64, Float32, Float64, Vector{UInt8} ,Nothing}
    TAG_Node(id::UInt8, name::String, payload::Vector{TAG_Node}, value) = new(id,name,payload, value)
end

## TODO use similar code in the parse function for each payload types
bitcat(x::Vector{UInt8}) = (UInt16(x[1]) << 8) | x[2]

function IsArrayTag(b::UInt8)::Bool
	b == TAG_Byte_Array || b == TAG_Int_Array || b == TAG_Long_Array
end

function ReadNBT(filepath::String, unzip::Bool=true)::TAG_Node
    fd = GZip.open(filepath)
    bytes =  read(fd)
    a, tree = ParseNBT(bytes,1)
    close(fd)
    return tree

end

length(x) = size(x)[1]
function interpret(type::Type, bytes, index::Int64)
    return ntoh(type(reinterpret(type, bytes[index:index+sizeof(type)-1]::Vector{UInt8})[1])), index+sizeof(type)
end

# Because the native structure is a container like, the parsing function needs to be recursive
function ParseNBT(bytes::Vector{UInt8}, i)
    root = TAG_Node(UInt8(0),"",  Vector{TAG_Node}(), Nothing() )
    while (i<length(bytes))
        id,i = interpret(UInt8, bytes, i)
        if id == TAG_Compound
            println("compound")
            root.id = TAG_Compound
            length,i = interpret(UInt16, bytes, i)
            name = String(bytes[i:i+length-1])
            i += length
            root.name = name
            i, child = ParseNBT(bytes,i)

            push!(root.payload, child)
            continue
        elseif id == TAG_Int
            println("int")
            length,i = interpret(UInt16, bytes, i)
            name = String(bytes[i:i+length-1])
            i += length
            value, i = interpret(Int32, bytes, i)
            push!(root.payload, TAG_Node(id, name, Vector{TAG_Node}(), value))
            continue
        elseif id == TAG_Short
            println("short")
            length,i = interpret(UInt16, bytes, i)
            name = String(bytes[i:i+length-1])
            i += length
            value, i = interpret(Int16, bytes, i)
            push!(root.payload, TAG_Node(id, name, Vector{TAG_Node}(),value))
            continue
        elseif id == TAG_Float
            println("float")
            length,i = interpret(UInt16, bytes, i)
            name = String(bytes[i:i+length-1])
            i += length
            value, i = interpret(Float32, bytes, i)
            push!(root.payload, TAG_Node(id, name,Vector{TAG_Node}(), value))
            continue
        elseif id == TAG_Double
            println("double")
            length,i = interpret(UInt16, bytes, i)
            name = String(bytes[i:i+length-1])
            i += length
            value, i = interpret(Float64, bytes, i)
            push!(root.payload, TAG_Node(id, name, Vector{TAG_Node}(), value))
            continue
        elseif id == TAG_Long
            length,i = interpret(UInt16, bytes, i)
            name = String(bytes[i:i+length-1])
            i += length
            value,i = interpret(Int64, bytes, i)
            push!(root.payload, TAG_Node(id, name, Vector{TAG_Node}(), value))
            println("Long",name)
            continue
        elseif id == TAG_String
            println("string")
            length,i = interpret(UInt16, bytes, i)
            name = String(bytes[i:i+length-1])
            i += length
            length,i = interpret(Int16, bytes, i)
            value = String(bytes[i:i+length-1])
            i+=length
            push!(root.payload, TAG_Node(id, name, Vector{TAG_Node}(), value))
        elseif id==TAG_Byte
            length,i = interpret(UInt16, bytes, i)
            name = String(bytes[i:i+length-1])
            i += length
            print(i)
            value,i = interpret(UInt8, bytes, i)
            push!(root.payload, TAG_Node(id, name, Vector{TAG_Node}(), value))
            println("Byte",name)
            continue
        elseif id==TAG_Byte_Array
            length,i = interpret(UInt16, bytes, i)
            name = String(bytes[i:i+length-1])
            i += length
            size,i = interpret(UInt32, bytes, i)
            i+=sizeof(UInt32)
            value = Vector{UInt8}()
            for a in 1:size
                push!(value, bytes[i+a])
            end
            println(value)
            i+=size
            push!(root.payload, TAG_Node(id, name, Vector{TAG_Node}(), value))
            println("Byte Array",name)
        elseif id==TAG_List
            length,i = interpret(UInt16, bytes, i)
            name = String(bytes[i:i+length-1])
            i += length
            type, i = interpret(UInt8, bytes, i)
            length, i = interpret(Int32, bytes, i)
            println(name, " ", type, " ", length)

        elseif id == TAG_End
            println("end")
            return i, root
        else
            println(id)
            println("error")
            continue

        end

    end

end

function ParseNBT_test(fd::GZip.GZipStream)

    bytes =  read(fd)
    i=1
    id,i = interpret(UInt8, bytes, i)
    length,i = interpret(UInt16, bytes, i)

    name = String(bytes[i:i+length-1])
    i += length

    println("id ", id)
    println("name length ",length)
    println("name ", name)

    id,i = interpret(UInt8, bytes, i)
    length,i = interpret(UInt16, bytes, i)

    name = String(bytes[i:i+length-1])
    i+=length

    long,i =interpret(Int64, bytes, i)

    println("id ", id)
    println("name length ",length)
    println("name ", name)
    println("value ", long)

    id,i = interpret(UInt8, bytes, i)
    length,i = interpret(UInt16, bytes, i)

    name = String(bytes[i:i+length-1])
    i += length

    short,i =interpret(Int16, bytes, i)

    println("id ", id)
    println("name length ",length)
    println("name ", name)
    println("value ", short)

    id,i = interpret(UInt8, bytes, i)
    length,i = interpret(UInt16, bytes, i)

    name = String(bytes[i:i+length-1])
    i += length

    str_length,i = interpret(UInt16, bytes, i)
    str = String(bytes[i:i+str_length-1])
    i += str_length
    println("id ", id)
    println("name length ",length)
    println("name ", name)
    println("value ", str)

    id,i = interpret(UInt8, bytes, i)
    length,i = interpret(UInt16, bytes, i)

    name = String(bytes[i:i+length-1])
    i += length

    float,i =interpret(Float32, bytes, i)

    println("id ", id)
    println("name length ",length)
    println("name ", name)
    println("value ", float)

end

# function WriteNBT(tree::Node, filepath::String)
#     # TODO
# end

end


text:
@testset "Cylinder" begin
    @testset "constructors" begin
        o, extr, r = Point2f0(1, 2), Point2f0(3, 4), 5f0
        s = Cylinder(o, extr, r)
        @test typeof(s) == Cylinder{2,Float32}
        @test typeof(s) == Cylinder2{Float32}
        @test origin(s) == o
        @test extremity(s) == extr
        @test radius(s) == r
        #@test abs(height(s)- norm([1,2]-[3,4]))<1e-5
        h = norm(o - extr)
        @test isapprox(height(s), h)
        #@test norm(direction(s) - Point{2,Float32}([2,2]./norm([1,2]-[3,4])))<1e-5
        @test isapprox(direction(s), Point2f0(2, 2) ./ h)
        v1 = rand(Point{3, Float64}); v2 = rand(Point{3, Float64}); R = rand()
        s = Cylinder(v1, v2, R)
        @test typeof(s) == Cylinder{3, Float64}
        @test typeof(s) == Cylinder3{Float64}
        @test origin(s) == v1
        @test extremity(s) == v2
        @test radius(s) == R
        @test height(s) == norm(v2 - v1)
        #@test norm(direction(s) - Point{3,Float64}((v2-v1)./norm(v2-v1)))<1e-10
        @test isapprox(direction(s), (v2-v1) ./ norm(v2 .- v1))
      end

    @testset "decompose" begin
        o, extr, r = Point2f0(1, 2), Point2f0(3, 4), 5f0
        s = Cylinder(o, extr, r)
        positions = Point{3, Float32}[
            (-0.7677671, 3.767767, 0.0),
            (2.767767, 0.23223293, 0.0),
            (0.23223293, 4.767767, 0.0),
            (3.767767, 1.2322329, 0.0),
            (1.2322329, 5.767767, 0.0),
            (4.767767, 2.232233, 0.0)
        ]
        @test decompose(Point3f0, s, (2, 3)) ≈ positions
        FT = Face{3, Int}
        faces = FT[
            (1,2,4),
            (1,4,3),
            (3,4,6),
            (3,6,5)
        ]
        @test faces == decompose(FT, s, (2,3))

        v1 = Point{3, Float64}(1,2,3); v2 = Point{3, Float64}(4,5,6); R = 5.0
        s = Cylinder(v1, v2, R)
        positions = Point{3,Float64}[
            (4.535533905932738,-1.5355339059327373,3.0),
            (7.535533905932738,1.4644660940672627,6.0),
            (3.0412414523193148,4.041241452319315,-1.0824829046386295),
            (6.041241452319315,7.041241452319315,1.9175170953613705),
            (-2.535533905932737,5.535533905932738,2.9999999999999996),
            (0.46446609406726314,8.535533905932738,6.0),
            (-1.0412414523193152,-0.04124145231931431,7.0824829046386295),
            (1.9587585476806848,2.9587585476806857,10.08248290463863)
        ]
        @test decompose(Point3{Float64},s,8) ≈ positions
        faces = Face[
            (3, 2, 1),
            (4, 2, 3),
            (5, 4, 3),
            (6, 4, 5),
            (7, 6, 5),
            (8, 6, 7),
            (1, 8, 7),
            (2, 8, 1)
        ]
        @test faces == decompose(Face{3, Int}, s, 6)
        m = GLPlainMesh(s, 8)
        @test m.faces == faces
        @test m.vertices ≈ positions
        m = GLNormalMesh(s)# just test that it works without explicit resolution parameter
        @test m isa GLNormalMesh

    end
end


text:
module XFields

using LinearAlgebra

const module_dir  = joinpath(@__DIR__, "..") |> normpath

# Abstract Transform and identity transform
# ==========================================
export Transform, size_in, size_out, eltype_in, eltype_out, plan

abstract type Transform{Tf<:Number,d} end

size_in(ft::Transform) = error("not yet defined")
size_out(ft::Transform) = error("not yet defined")
eltype_in(ft::Transform) = error("not yet defined")
eltype_out(ft::Transform) = error("not yet defined")
plan(ft::Transform) = error("not yet defined")

# Identity transform when one just wants to work with an array
struct Id{Tf,d} <: Transform{Tf,d}
	sz::NTuple{d,Int}
end
size_in(trn::Id) = trn.sz
size_out(trn::Id) = trn.sz
eltype_in(trn::Id{Tf}) where {Tf} = Tf
eltype_out(trn::Id{Tf}) where {Tf} = Tf
plan(trn::Id) = trn
Base.:*(trn::Id{Tf,d}, f::Array{Tf,d}) where {Tf,d} = f
Base.:\(trn::Id{Tf,d}, f::Array{Tf,d}) where {Tf,d} = f

# Abstract Field with Fourier <-> Map pair
# ==========================================
export	Field, FourierField, MapField, fielddata, fieldtransform,
		AbstractLinearOp, DiagOp, diag

abstract type Field{F<:Transform,Tf,Ti,d} end
abstract type FourierField{F<:Transform,Tf,Ti,d} <: Field{F,Tf,Ti,d} end
abstract type MapField{F<:Transform,Tf,Ti,d} <: Field{F,Tf,Ti,d} end

FourierField(::Type{X}) where {X<:Field} = error("not yet defined, should return the dual type")
MapField(::Type{X})     where {X<:Field} = error("not yet defined, should return the dual type")
# note:  FourierField(f::Field) and MapField(f::Field)
# fall back to convert using the above definitions

fieldtransform(x::Field) = error("not yet defined")
fielddata(x::Field)      = error("not yet defined")

include("convert_promote.jl")

include("field_methods.jl")

include("abstract_linear_ops.jl")

include("diag_linear_op.jl")

# Specific implimentation of Abstract Field
# =========================================
export Xmap, Xfourier, Xfield

include("xfield.jl")

end


text:
using Documenter, JultraDark

makedocs(;
    modules=[JultraDark],
    format=Documenter.HTML(),
    pages=[
        "Home" => "index.md",
    ],
    repo="https://github.com/musoke/JultraDark.jl/blob/{commit}{path}#L{line}",
    sitename="JultraDark.jl",
    authors="Nathan Musoke <n.musoke@auckland.ac.nz>",
    assets=String[],
    doctest=false,  # TODO: remove this line
)

deploydocs(;
    repo="github.com/musoke/JultraDark.jl",
)


text:
function key1u(x₀, y₀; Dx = 8.5, Dy = 11, Drot = -150)
    x₀′ = x₀+Dx
    y₀′ = y₀+Dy

    offsetx = 9.68
    offsety = 2.06

    sx₀ = x₀ + offsetx
    sy₀ = y₀ + offsety

    S = Switch(footprint="Button_Switch_Keyboard:SW_Cherry_MX1A_1.00u_PCB",
               centerx = x₀, centery = y₀,
               offsetx = offsetx, offsety = offsety,
               model="Cherry-MX1A-E1NW.wrl"=>Dict(:scale => 0.4*[1,1,1],
                                                 :at => [-0.1,-0.2,0]),
               text=Dict("reference"=>Dict(:at => [-6,-1])))
    D = Diode(footprint="Diode_SMD:D_SMA",
              #footprint="Diode_THT:D_DO-41_SOD81_P10.16mm_Horizontal",
              x = x₀′, y = y₀′, rot=Drot,
              layers = "F"=>"B",
              manf_num = "S1A", Mouser = "512-S1A")

    c = Circuit()
    push!(c, D)
    push!(c, S)
    connect!(c, S, 1, D, "+")
    unique_labels!(c)
    key = SubCircuit(c, S=>2, D=>"-")
    # Keys in the same column connect to pin 1 of the subcircuit; keys
    # in the same row connect to pin 2.

    segment = PCBs.Segment("B.Cu", "Net", sx₀, sy₀, 0, 7.268, 0.25)

    key,segment
end

function Base.convert(::Type{Circuit}, kbd::Keyboard)
    x₀ = 0
    y₀ = 0

    l = 15
    dx = l
    dy = l

    nrows = 5
    ncols = 7

    zl = 14.224
    zx₀ = x₀
    zy₀ = y₀

    key_circuit,key_segment = key1u(x₀, y₀)
    keyboard = Circuit()

    columns = Dict{Int,Int}()
    rows = Dict{Int,Int}()

    map(enumerate(find_columns(kbd))) do (j,(col,col_keys))
        column = []
        for key in col_keys
            x,y,row = key[:centerx],key[:centery],key[:row]
            conn = []
            j ∈ keys(columns) && push!(conn, 1 => columns[j])
            row ∈ keys(rows) && push!(conn, 2 => rows[row])
            a,b = attach!(keyboard, translate(key_circuit, dx=x/u"mm", dy=-y/u"mm"), conn...)
            columns[j] = a
            rows[row] = b
        end
    end

    keyboard
end

Base.convert(::Type{PCB}, kbd::Keyboard, name::String) =
    PCB(convert(Circuit, kbd), name)


text:
# test/example for writing 1D arrays to a Silo file.
@static if VERSION >= v"0.7-"
    using Printf
end

dbopt = Silo.DBMakeOptlist(1)
#     DBoptlist	*dbopt = DBMakeOptlist(1);
#     char         fn[100], title[100];
#     int     lbuf = ndp->rgr.lbuf;
#     int     ubuf = ndp->rgr.ubuf;
#     int     gmax = ndp->rgr.gmax;
#     int     N = gmax-lbuf-ubuf;

#     sprintf(fn,"./output/u%s_step%d.silo",ndp->par.output_fn,ndp->step);
testname = "testout"
stepnum = 1
fn = joinpath(dirname(@__FILE__), @sprintf "u%s_step%d.silo" testname stepnum)
#     dbfile = DBCreate(fn, DB_CLOBBER, DB_LOCAL, "Line Output", DB_PDB);
dbfile = Silo.DBCreate(fn, Silo.DB_CLOBBER, Silo.DB_LOCAL, "Line Output", Silo.DB_PDB)

#     sprintf(title,"time: %lf",ndp->time);
#     DBAddOption(dbopt, DBOPT_LABEL, &title);
title = @sprintf "time: %lf" 1e-4
Silo.DBAddOption(dbopt, Silo.DBOPT_LABEL, pointer_from_objref(title))

# Test data
N = 20
x = collect(range(0,stop=1,length=N))
y1 = collect(range(4,stop=5,length=N))
y2 = collect(range(10,stop=11,length=N))
y3 = y1 - y2

#     DBPutCurve(dbfile,"Numerical_Solution",x,y1,DB_FLOAT,N+1,NULL);
Silo.DBPutCurve(dbfile, "Numerical_Solution", x, y1, Silo.DB_FLOAT, N + 1, dbopt) # or C_NULL
#     DBPutCurve(dbfile,"Analytical_Solution",x,y2,DB_FLOAT,N+1,NULL);
Silo.DBPutCurve(dbfile, "Analytical_Solution", x, y2, Silo.DB_FLOAT, N + 1, dbopt)
#     DBPutCurve(dbfile,"Error",x,y3,DB_FLOAT,N+1,NULL);
Silo.DBPutCurve(dbfile, "Error", x, y1, Silo.DB_FLOAT, N + 1, dbopt)
#     DBClose(dbfile);
Silo.DBClose(dbfile)

# 	DBFreeOptlist(dbopt);
Silo.DBFreeOptlist(dbopt)

@test Silo.is_silofile(fn)

# contnames = names(fn)
# @test issubset(["Numerical_Solution", "Analytical_Solution", "Error"],
#                contnames)


text:
@testset "Single-CPU batch processing" begin

    som = initGigaSOM(pbmc8_data, 10, 10, seed = 1234)

    #check whether the distributed version works the same
    save_at(1, :test, pbmc8_data)
    som2 = initGigaSOM(LoadedDataInfo(:test, [1]), 10, 10, seed = 1234)
    @test som.codes == som2.codes
    remove_from(1, :test)

    @testset "Check SOM dimensions" begin
        @test size(som.codes) == (100, 10)
        @test som.xdim == 10
        @test som.ydim == 10
        @test som.numCodes == 100
    end

    som = trainGigaSOM(som, pbmc8_data, epochs = 1)

    winners = mapToGigaSOM(som, pbmc8_data)

    embed = embedGigaSOM(som, pbmc8_data, k = 10, smooth = 0.1, adjust = 2.3, m = 4.5)

    @testset "Check results" begin
        codes = som.codes
        @test size(codes) == (100, 10)

        dfCodes = DataFrame(codes, :auto)
        rename!(dfCodes, Symbol.(antigens))
        dfEmbed = DataFrame(embed, :auto)
        CSV.write(genDataPath * "/batchDfCodes.csv", dfCodes)
        CSV.write(genDataPath * "/batchWinners.csv", winners)
        CSV.write(genDataPath * "/batchEmbedded.csv", dfEmbed)

        #load the ref data
        refBatchDfCodes = CSV.File(refDataPath * "/refBatchDfCodes.csv") |> DataFrame
        refBatchWinners = CSV.File(refDataPath * "/refBatchWinners.csv") |> DataFrame
        refBatchEmbedded = CSV.File(refDataPath * "/refBatchEmbedded.csv") |> DataFrame

        #load the generated data
        batchDfCodes = CSV.File(genDataPath * "/batchDfCodes.csv") |> DataFrame
        batchDfCodesTest = first(batchDfCodes, 10)
        batchWinners = CSV.File(genDataPath * "/batchWinners.csv") |> DataFrame
        batchWinnersTest = first(batchWinners, 10)
        batchEmbedded = CSV.File(genDataPath * "/batchEmbedded.csv") |> DataFrame
        batchEmbeddedTest = first(batchEmbedded, 10)

        # test the generated data against the reference data
        @test refBatchWinners == batchWinnersTest
        @test Matrix{Float64}(refBatchDfCodes) ≈ Matrix{Float64}(batchDfCodesTest) atol =
            1e-4
        @test Matrix{Float64}(refBatchEmbedded) ≈ Matrix{Float64}(batchEmbeddedTest) atol =
            1e-4
    end

end


text:
numeric_predicates = [:(Base.iseven), :(Base.ispow2), :(Base.isfinite), :(Base.isinf), :(Base.isodd)]
isdefined(Base, :isprime) && push!(numeric_predicates, :(Base.isprime))

type_predicates = (:(Base.isinteger), :(Base.isreal))

container_predicates = (:(Base.isempty),)

for p in numeric_predicates
  @eval begin
    ($p)(v::NAtype) = NA
  end
end

for p in type_predicates
  @eval begin
    ($p)(v::NAtype) = NA
  end
end


text:
####################################################################
# Symplectic
####################################################################

"""
    T1A!(ψₒ, ψᵢ, dx, ops)

Compute `ψₒ`, i.e. `ψᵢ` advanced a step `dx` forward using a symplectic first order integrator of type A. The structure `ops::Operators` contains the FFT plans and the kinetic energy operators.

See also: [`solve!`](@ref), [`Operators`](@ref)
"""
function T1A!(ψₒ, ψᵢ, dx, ops)

    # Nonlinear
    @. ψₒ = cis(-dx * (-1*abs2(ψᵢ)))*ψᵢ

    # Dispersion
    ops.F̂*ψₒ
    ψₒ .= ops.K̂(dx) .* ψₒ
    ops.F̃̂*ψₒ
end

"""
    T1B!(ψₒ, ψᵢ, dx, ops)

Compute `ψₒ`, i.e. `ψᵢ` advanced a step `dx` forward using a symplectic first order integrator of type B. The structure `ops::Operators` contains the FFT plans and the kinetic energy operators.

See also: [`solve!`](@ref), [`Operators`](@ref)
"""
function T1B!(ψₒ, ψᵢ, dx, ops)

    # Dispersion
    ψₒ .= ops.K̂(dx) .* ψᵢ

    # Nonlinear
    ops.F̃̂*ψₒ
    @. ψₒ = cis(-dx * (-1*abs2(ψₒ)))*ψₒ
    ops.F̂*ψₒ
end

"""
    T2A!(ψₒ, ψᵢ, dx, ops)

Compute `ψₒ`, i.e. `ψᵢ` advanced a step `dx` forward using a symplectic second order integrator of type A. The structure `ops::Operators` contains the FFT plans and the kinetic energy operators.

See also: [`solve!`](@ref), [`Operators`](@ref)
"""
function T2A!(ψₒ, ψᵢ, dx, ops)
    # Nonlinear
    @. ψₒ = cis(-dx/2 * (-1*abs2(ψᵢ)))*ψᵢ

    # Dispersion
    ops.F̂*ψₒ
    ψₒ .= ops.K̂(dx) .* ψₒ
    ops.F̃̂*ψₒ

    # Nonlinear
    @. ψₒ = cis(-dx/2 * (-1*abs2(ψₒ)))*ψₒ
end

"""
    T2B!(ψₒ, ψᵢ, dx, ops)

Compute `ψₒ`, i.e. `ψᵢ` advanced a step `dx` forward using a symplectic second order integrator of type B. The structure `ops::Operators` contains the FFT plans and the kinetic energy operators.

See also: [`solve!`](@ref), [`Operators`](@ref)
"""
function T2B!(ψₒ, ψᵢ, dx, ops)

    # Dispersion
    ψₒ .= ops.K̂(dx/2) .* ψᵢ

    # Nonlinear
    ops.F̃̂*ψₒ
    @. ψₒ = cis(-dx * (-1*abs2(ψₒ)))*ψₒ
    ops.F̂*ψₒ

    # Dispersion
    ψₒ .= ops.K̂(dx/2) .* ψₒ
end

####################################################################
# Triple Jump
####################################################################
"""
    T4A_TJ!(ψₒ, ψᵢ, dx, ops)

Compute `ψₒ`, i.e. `ψᵢ` advanced a step `dx` forward using a symplectic Triple Jump Fourth order integrator of type A. The structure `ops::Operators` contains the FFT plans and the kinetic energy operators.

See also: [`solve!`](@ref), [`Operators`](@ref)
"""
function T4A_TJ!(ψₒ, ψᵢ, dx, ops)
    s = 2^(1 / 3)
    os = 1 / (2 - s)
    ft = os
    bt = -s * os

    T2A!(ψₒ, ψᵢ, ft*dx, ops)
    T2A!(ψₒ, ψₒ, bt*dx, ops)
    T2A!(ψₒ, ψₒ, ft*dx, ops)
end

"""
    T4B_TJ!(ψₒ, ψᵢ, dx, ops)

Compute `ψₒ`, i.e. `ψᵢ` advanced a step `dx` forward using a symplectic Triple Jump Fourth order integrator of type B. The structure `ops::Operators` contains the FFT plans and the kinetic energy operators.

See also: [`solve!`](@ref), [`Operators`](@ref)
"""
function T4B_TJ!(ψₒ, ψᵢ, dx, ops)
    s = 2^(1 / 3)
    os = 1 / (2 - s)
    ft = os
    bt = -s * os

    T2B!(ψₒ, ψᵢ, ft*dx, ops)
    T2B!(ψₒ, ψₒ, bt*dx, ops)
    T2B!(ψₒ, ψₒ, ft*dx, ops)
end

"""
    T6A_TJ!(ψₒ, ψᵢ, dx, ops)

Compute `ψₒ`, i.e. `ψᵢ` advanced a step `dx` forward using a symplectic Triple Jump Sixth order integrator of type A. The structure `ops::Operators` contains the FFT plans and the kinetic energy operators.

See also: [`solve!`](@ref), [`Operators`](@ref)
"""
function T6A_TJ!(ψₒ, ψᵢ, dx, ops)
    s = 2^(1 / 5)
    os = 1 / (2 - s)
    ft = os
    bt = -s * os

    T4A_TJ!(ψₒ, ψᵢ, ft*dx, ops)
    T4A_TJ!(ψₒ, ψₒ, bt*dx, ops)
    T4A_TJ!(ψₒ, ψₒ, ft*dx, ops)
end

"""
    T6B_TJ!(ψₒ, ψᵢ, dx, ops)

Compute `ψₒ`, i.e. `ψᵢ` advanced a step `dx` forward using a symplectic Triple Jump Sixth order integrator of type B. The structure `ops::Operators` contains the FFT plans and the kinetic energy operators.

See also: [`solve!`](@ref), [`Operators`](@ref)
"""
function T6B_TJ!(ψₒ, ψᵢ, dx, ops)
    s = 2^(1 / 5)
    os = 1 / (2 - s)
    ft = os
    bt = -s * os

    T4B_TJ!(ψₒ, ψᵢ, ft*dx, ops)
    T4B_TJ!(ψₒ, ψₒ, bt*dx, ops)
    T4B_TJ!(ψₒ, ψₒ, ft*dx, ops)
end

"""
    T8A_TJ!(ψₒ, ψᵢ, dx, ops)

Compute `ψₒ`, i.e. `ψᵢ` advanced a step `dx` forward using a symplectic Triple Jump Eighth order integrator of type A. The structure `ops::Operators` contains the FFT plans and the kinetic energy operators.

See also: [`solve!`](@ref), [`Operators`](@ref)
"""
function T8A_TJ!(ψₒ, ψᵢ, dx, ops)
    s = 2^(1 / 7)
    os = 1 / (2 - s)
    ft = os
    bt = -s * os

    T6A_TJ!(ψₒ, ψᵢ, ft*dx, ops)
    T6A_TJ!(ψₒ, ψₒ, bt*dx, ops)
    T6A_TJ!(ψₒ, ψₒ, ft*dx, ops)
end

"""
    T8A_TJ!(ψₒ, ψᵢ, dx, ops)

Compute `ψₒ`, i.e. `ψᵢ` advanced a step `dx` forward using a symplectic Triple Jump Eighth order integrator of type B. The structure `ops::Operators` contains the FFT plans and the kinetic energy operators.

See also: [`solve!`](@ref), [`Operators`](@ref)
"""
function T8B_TJ!(ψₒ, ψᵢ, dx, ops)
    s = 2^(1 / 7)
    os = 1 / (2 - s)
    ft = os
    bt = -s * os

    T6B_TJ!(ψₒ, ψᵢ, ft*dx, ops)
    T6B_TJ!(ψₒ, ψₒ, bt*dx, ops)
    T6B_TJ!(ψₒ, ψₒ, ft*dx, ops)
end

####################################################################
# Suzuki Fractal
####################################################################

"""
    T4A_SF!(ψₒ, ψᵢ, dx, ops)

Compute `ψₒ`, i.e. `ψᵢ` advanced a step `dx` forward using a symplectic Suzuki's Fractal Fourth order integrator of type A. The structure `ops::Operators` contains the FFT plans and the kinetic energy operators.

See also: [`solve!`](@ref), [`Operators`](@ref)
"""
function T4A_SF!(ψₒ, ψᵢ, dx, ops)
    s = 4^(1/3)
    os = 1/(4 - s)
    ft = os
    bt = -s*os

    T2A!(ψₒ, ψᵢ, ft*dx, ops)
    T2A!(ψₒ, ψₒ, ft*dx, ops)
    T2A!(ψₒ, ψₒ, bt*dx, ops)
    T2A!(ψₒ, ψₒ, ft*dx, ops)
    T2A!(ψₒ, ψₒ, ft*dx, ops)
end

"""
    T4B_SF!(ψₒ, ψᵢ, dx, ops)

Compute `ψₒ`, i.e. `ψᵢ` advanced a step `dx` forward using a symplectic Suzuki's Fractal Fourth order integrator of type B. The structure `ops::Operators` contains the FFT plans and the kinetic energy operators.

See also: [`solve!`](@ref), [`Operators`](@ref)
"""
function T4B_SF!(ψₒ, ψᵢ, dx, ops)
    s = 4^(1/3)
    os = 1/(4 - s)
    ft = os
    bt = -s*os

    T2B!(ψₒ, ψᵢ, ft*dx, ops)
    T2B!(ψₒ, ψₒ, ft*dx, ops)
    T2B!(ψₒ, ψₒ, bt*dx, ops)
    T2B!(ψₒ, ψₒ, ft*dx, ops)
    T2B!(ψₒ, ψₒ, ft*dx, ops)
end

"""
    T6A_SF!(ψₒ, ψᵢ, dx, ops)

Compute `ψₒ`, i.e. `ψᵢ` advanced a step `dx` forward using a symplectic Suzuki's Fractal Sixth order integrator of type A. The structure `ops::Operators` contains the FFT plans and the kinetic energy operators.

See also: [`solve!`](@ref), [`Operators`](@ref)
"""
function T6A_SF!(ψₒ, ψᵢ, dx, ops)
    s = 4^(1/5)
    os = 1/(4 - s)
    ft = os
    bt = -s*os

    T4A_SF!(ψₒ, ψᵢ, ft*dx, ops)
    T4A_SF!(ψₒ, ψₒ, ft*dx, ops)
    T4A_SF!(ψₒ, ψₒ, bt*dx, ops)
    T4A_SF!(ψₒ, ψₒ, ft*dx, ops)
    T4A_SF!(ψₒ, ψₒ, ft*dx, ops)
end

"""
    T6B_SF!(ψₒ, ψᵢ, dx, ops)

Compute `ψₒ`, i.e. `ψᵢ` advanced a step `dx` forward using a symplectic Suzuki's Fractal Sixth order integrator of type B. The structure `ops::Operators` contains the FFT plans and the kinetic energy operators.

See also: [`solve!`](@ref), [`Operators`](@ref)
"""
function T6B_SF!(ψₒ, ψᵢ, dx, ops)
    s = 4^(1/5)
    os = 1/(4 - s)
    ft = os
    bt = -s*os

    T4B_SF!(ψₒ, ψᵢ, ft*dx, ops)
    T4B_SF!(ψₒ, ψₒ, ft*dx, ops)
    T4B_SF!(ψₒ, ψₒ, bt*dx, ops)
    T4B_SF!(ψₒ, ψₒ, ft*dx, ops)
    T4B_SF!(ψₒ, ψₒ, ft*dx, ops)
end

"""
    T8A_SF!(ψₒ, ψᵢ, dx, ops)

Compute `ψₒ`, i.e. `ψᵢ` advanced a step `dx` forward using a symplectic Suzuki's Fractal Eighth order integrator of type A. The structure `ops::Operators` contains the FFT plans and the kinetic energy operators.

See also: [`solve!`](@ref), [`Operators`](@ref)
"""
function T8A_SF!(ψₒ, ψᵢ, dx, ops)
    s = 4^(1/7)
    os = 1/(4 - s)
    ft = os
    bt = -s*os

    T6A_SF!(ψₒ, ψᵢ, ft*dx, ops)
    T6A_SF!(ψₒ, ψₒ, ft*dx, ops)
    T6A_SF!(ψₒ, ψₒ, bt*dx, ops)
    T6A_SF!(ψₒ, ψₒ, ft*dx, ops)
    T6A_SF!(ψₒ, ψₒ, ft*dx, ops)
end

"""
    T8B_SF!(ψₒ, ψᵢ, dx, ops)

Compute `ψₒ`, i.e. `ψᵢ` advanced a step `dx` forward using a symplectic Suzuki's Fractal Eighth order integrator of type B. The structure `ops::Operators` contains the FFT plans and the kinetic energy operators.

See also: [`solve!`](@ref), [`Operators`](@ref)
"""
function T8B_SF!(ψₒ, ψᵢ, dx, ops)
    s = 4^(1/7)
    os = 1/(4 - s)
    ft = os
    bt = -s*os

    T6B_SF!(ψₒ, ψᵢ, ft*dx, ops)
    T6B_SF!(ψₒ, ψₒ, ft*dx, ops)
    T6B_SF!(ψₒ, ψₒ, bt*dx, ops)
    T6B_SF!(ψₒ, ψₒ, ft*dx, ops)
    T6B_SF!(ψₒ, ψₒ, ft*dx, ops)
end

####################################################################
# Multi-Product Nystrom
####################################################################
"""
    T4A_CMP!(ψₒ, ψᵢ, dx, ops)

Compute `ψₒ`, i.e. `ψᵢ` advanced a step `dx` forward using a Chin Multi-Product Fourth order integrator of type A. The structure `ops::Operators` contains the FFT plans and the kinetic energy operators.

See also: [`solve!`](@ref), [`Operators`](@ref)
"""
function T4A_CMP!(ψₒ, ψᵢ, dx, ops)
    T2A!(ψₒ, ψᵢ, dx/2, ops)
    T2A!(ψₒ, ψₒ, dx/2, ops)
    ψₒ .*= 4/3

    T2A!(ops.ψ₁, ψᵢ, dx,ops)
    ops.ψ₁ .*= -1/3

    ψₒ .+= ops.ψ₁
end

"""
    T4B_CMP!(ψₒ, ψᵢ, dx, ops)

Compute `ψₒ`, i.e. `ψᵢ` advanced a step `dx` forward using a Chin Multi-Product Fourth order integrator of type B. The structure `ops::Operators` contains the FFT plans and the kinetic energy operators.

See also: [`solve!`](@ref), [`Operators`](@ref)
"""
function T4B_CMP!(ψₒ, ψᵢ, dx, ops)
    T2B!(ψₒ, ψᵢ, dx/2, ops)
    T2B!(ψₒ, ψₒ, dx/2, ops)
    ψₒ .*= 4/3

    T2B!(ops.ψ₁, ψᵢ, dx,ops)
    ops.ψ₁ .*= -1/3

    ψₒ .+= ops.ψ₁
end

"""
    T6A_CMP!(ψₒ, ψᵢ, dx, ops)

Compute `ψₒ`, i.e. `ψᵢ` advanced a step `dx` forward using a Chin Multi-Product Sixth order integrator of type A. The structure `ops::Operators` contains the FFT plans and the kinetic energy operators.

See also: [`solve!`](@ref), [`Operators`](@ref)
"""
function T6A_CMP!(ψₒ, ψᵢ, dx, ops)
    T2A!(ψₒ, ψᵢ, dx/3, ops)
    T2A!(ψₒ, ψₒ, dx/3, ops)
    T2A!(ψₒ, ψₒ, dx/3, ops)
    ψₒ .*= 81/40

    T2A!(ops.ψ₁, ψᵢ, dx/2, ops)
    T2A!(ops.ψ₁, ops.ψ₁, dx/2, ops)
    ops.ψ₁ .*= -16/15

    T2A!(ops.ψ₂, ψᵢ, dx, ops)
    ops.ψ₂ .*= 1/24

    ψₒ .+= ops.ψ₁ .+ ops.ψ₂
end

"""
    T6B_CMP!(ψₒ, ψᵢ, dx, ops)

Compute `ψₒ`, i.e. `ψᵢ` advanced a step `dx` forward using a Chin Multi-Product Sixth order integrator of type B. The structure `ops::Operators` contains the FFT plans and the kinetic energy operators.

See also: [`solve!`](@ref), [`Operators`](@ref)
"""
function T6B_CMP!(ψₒ, ψᵢ, dx, ops)
    T2B!(ψₒ, ψᵢ, dx/3, ops)
    T2B!(ψₒ, ψₒ, dx/3, ops)
    T2B!(ψₒ, ψₒ, dx/3, ops)
    ψₒ .*= 81/40

    T2B!(ops.ψ₁, ψᵢ, dx/2, ops)
    T2B!(ops.ψ₁, ops.ψ₁, dx/2, ops)
    ops.ψ₁ .*= -16/15

    T2B!(ops.ψ₂, ψᵢ, dx, ops)
    ops.ψ₂ .*= 1/24

    ψₒ .+= ops.ψ₁ .+ ops.ψ₂
end

"""
    T8A_CMP!(ψₒ, ψᵢ, dx, ops)

Compute `ψₒ`, i.e. `ψᵢ` advanced a step `dx` forward using a Chin Multi-Product Eighth order integrator of type A. The structure `ops::Operators` contains the FFT plans and the kinetic energy operators.

See also: [`solve!`](@ref), [`Operators`](@ref)
"""
function T8A_CMP!(ψₒ, ψᵢ, dx, ops)
    T2A!(ψₒ, ψᵢ, dx/4, ops)
    T2A!(ψₒ, ψₒ, dx/4, ops)
    T2A!(ψₒ, ψₒ, dx/4, ops)
    T2A!(ψₒ, ψₒ, dx/4, ops)
    ψₒ .*= 1024/315

    T2A!(ops.ψ₁, ψᵢ, dx/3, ops)
    T2A!(ops.ψ₁, ops.ψ₁, dx/3, ops)
    T2A!(ops.ψ₁, ops.ψ₁, dx/3, ops)
    ops.ψ₁ .*= -729/280

    T2A!(ops.ψ₂, ψᵢ, dx/2, ops)
    T2A!(ops.ψ₂, ops.ψ₂, dx/2, ops)
    ops.ψ₂ .*= 16/45

    T2A!(ops.ψ₃, ψᵢ, dx, ops)
    ops.ψ₃ .*= -1/360

    ψₒ .+= ops.ψ₁ .+ ops.ψ₂ .+ ops.ψ₃
end

"""
    T8B_CMP!(ψₒ, ψᵢ, dx, ops)

Compute `ψₒ`, i.e. `ψᵢ` advanced a step `dx` forward using a Chin Multi-Product Eighth order integrator of type B. The structure `ops::Operators` contains the FFT plans and the kinetic energy operators.

See also: [`solve!`](@ref), [`Operators`](@ref)
"""
function T8B_CMP!(ψₒ, ψᵢ, dx, ops)
    T2B!(ψₒ, ψᵢ, dx/4, ops)
    T2B!(ψₒ, ψₒ, dx/4, ops)
    T2B!(ψₒ, ψₒ, dx/4, ops)
    T2B!(ψₒ, ψₒ, dx/4, ops)
    ψₒ .*= 1024/315

    T2B!(ops.ψ₁, ψᵢ, dx/3, ops)
    T2B!(ops.ψ₁, ops.ψ₁, dx/3, ops)
    T2B!(ops.ψ₁, ops.ψ₁, dx/3, ops)
    ops.ψ₁ .*= -729/280

    T2B!(ops.ψ₂, ψᵢ, dx/2, ops)
    T2B!(ops.ψ₂, ops.ψ₂, dx/2, ops)
    ops.ψ₂ .*= 16/45

    T2B!(ops.ψ₃, ψᵢ, dx, ops)
    ops.ψ₃ .*= -1/360

    ψₒ .+= ops.ψ₁ .+ ops.ψ₂ .+ ops.ψ₃
end

####################################################################
# Optimized
####################################################################
"""
    T6A_Ys7!(ψₒ, ψᵢ, dx, ops)

Compute `ψₒ`, i.e. `ψᵢ` advanced a step `dx` forward using Yoshida's s7 Symplectic Sixth order integrator of type A. The structure `ops::Operators` contains the FFT plans and the kinetic energy operators.

See also: [`solve!`](@ref), [`Operators`](@ref)
"""
function T6A_Ys7!(ψₒ, ψᵢ, dx, ops)
    γ₁ = 0.78451361047755726381949763
    γ₂ = 0.23557321335935813368479318
    γ₃ = -1.17767998417887100694641568
    γ₄ = 1.31518632068391121888424973
    γ₇ = γ₁
    γ₆ = γ₂
    γ₅ = γ₃

    T2A!(ψₒ, ψᵢ, γ₁*dx, ops)
    T2A!(ψₒ, ψₒ, γ₂*dx, ops)
    T2A!(ψₒ, ψₒ, γ₃*dx, ops)
    T2A!(ψₒ, ψₒ, γ₄*dx, ops)
    T2A!(ψₒ, ψₒ, γ₅*dx, ops)
    T2A!(ψₒ, ψₒ, γ₆*dx, ops)
    T2A!(ψₒ, ψₒ, γ₇*dx, ops)
end

"""
    T6B_Ys7!(ψₒ, ψᵢ, dx, ops)

Compute `ψₒ`, i.e. `ψᵢ` advanced a step `dx` forward using Yoshida's s7 Symplectic Sixth order integrator of type B. The structure `ops::Operators` contains the FFT plans and the kinetic energy operators.

See also: [`solve!`](@ref), [`Operators`](@ref)
"""
function T6B_Ys7!(ψₒ, ψᵢ, dx, ops)
    γ₁ = 0.78451361047755726381949763
    γ₂ = 0.23557321335935813368479318
    γ₃ = -1.17767998417887100694641568
    γ₄ = 1.31518632068391121888424973
    γ₇ = γ₁
    γ₆ = γ₂
    γ₅ = γ₃

    T2B!(ψₒ, ψᵢ, γ₁*dx, ops)
    T2B!(ψₒ, ψₒ, γ₂*dx, ops)
    T2B!(ψₒ, ψₒ, γ₃*dx, ops)
    T2B!(ψₒ, ψₒ, γ₄*dx, ops)
    T2B!(ψₒ, ψₒ, γ₅*dx, ops)
    T2B!(ψₒ, ψₒ, γ₆*dx, ops)
    T2B!(ψₒ, ψₒ, γ₇*dx, ops)
end

"""
    T6A_KLs9!(ψₒ, ψᵢ, dx, ops)

Compute `ψₒ`, i.e. `ψᵢ` advanced a step `dx` forward using Kahan & Li's s9 Symplectic Sixth order integrator of type A. The structure `ops::Operators` contains the FFT plans and the kinetic energy operators.

See also: [`solve!`](@ref), [`Operators`](@ref)
"""
function T6A_KLs9!(ψₒ, ψᵢ, dx, ops)
    γ₁ = 0.39216144400731413927925056
    γ₂ = 0.33259913678935943859974864
    γ₃ = -0.70624617255763935980996482
    γ₄ =  0.08221359629355080023149045
    γ₅ = 0.79854399093482996339895035
    γ₉ = γ₁
    γ₈ = γ₂
    γ₇ = γ₃
    γ₆ = γ₄

    T2A!(ψₒ, ψᵢ, γ₁*dx, ops)
    T2A!(ψₒ, ψₒ, γ₂*dx, ops)
    T2A!(ψₒ, ψₒ, γ₃*dx, ops)
    T2A!(ψₒ, ψₒ, γ₄*dx, ops)
    T2A!(ψₒ, ψₒ, γ₅*dx, ops)
    T2A!(ψₒ, ψₒ, γ₆*dx, ops)
    T2A!(ψₒ, ψₒ, γ₇*dx, ops)
    T2A!(ψₒ, ψₒ, γ₈*dx, ops)
    T2A!(ψₒ, ψₒ, γ₉*dx, ops)
end

"""
    T6B_KLs9!(ψₒ, ψᵢ, dx, ops)

Compute `ψₒ`, i.e. `ψᵢ` advanced a step `dx` forward using Kahan & Li's s9 Symplectic Sixth order integrator of type B. The structure `ops::Operators` contains the FFT plans and the kinetic energy operators.

See also: [`solve!`](@ref), [`Operators`](@ref)
"""
function T6B_KLs9!(ψₒ, ψᵢ, dx, ops)
    γ₁ = 0.39216144400731413927925056
    γ₂ = 0.33259913678935943859974864
    γ₃ = -0.70624617255763935980996482
    γ₄ =  0.08221359629355080023149045
    γ₅ = 0.79854399093482996339895035
    γ₉ = γ₁
    γ₈ = γ₂
    γ₇ = γ₃
    γ₆ = γ₄

    T2B!(ψₒ, ψᵢ, γ₁*dx, ops)
    T2B!(ψₒ, ψₒ, γ₂*dx, ops)
    T2B!(ψₒ, ψₒ, γ₃*dx, ops)
    T2B!(ψₒ, ψₒ, γ₄*dx, ops)
    T2B!(ψₒ, ψₒ, γ₅*dx, ops)
    T2B!(ψₒ, ψₒ, γ₆*dx, ops)
    T2B!(ψₒ, ψₒ, γ₇*dx, ops)
    T2B!(ψₒ, ψₒ, γ₈*dx, ops)
    T2B!(ψₒ, ψₒ, γ₉*dx, ops)
end

"""
    T6A_Ss14!(ψₒ, ψᵢ, dx, ops)

Compute `ψₒ`, i.e. `ψᵢ` advanced a step `dx` forward using Suzuki's s14 Symplectic Sixth order integrator of type A. The structure `ops::Operators` contains the FFT plans and the kinetic energy operators.

See also: [`solve!`](@ref), [`Operators`](@ref)
"""
function T6A_Ss14!(ψₒ, ψᵢ, dx, ops)
    γ₁ = 0.392256805238773
    γ₂ = γ₁
    γ₁₃ = γ₁
    γ₁₄ = γ₁
    γ₃ = 0.1177866066796810
    γ₄ = γ₃
    γ₁₁ = γ₃
    γ₁₂ = γ₃
    γ₅ = -0.5888399920894384
    γ₆ = γ₅
    γ₉ = γ₅
    γ₁₀ = γ₅
    γ₇ = 0.6575931603419684
    γ₈ = γ₇

    T2A!(ψₒ, ψᵢ, γ₁*dx, ops)
    T2A!(ψₒ, ψₒ, γ₂*dx, ops)
    T2A!(ψₒ, ψₒ, γ₃*dx, ops)
    T2A!(ψₒ, ψₒ, γ₄*dx, ops)
    T2A!(ψₒ, ψₒ, γ₅*dx, ops)
    T2A!(ψₒ, ψₒ, γ₆*dx, ops)
    T2A!(ψₒ, ψₒ, γ₇*dx, ops)
    T2A!(ψₒ, ψₒ, γ₈*dx, ops)
    T2A!(ψₒ, ψₒ, γ₉*dx, ops)
    T2A!(ψₒ, ψₒ, γ₁₀*dx, ops)
    T2A!(ψₒ, ψₒ, γ₁₁*dx, ops)
    T2A!(ψₒ, ψₒ, γ₁₂*dx, ops)
    T2A!(ψₒ, ψₒ, γ₁₃*dx, ops)
    T2A!(ψₒ, ψₒ, γ₁₄*dx, ops)
end

"""
    T6B_Ss14!(ψₒ, ψᵢ, dx, ops)

Compute `ψₒ`, i.e. `ψᵢ` advanced a step `dx` forward using Suzuki's s14 Symplectic Sixth order integrator of type B. The structure `ops::Operators` contains the FFT plans and the kinetic energy operators.

See also: [`solve!`](@ref), [`Operators`](@ref)
"""
function T6B_Ss14!(ψₒ, ψᵢ, dx, ops)
    γ₁ = 0.392256805238773
    γ₂ = γ₁
    γ₁₃ = γ₁
    γ₁₄ = γ₁
    γ₃ = 0.1177866066796810
    γ₄ = γ₃
    γ₁₁ = γ₃
    γ₁₂ = γ₃
    γ₅ = -0.5888399920894384
    γ₆ = γ₅
    γ₉ = γ₅
    γ₁₀ = γ₅
    γ₇ = 0.6575931603419684
    γ₈ = γ₇

    T2B!(ψₒ, ψᵢ, γ₁*dx, ops)
    T2B!(ψₒ, ψₒ, γ₂*dx, ops)
    T2B!(ψₒ, ψₒ, γ₃*dx, ops)
    T2B!(ψₒ, ψₒ, γ₄*dx, ops)
    T2B!(ψₒ, ψₒ, γ₅*dx, ops)
    T2B!(ψₒ, ψₒ, γ₆*dx, ops)
    T2B!(ψₒ, ψₒ, γ₇*dx, ops)
    T2B!(ψₒ, ψₒ, γ₈*dx, ops)
    T2B!(ψₒ, ψₒ, γ₉*dx, ops)
    T2B!(ψₒ, ψₒ, γ₁₀*dx, ops)
    T2B!(ψₒ, ψₒ, γ₁₁*dx, ops)
    T2B!(ψₒ, ψₒ, γ₁₂*dx, ops)
    T2B!(ψₒ, ψₒ, γ₁₃*dx, ops)
    T2B!(ψₒ, ψₒ, γ₁₄*dx, ops)
end

"""
    T8A_Ss15!(ψₒ, ψᵢ, dx, ops)

Compute `ψₒ`, i.e. `ψᵢ` advanced a step `dx` forward using Suzuki's s15 Symplectic Eighth order integrator of type A. The structure `ops::Operators` contains the FFT plans and the kinetic energy operators.

See also: [`solve!`](@ref), [`Operators`](@ref)
"""
function T8A_Ss15!(ψₒ, ψᵢ, dx, ops)
    γ₁ = 0.7416703643506129534482278
    γ₂ = -0.409100825800031593997300
    γ₃ = 0.1907547102962383799538763
    γ₄ = -0.5738624711160822666563877
    γ₅ = 0.2990641813036559238444635
    γ₆ = 0.3346249182452981837849580
    γ₇ = 0.3152930923967665966320567
    γ₈ = -0.7968879393529163540197888
    γ₉ = γ₇
    γ₁₀ = γ₆
    γ₁₁ = γ₅
    γ₁₂ = γ₄
    γ₁₃ = γ₃
    γ₁₄ = γ₂
    γ₁₅ = γ₁

    T2A!(ψₒ, ψᵢ, γ₁*dx, ops)
    T2A!(ψₒ, ψₒ, γ₂*dx, ops)
    T2A!(ψₒ, ψₒ, γ₃*dx, ops)
    T2A!(ψₒ, ψₒ, γ₄*dx, ops)
    T2A!(ψₒ, ψₒ, γ₅*dx, ops)
    T2A!(ψₒ, ψₒ, γ₆*dx, ops)
    T2A!(ψₒ, ψₒ, γ₇*dx, ops)
    T2A!(ψₒ, ψₒ, γ₈*dx, ops)
    T2A!(ψₒ, ψₒ, γ₉*dx, ops)
    T2A!(ψₒ, ψₒ, γ₁₀*dx, ops)
    T2A!(ψₒ, ψₒ, γ₁₁*dx, ops)
    T2A!(ψₒ, ψₒ, γ₁₂*dx, ops)
    T2A!(ψₒ, ψₒ, γ₁₃*dx, ops)
    T2A!(ψₒ, ψₒ, γ₁₄*dx, ops)
    T2A!(ψₒ, ψₒ, γ₁₅*dx, ops)
end

"""
    T8B_Ss15!(ψₒ, ψᵢ, dx, ops)

Compute `ψₒ`, i.e. `ψᵢ` advanced a step `dx` forward using Suzuki's s15 Symplectic Eighth order integrator of type B. The structure `ops::Operators` contains the FFT plans and the kinetic energy operators.

See also: [`solve!`](@ref), [`Operators`](@ref)
"""
function T8B_Ss15!(ψₒ, ψᵢ, dx, ops)
    γ₁ = 0.7416703643506129534482278
    γ₂ = -0.409100825800031593997300
    γ₃ = 0.1907547102962383799538763
    γ₄ = -0.5738624711160822666563877
    γ₅ = 0.2990641813036559238444635
    γ₆ = 0.3346249182452981837849580
    γ₇ = 0.3152930923967665966320567
    γ₈ = -0.7968879393529163540197888
    γ₉ = γ₇
    γ₁₀ = γ₆
    γ₁₁ = γ₅
    γ₁₂ = γ₄
    γ₁₃ = γ₃
    γ₁₄ = γ₂
    γ₁₅ = γ₁

    T2B!(ψₒ, ψᵢ, γ₁*dx, ops)
    T2B!(ψₒ, ψₒ, γ₂*dx, ops)
    T2B!(ψₒ, ψₒ, γ₃*dx, ops)
    T2B!(ψₒ, ψₒ, γ₄*dx, ops)
    T2B!(ψₒ, ψₒ, γ₅*dx, ops)
    T2B!(ψₒ, ψₒ, γ₆*dx, ops)
    T2B!(ψₒ, ψₒ, γ₇*dx, ops)
    T2B!(ψₒ, ψₒ, γ₈*dx, ops)
    T2B!(ψₒ, ψₒ, γ₉*dx, ops)
    T2B!(ψₒ, ψₒ, γ₁₀*dx, ops)
    T2B!(ψₒ, ψₒ, γ₁₁*dx, ops)
    T2B!(ψₒ, ψₒ, γ₁₂*dx, ops)
    T2B!(ψₒ, ψₒ, γ₁₃*dx, ops)
    T2B!(ψₒ, ψₒ, γ₁₄*dx, ops)
    T2B!(ψₒ, ψₒ, γ₁₅*dx, ops)
end

"""
    T8A_KLs17!(ψₒ, ψᵢ, dx, ops)

Compute `ψₒ`, i.e. `ψᵢ` advanced a step `dx` forward using Kahan & Li's s17 Symplectic Eighth order integrator of type A. The structure `ops::Operators` contains the FFT plans and the kinetic energy operators.

See also: [`solve!`](@ref), [`Operators`](@ref)
"""
function T8A_KLs17!(ψₒ, ψᵢ, dx, ops)
    γ₁ = 0.13020248308889008087881763
    γ₂ = 0.56116298177510838456196441
    γ₃ =  -0.38947496264484728640807860
    γ₄ = 0.15884190655515560089621075
    γ₅ = -0.39590389413323757733623154
    γ₆ = 0.18453964097831570709183254
    γ₇ = 0.25837438768632204729397911
    γ₈ = 0.29501172360931029887096624
    γ₉ = -0.60550853383003451169892108
    γ₁₇ = γ₁
    γ₁₆ = γ₂
    γ₁₅ = γ₃
    γ₁₄ = γ₄
    γ₁₃ = γ₅
    γ₁₂ = γ₆
    γ₁₁ = γ₇
    γ₁₀ = γ₈

    T2A!(ψₒ, ψᵢ, γ₁*dx, ops)
    T2A!(ψₒ, ψₒ, γ₂*dx, ops)
    T2A!(ψₒ, ψₒ, γ₃*dx, ops)
    T2A!(ψₒ, ψₒ, γ₄*dx, ops)
    T2A!(ψₒ, ψₒ, γ₅*dx, ops)
    T2A!(ψₒ, ψₒ, γ₆*dx, ops)
    T2A!(ψₒ, ψₒ, γ₇*dx, ops)
    T2A!(ψₒ, ψₒ, γ₈*dx, ops)
    T2A!(ψₒ, ψₒ, γ₉*dx, ops)
    T2A!(ψₒ, ψₒ, γ₁₀*dx, ops)
    T2A!(ψₒ, ψₒ, γ₁₁*dx, ops)
    T2A!(ψₒ, ψₒ, γ₁₂*dx, ops)
    T2A!(ψₒ, ψₒ, γ₁₃*dx, ops)
    T2A!(ψₒ, ψₒ, γ₁₄*dx, ops)
    T2A!(ψₒ, ψₒ, γ₁₅*dx, ops)
    T2A!(ψₒ, ψₒ, γ₁₆*dx, ops)
    T2A!(ψₒ, ψₒ, γ₁₇*dx, ops)
end

"""
    T8B_KLs17!(ψₒ, ψᵢ, dx, ops)

Compute `ψₒ`, i.e. `ψᵢ` advanced a step `dx` forward using Kahan & Li's s17 Symplectic Eighth order integrator of type B. The structure `ops::Operators` contains the FFT plans and the kinetic energy operators.

See also: [`solve!`](@ref), [`Operators`](@ref)
"""
function T8B_KLs17!(ψₒ, ψᵢ, dx, ops)
    γ₁ = 0.13020248308889008087881763
    γ₂ = 0.56116298177510838456196441
    γ₃ =  -0.38947496264484728640807860
    γ₄ = 0.15884190655515560089621075
    γ₅ = -0.39590389413323757733623154
    γ₆ = 0.18453964097831570709183254
    γ₇ = 0.25837438768632204729397911
    γ₈ = 0.29501172360931029887096624
    γ₉ = -0.60550853383003451169892108
    γ₁₇ = γ₁
    γ₁₆ = γ₂
    γ₁₅ = γ₃
    γ₁₄ = γ₄
    γ₁₃ = γ₅
    γ₁₂ = γ₆
    γ₁₁ = γ₇
    γ₁₀ = γ₈

    T2B!(ψₒ, ψᵢ, γ₁*dx, ops)
    T2B!(ψₒ, ψₒ, γ₂*dx, ops)
    T2B!(ψₒ, ψₒ, γ₃*dx, ops)
    T2B!(ψₒ, ψₒ, γ₄*dx, ops)
    T2B!(ψₒ, ψₒ, γ₅*dx, ops)
    T2B!(ψₒ, ψₒ, γ₆*dx, ops)
    T2B!(ψₒ, ψₒ, γ₇*dx, ops)
    T2B!(ψₒ, ψₒ, γ₈*dx, ops)
    T2B!(ψₒ, ψₒ, γ₉*dx, ops)
    T2B!(ψₒ, ψₒ, γ₁₀*dx, ops)
    T2B!(ψₒ, ψₒ, γ₁₁*dx, ops)
    T2B!(ψₒ, ψₒ, γ₁₂*dx, ops)
    T2B!(ψₒ, ψₒ, γ₁₃*dx, ops)
    T2B!(ψₒ, ψₒ, γ₁₄*dx, ops)
    T2B!(ψₒ, ψₒ, γ₁₅*dx, ops)
    T2B!(ψₒ, ψₒ, γ₁₆*dx, ops)
    T2B!(ψₒ, ψₒ, γ₁₇*dx, ops)
end


text:
@testset "ch09_logger2" begin
    dt = 3.0
    # environment
    xlim = [-5.0, 5.0]
    ylim = [-5.0, 5.0]
    # id of landmark must start from 0 with 1 step
    landmarks = [
        Landmark([-4.0, 2.0], 0),
        Landmark([2.0, -3.0], 1),
        Landmark([3.0, 3.0], 2),
        Landmark([0.0, 4.0], 3),
        Landmark([1.0, 1.0], 4),
        Landmark([-3.0, -1.0], 5),
    ]
    envmap = Map()
    push!(envmap, landmarks)
    world = World(xlim, ylim)
    push!(world, envmap)
    # robot side
    initial_pose = [0.0, -3.0, 0.0]
    logger_agent = LoggerAgent(0.2, 5.0 * pi / 180, dt, initial_pose)
    robot = RealRobot(initial_pose, logger_agent, PsiCamera(landmarks); color = "red")
    push!(world, robot)
    anim = @animate for i = 1:60
        t = dt * i
        annota = "t = $(round(t, sigdigits=3))[s]"
        z = observations(robot.sensor_, robot.pose_)
        v, ω = decision(logger_agent, z)
        p = draw(world, annota)
        state_transition(robot, v, ω, dt; move_noise = true, vel_bias_noise = true)
    end
    if GUI
        gif(anim, "ch09_logger2.gif", fps = 0.5)
    end
end

using Plots, Random, Distributions, LinearAlgebra, JuliaProbo
import Plots: Plot
gr();

abstract type AbstractEdge end
struct ObsEdge <: AbstractEdge
    t1::Int64
    t2::Int64
    x1::Vector{Float64}
    x2::Vector{Float64}
    z1::Vector{Float64}
    z2::Vector{Float64}
    ω_ul::Matrix{Float64}
    ω_ur::Matrix{Float64}
    ω_ll::Matrix{Float64}
    ω_lr::Matrix{Float64}
    xi_u::Vector{Float64}
    xi_l::Vector{Float64}
    function ObsEdge(
        t1::Int64,
        t2::Int64,
        z1_::Vector{Float64},
        z2_::Vector{Float64},
        xs::Dict{Int64,Vector{Float64}},
        sensor_noise_rate = [0.14, 0.05, 0.05],
    )
        z1_[1] == z2_[1] || error("Landmark Ids do not match")
        x1 = copy(xs[t1])
        x2 = copy(xs[t2])
        z1 = z1_[2:4]
        z2 = z2_[2:4]
        θ1 = x1[3]
        ϕ1 = z1[2]
        θ2 = x2[3]
        ϕ2 = z2[2]
        s1 = sin(θ1 + ϕ1)
        c1 = cos(θ1 + ϕ1)
        s2 = sin(θ2 + ϕ2)
        c2 = cos(θ2 + ϕ2)

        ê = [
            z2[1] * c2 - z1[1] * c1,
            z2[1] * s2 - z1[1] * s1,
            z2[2] - z2[3] - z1[2] + z1[3],
        ]
        ê = ê + x2 - x1
        while ê[3] >= pi
            ê[3] -= 2 * pi
        end
        while ê[3] < -pi
            ê[3] += 2 * pi
        end

        Q₁ = Diagonal([
            (z1[1] * sensor_noise_rate[1])^2,
            sensor_noise_rate[2]^2,
            sensor_noise_rate[3]^2,
        ])
        R₁ = -[c1 (-z1[1]*s1) 0; s1 (z1[1]*c1) 0; 0 1 -1]
        Q₂ = Diagonal([
            (z2[1] * sensor_noise_rate[1])^2,
            sensor_noise_rate[2]^2,
            sensor_noise_rate[3]^2,
        ])
        R₂ = [c2 (-z2[1]*s2) 0; s2 (z2[1]*c2) 0; 0 1 -1]
        Σ = R₁ * Q₁ * transpose(R₁) + R₂ * Q₂ * transpose(R₂)
        Ω = inv(Σ)
        B1 = -[1 0 -z1[1]*s1; 0 1 z1[1]*c1; 0 0 1]
        B2 = [1 0 -z2[1]*s2; 0 1 z2[1]*c2; 0 0 1]
        ω_ul = transpose(B1) * Ω * B1
        ω_ur = transpose(B1) * Ω * B2
        ω_ll = transpose(B2) * Ω * B1
        ω_lr = transpose(B2) * Ω * B2
        xi_u = -transpose(B1) * Ω * ê
        xi_l = -transpose(B2) * Ω * ê
        new(t1, t2, x1, x2, z1, z2, ω_ul, ω_ur, ω_ll, ω_lr, xi_u, xi_l)
    end
end

mutable struct MotionEdge <: AbstractEdge
    t1::Int64
    t2::Int64
    x̂₁::Vector{Float64}
    x̂₂::Vector{Float64}
    ω_ul::Matrix{Float64}
    ω_ur::Matrix{Float64}
    ω_ll::Matrix{Float64}
    ω_lr::Matrix{Float64}
    xi_u::Vector{Float64}
    xi_l::Vector{Float64}
    function MotionEdge(
        t1::Int64,
        t2::Int64,
        xs::Dict{Int64,Vector{Float64}},
        us::Dict{Int64,Vector{Float64}},
        δ::Float64,
        motion_noise_stds = Dict("vv" => 0.19, "vω" => 0.001, "ωv" => 0.13, "ωω" => 0.2),
    )
        x̂₁ = copy(xs[t1])
        x̂₂ = copy(xs[t2])
        v, ω = us[t2][1], us[t2][2]
        if abs(ω) < 1e-5
            ω = sign(ω) * 1e-5
        end

        M = matM(v, ω, δ, motion_noise_stds)
        A = matA(v, ω, δ, x̂₁[3])
        F = matF(v, ω, δ, x̂₁[3])

        Ω = inv(A * M * transpose(A) + Matrix(0.0001I, 3, 3))
        ω_ul = transpose(F) * Ω * F
        ω_ur = -transpose(F) * Ω
        ω_ll = -Ω * F
        ω_lr = Ω
        x2 = state_transition(x̂₁, v, ω, δ)
        xi_u = transpose(F) * Ω * (x̂₂ - x2)
        xi_l = -Ω * (x̂₂ - x2)
        new(t1, t2, x̂₁, x̂₂, ω_ul, ω_ur, ω_ll, ω_lr, xi_u, xi_l)
    end
end

struct MapEdge <: AbstractEdge
    x::Vector{Float64}
    z::Vector{Float64}
    m::Vector{Float64}
    Ω::Matrix{Float64}
    xi::Vector{Float64}
    function MapEdge(
        t::Int64,
        z::Vector{Float64},
        head_t::Int64,
        head_z::Vector{Float64},
        xs::Dict{Int64,Vector{Float64}},
        sensor_noise_rate = [0.14, 0.05, 0.05],
    )
        x = copy(xs[t])
        m =
            x + [
                z[1] * cos(x[3] + z[2]),
                z[1] * sin(x[3] + z[2]),
                -xs[head_t][3] + z[2] - head_z[2] - z[3] + head_z[3],
            ]
        while m[3] >= pi
            m[3] -= 2.0 * pi
        end
        while m[3] < -pi
            m[3] += 2.0 * pi
        end
        Q₁ = Diagonal([
            (z[1] * sensor_noise_rate[1])^2,
            sensor_noise_rate[2]^2,
            sensor_noise_rate[3]^2,
        ])
        s1 = sin(x[3] + z[2])
        c1 = cos(x[3] + z[2])
        R = [-c1 (z[1]*s1) 0; -s1 (-z[1]*c1) 0; 0 -1 1]
        Ω = inv(R * Q₁ * transpose(R))
        xi = Ω * m
        new(x, copy(z), m, Ω, xi)
    end
end

function make_landmarks(hat_xs, zlist_landmark)
    ms = Dict{Int64,Vector{Float64}}()
    for (landmark_id, zlist) in zlist_landmark
        edges = Vector{MapEdge}(undef, 0)
        head_z = zlist[1]
        for z in zlist
            # step, ID of landmark, d, ϕ, m
            push!(
                edges,
                MapEdge(
                    convert(Int64, z[1]),
                    z[3:5],
                    convert(Int64, head_z[1]),
                    head_z[3:5],
                    hat_xs,
                ),
            )
        end
        average = sum([e.m for e in edges], dims = 1) / length(edges)

        Ω = zeros(Float64, 3, 3)
        xi = zeros(Float64, 3)
        for e in edges
            Ω += e.Ω
            xi += e.xi
        end
        ms[landmark_id] = inv(Ω) * xi
    end
    return ms
end

function make_ax(xlim, ylim)
    p = plot(aspect_ratio = :equal, xlim = xlim, ylim = ylim, xlabel = "X", ylabel = "Y")
    return p
end

function make_edges(
    hat_xs::Dict{Int64,Vector{Float64}},
    zlist::Dict{Int64,Vector{Vector{Float64}}},
)
    landmark_keys_zlist = Dict{Int64,Vector{Vector{Float64}}}()

    for (step, zs) in zlist
        for i = 1:lastindex(zs)
            z = zs[i]
            landmark_id = convert(Int64, z[1])
            if !haskey(landmark_keys_zlist, landmark_id)
                landmark_keys_zlist[landmark_id] = Vector{Vector{Float64}}(undef, 0)
            end
            # step, ID of landmark, d, ϕ, m
            push!(landmark_keys_zlist[landmark_id], vcat([step * 1.0], z))
        end
    end

    edges = Vector{AbstractEdge}(undef, 0)

    for (landmark_id, lm_zlist) in landmark_keys_zlist
        for i = 1:lastindex(lm_zlist)
            for j = (i+1):lastindex(lm_zlist)
                z1 = lm_zlist[i]
                z2 = lm_zlist[j]
                t1 = convert(Int64, z1[1])
                t2 = convert(Int64, z2[1])
                push!(edges, ObsEdge(t1, t2, z1[2:5], z2[2:5], hat_xs))
            end
        end
    end

    return edges, landmark_keys_zlist
end

function add_edge!(edge::AbstractEdge, Ω::Matrix{Float64}, xi::Vector{Float64})
    f1, f2 = edge.t1 * 3 + 1, edge.t2 * 3 + 1
    t1, t2 = f1 + 2, f2 + 2
    Ω[f1:t1, f1:t1] += edge.ω_ul
    Ω[f1:t1, f2:t2] += edge.ω_ur
    Ω[f2:t2, f1:t1] += edge.ω_ll
    Ω[f2:t2, f2:t2] += edge.ω_lr
    xi[f1:t1] += edge.xi_u
    xi[f2:t2] += edge.xi_l
end

function draw_traj!(xs, p::Plot{T}) where {T}
    # xs is dictionary
    poses = [xs[s] for s = 0:length(xs)-1]
    p = scatter!(
        [e[1] for e in poses],
        [e[2] for e in poses],
        markershape = :circle,
        markersize = 1,
        color = "black",
    )
    p = plot!(
        [e[1] for e in poses],
        [e[2] for e in poses],
        linewidth = 0.5,
        color = "black",
    )
end

function draw_observations!(
    xs::Dict{Int64,Vector{Float64}},
    zlist::Dict{Int64,Vector{Vector{Float64}}},
    p::Plot{T},
) where {T}
    for s = 0:length(xs)-1
        if !haskey(zlist, s)
            continue
        end
        v = zlist[s]
        N = size(v)[1]
        for i = 1:N
            x, y, θ = xs[s][1], xs[s][2], xs[s][3]
            obsv = v[i]
            d, ϕ = obsv[2], obsv[3]
            mx = x + d * cos(θ + ϕ)
            my = y + d * sin(θ + ϕ)
            p = plot!([x, mx], [y, my], color = "pink", alpha = 0.5)
        end
    end
end

function draw_edges!(edges::Vector{ObsEdge}, p::Plot{T}) where {T}
    for edge in edges
        p = plot!(
            [edge.x1[1], edge.x2[1]],
            [edge.x1[2], edge.x2[2]],
            color = "red",
            alpha = 0.5,
        )
    end
end

function draw_all(
    xlim,
    ylim,
    xs::Dict{Int64,Vector{Float64}},
    zlist::Dict{Int64,Vector{Vector{Float64}}},
    p::Plot{T};
    landmarks = Dict{Int64,Vector{Float64}}(),
) where {T}
    p = make_ax(xlim, ylim)
    draw_observations!(xs, zlist, p)
    draw_traj!(xs, p)
    for (k, landmark) in landmarks
        p = scatter!(
            [landmark[1]],
            [landmark[2]],
            markersize = 10,
            markershape = :star,
            color = "blue",
        )
    end
    # draw_edges!(edges, p)
    return p
end

# hat_xs contains all the keys in 0:length(xs), but z does not (there are some time steps with no observation)
function read_data(fname = "log.txt")
    hat_xs = Dict{Int64,Vector{Float64}}()
    zlist = Dict{Int64,Vector{Vector{Float64}}}()
    delta = 0.0
    us = Dict{Int64,Vector{Float64}}()

    fd = open(fname, "r")
    lines = readlines(fd)
    for i = 1:length(lines)
        line = lines[i]
        tokens = split(line, ' ')
        if tokens[1] == "delta"
            delta = parse(Float64, tokens[2])
            continue
        end
        step = parse(Int64, tokens[2])
        if tokens[1] == "x"
            # x, y, θ
            hat_xs[step] = [
                parse(Float64, tokens[3]),
                parse(Float64, tokens[4]),
                parse(Float64, tokens[5]),
            ]
        elseif tokens[1] == "z"
            if !haskey(zlist, step)
                zlist[step] = Vector{Vector{Float64}}(undef, 0)
            end
            # ID of landmark, d, ϕ, m
            z = [
                parse(Float64, tokens[3]),
                parse(Float64, tokens[4]),
                parse(Float64, tokens[5]),
                parse(Float64, tokens[6]),
            ]
            push!(zlist[step], copy(z))
        elseif tokens[1] == "u"
            us[step] = [parse(Float64, tokens[3]), parse(Float64, tokens[4])]
        end
    end
    return hat_xs, zlist, us, delta
end

@testset "ch09_graphslam9" begin
    hat_xs, zlist, us, delta = read_data("log.txt")
    dim = length(hat_xs) * 3 # dimension of trajectory
    diff1 = 0.0
    diff2 = 0.0
    xlim = [-5.0, 5.0]
    ylim = [-5.0, 5.0]
    xpos = xlim[1] + (xlim[2] - xlim[1]) * 0.3
    ypos = ylim[2] - 0.05 * (ylim[2] - ylim[1])

    anim = @animate for cnt = 1:1000
        edges, zlist_landmark = make_edges(hat_xs, zlist)

        for i = 0:length(hat_xs)-2
            push!(edges, MotionEdge(i, i + 1, hat_xs, us, delta))
        end
        Ω = zeros(Float64, dim, dim)
        xi = zeros(Float64, dim)
        Ω[1:3, 1:3] += Matrix(10000000.0I, 3, 3)

        for edge in edges
            add_edge!(edge, Ω, xi)
        end

        Δxs = inv(Ω) * xi

        for i = 0:length(hat_xs)-1
            hat_xs[i] += Δxs[i*3+1:(i+1)*3]
        end

        diff2 = diff1
        diff1 = norm(Δxs)
        annota = "$(cnt)-th iter: ϵ = $(round(diff1, sigdigits=3))"

        landmarks = make_landmarks(hat_xs, zlist_landmark)
        p = make_ax(xlim, ylim)
        p = draw_all(xlim, ylim, hat_xs, zlist, p; landmarks = landmarks)
        p = annotate!(xpos, ypos, annota)
        plot(p, legend = nothing)
        if abs(diff2 - diff1) < 0.00005
            break
        end
    end
    if GUI
        gif(anim, "ch09_graphslam8.gif", fps = 1)
    end
end

max_stars_repo_name: soblin/JuliaProbo

max_issues_repo_name: soblin/JuliaProbo

max_forks_repo_name: soblin/JuliaProbo


text:
export gpu_info, get_gpu, has_gpu

"""
    gpu_info()

Returns the CUDA and GPU information. An examplary output is
```
- NVCC: /usr/local/cuda/bin/nvcc
- CUDA library directories: /usr/local/cuda/lib64
- Latest supported version of CUDA: 11000
- CUDA runtime version: 10010
- CUDA include_directories: /usr/local/cuda/include
- CUDA toolkit directories: /home/kailaix/.julia/adcme/pkgs/cudatoolkit-10.0.130-0/lib:/home/kailaix/.julia/adcme/pkgs/cudnn-7.6.5-cuda10.0_0/lib
- Number of GPUs: 7
```
"""
function gpu_info()
    NVCC = "missing"
    CUDALIB = "missing"
    v = "missing"
    u = "missing"
    dcount = 0
    try
        NVCC = strip(String(read(`which nvcc`)))
        CUDALIB = abspath(joinpath(NVCC, "../../lib64"))
        v = zeros(Int32,1)
        @eval ccall((:cudaRuntimeGetVersion, $(joinpath(CUDALIB, "libcudart.so"))), Cvoid, (Ref{Cint},), $v)
        u = zeros(Int32,1)
        @eval ccall((:cudaDriverGetVersion, $(joinpath(CUDALIB, "libcudart.so"))), Cvoid, (Ref{Cint},), $u)
        dcount = zeros(Int32, 1)
        @eval ccall((:cudaGetDeviceCount, $(joinpath(CUDALIB, "libcudart.so"))), Cvoid, (Ref{Cint},), $dcount)
        v = v[1]
        if v==0
            v = "missing"
        end
        u = u[1]
        dcount = dcount[1]
    catch

    end

    println("- NVCC: ", NVCC)
    println("- CUDA library directories: ", CUDALIB)
    println("- Latest supported version of CUDA: ", u)
    println("- CUDA runtime version: ", v)
    println("- CUDA include_directories: ", length(ADCME.CUDA_INC)==0 ? "missing" : ADCME.CUDA_INC)
    println("- CUDA toolkit directories: ", length(ADCME.LIBCUDA)==0 ? "missing" : ADCME.LIBCUDA)
    println("- Number of GPUs: ", dcount)

    if NVCC == "missing"
        println("\nTips: nvcc is not found in the path. Please add nvcc to your environment path if you intend to use GPUs.")
    end

    if length(ADCME.CUDA_INC)==0
        println("\nTips: ADCME is not configured to use GPUs. See https://kailaix.github.io/ADCME.jl/latest/tu_customop/#Install-GPU-enabled-TensorFlow-(Linux-and-Windows) for instructions.")
    end

    if dcount==0
        println("\nTips: No GPU resources found. Do you have access to GPUs?")
    end
end

"""
    get_gpu()

Returns the compiler information for GPUs. An examplary output is
> (NVCC = "/usr/local/cuda/bin/nvcc", LIB = "/usr/local/cuda/lib64", INC = "/usr/local/cuda/include", TOOLKIT = "/home/kailaix/.julia/adcme/pkgs/cudatoolkit-10.0.130-0/lib", CUDNN = "/home/kailaix/.julia/adcme/pkgs/cudnn-7.6.5-cuda10.0_0/lib")

"""
function get_gpu()
    NVCC = missing
    CUDALIB = missing
    CUDAINC = missing
    CUDATOOLKIT = missing
    CUDNN = missing
    try
        NVCC = strip(String(read(`which nvcc`)))
        CUDALIB = abspath(joinpath(NVCC, "../../lib64"))
        CUDAINC = abspath(joinpath(NVCC, "../../include"))
        if length(ADCME.CUDA_INC)>0 && CUDAINC!=ADCME.CUDA_INC
            @warn """
Inconsistency detected:
ADCME CUDAINC: $(ADCME.CUDA_INC)
Implied CUDAINC: $CUDAINC
"""
        end
    catch
    end
    if length(ADCME.LIBCUDA)>0
        CUDATOOLKIT = split(ADCME.LIBCUDA, ":")[1]
        CUDNN = split(ADCME.LIBCUDA, ":")[2]
    else
    end
    return (NVCC=NVCC, LIB=CUDALIB, INC=CUDAINC, TOOLKIT=CUDATOOLKIT, CUDNN=CUDNN)
end

"""
    has_gpu()

Check if the TensorFlow backend is using CUDA GPUs. Operators that have GPU implementations will be executed on GPU devices.
See also [`get_gpu`](@ref)

!!! note
    ADCME will use GPU automatically if GPU is available. To disable GPU, set the environment variable `ENV["CUDA_VISIBLE_DEVICES"]=""` before importing ADCME
"""
function has_gpu()
    s = tf.test.gpu_device_name()
    if length(s)==0
        return false
    else
        return true
    end
end


text:
x = [1, 2, 3, 7]

parr = pointer(x)

xx = unsafe_load(parr, 4)

println(xx)  #  Prints 7

max_stars_repo_name: ethansaxenian/RosettaDecode

max_issues_repo_name: ethansaxenian/RosettaDecode

max_forks_repo_name: ethansaxenian/RosettaDecode