text:
TriangulatedComplex{N,K}() where {N, K} = TriangulatedComplex{N,K}(
    CellComplex{N,K}(), Dict{Cell{N}, Vector{SignedSimpleSimplex{N}}}())

function TriangulatedComplex(simplices::AbstractVector{Simplex{N, K}}) where {N, K}
    # cache to make sure the same simplex isn't turned into a cell twice
    cells = Dict{Set{Point{N}}, Cell{N}}()
    simplex_mapping = Dict{Cell{N}, Vector{SignedSimpleSimplex{N}}}()
    for k in 1:K
        for simplex in simplices
            for s in subsimplices(simplex, k)
                key = Set(s.points)
                if !(key in keys(cells))
                    s_cell = Cell(s)
                    cells[key] = s_cell
                    simplex_mapping[s_cell] = [(SimpleSimplex(s), true)]
                    for face in subsimplices(s, k-1)
                        face_cell = cells[Set(face.points)]
                        i = findfirst(p -> !(p in face.points), s.points)
                        pm = sign_of_permutation(face.points, face_cell.points)
                        parent!(face_cell, s_cell, pm * (1 - 2 * mod(i-1, 2)) > 0)
                    end
                end
            end
        end
    end
    complex = CellComplex{N,K}(collect(values(cells)))
    return TriangulatedComplex{N,K}(complex, simplex_mapping)
end

import Base: show
show(io::IO, tcomp::TriangulatedComplex{N,K}) where {N, K} = print(
     io, "TriangulatedComplex{$N,$K}$(tuple(map(length, tcomp.complex.cells)...))")

Mesh(tcomp::TriangulatedComplex{N}, center::Function) where N =
    Mesh(tcomp, dual(tcomp.complex, center))

show(io::IO, m::Mesh{N,K}) where {N, K} = print(io, "Mesh{$N,$K}($(m.primal), $(m.dual))")

export signed_volume
"""
    signed_volume(m::Metric{N}, tcomp::TriangulatedComplex{N}, c::Cell{N}) where N

Find the signed volume of a cell in a TriangulatedComplex.
"""
signed_volume(m::Metric{N}, tcomp::TriangulatedComplex{N}, c::Cell{N}) where N =
    sum([volume(m, Simplex(s)) * (2 * b - 1) for (s, b) in tcomp.simplices[c]])

export volume
"""
    signed_volume(m::Metric{N}, tcomp::TriangulatedComplex{N}, c::Cell{N}) where N

Find the absolute value of the signed volume of a cell in a TriangulatedComplex.
"""
volume(m::Metric{N}, tcomp::TriangulatedComplex{N}, c::Cell{N}) where N =
    abs(signed_volume(m, tcomp, c))

SimpleBarySimplex{N} = Vector{SimpleBarycentric{N}}

"""
    elementary_duals!(simplices::Dict{Cell{N}, Vector{Tuple{SimpleBarySimplex{N}, Bool}}},
        center::Function, c::Cell{N}) where N

Compute the elementary dual simplices of `cell`. `simplices` is a mapping from primal cells
to dual elementary simplices, specified as Barycentrics along with signs, and is used for
memoization. `center` is a function that takes a `Simplex{N, K}` to a `Barycentric{N, K}`.
"""
function elementary_duals!(simplices::Dict{Cell{N}, Vector{Tuple{SimpleBarySimplex{N}, Bool}}},
    center::Function, c::Cell{N}) where N
    if !(c in keys(simplices))
        c_center = SimpleBarycentric(center(Simplex(c)))
        simplices[c] = Tuple{SimpleBarySimplex{N}, Bool}[]
        if isempty(c.parents)
            push!(simplices[c], ([c_center], true))
        end
        for p in keys(c.parents)
            for (ps, sign) in elementary_duals!(simplices, center, p)
                opposite_index = first_setdiff_index(ps[1].simplex.points, c.points)
                new_sign = (ps[1].coords[opposite_index] >= 0) == sign
                new_barycentrics = [c_center, ps...]
                push!(simplices[c], (new_barycentrics, new_sign))
            end
        end
    end
    return simplices[c]
end

export dual
"""
    dual(primal::CellComplex{N, K}, center::Function) where {N, K}

Compute the dual TriangulatedComplex of a simplicial complex. `center` is a function that
takes a `Simplex{N, K}` to a `Barycentric{N, K}`.
"""
function dual(primal::CellComplex{N, K}, center::Function) where {N, K}
    dual_tcomp = TriangulatedComplex{N, K}()
    primal_to_elementary_duals = Dict{Cell{N}, Vector{Tuple{SimpleBarySimplex{N}, Bool}}}()
    primal_to_duals = Dict{Cell{N}, Cell{N}}()
    # iterate from high to low dimension so the cells of
    # the dual are constructed in order of dimension
    for k in reverse(1:K)
        for cell in primal.cells[k]
            elementary_duals = elementary_duals!(primal_to_elementary_duals, center, cell)
            signed_elementary_duals = [(SimpleSimplex(p[1]), p[2])
                for p in elementary_duals]
            points = unique(vcat([p[1].points for p in signed_elementary_duals]...))
            dual_cell = Cell(points, K-k+1)
            push!(dual_tcomp.complex, dual_cell)
            dual_tcomp.simplices[dual_cell] = signed_elementary_duals
            primal_to_duals[cell] = dual_cell
            for parent in keys(cell.parents)
                o = cell.parents[parent]
                dual_child = primal_to_duals[parent]
                # for k > 1, use o but for k == 1 use !o. This guarantees
                # that d² = 0 and that the dual mesh is positively oriented.
                parent!(dual_child, dual_cell, k == 1 ? !o : o)
            end
        end
    end
    return dual_tcomp
end

"""
    dual(mesh::Mesh{N, K}, c::Cell{N}) where {N, K}

Find the dual cell of `c` in `mesh`.
"""
function dual(mesh::Mesh{N, K}, c::Cell{N}) where {N, K}
    primal = mesh.primal.complex
    dual = mesh.dual.complex
    comp1, comp2 = (c in primal.cells[c.K]) ? (primal, dual) : (dual, primal)
    i = findfirst(isequal(c), comp1.cells[c.K])
    return comp2.cells[K-c.K+1][i]
end


text:
"
    This object manages the hamiltonian environments for an InfiniteMPS
"
mutable struct MPOHamInfEnv{H<:MPOHamiltonian,V,S<:InfiniteMPS} <:AbstractInfEnv
    opp :: H

    dependency :: S
    tol :: Float64
    maxiter :: Int

    lw :: PeriodicArray{V,2}
    rw :: PeriodicArray{V,2}

    lock :: ReentrantLock
end

Base.copy(p::MPOHamInfEnv) = MPOHamInfEnv(p.opp,p.dependency,p.tol,p.maxiter,copy(p.lw),copy(p.rw));

function gen_lw_rw(st::InfiniteMPS{A,B},ham::MPOHamiltonian) where {A,B}
    lw = PeriodicArray{A,2}(undef,length(st),ham.odim)
    rw = PeriodicArray{A,2}(undef,length(st),ham.odim)

    for i = 1:length(st), j = 1:ham.odim
        lw[i,j] = TensorMap(rand,eltype(eltype(st)),space(st.AL[i],1)*space(ham[i,j,1],1)',space(st.AL[i],1))
        rw[i,j] = TensorMap(rand,eltype(eltype(st)),space(st.AR[i],3)'*space(ham[i,1,j],3)',space(st.AR[i],3)')
    end

    return (lw,rw)
end

#randomly initialize envs
function environments(st::InfiniteMPS,ham::MPOHamiltonian;tol::Float64=Defaults.tol,maxiter::Int=Defaults.maxiter)
    (lw,rw) = gen_lw_rw(st,ham);
    envs = MPOHamInfEnv(ham,similar(st),tol,maxiter,lw,rw,ReentrantLock())
    recalculate!(envs,st);
end

function recalculate!(envs::MPOHamInfEnv, nstate)
    sameDspace = reduce((prev,i) -> prev && _lastspace(envs.lw[i,1]) == _firstspace(nstate.CR[i])',1:length(nstate),init=true);

    if !sameDspace
        (envs.lw,envs.rw) = gen_lw_rw(nstate,envs.opp)
    end

    @sync begin
        @Threads.spawn calclw!(envs.lw,nstate,envs.opp,tol = envs.tol,maxiter = envs.maxiter)
        @Threads.spawn calcrw!(envs.rw,nstate,envs.opp,tol = envs.tol,maxiter = envs.maxiter)
    end

    envs.dependency = nstate;

    envs
end

function calclw!(fixpoints,st::InfiniteMPS,ham::MPOHamiltonian; tol = Defaults.tol, maxiter = Defaults.maxiter)
    len = length(st);
    alg = GMRES(tol=tol,maxiter=maxiter)

    #the start element
    leftutil = Tensor(ones,eltype(eltype(st)),space(ham[1,1,1],1))
    @tensor fixpoints[1,1][-1 -2;-3] = l_LL(st)[-1,-3]*conj(leftutil[-2])
    (len>1) && left_cyclethrough!(1,fixpoints,ham,st)

    for i = 2:ham.odim
        prev = copy(fixpoints[1,i]);

        rmul!(fixpoints[1,i],0);
        left_cyclethrough!(i,fixpoints,ham,st)

        if(isid(ham,i)) #identity matrices; do the hacky renormalization

            #subtract fixpoints
            @tensor tosvec[-1 -2;-3] := fixpoints[1,i][-1,-2,-3]-fixpoints[1,i][1,-2,2]*r_LL(st)[2,1]*l_LL(st)[-1,-3]

            (fixpoints[1,i],convhist) = @closure linsolve(tosvec,prev,alg) do x
                x-transfer_left(x,st.AL,st.AL,rvec=r_LL(st),lvec=l_LL(st))
            end

            convhist.converged==0 && @info "calclw failed to converge $(convhist.normres)"

            (len>1) && left_cyclethrough!(i,fixpoints,ham,st)

            #go through the unitcell, again subtracting fixpoints
            for potato in 1:len
                @tensor fixpoints[potato,i][-1 -2;-3]-=fixpoints[potato,i][1,-2,2]*r_LL(st,potato-1)[2,1]*l_LL(st,potato)[-1,-3]
            end

        else
            if reduce((a,b)->a&&b, [contains(ham,x,i,i) for x in 1:len])

                (fixpoints[1,i],convhist) = @closure linsolve(fixpoints[1,i],prev,alg) do x
                    x-transfer_left(x,[ham[j,i,i] for j in 1:len],st.AL,st.AL)
                end

                convhist.converged==0 && @info "calclw failed to converge $(convhist.normres)"

            end
            (len>1) && left_cyclethrough!(i,fixpoints,ham,st)
        end

    end

    return fixpoints
end

function calcrw!(fixpoints,st::InfiniteMPS,ham::MPOHamiltonian; tol = Defaults.tol, maxiter = Defaults.maxiter)
    len = length(st)
    alg = GMRES(tol=tol,maxiter=maxiter);

    #the start element
    rightutil = Tensor(ones,eltype(eltype(st)),space(ham[len,1,1],3))
    @tensor fixpoints[end,end][-1 -2;-3] = r_RR(st)[-1,-3]*conj(rightutil[-2])

    (len>1) && right_cyclethrough!(ham.odim,fixpoints,ham,st) #populate other sites

    for i = (ham.odim-1):-1:1
        prev = copy(fixpoints[end,i])
        rmul!(fixpoints[end,i],0);
        right_cyclethrough!(i,fixpoints,ham,st)

        if(isid(ham,i)) #identity matrices; do the hacky renormalization

            #subtract fixpoints
            @tensor tosvec[-1 -2;-3]:=fixpoints[end,i][-1,-2,-3]-fixpoints[end,i][1,-2,2]*l_RR(st)[2,1]*r_RR(st)[-1,-3]

            (fixpoints[end,i],convhist) = @closure linsolve(tosvec,prev,alg) do x
                x-transfer_right(x,st.AR,st.AR,lvec=l_RR(st),rvec=r_RR(st))
            end
            convhist.converged==0 && @info "calcrw failed to converge $(convhist.normres)"

            len>1 && right_cyclethrough!(i,fixpoints,ham,st)

            #go through the unitcell, again subtracting fixpoints
            for potatoe in 1:len
                @tensor fixpoints[potatoe,i][-1 -2;-3]-=fixpoints[potatoe,i][1,-2,2]*l_RR(st,potatoe+1)[2,1]*r_RR(st,potatoe)[-1,-3]
            end
        else
            if reduce((a,b)->a&&b, [contains(ham,x,i,i) for x in 1:len])

                (fixpoints[end,i],convhist) = @closure linsolve(fixpoints[end,i],prev,alg) do x
                    x-transfer_right(x,[ham[j,i,i] for j in 1:len],st.AR,st.AR)
                end
                convhist.converged==0 && @info "calcrw failed to converge $(convhist.normres)"

            end

            (len>1) && right_cyclethrough!(i,fixpoints,ham,st)
        end
    end

    return fixpoints
end

function left_cyclethrough!(index::Int,fp,ham,st) #see code for explanation
    for i=1:size(fp,1)
        rmul!(fp[i+1,index],0);

        for j=index:-1:1
            contains(ham,i,j,index) || continue

            if isscal(ham,i,j,index)
                fp[i+1,index] += transfer_left(fp[i,j],st.AL[i],st.AL[i])*ham.Os[i,j,index]
            else
                fp[i+1,index] += transfer_left(fp[i,j],ham[i,j,index],st.AL[i],st.AL[i])
            end
        end
    end
end

function right_cyclethrough!(index,fp,ham,st) #see code for explanation
    for i=size(fp,1):(-1):1
        rmul!(fp[i-1,index],0);

        for j=index:ham.odim
            contains(ham,i,index,j) || continue

            if isscal(ham,i,index,j)
                fp[i-1,index] += transfer_right(fp[i,j], st.AR[i], st.AR[i]) * ham.Os[i,index,j]
            else
                fp[i-1,index] += transfer_right(fp[i,j], ham[i,index,j], st.AR[i], st.AR[i])
            end
        end
    end
end

ext: jl


text:
using Test
using JuTeX: @decr!, @negate!
using JuTeX.FixPoint

function range_eq_test(f::Function, g::Function, r::AbstractArray)
    for x in r
        if f(x) != g(x)
            return false
        end
    end
    true
end

@testset "JuTeX.jl" begin

function test_decr!(r::AbstractArray)
    for x in r
        y = x - 1
        @decr! x
        if x != y
            return false
        end
    end
    true
end

function test_negate!(r::AbstractArray)
    for x in r
        y = -x
        @negate! x
        if x != y
            return false
        end
    end
    true
end

@testset "macros.jl" begin
    @test test_decr!(-257:257)
    @test test_negate!(-257:257)
end

@testset "fixpoint.jl" begin
    @test UNITY*2 == TWO
    @test range_eq_test(half, x->div(x, 2, RoundNearestTiesUp), -257:257)
end

end # JuTeX.jl


text:
"""
The four adjacent digits in the 1000-digit number that have the greatest product are 9 × 9 × 8 × 9 = 5832.

73167176531330624919225119674426574742355349194934
96983520312774506326239578318016984801869478851843
85861560789112949495459501737958331952853208805511
12540698747158523863050715693290963295227443043557
66896648950445244523161731856403098711121722383113
62229893423380308135336276614282806444486645238749
30358907296290491560440772390713810515859307960866
70172427121883998797908792274921901699720888093776
65727333001053367881220235421809751254540594752243
52584907711670556013604839586446706324415722155397
53697817977846174064955149290862569321978468622482
83972241375657056057490261407972968652414535100474
82166370484403199890008895243450658541227588666881
16427171479924442928230863465674813919123162824586
17866458359124566529476545682848912883142607690042
24219022671055626321111109370544217506941658960408
07198403850962455444362981230987879927244284909188
84580156166097919133875499200524063689912560717606
05886116467109405077541002256983155200055935729725
71636269561882670428252483600823257530420752963450

Find the thirteen adjacent digits in the 1000-digit number that have the greatest product. What is the value of this product?
"""

function max_adj_digit_prod(series::String, n::Integer)
    max = 0
    for i in 1:(length(series)-n)
        adj_digits = parse.(Int,collect(series[i:i+n-1]))
        digit_product = *(adj_digits...)
        if digit_product > max
            max = digit_product
        end
    end
    return max
end

digits = """73167176531330624919225119674426574742355349194934
96983520312774506326239578318016984801869478851843
85861560789112949495459501737958331952853208805511
12540698747158523863050715693290963295227443043557
66896648950445244523161731856403098711121722383113
62229893423380308135336276614282806444486645238749
30358907296290491560440772390713810515859307960866
70172427121883998797908792274921901699720888093776
65727333001053367881220235421809751254540594752243
52584907711670556013604839586446706324415722155397
53697817977846174064955149290862569321978468622482
83972241375657056057490261407972968652414535100474
82166370484403199890008895243450658541227588666881
16427171479924442928230863465674813919123162824586
17866458359124566529476545682848912883142607690042
24219022671055626321111109370544217506941658960408
07198403850962455444362981230987879927244284909188
84580156166097919133875499200524063689912560717606
05886116467109405077541002256983155200055935729725
71636269561882670428252483600823257530420752963450"""
digits = replace(digits,"\n"=>"")

@assert max_adj_digit_prod(digits,4) == 5832
@show max_adj_digit_prod(digits,13)


text:
module OfflinePluto

export Pluto

include(joinpath(@__DIR__(), "..", "deps", "Pluto", "src", "Pluto.jl"))

end


text:
using PyPlot, PyCall, LinearAlgebra

n = 49
h = 1 / (n+1)
x = h*(1:n)
f(x) = 10exp(2x)*sin(2π*x)

A = SymTridiagonal(-2ones(n), ones(n))
b = h^2*f.(x)
b[1] -= -1
b[end] -= 1
u = A \ b

plot(x, u)
grid(true)


text:
using Logging

# So that macro tests work
# Otherwise, log_test2 uses the default log level of WARNING
@Logging.configure(level=DEBUG)

function log_test()
    println("\nTesting function calls:\n")
    debug("debug message")
    info("info message")
    warn("warning message")
    err("error message")
    critical("critical message")
end

function log_test2()
    println("\nTesting macros:\n")
    @debug("debug message")
    @info("info message")
    @warn("warning message")
    @err("error message")
    @critical("critical message")
end

println("\nSetting level=DEBUG")
Logging.configure(level=DEBUG)
log_test()
log_test2()

println()
println("\nSetting level=INFO")
Logging.configure(level=INFO)
log_test()
log_test2()

println()
println("\nSetting level=WARNING")
Logging.configure(level=WARNING)
log_test()
log_test2()

println()
println("\nSetting level=ERROR")
Logging.configure(level=ERROR)
log_test()
log_test2()

println()
println("\nSetting level=CRITICAL")
Logging.configure(level=CRITICAL)
log_test()
log_test2()


text:
#TODO tests

module PTDF

using DataStructures
using Parameters
using Printf
using SparseArrays, LinearAlgebra

include("utils.jl")

mutable struct Bus
    id::Int
    name::String
end

mutable struct Branch
    from::Int
    to::Int
    r::Float64
    x::Float64
    name::String
end
function get_b(branch::Branch)
    # return branch.x / sqrt(branch.r * branch.r + branch.x * branch.x)
    return 1/branch.x;
end

@with_kw mutable struct Network
    bus_to_i::Dict{Int,Int} = Dict{Int,Int}()
    branch_to_i::Dict{Int,Int} = Dict{Int,Int}()

    branches::Dict{Int,Branch} = Dict{Int,Branch}()
    buses::Dict{Int,Bus} = Dict{Int,Bus}()
    CC0::Dict{Int,Int} = Dict{Int,Int}()
end

function read_network(dir_path)
    network = Network();
    read_buses!(network, dir_path);
    read_branches!(network, dir_path);

    return network
end

function get_or_add!(d::Dict{Int,Int}, i::Int)
    if ! haskey(d, i)
        next_id = length(d) + 1;
        d[i] = next_id;
    end
    return d[i];
end

function read_buses!(network::Network, dir_path)
    open(joinpath(dir_path, "buses.txt"), "r") do file
        for ln in eachline(file)
            # don't read commentted line
            if ln[1] != '#'
                buffer = split_with_space(ln);

                name = buffer[1];
                num = parse(Int, buffer[2]);
                numCC = parse(Int, buffer[3]);
                if numCC == 0
                    id = get_or_add!(network.bus_to_i, num);
                    push!(network.buses, id => Bus(num, name));
                    network.CC0[num] = 1;
                end
            end
        end
    end
end

function cut_branch!(network::Network, i_branch_to_cut::Int)
    cut_branch = pop!(network.branches, i_branch_to_cut)
    @info("removed branch ", cut_branch.name, " from network!")
    #no deletion in branch_to_i to keep the idx and to show the branch in the output but with zeros
    #change branch_to_i ?
    return cut_branch, network
end

function reduced_network(network::Network, i_branch_to_cut::Int)
    # if a new CC is generated even the EOD constraint is no longer global,
    #the two induced networks function independantly
    @warn("TODO : verify that the network is still connected and no new CC was generated!")
    network_l = deepcopy(network)

    return cut_branch!(network_l, i_branch_to_cut)
end

function read_branches!(network::Network, dir_path)
    open(joinpath(dir_path, "branches.txt"), "r") do file
        for ln in eachline(file)
            # don't read commentted line
            if ln[1] != '#'
                buffer = split_with_space(ln);
                name = buffer[1]
                num  = parse(Int, buffer[2]);
                num_or = parse(Int, buffer[3]);
                num_ex = parse(Int, buffer[4]);

                on_CC0 = haskey(network.CC0, num_or) && haskey(network.CC0, num_ex);

                if on_CC0 && num_or != -1  && num_ex != -1
                    id = get_or_add!(network.branch_to_i, num);
                    ior = get_or_add!(network.bus_to_i, num_or);
                    iex = get_or_add!(network.bus_to_i, num_ex);
                    r = parse(Float64, buffer[5]);
                    x = parse(Float64, buffer[6]);
                    push!(network.branches, id => Branch(ior, iex, r, x, name));
                end
            end
        end
    end
end

function get_i_for_bus(network_p::Network, bus_name_p::String)
    bus_id_l = first(filter( x -> x[2].name == bus_name_p , network_p.buses))[2].id
    println(bus_name_p, " ; id ", bus_id_l, " ; i ", network_p.bus_to_i[bus_id_l])
    println("BUSES:\n", network_p.buses)
    return network_p.bus_to_i[bus_id_l]
    #or for now, return first(filter( x -> x[2].name == bus_name_l , network.buses))[1]
end

function get_B(network::Network, DIAG_EPS::Float64, dense::Bool)
    B_dict = Dict{Tuple{Int,Int},Float64}();
    for kvp in collect(network.branches)
        b = get_b(kvp[2]);

        ior = kvp[2].from;
        iex = kvp[2].to;

        # ior->iex: + dans ior, - dans iex
        # sur ior :  +theta_ior,  -theta_iex
        B_dict[ior, ior] = get(B_dict, (ior, ior), DIAG_EPS) + b;
        B_dict[ior, iex] = get(B_dict, (ior, iex), 0) - b;

        # sur iex:  -theta_ior,  +theta_iex
        B_dict[iex, ior] = get(B_dict, (iex, ior), 0) - b;
        B_dict[iex, iex] = get(B_dict, (iex, iex), DIAG_EPS) + b;
    end
    Brow = Int[];
    Bcol = Int[];
    Bval = Float64[];
    for kvp in collect(B_dict)
        push!(Brow, kvp[1][1]);
        push!(Bcol, kvp[1][2]);
        push!(Bval, kvp[2]);
    end
    n = length(network.bus_to_i);
    println("n = ", n)
    B = sparse(Brow, Bcol, Bval, n, n);
    if dense
        Bdense = Matrix(B);
        return Bdense;
    else
        return B;
    end
end

function write_B(file_path_p, B::Matrix, network_p::Network)
    nb_buses_l = size(B)[1]
    @assert( size(B)[1] == size(B)[2] == length(network_p.bus_to_i) )

    open(file_path_p, "w") do file_l
        write(file_l, @sprintf("#%20s %20s %20s\n", "BUS_1", "BUS_2", "b"))
        for i_bus1_l in 1:nb_buses_l, i_bus2_l in 1:nb_buses_l
            bus1_name_l =  @sprintf("\"%s\"", network.buses[i_bus1_l].name)
            bus2_name_l =  @sprintf("\"%s\"", network.buses[i_bus2_l].name)
            write(file_l, @sprintf("%20s %20s %20.6E\n", bus1_name_l, bus2_name_l, B[i_bus1_l, i_bus2_l]))
        end
    end
end

function get_B_inv(Bdense::Matrix, ref_bus::Int64)
    println("ref_bus is ", ref_bus)
    n = size(Bdense)[1]
    Bdense2 = Bdense
    Bdense2[ref_bus, 1:n] .= 0
    Bdense2[1:n, ref_bus] .= 0
    Bdense2[ref_bus, ref_bus] = 1
    binv = inv(Bdense2);
    binv[ref_bus,ref_bus] = 0;
    return binv
end

function get_PTDF(network::Network, binv::Matrix, ref_bus::Int)
    n = length(network.bus_to_i);
    m = length(network.branch_to_i);
    println("n ", n)
    println("m ", m)
    PTDF = zeros(Float64, m, n);
    for kvp in collect(network.branches)
        b = get_b(kvp[2]);

        branchid = kvp[1];
        ior = kvp[2].from;
        iex = kvp[2].to;

        for i in 1:n
            PTDF[branchid, i] += b * binv[ior, i];
            PTDF[branchid, i] -= b * binv[iex, i];
            # if ior != ref_bus
            #     PTDF[branchid, i] += b * binv[ior, i];
            # end
            # if iex != ref_bus
            #     PTDF[branchid, i] -= b * binv[iex, i];
            # end
        end
    end
    # println("PTDF=\n",PTDF)
    return PTDF;
end

function distribute_slack(PTDF::Matrix, coeffs::Vector{Float64})
    m, n = size(PTDF);
    v = PTDF * coeffs;
    result = -v*(ones(n)');
    result +=PTDF ;
    return result;
end

function distribute_slack(PTDF::Matrix, coeffs_p::Dict{String,Float64}, network_p::Network)
    m, n = size(PTDF);
    @assert( n == length(coeffs_p) )

    vect_coeffs_l = zeros(n)
    for (bus_name_l, coeff_l) in coeffs_p
        i_l = get_i_for_bus(network_p, bus_name_l)
        vect_coeffs_l[i_l] = coeff_l
    end

    #normalize coeffs
    vect_coeffs_l = vect_coeffs_l / sum(vect_coeffs_l)

    return distribute_slack(PTDF, vect_coeffs_l)
end

function distribute_slack(PTDF::Matrix)
    m, n = size(PTDF);
    μ = 1/n;
    coeffs_l = ones(n)*μ
    return distribute_slack(PTDF, coeffs_l)
end

function write_slack_distribution(file_path::String, network::Network, coeffs_p::Vector{Float64})
    n = length(network.bus_to_i);
    m = length(network.branches);
    open(file_path, "w") do file
        write(file, @sprintf("#%20s %20s\n", "BUS", "COEFF"))
        for bus_i_l in 1:n
            bus_name =  @sprintf("\"%s\"", network.buses[bus_i_l].name)
            write(file, @sprintf("%20s %20.6E\n", bus_name, coeffs_p[bus_i_l]))
        end
    end
end

function write_PTDF(file_path::String,
                    network::Network, PTDF::Matrix,
                    distributed=false, ref_bus::Int=-1,
                    i_cut_branch::Union{Nothing,Int}=nothing;
                    PTDF_TRIMMER::Float64=1e-06,)
    n = length(network.bus_to_i);
    m = length(network.branch_to_i);
    open(file_path, "w") do file
        ref_name =  distributed ? "\"distributed\"" : @sprintf("\"%s\"", network.buses[ref_bus].name)
        cut_branch =  isnothing(i_cut_branch) ? "\"NONE\"" : @sprintf("\"%s\"", network.branches[i_cut_branch].name)
        write(file, @sprintf("#%20s %20s %20s %20s\n", "REF_BUS", ref_name, "CUT_BRANCH", cut_branch))
        for branch_id in 1:m
            for bus_id in 1:n
                branch_name =  @sprintf("\"%s\"", network.branches[branch_id].name)
                bus_name =  @sprintf("\"%s\"", network.buses[bus_id].name)
                ptdf = PTDF[branch_id,bus_id]
                if abs(ptdf)<PTDF_TRIMMER
                    ptdf = 0.
                end
                write(file, @sprintf("%20s %20s %20.6E\n", branch_name, bus_name,ptdf))
            end
        end
    end
end

function compute_ptdf(network, ref_bus::Int, EPS_DIAG::Float64)
    B = get_B(network, EPS_DIAG, true);
    Binv = get_B_inv(B, ref_bus);
    PTDF = get_PTDF(network, Binv, ref_bus);

    return PTDF
end

export compute_ptdf

export get_b;
export get_B;
export get_B_inv;
export get_PTDF;
export write_PTDF;
export distribute_slack;

end #module PTDF

max_stars_repo_name: klorel/pscopf

max_issues_repo_name: klorel/pscopf

max_forks_repo_name: klorel/pscopf


text:
export VoiceStateUpdate,
    VoiceServerUpdate

"""
Sent when a [`User`](@ref) updates their [`VoiceState`](@ref).
"""
struct VoiceStateUpdate <: AbstractEvent
    state::VoiceState
end
@boilerplate VoiceStateUpdate :docs :mock
VoiceStateUpdate(; kwargs...) = VoiceStateUpdate(VoiceState(; kwargs...))
VoiceStateUpdate(d::Dict{Symbol, Any}) = VoiceStateUpdate(; d...)

"""
Sent when a [`Guild`](@ref)'s voice server is updated.
"""
struct VoiceServerUpdate <: AbstractEvent
    token::String
    guild_id::Snowflake
    endpoint::String
end
@boilerplate VoiceServerUpdate :constructors :docs :mock


text:
using Logging

using AbstractAlgebra
using Nemo

using StructuralIdentifiability
using StructuralIdentifiability: extract_identifiable_functions_raw

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

using .RationalFunctionFields: simplify_generators!, compute_groebner!, RationalFunctionField

logger = SimpleLogger(stdout, Logging.Debug)

models = [
    # Current result: -a12 - a01 - a21, -a12 - a01, a12*a01
    # Simpler version: a21, a12 + a01, a12*a01
    @ODEmodel(
        x1'(t) = -(a01 + a21) * x1(t) + a12 * x2(t) + u(t),
        x2'(t) = a21 * x1(t) - a12 * x2(t),
        y(t) = x2(t)
    ),
    # Goodwin
    # Current result: -delta - beta, delta*beta, -sigma, -c, -b
    # Simpler version: delta + beta, delta*beta, sigma, c, b
    @ODEmodel(
        x1'(t) = -b * x1(t) + 1 / (c + x4(t)),
        x2'(t) = alpha * x1(t) - beta * x2(t),
        x3'(t) = gama * x2(t) - delta * x3(t),
        x4'(t) = sigma * x4(t) * (gama * x2(t) - delta * x3(t)) / x3(t),
        y(t) = x1(t)
    ),
    # Current result: -M^2, -nu - 432//491*mu, -g, -mu, -b0
    # Simpler version: M^2, mu, g, mu, b0
    @ODEmodel(
        s'(t) = mu - mu * s(t) - b0 * (1 + b1 * x1(t)) * i(t) * s(t) + g * r(t),
        i'(t) = b0 * (1 + b1 * x1(t)) * i(t) * s(t) - (nu + mu) * i(t),
        r'(t) = nu * i(t) - (mu + g) * r(t),
        x1'(t) = -M * x2(t),
        x2'(t) = M * x1(t),
        y1(t) = i(t),
        y2(t) = r(t)
    ),
    # Current result: (lm*q - 1)//(lm*d*k^2*u), -1//(c*h*lm*d*k*beta^2), -k*q*beta, -b - 2*h - 2*a, -a, -u, -d, -h
    # Simpler version: (lm*q - 1)//(lm*k^2), c*lm*d*k*beta^2, k*q*beta, b, a, u, d, h (Could be simpler ?)
    @ODEmodel(
        x'(t) = lm - d * x(t) - beta * x(t) * v(t),
        y'(t) = beta * x(t) * v(t) - a * y(t),
        v'(t) = k * y(t) - u * v(t),
        w'(t) = c * x(t) * y(t) * w(t) - c * q * y(t) * w(t) - b * w(t),
        z'(t) = c * q * y(t) * w(t) - h * z(t),
        y1(t) = w(t),
        y2(t) = z(t)
    )
]

for (i, m) in enumerate(models)
    ioeqs = find_ioequations(m)
    identifiable_functions_raw = extract_identifiable_functions_raw(collect(values(ioeqs)), m.parameters)

    println( Array{AbstractAlgebra.Generic.Frac{Nemo.fmpq_mpoly}, 1}(identifiable_functions_raw), " !!\n")

    FF = RationalFunctionField(Array{AbstractAlgebra.Generic.Frac{Nemo.fmpq_mpoly}, 1}(identifiable_functions_raw))
    # compute_groebner!(FF)
    # simplify_generators!(FF)
    @info "Simplified $(gens(FF))"
    if i == 2
        break
    end
end


text:
## test to run a GASS optimization
##

rootdir = "/home/stephane/Science/ALMA/ArrayConfig/GASS"

push!(LOAD_PATH,"$rootdir/master/src")
using GASS

## directory
wdir    = "$rootdir/products"

cd(wdir)

function main(inpfile)
    println("# GASS subarray optimization")
    cfg = read_cfg(inpfile)

    res= gass_optimization(cfg)
    println(res)
end

pop= @time main("../master/data/GA_Parameters_O-3.txt.julia")

max_stars_repo_name: bosscha/GASS

max_issues_repo_name: bosscha/GASS

max_forks_repo_name: bosscha/GASS


text: versions = 52:-1:40


text:
struct ConfigFileError <: Exception
    msg::AbstractString
end
Base.showerror(io::IO, e::ConfigFileError) = print(io, "ConfigFileError: ", e.msg)

const CSG_dict = Dict{String, Any}(
    "tube" => Cone,
    "cone" => Cone,
    "sphere" => Ellipsoid,
    "box" => Box,
    "torus" => Torus,
    "TriangularPrism" => TriangularPrism,
    "QuadranglePrism" => QuadranglePrism,
    "PentagonalPrism" => PentagonalPrism,
    "HexagonalPrism"  => HexagonalPrism,
    "union" => CSGUnion,
    "difference" => CSGDifference,
    "intersection" => CSGIntersection,
    "translate" => CartesianVector, # we just ne some type to dispatch on
    "rotate" => Rotations.Rotation  # we just ne some type to dispatch on
)

function get_geometry_key(::Type{T}, dict::AbstractDict, input_units::NamedTuple) where {T}
    g = collect(filter(k -> k in keys(CSG_dict), keys(dict)))
    filter!(k -> !(
        (k == "translate" && !any(broadcast(key -> key in keys(CSG_dict), keys(dict["translate"])))) ||
        (k == "rotate" && !any(broadcast(key -> key in keys(CSG_dict), keys(dict["rotate"]))))
    )  , g) # exclude translate and rotate that have no primitives inside
    @assert !(length(g) > 1) "Too many ($(length(g))) geometry entries in dictionary: $(g)."
    @assert !(length(g) == 0) "None of the entries $(keys(dict)) describes a Geometry."
    g[1]
end

function parse_CSG_transformation(::Type{T}, dict::AbstractDict, input_units::NamedTuple) where {T}
    r = if haskey(dict, "rotate")
        SMatrix{3, 3, T, 9}(parse_rotation_matrix(T, dict["rotate"], input_units.angle))
        # We might want to improve this to avoid numerical precision errors.
        # For now this is fine.
    else
        one(SMatrix{3, 3, T, 9})
    end
    # s = if haskey(dict, "scale")
    #     _s = parse_scale_vector(T, dict["scale"])
    # else
    #     ones(SVector{3,T})
    # end
    t = if haskey(dict, "translate")
        parse_translate_vector(T, dict["translate"], input_units.length)
    else
        zero(CartesianVector{T})
    end
    (rotation = r, translation = t)
end

#### INTERNAL PARSE FUNCTIONS

# parses dictionary entries of type Real or String to their value in internal units
@inline _parse_value(::Type{T}, x::Real, unit::Unitful.Units) where {T} = T(to_internal_units(float(x) * unit))
@inline _parse_value(::Type{T}, s::String, ::Unitful.Units) where {T} = T(to_internal_units(uparse(s)))
@inline _parse_value(::Type{T}, a::Vector, unit::Unitful.Units) where {T} = _parse_value.(T, a, unit)

# parses dictionary entries of type {"from": ..., "to": ... } to a Tuple of the interval boundaries
function _parse_interval_from_to(::Type{T}, dict::AbstractDict, unit::Unitful.Units)::Tuple{T,T} where {T}
    To::T = _parse_value(T, dict["to"], unit)
    From::T = _parse_value(T, dict["from"], unit)
    @assert From <= To "Entry 'from' should be smaller than entry 'to' in $(dict)."
    (From, To)
end

# parses dictionary entries of type Real, String or {"from": ..., "to": ... } to respective AbstractFloat/Interval
@inline _parse_radial_interval(::Type{T}, x::Union{Real, String}, unit::Unitful.Units) where {T} = _parse_value(T, x, unit)
function _parse_radial_interval(::Type{T}, dict::AbstractDict, unit::Unitful.Units) where {T}
    @assert haskey(dict, "from") && haskey(dict, "to") "Please specify 'from' and 'to' in $(dict)."
    From::T, To::T = _parse_interval_from_to(T, dict, unit)
    @assert From >= 0 && To >= 0 "Entries 'from' and 'to' of radial $(dict) should `>= 0`."
    (From, To)
end

# parses dictionary entries of type Real, String or {"from": ..., "to": ... } to respective AbstractFloat/Interval
@inline _parse_linear_interval(::Type{T}, x::Union{Real, String}, unit::Unitful.Units) where {T} = _parse_value(T, x, unit)/2
function _parse_linear_interval(::Type{T}, dict::AbstractDict, unit::Unitful.Units) where {T}
    @assert haskey(dict, "from") && haskey(dict, "to") "Please specify 'from' and 'to' in $(dict)."
    From::T, To::T = _parse_interval_from_to(T, dict, unit)
    To == -From == zero(T) ? To : (From, To) # if != 0 is influences the origin
end

# parses dictionary entry for φ-interval that has {"from" ..., "to": ...} to the respective Nothing/Interval
function _parse_angular_interval(::Type{T}, dict::AbstractDict, unit::Unitful.Units) where {T}
    φTo::T = _parse_value(T, dict["to"], unit)
    φFrom::T = _parse_value(T, dict["from"], unit)
    if abs(rem2pi(φFrom - φTo, RoundNearest)) > 2*eps(T)
        φFrom, φTo
    else
        nothing
    end
end

### ADAPTED FOR PRIMITIVES (should throw Errors if something is not defined!)

# converts "r" to the respective AbstractFloat/Interval/Tuple for Cone
function parse_r_of_primitive(::Type{T}, dict::AbstractDict, unit::Unitful.Units) where {T}
    @assert haskey(dict, "r") "Please specify 'r'."
    dictr = dict["r"]
    if haskey(dictr, "bottom") && haskey(dictr, "top")
        # "r" : {"bottom": {"from": ..., "to": ...}, "top": {"from": ..., "to": ...}}
        bottom = _parse_radial_interval(T, dictr["bottom"], unit)
        top = _parse_radial_interval(T, dictr["top"], unit)
        bottom, top
    else
        # "r" : {"from": ... , "to": ...}
        _parse_radial_interval(T, dictr, unit)
    end
end
# converts "r" to the respective AbstractFloat/Interval/Tuple for Cone
function parse_r_of_primitive(::Type{T}, dict::AbstractDict, unit::Unitful.Units, ::Type{Cone}) where {T}
    @assert haskey(dict, "r") "Please specify 'r'."
    dictr = dict["r"]
    r = if haskey(dictr, "bottom") && haskey(dictr, "top")
        # "r" : {"bottom": {"from": ..., "to": ...}, "top": {"from": ..., "to": ...}}
        bottom = _parse_radial_interval(T, dictr["bottom"], unit)
        top = _parse_radial_interval(T, dictr["top"], unit)
        if bottom isa Real bottom = (zero(T), bottom) end
        if top isa Real top = (zero(T), top) end
        (bottom, top)
    elseif haskey(dictr, "from") && haskey(dictr, "to")
        # "r" : {"from": ... , "to": ...}
        r = _parse_radial_interval(T, dictr, unit)
        if r isa Real r = (zero(T), r) end
        (r, r)
    else
        r = _parse_radial_interval(T, dictr, unit)
        ((zero(T),r),(zero(T),r))
    end
    # Not all cases implemented yet
    r_bot_in, r_bot_out, r_top_in, r_top_out = r[1][1], r[1][2], r[2][1], r[2][2]
    if r_bot_in == r_top_in == zero(T)
        if r_bot_out == r_top_out
            return r_bot_out # Cylinder
        else
            return ((r_bot_out,), (r_top_out,)) # VaryingCylinder
        end
    elseif r_top_in == r_top_out && r_bot_in != r_bot_out
        if r_top_in == 0
            return ((r_bot_in, r_bot_out), nothing)
        else
            return r # return ((r_bot_in, r_bot_out), r_top_in)
        end
    elseif r_top_in != r_top_out && r_bot_in == r_bot_out
        if r_bot_in == 0
            return (nothing, (r_top_in, r_top_out))
        else
            return r # return (r_bot_in, (r_top_in, r_top_out))
        end
    else
        r # VaryingTube
    end
end

# converts "phi" : {...} to the respective Nothing/Interval
# if no φ is given, then a full 360° interval (i.e. φ = nothing) is assumed
function parse_φ_of_primitive(::Type{T}, dict::AbstractDict, unit::Unitful.Units) where {T}
    φ = if !haskey(dict,"phi")
        #@info "No 'phi' is specified. Assuming 'phi' to go from 0 to 360°."
        nothing
    else #haskey(dict, "phi")
        _parse_angular_interval(T, dict["phi"], unit)
    end
end

function parse_θ_of_primitive(::Type{T}, dict::AbstractDict, unit::Unitful.Units) where {T}
    θ = if !haskey(dict,"theta")
        #@info "No 'theta' is specified. Assuming 'theta' to go from 0 to 360°."
        nothing
    else #haskey(dict, "theta")
        _parse_angular_interval(T, dict["theta"], unit)
    end
end

# converts either "h" or "z" to the respective AbstractFloat/Interval
function parse_height_of_primitive(::Type{T}, dict::AbstractDict, unit::Unitful.Units) where {T}
    @assert haskey(dict,"h") || haskey(dict,"z") "Please specify 'h' or 'z'."
    haskey(dict,"h") ? _parse_linear_interval(T, dict["h"], unit) : _parse_linear_interval(T, dict["z"], unit)
end

# converts the content of the dictionary to respective AbstractFloat/Interval.
function parse_interval_of_primitive(::Type{T}, s::String, dict::AbstractDict, unit::Unitful.Units) where {T}
    @assert haskey(dict, s) "Please specify '$(s)'."
    _parse_linear_interval(T, dict[s], unit)
end

# converts {"x": ..., "y": ..., "z": ... } to the respective CartesianVector
function parse_translate_vector(::Type{T}, dict::AbstractDict, unit::Unitful.Units)::CartesianVector{T} where {T}
    x::T = haskey(dict, "x") ? _parse_value(T, dict["x"], unit) : T(0)
    y::T = haskey(dict, "y") ? _parse_value(T, dict["y"], unit) : T(0)
    z::T = haskey(dict, "z") ? _parse_value(T, dict["z"], unit) : T(0)
    CartesianVector{T}(x,y,z)
end

function parse_CSG_transformation(::Type{T}, dict::AbstractDict, ::Type{CartesianVector}, input_units::NamedTuple) where {T}
    parse_translate_vector(T, dict["translate"], input_units.length)
end

function Geometry(::Type{T}, ::Type{CartesianVector}, dict::AbstractDict, input_units::NamedTuple, outer_transformations::Transformations{T}) where {T}
    translate_vector = parse_translate_vector(T, dict, input_units.length)
    key = get_geometry_key(T, dict, input_units)
    # inner_transformations = (rotation = one(SMatrix{3, 3, T, 9}), translation = translate_vector)
    # transformations = combine_transformations(inner_transformations, outer_transformations)
    Geometry(T, CSG_dict[key], dict[key], input_units, outer_transformations)
end

function parse_scale_vector(::Type{T}, dict::AbstractDict)::SVector{3,T} where {T}
    x::T = haskey(dict, "x") ? _parse_value(T, dict["x"], Unitful.FreeUnits{(), NoDims, nothing}()) : T(1)
    y::T = haskey(dict, "y") ? _parse_value(T, dict["y"], Unitful.FreeUnits{(), NoDims, nothing}()) : T(1)
    z::T = haskey(dict, "z") ? _parse_value(T, dict["z"], Unitful.FreeUnits{(), NoDims, nothing}()) : T(1)
    SVector{3,T}(x,y,z)
end

function get_origin(::Type{T}, dict::AbstractDict, unit::Unitful.Units) where {T}
    origin_names = ["origin", "translate", "translation"]
    haskeys = map(n -> haskey(dict, n), origin_names)
    i = findfirst(i -> i, haskeys)
    haskeyssum = sum(haskeys)
    if haskeyssum == 1
        o = dict[origin_names[i]]
        if o isa Vector
            CartesianPoint{T}(_parse_value(T, o, unit))
        else
            x, y, z = zero(T), zero(T), zero(T)
            if haskey(o, "x") x = _parse_value(T, o["x"], unit) end
            if haskey(o, "y") y = _parse_value(T, o["y"], unit) end
            if haskey(o, "z") z = _parse_value(T, o["z"], unit) end
            CartesianPoint{T}(x, y, z)
        end
    elseif haskeyssum > 1
        throw(ConfigFileError("Multiple fields for origin of primitive detected. Use only `origin`, `translate` or `translation`."))
    else
        zero(CartesianPoint{T})
    end
end

# function Geometry(::Type{T}, ::Type{ScaledGeometry}, dict::AbstractDict, input_units::NamedTuple) where {T}
#     length_unit = input_units.length
#     scale_vector::SVector{3,T} = parse_scale_vector(T, dict, length_unit)
#     key::String, transformations = get_geometry_key(T, dict, input_units)
#     scale(transform(Geometry(T, CSG_dict[key], dict[key], input_units), transformations), scale_vector)
# end

_rot_keys = ["X","Y","Z","XY","XZ","YX","YZ","ZX","ZY","XYX","XYZ","XZX","XZY","YXY","YXZ","YZX","YZY","ZXY","ZXZ","ZYX","ZYZ"]
function parse_rotation_matrix(::Type{T}, dict::AbstractDict, unit::Unitful.Units)::RotMatrix3{T} where {T}
    dict_keys = keys(dict)
    if "M" in dict_keys
        R = RotMatrix3{T}(dict["M"])
        @assert isrotation(R) "R is not a Rotation Matrix."
        return R
    else
        rotation = filter(k -> uppercase(k) in _rot_keys, dict_keys)
        @assert length(rotation) == 1 "Rotations must be defined using exactly one of the following keywords:\n$(vcat("M",_rot_keys))"
        key = first(rotation)
        R = getfield(Rotations, Symbol("Rot"*uppercase(key)))
        RotMatrix3{T}(R{Float64}(_parse_value(Float64,dict[key],unit)...)) # parse rotations matrix values as Float64 to minimize rounding errors
    end
end

function get_rotation(::Type{T}, dict::AbstractDict, unit::Unitful.Units) where {T}
    rot_names = ["rotate", "rotation"]
    haskeys = map(n -> haskey(dict, n), rot_names)
    i = findfirst(i -> i, haskeys)
    haskeyssum = sum(haskeys)
    if haskeyssum == 1
        parse_rotation_matrix(T, dict[rot_names[i]], unit)
    elseif haskeyssum > 1
        throw(ConfigFileError("Multiple fields for rotation of primitive detected. Use only `rotation` or `rotate`."))
    else
        one(SMatrix{3, 3, T, 9})
    end
end

function parse_CSG_transformation(::Type{T}, dict::AbstractDict, ::Type{Rotation}, input_units::NamedTuple) where {T}
    parse_rotation_matrix(T, dict["rotate"], input_units.angle)
end

# function Geometry(::Type{T}, ::Type{RotatedGeometry}, dict::AbstractDict, input_units::NamedTuple) where {T}
#     angle_unit = input_units.angle
#     rotation_matrix = parse_rotation_matrix(T, dict, angle_unit)
#     key::String, transformations = get_geometry_key(T, dict, input_units)
#     rotate(transform(Geometry(T, CSG_dict[key], dict[key], input_units), transformations), rotation_matrix)
# end

function Geometry(::Type{T}, ::Type{Rotations.Rotation}, dict::AbstractDict, input_units::NamedTuple, outer_transformations::Transformations{T}) where {T}
    rotation_matrix = SMatrix{3, 3, T, 9}(parse_rotation_matrix(T, dict, input_units.angle))
    key = get_geometry_key(T, dict, input_units)
    Geometry(T, CSG_dict[key], dict[key], input_units, outer_transformations)
end

function Geometry(::Type{T}, dict::AbstractDict, input_units::NamedTuple, outer_transformations::Transformations{T}) where {T}
    key = get_geometry_key(T, dict, input_units)
    inner_transformations = parse_CSG_transformation(T, dict, input_units)
    transformations = combine_transformations(inner_transformations, outer_transformations)
    Geometry(T, CSG_dict[key], dict[key], input_units, transformations)
end

function Geometry(::Type{T}, filename::String, input_units::NamedTuple = (length = internal_length_unit, angle = internal_angle_unit)) where {T}
    @assert isfile(filename) "The given filename '$(filename)' does not lead to a valid file."
    dict = if endswith(filename, ".json")
        JSON.parsefile(filename)
    elseif endswith(filename, ".yaml")
        YAML.load_file(filename)
    else
        @error "Only JSON and YAML formats are supported at the moment."
    end
    transformation = (rotation = one(SMatrix{3, 3, T, 9}), translation = zero(CartesianVector{T}))
    Geometry(T, dict, input_units, transformation)
end

function show_CSG_tree(csgtree; start = "", tab = "", CSG = false)
    CT = typeof(csgtree)
    if CT <: AbstractConstructiveGeometry
        if !CSG
            println(start*" $(CT.name.name){$(CT.parameters[1])}")
            tab *= "\t"
        end
        next_CSG::Bool = typeof(csgtree.a).name == CT.name
        show_CSG_tree(csgtree.a, start = tab*"├"*(CT <: CSGDifference ? " +" : "──"), tab = tab*(next_CSG ? "" : "│"), CSG = next_CSG)
        show_CSG_tree(csgtree.b, start = tab*(CSG ? "├" : "└")*(CT <: CSGDifference ? " ─" : "──"), tab = tab*(CSG ? "│" : " "))
    end
    # if CT <: AbstractTransformedGeometry
    #     println(start*" $(CT.name.name){$(CT.parameters[1])}")
    #     tab *= "\t"
    #     show_CSG_tree(csgtree.p, start = tab*"└──", tab = tab)
    # end
    if CT <: AbstractPrimitive
        println(start*" $(CT.name.name){$(CT.parameters[1])}")
    end
end


text:
using Documenter, RigidBodyDynamics, RigidBodyDynamics.OdeIntegrators

makedocs(
    modules = [RigidBodyDynamics, RigidBodyDynamics.OdeIntegrators],
    root = @__DIR__,
    checkdocs = :exports,
    sitename ="RigidBodyDynamics.jl",
    authors = "Twan Koolen and contributors.",
    pages = [
        "Home" => "index.md",
        "Quick start guide" => "quickstart.md",
        "Spatial vector algebra" => "spatial.md",
        "Joints" => "joints.md",
        "Rigid bodies" => "rigidbody.md",
        "Mechanism" => "mechanism.md",
        "MechanismState" => "mechanismstate.md",
        "Kinematics/dynamics algorithms" => "algorithms.md",
        "Custom collection types" => "customcollections.md",
        "Cache types" => "caches.md",
        "Simulation" => "simulation.md",
        "URDF parsing and writing" => "urdf.md",
        "Benchmarks" => "benchmarks.md"
      ],
    format = Documenter.HTML(prettyurls = parse(Bool, get(ENV, "CI", "false")))
)

deploydocs(
    repo = "github.com/JuliaRobotics/RigidBodyDynamics.jl.git"
)


text:
import Nemo.isone, Nemo.divexact, Base.copy
export divexact!, gcd_into!, coprime_base, coprime_base_insert

function divexact!(a::fmpz, b::fmpz)
  ccall((:fmpz_divexact, libflint), Nothing,
          (Ref{fmpz}, Ref{fmpz}, Ref{fmpz}), a, a, b)
  return a
end

function gcd_into!(a::fmpz, b::fmpz, c::fmpz)
  ccall((:fmpz_gcd, libflint), Nothing,
          (Ref{fmpz}, Ref{fmpz}, Ref{fmpz}), a, b, c)
  return a
end

function gcd_into!(a::T, b::T, c::T) where T <: Integer
  return gcd(b, c)::T
end

function mul_into!(a::T, b::T, c::T) where T
  return (b*c)::T
end

function mul_into!(a::fmpz, b::fmpz, c::fmpz)
  mul!(a, b, c)
  return a
end

function copy(a::fmpz)
  return deepcopy(a)
end

function copy_into!(a, b)
  return b
end

function copy_into!(a::fmpz, b::fmpz)
  ccall((:fmpz_set, libflint), Nothing, (Ref{fmpz}, Ref{fmpz}), a, b)
  return a
end

#for larger lists much better than Bill's (Nemo's) prod function
# the build-in Julia is better than Bill's Nemo function anyway

mutable struct ProdEnv{T}
  level :: Vector{Int}
  val   :: Vector{T}
  last  :: Int

  function ProdEnv{T}(n::Int) where {T}
    r = new{T}()
    m = nbits(n)
    r.level = Array{Int}(undef, m)
    r.val   = Array{T}(undef, m)
    r.last = 0
    for i=1:m
      r.val[i] = T()
    end
    return r
  end
end

function prod_mul!(A::ProdEnv{T}, b::T) where T
  if A.last == 0
    A.level[1] = 1
    A.val[1] = copy_into!(A.val[1], b)
    A.last = 1
    return
  end
  if A.level[A.last] > 1
    A.last += 1
    A.level[A.last] = 1
    A.val[A.last] = copy_into!(A.val[A.last], b)
    return
  end

  lev = 1
  while A.last > 0 && A.level[A.last] <= lev
    b = mul_into!(A.val[A.last], A.val[A.last], b)
    lev += A.level[A.last]
    A.last -= 1
  end
  A.last += 1
  A.level[A.last] = lev
  A.val[A.last] = b
  return
end

function prod_end(A::ProdEnv)
  b = A.val[A.last]
  while A.last >1
    A.last -= 1
    b = mul_into!(b, b, A.val[A.last])
  end
  return b
end

function my_prod(a::AbstractVector{T}) where T
  if length(a) <100
    return prod(a)
  end

  b = ProdEnv{T}(length(a))
  for x in a
    prod_mul!(b, x)
  end
  return prod_end(b)
end

#coprime base Bach/ Schallit/ ???

function pair_bach(a::E, b::E) where E
  if isunit(a)
    if isunit(b)
      return Vector{E}()
    else
      return [b]
    end
  end
  if isunit(b)
    return [a]
  end

  n = [a, b]
  i = 1
  while i < length(n)
    g = gcd(n[i], n[i+1])
    if isone(g)
      i += 1
    else
      n[i] = divexact!(n[i], g)
      n[i+1] = divexact!(n[i+1], g)
      insert!(n, i+1, g)
      if isunit(n[i+2])
        deleteat!(n, i+2)
      end
      if isunit(n[i])
        deleteat!(n, i)
      end
    end
  end

  return n
end

function augment_bach(S::Vector{E}, m::E) where E
  T = Vector{E}()
  i = 1
  while i <= length(S) && !isunit(m)
    if !isunit(S[i])
      Ts = pair_bach(m, S[i])
      T = vcat(T, view(Ts, 2:length(Ts)))
      m = Ts[1]
    end
    i += 1
  end
  if i <= length(S)
    T = vcat(T, view(S, i:length(S)))
  end
  if !isunit(m)
    push!(T, m)
  end
  return T
end

function coprime_base_bach(a::Vector{E}) where E #T need to support GCDs
  if length(a) < 2
    return a
  end

  T = pair_bach(a[1], a[2])
  j = 3
  while j <= length(a)
    T = augment_bach(T, a[j])
    j += 1
  end
  return T
end

# Bernstein: coprime bases
#
#Note: Bernstein needs bigints, either Integer or fmpz
#      well, polys are also OK, small integers are not.

# could/ should be optimsed using divexact! and gcd_into!
# probably should also be combined with ppio somewhere

function ppgle(a::E, b::E) where E
  n = gcd(a,b)
  r = divexact(a, n)
  m = n
  g = gcd(r, n)
  while !isone(g)
    r *= g
    n = divexact(n, g)
    g = gcd(r, n)
  end
  return m, r, n
end

function pair_bernstein(a::E, b::E) where E
  T = Vector{E}()
  if isunit(b)
    if isunit(a)
      return T
    else
      return push!(T, a)
    end
  end
  if isunit(a)
    return push!(T, b)
  end

  a,r = ppio(a,b)
  if !isunit(r)
    push!(T, r)
  end
  g,h,c = ppgle(a, b)
  c0 = c
  r = c0
  k = 1
  while !isone(h)
    g,h,c = ppgle(h, g^2)
    d = gcd(c, b)
    if isone(d)
      push!(T, c)
      continue
    end
    r *= d
    n = d^(2^(k-1))
    T = vcat(T, pair_bernstein(divexact(c, n), d))
    k += 1
  end
  T = vcat(T, pair_bernstein(divexact(b, r), c0))
  return T
end

function split_bernstein(a::T, P::Hecke.node{T}) where T
  b = Hecke.ppio(a, P.content)[1]
  if !isdefined(P, :left)
    if !isdefined(P, :right)
      return [(P.content, b)]
    else
      return split_bernstein(b, P.right)
    end
  else
    if !isdefined(P, :right)
      return split_bernstein(b, P.left)
    else
      return vcat(split_bernstein(b, P.left), split_bernstein(b, P.right))
    end
  end
end

function split_bernstein(a::T, P::Vector{T}) where T
  if length(P) == 0
    return P
  end
  F = FactorBase(P, check = false)
  b = Hecke.ppio(a, F.prod)[1]
  if length(P)==1
    return [(P[1], b)]
  end
  return vcat(split_bernstein(b, F.ptree.left), split_bernstein(b, F.ptree.right))
end

function augment_bernstein(P::Vector{E}, b::E) where E
  T = Vector{E}()
  if length(P) == 0
    if isunit(b)
      return T
    else
      return push!(T, b)
    end
  end
  F = FactorBase(P, check = false)
  a,r = Hecke.ppio(b, F.prod)
  if ! isunit(r)
    push!(T, r)
  end
  S = split_bernstein(a, F.ptree)
  for X in S
    T = vcat(T, pair_bach(X[1], X[2]))
  end
  return T
end

function merge_bernstein(P::Vector{E}, Q::Vector{E}) where E
  m = length(Q)
  b = nbits(m)
  S = P
  i = 0
  while i<=b
    R = prod(view(Q, findall(x -> x & (2^i) ==0, 1:length(Q))))
    T = augment_bernstein(S, R)
    R = prod(view(Q, findall(x -> x & (2^i) !=0, 1:length(Q))))
    S = augment_bernstein(T, R)
    i += 1
  end
  return S
end

function coprime_base_bernstein(S::Vector{E}) where E
  if length(S)<2
    return S
  end
  P1 = coprime_base_bernstein([S[i] for i=1:div(length(S), 2)])
  P2 = coprime_base_bernstein([S[i] for i=(div(length(S), 2)+1):length(S)])
  return merge_bernstein(P1, P2)
end

function augment_steel(S::Vector{E}, a::E, start::Int = 1) where E
  i = start
  if isunit(a)
    return S
  end

  g = a
  new = true

  while i<=length(S) && !isone(a)
    if new
      g = gcd(S[i], a)
      new = false
    else
      g = gcd_into!(g, S[i], a)
    end
    if isunit(g)
      i += 1
      continue
    end
    si = divexact(S[i], g)
    a = divexact(a, g)
    if isunit(si) # g = S[i] and S[i] | a
      continue
    end
    S[i] = si
    if isunit(a) # g = a and a | S[i]
      a = copy(g)
      continue
    end
    augment_steel(S, copy(g), i)
    continue
  end
  if !isunit(a)
    push!(S, a)
  end

  return S;
end

function coprime_base_steel(S::Vector{E}) where E
  @assert !isempty(S)
  T = Array{E}(undef, 1)
  T[1] = S[1]
  for i=2:length(S)
    augment_steel(T, S[i])
  end
  return T
end

##implemented
# Bernstein: asymptotically fastest, linear in the total input size
#   pointless for small ints as it requires intermediate numbers to be
#   huge
# Bach/Shallit/???: not too bad, source Epure's Masters thesis
#   can operate on Int types as no intermediate is larger than the input
# Steel: a misnomer: similar to Magma, basically implements a memory
#   optimised version of Bach
#   faster than Magma on
# > I := [Random(1, 10000) * Random(1, 10000) : x in [1..10000]];
# > time c := CoprimeBasis(I);
# julia> I = [fmpz(rand(1:10000))*rand(1:10000) for i in 1:10000];
#
# experimentally, unless the input is enormous, Steel wins
# on smallish input Bach is better than Bernstein, on larger this
# changes
#
# needs
# isone, gcd_into!, divexact!, copy
# (some more for Bernstein: FactorBase, gcd, divexact)

@doc Markdown.doc"""
    coprime_base{E}(S::Vector{E}) -> Vector{E}

Returns a coprime base for $S$, i.e. the resulting array contains pairwise coprime objects that multiplicatively generate the same set as the input array.
"""
coprime_base(x) = coprime_base_steel(x)

@doc Markdown.doc"""
    coprime_base_insert{E}(S::Vector{E}, a::E) -> Vector{E}

Given a coprime array $S$, insert a new element, i.e. find a coprime base for `push(S, a)`.
"""
coprime_base_insert(S, a) = augment_steel(S, a)


text:
@testset "geometry.coords3d" begin

@testset "cartesian3d" begin
    c3d = cartesian3d(float.([1 2 3; 4 5 6; 7 8 9; 10 11 12]))
    @test size(c3d.coords) == (4, 3)
    @test sum(x_components(c3d)) == 22
    @test sum(y_components(c3d)) == 26
    @test sum(z_components(c3d)) == 30
end

end # geometry.coords3d


text:
# Julia wrapper for header: /usr/local/include/libavutil/log.h
# Automatically generated using Clang.jl wrap_c, version 0.0.0

export
    av_vlog,
    av_log_get_level,
    av_log_set_level,
    av_log_set_callback,
    av_log_default_callback,
    av_default_item_name,
    av_default_get_category,
    av_log_format_line,
    av_log_set_flags,
    av_log_get_flags

function av_vlog(avcl,level::Integer,fmt,vl)
    ccall((:av_vlog,libavutil),Void,(Ptr{Void},Cint,Cstring,Ptr{__va_list_tag}),avcl,level,fmt,vl)
end

function av_log_get_level()
    ccall((:av_log_get_level,libavutil),Cint,())
end

function av_log_set_level(level::Integer)
    ccall((:av_log_set_level,libavutil),Void,(Cint,),level)
end

function av_log_set_callback(callback)
    ccall((:av_log_set_callback,libavutil),Void,(Ptr{Void},),callback)
end

function av_log_default_callback(avcl,level::Integer,fmt,vl)
    ccall((:av_log_default_callback,libavutil),Void,(Ptr{Void},Cint,Cstring,Ptr{__va_list_tag}),avcl,level,fmt,vl)
end

function av_default_item_name(ctx)
    ccall((:av_default_item_name,libavutil),Cstring,(Ptr{Void},),ctx)
end

function av_default_get_category(ptr)
    ccall((:av_default_get_category,libavutil),AVClassCategory,(Ptr{Void},),ptr)
end

function av_log_format_line(ptr,level::Integer,fmt,vl,line,line_size::Integer,print_prefix)
    ccall((:av_log_format_line,libavutil),Void,(Ptr{Void},Cint,Cstring,Ptr{__va_list_tag},Cstring,Cint,Ptr{Cint}),ptr,level,fmt,vl,line,line_size,print_prefix)
end

function av_log_set_flags(arg::Integer)
    ccall((:av_log_set_flags,libavutil),Void,(Cint,),arg)
end

function av_log_get_flags()
    ccall((:av_log_get_flags,libavutil),Cint,())
end


text:
"""
	Particle
"""
struct Particle{I, T} <: Model{I, T}
	n::Int
	m::Int
	d::Int

    mass
	g
    μ

    nq
    nu
    nc
    nf
    nb
	ns

    idx_u
    idx_λ
    idx_b
    idx_ψ
    idx_η
    idx_s
end

# Dimensions
nq = 3                    # configuration dimension
nu = 3                    # control dimension
nc = 1                    # number of contact points
nf = 4                    # number of parameters for friction cone
nb = nc * nf
ns = 1

# Parameters
μ = 0.5      # coefficient of friction
mass = 1.0   # mass
g = 9.81

n = 2 * nq
m = nu + nc + nb + nc + nb + ns
d = 0

idx_u = (1:nu)
idx_λ = nu .+ (1:nc)
idx_b = nu + nc .+ (1:nb)
idx_ψ = nu + nc + nb .+ (1:nc)
idx_η = nu + nc + nb + nc .+ (1:nb)
idx_s = nu + nc + nb + nc + nb .+ (1:ns)

# Methods
function M_func(model::Particle, q)
    @SMatrix [model.mass 0.0 0.0;
              0.0 model.mass 0.0;
              0.0 0.0 model.mass]
end

C_func(model::Particle, q, q̇) = @SVector [0.0, 0.0, model.mass * model.g]

function ϕ_func(::Particle, q)
    @SVector[q[3]]
end

B_func(::Particle, q) = @SMatrix [1.0 0.0 0.0;
                                  0.0 1.0 0.0;
                                  0.0 0.0 1.0]

N_func(::Particle, q) = @SMatrix [0.0 0.0 1.0]

function _P_func(model::Particle, q)
   return @SMatrix [1.0 0.0 0.0;
                    0.0 1.0 0.0]
end

function P_func(model::Particle, q)
   return @SMatrix [1.0 0.0 0.0;
                    0.0 1.0 0.0;
                    -1.0 0.0 0.0;
                    0.0 -1.0 0.0]
end

function lagrangian_derivatives(model, q, v)
	D1L = -1.0 * C_func(model, q, v)
    D2L = M_func(model, q) * v
	return D1L, D2L
end

function friction_cone(model::Particle, u)
	λ = u[model.idx_λ]
	b = u[model.idx_b]
	return @SVector [model.μ * λ[1] - sum(b)]
end

function maximum_dissipation(model::Particle, x⁺, u, h)
	q3 = x⁺[model.nq .+ (1:model.nq)]
	q2 = x⁺[1:model.nq]
	ψ = u[model.idx_ψ]
	ψ_stack = ψ[1] * ones(4)
	η = u[model.idx_η]
	return P_func(model, q3) * (q3 - q2) / h + ψ_stack - η
end

function no_slip(model::Particle, x⁺, u, h)
	q3 = view(x⁺, model.nq .+ (1:model.nq))
	q2 = view(x⁺, 1:model.nq)
	λ = view(u, model.idx_λ)
	s = view(u, model.idx_s)
	λ_stack = λ[1] * ones(2)
	return (λ_stack' * _P_func(model, q3) * (q3 - q2) / h)[1]
end

function fd(model::Particle{Discrete, FixedTime}, x⁺, x, u, w, h, t)
	q3 = view(x⁺, model.nq .+ (1:model.nq))
	q2⁺ = view(x⁺, 1:model.nq)
	q2⁻ = view(x, model.nq .+ (1:model.nq))
	q1 = view(x, 1:model.nq)
	u_ctrl = view(u, model.idx_u)
	λ = view(u, model.idx_λ)
	b = view(u, model.idx_b)

	# evalutate at midpoint
	qm1 = 0.5 * (q1 + q2⁺)
    vm1 = (q2⁺ - q1) / h
    qm2 = 0.5 * (q2⁺ + q3)
    vm2 = (q3 - q2⁺) / h

	D1L1, D2L1 = lagrangian_derivatives(model, qm1, vm1)
	D1L2, D2L2 = lagrangian_derivatives(model, qm2, vm2)

    [q2⁺ - q2⁻;
	 (0.5 * h * D1L1 + D2L1 + 0.5 * h * D1L2 - D2L2
	   + transpose(B_func(model, qm2)) * u_ctrl
	   + transpose(N_func(model, q3)) * λ
	   + transpose(P_func(model, q3)) * b)]
end

model = Particle{Discrete, FixedTime}(n, m, d,
				 mass, g, μ,
				 nq,
				 nu,
				 nc,
				 nf,
				 nb,
				 ns,
				 idx_u,
				 idx_λ,
				 idx_b,
				 idx_ψ,
				 idx_η,
				 idx_s)

function visualize!(vis, model::Particle, q;
	Δt = 0.1, r = 0.25)

	default_background!(vis)
    setobject!(vis["particle"],
		Rect(Vec(0, 0, 0),Vec(2r, 2r, 2r)),
		MeshPhongMaterial(color = RGBA(1.0, 165.0 / 255.0, 0, 1.0)))

    anim = MeshCat.Animation(convert(Int, floor(1.0 / Δt)))

    for t = 1:length(q)
        MeshCat.atframe(anim, t) do
            settransform!(vis["particle"], Translation(q[t][1:3]...))
        end
    end

	settransform!(vis["/Cameras/default"],
	    compose(Translation(-2.5, 7.5, 1.0),LinearMap(RotZ(0.0))))

    MeshCat.setanimation!(vis, anim)
end


text:
using LocalProjections
using Test

@testset "LocalProjections.jl" begin
    try
        include("level_test.jl")
    catch e
        showerror(stdout, e, backtrace())
        rethrow(e)
    end
end


text:
using IcanProj
using NetCDF
using DataFrames
using PyPlot
using Statistics
using CSV

function compute_errors(path_simulations, cfg, target_variable)

  file_fine = joinpath(path_simulations, "fsmres_forest", "results_$(cfg)", "$(target_variable)_1km.nc")

  file_coarse = joinpath(path_simulations, "fsmres_forest", "results_$(cfg)", "$(target_variable)_50km.nc")

  df_links = link_results(file_fine, file_coarse)

  data_coarse, data_aggregated, ngrids  = unify_results(file_fine, file_coarse, df_links, target_variable)

  bias = mean(data_coarse .- data_aggregated, dims = 1)

  bias = dropdims(bias, dims = 1)

  rmse = sqrt.(mean((data_coarse .- data_aggregated).^2 , dims = 1))

  rmse = dropdims(rmse, dims = 1)

  return rmse, bias

end

function get_metadata(path_simulations)

  file_fine = joinpath(path_simulations, "fsmres_forest", "results_1", "swe_1km.nc")

  file_coarse = joinpath(path_simulations, "fsmres_forest", "results_1", "swe_50km.nc")

  df_links = link_results(file_fine, file_coarse)

  df_agg = aggregate(df_links, :id_coarse, [mean, std])

  return df_agg

end

function compute_correlations(rmse, bias, df_links, target_variable)

  df = DataFrame()

  ikeep = .!isnan.(df_links[:elev_std])

  df[Symbol("r_" * target_variable * "_elev_for_bias")] = cor(bias[ikeep], df_links[:elev_std][ikeep])

  df[Symbol("r_" * target_variable * "_elev_for_rmse")] = cor(rmse[ikeep], df_links[:elev_std][ikeep])

  return df

end

function run_all()

  path_results = joinpath(dirname(pathof(IcanProj)), "..", "data")

  path_simulations = "/data04/jmg/fsm_simulations/netcdf"

  target_variables = ["swe", "latmo", "hatmo", "rnet"]

  df_links = get_metadata(path_simulations)

  for target_variable in target_variables

    @show target_variable

    df_all = []

    for cfg in 1:32

      @show cfg

      rmse, bias = compute_errors(path_simulations, cfg, target_variable)

      if cfg == 1
        df_all = compute_correlations(rmse, bias, df_links, target_variable)
      else
        df_tmp = compute_correlations(rmse, bias, df_links, target_variable)
        append!(df_all, df_tmp)
      end

    end

    file_final = joinpath(path_results, "correlation_error_$(target_variable).csv")

    df_all |> CSV.write(file_final, delim = ";")

  end

end

run_all()


text:
function transient(
  F,  # coeff. matrix of the sparse tableau eqs.
  nbset,  # tuple with the following elements.
    # nodes: array of Strings containing independent node names.
    # num_nodes: number of nodes.
    # branches: array of struct Branch.
    # num_branches: sum of branch phases.
    # num_dyns: number of dynamic branches.
  h,  # time-step size.
  Tmax  # time where the simulation stops.
  # integration method.
  )  #=

performs a transient simulation. The arguments are described above.

Author: Taku Noda
Started on Jan. 6, 2021
Update on June 29, 2021

Copyright 2021 Central Research Institute of Electric Power Industry

Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.

=#

( nodes, num_nodes, branches, num_branches, num_dyns ) = nbset

N = size( F, 1 )
x = zeros( N, 1 )
y = zeros( N, 1 )
discretize_dyns!( F, nbset, h )
println( "F =" )
display( Array( F ) )

t_dat = Float64[]
i_dat = Float64[]

while t <= Tmax
  global t = t + h
  update_y!( y, nbset, h, x )
  x = F\y
  push!( t_dat, t )
  push!( i_dat, -x[ num_nodes + 1 ] )
end

display( plot( t_dat, i_dat ) )

return nothing

end

max_stars_repo_name: takunoda/omegasim

max_issues_repo_name: takunoda/omegasim

max_forks_repo_name: takunoda/omegasim


text:
immutable PiecewiseInterval{T} <: UnivariateDomain{T}
    points::Vector{T}
    PiecewiseInterval(d::Vector{T})=new(d)
end
PiecewiseInterval(d::AbstractVector)=PiecewiseInterval{eltype(d)}(collect(d))
PiecewiseInterval(d::Number...)=PiecewiseInterval([d...])

function PiecewiseInterval{IT<:Interval}(pcsin::AbstractVector{IT})
    pcs=collect(pcsin)
    p=∂(pop!(pcs))
    successful=true
    while successful
        successful=false
        for k=1:length(pcs)
            if first(pcs[k])==last(p)
                push!(p,last(pcs[k]))
                deleteat!(pcs,k)
                successful=true
                break
            end
        end
    end
    @assert isempty(pcs)
    PiecewiseInterval(p)
end

==(a::PiecewiseInterval,b::PiecewiseInterval)=a.points==b.points

canonicaldomain(d::PiecewiseInterval)=d
numpieces(d::PiecewiseInterval)=length(d.points)-1
pieces(d::PiecewiseInterval,k)=d[k]
pieces{T}(d::PiecewiseInterval{T})=Interval{T}[d[k] for k=1:numpieces(d)]

for OP in (:arclength,:complexlength)
    @eval $OP(d::PiecewiseInterval)=mapreduce($OP,+,pieces(d))
end

Base.getindex(d::PiecewiseInterval,j::Integer)=Interval(d.points[j],d.points[j+1])
isperiodic(d::PiecewiseInterval)=first(d.points)==last(d.points)

Base.reverse(d::PiecewiseInterval)=PiecewiseInterval(reverse(d.points))

isambiguous(d::PiecewiseInterval)=isempty(d.points)
Base.convert{T<:Number}(::Type{PiecewiseInterval{T}},::AnyDomain)=PiecewiseInterval{T}([])
Base.convert{IT<:PiecewiseInterval}(::Type{IT},::AnyDomain)=PiecewiseInterval(Float64[])

function points(d::PiecewiseInterval,n)
   k=div(n,numpieces(d))
    r=n-numpieces(d)*k

    eltype(d)[vcat([points(d[j],k+1) for j=1:r]...);
        vcat([points(d[j],k) for j=r+1:numpieces(d)]...)]
end

Base.rand(d::PiecewiseInterval)=rand(d[rand(1:numpieces(d))])
checkpoints{T}(d::PiecewiseInterval{T})=mapreduce(checkpoints,union,pieces(d))

for OP in (:(Base.first),:(Base.last))
    @eval $OP(d::PiecewiseInterval)=$OP(d.points)
end

# Comparison with UnionDomain
for OP in (:(Base.isapprox),:(==))
    @eval begin
        $OP(a::PiecewiseInterval,b::UnionDomain)=$OP(UnionDomain(pieces(a)),b)
        $OP(b::UnionDomain,a::PiecewiseInterval)=$OP(UnionDomain(pieces(a)),b)
    end
end


text:
module SpecialMatricesModule
export computeMatrixKinetic, computeTensorElectronRepulsionIntegrals, computeMatrixNuclearAttraction, computeMatrixOverlap, computeMatrixCoulomb, computeMatrixExchange, computeMatrixFock
using ..BaseModule
using ..AtomModule
using ..IntegralsModule
using ..BasisModule
using ..GeometryModule
using TensorOperations
using LinearAlgebra
using ..LibInt2Module
using ..ShellModule
using ..ShellModule: nbf
import ..IntegralsModule.computeTensorBlockElectronRepulsionIntegrals

#HELPERS
function scatterMatrixBlocks2D(shells,totaldim::Integer,blockevaluator::Function,blocklength::Function)
  result = zeros(Float64,totaldim,totaldim)
  # column-major order (sh1 counts top to bottom, sh2 counts left to right)
  lastcol,lastrow = 0,0
  for sh1 in shells
    block_col_size = blocklength(sh1)
    lastcol += block_col_size
    for sh2 in shells
      block_row_size = blocklength(sh2)
      lastrow += block_row_size
      result[lastcol-block_col_size+1:lastcol, lastrow-block_row_size+1:lastrow] = blockevaluator(sh1,sh2)
    end
    lastrow = 0
  end
  return result
end

function contractTensorBlocksWithMatrix_CoulombLike(shells,density,totaldim::Integer,tensorblockevaluator::Function,blocklength::Function)
  result = zeros(Float64,totaldim,totaldim)
  # column-major order (sh1 counts top to bottom ("col"-direction), sh2 counts left to right ("row"-direction)
  lastcol,lastrow = 0,0
  lastcol_in_P,lastrow_in_P = 0,0
  for sh1 in shells
    block_col_size = blocklength(sh1)
    lastcol += block_col_size
    for sh2 in shells
      block_row_size = blocklength(sh2)
      lastrow += block_row_size
      for sh3 in shells
	P_block_col_size = blocklength(sh3)
	lastcol_in_P += P_block_col_size
	for sh4 in shells
	  P_block_row_size = blocklength(sh4)
	  lastrow_in_P += P_block_row_size
	  P = density[lastcol_in_P-P_block_col_size+1:lastcol_in_P, lastrow_in_P-P_block_row_size+1:lastrow_in_P]
	  tensor = tensorblockevaluator(sh1,sh2,sh3,sh4)
	  #@tensor block[μ,ν] := tensor[μ,ν,λ,σ] * P[λ,σ]
	  block = zeros(block_col_size,block_row_size)
	  for μ in 1:block_col_size
	    for ν in 1:block_row_size
	      block[μ,ν] = dot(vec(tensor[μ,ν,:,:]),vec(P))
	    end
	  end
	  #end tensor stuff
	  result[lastcol-block_col_size+1:lastcol, lastrow-block_row_size+1:lastrow] += block
	end
	lastrow_in_P = 0
      end
      lastcol_in_P = 0
    end
    lastrow = 0
  end
  return result
end

function contractTensorBlocksWithMatrix_ExchangeLike(shells,density,totaldim::Integer,tensorblockevaluator::Function,blocklength::Function)
  result = zeros(Float64,totaldim,totaldim)
  # column-major order (sh1 counts top to bottom ("col"-direction), sh2 counts left to right ("row"-direction)
  lastcol,lastrow = 0,0
  lastcol_in_P,lastrow_in_P = 0,0
  for sh1 in shells
    block_col_size = blocklength(sh1)
    lastcol += block_col_size
    for sh2 in shells
      block_row_size = blocklength(sh2)
      lastrow += block_row_size
      for sh3 in shells
	P_block_col_size = blocklength(sh3)
	lastcol_in_P += P_block_col_size
	for sh4 in shells
	  P_block_row_size = blocklength(sh4)
	  lastrow_in_P += P_block_row_size
	  P = density[lastcol_in_P-P_block_col_size+1:lastcol_in_P, lastrow_in_P-P_block_row_size+1:lastrow_in_P]
	  tensor = tensorblockevaluator(sh1,sh3,sh2,sh4)
	  @tensor block[μ,ν] := tensor[μ,λ,ν,σ] * P[λ,σ]
	  result[lastcol-block_col_size+1:lastcol, lastrow-block_row_size+1:lastrow] += block
	end
	lastrow_in_P = 0
      end
      lastcol_in_P = 0
    end
    lastrow = 0
  end
  return result
end

#OVERLAP
function computeMatrixOverlap(basis::GaussianBasis)
  return [computeIntegralOverlap(cgb1,cgb2) for cgb1 in basis.contractedBFs, cgb2 in basis.contractedBFs]
end

function computeMatrixOverlap(shells::Vector{Shell})
  totaldim = mapreduce(nbf,+,shells)
  return scatterMatrixBlocks2D(shells,totaldim,computeMatrixBlockOverlap,nbf)
end

if (LibInt2Shell != Shell)
  function computeMatrixOverlap(shells::Vector{LibInt2Shell})
    totaldim, maxprims, maxlqn = LibInt2Module.computeDimensions(shells)
    engine = LibInt2EngineOverlap(maxprims,maxlqn)
    result = scatterMatrixBlocks2D(shells,totaldim,(sh1,sh2)->computeMatrixBlockOverlap(engine,sh1,sh2),nbf)
    destroy!(engine)
    return result
  end
end

#KINETIC
function computeMatrixKinetic(basis::GaussianBasis)
  return [computeIntegralKinetic(cgb1,cgb2) for cgb1 in basis.contractedBFs, cgb2 in basis.contractedBFs]
end

function computeMatrixKinetic(shells::Vector{Shell})
  totaldim = mapreduce(nbf,+,shells)
  return scatterMatrixBlocks2D(shells,totaldim,computeMatrixBlockKinetic,nbf)
end

if (LibInt2Shell != Shell)
  function computeMatrixKinetic(shells::Vector{LibInt2Shell})
    totaldim, maxprims, maxlqn = LibInt2Module.computeDimensions(shells)
    engine = LibInt2EngineKinetic(maxprims,maxlqn)
    result = scatterMatrixBlocks2D(shells,totaldim,(sh1,sh2)->computeMatrixBlockKinetic(engine,sh1,sh2),nbf)
    destroy!(engine)
    return result
  end
end

#NUCLEARATTRACTION
function computeMatrixNuclearAttraction(basis::GaussianBasis,geo::Geometry)
  return [sum([computeIntegralNuclearAttraction(cgb1,cgb2,atom) for atom in geo.atoms]) for cgb1 in basis.contractedBFs, cgb2 in basis.contractedBFs]
end

function computeMatrixNuclearAttraction(shells::Vector{Shell},atom::Atom)
  totaldim = mapreduce(nbf,+,shells)
  return scatterMatrixBlocks2D(shells,totaldim,(sh1,sh2)->computeMatrixBlockNuclearAttraction(sh1,sh2,atom),nbf)
end

function computeMatrixNuclearAttraction(shells::Vector{Shell},geo::Geometry)
  totaldim = mapreduce(nbf,+,shells)
  result = zeros(totaldim,totaldim)
  for atom in geo.atoms
    result += scatterMatrixBlocks2D(shells,totaldim,(sh1,sh2)->computeMatrixBlockNuclearAttraction(sh1,sh2,atom),nbf)
  end
  return result
end

if (LibInt2Shell != Shell)
  function computeMatrixNuclearAttraction(shells::Vector{LibInt2Shell},atom::Atom)
    totaldim, maxprims, maxlqn = LibInt2Module.computeDimensions(shells)
    engine = LibInt2EngineNuclearAttraction(maxprims,maxlqn,atom)
    result = scatterMatrixBlocks2D(shells,totaldim,(sh1,sh2)->computeMatrixBlockNuclearAttraction(engine,sh1,sh2),nbf)
    destroy!(engine)
    return result
  end

  function computeMatrixNuclearAttraction(shells::Vector{LibInt2Shell},geo::Geometry)
    totaldim, maxprims, maxlqn = LibInt2Module.computeDimensions(shells)
    engine = LibInt2EngineNuclearAttraction(maxprims,maxlqn,geo)
    result = scatterMatrixBlocks2D(shells,totaldim,(sh1,sh2)->computeMatrixBlockNuclearAttraction(engine,sh1,sh2),nbf)
    destroy!(engine)
    return result
  end
end

#ERIS
function computeTensorElectronRepulsionIntegrals(basis::GaussianBasis)
	return ERIs = [computeElectronRepulsionIntegral(μ,ν,λ,σ) for μ in basis.contractedBFs, ν in basis.contractedBFs, λ in basis.contractedBFs, σ in basis.contractedBFs]
end

function computeTensorBlockElectronRepulsionIntegrals(μ::Shell,ν::Shell,λ::Shell,σ::Shell)
  [computeElectronRepulsionIntegral(μcgbf,νcgbf,λcgbf,σcgbf) for μcgbf in expandShell(μ), νcgbf in expandShell(ν), λcgbf in expandShell(λ), σcgbf in expandShell(σ)]
end

if (LibInt2Shell != Shell)
  function computeTensorElectronRepulsionIntegrals(shells::Vector{LibInt2Shell})
    totaldim, maxprims, maxlqn = LibInt2Module.computeDimensions(shells)
    engine = LibInt2EngineCoulomb(maxprims,maxlqn)
    result = computeTensorElectronRepulsionIntegrals(shells,computeBlockERI=((μ,ν,λ,σ)->copy(computeTensorBlockElectronRepulsionIntegrals(engine,μ,ν,λ,σ))))
    destroy!(engine)
    return result
  end
end

function computeTensorElectronRepulsionIntegrals(shells::Union{Vector{Shell},Vector{LibInt2Shell}}; computeBlockERI=computeTensorBlockElectronRepulsionIntegrals)
  cat(
      [cat(
	   [cat(
		[cat(
		     [computeBlockERI(μsh,νsh,λsh,σsh) for μsh in shells]...; dims=1)
		 for νsh in shells]...; dims=2)
	    for λsh in shells]...; dims=3)
       for σsh in shells]...; dims=4)
end

#COULOMB
function computeMatrixCoulomb(basis::GaussianBasis, density::Matrix)
  N = length(basis.contractedBFs)

  J = zeros(N,N)
  μIndex = 0
  for μ in basis.contractedBFs
    μIndex+=1
    for νIndex in 1:μIndex
      ν = basis.contractedBFs[νIndex]
      ERImatrix = [computeElectronRepulsionIntegral(μ,ν,λ,σ) for λ in basis.contractedBFs, σ in basis.contractedBFs]
      J[μIndex,νIndex] = tr(ERImatrix*density)
      J[νIndex,μIndex] = J[μIndex,νIndex]
    end
  end
  return J
end

function computeMatrixCoulomb(electronRepulsionIntegrals::Array{Float64,4}, density::Matrix)
  @tensor J[μIndex,νIndex] := electronRepulsionIntegrals[μIndex,νIndex,λIndex,σIndex] * density[λIndex,σIndex]
end

function computeMatrixCoulomb(shells::Vector{Shell}, density)
  totaldim = mapreduce(nbf,+,shells)
  contractTensorBlocksWithMatrix_CoulombLike(shells,density,totaldim,computeTensorBlockElectronRepulsionIntegrals,nbf)
end

if (LibInt2Shell != Shell)
  function computeMatrixCoulomb(shells::Vector{LibInt2Shell}, density::Matrix)
    totaldim, maxprims, maxlqn = LibInt2Module.computeDimensions(shells)
    engine = LibInt2EngineCoulomb(maxprims,maxlqn)
    result = contractTensorBlocksWithMatrix_CoulombLike(shells,density,totaldim,(μ,ν,λ,σ)->computeTensorBlockElectronRepulsionIntegrals(engine,μ,ν,λ,σ),nbf)
    destroy!(engine)
    return result
  end
end

#EXCHANGE
function computeMatrixExchange(basis::GaussianBasis, density::Matrix)
  N = length(basis.contractedBFs)

  K = zeros(N,N)
  μIndex = 0
  for μ in basis.contractedBFs
    μIndex+=1
    for νIndex in 1:μIndex
      ν = basis.contractedBFs[νIndex]
      ERImatrix = [computeElectronRepulsionIntegral(μ,λ,ν,σ) for λ in basis.contractedBFs, σ in basis.contractedBFs]
      K[μIndex,νIndex] = tr(ERImatrix*density)
      K[νIndex,μIndex] = K[μIndex,νIndex]
    end
  end
  return K
end

function computeMatrixExchange(electronRepulsionIntegrals::Array{Float64,4}, density::Matrix)
  @tensor K[μIndex,νIndex] := electronRepulsionIntegrals[μIndex,λIndex,νIndex,σIndex] * density[λIndex,σIndex]
end

function computeMatrixExchange(shells::Vector{Shell}, density)
  totaldim = mapreduce(nbf,+,shells)
  contractTensorBlocksWithMatrix_ExchangeLike(shells,density,totaldim,computeTensorBlockElectronRepulsionIntegrals,nbf)
end

if (LibInt2Shell != Shell)
  function computeMatrixExchange(shells::Vector{LibInt2Shell}, density::Matrix)
    totaldim, maxprims, maxlqn = LibInt2Module.computeDimensions(shells)
    engine = LibInt2EngineCoulomb(maxprims,maxlqn)
    result = contractTensorBlocksWithMatrix_ExchangeLike(shells,density,totaldim,(μ,ν,λ,σ)->computeTensorBlockElectronRepulsionIntegrals(engine,μ,ν,λ,σ),nbf)
    destroy!(engine)
    return result
  end
end

#FOCK
function computeMatrixFock(
  matrixKinetic::Matrix,
  matrixNuclearAttraction::Matrix,
  matrixCoulomb::Matrix,
  matrixExchange::Matrix)
  return matrixKinetic + matrixNuclearAttraction + 2*matrixCoulomb-matrixExchange
end

function computeMatrixFock(
  basis::GaussianBasis,
  geometry::Geometry,
  density::Matrix,
	computeMatrixCoulomb=computeMatrixCoulomb,
	computeMatrixExchange=computeMatrixExchange)
  T = computeMatrixKinetic(basis)
  V = computeMatrixNuclearAttraction(basis,geometry)
  J = computeMatrixCoulomb(basis,density)
  K = computeMatrixExchange(basis,density)
  return computeMatrixFock(T,V,J,K)
end

function computeMatrixFock(
  density::Matrix,
  matrixKinetic::Matrix,
  matrixNuclearAttraction::Matrix,
  electronRepulsionIntegralsColoumb::Array{Float64,4},
  electronRepulsionIntegralsExchange::Array{Float64,4}=electronRepulsionIntegralsColoumb)
  T = matrixKinetic
  V = matrixNuclearAttraction
  J = computeMatrixCoulomb(electronRepulsionIntegralsColoumb,density)
  K = computeMatrixExchange(electronRepulsionIntegralsExchange,density)
  return computeMatrixFock(T,V,J,K)
end

end


text:
import Base: iterate, length

export Cluster

"A collection of multiple engines attached to a rocket"
abstract type Cluster <: Engine end

mass(cl::Cluster) = sum(mass, cl)
thrust(cl::Cluster) = sum(thrust, cl)

struct UniformCluster <: Cluster
    engine::Engine
    count::Int
end

struct MixedCluster <: Cluster
   engines::Vector{Engine}
end

"""
    Cluster(e::Engine, n::Integer)
Create a homogenous cluster of `n` engine of type `e`.
"""
function Cluster(engine::Engine, count::Integer)
    UniformCluster(engine, count)
end

"""
    Cluster(engines::Engine...)
Create a cluster of at least one engine.
"""
function Cluster(engine::Engine, engines::Engine...)
    sametype(e) = (typeof(engine) == typeof(e))
    if all(sametype, engines)
        UniformCluster(engine, length(engines) + 1)
    else
        MixedCluster([engine, engines...])
    end
end

function iterate(cluster::UniformCluster)
    cluster.engine, 2
end

function iterate(cluster::UniformCluster, i::Integer)
    if i > cluster.count
        nothing
    else
        cluster.engine, i+1
    end
end

function iterate(cluster::MixedCluster)
    cluster.engines[1], 2
end

function iterate(cluster::MixedCluster, i::Integer)
    if i > length(cluster.engines)
        nothing
    else
        cluster.engines[i], i+1
    end
end

length(cluster::UniformCluster) = cluster.count
length(cluster::MixedCluster) = length(cluster.engines)

Isp(cl::UniformCluster) = Isp(cl.engine)
Isp(cl::MixedCluster) = sum(Isp, cl)/length(cl)


text:
# Use baremodule to shave off a few KB from the serialized `.ji` file
baremodule Triangle_jll
using Base
using Base: UUID
import JLLWrappers

JLLWrappers.@generate_main_file_header("Triangle")
JLLWrappers.@generate_main_file("Triangle", UUID("5639c1d2-226c-5e70-8d55-b3095415a16a"))
end  # module Triangle_jll


text:
using FileIO, DelimitedFiles
using LinearAlgebra
using Rotations#, StaticArrays
# import Makie
# import GeometryTypes

mutable struct Trajectory
    n::Int64
    fixed_id::Int64
    ids::Vector{Int64}
    poses::Dict{Int64, Matrix{Float64}}
    is_pose_Twc::Bool # whether poses are in Twc format
end

function Base.getindex(X::Trajectory, i::Int)
    return X.poses[X.ids[i]]
end

function getTno2Idx(traj::Trajectory, k::Int64)
    @assert k>1
    idn =traj.ids[k]
    ido = traj.ids[k-1]

    To = traj.poses[ido]
    Tn = traj.poses[idn]
    Tno = Tn*inv(To)
    return Tno
end

# function load_trajectory(traj_filename::String)
#     traj_data = readdlm(traj_filename)
#     n = size(traj_data, 1)
#     ids = Vector{Int64}(undef, n)
#     poses = Dict{Int64, Matrix{Float64}}()
#     fixed_id = 0
#     for i=1:n

#         ids[i] = convert(Int64, traj_data[i, 1])
#         if convert(Int64, traj_data[i, 2]) == 1
#             fixed_id = ids[i]
#         end
#         # load
#         T = Matrix{Float64}(I, 4, 4)
#         T[1:3, 4] = traj_data[i, 3:5]
#         x, y, z, w = traj_data[i, 6:9]
#         T[1:3, 1:3] = Array(Quat(w,x,y,z))
#         poses[ids[i]] = T
#     end
#     return Trajectory(n, fixed_id, ids, poses)
# end

function load_covs(cov_fname::String)
    cov_data = readdlm(cov_fname)
    n = size(cov_data, 1)
    covs = Dict{Int64, Matrix{Float64}}()
    for i=1:n
        id = Int64(cov_data[i, 1])
        covs[id] = copy(reshape(cov_data[i, 2:37], 6, 6))
    end
    return covs
end

"""
Redwood dataset use Twc to represent camera pose
"""
function load_redwood_traj(fname::String)
    ids = Vector{Int64}(undef, 0)
    poses = Dict{Int64, Matrix{Float64}}()
    open(fname) do file
        # do stuff with the open file
        while !eof(file)
            aline = readline(file)
            split_strs = split(aline, (' ', '\t'))
            # @show split_strs
            id = parse(Int64, split_strs[1])
            num2 = parse(Int64, split_strs[2])
            num3 = parse(Int64, split_strs[3])

            mat4 = Matrix{Float64}(undef, 4, 4)
            for i=1:4
                line_mat = readline(file)
                line_strs_raw = split(line_mat, (' ', '\t'))
                line_strs = [s for s = line_strs_raw if length(s)>0]
                for j=1:4
                    mat4[i, j] = parse(Float64, line_strs[j])
                end
            end

            # @show line1, line2, line3, line4
            # matchall(nmbr, line[linenum])[index]
            push!(ids, id)
            poses[id] = mat4
            # @show id, num2, num3
            # @show mat4
        end
    end
    sort!(ids)
    is_pose_Twc = true
    return Trajectory(length(ids), ids[1], ids, poses, is_pose_Twc)
end

# function save_redwood_poses(fname::String, poses::Dict{Tuple{Int64, Int64}, Matrix{Float64}}; num3=0)
#     open(fname, "w") do file
#         for ((num1, num2), T) = poses
#             if num1 > num2
#                 continue
#             end
#             write(file, "$num1\t$num2\t$num3\n")
#             DelimitedFiles.writedlm(file, round.(T, digits=7), " ")
#         end
#     end
# end

"""
pose should translate id1 to id2
id1 should be smaller than id2
Redwood has T12
"""
function save_redwood_poses(
    fname::String,
    poses::Dict{Tuple{Int64, Int64}, Matrix{Float64}};
    num3=0,
    keep_orginal::Bool=false)
    open(fname, "w") do file
        for ((num1, num2), T) = poses
            if num1 > num2
                continue
            end
            T_out = if keep_orginal
                T
            else
                LinearAlgebra.inv(T)
            end

            write(file, "$num1\t$num2\t$num3\n")
            DelimitedFiles.writedlm(file, round.(T_out, digits=7), " ")
        end
    end

end

"""
pose should translate id1 to id2
id1 should be smaller than id2
Redwood has T12
"""
function save_redwood_poses(
    fname::String,
    poses::Dict{Tuple{Int64, Int64, Int64}, Matrix{Float64}};
    keep_orginal::Bool=false)
    open(fname, "w") do file
        for ((num1, num2, num3), T) = poses
            if (num1 > num2) && !keep_orginal
                continue
            end
            T_out = if keep_orginal
                T
            else
                LinearAlgebra.inv(T)
            end

            write(file, "$num1\t$num2\t$num3\n")
            DelimitedFiles.writedlm(file, round.(T_out, digits=7), " ")
        end
    end

end

"""
pose should translate id1 to id2, unless use keep_orginal
This can also be used for writing information
"""
function save_redwood_file(
    fname::String,
    poses::Dict{Tuple{Int64, Int64}, Matrix{Float64}};
    num3=0,
    keep_orginal::Bool=false)
    open(fname, "w") do file
        for ((num1, num2), T) = poses
            if keep_orginal
                write(file, "$num1\t$num2\t$num3\n")
                DelimitedFiles.writedlm(file, round.(T, digits=7), " ")
            elseif (num1 > num2)
                write(file, "$num2\t$num1\t$num3\n")
                DelimitedFiles.writedlm(file, round.(T, digits=7), " ")
            else
                write(file, "$num1\t$num2\t$num3\n")
                DelimitedFiles.writedlm(file, round.(LinearAlgebra.inv(T), digits=7), " ")
            end
        end
    end
end

"""
pose should translate id1 to id2
default is_Twc should keeo T as original
"""
function save_redwood_traj(
    fname::String,
    poses::Dict{Int64, Matrix{NT}};
    is_Twc::Bool=true
) where NT<:Number
    open(fname, "w") do file
        for num1 = sort(collect(keys(poses)))
            T = poses[num1]
            write(file, "$num1\t$num1\t$(num1+1)\n")
            if is_Twc
                DelimitedFiles.writedlm(file, round.(T, digits=7), " ")
            else
                DelimitedFiles.writedlm(file, round.(inv(T), digits=7), " ")
            end
        end
    end
end

# """
# vector of (src_id, dst_id, T)
# pose should translate id1 to id2
# """
# function save_redwood_file(fname::String, poses::Vector{Tuple{Int64, Int64, Matrix{Float64}}}; num3=0)
#     open(fname, "w") do file
#         for (num1, num2, T) = poses
#             if num1 > num2
#                 write(file, "$num2\t$num1\t$num3\n")
#                 DelimitedFiles.writedlm(file, round.(T, digits=7), " ")
#             else
#                 write(file, "$num1\t$num2\t$num3\n")
#                 DelimitedFiles.writedlm(file, round.(LinearAlgebra.inv(T), digits=7), " ")
#             end
#
#         end
#     end
# end

"""
note that the file has dst_id, src_id, num and T_dst_src
but we read it to be [src_id, dst_id, Tds]
Unless keep_orginal=true, then data is read in the same way that it is stored in
the file.

When keep_orginal=true, the dict is [dst_id, src_id, num and Tds]
"""
function load_redwood_loop(fname::String; keep_orginal::Bool=false)
    # ids = Vector{Int64}(undef, 0)
    poses = Dict{Tuple{Int64, Int64, Int64}, Matrix{Float64}}()
    open(fname) do file
        # do stuff with the open file
        while !eof(file)
            aline = readline(file)
            split_strs = split(aline, (' ', '\t'))
            # @show split_strs
            num1 = parse(Int64, split_strs[1])
            num2 = parse(Int64, split_strs[2])
            num3 = parse(Int64, split_strs[3])

            mat4 = Matrix{Float64}(undef, 4, 4)
            for i=1:4
                line_mat = readline(file)
                line_strs_raw = split(line_mat, (' ', '\t'))
                line_strs = [s for s = line_strs_raw if length(s)>0]
                for j=1:4
                    mat4[i, j] = parse(Float64, line_strs[j])
                end
            end

            # @show line1, line2, line3, line4
            # matchall(nmbr, line[linenum])[index]
            # push!(ids, id)
            if keep_orginal
                poses[(num1, num2, num3)] = mat4
            else
                if num1 > num2
                    poses[(num2, num1, num3)] = mat4
                else
                    poses[(num1, num2, num3)] = LinearAlgebra.inv(mat4)
                end
            end

            # @show id, num2, num3
            # @show mat4
        end
    end
    # sort!(ids)
    return poses
end

function load_redwood_loop_info(fname::String)::Dict{Tuple{Int64, Int64, Int64}, Matrix{Float64}}
    # ids = Vector{Int64}(undef, 0)
    infos = Dict{Tuple{Int64, Int64, Int64}, Matrix{Float64}}()
    open(fname) do file
        # do stuff with the open file
        while !eof(file)
            aline = readline(file)
            split_strs = split(aline, (' ', '\t'))
            # @show split_strs
            num1 = parse(Int64, split_strs[1])
            num2 = parse(Int64, split_strs[2])
            num3 = parse(Int64, split_strs[3])

            mat6 = Matrix{Float64}(undef, 6, 6)
            for i=1:6
                line_mat = readline(file)
                line_strs_raw = split(line_mat, (' ', '\t'))
                line_strs = [s for s = line_strs_raw if length(s)>0]
                for j=1:6
                    mat6[i, j] = parse(Float64, line_strs[j])
                end
            end

            # @show line1, line2, line3, line4
            # matchall(nmbr, line[linenum])[index]
            # push!(ids, id)
            infos[(num1, num2, num3)] = mat6

            # @show id, num2, num3
            # @show mat4
        end
    end
    # sort!(ids)
    return infos
end

"""
build pose from odom for redwood data
returns Twc
"""
function odom2pose(
	    odom::Dict{Tuple{Int64, Int64, Int64}, Matrix{Float64}};
		start_id::Int64=0,
		dst_first=true)
	@assert dst_first == true
	num = first(odom).first[3]
	poses = Dict{Int64, Matrix{Float64}}()
	poses[start_id] = Matrix{Float64}(LinearAlgebra.I, 4, 4)
	for id = start_id+1:num-1 # last id is num -1?
		increment = odom[(id-1, id, num)]
		last_pose = poses[id-1]
		poses[id] = last_pose * increment

	end
	return poses
end

"""
load BundleFusion Trajectory file format
use Twc to represent camera pose
"""
function load_boundlefusion_traj(fname::String)
    ids = Vector{Int64}(undef, 0)
    poses = Dict{Int64, Matrix{Float64}}()
    open(fname) do file
        # do stuff with the open file
        while !eof(file)
            # read line for index
            id_line = readline(file)
            id = parse(Int64, strip(id_line))

            # aline = readline(file)
            # split_strs = split(aline, (' ', '\t'))
            # # @show split_strs
            # id = parse(Int64, split_strs[1])
            # num2 = parse(Int64, split_strs[2])
            # num3 = parse(Int64, split_strs[3])

            # read matrix
            mat4 = Matrix{Float64}(undef, 4, 4)
            for i=1:4
                line_mat = readline(file)
                line_strs_raw = split(line_mat, (' ', '\t'))
                line_strs = [s for s = line_strs_raw if length(s)>0]
                for j=1:4
                    mat4[i, j] = parse(Float64, line_strs[j])
                end
            end
            # read empty line
            readline(file)

            # @show line1, line2, line3, line4
            # matchall(nmbr, line[linenum])[index]
            push!(ids, id)
            poses[id] = mat4
            # @show id, num2, num3
            # @show mat4
        end
    end
    sort!(ids)
    return Trajectory(length(ids), ids[1], ids, poses)
end

# function view_traj!(org_graph_scene, traj::Trajectory; linecolor=:red)
#     # traj = load_redwood_traj(pose_fname)
#
#     # 1.2 visualize all pcds
#     locations = Vector{Makie.Point3f0}(undef, traj.n)
#     # edges = Vector{Pair{Point3f0, Point3f0}}(undef, 0)
#     arrow_dirs = Vector{Makie.Point3f0}(undef, traj.n-1)
#
#     for id = traj.ids
#         locations[id + 1] = Makie.Point3f0(traj.poses[id][1:3, 4])
#         if id != 0
#             a_edge = Pair(locations[id], locations[id+1])
#             # push!(edges, a_edge)
#             arrow_dirs[id] = locations[id+1] - locations[id]
#         end
#     end
#     # org_graph_scene = Makie.Scene()
#     # scatter!(org_graph_scene, locations)
#     # linesegments!(org_graph_scene, edges, color = :red)
#     Makie.arrows!(org_graph_scene,
#             locations[1:traj.n-1],
#             arrow_dirs,
#             arrowcolor=:black,
#             arrowsize=0.1,
#             linecolor=linecolor)
# end
#
#
# function view_traj(traj::Trajectory; linecolor=:red)
#     org_graph_scene = Makie.Scene()
#     view_traj!(org_graph_scene, traj, linecolor=linecolor)
#     return org_graph_scene
# end