text:
"Abstract base class for linear system solvers"
abstract LinearSolver

"Abstract class for solvers using static condensation"
abstract StaticCond <: LinearSolver

"Solver using Static condensation on symmetric positive definite problems."
type CholeskySC{T <: Number, Mat<:BBSolver, Dof <: DofMap} <: StaticCond
    dof::Dof
    Abb::Mat
    Aii::Vector{Matrix{T}}
    Aiifact::Vector{Base.LinAlg.LU{T,Array{T,2}}}
    M::Vector{Matrix{T}}
    ub::Vector{T}
    Fi::Matrix{T}
    lft::Dict{Int,DirichiletLift}
    decomp::Bool
    Abbe::Matrix{T}
    Abie::Matrix{T}
    fbe::Vector{T}
end

bbmatrix(solver::CholeskySC) = solver.Abb
dofmap(solver::CholeskySC) = solver.dof

function trf!(solver::CholeskySC)
    if nbslvmodes(solver.dof) > 0
        trf!(solver.Abb)
    end
    solver.decomp = true
end

function CholeskySC{T<:Number, Mat<:BBSolver, Dof <: DofMap}(dof::Dof, ::Type{Mat},
                                                             ::Type{T}=Float64)
    nel = num_elems(dof)
    nbslv  = nbslvmodes(dof)
    nb = nbmodes(dof)
    Abb = Mat{T}(dof)
    Aii = Vector{Array{T,2}}(nel)
    #Aiifact = Vector{Base.LinAlg.Cholesky{T,Array{T,2}}}(nel)
    Aiifact = Vector{Base.LinAlg.LU{T,Array{T,2}}}(nel)
    M = Vector{Array{T,2}}(nel)

    lmap = locmap(dof)

    nbe = nbndry(lmap)
    nie = ninterior(lmap)
    Fi = zeros(T, nie, nel)
    for i = 1:nel
        Aii[i] = zeros(T, nie, nie)
        M[i] = zeros(T, nie, nbe)
    end
    ub = zeros(T, nbslv)
    lft = Dict{Int,DirichiletLift}()
    Abbe = zeros(T, nbe, nbe)
    Abie = zeros(T, nbe, nie)
    fbe = zeros(T, nbe)
    CholeskySC(dof, Abb, Aii, Aiifact, M, ub, Fi, lft, false, Abbe, Abie, fbe)

end

#using Base.LinAlg.BLAS.gemm!
#using Base.LinAlg.BLAS.gemv!
#using Base.LinAlg.LAPACK.potrf!
#using Base.LinAlg.LAPACK.potrs!

function add_local_matrix{T<:Number}(solver::CholeskySC{T}, e::Integer,
                                     Ae::AbstractMatrix{T})
    dof = dofmap(solver)
    lmap = locmap(dof)
    nb = nbndry(lmap)
    ni = ninterior(lmap)
    ib = bndidx(lmap)
    ii = intidx(lmap)

    Aii = solver.Aii[e]
    for i = 1:ni
        for k = 1:ni
            Aii[k,i] = Ae[ ii[k], ii[i] ]
        end
    end
    #potrf!('L', Aii)
    #fact = cholfact!(Aii)
    fact = lufact!(Aii)
    solver.Aiifact[e] = fact
    M = solver.M[e]
    for k = 1:nb
        for i = 1:ni
            M[i,k] = Ae[ii[i],ib[k]]
        end
    end

    A_ldiv_B!(fact, M)
    Abb = solver.Abbe
    Abi = solver.Abie
    for i in 1:nb
        for k in 1:nb
            Abb[k,i] = Ae[ib[k], ib[i]]
        end
    end

    for i in 1:ni
        for k in 1:nb
            Abi[k,i] = Ae[ib[k], ii[i]]
        end
    end

    gemm!('N', 'N', -one(T), Abi, M, one(T), Abb)

    if hasdirbc(dof, e)
        solver.lft[e] = DirichiletLift(Abb, idirbc(dof, e))
    end

    assemble!(bbmatrix(solver), Abb, bmap(dof, e))
end

function solve!{T<:Number}(solver::CholeskySC{T}, Fe::AbstractMatrix{T})
    if !solver.decomp
        trf!(solver)
        solver.decomp = true
    end
    dof = dofmap(solver)
    lmap = locmap(dof)
    ib = bndidx(lmap)
    ii = intidx(lmap)
    nel = num_elems(dof)

    nbe = nbndry(lmap)
    nie = ninterior(lmap)
    Fb = solver.ub
    nb = nbmodes(dof)
    nbslv = nbslvmodes(dof)
    for i = 1:nbslv
        Fb[i] = zero(T)
    end
    Fbe = solver.fbe
    for e = 1:nel

        Fie = view(solver.Fi, :, e)

        for i = 1:nbe
            Fbe[i] = Fe[ib[i],e]
        end
        if hasdirbc(dof, e)
            lift!(solver.lft[e], Fbe)
        end
        for i = 1:nie
            Fie[i] = Fe[ii[i],e]
        end
        gemv!('T', -one(T), solver.M[e], Fie, one(T), Fbe)

        m = bmap(dof, e)

        for i in 1:nbe
            ig = m[i]
            if ig <= nbslv
                Fb[ig] += Fbe[i]
            end
        end

    end

    # Solve linear system (boundary-boundary system
    Abb = bbmatrix(solver)
    if nbslv > 0
        trs!(Abb, Fb)
    end
    # Scatter the results and solve for each element:
    for e = 1:nel
        m = bmap(dof, e)
        for i = 1:nbe
            ig = m[i]
            if ig <= nbslv
                Fbe[i] = Fb[ig]
            else
                # Dirichilet BC
                Fbe[i] = Fe[ib[i],e]
            end
        end
        Fie = view(solver.Fi, :, e)
        #potrs!('L', solver.Aii[e], Fie)
        A_ldiv_B!(solver.Aiifact[e], Fie)
        gemv!('N', -one(T), solver.M[e], Fbe, one(T), Fie)

        for i = 1:nbe
            Fe[ib[i], e] = Fbe[i]
        end
        for i = 1:nie
            Fe[ii[i], e] = Fie[i]
        end
    end

    return Fe

end

type LU_SC{Mat<:BBSolver, T <: Number} <: StaticCond
    dof::DofMap
    Abb::Mat
    Aii::Array{T, 3}
    M::Array{T,3}
end


text:
### A Pluto.jl notebook ###
# v0.12.7

using Markdown
using InteractiveUtils

# ╔═╡ e077ab16-22e5-11eb-15ed-5f0654bf7328
using Pkg, DrWatson

# ╔═╡ ca5fbdc8-22e5-11eb-0a60-cb0a127a3ca5
md"## Listing4.01"

# ╔═╡ e75a9a06-22e5-11eb-2e36-8d4f62b830ed
begin
	@quickactivate "StatisticsWithJuliaPlutoNotebooks"
end

# ╔═╡ fa68607e-22e5-11eb-0558-c9a4d9f77426
begin
	f(z::Int) = begin z = 0 end
	f(z::Array{Int}) = begin z[1] = 0 end
end

# ╔═╡ 572972b2-22e6-11eb-1ec4-2d53fc807d03
begin
	x1 = 1
	typeof(x1)
end

# ╔═╡ 5729a994-22e6-11eb-00b0-e525c9e271cb
isimmutable(x1)

# ╔═╡ 572a4444-22e6-11eb-351f-65750fcd0d5e
Text("Before call by value: $x1")

# ╔═╡ 57305f50-22e6-11eb-1aef-61c42e0c6363
f(x1)

# ╔═╡ 573d81bc-22e6-11eb-3b89-010224cc8027
Text("After call by value: $x1")

# ╔═╡ 573e105a-22e6-11eb-32cb-859417a95d32
begin
	x2 = [1]
	typeof(x2)
end

# ╔═╡ 574411da-22e6-11eb-2e9e-739f51ea9fc6
isimmutable(x2)

# ╔═╡ 574ff9c8-22e6-11eb-3866-817010612a7e
Text("Before call by reference: $x2")

# ╔═╡ 575094b4-22e6-11eb-25b6-fb702eec82a2
f(x2)

# ╔═╡ 575b02be-22e6-11eb-29ad-3f48f1b6eae6
Text("After call by reference: $x2")

# ╔═╡ 475ff888-22e6-11eb-2354-09f8f40a8e12
md"## End of listing4.01"

# ╔═╡ Cell order:
# ╟─ca5fbdc8-22e5-11eb-0a60-cb0a127a3ca5
# ╠═e077ab16-22e5-11eb-15ed-5f0654bf7328
# ╠═e75a9a06-22e5-11eb-2e36-8d4f62b830ed
# ╠═fa68607e-22e5-11eb-0558-c9a4d9f77426
# ╠═572972b2-22e6-11eb-1ec4-2d53fc807d03
# ╠═5729a994-22e6-11eb-00b0-e525c9e271cb
# ╠═572a4444-22e6-11eb-351f-65750fcd0d5e
# ╠═57305f50-22e6-11eb-1aef-61c42e0c6363
# ╠═573d81bc-22e6-11eb-3b89-010224cc8027
# ╠═573e105a-22e6-11eb-32cb-859417a95d32
# ╠═574411da-22e6-11eb-2e9e-739f51ea9fc6
# ╠═574ff9c8-22e6-11eb-3866-817010612a7e
# ╠═575094b4-22e6-11eb-25b6-fb702eec82a2
# ╠═575b02be-22e6-11eb-29ad-3f48f1b6eae6
# ╟─475ff888-22e6-11eb-2354-09f8f40a8e12

ext: jl


text:
@testset "2bit" begin
    buffer = IOBuffer()
    writer = TwoBit.Writer(buffer, ["chr1", "chr2"])
    chr1 = dna"ACGTNN"
    chr2 = dna"N"^100 * dna"ACGT"^100 * dna"N"^100
    write(writer, TwoBit.Record("chr1", chr1))
    write(writer, TwoBit.Record("chr2", chr2))
    seekstart(buffer)
    reader = TwoBit.Reader(buffer)
    @test length(reader) == 2
    @test TwoBit.seqnames(reader) == ["chr1", "chr2"]
    @test TwoBit.sequence(reader["chr1"]) == chr1
    @test TwoBit.sequence(reader["chr2"]) == chr2
    @test TwoBit.sequence(reader["chr1"]) == chr1
    @test_throws KeyError reader["chr10"]
    @test_throws BoundsError reader[3]

    function check_2bit_parse(filename)
        stream = open(TwoBit.Reader, filename)
        @test eltype(stream) === TwoBit.Record
        # read from a stream
        for record in stream
            @test hassequence(record) == TwoBit.hassequence(record) == true
            a = TwoBit.sequence(ReferenceSequence, record)
            b = TwoBit.sequence(DNASequence, record)
            @test TwoBit.sequence(ReferenceSequence, record) == TwoBit.sequence(DNASequence, record)
        end
        close(stream)

        # round trip
        buffer = IOBuffer()
        reader = open(TwoBit.Reader, filename)
        writer = TwoBit.Writer(buffer, TwoBit.seqnames(reader))
        expected_entries = TwoBit.Record[]
        for (name, record) in zip(TwoBit.seqnames(reader), reader)
            write(writer, TwoBit.Record(name, TwoBit.sequence(record), TwoBit.maskedblocks(record)))
            push!(expected_entries, record)
        end

        read_entries = TwoBit.Record[]
        seekstart(buffer)
        for record in TwoBit.Reader(buffer)
            push!(read_entries, record)
        end

        return expected_entries == read_entries
    end

    get_bio_fmt_specimens()
    path = joinpath(dirname(@__FILE__), "..", "BioFmtSpecimens", "2bit")
    for specimen in YAML.load_file(joinpath(path, "index.yml"))
        valid = get(specimen, "valid", true)
        filepath = joinpath(path, specimen["filename"])
        if valid
            @test check_2bit_parse(filepath)
        else
            @test_throws Exception check_fasta_parse(filepath)
        end
    end
end


text:
# EXCLUDE FROM TESTING
# Note: this is excluded as it would slow doen the tests too much
#
# This is an example of a 2D viscoelastic porosity wave as described in
# Vasyliev et al. Geophysical Research Letters (25), 17. p. 3239-3242
#
#
# This example only requires the specification of a residual routine; automatic
# differentiation is used to generate the jacobian. See the equivalent 1D version
# as well.
# This example also shows how to employ ghost points to set lateral boundary conditions

using PETSc, MPI
using Plots
using SparseArrays, SparseDiffTools, ForwardDiff

petsclib = PETSc.petsclibs[1];
PETSc.initialize(petsclib)

function FormRes!(ptr_fx_g, ptr_x_g, user_ctx)

    # Note that in PETSc, cx and cfx are pointers to global vectors.

    # Copy global to local vectors
    PETSc.update!(user_ctx.x_l, ptr_x_g,   PETSc.INSERT_VALUES)
    PETSc.update!(user_ctx.f_l, ptr_fx_g,  PETSc.INSERT_VALUES)

    # Retrieve arrays from the local vectors
    ArrayLocal_x     =   PETSc.DMStagVecGetArrayRead(user_ctx.dm,   user_ctx.x_l);      # array with all local x-data
    ArrayLocal_f     =   PETSc.DMStagVecGetArray(user_ctx.dm,       user_ctx.f_l);      # array with all local residual

    # Compute local residual
    ComputeLocalResidual(user_ctx.dm, ArrayLocal_x, ArrayLocal_f, user_ctx)

    # Finalize local arrays
    Base.finalize(ArrayLocal_x)
    Base.finalize(ArrayLocal_f)

    # Copy local into global residual vector
    PETSc.update!(ptr_fx_g, user_ctx.f_l, PETSc.INSERT_VALUES)
    #PETSc.DMLocalToGlobal(user_ctx.dm,user_ctx.f_l, PETSc.INSERT_VALUES, cfx_g)

end

function  ForwardDiff_res(x, user_ctx)
    f   = zero(x)               # vector of zeros, of same type as x (local vector)

    ArrayLocal_x     =   PETSc.DMStagVecGetArray(user_ctx.dm, x);        # array with all local x-data
    ArrayLocal_f     =   PETSc.DMStagVecGetArray(user_ctx.dm, f);        # array with all local residual

    ComputeLocalResidual(user_ctx.dm, ArrayLocal_x, ArrayLocal_f, user_ctx);

    # As the residual vector f is linked with ArrayLocal_f, we don't need to
    # pass ArrayLocal_f back to f

    return f;
end

function  func(out, x, user_ctx)

    ArrayLocal_x     =   PETSc.DMStagVecGetArray(user_ctx.dm, x);        # array with all local x-data
    ArrayLocal_f     =   PETSc.DMStagVecGetArray(user_ctx.dm, out);        # array with all local residual

    ComputeLocalResidual(user_ctx.dm, ArrayLocal_x, ArrayLocal_f, user_ctx);

    return nothing
end

function FormJacobian!(ptr_x_g, J, P, user_ctx)
    # This requires several steps:
    #
    #   1) Extract local vector from global solution (x) vector
    #   2) Compute local jacobian from the residual routine (note that
    #       this routine requires julia vectors as input)

    # Extract the local vector
    PETSc.update!(user_ctx.x_l, ptr_x_g, PETSc.INSERT_VALUES)
    x               =   PETSc.unsafe_localarray(Float64, user_ctx.x_l.ptr;  write=false, read=true)

    if isnothing(user_ctx.jac)
        # Compute sparsity pattern of jacobian. This is relatvely slow, but only has to be done once.
        # Theoretically, more efficient tools for this exists (jacobian_sparsity in the SparsityDetection.jl package),
        # but they don't seem to work with the PETSc approach we use. Therefore we employ
        f_Residual  =   (x -> ForwardDiff_res(x, user_ctx));        # pass additional arguments into the routine
        J_julia     =   ForwardDiff.jacobian(f_Residual,x);

        # employ sparse structure to compute jacobian - to be moved inside routines
        jac         =   sparse(J_julia);
        colors      =   matrix_colors(jac)          # the number of nonzeros per row

    else
        jac     =   user_ctx.jac;
        colors  =   user_ctx.colors;
    end
    out         =   similar(x);

    f_Res           =   ((out,x)->func(out, x, user_ctx));        # pass additional arguments into the routine
    forwarddiff_color_jacobian!(jac, f_Res, x, colorvec = colors)

    ind             =   PETSc.LocalInGlobalIndices(user_ctx.dm);    # extract indices
    if PETSc.assembled(J) == false
        println("Assembling J")
        J           =   PETSc.MatSeqAIJ(sparse(jac[ind,ind]));
    else
        J           .=   sparse(jac[ind,ind]);
    end

    user_ctx.jac    =   jac;
    user_ctx.colors =   colors;

   return jac, ind
end

# Define a struct that holds data we need in the residual SNES routines below
mutable struct Data_PorWav2D
    dm
    x_l
    xold_l
    xold_g
    f_l
    dt
    dx
    dz
    De
    jac
    colors
    jac_cache
end
user_ctx = Data_PorWav2D(nothing, nothing, nothing, nothing, nothing, nothing, nothing, nothing, nothing, nothing, nothing, nothing);  # holds data we need in the local

function ComputeSparsityPatternJacobian(x_l_vec, user_ctx)
    # This computes the sparsity pattern of our jacobian by hand.
    # That is signficantly faster than the automatic method, yet you will have to analyze your residual routine for it.
    #
    # As you will see, there are however any similaries between the two routines, so it is usually not all that difficult
    # to define this
    x_l               =   PETSc.unsafe_localarray(Float64, x_l_vec.ptr;  write=false, read=true)

    AddInd!(i, array) =  append!(i,vec(array));   # small helper routine for readability of the lines below

    n               =   length(x_l);
    ind_x           =   Vector(1:n);
    ind_f           =   Vector(1:n);
    ArrayLocal_x    =   PETSc.DMStagVecGetArray(user_ctx.dm, ind_x);        # array with all local x-data
    ArrayLocal_f    =   PETSc.DMStagVecGetArray(user_ctx.dm, ind_f);        # array with all local residual

    Phi             =   PETSc.DMStagGetGhostArrayLocationSlot(user_ctx.dm,ArrayLocal_x   ,   PETSc.DMSTAG_ELEMENT,   0);
    Pe              =   PETSc.DMStagGetGhostArrayLocationSlot(user_ctx.dm,ArrayLocal_x   ,   PETSc.DMSTAG_ELEMENT,   1);
    res_Phi         =   PETSc.DMStagGetGhostArrayLocationSlot(user_ctx.dm,ArrayLocal_f,      PETSc.DMSTAG_ELEMENT,   0);
    res_Pe          =   PETSc.DMStagGetGhostArrayLocationSlot(user_ctx.dm,ArrayLocal_f,      PETSc.DMSTAG_ELEMENT,   1);

    # compute the FD stencil
    ind             =   PETSc.DMStagGetIndices(user_ctx.dm);          # indices of (center/element) points, not including ghost values.

    # Porosity residual @ center points
    ix                  =   ind.center[1];
    iz                  =   ind.center[2];

    #res_Phi[ix,iz]      =   (Phi[ix,iz] - Phi_old[ix,iz])/dt + De.*(Pe[ix,iz]-Pe_old[ix,iz])/dt + (Phi[ix,iz].^m) .* Pe[ix,iz]
    i,j                 =   Vector{Int64}(), Vector{Int64}()
    AddInd!(i,res_Phi[ix,iz]), AddInd!(j,Phi[ix,iz]);   #   (Phi[ix,iz])/dt term
    AddInd!(i,res_Phi[ix,iz]), AddInd!(j,Pe[ix,iz]);    #   new non-zeros because of (Phi[ix,iz].^m) .* Pe[ix,iz] term

    # Pressure update @ nodal points
    ix                  =   ind.center[1];                # lateral BC's are set using ghost points
    iz                  =   ind.center[2];            # constant Pe on top and bottom, so this is only for center points
    ix_c = ix[1:end];
    iz_c = iz[2:end-1];

    Pe[ix[1]-1,:]       =   Pe[ix[1],:];                # ghost points on left and right size (here: flux free)
    Pe[ix[end]+1,:]     =   Pe[ix[end],:];

    # BC's
    AddInd!(i, res_Pe[:,iz[1]]),      AddInd!(j,Pe[:,iz[1]]);       # bottom BC
    AddInd!(i, res_Pe[:,iz[end]]),    AddInd!(j,Pe[:,iz[end]]);     # top BC

#    res_Pe[ix,iz]       =   De*( Pe[ix,iz] - Pe_old[ix,iz])/dt +
#    (Phi[ix,iz].^m).* Pe[ix,iz]     -
#    ((   ((0.5.*(Phi[ix,iz .+ 1] + Phi[ix    ,iz ])).^n) .* ( (Pe[ix,iz .+ 1] - Pe[ix, iz     ])/dz .+ 1.0)  -
#         ((0.5.*(Phi[ix,iz .- 1] + Phi[ix    ,iz ])).^n) .* ( (Pe[ix,iz     ] - Pe[ix, iz .- 1])/dz .+ 1.0))/dz)
# -  ((   ((0.5.*(Phi[ix,iz     ] + Phi[ix.+ 1,iz ])).^n) .* ( (Pe[ix.+ 1,iz ] - Pe[ix, iz     ])/dx       )  -
#         ((0.5.*(Phi[ix,iz     ] + Phi[ix.- 1,iz ])).^n) .* ( (Pe[ix    ,iz ] - Pe[ix.- 1,iz  ])/dx       ))/dx)
    AddInd!(i, res_Pe[ix_c,iz_c]),  AddInd!(j,Phi[ix_c,iz_c]);          # new part of (Phi[ix,iz].^m).* Pe[ix,iz]
    AddInd!(i, res_Pe[ix_c,iz_c]),  AddInd!(j,Phi[ix_c,iz_c .+ 1 ]);    # Phi[ix,iz .+ 1]
    AddInd!(i, res_Pe[ix_c,iz_c]),  AddInd!(j,Phi[ix_c,iz_c .- 1 ]);    #
    AddInd!(i, res_Pe[ix_c,iz_c]),  AddInd!(j,Phi[ix_c .- 1, iz_c]);    #
    AddInd!(i, res_Pe[ix_c,iz_c]),  AddInd!(j,Phi[ix_c .+ 1, iz_c]);    #
    AddInd!(i, res_Pe[ix_c,iz_c]),  AddInd!(j, Pe[ix_c,iz_c]      );    # De*( Pe[ix,iz] - Pe_old[ix,iz])/dt
    AddInd!(i, res_Pe[ix_c,iz_c]),  AddInd!(j, Pe[ix_c .+ 1, iz_c]);    # Pe[ix.+ 1,iz ]
    AddInd!(i, res_Pe[ix_c,iz_c]),  AddInd!(j, Pe[ix_c .- 1, iz_c]);    # Pe[ix.- 1,iz ]
    AddInd!(i, res_Pe[ix_c,iz_c]),  AddInd!(j, Pe[ix_c, iz_c .- 1]);    # Pe[ix ,iz - 1]
    AddInd!(i, res_Pe[ix_c,iz_c]),  AddInd!(j, Pe[ix_c, iz_c .+ 1]);    # Pe[ix ,iz + 1]

    jac                 =   sparse(i,j,ones(Float64,length(i[:])),n,n); # create the sparse jacobian
    colors              =   matrix_colors(jac)                          # the number of nonzeros per row
    return jac, colors
end

function ComputeSparsityPatternJacobian_automatic(x_l, user_ctx)
    # This computes the sparsity pattern and coloring of the jacobian automatically
    # This will works for any equation but is slow @ high resolutions

    f_Residual  =   (x -> ForwardDiff_res(x, user_ctx));        # pass additional arguments into the routine
    J_julia     =   ForwardDiff.jacobian(f_Residual,x_l*0 .+ 1);

    # employ sparse structure to compute jacobian - to be moved inside routines
    jac         =   sparse(J_julia);
    colors      =   matrix_colors(jac)          # the number of nonzeros per row

    return jac, colors
end

function ComputeLocalResidual(dm, ArrayLocal_x, ArrayLocal_f, user_ctx)
    # Compute the local residual. The vectors include ghost points
    n           =   3.0
    m           =   2.0
    dt          =   user_ctx.dt;
    dz          =   user_ctx.dz;
    dx          =   user_ctx.dx;
    De          =   user_ctx.De;

    Phi         =   PETSc.DMStagGetGhostArrayLocationSlot(dm,ArrayLocal_x   ,   PETSc.DMSTAG_ELEMENT,   0);
    Phi_old     =   PETSc.DMStagGetGhostArrayLocationSlot(dm,user_ctx.xold_l,   PETSc.DMSTAG_ELEMENT,   0);
    Pe          =   PETSc.DMStagGetGhostArrayLocationSlot(dm,ArrayLocal_x   ,   PETSc.DMSTAG_ELEMENT,   1);
    Pe_old      =   PETSc.DMStagGetGhostArrayLocationSlot(dm,user_ctx.xold_l,   PETSc.DMSTAG_ELEMENT,   1);

    res_Phi     =   PETSc.DMStagGetGhostArrayLocationSlot(dm,ArrayLocal_f,      PETSc.DMSTAG_ELEMENT,   0);
    res_Pe      =   PETSc.DMStagGetGhostArrayLocationSlot(dm,ArrayLocal_f,      PETSc.DMSTAG_ELEMENT,   1);

    # compute the FD stencil
    ind                 =   PETSc.DMStagGetIndices(dm);          # indices of (center/element) points, not including ghost values.

    # Porosity residual @ center points
    ix                  =   ind.center[1];
    iz                  =   ind.center[2];
    ix_c                =   ix[1:end];
    iz_c                =   iz[1:end];
    res_Phi[ix_c,iz_c]  =   (Phi[ix_c,iz_c] - Phi_old[ix_c,iz_c])/dt + De.*(Pe[ix_c,iz_c]-Pe_old[ix_c,iz_c])/dt + (Phi[ix_c,iz_c].^m) .* Pe[ix_c,iz_c]

    # Pressure update @ center points
    ix                  =   ind.center[1];              # lateral BC's are set using ghost points
    iz                  =   ind.center[2];              # constant Pe on top and bottom, so this is only for center points
    ix_c                =   ix[1:end];
    iz_c                =   iz[2:end-1];
    Pe[ix[1]-1,:]       =   Pe[ix[1],:];                # ghost points on left and right size (here: flux free)
    Pe[ix[end]+1 ,:]    =   Pe[ix[end],:];
    res_Pe[:,iz[1]]     =   Pe[:,iz[1]]   .- 0.;        # bottom BC
    res_Pe[:,iz[end]]   =   Pe[:,iz[end]] .- 0.;        # top BC

    res_Pe[ix_c,iz_c]   =   De*( Pe[ix_c,iz_c] - Pe_old[ix_c,iz_c])/dt +
                                  (Phi[ix_c,iz_c].^m).* Pe[ix_c,iz_c]     -
                                ((   ((0.5.*(Phi[ix_c,iz_c .+ 1] + Phi[ix_c    ,iz_c ])).^n) .* ( (Pe[ix_c,iz_c .+ 1] - Pe[ix_c, iz_c     ])/dz .+ 1.0)  -
                                     ((0.5.*(Phi[ix_c,iz_c .- 1] + Phi[ix_c    ,iz_c ])).^n) .* ( (Pe[ix_c,iz_c     ] - Pe[ix_c, iz_c .- 1])/dz .+ 1.0))/dz); #  -
                                ((   ((0.5.*(Phi[ix_c,iz_c     ] + Phi[ix_c.+ 1,iz_c ])).^n) .* ( (Pe[ix_c.+ 1,iz_c ] - Pe[ix_c, iz_c     ])/dx       )  -
                                     ((0.5.*(Phi[ix_c,iz_c     ] + Phi[ix_c.- 1,iz_c ])).^n) .* ( (Pe[ix_c    ,iz_c ] - Pe[ix_c.- 1,iz_c  ])/dx       ))/dx)

    # remark to the lines above: these are long, multi line, expressions. For code readability, I chopped them up over several lines.
    # In that case you MUST put the +/- sign at the end of every line. If not, some lines will not be evaluated but the code will still run, which is tricky to spot

    # Cleanup
    Base.finalize(Phi);  Base.finalize(Phi_old);
    Base.finalize(Pe);   Base.finalize(Pe_old);

end

function SetInitialPerturbations(user_ctx, x_g)
    # Computes the initial perturbations as in the paper

    # Retrieve coordinates from DMStag
    DMcoord     =   PETSc.getcoordinateDM(user_ctx.dm)
    vec_coord   =   PETSc.getcoordinateslocal(user_ctx.dm);
    Coord       =   PETSc.DMStagVecGetArray(DMcoord, vec_coord);
    X_cen       =   Coord[:,:,1];
    Z_cen       =   Coord[:,:,2];

    user_ctx.dz =   Z_cen[2,2]-Z_cen[2,1];
    user_ctx.dx =   X_cen[1,2]-X_cen[2,2];

    Phi         =   PETSc.DMStagGetGhostArrayLocationSlot(user_ctx.dm,user_ctx.x_l,     PETSc.DMSTAG_ELEMENT, 0); # center
    Pe          =   PETSc.DMStagGetGhostArrayLocationSlot(user_ctx.dm,user_ctx.x_l,     PETSc.DMSTAG_ELEMENT, 1); # center

    Phi_old     =   PETSc.DMStagGetGhostArrayLocationSlot(user_ctx.dm,user_ctx.xold_l,  PETSc.DMSTAG_ELEMENT, 0);
    Pe_old      =   PETSc.DMStagGetGhostArrayLocationSlot(user_ctx.dm,user_ctx.xold_l,  PETSc.DMSTAG_ELEMENT, 1);

    Phi0 =1.0; dPhi1=8.0; dPhi2=1.0; z1=0.0; z2=40.0; x1=0.0; x2=0.0; lambda=1.0
    dPe1        =   dPhi1/user_ctx.De;
    dPe2        =   dPhi2/user_ctx.De;

    Phi        .=   Phi0 .+ dPhi1.*exp.( -((Z_cen .- z1).^2.0)/lambda^2) +  dPhi2.*exp.( -((Z_cen .- z2).^2.0)/lambda^2);
    Pe         .=          -dPe1 .*exp.( -((Z_cen .- z1).^2.0)/lambda^2) -   dPe2.*exp.( -((Z_cen .- z2).^2.0)/lambda^2);

    Phi_old    .=   Phi0 .+ dPhi1.*exp.( -((Z_cen .- z1).^2.0)/lambda^2) +  dPhi2.*exp.( -((Z_cen .- z2).^2.0)/lambda^2);
    Pe_old     .=          -dPe1 .*exp.( -((Z_cen .- z1).^2.0)/lambda^2) -   dPe2.*exp.( -((Z_cen .- z2).^2.0)/lambda^2);

    # Copy local into global residual vector
    PETSc.update!(x_g, user_ctx.x_l, PETSc.INSERT_VALUES);

    # send back coordinates (mainly for plotting)
    return X_cen, Z_cen
end

# Main solver
nx, nz          =   100, 500;
#nx, nz          =   6, 5;
W, L            =   100,150;

#user_ctx.De     =   1e-1;        # Deborah number
#user_ctx.dt     =   1e-5;       # Note that the timestep has to be tuned a bit depending on the

user_ctx.De     =   1e2;
user_ctx.dt     =   1e-2;        # Note that the timestep has to be tuned a bit depending on De in order to obtain convergence

# Both Pe and Phi are @ defined centers
user_ctx.dm =   PETSc.DMStagCreate2d(petsclib,MPI.COMM_WORLD,
    PETSc.DM_BOUNDARY_GHOSTED,
    PETSc.DM_BOUNDARY_NONE,
    nx,nz,1,1,0,0,2,PETSc.DMSTAG_STENCIL_BOX,1)

PETSc.DMStagSetUniformCoordinatesExplicit(user_ctx.dm, -W/2, W/2, -20, L) # set coordinates
x_g             =   PETSc.createglobalvector(user_ctx.dm)

f_g             =   PETSc.createglobalvector(user_ctx.dm)
user_ctx.x_l    =   PETSc.createlocalvector(user_ctx.dm)
user_ctx.xold_l =   PETSc.createlocalvector(user_ctx.dm)
user_ctx.xold_g =   PETSc.createglobalvector(user_ctx.dm)
user_ctx.f_l    =   PETSc.createlocalvector(user_ctx.dm)
J               =   PETSc.creatematrix(user_ctx.dm);                  # Jacobian from DMStag

# initial non-zero structure of jacobian
X_cen, Z_cen    =   SetInitialPerturbations(user_ctx, x_g)

# plotting stuff initial setup
Phi         =   PETSc.DMStagGetGhostArrayLocationSlot(user_ctx.dm,user_ctx.x_l,     PETSc.DMSTAG_ELEMENT,   0);
Pe          =   PETSc.DMStagGetGhostArrayLocationSlot(user_ctx.dm,user_ctx.x_l,     PETSc.DMSTAG_ELEMENT,   1);
Phi_pl      =   Phi[2:end-1,2:end-1];
Pe_pl       =   Pe[2:end-1,2:end-1]
xc_1D,zc_1D =   X_cen[2:end-1,2], Z_cen[2,2:end-1];

#heatmap(xc_1D,zc_1D,Phi_pl', xlabel="Width", ylabel="Depth", title="Phi")
heatmap(xc_1D,zc_1D,Pe_pl', xlabel="Width", ylabel="Depth", title="Pe")

# Compute sparsity & coloring
# do this in case you solve new equations:
#@time user_ctx.jac, user_ctx.colors = ComputeSparsityPatternJacobian_automatic(user_ctx.x_l, user_ctx);

# much faster, but requires reformulation in case you solve new equations:
@time user_ctx.jac, user_ctx.colors = ComputeSparsityPatternJacobian(user_ctx.x_l, user_ctx);

ind             =   PETSc.LocalInGlobalIndices(user_ctx.dm);    # extract indices
J           =   PETSc.MatSeqAIJ(sparse(user_ctx.jac[ind,ind]));

#

S = PETSc.SNES{Float64}(petsclib,MPI.COMM_WORLD;
        snes_rtol=1e-12,
        snes_monitor=true,
        snes_max_it = 500,
        snes_monitor_true_residual=true,
        snes_converged_reason=true);
S.user_ctx  =       user_ctx;

if true

#SetInitialPerturbations(user_ctx, x_g)

PETSc.setfunction!(S, FormRes!, f_g)
PETSc.setjacobian!(S, FormJacobian!, J, J)

# Preparation of visualisation
ENV["GKSwstype"]="nul";
if isdir("viz2D_out")==true
    rm("viz2D_out", recursive=true)
end
mkdir("viz2D_out")
loadpath = "./viz2D_out/"; anim = Animation(loadpath,String[])

t    = 0.0;
it   = 1;
while t < 25.0
    global t, Z, Z_cen, it

    # Solve one (nonlinear) timestep
    PETSc.solve!(x_g, S);

    # Update old local values
    user_ctx.xold_g  =  x_g;
    PETSc.update!(user_ctx.x_l, x_g,  PETSc.INSERT_VALUES)
    PETSc.update!(user_ctx.xold_l, user_ctx.xold_g,  PETSc.INSERT_VALUES)

    # Update time
    t    += user_ctx.dt;
    it   += 1;

    if mod(it,20)==0  # Visualisation
        # Extract values and plot
        Phi         =   PETSc.DMStagGetGhostArrayLocationSlot(user_ctx.dm,user_ctx.x_l,     PETSc.DMSTAG_ELEMENT, 0);
        Pe          =   PETSc.DMStagGetGhostArrayLocationSlot(user_ctx.dm,user_ctx.x_l,     PETSc.DMSTAG_ELEMENT, 1);

        #p1 = plot(Phi[2,2:end-1], zc_1D,  ylabel="Z", xlabel="ϕ",  xlims=( 0.0, 2.0), label=:none, title="De=$(user_ctx.De)");
        #p2 = plot(Pe[2,2:end-1],  zc_1D ,                       xlabel="Pe", xlims=(-0.1, 0.1), label=:none, title="$(round(time;sigdigits=3))");
        p1 = plot(Phi[2,2:end-1], zc_1D,  ylabel="Z", xlabel="ϕ", label=:none, title="De=$(user_ctx.De)");
        p2 = plot(Pe[2,2:end-1],  zc_1D ,                       xlabel="Pe",  label=:none, title="$(round(t;sigdigits=3))");

        #p1 = heatmap(xc_1D,zc_1D,Phi_pl', xlabel="Width", ylabel="Depth", title="Phi")
        #p2 = heatmap(xc_1D,zc_1D,Pe_pl', xlabel="Width", ylabel="Depth", title="Pe")

        plot(p1, p2, layout=(1,2)); frame(anim)

        Base.finalize(Pe);  Base.finalize(Phi)
    end

    println("Timestep $it, time=$t")

end

gif(anim, "Example_2D.gif", fps = 15)   # create a gif animation
end


text:
using Catalyst
using Test
using DelaySSAToolkit
rn = @reaction_network begin
    ρ, S+I --> E+I
    1/I, I --> R
    r, I --> R
end ρ r

m = get_reaction_idx(rn)
eqs = equations(rn)
@test m[:,2] == [1,3,2]
@test m[:,1] == eqs[[1,3,2]]


text:
include("plots.jl")
include("options.jl")
include("render.jl")


text:
struct OAuth1a <: AbstractOAuth
    consumer_key::String
    consumer_secret::String
    token::String
    token_secret::String
end

oauth_version_string(::OAuth1a) = "1.0"

"""
    oauth_signing_key(oauth_consumer_secret::String, oauth_token_secret::String)

Returns a signing key based on a consumer secret and token secret.

# Examples
```jldoctest
julia> oauth_signing_key("foo", "bar")
"foo&bar"
```
"""
function oauth_signing_key(oauth::OAuth1a)
    "$(oauth.consumer_secret)&$(oauth.token_secret)"
end

"""
    function oauth_header(httpmethod, baseurl, options, oauth_consumer_key, oauth_consumer_secret, oauth_token, oauth_token_secret; oauth_signature_method = "HMAC-SHA1", oauth_version = "1.0")

Builds OAuth header, defaulting to OAuth 1.0. Function assumes `options` has already
been run through `encodeURI!`.

"""
function oauth_header(oauth::OAuth1a, httpmethod, baseurl, options; oauth_signature_method = "HMAC-SHA1")
    oauth_consumer_key = oauth.consumer_key
    oauth_consumer_secret = oauth.consumer_secret
    oauth_token = oauth.token
    oauth_token_secret = oauth.token_secret

    #keys for parameter string
    options["oauth_consumer_key"] = oauth_consumer_key
    options["oauth_nonce"] = oauth_nonce(32)
    options["oauth_signature_method"] = oauth_signature_method
    options["oauth_timestamp"] = oauth_timestamp()
    options["oauth_token"] = oauth_token
    options["oauth_version"] = oauth_version(oauth)

    #options encoded
    oauth_percent_encode_keys!(options)

    #Create ordered query string
    parameterstring = oauth_serialize_url_parameters(options)

    #Calculate signature_base_string
    signature_base_string = oauth_signature_base_string(uppercase(httpmethod), baseurl, parameterstring)

    #Calculate oauth_signature
    oauth_sig = oauth_signature(signature_base_string, oauth, oauth_signature_method)

    return "OAuth oauth_consumer_key=\"$(options["oauth_consumer_key"])\", oauth_nonce=\"$(options["oauth_nonce"])\", oauth_signature=\"$(oauth_sig)\", oauth_signature_method=\"$(options["oauth_signature_method"])\", oauth_timestamp=\"$(options["oauth_timestamp"])\", oauth_token=\"$(options["oauth_token"])\", oauth_version=\"$(options["oauth_version"])\""

end


text:
_padded_tuple(v::AbstractArray{T, N}, n::NTuple{N, Int}) where {T, N} = n
_padded_tuple(v::AbstractArray{T, N}, n::Int) where {T, N} = _padded_tuple(v, (n,))
_padded_tuple(v::AbstractArray{T, N}, n) where {T, N} = Tuple(i <= length(n) ? n[i] : 0 for i in 1:N)

"""
    ShiftedArray(parent::AbstractArray, shifts, default)

Custom `AbstractArray` object to store an `AbstractArray` `parent` shifted by `shifts` steps (where `shifts` is
a `Tuple` with one `shift` value per dimension of `parent`).
For `s::ShiftedArray`, `s[i...] == s.parent[map(-, i, s.shifts)...]` if `map(-, i, s.shifts)`
is a valid index for `s.parent`, and `s.v[i, ...] == default` otherwise.
Use `copy` to collect the values of a `ShiftedArray` into a normal `Array`.
The recommended constructor is `ShiftedArray(parent, shifts; default = missing)`

# Examples

```jldoctest shiftedarray
julia> v = [1, 3, 5, 4];

julia> s = ShiftedArray(v, (1,))
4-element ShiftedArrays.ShiftedArray{Int64,Missing,1,Array{Int64,1}}:
  missing
 1
 3
 5

julia> v = [1, 3, 5, 4];

julia> s = ShiftedArray(v, (1,))
4-element ShiftedArrays.ShiftedArray{Int64,Missing,1,Array{Int64,1}}:
  missing
 1
 3
 5

julia> copy(s)
4-element Array{Union{Int64, Missing},1}:
  missing
 1
 3
 5

julia> v = reshape(1:16, 4, 4);

julia> s = ShiftedArray(v, (0, 2))
4×4 ShiftedArrays.ShiftedArray{Int64,Missing,2,Base.ReshapedArray{Int64,2,UnitRange{Int64},Tuple{}}}:
 missing  missing  1  5
 missing  missing  2  6
 missing  missing  3  7
 missing  missing  4  8

julia> shifts(s)
(0, 2)
```
"""
struct ShiftedArray{T, M, N, S<:AbstractArray} <: AbstractArray{Union{T, M}, N}
    parent::S
    shifts::NTuple{N, Int}
    default::M
end

ShiftedArray(v::AbstractArray{T, N}, n = Tuple(0 for i in 1:N); default::M = missing) where {T, N, M} =
     ShiftedArray{T, M, N, typeof(v)}(v, _padded_tuple(v, n), default)

"""
    ShiftedVector{T, S<:AbstractArray}

Shorthand for `ShiftedArray{T, 1, S}`.
"""
const ShiftedVector{T, M, S<:AbstractArray} = ShiftedArray{T, M, 1, S}

ShiftedVector(v::AbstractVector, n = (0,); default = missing) = ShiftedArray(v, n; default = default)

size(s::ShiftedArray) = size(parent(s))

@inline function getindex(s::ShiftedArray{<:Any, <:Any, N}, x::Vararg{Int, N}) where {N}
    i = OffsetArrays.offset(shifts(s), x)
    v = parent(s)
    if checkbounds(Bool, v, i...)
        @inbounds ret = v[i...]
    else
        ret = default(s)
    end
    ret
end

parent(s::ShiftedArray) = s.parent

"""
    shifts(s::ShiftedArray)

Returns amount by which `s` is shifted compared to `parent(s)`.
"""
shifts(s::ShiftedArray) = s.shifts

"""
    default(s::ShiftedArray)

Returns default value.
"""
default(s::ShiftedArray) = s.default

checkbounds(::ShiftedArray, I...) = nothing
checkbounds(::Type{Bool}, ::ShiftedArray, I...) = true


text:
include("ClassMacro.jl")
include("AutodestroyMacro.jl")

export destroy

function destroy(objList::Vector{T}) where T
    for obj in objList
        destroy(obj)
    end
end

function destroy!(objList::Vector{T}) where T
    for obj in objList
        destroy(obj)
    end
    empty!(objList)
end

function Base.getproperty(objList::Vector{T}, sym::Symbol) where T
    if sym == :destroy
        return () -> begin
            destroy(objList)
        end
    end
end


text:
using StaticArrays

const inv_2 = 1.0 / 2.0
const inv_3 = 1.0 / 3.0
const inv_4 = 1.0 / 4.0
const inv_6 = 1.0 / 6.0
const inv_8 = 1.0 / 8.0
const inv_12 = 1.0 / 12.0
const inv_18 = 1.0 / 18.0
const inv_20 = 1.0 / 20.0
const inv_24 = 1.0 / 24.0
const inv_30 = 1.0 / 30.0
const inv_36 = 1.0 / 36.0
const inv_48 = 1.0 / 48.0
const inv_72 = 1.0 / 72.0
const inv_120 = 1.0 / 120.0
const inv_144 = 1.0 / 144.0
const inv_720 = 1.0 / 720.0

"""
    SplinePP( degree, ncells)

- `degree` : degree of 1d spline
- `poly_coeffs` : `poly_coeffs[i,j]` coefficient of ``x^{deg+1-j}`` for ith B-spline function  size= (degree+1, degree+1)
- `poly_coeffs_fp` : `poly_coeffs[i,j]` coefficient of ``x^{deg+1-j}`` for ith B-spline function  size= (degree+1, degree+1)
- `ncells` : number of gridcells
- `scratch_b` : scratch data for `b_to_pp-converting`
- `scratch_p` : scratch data for `b_to_pp-converting`
"""
struct SplinePP{N}
    degree::Int
    poly_coeffs::SMatrix{N,N,Float64}
    poly_coeffs_fp::SMatrix{N,N,Float64}
    ncells::Int

    function SplinePP(degree, ncells)
        @assert (ncells >= degree)

        if degree == 1
            poly_coeffs = SMatrix{2,2}(-1.0, 1.0, 1.0, 0.0)
            poly_coeffs_fp = SMatrix{2,2}(-inv_2, 1.0, inv_2, 0.0)

        elseif degree == 2
            poly_coeffs = SMatrix{3,3}(
                inv_2, -1.0, inv_2, -1.0, 1.0, inv_2, inv_2, 0.0, 0.0
            )
            poly_coeffs_fp = SMatrix{3,3}(
                inv_6, -inv_2, inv_2, -inv_3, inv_2, inv_2, inv_6, 0.0, 0
            )

        elseif degree == 3
            poly_coeffs = SMatrix{4,4}(
                -inv_6,
                inv_2,
                -inv_2,
                inv_6,
                inv_2,
                -1.0,
                0.0,
                4 * inv_6,
                -inv_2,
                inv_2,
                inv_2,
                inv_6,
                inv_6,
                0.0,
                0.0,
                0.0,
            )

            poly_coeffs_fp = SMatrix{4,4}(
                -inv_24,
                inv_6,
                -inv_4,
                inv_6,
                inv_8,
                -inv_3,
                0.0,
                4 * inv_6,
                -inv_8,
                inv_6,
                inv_4,
                inv_6,
                inv_24,
                0.0,
                0.0,
                0.0,
            )

        elseif degree == 4
            poly_coeffs = SMatrix{5,5}(
                inv_24,
                -inv_6,
                inv_4,
                -inv_6,
                inv_24,
                -inv_6,
                inv_2,
                -inv_4,
                -inv_2,
                11 * inv_24,
                inv_4,
                -inv_2,
                -inv_4,
                inv_2,
                11 * inv_24,
                -inv_6,
                inv_6,
                inv_4,
                inv_6,
                inv_24,
                inv_24,
                0.0,
                0.0,
                0.0,
                0.0,
            )

            poly_coeffs_fp = SMatrix{5,5}(
                inv_120,
                -inv_24,
                inv_12,
                -inv_12,
                inv_24,
                -inv_30,
                inv_8,
                -inv_12,
                -inv_2,
                11 * inv_24,
                inv_20,
                -inv_8,
                -inv_12,
                inv_4,
                11 * inv_24,
                -inv_30,
                inv_24,
                inv_12,
                inv_12,
                inv_24,
                inv_120,
                0.0,
                0.0,
                0.0,
                0.0,
            )

        elseif degree == 5
            poly_coeffs = SMatrix{6,6}(
                -inv_120,
                inv_24,
                -inv_12,
                inv_12,
                -inv_24,
                inv_120,
                inv_24,
                -inv_6,
                inv_6,
                inv_6,
                -5 * inv_12,
                26 * inv_120,
                -inv_12,
                inv_4,
                0.0,
                -inv_2,
                0.0,
                11 * inv_20,
                inv_12,
                -inv_6,
                -inv_6,
                inv_6,
                5 * inv_12,
                26 * inv_120,
                -inv_24,
                inv_24,
                inv_12,
                inv_12,
                inv_24,
                inv_120,
                inv_120,
                0.0,
                0.0,
                0.0,
                0.0,
                0.0,
            )

            poly_coeffs_fp = SMatrix{6,6}(
                -inv_720,
                inv_120,
                -inv_48,
                inv_36,
                -inv_48,
                inv_120,
                inv_144,
                -inv_30,
                inv_24,
                inv_18,
                -5 * inv_24,
                26 * inv_120,
                -inv_72,
                inv_20,
                0.0,
                -inv_6,
                0.0,
                11 * inv_20,
                inv_72,
                -inv_30,
                -inv_24,
                inv_18,
                5 * inv_24,
                26 * inv_120,
                -inv_144,
                inv_120,
                inv_48,
                inv_36,
                inv_48,
                inv_120,
                inv_720,
                0.0,
                0.0,
                0.0,
                0.0,
                0.0,
            )
        else
            throw(ArgumentError(" degree $degree not implemented"))
        end

        N = degree + 1

        return new{N}(degree, poly_coeffs, poly_coeffs_fp, ncells)
    end
end

"""
    b_to_pp( SplinePP, ncells, b_coeffs)

Convert 1d spline in B form to spline in pp form with
periodic boundary conditions
"""
function b_to_pp(self::SplinePP, ncells::Int, b_coeffs::Vector{Float64})
    dp1 = self.degree + 1
    pp_coeffs = zeros(Float64, (dp1, ncells))
    coeffs = zeros(Float64, dp1)

    @inbounds for i in 1:(self.degree)
        coeffs .= vcat(b_coeffs[(end - self.degree + i):end], b_coeffs[1:i])
        for j in 1:dp1
            pp_coeffs[j, i] = sum(coeffs .* self.poly_coeffs[j, :])
        end
    end

    @inbounds for i in (self.degree + 1):ncells
        coeffs .= b_coeffs[(i - self.degree):i]
        for j in 1:dp1
            pp_coeffs[j, i] = sum(coeffs .* self.poly_coeffs[j, :])
        end
    end

    return pp_coeffs
end

"""
    horner_1d(degree, pp_coeffs, x, index)

Perform a 1d Horner schema on the `pp_coeffs` at index
"""
function horner_1d(degree::Int, pp_coeffs, x::Float64, index::Int)
    res = pp_coeffs[1, index]
    @inbounds for i in 1:degree
        res = res * x + pp_coeffs[i + 1, index]
    end
    return res
end

"""
    horner_primitive_1d(val, degree, pp_coeffs, x)

Perform a 1d Horner schema on the `pp_coeffs` evaluate at x
"""
function horner_primitive_1d(val::Vector{Float64}, degree, pp_coeffs, x)
    @inbounds for i in eachindex(val)
        val[i] = horner_1d(degree, pp_coeffs, x, i) * x
    end
end

"""
    horner_m_2d!(val, spl1, spl2, degree, x)

Perform two times a 1d hornerschema on the poly_coeffs
- val : array of values
- degree : degree of the spline
- x : point at which we evaluate our spline
"""
function horner_m_2d!(val, spl1::SplinePP, spl2::SplinePP, degree, x, y)
    for i in 1:(degree + 1)
        val[i, 1] = horner_1d(degree, spl1.poly_coeffs, x, i)
        val[i, 2] = horner_1d(degree, spl2.poly_coeffs, y, i)
    end
end

"""
    b_to_pp_1d_cell( self, b_coeffs, pp_coeffs )

Convert 1d spline in B form in a cell to spline in pp form with periodic boundary conditions
- spline : arbitrary degree 1d spline
- b_coeffs(self%degree+1) : coefficients of spline in B-form
- pp_coeffs(self%degree+1) : coefficients of spline in pp-form
"""
function b_to_pp_1d_cell!(pp, spline, b)
    dp1 = spline.degree + 1
    fill!(pp, 0.0)
    for i in 1:dp1, j in 1:dp1
        pp[j] += b[i] * spline.poly_coeffs[j, i]
    end
end

"""
    b_to_pp_2d_cell(spline1, spline2, b_coeffs, pp_coeffs, i, j)

Convert 2d spline in B form in a cell to spline in pp form with periodic boundary conditions

- spline1 : arbitrary degree 1d spline
- spline2 : arbitrary degree 1d spline
- n_cells(2) : number of gridcells
- b_coeffs(n_cells(1)*n_cells(2)) : coefficients of spline in B-form
- pp_coeffs((spline1.degree+1)*(spline2.degree+1),n_cells(1)*n_cells(2)) : coefficients of spline in pp-form
"""
function b_to_pp_2d_cell!(pp, spline1::SplinePP, spline2::SplinePP, b, i, j)
    n1 = spline1.ncells
    n2 = spline2.ncells
    d1 = spline1.degree
    d2 = spline2.degree
    dp1 = d1 + 1
    dp2 = d2 + 1

    b1 = zeros(dp1)
    b2 = zeros(dp2)
    p2 = zeros(dp2)
    # convert b-coefficients in pp-coefficients in first dimension
    if i > d1
        if j > d2
            for l in 0:d2
                k = i + (j - dp2 + l) * n1
                b1 .= b[(k - d1):k]
                b_to_pp_1d_cell!(
                    view(pp, (1 + l * dp1):(dp1 * (l + 1)), i + n1 * (j - 1)), spline1, b1
                )
            end
        else
            # use of modulo for boundary cells in second dimension
            for l in 0:d2
                k = i + mod(j - dp2 + l, n2) * n1
                b1 .= b[(k - d1):k]
                b_to_pp_1d_cell!(
                    view(pp, (1 + l * dp1):(dp1 * (l + 1)), i + n1 * (j - 1)), spline1, b1
                )
            end
        end
    else
        # use of modulo for boundary cells in both dimensions
        for l in 0:d2
            for k in 0:d1
                b1[k + 1] = b[mod(i - dp1 + k, n1) + 1 + mod(j - dp2 + l, n2) * n1]
            end
            b_to_pp_1d_cell!(
                view(pp, (1 + l * dp1):(dp1 * (l + 1)), i + n1 * (j - 1)), spline1, b1
            )
        end
    end

    # convert b-coefficients in pp_coefficients in second dimension
    for k in 1:dp1
        for l in 1:dp2
            b2[l] = pp[k + dp1 * (l - 1), i + n1 * (j - 1)]
        end
        b_to_pp_1d_cell!(p2, spline2, b2)
        for l in 1:dp2
            pp[k + dp1 * (l - 1), i + n1 * (j - 1)] = p2[l]
        end
    end
end

"""
    b_to_pp_2d!( pp, spl1, spl2, b)

Convert 2d spline in B form to spline in pp form
- n_cells(2) : number of gridcells
- b_coeffs   : coefficients of spline in B-form
- pp_coeffs  : coefficients of spline in pp-form
"""
function b_to_pp_2d!(pp, spl1::SplinePP, spl2::SplinePP, b)
    ncells1 = spl1.ncells
    ncells2 = spl2.ncells

    for j in 1:ncells2, i in 1:ncells1
        b_to_pp_2d_cell!(pp, spl1, spl2, b, i, j)
    end
end

"""
    horner_2d(degrees, pp_coeffs, position, indices, ncells)

Perform a 2d hornerschema on the pp_coeffs at the indices
- degree : degree of the spline
- pp_coeffs : coefficients of spline in pp-form
- position(2) : point at which we evaluate our spline
- indices(2) : indices of cell in which is x
- ncells(2) : number of gridcells
- res : value of the splinefunction at position
"""
function horner_2d(degrees, pp, position, indices, ncells)
    x1, x2 = position
    d1, d2 = degrees
    i1, i2 = indices
    n1, n2 = ncells

    pp_1d = zeros(d2 + 1, 1)

    for i in 0:d2
        istart = 1 + i * (d1 + 1)
        istop = (d1 + 1) * (i + 1)
        jstart = 1 + (i2 - 1) * n1
        jstop = n1 * i2
        pp_1d[i + 1, 1] = horner_1d(d1, view(pp, istart:istop, jstart:jstop), x1, i1)
    end

    res = pp_1d[1, 1]
    for i in 1:d2
        res = res * x2 + pp_1d[i + 1, 1]
    end

    return res
end


text:
@testset "Verbose" for T in [Float64]
    using ProximalOperators
    using ProximalAlgorithms
    using LinearAlgebra

    A = T[
        1.0 -2.0 3.0 -4.0 5.0
        2.0 -1.0 0.0 -1.0 3.0
        -1.0 0.0 4.0 -3.0 2.0
        -1.0 -1.0 -1.0 1.0 3.0
    ]
    b = T[1.0, 2.0, 3.0, 4.0]

    m, n = size(A)

    R = real(T)

    lam = R(0.1) * norm(A' * b, Inf)
    @test typeof(lam) == R

    f = Translate(SqrNormL2(R(1)), -b)
    f2 = LeastSquares(A, b)
    g = NormL1(lam)

    Lf = opnorm(A)^2

    x_star = T[-3.877278911564627e-01, 0, 0, 2.174149659863943e-02, 6.168435374149660e-01]

    TOL = R(1e-4)

    @testset "ForwardBackward" begin

        ## Nonfast/Nonadaptive

        x0 = zeros(T, n)
        solver = ProximalAlgorithms.ForwardBackward(tol = TOL, verbose = true)
        x, it = solver(x0, f = f2, g = g, Lf = Lf)
        @test eltype(x) == T
        @test norm(x - x_star, Inf) <= TOL
        @test it < 150

        # Nonfast/Adaptive

        x0 = zeros(T, n)
        solver = ProximalAlgorithms.ForwardBackward(
            tol = TOL,
            adaptive = true,
            verbose = true,
        )
        x, it = solver(x0, f = f2, g = g)
        @test eltype(x) == T
        @test norm(x - x_star, Inf) <= TOL
        @test it < 300

        # Fast/Nonadaptive

        x0 = zeros(T, n)
        solver =
            ProximalAlgorithms.FastForwardBackward(tol = TOL, verbose = true)
        x, it = solver(x0, f = f2, g = g, Lf = Lf)
        @test eltype(x) == T
        @test norm(x - x_star, Inf) <= TOL
        @test it < 100

        # Fast/Adaptive

        x0 = zeros(T, n)
        solver = ProximalAlgorithms.FastForwardBackward(
            tol = TOL,
            adaptive = true,
            verbose = true,
        )
        x, it = solver(x0, f = f2, g = g)
        @test eltype(x) == T
        @test norm(x - x_star, Inf) <= TOL
        @test it < 200
    end

    @testset "ZeroFPR" begin

        # ZeroFPR/Nonadaptive

        x0 = zeros(T, n)
        solver = ProximalAlgorithms.ZeroFPR(tol = TOL, verbose = true)
        x, it = solver(x0, f = f, A = A, g = g, Lf = Lf)
        @test eltype(x) == T
        @test norm(x - x_star, Inf) <= TOL
        @test it < 20

        # ZeroFPR/Adaptive

        x0 = zeros(T, n)
        solver = ProximalAlgorithms.ZeroFPR(adaptive = true, tol = TOL, verbose = true)
        x, it = solver(x0, f = f, A = A, g = g)
        @test eltype(x) == T
        @test norm(x - x_star, Inf) <= TOL
        @test it < 20

    end

    @testset "PANOC" begin

        # PANOC/Nonadaptive

        x0 = zeros(T, n)
        solver = ProximalAlgorithms.PANOC(tol = TOL, verbose = true)
        x, it = solver(x0, f = f, A = A, g = g, Lf = Lf)
        @test eltype(x) == T
        @test norm(x - x_star, Inf) <= TOL
        @test it < 20

        ## PANOC/Adaptive

        x0 = zeros(T, n)
        solver = ProximalAlgorithms.PANOC(adaptive = true, tol = TOL, verbose = true)
        x, it = solver(x0, f = f, A = A, g = g)
        @test eltype(x) == T
        @test norm(x - x_star, Inf) <= TOL
        @test it < 20

    end

    @testset "PANOCplus" begin

        # PANOCplus/Nonadaptive

        x0 = zeros(T, n)
        solver = ProximalAlgorithms.PANOCplus(tol = TOL, verbose = true)
        x, it = solver(x0, f = f, A = A, g = g, Lf = Lf)
        @test eltype(x) == T
        @test norm(x - x_star, Inf) <= TOL
        @test it < 20

        ## PANOCplus/Adaptive

        x0 = zeros(T, n)
        solver = ProximalAlgorithms.PANOCplus(adaptive = true, tol = TOL, verbose = true)
        x, it = solver(x0, f = f, A = A, g = g)
        @test eltype(x) == T
        @test norm(x - x_star, Inf) <= TOL
        @test it < 20

    end

    @testset "DouglasRachford" begin

        # Douglas-Rachford

        x0 = zeros(T, n)
        solver = ProximalAlgorithms.DouglasRachford(
            gamma = R(10) / Lf,
            tol = TOL,
            verbose = true,
        )
        y, z, it = solver(x0, f = f2, g = g)
        @test eltype(y) == T
        @test eltype(z) == T
        @test norm(y - x_star, Inf) <= TOL
        @test norm(z - x_star, Inf) <= TOL
        @test it < 30

    end

end


text:
using JuMP, Cbc, CSV

lightModel = Model(Cbc.Optimizer)

@variable(lightModel, fixtures[1:5], binary=true)
@variable(lightModel, 0 <= luminosity[1:5] <= 300)
@variable(lightModel, w, binary=true)
@variable(lightModel, z[1:5], binary=true)

@objective(lightModel, Min, 450*sum(fixtures)+2*sum(luminosity))

corridors = [1 2; 1 3; 2 3; 2 4; 3 5; 4 5]
minLuminosity = [250,300,150,200,350,180]

for y = 1:size(fixtures,1)
    @constraint(lightModel, luminosity[y] <= 300*fixtures[y])
end
for c = 1:size(corridors,1)
    @constraint(lightModel, luminosity[corridors[c,1]]+luminosity[corridors[c,2]] >= minLuminosity[c])
end
@constraint(lightModel, luminosity[1]>=300*w)
@constraint(lightModel, luminosity[4]>=300*(1-w))
@constraint(lightModel, sum(z)>=3)
for zi = 1:size(z,1)
    @constraint(lightModel, luminosity[zi]<=150+150*(1-z[zi]))
end
@constraint(lightModel, .6*luminosity[1]-.4*luminosity[3]<=0)

result = JuMP.optimize!(lightModel)

JuMP.value.(z)
JuMP.value.(fixtures)
JuMP.value.(luminosity)

max_stars_repo_name: valenmgama/optimization-tec

max_issues_repo_name: valenmgama/optimization-tec

max_forks_repo_name: valenmgama/optimization-tec


text:
# Idea is that instead of having our own Tracer functionality we would use the
# new Logging interface in Julia StdLib from 0.7. Here we test how this could work.
using Logging: AbstractLogger, with_logger, global_logger

timestamp(t = time()) = Libc.strftime("%Y-%m-%d %H:%M.%S", t)
elapsed(st, t = time()) = round(t - st; digits=4)

# Try the default logger
@info "Optimization step" time=timestamp() elapsed=elapsed(ST) best_fitness=3.4

# Let's make a CSVLogger to log to CSV file.
struct CSVLogger <: AbstractLogger
    starttime::Float64
    filename::String
    filehandle::IOStream
    columns::Vector{Symbol}
    group::Union{Nothing, AbstractString, Vector{AbstractString}}
end
function CSVLogger(filename::String, columns::Vector{Symbol},
    group::Union{Nothing, AbstractString, Vector{AbstractString}} = nothing)
    if isfile(filename)
        error("Cannot open csv log file $filename, since it already exists")
    end
    fh = open(filename, "w")
    writeheader(fh, columns)
    CSVLogger(time(), filename, fh, columns, group)
end

function writeheader(fh::IOStream, columnnames::Vector{Symbol})
    names = vcat(["Time", "Elapsed"], map(string, columnnames))
    println(fh, join(names, ","))
    flush(fh)
end

reset!(l::CSVLogger) = (l.starttime = time())

import Logging: min_enabled_level, shouldlog, handle_message
@inline min_enabled_level(l::CSVLogger) = min_enabled_level(global_logger())

function shouldlog(l::CSVLogger, level, _module, group, id)
    @show (group, typeof(group))
    true
end

@inline shouldlog_message(g::Nothing, message::AbstractString) = true # Log everything if no group set in CSVLogger.group
@inline shouldlog_message(g::AbstractString, message::AbstractString) = (g == message)
@inline shouldlog_message(g::Vector{AbstractString}, message::AbstractString) = in(message, g)

function handle_message(l::CSVLogger, level, message, _module, group, id, file, line; kwargs...)
    t = time()
    ts = timestamp(t)
    el = elapsed(l.starttime, t)
    if shouldlog_message(l.group, message)
        @show (ts, el, level, message, _module, group, id, file, line, kwargs)
        values = map(k -> kwargs[k], l.columns)
        @show values
        println(l.filehandle, ts * "," * string(el) * "," * join(values, ","))
        flush(l.filehandle)
    end
end

l = CSVLogger("t.csv", [:BestFitness]) # We will only log best_fitness (and add time and elapsed num seconds)
with_logger(l) do
    @info "Optimization step" BestFitness=3.4 Fitness=45.6 Ind="1+3"
end

with_logger(l) do
    @info "Optimization step" BestFitness=2.9714 Fitness=32.6 Ind="1+578"
end


text:
lada_dos() = numerify(executor(data["en"]["faker"]["phone_number"]["lada_dos"]))
lada_tres() = numerify(executor(data["en"]["faker"]["phone_number"]["lada_tres"]))
area_code() = numerify(executor(data["en"]["faker"]["phone_number"]["area_code"]))
exchange_code() = numerify(executor(data["en"]["faker"]["phone_number"]["exchange_code"]))

phone_number() = numerify(executor(data["en"]["faker"]["phone_number"]["formats"]))
cell_phone() = numerify(executor(data["en"]["faker"]["cell_phone"]["formats"]))


text:
export PIBTPlanner

"""
    PIBTPlanner{C}

Planner based on Priority Inheritance with BackTracking.

Priority Inheritance with Backtracking for Iterative Multi-agent Path Finding
Okumura et al, IJCAI 2019
https://www.ijcai.org/Proceedings/2019/0076.pdf
"""
@with_kw struct PIBTPlanner{C}
    logger::SolverLogger{C} = SolverLogger{C}()
    partial::Bool           = false # if true, allow partial solutions
end

export PIBTReservationTable

"""
    PIBTReservationTable
"""
struct PIBTReservationTable
    state_reservations::SparseVector{Bool,Int}
    action_reservations::SparseVector{Bool,Int}
end
function PIBTReservationTable(n_states::Int,n_actions::Int)
    PIBTReservationTable(
        sparse(zeros(Bool,n_states)),
        sparse(zeros(Bool,n_actions)),
        )
end
function PIBTReservationTable(env)
    PIBTReservationTable(num_states(env),num_actions(env))
end
function reserve_state!(table::PIBTReservationTable,env,s)
    idx,_ = serialize(env,s)
    table.state_reservations[idx] = true
    table
end
function reserve_action!(table::PIBTReservationTable,env,a)
    idx,_ = serialize(env,a)
    table.action_reservations[idx] = true
    table
end
function is_reserved(table::PIBTReservationTable,env,s,a,sp)
    s_idx,_ = serialize(env,sp)
    a_idx,_ = serialize(env,a)
    return table.state_reservations[s_idx] || table.action_reservations[a_idx]
end
function reserve!(table::PIBTReservationTable,env,s,a,sp)
    if is_reserved(table,env,s,a,sp)
        return false
    end
    reserve_state!(table,env,sp)
    reserve_action!(table,env,a)
    # table
    return true
end
function reset_reservations!(table::PIBTReservationTable)
    table.state_reservations .= false
    table.action_reservations .= false
    dropzeros!(table.state_reservations)
    dropzeros!(table.action_reservations)
    table
end

abstract type AbstractPIBTCache end

export PIBTCache

"""
    PIBTCache{S,A}

Contains info to be passed along through recursive calls to the PIBT algorithm
for multi-agent path planning.
Info to be stored:
- current state of each agent (should be lined up at the same time step)
- priority of each agent
- the planned action (and associated next state) for each agent
- the search environment for each agent, which contains e.g., the agent's goal,
    cost_model, heuristic_model, etc.
- a conflict table of sorts to indicate which states/actions are reserved
- countdown flags that identify which paths are "active". If pibt is operating
on a "ragged" plan, where some paths have been planned further into the future
than others, it needs to ensure that planning does not continue for a given path
until all of the other paths have "caught up" to it.
"""
struct PIBTCache{T,E,S,A} <: AbstractPIBTCache
    solution::T
    envs::Vector{E}
    states::Vector{S}
    actions::Vector{A}
    ordering::Vector{Int}
    undecided::Set{Int} # agent ids
    # reservation_table::Set{Int}
    # reservation_table::PIBTReservationTable
    reservation_table::ReservationTable{Float64}
    timers::Vector{Int}
    active_countdowns::Vector{Int}
end
get_envs(cache::PIBTCache) = cache.envs
get_solution(cache::PIBTCache) = cache.solution
get_states(cache::PIBTCache) = cache.states
get_actions(cache::PIBTCache) = cache.actions
get_ordering(cache::PIBTCache) = cache.ordering
get_undecided(cache::PIBTCache) = cache.undecided # agent ids
get_reservation_table(cache::PIBTCache) = cache.reservation_table
get_timers(cache::PIBTCache) = cache.timers
get_active_countdowns(cache::PIBTCache) = cache.active_countdowns
function get_current_priority(cache::PIBTCache,i)
    idx = findfirst(j->j==i,get_ordering(cache))
    @assert !isnothing(idx)
    return idx
end

get_active_agents(cache::PIBTCache) = findall(get_active_countdowns(cache) .<= 0)
get_inactive_agents(cache::PIBTCache) = findall(get_active_countdowns(cache) .> 0)

get_cost_model(cache::PIBTCache) = get_cost_model(cache.envs[1])

function reset_reservations!(cache::PIBTCache)
    # empty!(get_reservation_table(cache))
    reset_reservations!(get_reservation_table(cache))
    for i in get_inactive_agents(cache)
        env = get_envs(cache)[i]
        s = get_states(cache)[i]
        a = get_actions(cache)[i]
        sp = get_next_state(env,s,a)
        reserve!(cache,env,s,a,sp)
    end
end
is_reserved(cache::PIBTCache,args...) = is_reserved(get_reservation_table(cache),args...)
reserved_by(cache::PIBTCache,args...) = reserved_by(get_reservation_table(cache),args...)
# reserve!(cache::PIBTCache,args...) = reserve!(get_reservation_table(cache),args...)

function reserve!(cache::PIBTCache,args...)
    table = get_reservation_table(cache)
    valid = true
    for res in create_reservations(cache,args...)
        valid &= reserve!(table,res)
    end
    return valid
end
function create_reservations(cache::PIBTCache,args...)
    reservations = create_reservations(args...)[2:end]
end

"""
    Fills `undecided` with all active agents (inactive agents have already
    selected their actions)
"""
function reset_undecided!(cache::PIBTCache)
    union!(get_undecided(cache),Set(get_active_agents(cache)))
end
function set_action!(cache::PIBTCache,i,a)
    get_actions(cache)[i] = a
end
function is_active(cache::PIBTCache,i)
    return get_active_countdowns(cache)[i] <= 0
end

"""
    get_conflict_index(cache,i,s,a,sp)

Returns the index of an agent that currently occupies `sp`, or -1 if there is no
such agent.
"""
function get_conflict_index(cache::PIBTCache,i,s,a,sp)
    for (k,(env,sk)) in enumerate(zip(get_envs(cache),get_states(cache)))
        if k != i && k in get_undecided(cache)
            ak = wait(env,sk)
            spk = get_next_state(env,sk,ak)
            # if states_match(sp,sk)
            if detect_state_conflict(PathNode(s,a,sp),PathNode(sk,ak,spk))
                return k
            end
        end
    end
    return -1
end

function is_consistent(cache::PIBTCache,mapf)
    for (env,s) in zip(get_envs(cache),get_states(cache))
        if !is_goal(env,s) # || is_valid(get_goal(env))
            return false
        end
    end
    return true
end

export pibt_priority_law
"""
    pibt_priority_law(solver,mapf,cache,i)

Returns a value that will determine the priority of agent i relative to other
agents. A lower value means higher priority.
"""
pibt_priority_law(solver,mapf,cache,i) = (-get_timers(cache)[i],i)

function pibt_preprocess!(solver,mapf,cache) end

function pibt_set_ordering!(solver,mapf,cache)
    get_ordering(cache) .= sortperm(
        map(i->pibt_priority_law(solver,mapf,cache,i),1:length(get_envs(cache)))
        )
    @log_info(3,verbosity(solver),"ordering: ", get_ordering(cache))
    return cache
end
function pibt_init_cache(solver,mapf,solution=get_initial_solution(mapf))
    N = num_agents(mapf)
    node = initialize_root_node(solver,mapf)
    envs = Vector{base_env_type(mapf)}(map(i->build_env(solver,mapf,node,i), 1:N))
    ordering = collect(1:N)
    undecided = Set{Int}()
    # reservation_table = Set{Int}()
    # reservation_table = PIBTReservationTable(mapf)
    reservation_table = ReservationTable{Float64}(num_states(mapf)+num_actions(mapf))
    timers = zeros(Int,N)
    end_idxs = map(get_end_index, get_paths(solution))
    active_countdowns = end_idxs .- minimum(end_idxs)
    # states = map(p->get_final_state(p), get_paths(solution))
    states = [
        get_s(p,get_end_index(p)+1-t) for (p,t) in zip(
            get_paths(solution),active_countdowns)
            ]
    # actions = map(i->wait(envs[i],states[i]), 1:N)
    actions = [
        get_a(p,get_end_index(p)+1-t) for (p,t) in zip(
            get_paths(solution),active_countdowns)
            ]
    cache = PIBTCache(
        solution,
        envs,
        states,
        actions,
        ordering,
        undecided,
        reservation_table,
        timers,
        active_countdowns,
    )
    pibt_preprocess!(solver,mapf,cache)
    pibt_set_ordering!(solver,mapf,cache)
    reset_undecided!(cache)
    reset_reservations!(cache)
    cache
end

function pibt_update_solution!(solver,solution,cache)
    for (i,(p,env,s,a)) in enumerate(zip(get_paths(solution),get_envs(cache),get_states(cache),get_actions(cache)))
        if !is_active(cache,i)
            continue
        end
        sp = get_next_state(env,s,a)
        add_to_path!(p,env,s,a,sp)
        set_path_cost!(solution,get_cost(p),i)
    end
    set_cost!(solution, aggregate_costs(
        get_cost_model(cache),
        get_path_costs(solution)
        ))
    solution
end
function pibt_update_env!(solver,mapf,cache,i)
    node = initialize_root_node(solver,mapf)
    # Trying to set a dummy task for the goal (serves as a marker that this agent's task is complete)
    # But still need to set agent_id = i
    env = build_env(solver,mapf,node,-1)
    get_envs(cache)[i] = base_env_type(mapf)(env,agent_idx=i)
end
function pibt_update_envs!(solver,mapf,cache)
    for (i,(s,env)) in enumerate(zip(get_states(cache),get_envs(cache)))
        if !is_active(cache,i)
            continue
        end
        if is_goal(env,s) && is_valid(get_goal(env))
            pibt_update_env!(solver,mapf,cache,i)
            get_timers(cache)[i] = 0
        end
    end
end
function pibt_update_cache!(solver,mapf,cache)
    pibt_update_solution!(solver,get_solution(cache),cache)
    get_timers(cache) .+= 1
    get_active_countdowns(cache) .= max.(0, get_active_countdowns(cache) .- 1)
    for (i,p) in enumerate(get_paths(get_solution(cache)))
        t = get_end_index(p)+1-get_active_countdowns(cache)[i]
        get_states(cache)[i] = get_s(p,t)
        get_actions(cache)[i] = get_a(p,t)
    end
    reset_undecided!(cache)
    reset_reservations!(cache)
    pibt_update_envs!(solver,mapf,cache)
    if any(get_timers(cache) .== 0)
        pibt_set_ordering!(solver,mapf,cache)
    end
    return cache
end
function pibt_next_agent_id(solver,cache)
    for i in get_ordering(cache)
        if i in get_undecided(cache)
            return i
        end
    end
    return -1
end

export pibt_step!

"""
    pibt_step!(solver,mapf,i,j=-1)

i is the id of the higher priority agent, j is the index of the lower priority
agent.
"""
function pibt_step!(solver,mapf,cache,i=pibt_next_agent_id(solver,cache),j=-1,PRIORITY=-1)
    env = get_envs(cache)[i]
    s = get_states(cache)[i]
    sj = get(get_states(cache), j, state_type(mapf)())
    a_list = sorted_actions(env,s) # NOTE does NOT need to exclude wait()
    @log_info(3,verbosity(solver),"PIBT: pibt_step!( ... i = ",i,", j = ",j," )")
    @log_info(3,verbosity(solver),"  s        = ",string(s))
    @log_info(3,verbosity(solver),"  actions  = ",map(string,a_list))
    @log_info(3,verbosity(solver),"  env.goal = ",string(get_goal(env)))
    setdiff!(cache.undecided,i) # NOTE does this need to be uncommented after all?
    while ~isempty(a_list)
        a = a_list[1]
        sp = get_next_state(env,s,a)
        if !is_reserved(cache,env,s,a,sp) && !states_match(sp,sj) # Problem: Does not filter out self reservations
            @log_info(3,verbosity(solver),"  agent $i reserve!( ... a = ",string(a),", sp = ",string(sp)," )")
            reserve!(cache,env,s,a,sp)
            k = get_conflict_index(cache,i,s,a,sp)
            if k != -1
                @log_info(3,verbosity(solver),"  agent $i get_conflict_index( i = ",i,", sp = ",string(sp)," ) : ",k)
                if pibt_step!(solver,mapf,cache,k,i)
                    set_action!(cache,i,a)
                    # setdiff!(cache.undecided,i)
                    return true
                else
                    deleteat!(a_list,1)
                    # break # <--- NOTE is this the problem? What if we don't break here?
                end
            else
                set_action!(cache,i,a)
                # setdiff!(cache.undecided,i)
                return true
            end
        else
            @log_info(3,verbosity(solver),"  agent $i illegal action ",string(a),
                ". Reserved by ",reserved_by(cache,env,s,a,sp))
            deleteat!(a_list,1)
        end
    end
    set_action!(cache,i,wait(env,s))
    return false
end

export pibt!

function pibt!(solver, mapf)
    cache = pibt_init_cache(solver,mapf)
    @log_info(0,verbosity(solver),"PIBT: Entering...")
    while !is_consistent(cache,mapf)
        try
            increment_iteration_count!(solver)
            enforce_iteration_limit!(solver)
            enforce_time_limit!(solver)
        catch e
            if isa(e,SolverException)
                handle_solver_exception(solver,e)
                break
            else
                rethrow(e)
            end
        end
        @log_info(1,verbosity(solver),"PIBT iterations = ",iterations(solver))
        while !isempty(cache.undecided)
            if ~pibt_step!(solver,mapf,cache)
                return get_solution(cache), false
            end
        end
        # update cache
        pibt_update_cache!(solver,mapf,cache)
        @log_info(3,verbosity(solver),"solution: ",convert_to_vertex_lists(get_solution(cache)))
    end
    if solver.partial
        return get_solution(cache), true
    end
    return get_solution(cache), is_consistent(cache,mapf)
end

function solve!(solver::PIBTPlanner,mapf)
    solution, valid = pibt!(solver,mapf)
    if valid
        @log_info(0,verbosity(solver),"SUCCESS! Valid solution returned by PIBT")
        set_best_cost!(solver, get_cost(solution))
    else
        @log_info(0,verbosity(solver),"FAILED! Invalid solution returned by PIBT")
        set_cost!(solution,typemax(cost_type(mapf)))
    end
    return solution, best_cost(solver)
end


text:
struct BasicIdealParam <: EoSParam
end

abstract type BasicIdealModel <: IdealModel end
struct BasicIdeal <: BasicIdealModel
    params::BasicIdealParam
end

export BasicIdeal
function BasicIdeal(components::Array{String,1}; userlocations::Array{String,1}=String[], verbose=false)
    return BasicIdeal(BasicIdealParam())
end

function BasicIdeal(; userlocations::Array{String,1}=String[], verbose=false)
    return BasicIdeal(BasicIdealParam())
end
is_splittable(::BasicIdeal) = false

function a_ideal(model::BasicIdeal, V, T, z)
    N = ∑(z)
    #x = z/∑(z)
    res = ∑(xlogx,z)
    res /= N
    res -= log(V)
    res -= log(T^1.5)
    res -= one(res)
    # ∑(x .* log.(z/V)) - 1 original formulation, prone no NaN when passing pure Fractions
    return res
end


text:
function parsexml11(dir::String)
    for file in readdir(dir)

    end

    xml = readxml(path)
    arxivs = find(xml, "//arXiv")
    for arxiv in arxivs
        id = "arXiv" * nodecontent(findfirst(arxiv,"./id"))
        title = nodecontent(findfirst(arxiv,"./title"))
        abst = nodecontent(findfirst(arxiv,"./abstract"))

        doc = """
        <?xml version="1.0" encoding="UTF-8"?>

        <front>
        <id>$id</id>
        <journal-title>arXiv</journal-title>

$title

        <year>2016</year>
        <abstract>

$(strip(abst))

        </abstract>

        """

        #dir = joinpath()
        #isdir(dir) || mkdir(dir)
        #open(io -> print(io,doc), "$dir", "w")
    end
end


text:
#!/usr/bin/env julia

using Test
push!(LOAD_PATH, "../src")

include("CornerPointsTests.jl")
#include("ModelsTests.jl")
#include("Check3DPackingsTests.jl")
#include("ExtremumPlanesTests.jl")


text:
using JuMP
using Ipopt
using FiniteDiff
using Polynomials
using MorePolynomials

struct BoundaryConstraint # make a default setting if unbounded
    initialState::Array # make sure these are Nx1 size matricies
    finalState::Array # incorperate lower bound and ub
end

mutable struct TrajProblem
    objectiveFunc::Function
    dynamicsFunc::Function
    pathConstraintFunc::Function
    stateVector::Array{AbstractPolynomial}
    sVector::Array{AbstractPolynomial}
    stateVectorDiff::Array{AbstractPolynomial}
    sVectorDiff::Array{AbstractPolynomial}
    stateDiffMat::AbstractArray # differentiation matrix for state
    controlVector::Array{AbstractPolynomial}
    cVector::Array{AbstractPolynomial}
    boundaryConstraints::BoundaryConstraint # add checks that these are dimensionally correct
    numStates::Int   # can determine these from size of guess
    numControls::Int
    τf
    τfWeights
    τp
    τpWeights
    τo
    τoWeights
    t0::Number
    tf::Number
    endJac::Array
    colJac::Array
    objJac::Array
    patJac::Array
    stateControlCache
    endJacCache
    colJacCache
    objJacCache
    patJacCache
    function TrajProblem(objectiveFunc, dynamicsFunc, pathConstraintFunc, stateVector, controlVector, boundaryConstraints, τf, τp, τo, t0,tf)
        numStates = length(stateVector)
        numControls = length(controlVector)
        sVector = [LagrangePoly(stateVector[i].x, stateVector[i][:]) for i = 1:numStates]
        cVector = [LagrangePoly(controlVector[i].x, controlVector[i][:]) for i = 1:numControls]
        stateVectorDiff = [derivative(stateVector[i]) for i = 1:numStates]
        sVectorDiff = [derivative(stateVector[i]) for i = 1:numStates]
        stateDiffMat = [derivmatrix(stateVector[i]) for i = 1:numStates]
        τfWeights = lgr_weights(length(τf))
        τpWeights = lgr_weights(length(τp))
        τoWeights = lgr_weights(length(τo))
        stateControlLen = numStates*length(stateVector[1]) + numControls * length(controlVector[1])
        endJac = zeros(numStates, stateControlLen)
        colJac = zeros(length(τf)*numStates, stateControlLen)
        objJac = zeros(stateControlLen)
        patJac = zeros(length(pathConstraintFunc(stateVector, controlVector, τp[1])) * length(τp), stateControlLen)
        stateControlCache = tuple(zeros(numStates*length(stateVector[1]) + numControls*length(controlVector[1]))...)
        endJacCache = tuple(zeros(numStates*length(stateVector[1]) + numControls*length(controlVector[1]))...)
        colJacCache = tuple(zeros(numStates*length(stateVector[1]) + numControls*length(controlVector[1]))...)
        objJacCache = tuple(zeros(numStates*length(stateVector[1]) + numControls*length(controlVector[1]))...)
        patJacCache = tuple(zeros(numStates*length(stateVector[1]) + numControls*length(controlVector[1]))...)
        return new(objectiveFunc, dynamicsFunc, pathConstraintFunc, stateVector, sVector, stateVectorDiff, sVectorDiff, stateDiffMat, controlVector, cVector, boundaryConstraints, numStates, numControls, τf, τfWeights, τp, τpWeights, τo, τoWeights, t0, tf, endJac, colJac, objJac, patJac, stateControlCache, endJacCache,colJacCache, objJacCache, patJacCache)
    end
end

# work around to make current problem globally available
mutable struct CurrentProblem
    nullableProblem::Union{TrajProblem,Nothing}
end

const CURRENT_PROBLEM = CurrentProblem(nothing)
setCurrentProblem(problem::TrajProblem) = (CURRENT_PROBLEM.nullableProblem = problem)
getCurrentProblem() = CURRENT_PROBLEM.nullableProblem

function solve(problem::TrajProblem) # multiple dispatch on this function
    setCurrentProblem(problem)
    model = Model(with_optimizer(Ipopt.Optimizer)) # remove cpu time at some point
    stateLen = length(problem.stateVector[1]) # number of points on each state vector
    controlLen = length(problem.controlVector[1])
    numStates = problem.numStates # number of state vectors
    numControls = problem.numControls

    # assign state and control varibles
    @variable(model, x[1:numStates, 1:stateLen])
    @variable(model, u[1:numControls, 1:controlLen])
    # set state and control variables to problem guess
    [set_start_value(x[i,j], problem.stateVector[i][j][2]) for i = 1:numStates, j = 1:stateLen]
    [set_start_value(u[i,j], problem.controlVector[i][j][2]) for i = 1:numControls, j = 1:controlLen]

    #register custom defined functions
    register(model, :collocateConstraint, (problem.numStates + problem.numControls) * stateLen + 2, collocateConstraint, ΔcollocateConstraint)
    register(model, :pathConstraint, (problem.numStates + problem.numControls) * stateLen + 1, pathConstraint, ΔpathConstraint)
    register(model, :objectiveFuncInterp, (problem.numStates + problem.numControls) * stateLen, objectiveFuncInterp, ΔobjectiveFuncInterp)
    register(model, :endPointInterp, (problem.numStates + problem.numControls) * stateLen + 1, endPointInterp, ΔendPointInterp)

    # state boundary constraints
    # initial state
    [fix(x[i,1], problem.boundaryConstraints.initialState[i]) for i = 1:length(problem.boundaryConstraints.initialState)]   # adjust this for lb and ub
    # final state
    [fix(x[i,end], problem.boundaryConstraints.finalState[i]) for i = 1:length(problem.boundaryConstraints.finalState)]   # adjust this for lb and ub

    for i = 1:length(problem.pathConstraintFunc(problem.stateVector, problem.controlVector, problem.τp[1])) * length(problem.τp)
        @NLconstraint(model, pathConstraint(i,x...,u...) <=0)
    end

    for i = 1:problem.numStates
        @NLconstraint(model, endPointInterp(i,x...,u...) ==0)
        for j = 1:length(problem.τf)
            @NLconstraint(model, collocateConstraint(i,j,x...,u...) == 0)
        end
    end

    @NLobjective(model, Min, objectiveFuncInterp(x...,u...))
    optimize!(model)
    return value.(x), value.(u), model
end

function endPointInterp(ζr, stateControlVector...)
    ζr = convert(Int, ζr)
    problem = getCurrentProblem()
    τf = problem.τf
    tf = problem.tf
    t0 = problem.t0
    updateStateControl(problem, stateControlVector)
    # return interpolated integral
    f = map(t -> problem.dynamicsFunc(problem.stateVector, problem.controlVector, t), τf)
    e = zeros(1, problem.numStates)
    weights = lgr_weights(length(τf))
    for i = 1:problem.numStates
        e[i] = problem.stateVector[i](1) - problem.stateVector[i](-1) -  (tf-t0) * 0.5 * sum([problem.τfWeights[k]*f[k][i] for k = 1:length(τf)])
    end
    return e[ζr]
end

function endPointInterp(stateControlVector::Array)  # can we type union this? so we don't have to define two functions, only one
    problem = getCurrentProblem()
    τf = problem.τf
    tf = problem.tf
    t0 = problem.t0
    updateSC(problem, stateControlVector)
    s = problem.sVector
    c = problem.cVector
    f = map(t -> problem.dynamicsFunc(s, c, t), τf)
    e = zeros(1, problem.numStates)
    for i = 1:problem.numStates
        e[i] = s[i](1) - s[i](-1) -  (tf-t0) * 0.5 * sum([problem.τfWeights[k]*f[k][i] for k = 1:length(τf)])
    end
    return e
end

function ΔendPointInterp(g, ζr, stateControlVector...)
    problem = getCurrentProblem()
    g[1] = 0 # gradient of xr and xc is zero as they're just indexes and will not be changed
    ζr = convert(Int, ζr)
    if stateControlVector!== problem.endJacCache
        stateVector, controlVector = getXUFromStateControl(problem, stateControlVector)
        problem.endJac = FiniteDiff.finite_difference_jacobian(endPointInterp, [stateVector; controlVector])
        problem.endJacCache = stateControlVector
    end
    g[2:end] = getStateControlFromXU(problem, problem.endJac[ζr,:])
    return g
end

function objectiveFuncInterp(stateControlVector...)
    problem = getCurrentProblem()
    τo = problem.τo
    tf = problem.tf
    t0 = problem.t0
    updateStateControl(problem, stateControlVector)
    # return interpolated integral
    obj = map(t -> problem.objectiveFunc(problem.stateVector, problem.controlVector, t), τo)
    return (tf-t0) * 0.5 * sum(problem.τoWeights.*obj)
end

function objectiveFuncInterp(stateControlVector::Array)  # can we type union this? so we don't have to define two functions, only one
    problem = getCurrentProblem()
    τo = problem.τo
    tf = problem.tf
    t0 = problem.t0
    updateSC(problem, stateControlVector)
    s = problem.sVector
    c = problem.cVector
    obj = map(t -> problem.objectiveFunc(s, c, t), problem.τo )
    return (tf-t0) * 0.5 * sum(problem.τoWeights.*obj)
end

function ΔobjectiveFuncInterp(g, stateControlVector...)
    problem = getCurrentProblem()
    if stateControlVector!== problem.objJacCache
        stateVector, controlVector = getXUFromStateControl(problem, stateControlVector)
        problem.objJac = FiniteDiff.finite_difference_jacobian(objectiveFuncInterp, [stateVector; controlVector])
        problem.objJacCache = stateControlVector
    end
    g[:] = getStateControlFromXU(problem, problem.objJac)
    return g
end

function collocateConstraint(ζr, ζc, stateControlVector...) # make sure timestep vector matches length of state vector
    ζr = convert(Int, ζr)
    ζc = convert(Int, ζc) # convert from jumps conversion to float64
    τf = problem.τf
    tf = problem.tf
    t0 = problem.t0
    updateStateControl(problem, stateControlVector)
    ζ = zeros(problem.numStates, length(τf))
    f = map(t -> problem.dynamicsFunc(problem.stateVector, problem.controlVector, t), τf)
    for i = 1:problem.numStates
        for j = 1:length(τf)
            ζ[i,j] = problem.stateVectorDiff[i](τf[j]) - (tf-t0) * 0.5 * f[j][i]
        end
    end
    return ζ[ζr, ζc]
end

function collocateConstraint(stateControlVector::Array) # make sure timestep vector matches length of state vector
    problem = getCurrentProblem()
    problem = getCurrentProblem()
    τf = problem.τf
    tf = problem.tf
    t0 = problem.t0
    updateSC(problem, stateControlVector)
    s = problem.sVector
    c = problem.cVector
    sDiff = problem.sVectorDiff
    ζ = zeros(problem.numStates, length(τf))
    f = map(t -> problem.dynamicsFunc(s, c, t), τf)
    for i = 1:problem.numStates
        for j = 1:length(τf)
            ζ[i,j] = sDiff[i](τf[j]) - (tf-t0) * 0.5 * f[j][i]
        end
    end
    return ζ
end

function ΔcollocateConstraint(g, ζr, ζc, stateControlVector...)
    problem = getCurrentProblem()
    g[1:2] .= 0 # gradient of xr and xc is zero as they're just indexes and will not be changed
    ζr = convert(Int, ζr)
    ζc = convert(Int, ζc) # convert from jumps conversion to float64
    if stateControlVector!== problem.colJacCache
        stateVector, controlVector = getXUFromStateControl(problem, stateControlVector)
        problem.colJac = FiniteDiff.finite_difference_jacobian(collocateConstraint, [stateVector; controlVector])
        problem.colJacCache = stateControlVector
    end
    jacobianRow = (ζc - 1) * problem.numStates + ζr # get the row of the jacobian that matches our output function
    g[3:end] = getStateControlFromXU(problem, problem.colJac[jacobianRow,:])
    return g
end

function pathConstraint(ζr, stateControlVector...) # make sure timestep vector matches length of state vector
    ζr = convert(Int, ζr)
    problem = getCurrentProblem()
    τp = problem.τp
    updateStateControl(problem, stateControlVector)
    notFlatPaths = map(t -> problem.pathConstraintFunc(problem.stateVector, problem.controlVector, t), τp) # paths before array flattening
    ζ = notFlatPaths[1]
    for i = 2:length(notFlatPaths)
        push!(ζ, notFlatPaths[i]...)
    end
    return ζ[ζr]
end

function pathConstraint(stateControlVector::Array) # make sure timestep vector matches length of state vector
    problem = getCurrentProblem()
    τp = problem.τp
    updateSC(problem, stateControlVector)
    s = problem.sVector
    c = problem.cVector

    notFlatPaths = map(t -> problem.pathConstraintFunc(s, c, t), τp) # paths before array flattening
    ζ = notFlatPaths[1]
    for i = 2:length(notFlatPaths)
        push!(ζ, notFlatPaths[i]...)
    end
    return ζ
end

function ΔpathConstraint(g, ζr,stateControlVector...)
    problem = getCurrentProblem()
    g[1] = 0 # gradient of xr and xc is zero as they're just indexes and will not be changed
    ζr = convert(Int, ζr)
    if stateControlVector!== problem.patJacCache
        stateVector, controlVector = getXUFromStateControl(problem, stateControlVector)
        problem.patJac = FiniteDiff.finite_difference_jacobian(pathConstraint, [stateVector; controlVector])
        problem.patJacCache = stateControlVector
    end
    g[2:end] = getStateControlFromXU(problem, problem.patJac[ζr,:])
    return g
end

function getXUFromStateControl(problem, stateControlVector::Tuple) # get separate state and control vector matricies from input tuple, add type to problem
    stateControlVector = collect(stateControlVector)
    stateVector = zeros(problem.numStates, length(problem.stateVector[1]))
    controlVector = zeros(problem.numControls, length(problem.stateVector[1]))
    for i = 1:length(stateControlVector) ÷ (problem.numStates + problem.numControls)
        jx = (i-1) * (problem.numStates) + 1
        ju = (i-1) * (problem.numControls) + length(problem.stateVector[1]) * problem.numStates + 1
        stateVector[:,i] = stateControlVector[jx:jx+problem.numStates-1]
        controlVector[:,i] = stateControlVector[ju:ju+problem.numControls-1]
    end
    return stateVector, controlVector
end

function getXUFromStateControl(problem, stateControlVector::Array) # get separate state and control vector matricies from input tuple
    stateVector = Array{Union{Missing, typeof(stateControlVector[1]) }}(missing, problem.numStates, length(problem.stateVector[1]))
    controlVector = Array{Union{Missing,typeof(stateControlVector[1]) }}(missing, problem.numControls, length(problem.stateVector[1]))
    stateVector[:,:] = stateControlVector[1:problem.numStates, :]
    controlVector[:,:] = stateControlVector[problem.numStates + 1, :]
    return stateVector, controlVector
end

function getStateControlFromXU(problem, stateControlVector::Array) # get tuple of statecontrols from a 1D array with states and controls in a column row pattern
    x = ones(1,length(problem.stateVector[1])*problem.numStates)
    u = ones(1,length(problem.stateVector[1])*problem.numControls)
    for i = 1:length(problem.stateVector[1])
        j = (i-1)*(problem.numStates + problem.numControls) + 1
        x[(i-1)*problem.numStates+1:i*problem.numStates] = stateControlVector[j:j+problem.numStates-1]
        u[(i-1)*problem.numControls+1:i*problem.numControls] = stateControlVector[j+problem.numStates:j+problem.numStates+problem.numControls-1]
    end
    return [x u]
end

function updateStateControl(problem, stateControlVector)
    stateControlVector == problem.stateControlCache && return 1
    problem.stateControlCache = stateControlVector
    # assemble state and control vector from tuple inputs
    stateVector, controlVector = getXUFromStateControl(problem, stateControlVector)
    for i = 1:problem.numStates
        problem.stateVector[i][:] = stateVector[i,:]
        problem.stateVectorDiff[i][:] = problem.stateDiffMat[i]*problem.stateVector[i][:]
    end
    for i = 1:problem.numControls
        problem.controlVector[i][:] = controlVector[i,:]
    end
end

function updateSC(problem, stateControlVector)
    # assemble state and control vector from tuple inputs
    stateVector, controlVector = getXUFromStateControl(problem, stateControlVector)
    for i = 1:problem.numStates
        problem.sVector[i][:] = convert(Array{Float64,1}, stateVector[i,:])
        problem.sVectorDiff[i][:] = convert(Array{Float64,1}, problem.stateDiffMat[i]*stateVector[i,:])
    end
    for i = 1:problem.numControls
        problem.cVector[i][:] = convert(Array{Float64,1}, controlVector[i,:])
    end
end

####################################################################################################################################
### problem specific functions #####################################################################################################
####################################################################################################################################

function dynamicsFunc(stateVector, controlVector, t)
    x1dot = 2*stateVector[1](t) + 2 * controlVector[1](t) * sqrt(abs(stateVector[1](t)))
    return [x1dot]
end
objectiveFunc(stateVector, controlVector, t) = 0.5 * stateVector[1](t)+controlVector[1](t)^2

function pathConstraintFunc(stateVector, controlVector, t) # less than or equal to output value. Output must by 1 D vector of constraints
    return []
end

####################################################################################################################################
### example code  ##################################################################################################################
####################################################################################################################################

numCP = 10
T = 5
lgrPoints = lgr_points(numCP)
controlVector = LagrangePoly([lgrPoints...,1],ones(numCP+1))
timeStep = ones(1,numCP) * T/(numCP)
time = pushfirst!(cumsum(timeStep';dims=1)[:,1],0.0)

stateVector1 = LagrangePoly([lgrPoints...,1],ones(numCP+1))
boundaryConstraints = BoundaryConstraint([2],[1])
τf = lgr_points(numCP+5)
τp = lgr_points(numCP)
τo = lgr_points(numCP)
problem = TrajProblem(objectiveFunc, dynamicsFunc, pathConstraintFunc, [stateVector1], [controlVector], boundaryConstraints,τf, τp, τo, 0, T)
x,u,model = solve(problem)

using Plots
plotly()
p1 = plot(problem.stateVector[1],labels=["" ""], ylabel="Position q")
p3 = plot(problem.controlVector[1],labels=["" ""],ylabel="Force", xlabel="time t")
plot(p1,p3,layout = (2,1), )

### temp
y = [2.0, -3584.1048030787497, 1317.8705472041995, -7.696519811573808e-7, -8.92243301147125e-9, 1.0]
u = [5435.382575539613, 45.135100639330325, -57.78216237982584, 1.7698584538630157e9, -9.976402056796717e6, 1.0000000149011612]
j = [9606.98974609375 7.071067850372862 9.109641175492397 0.0 -4.388556840394551 0.0 3.48486328125 0.0 -4.2060546875 0.0 2.5 0.0; -1.404998779296875 0.0 2.824588672037624 299.33696960174393 2.594170444135571 0.0 -1.69659423828125 0.0 1.9393310546875 0.0 -1.141357421875 0.0; 0.328521728515625 0.0 -1.2591604659143418 0.0 0.5924021074809049 181.5124324739676 2.24798583984375 0.0 -2.06109619140625 0.0 1.172088623046875 0.0; -0.125 0.0 0.43295329943115646 0.0 -1.1818782226405284 0.0 -5.068142991574625e12 0.004386490580958561 3.5625 0.0 -1.8125 0.0; 0.0668487548828125 0.0 -0.19985260000179755 0.0 0.43759317888452565 0.0 -1.44598388671875 0.0 5.736451210761557e10 0.0004722931523938615 5.51934814453125 0.0]

time = 2.5
stateJac = (2 .+ u .* abs.(y).^(-0.5)) .* time
controlJac =  (2 .* sqrt.(abs.(y))).*time
function g(x)
    p = derivative(LagrangePoly([lgrPoints...,1],x[1,:]))
    p.y - time*(2 .* x[1,:] .+ 2 .* x[2,:] .* sqrt.(abs.(x[1,:])))
end
homej = FiniteDiff.finite_difference_jacobian(g, [y';u'])
#for i = 1:length(y)+length(u)
#    if mod(i,2)==1
#        homej[:,i] += derivmatrix(stateVector1)[:,(i+1)÷2]
#    end
#end
homej = homej[1:5,:]

j[j - homej .< 0.1] .= 0
j

max_stars_repo_name: sprockmonty/JuliaFYP

max_issues_repo_name: sprockmonty/JuliaFYP

max_forks_repo_name: sprockmonty/JuliaFYP


text:
module SfMaterials

using Unitful, ..SfMatter

aluminium = Material(2.7u"g/cm^3", 270u"MPa") # yield strength for 6061 alloy, https://en.wikipedia.org/wiki/6061_aluminium_alloy
iron = Material(7.874u"g/cm^3")
steel = Material(7700u"kg/m^3", 350u"MPa") # 1020 steel, http://www.matweb.com/search/datasheet.aspx?bassnum=MS0001
titanium = Material(4.506u"g/cm^3", 140u"MPa") # http://www.matweb.com/search/datasheet.aspx?bassnum=METi00&ckck=1
titanium_alloy = Material(4.51u"g/cm^3", 830u"MPa") # 6% Al, 4% V, https://en.wikipedia.org/wiki/Yield_(engineering)
tungsten = Material(19.25u"g/cm^3", 550u"MPa") # annealed, https://en.wikipedia.org/wiki/Yield_(engineering)
water = Material(997u"kg/m^3", 0u"Pa")
ice = Material(0.9168u"g/cm^3", 0u"Pa")

end

max_stars_repo_name: icantbelieveitsnotphysics/SfPhysics

max_issues_repo_name: icantbelieveitsnotphysics/SfPhysics

max_forks_repo_name: icantbelieveitsnotphysics/SfPhysics


text:
# # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # #
#
# Description
#
#   Main file for TLE.
#
# # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # #

export @tle_str, @tlenc_str
export read_tle, read_tle_from_string

################################################################################
#                                    Macros
################################################################################

"""
    macro tle_str(str)

Parse a set of TLEs in the string `str` and return as an array of `TLE`. This
version verifies the checksum of the TLE. If the checksum verification is not
desired, see `@tlenc_str`.

# Example

```julia-repl
julia> tles = tle\"""
       CBERS 4
       1 40336U 14079A   18166.15595376 -.00000014  00000-0  10174-4 0  9993
       2 40336  98.4141 237.7928 0001694  75.7582 284.3804 14.35485112184485
       SCD 1
       1 22490U 93009B   18165.62596833  .00000225  00000-0  11410-4 0  9991
       2 22490  24.9690 231.7852 0042844 200.7311 292.7198 14.44524498338066
       SCD 2
       1 25504U 98060A   18165.15074951  .00000201  00000-0  55356-5 0  9994
       2 25504  24.9961  80.1303 0017060 224.4822 286.6438 14.44043397 37312
       \"""
```
"""
macro tle_str(str)
    read_tle_from_string(str, true)
end

"""
    macro tlenc_str(str)

Parse a set of TLEs in the string `str` and return as an array of `TLE`. This
version **does not** verify the checksum of the TLE. If the checksum
verification is required, see `@tle_str`.

# Example

```julia-repl
julia> tles = tlenc\"""
       CBERS 4
       1 40336U 14079A   18166.15595376 -.00000014  00000-0  10174-4 0  9993
       2 40336  98.4141 237.7928 0001694  75.7582 284.3804 14.35485112184485
       SCD 1
       1 22490U 93009B   18165.62596833  .00000225  00000-0  11410-4 0  9991
       2 22490  24.9690 231.7852 0042844 200.7311 292.7198 14.44524498338066
       SCD 2
       1 25504U 98060A   18165.15074951  .00000201  00000-0  55356-5 0  9994
       2 25504  24.9961  80.1303 0017060 224.4822 286.6438 14.44043397 37312
       \"""
```
"""
macro tlenc_str(str)
    read_tle_from_string(str, false)
end

################################################################################
#                                  Overloads
################################################################################

function show(io::IO, tle::TLE)
    print(io, "TLE: ", tle.name, " (Epoch = ", julian2datetime(tle.epoch), ")")
    return nothing
end

function show(io::IO, mime::MIME"text/plain", tle::TLE)
    color = get(io, :color, false)
    print_tle(io, tle, color)
end

################################################################################
#                                  Functions
################################################################################

"""
    function read_tle(tle_filename::String, verify_checksum::Bool = true)

Read the TLEs in the file `tle_filename` and return an array of `TLE` with the
parsed TLEs.

If `verify_checksum` if `true`, then the checksum of both TLE lines will be
verified. Otherwise, the checksum will not be checked. If `verify_checksum` is
omitted, then it defaults to `true`.

"""
@inline function read_tle(tle_filename::String, verify_checksum::Bool = true)
    # Open the file in read mode.
    file = open(tle_filename, "r")

    _parse_tle(file, verify_checksum)
end

"""
    function read_tle_from_string(tles::String, verify_checksum::Bool = true)
    function read_tle_from_string(tle_l1::String, tle_l2::String, verify_checksum::Bool = false)

Parse a set of TLEs in the string `tles` or one TLE with first line `tle_l1` and
second line `tle_l2`. This function returns an array of `TLE` with the parsed
TLEs.

If `verify_checksum` if `true`, then the checksum of both TLE lines will be
verified. Otherwise, the checksum will not be checked. If `verify_checksum` is
omitted, then it defaults to `true`.

"""
@inline function read_tle_from_string(tles::String,
                                      verify_checksum::Bool = true)
    # Convert the string to an IOBuffer and call the function to parse it.
    _parse_tle(IOBuffer(tles), verify_checksum)
end

@inline function read_tle_from_string(tle_l1::String,
                                      tle_l2::String,
                                      verify_checksum::Bool = false)
    # Assemble the TLE into one string and call the function to parse it.
    read_tle_from_string(tle_l1 * '\n' * tle_l2, verify_checksum)
end


text:
@testset "sort.jl" begin
    @testset "sort" begin
        a = rand(10)
        test_rrule(sort, a)
        test_rrule(sort, a; fkwargs=(;rev=true))
    end
    @testset "partialsort" begin
        a = rand(10)
        test_rrule(partialsort, a, 4)
        test_rrule(partialsort, a, 3:5)
        test_rrule(partialsort, a, 1:2:6)

        test_rrule(partialsort, a, 4, fkwargs=(;rev=true))
    end

    @testset "sortslices" begin
        test_rrule(sortslices, rand(3,4); fkwargs=(; dims=2))
        test_rrule(sortslices, rand(5,4); fkwargs=(; dims=1, rev=true, by=last))
        test_rrule(sortslices, rand(3,4,5); fkwargs=(; dims=3, by=sum), check_inferred=false)

        @test_throws Exception sortslices(Diagonal(1:3), dims=1)
    end

    @testset "unique" begin
        # Trivial case, all unique:
        test_rrule(unique, rand(5))
        test_rrule(unique, rand(3,4))
        test_rrule(unique, rand(3,4); fkwargs=(; dims=2))

        # Not all unique:
        @test rrule(unique, [1,1,2,3])[1] == [1,2,3]
        @test rrule(unique, [1,1,2,3])[2]([10,20,30]) == (NoTangent(), [10, 0, 20, 30])

        @test rrule(unique, [1 2; 1 4])[1] == [1,2,4]
        @test rrule(unique, [1 2; 1 4])[2]([10,20,30]) == (NoTangent(), [10 20; 0 30])

        @test rrule(unique, [1 2 1 2; 1 2 1 4], dims=2)[1] == [1 2 2; 1 2 4]
        @test rrule(unique, [1 2 1 2; 1 2 1 4], dims=2)[2]([10 20 30; 40 50 60])[2] == [10 20 0 30; 40 50 0 60]

        @test rrule(unique, Diagonal([1,2,3]))[1] == [1,0,2,3]
        @test rrule(unique, Diagonal([1,2,3]))[2]([10 20 30 40])[2] == [10.0 0.0 0.0; 0.0 30.0 0.0; 0.0 0.0 40.0]
        @test rrule(unique, Diagonal([1,2,3]))[2]([10 20 30 40])[2] isa Diagonal
    end
end


text:
module MCpack

# Write your package code here.

end # end module MCpack


text:
using Documenter, PandaModels

makedocs(
    modules = [PandaModels],
    authors = "e2nIEE",
    sitename = "PandaModels",
    format = Documenter.HTML(
        prettyurls = get(ENV, "CI", nothing) == "true",
        canonical="https://e2nIEE.github.io/PandaModels.jl/stable/",
        assets = String[],
    ),
    pages = [
        "Home" => "index.md",
        "Manual" => ["Getting Started" => "quickguide.md"],
        "Tutorials" => [
            "Optimazion Problems"  => "pptutorial.md",
            # "Power Flow" => "pf.md",
            # "Optimal Power Flow" => "opf.md",
            # "Optimal Transmission Switching" => "ots.md",
            # "Timeseries and Multinetwork" => "ts_mn.md",
            # "Optimal MultiNetwork Storage" => "omns.md",
            # "Transmission Network Expansion Planning" => "tnep.md",
            # "Optimal Voltage Deviation" => "vd.md",
            # "Radial Distribution Network" => "rds.md",
        ],
        "Developer" => [
            "Develop Mode" => "develop.md",
            # "Model Guidlines" => "model.md",
            # "Call Model in pandapower" => "modelpp.md",
            "Add Test" => "test.md",
        ],
    ],
    doctest = true,
    linkcheck = true,
)

deploydocs(
    repo = "github.com/e2nIEE/PandaModels.jl.git",
    push_preview = true,
    devbranch = "main",
    # devurl = "dev",
    # versions = ["stable" => "v^", "v#.#", devurl => devurl]
)


text:
import Base.*, Base.show

abstract type DistanceUnits end

struct DistanceUnit <: DistanceUnits
    value::Float64
    factor::Int8
    name::String
end

DistanceUnit( fact::T1, name::String ) where {T1<:Integer} = DistanceUnit( 1.0, Int8(fact), name )
DistanceUnit( val::T1, fact::T2, name::String ) where {T1<:Real, T2<:Integer} = DistanceUnit( Float64(val), Int8(fact), name )

# defining and exporting each distance unit name
prefixes = [ ("yotta",24), ("zetta",21), ("exa",18), ("peta",15), ("tera",12), ("giga",9), ("mega",6),
             ("kilo",3), ("hecto",2), ("deca",1), ("",0), ("deci",-1), ("centi",-2), ("milli",-3),
             ("micro",-6), ("nano",-9), ("pico",-12), ("femto",-15), ("atto",-18), ("zepto",-21), ("yocto",-24) ]

for ( prefix, exponent ) in prefixes
    definition = "const $(prefix)m = DistanceUnit( $(exponent), \"$(prefix)meter\" )";
    exportstatement = "export $(prefix)m";

    eval( Meta.parse( definition ) );
    eval( Meta.parse( exportstatement ) );
end

Base.:*( num::T, un::DistanceUnits ) where {T<:Real} = DistanceUnit( num, un.factor, un.name )

changeUnits( from::DistanceUnits, to::DistanceUnits ) = DistanceUnit( from.value * 10.0^(from.factor - to.factor), to.factor, to.name )

Base.show(out::IO, un::DistanceUnits ) where U = print(out, un.value, un.name*"s" )

abstract type TimeUnits end

struct TimeUnit <: TimeUnits
    value::Float64
    seconds::Float64
    name::String
end

TimeUnit( secs::T1, name::String ) where {T1<:Real} = TimeUnit( 1.0, Float64(secs), name )
TimeUnit( val::T1, fact::T2, name::String ) where {T1<:Real, T2<:Real} = TimeUnit( Float64(val), Float64(fact), name )

# defining and exporting each time unit name
names = [ ("day",86400), ("hour",3600), ("minute",60), ("second",1), ("millisecond",0.001) ]

for ( name, numseconds ) in names
    definition = "const $(name) = TimeUnit( $(numseconds), \"$(name)\" )";
    exportstatement = "export $(name)";

    eval( Meta.parse( definition ) );
    eval( Meta.parse( exportstatement ) );
end

Base.:*( num::T, un::TimeUnits ) where {T<:Real} = TimeUnit( num, un.seconds, un.name )

changeUnits( from::TimeUnits, to::TimeUnits ) = TimeUnit( from.value * from.seconds/to.seconds, to.seconds, to.name )

Base.show(out::IO, un::TimeUnits ) where U = print(out, un.value, un.name*"s" )

max_issues_repo_name: ErichZimmer/quickPIV

max_forks_repo_name: ErichZimmer/quickPIV