"""
This test is the using the same procedure as identificationRobustInference.jmd
with using new funciton which have been added to scr/HypothesisTests.
For compating, the same methods in identificationRobustInference.jmd also
has been presented.
"""

using Optim, ForwardDiff, LinearAlgebra, Distributions, LaTeXStrings
"""
    AR_gmm_obj(gi:Function)

Returns a function `ar(θ)` where:
``ar(θ) = n [1/n \\sum g_i(θ)]' \\widehat{Var}(g_i(θ))^{-1}[1/n \\sum g_i(θ)] ``
"""
function AR_gmm_obj(gi::Function)
  function(θ)
    g = gi(θ)
    m = mean(g,dims=1)
    W = inv(cov(g))
    (size(g)[1]*( m*W*m')[1])
  end
end
"""
    AR_test(n::Integer, β::AbstractVector,
                      π::AbstractMatrix, ρ)

Returns a function `ar(θ)` where:
``ar(θ) = n [1/n \\sum g_i(θ)]' \\widehat{Var}(g_i(θ))^{-1}[1/n \\sum g_i(θ)] ``

Or equivalent to:
``AR_gmm_obj(get_gi(IVLogitShare(n,β0,π0,ρ)))``
"""
function AR_test(n::Integer, β::AbstractVector,
                      π::AbstractMatrix, ρ)
    ivlogitshare = IVLogitShare(n,β,π,ρ)
    gi = get_gi(ivlogitshare)
    return AR_gmm_obj(gi)
end

"""
There are also identification robust versions of likelihood ratio and
lagrange multiplier test. Moreire (2003)[@moreira2003] proposed a conditional
likelihood ratio (CLR) test for weakly identified linear IV
models. Kleibergen (2005)[@kleibergen2005] developed a Lagrange multiplier (often called
the KLM) test and extended Moreira's CLR test to weakly identified GMM
models.  More recently, Andrews and Guggenberge (2015) [@andrews2015]
and Andrews and Guggenberge (2017) [@andrews2017] showed the
validity of these tests under more general conditions. These tests are
somewhat more complicated than the AR test, but they have the
advantage that they are often more powerful. The AR test statistic has
a $\chi^2_{m}$ distribution, where $m$ is the number of moments. The
CLR and KLM statistics under strong identification have $\chi^2_k$
distributions (as does the Wald statistic), where $k$ is the number of
parameters. Consequently, when the model is overidentified, the CLR
and LM tests are more powerful than the AR test.

Here is an implementation of the KLM and CLR statistics. The names of
variables roughly follows the notation of Andrews and
Guggenberge(2017

"""

"""
    statparts(gi::Function)

Return a function.

compute common components of klm, rk, & clr stats
follows notation of Andrews & Guggenberger 2017, section 3.1
"""
function statparts(gi::Function)
  # compute common components of klm, rk, & clr stats
  # follows notation of Andrews & Guggenberger 2017, section 3.1
    function P(A::AbstractMatrix) # projection matrix
    A*pinv(A'*A)*A'
    end
    function(θ)
        giθ = gi(θ)
        p = length(θ)
        (n, k) = size(giθ)
        Ω = cov(giθ)
        gn=mean(gi(θ), dims=1)'
        #G = ForwardDiff.jacobian(θ->mean(gi(θ),dims=1),θ)
        Gi= ForwardDiff.jacobian(gi,θ)
        Gi = reshape(Gi, n , k, p)
        G = mean(Gi, dims=1)
        Γ = zeros(eltype(G),p,k,k)
        D = zeros(eltype(G),k, p)
        for j in 1:p
        for i in 1:n
          Γ[j,:,:] += (Gi[i,:,j] .- G[1,:,j]) * giθ[i,:]'
        end
        Γ[j,:,:] ./= n
        D[:,j] = G[1,:,j] - Γ[j,:,:]*inv(Ω)*gn
        end
        return(n,k,p,gn, Ω, D, P)
    end
end

"""
    klm(gi::Function)

Return test statistic for KLM method.
"""

function klm(gi::Function)
    SP = statparts(gi)
    function(θ)
        (n,k,p,gn, Ω, D, P) = SP(θ)
        return n*(gn'*Ω^(-1/2)*P(Ω^(-1/2)*D)*Ω^(-1/2)*gn)[1]
    end
end

"""
    clr(gi::Function)

Return test statistic for CLR method.
"""
function clr(gi::Function)
  SP = statparts(gi)
  function(θ)
        (n,k,p,gn, Ω, D, P) = SP(θ)
        rk = eigmin(n*D'*inv(Ω)*D)
        AR  = (n*gn'*inv(Ω)*gn)[1]
        lm = (n*(gn'*Ω^(-1/2)*P(Ω^(-1/2)*D)*Ω^(-1/2)*gn))[1]
        lr = 1/2*(AR - rk + sqrt( (AR-rk)^2 + 4*lm*rk))

        # simulate to find p-value
        S = 5000
        function randc(k,p,r,S)
        χp = rand(Chisq(p),S)
        χkp = rand(Chisq(k-p),S)
        0.5.*(χp .+ χkp .- r .+
              sqrt.((χp .+ χkp .- r).^2 .+ 4 .* χp.*r))
        end
        csim = randc(k,p,rk,S)
        return mean(csim.<=lr)
    end
end

function plot_cr(β,V, tests::AbstractArray{Function}, labels; ngrid=30)
  lb = β - sqrt.(diag(V))*5
  ub = β + sqrt.(diag(V))*5
  fig=scatter([β0[1]], [β0[2]], markersize=8, legend=false,
              xlabel=L"\beta_1", ylabel=L"\beta_2")
  ntest = 1000
  βtest = [rand(2).*(ub-lb) .+ lb for i in 1:ntest]
  pval = tests[1].(βtest)
  βtest = vcat(βtest'...)
  crit = 0.9
  fig=scatter!(βtest[:,1],βtest[:,2], group=(pval.<crit), legend=false,
               markersize=4, markerstrokewidth=0.0, seriesalpha=0.5,
               palette=:heat)
  b1 = lb[1]:(ub[1]-lb[1])/ngrid:ub[1]
  b2 = lb[2]:(ub[2]-lb[2])/ngrid:ub[2]
  colors = [:black, :red, :blue, :green]
  for t in 1:length(tests)
    fig=contour!(b1,b2,(a,b)->tests[t]([a,b]),
             levels = [0.9, 0.95],
             contour_labels=false, legend=false,
             label = labels[t],
             c=cgrad([colors[t],colors[t]],[0.0,1.0]))
  end
  fig
end

function gmmVar(θ,gi,W)
  g = gi(θ)
  n = size(g)[1]
  D = ForwardDiff.jacobian(θ->mean(gi(θ),dims=1),θ)
  Σ = cov(gi(θ))
  1/n*inv(D'*W*D)*(D'*W*Σ*W*D)*inv(D'*W*D)
end
function gmmObj(θ,gi,W)
  g = gi(θ)
  m = mean(g,dims=1)
  (size(g)[1]*( m*W*m')[1]) # return scalar, not 1x1 array
end

n = 50
k = 2
iv =3
π0 = vcat(0.1*diagm(0=>ones(k)),0.2*ones(iv-k,k))
ρ = 0.5
data = IVLogitShare(n,β0,π0,ρ)
opt1 = optimize(θ->gmmObj(θ, get_gi(data) ,I),
                β0, BFGS(), autodiff =:forward)
β1 = opt1.minimizer
V1 = gmmVar(β1,get_gi(data),I)

KLM_f = klm(get_gi(data))
pklm = θ -> cdf(Chisq(length(β1)), KLM_f(θ) )

AR_f = AR_gmm_obj(get_gi(data))
par  = θ ->cdf(Chisq(size(data.z)[2]), AR_f(θ) )
pclr  = clr(get_gi(data))
pwald = θ -> cdf(Chisq(length(β1)),(θ-β1)'*inv(V1)*(θ-β1))
plot_cr(β1,V1, [pclr, pklm, par, pwald],
        ["CLR","KLM","AR","Wald"], ngrid=40)

"""
Now the equivalent codes from part of identificationRobustInference.jmd
For comparison
"""
function simulate_ivshare(n,β,γ,ρ)
  z = randn(n, size(γ)[1])
  endo = randn(n, length(β))
  x = z*γ .+ endo
  ξ = rand(Normal(0,sqrt((1.0-ρ^2))),n).+endo[:,1]*ρ
  y = cdf.(Logistic(), x*β .+ ξ)
  return((y=y,x=x,z=z))
end
n = 100
k = 2
iv = 3
β0 = ones(k)
π0 = vcat(5*I,ones(iv-k,k))
ρ = 0.5
(y,x,z) = simulate_ivshare(n,β0,π0,ρ)

function gi_ivshare(β,y,x,z)
  ξ = quantile.(Logistic(),y) .- x*β
  ξ.*z
end

function gmmObj(θ,gi,W)
  g = gi(θ)
  m = mean(g,dims=1)
  (size(g)[1]*( m*W*m')[1]) # return scalar, not 1x1 array
end

function gmmVar(θ,gi,W)
  g = gi(θ)
  n = size(g)[1]
  D = ForwardDiff.jacobian(θ->mean(gi(θ),dims=1),θ)
  Σ = cov(gi(θ))
  1/n*inv(D'*W*D)*(D'*W*Σ*W*D)*inv(D'*W*D)
end

function ar(θ,gi)
  gmmObj(θ,gi,inv(cov(gi(θ))))
end

using ForwardDiff, Plots, Optim
Plots.gr()
function statparts(θ,gi)
  # compute common components of klm, rk, & clr stats
  # follows notation of Andrews & Guggenberger 2017, section 3.1
  function P(A::AbstractMatrix) # projection matrix
    A*pinv(A'*A)*A'
  end
  giθ = gi(θ)
  p = length(θ)
  (n, k) = size(giθ)
  Ω = cov(giθ)
  gn=mean(gi(θ), dims=1)'
  #G = ForwardDiff.jacobian(θ->mean(gi(θ),dims=1),θ)
  Gi= ForwardDiff.jacobian(gi,θ)
  Gi = reshape(Gi, n , k, p)
  G = mean(Gi, dims=1)
  Γ = zeros(eltype(G),p,k,k)
  D = zeros(eltype(G),k, p)
  for j in 1:p
    for i in 1:n
      Γ[j,:,:] += (Gi[i,:,j] .- G[1,:,j]) * giθ[i,:]'
    end
    Γ[j,:,:] ./= n
    D[:,j] = G[1,:,j] - Γ[j,:,:]*inv(Ω)*gn
  end
  return(n,k,p,gn, Ω, D, P)
end

function klm(θ,gi)
  (n,k,p,gn, Ω, D, P) = statparts(θ,gi)
  lm = n*(gn'*Ω^(-1/2)*P(Ω^(-1/2)*D)*Ω^(-1/2)*gn)[1]
end

function clr(θ,gi)
  (n,k,p,gn, Ω, D, P) = statparts(θ,gi)

  rk = eigmin(n*D'*inv(Ω)*D)
  AR  = (n*gn'*inv(Ω)*gn)[1]
  lm = (n*(gn'*Ω^(-1/2)*P(Ω^(-1/2)*D)*Ω^(-1/2)*gn))[1]
  lr = 1/2*(AR - rk + sqrt( (AR-rk)^2 + 4*lm*rk))

  # simulate to find p-value
  S = 5000
  function randc(k,p,r,S)
    χp = rand(Chisq(p),S)
    χkp = rand(Chisq(k-p),S)
    0.5.*(χp .+ χkp .- r .+
          sqrt.((χp .+ χkp .- r).^2 .+ 4 .* χp.*r))
  end
  csim = randc(k,p,rk,S)
  pval = mean(csim.<=lr)
end

function plot_cr(β,V, tests::AbstractArray{Function}, labels; ngrid=30)
  lb = β - sqrt.(diag(V))*5
  ub = β + sqrt.(diag(V))*5
  fig=scatter([β0[1]], [β0[2]], markersize=8, legend=false,
              xlabel=L"\beta_1", ylabel=L"\beta_2")
  ntest = 1000
  βtest = [rand(2).*(ub-lb) .+ lb for i in 1:ntest]
  pval = tests[1].(βtest)
  βtest = vcat(βtest'...)
  crit = 0.9
  fig=scatter!(βtest[:,1],βtest[:,2], group=(pval.<crit), legend=false,
               markersize=4, markerstrokewidth=0.0, seriesalpha=0.5,
               palette=:heat)
  b1 = lb[1]:(ub[1]-lb[1])/ngrid:ub[1]
  b2 = lb[2]:(ub[2]-lb[2])/ngrid:ub[2]
  colors = [:black, :red, :blue, :green]
  for t in 1:length(tests)
    fig=contour!(b1,b2,(a,b)->tests[t]([a,b]),
             levels = [0.9, 0.95],
             contour_labels=false, legend=false,
             label = labels[t],
             c=cgrad([colors[t],colors[t]],[0.0,1.0]))
  end
  fig
end

n = 50
k = 2
iv =3
π0 = vcat(0.1*diagm(0=>ones(k)),0.2*ones(iv-k,k))
ρ = 0.5
(y,x,z) = simulate_ivshare(n,β0,π0,ρ)
opt1 = optimize(θ->gmmObj(θ, β->gi_ivshare(β,y,x,z) ,I),
                β0, BFGS(), autodiff =:forward)
β1 = opt1.minimizer
V1 = gmmVar(β1,β->gi_ivshare(β,y,x,z),I)

pklm = θ->cdf(Chisq(length(βcue)),klm(θ, β->gi_ivshare(β,y,x,z)))
par  = θ->cdf(Chisq(size(z)[2]), ar(θ, β->gi_ivshare(β,y,x,z)))
pclr  = θ->clr(θ, β->gi_ivshare(β,y,x,z))
pwald = θ -> cdf(Chisq(length(β1)),(θ-β1)'*inv(V1)*(θ-β1))
plot_cr(β1,V1, [pclr, pklm, par, pwald],
        ["CLR","KLM","AR","Wald"], ngrid=40)


module SubsetSelectionCIO

using MathProgBase, JuMP, Gurobi, CPLEX, Compat, LinearAlgebra
using DataFrames, CSV

import Compat.String

include("inner_op.jl")

export oa_formulation

getthreads() = haskey(ENV, "SLURM_JOB_CPUS_PER_NODE") ? parse(Int, ENV["SLURM_JOB_CPUS_PER_NODE"]) : 0

###########################
# FUNCTION oa_formulation
###########################
"""Computes the minimum regression error with Ridge regularization subject an explicit
cardinality constraint using cutting-planes.

w^* := arg min  ∑_i ℓ(y_i, x_i^T w) +1/(2γ) ||w||^2
           st.  ||w||_0 = k

INPUTS
  ℓ           - LossFunction to use
  Y           - Vector of outputs. For classification, use ±1 labels
  X           - Array of inputs
  k           - Sparsity parameter
  γ           - ℓ2-regularization parameter
  indices0    - (optional) Initial solution
  ΔT_max      - (optional) Maximum running time in seconds for the MIP solver. Default is 60s
  gap         - (optional) MIP solver accuracy

OUTPUT
  indices     - Indicates which features are used as regressors
  w           - Regression coefficients
  Δt          - Computational time (in seconds)
  status      - Solver status at termination
  Gap         - Optimality gap at termination
  cutCount    - Number of cuts needed in the cutting-plane algorithm
  """
function oa_formulation(ℓ::LossFunction, Y, X, k::Int, γ;
          indices0=findall(x-> x<k/size(X,2), rand(size(X,2))), ΔT_max=60, verbose=false, Gap=0e-3, solver::Symbol=:Gurobi)

  n,p = size(X)

  miop = (solver == :Gurobi) ?    Model(solver=GurobiSolver(MIPGap=Gap, TimeLimit=ΔT_max,
                                    OutputFlag=1*verbose, LazyConstraints=1, Threads=getthreads())) :
                                  Model(solver=CplexSolver(CPX_PARAM_EPGAP=Gap, CPX_PARAM_TILIM=ΔT_max,
                                    CPX_PARAM_SCRIND=1*verbose))
  s0 = zeros(p); s0[indices0].=1

  c0, ∇c0 = inner_op(ℓ, Y, X, s0, γ)

  # Optimization variables
  @variable(miop, s[j=1:p], Bin, start=s0[j])
  @variable(miop, t>=0, start=c0)

  # Objective
  @objective(miop, Min, t)

  # Constraints
  @constraint(miop, sum(s)<=k)

  cutCount=1; bestObj=c0; bestSolution=s0[:];
  @constraint(miop, t>= c0 + dot(∇c0, s-s0))

  #Info array
  bbdata = DataFrame(time=Float64[], node=Int[], obj=Float64[], bestbound=Float64[])

  # Outer approximation method for Convex Integer Optimization (CIO)
  function outer_approximation(cb)
    cutCount += 1
    c, ∇c = inner_op(ℓ, Y, X, getvalue(s), γ)
    if c<bestObj
      bestObj = c; bestSolution=getvalue(s)[:]
    end

    node      = MathProgBase.cbgetexplorednodes(cb)
    obj       = bestObj
    bestbound = MathProgBase.cbgetbestbound(cb)
    push!(bbdata, [time()-t0,node,obj,bestbound])

    @lazyconstraint(cb, t>=c + dot(∇c, s-getvalue(s)))
  end
  addlazycallback(miop, outer_approximation)

  t0=time()
  status = solve(miop)
  Δt = getsolvetime(miop)

  if status != :Optimal
    Gap = 1 - JuMP.getobjbound(miop) /  getobjectivevalue(miop)
  end
  if status == :Optimal
    bestSolution = getvalue(s)[:]
  end
  # Find selected regressors and run a standard linear regression with Tikhonov regularization
  indices = findall(s->s>0.5, bestSolution)
  w = SubsetSelection.recover_primal(ℓ, Y, X[:, indices], γ)

  return indices, w, Δt, status, Gap, cutCount
end

end # module


uniformity(ls::Union{LinkStream,DirectedLinkStream})=1.0

function uniformity(s::Union{StreamGraph,DirectedStreamGraph})
    if length(s.V)==0
        return 0.0
    end
    nom=sum([length(times(s,u) ∩ times(s,v)) for (u,v) in s.V ⊗ s.V])
    denom=sum([length(times(s,u) ∪ times(s,v)) for (u,v) in s.V ⊗ s.V])
    denom != 0 ? nom/denom : 0.0
end


function erdos_renyi(n::Int64, p::Float64; seed::Integer=-1)
    if seed > 0
        Random.seed!(seed)
    end
    g = Graph();
    add_vertex!(g, n);
    m = round(Int, n * (n - 1) / 2);
    ne = rand(Binomial(m, p));
    g = Graph(n, ne);
    return g
end


using LinearAlgebra: ishermitian, issymmetric, mul!
using SparseArrays: AbstractSparseMatrix
using SimpleValueGraphs: OneEdgeValGraph
using Graphs: DefaultDistance

@testset "matrices" begin

    @testset "AdjacencyMatrix" begin

        for G      in (ValGraph, ValDiGraph),
            V      in TEST_VERTEX_TYPES_SMALL,
            E_VALS in TEST_EDGEVAL_TYPES_SMALL,
            gs     in make_testgraphs(is_directed(G) ? SimpleDiGraph{V} : SimpleGraph{V})

            g = DummyValGraph(G{V, Tuple{}, E_VALS}(gs.graph;
                edgeval_init=(s, d) -> rand_sample(E_VALS)
            ))

            @testset "g::$(typeof(g))" begin

                a = adjacency_matrix(g)

                @testset "constructor" begin
                    @test typeof(a) == AdjacencyMatrix{typeof(g)}
                    @test a isa AbstractSparseMatrix{Bool, Int}
                end

                @testset "size(a)" begin
                     @test size(a) == (nv(g), nv(g))
                end

                @testset "getindex" begin
                    for u in 1:nv(g), v in 1:nv(g)
                        @test a[u, v] == has_edge(g, u, v)
                    end
                end

                if !is_directed(g)
                    @testset "ishermitian for undirected" begin
                        @test ishermitian(a)
                    end

                    @testset "issymmetric for undirected" begin
                        @test issymmetric(a)
                    end

                    @testset "adjoint is same matrix for undirected" begin
                        @test adjoint(a) === a
                    end

                    @testset "transpose is same matrix for undirected" begin
                        @test transpose(a) === a
                    end
                end

                @testset "ishermitian" begin
                    @test ishermitian(a) == ishermitian(Matrix(a))
                end

                @testset "issymmetric" begin
                    @test issymmetric(a) == issymmetric(Matrix(a))
                end

                @testset "adjoint" begin
                    @test adjoint(a) == adjoint(Matrix(a))
                end

                @testset "transpose" begin
                    @test transpose(a) == transpose(Matrix(a))
                end

            end
        end

    end

    @testset "mul! with AdjacencyMatrix and Vector" begin

        g1 = DummyValGraph(ValGraph{Int8}(6))
        add_edge!(g1, 2, 3)
        add_edge!(g1, 2, 4)
        add_edge!(g1, 5, 5)
        v1 = zeros(Float64, 6)
        mul!(v1, adjacency_matrix(g1), [1.0, 2.0, 3.0, 4.0, 5.0, 6.0])
        @test v1 == [0.0, 7.0, 2.0, 2.0, 5.0, 0.0]

        g2 = DummyValGraph(ValDiGraph{Int16}(6))
        add_edge!(g2, 1, 1)
        add_edge!(g2, 2, 3)
        add_edge!(g2, 3, 4)
        add_edge!(g2, 4, 3)
        add_edge!(g2, 4, 5)
        v2 = zeros(Int16, 6)
        mul!(v2, adjacency_matrix(g2), [1, 2, 3, 4, 5, 6])
        @test v2 == [1, 3, 4, 8, 0, 0]
    end

    @testset "ValMatrix" begin
        for G      in (ValGraph, ValDiGraph),
            V      in TEST_VERTEX_TYPES_SMALL,
            E_VALS in TEST_EDGEVAL_TYPES_SMALL,
            gs     in make_testgraphs(is_directed(G) ? SimpleDiGraph{V} : SimpleGraph{V})

            g = DummyValGraph(G{V, Tuple{}, E_VALS}(gs.graph;
                edgeval_init=(s, d) -> rand_sample(E_VALS)
            ))

            @testset "g::$(typeof(g))" begin
                for key in allkeys_for_E_VALS(E_VALS)

                    @testset "key = $key, zerovalue=$zv" for zv in (nothing, rand_sample(fieldtype(E_VALS, key)))

                        M = ValMatrix(g, key, zv)

                        @testset "size" begin
                            @test size(M) == (nv(g), nv(g))
                        end

                        @testset "getindex" begin
                            @test all(Iterators.product(vertices(g), vertices(g))) do (s, d)
                                M[s, d] == (has_edge(g, s, d) ? get_edgeval(g, s, d, key) : zv)
                            end
                        end

                        if eltype(M) <: Union{Number, Missing}
                            @testset "ishermitian" begin
                                 @test ishermitian(M) == ishermitian(Matrix(M))
                            end
                        end

                        if eltype(M)  <: Union{Number, Missing}
                            @testset "issymmetric" begin
                                @test issymmetric(M) == issymmetric(Matrix(M))
                            end
                        end

                        if eltype(M) <: Real && !is_directed(g)
                            @testset "adjoint for real, symmetric" begin
                                @test adjoint(M) === M
                            end
                        end

                        if eltype(M) <: Union{Number, Missing}
                            @testset "adjoint" begin
                                @test adjoint(M) == adjoint(Matrix(M))
                            end
                        end

                        if !is_directed(g)
                            @testset "transpose for symmetric" begin
                                @test transpose(M) === M
                            end
                        end

                        if eltype(M) <: Union{Number, Missing}
                            @testset "transpose" begin
                                @test transpose(M) == transpose(Matrix(M))
                            end
                        end

                        @testset "weights" begin

                            @testset "weights(g, $key; zerovalue=$zv)" begin
                                Mw = weights(g, key, zerovalue=zv)
                                @test typeof(M) == typeof(Mw)
                                @test M == Mw
                            end

                            if fieldtype(E_VALS, key) isa Number
                                @testset "weights(g, $key)" begin
                                    Mw = weights(g, key)
                                    @test M isa ValMatrix{fieldtype(E_VALS, key), typeof(g), key}
                                end
                            end

                            if g isa OneEdgeValGraph
                                @testset "weights(g; zerovalue=$zv)" begin
                                    Mw = weights(g, zerovalue=zv)
                                    @test M == Mw
                                end

                                if fieldtype(E_VALS, key) isa Number
                                    @testset "weights(g)" begin
                                        Mw = weights(g)
                                        @test M isa ValMatrix{fieldtype(E_VALS, key), typeof(g), key}
                                    end
                                end
                            end
                        end

                    end
                end
            end
        end
    end

    @testset "weights for value graphs without values" for
        G      in (ValGraph, ValDiGraph),
        V      in TEST_VERTEX_TYPES_SMALL,
        E_VALS in (Tuple{}, NamedTuple{(), Tuple{}}),
        gs     in make_testgraphs(is_directed(G) ? SimpleDiGraph{V} : SimpleGraph{V})

        g = DummyValGraph(G{V, Tuple{}, E_VALS}(gs.graph;
            edgeval_init=(s, d) -> rand_sample(E_VALS))
        )

        @test weights(g) == DefaultDistance(nv(g))
    end

    @testset "weights for graph with two edge values and no key specified" begin

        @test_throws ArgumentError weights(DummyValGraph(ValGraph(3, edgeval_types=(a=Int, b=String))))
        @test_throws ArgumentError weights(DummyValGraph(ValDiGraph(2, edgeval_types=(Int, String))); zerovalue=nothing)
    end

    @testset "convert AdjacencyMatrix to SparseMatrixCSC" begin

        g1 = ValGraph{Int8}(4)
        add_edge!(g1, 1, 2)
        add_edge!(g1, 2, 3)
        add_edge!(g1, 3, 3)
        m1 = AdjacencyMatrix(g1)
        @test SparseMatrixCSC(m1) isa SparseMatrixCSC{Bool, Int}
        @test SparseMatrixCSC(m1) == [0 1 0 0; 1 0 1 0; 0 1 1 0; 0 0 0 0]

        g2 = ValDiGraph{Int16}(4; edgeval_types=(a=String, b=Int))
        add_edge!(g2, 1, 1, ("", 1))
        add_edge!(g2, 1, 2, ("", 2))
        add_edge!(g2, 1, 3, ("", 3))
        add_edge!(g2, 2, 1, ("", 3))
        add_edge!(g2, 3, 2, ("", 0))
        m2 = AdjacencyMatrix(g2)
        @test SparseMatrixCSC(m2) isa SparseMatrixCSC{Bool, Int}
        @test SparseMatrixCSC(m2) == [1 1 1 0; 1 0 0 0; 0 1 0 0; 0 0 0 0]

        g3 = ValOutDiGraph{UInt32}(4; vertexval_types=(String, Int), vertexval_init=undef)
        add_edge!(g3, 1, 1)
        add_edge!(g3, 1, 3)
        add_edge!(g3, 2, 3)
        add_edge!(g3, 3, 1)
        m3 = AdjacencyMatrix(g3)
        @test SparseMatrixCSC(m3) isa SparseMatrixCSC{Bool, Int}
        @test SparseMatrixCSC(m3) == [1 0 1 0; 0 0 1 0; 1 0 0 0; 0 0 0 0]

        g4 = DummyValGraph(ValGraph(0, graphvals=(1, 2, "xyz")))
        m4 = AdjacencyMatrix(g4)
        @test SparseMatrixCSC(m4) isa SparseMatrixCSC{Bool, Int}
        @test SparseMatrixCSC(m4) == Matrix{Bool}(undef, 0, 0)

    end

    @testset "convert ValMatrix to SparseMatrixCSC" begin

        g1 = ValGraph{Int8}(4, edgeval_types=(Int,))
        add_edge!(g1, 1, 2, (12,))
        add_edge!(g1, 2, 3, (23,))
        add_edge!(g1, 3, 3, (33,))
        m1 = ValMatrix(g1, 1, 0)
        @test SparseMatrixCSC(m1) isa SparseMatrixCSC{Int, Int}
        @test SparseMatrixCSC(m1) == [0 12 0 0; 12 0 23 0; 0 23 33 0; 0 0 0 0]

        g2 = ValDiGraph{Int16}(4; edgeval_types=(a=String, b=Int))
        add_edge!(g2, 1, 1, ("", 11))
        add_edge!(g2, 1, 2, ("", 12))
        add_edge!(g2, 1, 3, ("", 13))
        add_edge!(g2, 2, 1, ("", 21))
        add_edge!(g2, 3, 2, ("", 32))
        m2 = ValMatrix(g2, :b, 0)
        @test SparseMatrixCSC(m2) isa SparseMatrixCSC{Int, Int}
        @test SparseMatrixCSC(m2) == [11 12 13 0; 21 0 0 0; 0 32 0 0; 0 0 0 0]

        g3 = ValOutDiGraph{UInt32}(4; edgeval_types=(Int, Float64))
        add_edge!(g3, 1, 1, (11, 1.0))
        add_edge!(g3, 1, 3, (13, 1.3))
        add_edge!(g3, 2, 3, (23, 2.3))
        add_edge!(g3, 3, 1, (31, 3.1))
        m3 = ValMatrix(g3, 2, 0.0)
        @test SparseMatrixCSC(m3) isa SparseMatrixCSC{Float64, Int}
        @test SparseMatrixCSC(m3) == [1.0 0 1.3 0; 0 0 2.3 0; 3.1 0 0 0; 0 0 0 0]

        g4 = ValGraph(0, graphvals=(1, 2, "xyz"), edgeval_types=(Float64, String))
        m4 = ValMatrix(g4, 1, 0.0)
        @test SparseMatrixCSC(m4) isa SparseMatrixCSC{Float64, Int}
        @test SparseMatrixCSC(m4) == Matrix{Float64}(undef, 0, 0)

    end

end # testset


###############
# H-OBCA: Hierarchical Optimization-based Collision Avoidance - a path planner for autonomous parking
# Copyright (C) 2018
# Alexander LINIGER [liniger@control.ee.ethz.ch; Automatic Control Lab, ETH Zurich]
# Xiaojing ZHANG [xiaojing.zhang@berkeley.edu; MPC Lab, UC Berkeley]
# Atsushi SAKAI [atsushisakai@global.komatsu; Komatsu Ltd / MPC Lab]
#
# This program is free software: you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with this program.  If not, see <http://www.gnu.org/licenses/>.
###############
# The paper describing the theory can be found here:
# 	X. Zhang, A. Liniger and F. Borrelli; "Optimization-Based Collision Avoidance"; Technical Report, 2017, [https://arxiv.org/abs/1711.03449]
#   X. Zhang, A. Liniger, A. Sakai and F. Borrelli; "Autonomous  Parking  using  Optimization-Based  Collision  Avoidance"; Technical Report, 2018 [add URL]
###############

###############
# DualMultWS.jl: computes warm starting points for dual multipliers lambda and mu
###############
#
	function DualMultWS(N,nOb,vOb, A, b,rx,ry,ryaw)

	x = zeros(3,N+1)
	x[1,:] = rx
	x[2,:] = ry
	x[3,:] = ryaw

	m = Model(solver=IpoptSolver(hessian_approximation="exact",mumps_pivtol=1e-5,
	                             max_iter=100,tol=1e-5, print_level=3, suppress_all_output="yes"))

	W_ev = ego[2]+ego[4]
	L_ev = ego[1]+ego[3]

	g = [L_ev/2,W_ev/2,L_ev/2,W_ev/2]

	# ofset from X-Y to the center of the ego set
	offset = (ego[1]+ego[3])/2 - ego[3]

	@variable(m, l[1:sum(vOb),1:(N+1)])	# dual multiplier associated with obstacleShape
	@variable(m, n[1:nOb*4,1:(N+1)])	# dual multiplier associated with carShape
	@variable(m, d[1:nOb,1:(N+1)])

	@NLobjective(m, Max,sum(sum(d[i,k] for k=1:N+1) for i=1:nOb ))

	@constraint(m, l .>= 0 )
	@constraint(m, n .>= 0)

	for i in 1:N+1 	# iterate over time steps
		for j = 1 : nOb 	# iterate over obstacles
			Aj = A[sum(vOb[1:j-1])+1 : sum(vOb[1:j]) ,:]	# extract obstacle matrix associated with j-th obstacle
			lj = l[sum(vOb[1:j-1])+1 : sum(vOb[1:j]) ,:]	# extract lambda dual variables associate j-th obstacle
			nj = n[(j-1)*4+1:j*4 ,:] 						# extract mu dual variables associated with j-th obstacle
			bj = b[sum(vOb[1:j-1])+1 : sum(vOb[1:j])]		# extract obstacle matrix associated with j-th obstacle

			# norm(A'*lambda) <= 1
			@constraint(m, (sum(Aj[k,1]*lj[k,i] for k = 1 : vOb[j]))^2 + (sum(Aj[k,2]*lj[k,i] for k = 1 : vOb[j]))^2  <= 1   )

			# G'*mu + R'*A*lambda = 0
			@constraint(m, (nj[1,i] - nj[3,i]) +  cos(x[3,i])*sum(Aj[k,1]*lj[k,i] for k = 1:vOb[j]) + sin(x[3,i])*sum(Aj[k,2]lj[k,i] for k = 1:vOb[j]) == 0  )
			@constraint(m, (nj[2,i] - nj[4,i]) -  sin(x[3,i])*sum(Aj[k,1]*lj[k,i] for k = 1:vOb[j]) + cos(x[3,i])*sum(Aj[k,2]lj[k,i] for k = 1:vOb[j]) == 0  )

			# -g'*mu + (A*t - b)*lambda > 0
			@constraint(m, d[j,i] == -sum(g[k]*nj[k,i] for k = 1:4) + (x[1,i]+cos(x[3,i])*offset)*sum(Aj[k,1]*lj[k,i] for k = 1:vOb[j])
							+ (x[2,i]+sin(x[3,i])*offset)*sum(Aj[k,2]*lj[k,i] for k=1:vOb[j]) - sum(bj[k]*lj[k,i] for k=1:vOb[j]))
		end
	end
	tic()
	solve(m; suppress_warnings=true)
	time = toq();
	# print("Auxillery problem time (to warm start dual variables) = ",time," s \n")

	lp = getvalue(l)'
	np = getvalue(n)'

	return lp,np

	end


module Suri
using Hecke

#= follows Sebastian Posur's idea
  Start with a pseudo_matrix with ideals A_i and rows alpha_i
  plus an additional pseudo-element      A            alpha
  assume, at least for now, the alpha_i are independent.
  assume sum gamma_i alpha_i + gamma alpha = 0
  (gamma are the dependency)
  Now P :=
  (inv(A_i) gamma_i)
  ( inv(A)  gamma  )
  is a (n+1) x 1 pseudo matrix. Using the (pseudo) HNF we find a matrix
  T in Gl(n+1, K) and ideals C_i s.th.
  T*P = H is a pseudo-HNF. By construction, H has 1. row 1, all other rows 0
  and the coeff. ideals C_i
  Then T is an isomorphism between
  sum inv(A_i) + inv(A) and sum C_i
  as zk-modules
  The inverse-transpose of T is an isomorphism between
  sum inv(C_i) and sum A_i + A
  the dual modules to the ones above

  The idea:
  T is an iso between all modules given by the ideals
  The construction forces inv(T) to have the relation as 1. row/col
  The dual of an abstract module is giving by the inverse ideals
  The corresponding map is transpose(inv(T)) = inv(T)'
  Thus it works...
=#

function extend(M::Hecke.PMat, b::Generic.MatSpaceElem{nf_elem}, gamma::Generic.MatSpaceElem{nf_elem})

  zk = base_ring(M)
  nc = ncols(gamma)
  @assert nc == ncols(b)
  p = pseudo_matrix(gamma, vcat(map(inv, coefficient_ideals(M)), [1*zk for i=1:nc]))
  h, T = Hecke.pseudo_hnf_with_transform(p)
  #for n x 1 matrices, the trasform, especially the inverse can more easily be computed
#  @assert prod(coefficient_ideals(p)) ==  (det(T))*(prod(coefficient_ideals(h)))
#  @assert all(T[j,i] in (coefficient_ideals(p)[i])*inv(coefficient_ideals(h)[j]) for i=1:nrows(M) for j=1:nrows(M))
  Ti = inv(T)
#  @assert all(Ti[i,j] in inv(coefficient_ideals(p)[i])*(coefficient_ideals(h)[j]) for i=1:nrows(M) for j=1:nrows(M))
  e = pseudo_matrix((Ti'*hcat(M.matrix', b)')[nc+1:nrows(M)+nc, :], map(inv, coefficient_ideals(h)[nc+1:nrows(M)+nc]))
  return e
end

function Hecke.denominator(P::Hecke.PMat, M::NfOrd)
  l = fmpz(1)
  p = matrix(P)
  for i=1:nrows(P)
    I = coefficient_ideals(P)[i]
    Hecke.assure_2_normal(I.num)
    for j=1:ncols(P)
      l = lcm(l, denominator(simplify(p[i,j]*I)))
    end
  end
  return l
end

function Hecke.simplify(P::Hecke.PMat)
  c = copy(coefficient_ideals(P))
  m = deepcopy(matrix(P))
  for i=1:length(c)
    a, b = Hecke.reduce_ideal2(c[i])
    m[i, :] *= inv(b)
    c[i] = fractional_ideal(order(a), a)
  end
  return pseudo_matrix(m, c)
end

function Hecke.dual(P::Hecke.PMat)
  return pseudo_matrix(inv(P.matrix)', map(inv, coefficient_ideals(P)))
end

end

#= example

Hecke.example("Suri.jl")
Hecke.revise("Suri.jl")

k, a = wildanger_field(3, 13)
m = rand(MatrixSpace(k, 10, 10), 1:10);
m = cat(m,m, dims=(1,2));
b = rand(MatrixSpace(k, 20, 1), 1:10);
S = kernel(hcat(m, b));

m1 = Suri.extend(pseudo_matrix(m'), b, S[2]);

norm(det(m))
norm(det(m1))

b = rand(MatrixSpace(k, 20, 1), 1:10);
S = kernel(hcat(m1.matrix', b));
m2 = Suri.extend(m1, b, S[2]);

norm(det(m2))

=#


export constructtriad
export constructtorus
export constructsphere
export constructcylinder
export constructbox
export constructwhirl
export constructframe
export getplane

"""
    constructtriad(q [; length])

Construct a triad with the given configuration `q` and `length`.
"""
function constructtriad(q::Biquaternion; length::Float64 = 1.0)
    map(x -> x + gettranslation(q), rotate(ℝ³.([[0; 0; 0], [length; 0; 0],
                                                [0; 0; 0], [0; length; 0],
                                                [0; 0; 0], [0; 0; length]]),
                                           getrotation(q)))
end

"""
    constructtorus(q, r, R)

Construct a torus of revolution with the given configuration `q`, the smaller radius `r`
and the bigger radius `R`.
"""
function constructtorus(q::Biquaternion,
                        r::Real,
                        R::Real;
                        segments::Int = 36)
    array = Array{ℝ³,2}(undef, segments, segments)
    for i in 1:segments
        for j in 1:segments
            ϕ = i * 2pi / (segments - 1)
            θ = j * 2pi / (segments - 1)
            x₁ = (R + r * cos(ϕ)) * cos(θ)
            x₂ = (R + r * cos(ϕ)) * sin(θ)
            x₃ = r * sin(ϕ)
            array[i, j] = ℝ³(x₁, x₂, x₃)
        end
    end
    map(x -> x + gettranslation(q), rotate(array, getrotation(q)))
end

"""
    constructsphere(q, radius)

Construct a sphere with the given configuration `q` and `radius`.
"""
function constructsphere(q::Biquaternion,
                         radius::Real;
                         segments::Int = 36)
    array = Array{ℝ³,2}(undef, segments, segments)
    lspace = collect(range(float(-pi), stop = float(pi), length = segments))
    for i in 1:segments
        for j in 1:segments
            ϕ = lspace[i]
            θ = lspace[j] / 2
            array[i, j] = ℝ³(Cartesian(Geographic(ϕ, θ))) * float(radius)
        end
    end
    map(x -> x + gettranslation(q), rotate(array, getrotation(q)))
end

"""
    constructcylinder(q, height, radius)

Construct a cylinder with the given configuration `q`, `height` and `radius`.
"""
function constructcylinder(q::Biquaternion,
                           height::Float64,
                           radius::Float64;
                           segments::Int = 36)
    array = Array{ℝ³,2}(undef, segments, segments)
    lspace = collect(range(float(-pi), stop = float(pi), length = segments))
    for i in 1:segments
        for j in 1:segments
            α = lspace[j]
            x₁ = radius * cos(α)
            x₂ = radius * sin(α)
            x₃ = (i - 1) * height / (segments - 1)
            array[i, j] = ℝ³(x₁, x₂, x₃)
        end
    end
    map(x -> x + gettranslation(q), rotate(array, getrotation(q)))
end

"""
    constructbox(p, q, s)

Construct a box with the given translation `p`, rotation `q` and scale `s`.
"""
function constructbox(p::ℝ³, q::Quaternion, s::ℝ³)
    x, y, z = vec(s) ./ 2
    points = [ℝ³(-x, -y, -z) ℝ³(x, -y, -z) ℝ³(-x, -y, -z) ℝ³(x, -y, -z);
              ℝ³(-x, -y, +z) ℝ³(x, -y, +z) ℝ³(-x, -y, +z) ℝ³(x, -y, +z);
              ℝ³(-x, +y, +z) ℝ³(x, +y, +z) ℝ³(-x, +y, +z) ℝ³(x, +y, +z);
              ℝ³(-x, +y, -z) ℝ³(x, +y, -z) ℝ³(-x, +y, -z) ℝ³(x, +y, -z)]
    map(x -> x + p, rotate(points, q))
end

"""
    constructwhirl(points, [top, [bottom, [s3rotation, [config, [segments]]]]])

Construct a whirl with the given `points` in the base space, `top`, `bottom`, S³
rotation `s3rotation`, configuration `config` and the number of `segments`.
"""
function constructwhirl(points::Array{<:S²,1};
                        top::S¹ = U1(-pi),
                        bottom::S¹ = U1(pi),
                        s3rotation::S³ = Quaternion(1, 0, 0, 0),
                        config::Biquaternion = Biquaternion(ℝ³(0, 0, 0)),
                        segments::Int = 36)
    # Map from S² into its upper hemisphere
    f(b::S²) = begin
        p = Geographic(b)
        r = sqrt((1 - sin(p.θ)) / 2)
        Geographic(r * cos(p.ϕ), r * sin(p.ϕ))
    end
    # Map from S² into S³
    function σ(b::S², f::S¹)
        p = Geographic(b)
        arg = angle(exp(-im * p.ϕ) * sqrt(1 + sin(p.θ)))
        z = exp(im * angle(f)) * exp(-im * (arg + p.ϕ)) * sqrt(1 + sin(p.θ)) / sqrt(2)
        w = exp(im * angle(f)) * exp(-im * arg) * sqrt(1 - sin(p.θ)) / sqrt(2)
        Quaternion(ComplexPlane(z, w)) # σ ⊂ [|z| ≤ 1, w > 0]
    end
    # Map from S² into S³
    function π(b::S², f::S¹)
        p = Geographic(b)
        arg = angle(exp(-im * p.ϕ) * sqrt(1 + sin(p.θ)))
        z = exp(im * angle(f)) * exp(-im * arg) * sqrt(1 - sin(p.θ)) / sqrt(2)
        w = exp(im * angle(f)) * exp(-im * (arg + p.ϕ)) * sqrt(1 + sin(p.θ)) / sqrt(2)
        Quaternion(ComplexPlane(z, w)) # π ⊂ [z > 0, |w| ≤ 1]
    end
    array = Array{ℝ³,2}(undef, segments, length(points))
    lspace = range(angle(top), stop = angle(bottom), length = segments)
    for (i, α) in enumerate(lspace)
        for (j, point) in enumerate(points)
            array[i, j] = σmap(rotate(s3rotation, σ(f(point), U1(α))))
        end
    end
    applyconfig(array, config)
end

"""
    constructframe(circle, [s3rotation, [config, [segments]]])

Construct a frame that is like an S³ cross-section under stereographic projection with the
given `circle` that determines the phase angle in the fiber space, S³ rotation `s3rotation`,
configuration `config` and the number of `segments`. The Clifford bundle does not have a
cross section and so we restrict the base space coordinate chart to its upper hemisphere.
"""
function constructframe(circle::S¹;
                        s3rotation::S³ = Quaternion(1, 0, 0, 0),
                        config::Biquaternion = Biquaternion(ℝ³(0, 0, 0)),
                        segments::Int = 36)
    # Map from S² into its upper hemisphere
    f(b::S²) = begin
        p = Geographic(b)
        r = sqrt((1 - sin(p.θ)) / 2)
        Geographic(r * cos(p.ϕ), r * sin(p.ϕ))
    end
    # Map from S² into S³
    function σ(b::S², f::S¹)
        p = Geographic(b)
        arg = angle(exp(-im * p.ϕ) * sqrt(1 + sin(p.θ)))
        z = exp(im * angle(f)) * exp(-im * (arg + p.ϕ)) * sqrt(1 + sin(p.θ)) / sqrt(2)
        w = exp(im * angle(f)) * exp(-im * arg) * sqrt(1 - sin(p.θ)) / sqrt(2)
        Quaternion(ComplexPlane(z, w)) # σ ⊂ [|z| ≤ 1, w > 0]
    end
    # Map from S² into S³
    function π(b::S², f::S¹)
        p = Geographic(b)
        arg = angle(exp(-im * p.ϕ) * sqrt(1 + sin(p.θ)))
        z = exp(im * angle(f)) * exp(-im * arg) * sqrt(1 - sin(p.θ)) / sqrt(2)
        w = exp(im * angle(f)) * exp(-im * (arg + p.ϕ)) * sqrt(1 + sin(p.θ)) / sqrt(2)
        Quaternion(ComplexPlane(z, w)) # π ⊂ [z > 0, |w| ≤ 1]
    end
    array = Array{ℝ³,2}(undef, segments, segments)
    lspaceϕ = collect(range(-pi, stop = pi, length = segments))
    lspaceθ = collect(range(-pi / 2, stop = pi / 2, length = segments))
    for (i, θ) in enumerate(lspaceθ)
        for (j, ϕ) in enumerate(lspaceϕ)
            array[i, j] = σmap(rotate(s3rotation, σ(f(Geographic(ϕ, θ)), circle)))
        end
    end
    applyconfig(array, config)
end

getplane(p::Array{Float64}, q::Quaternion, s::Array{Float64}) = begin
    plane = Array{Float64,3}(undef, 2, 2, 3)
    plane[1, 1, :] = vec(rotate(ℝ³([0.0; -1.0; 1.0]), q)) .* s + p
    plane[1, 2, :] = vec(rotate(ℝ³([0.0; 1.0; 1.0]), q)) .* s + p
    plane[2, 1, :] = vec(rotate(ℝ³([0.0; -1.0; -1.0]), q)) .* s + p
    plane[2, 2, :] = vec(rotate(ℝ³([0.0; 1.0; -1.0]), q)) .* s + p
    plane
end


######################### Oblivious Algorithm ##################################
# chooses the k features most correlated with the target
function oblivious(A::AbstractMatOrFac, b::AbstractVector, k::Int)
    nzind = partialsortperm(abs.(A'b), 1:k, rev = true)
    x = spzeros(size(b))
    x[nzind] = @view(A[:, nzind]) \ b
    return x
end


@inline function U!(k, U)
  @inbounds for r = 1:k
    U[1,r] = -r
    for j = 2:k
      U[j,r] = U[j-1,r] * ((j-1) - r)/j
    end
  end
  nothing
end

function R!(k, ρ, cache)
  @unpack R = cache
  @inbounds for r = 1:k
    R[1,r] = -r * ρ
    for j = 2:k
      R[j,r] = R[j-1,r] * ((j-1) - r * ρ)/j
    end
  end
  nothing
end

# This functions takes help of D2 array to create backward differences array D
# Ith row of D2 keeps Ith order backward differences (∇ⁱyₙ)

function backward_diff!(cache::OrdinaryDiffEqMutableCache, D, D2, k, flag=true)
  flag && copyto!(D[1], D2[1,1])
  for i = 2:k
    for j = 1:(k-i+1)
      @.. D2[i,j] = D2[i-1,j] - D2[i-1,j+1]
    end
    flag && copyto!(D[i], D2[i,1])
  end
end

function backward_diff!(cache::OrdinaryDiffEqConstantCache, D, D2, k, flag=true)
  flag && (D[1] = D2[1,1])
  for i = 2:k
    for j = 1:(k-i+1)
      D2[i,j] = D2[i-1,j] - D2[i-1,j+1]
    end
    flag && (D[i] = D2[i,1])
  end
end

# this function updates backward difference array D when stepsize gets change
# Formula -> D = D * (R * U)
# and it is taken from the paper -
# Implementation of an Adaptive BDF2 Formula and Comparison with the MATLAB Ode15s paper
# E. Alberdi Celaya, J. J. Anza Aguirrezabala, and P. Chatzipantelidis
function reinterpolate_history!(cache::OrdinaryDiffEqMutableCache, D, R, k)
  @unpack tmp = cache.nlsolver
  fill!(tmp,zero(eltype(D[1])))
  for j = 1:k
    for k = 1:k
      @. tmp += D[k] * R[k,j]
    end
    D[j] .= tmp
    fill!(tmp, zero(eltype(tmp)))
  end
end

function reinterpolate_history!(cache::OrdinaryDiffEqConstantCache, D, R, k)
  tmp = zero(D[1])
  for j = 1:k
    for k = 1:k
      tmp += D[k] * R[k,j]
    end
    D[j] = tmp
  end
end

function calc_R(ρ, k, ::Val{N}) where {N}
  R = zero(MMatrix{N,N,typeof(ρ)})
  @inbounds for r = 1:k
    R[1,r] = -r * ρ
    for j = 2:k
      R[j,r] = R[j-1,r] * ((j-1) - r * ρ)/j
    end
  end
  SArray(R)
end

function update_D!(D, dd, k)
  dd = _vec(dd)
  @views @.. D[:,k+2] = dd - D[:,k+1]
  @views @.. D[:,k+1] = dd
  for i in k:-1:1
    @views @.. D[:,i] = D[:,i] + D[:,i+1]
  end
  return nothing
end

const γₖ = @SVector[sum(1//j for j in 1:k) for k in 1:6]

error_constant(integrator, order) = error_constant(integrator, integrator.alg, order)
function error_constant(integrator, alg::QNDF, k)
  @unpack γₖ = integrator.cache
  κ = alg.kappa[k]
  κ*γₖ[k] + inv(k+1)
end


# Write function to automate plotting of scale-location plot
# - Plots scale-location plot for assessing linear model fits

function plot_scale_loc(model)

    # Extract predicted values from model object
    pred = GLM.predict(model)

    # Extract residuals from model object
    res = GLM.residuals(model)

    # Calculate mean of residuals
    res_mean = StatsBase.mean(res)

    # Calculate length (n) of residuals vector
    n = length(res)

    # Calculate standardised residuals
    stdRes = (res - res_mean * ones(n)) / (Statistics.std(res))

    # Calculate square-root of standardised residuals
    sqrtStdRes = sqrt.(abs.(stdRes))

    # Create plot
    p = Gadfly.plot(x = pred,
                    y = sqrtStdRes,
                    Gadfly.layer(Gadfly.Geom.point),
                    Gadfly.layer(Gadfly.Geom.smooth(method = :loess,
                                                    smoothing = 0.9)),
                    # Change plot aesthetics
         Gadfly.Guide.title("Scale-Location"),
         Gadfly.Guide.xlabel("Fitted values",
                             orientation=:horizontal),
         Gadfly.Guide.ylabel("(|Standardized residuals|)"))

end


using DiscreteExteriorCalculus: Metric, Simplex, Cell, CellComplex, Mesh, sharp,
    subcomplex, orient!, differential_operator_sequence, differential_operator,
    zero_constraint, constant_constraint
using SparseArrays: sparse, spdiagm
using AdmittanceModels: Blackbox, PSOModel, apply_transform, sparse_nullbasis,
    impedance_matrices, admittance_matrices

export electrostatics_blackbox
"""
    electrostatics_blackbox(m::Metric{N}, mesh::Mesh{N, K},
        sources::AbstractVector{<:AbstractVector{Cell{N}}},
        boundary_points::AbstractVector{Cell{N}},
        ϵ::AbstractVector{<:Real}) where {N, K}
    electrostatics_blackbox(m::Metric{N}, mesh::Mesh{N, K},
        sources::AbstractVector{<:AbstractVector{Cell{N}}},
        boundary::CellComplex{N},
        ϵ::AbstractVector{<:Real}) where {N, K}

Each source is a vector of cells with `K == 1` representing a conductor. Discretize the
Poisson equation to find a Blackbox that maps charge to voltage as well as a sparse
constraint matrix that asserts that the voltage is constant on each source.
"""
function electrostatics_blackbox(m::Metric{N}, mesh::Mesh{N, K},
    sources::AbstractVector{<:AbstractVector{Cell{N}}},
    boundary_points::AbstractVector{Cell{N}},
    ϵ::AbstractVector{<:Real}) where {N, K}
    k = 1
    ★★, d₁, ★, d₀ = differential_operator_sequence(m, mesh, "★★d★d", k, true)
    Y = -★★ * d₁ * ★ * spdiagm(0 => ϵ) * d₀
    comp = mesh.primal.complex
    row_inds = [findfirst(isequal(s[1]), comp.cells[k]) for s in sources]
    n = length(sources)
    P = sparse(row_inds, 1:n, ones(n), length(comp.cells[k]), n)
    bbox = Blackbox([0.0], [Y], P, P, collect(1:n))
    conductors = filter(s -> length(s) > 1, sources)
    constraint = vcat(zero_constraint(comp, boundary_points, k),
        [constant_constraint(comp, s, k) for s in conductors]...)
    return bbox, sparse_nullbasis(constraint)
end

electrostatics_blackbox(m::Metric{N}, mesh::Mesh{N, K},
    sources::AbstractVector{<:AbstractVector{Cell{N}}},
    boundary::CellComplex{N},
    ϵ::AbstractVector{<:Real}) where {N, K} =
    electrostatics_blackbox(m, mesh, sources, boundary.cells[1], ϵ)

export get_charge_source
"""
    get_charge_source(comp::CellComplex{N},
        group::AbstractVector{Simplex{N, K}}) where {N, K}

Return a vector of cells representing the points in the given simplices.
"""
get_charge_source(comp::CellComplex{N}, group::AbstractVector{Simplex{N, K}}) where {N, K} =
    subcomplex(comp, group).cells[1]

export magnetostatics_blackbox
"""
    magnetostatics_blackbox(m::Metric{N}, mesh::Mesh{N, K},
        sources::AbstractVector{<:AbstractVector{Cell{N}}},
        boundary_points::AbstractVector{Cell{N}},
        boundary_edges::AbstractVector{Cell{N}},
        μ⁻::AbstractVector{<:Real}, Λ⁻::AbstractVector{<:Real}) where {N, K}
    magnetostatics_blackbox(m::Metric{N}, mesh::Mesh{N, K},
        sources::AbstractVector{<:AbstractVector{Cell{N}}},
        boundary::CellComplex{N}, μ⁻::AbstractVector{<:Real},
        Λ⁻::AbstractVector{<:Real}) where {N, K}

Each source is a vector of cells with `K == 2` representing a current line. In order to get
correct results, the current specified must have divergence 0. Discretize the Maxwell
equations in the Coulomb gauge to find a Blackbox mapping current to the line integral of
the vector potential (for a closed loop this is magnetic flux) as well as a sparse
constraint matrix that asserts that the vector potential is orthogonal to the boundary
and has divergence 0 in the interior.
"""
function magnetostatics_blackbox(m::Metric{N}, mesh::Mesh{N, K},
    sources::AbstractVector{<:AbstractVector{Cell{N}}},
    boundary_points::AbstractVector{Cell{N}},
    boundary_edges::AbstractVector{Cell{N}},
    μ⁻::AbstractVector{<:Real}, Λ⁻::AbstractVector{<:Real}) where {N, K}
    pso, null_basis = electrodynamics_pso(m, mesh, sources, boundary_points,
        boundary_edges, μ⁻, Λ⁻, zeros(size(Λ⁻)), ones(size(Λ⁻)))
    bbox = Blackbox([0.0], [pso.K], pso.P, pso.Q, pso.ports)
    return bbox, null_basis
end

magnetostatics_blackbox(m::Metric{N}, mesh::Mesh{N, K},
    sources::AbstractVector{<:AbstractVector{Cell{N}}},
    boundary::CellComplex{N}, μ⁻::AbstractVector{<:Real},
    Λ⁻::AbstractVector{<:Real}) where {N, K} = magnetostatics_blackbox(m, mesh,
    sources, boundary.cells[1], boundary.cells[2], μ⁻, Λ⁻)

export get_current_source
"""
    get_current_source(comp::CellComplex{N},
        group::AbstractVector{Simplex{N, K}}) where {N, K}

Return a vector of cells representing the lines in the given simplices.
"""
get_current_source(comp::CellComplex{N}, group::AbstractVector{Simplex{N, K}}) where {N, K} =
    orient!(subcomplex(comp, group).cells[2])

export electrodynamics_pso
"""
    electrodynamics_pso(m::Metric{N}, mesh::Mesh{N, K},
        sources::AbstractVector{<:AbstractVector{Cell{N}}},
        boundary_points::AbstractVector{Cell{N}},
        boundary_edges::AbstractVector{Cell{N}},
        μ⁻::AbstractVector{<:Real}, Λ⁻::AbstractVector{<:Real},
        σ::AbstractVector{<:Real}, ϵ::AbstractVector{<:Real}) where {N, K}
    electrodynamics_pso(m::Metric{N}, mesh::Mesh{N, K},
        sources::AbstractVector{<:AbstractVector{Cell{N}}},
        boundary::CellComplex{N},
        μ⁻::AbstractVector{<:Real}, Λ⁻::AbstractVector{<:Real},
        σ::AbstractVector{<:Real}, ϵ::AbstractVector{<:Real}) where {N, K}

Each source is a vector of cells with `K == 2` representing a current line. In order to get
correct results, the current specified must have divergence 0. Discretize the Maxwell
equations in the Coulomb gauge to find a Positive Second Order model mapping current to
voltage as well as a sparse constraint matrix that asserts that the vector potential is
orthogonal to the boundary and has divergence 0 in the interior.
"""
function electrodynamics_pso(m::Metric{N}, mesh::Mesh{N, K},
    sources::AbstractVector{<:AbstractVector{Cell{N}}},
    boundary_points::AbstractVector{Cell{N}},
    boundary_edges::AbstractVector{Cell{N}},
    μ⁻::AbstractVector{<:Real}, Λ⁻::AbstractVector{<:Real},
    σ::AbstractVector{<:Real}, ϵ::AbstractVector{<:Real}) where {N, K}
    k = 2
    ★★, d₁, ★, d₀ = differential_operator_sequence(m, mesh, "★★d★d", k, true)
    K₀ = ★★ * d₁ * ★ * spdiagm(0 => μ⁻) * d₀
    d, ★ = differential_operator_sequence(m, mesh, "d★", k, true)
    K₁ = ★ * spdiagm(0 => Λ⁻)
    G = ★ * spdiagm(0 => σ)
    C = ★ * spdiagm(0 => ϵ)
    comp = mesh.primal.complex
    row_inds = vcat([[findfirst(isequal(c), comp.cells[k]) for c in s] for s in sources]...)
    col_inds = vcat([[i for _ in s] for (i, s) in enumerate(sources)]...)
    P = sparse(row_inds, col_inds, ones(length(row_inds)), length(comp.cells[k]), length(sources))
    pso = PSOModel(K₀ + K₁, G, C, P, P, collect(1:length(sources)))
    interior_inds = findall(i -> !(comp.cells[1][i] in boundary_points), 1:length(comp.cells[1]))
    d★ϵ_constraint = (d * C/maximum(abs.(ϵ)))[interior_inds, :] # only enforce d★ϵA = 0 in the interior
    constraint = vcat(zero_constraint(comp, boundary_edges, k), d★ϵ_constraint)
    return pso, sparse_nullbasis(constraint)
end

electrodynamics_pso(m::Metric{N}, mesh::Mesh{N, K},
    sources::AbstractVector{<:AbstractVector{Cell{N}}},
    boundary::CellComplex{N},
    μ⁻::AbstractVector{<:Real}, Λ⁻::AbstractVector{<:Real},
    σ::AbstractVector{<:Real}, ϵ::AbstractVector{<:Real}) where {N, K} =
    electrodynamics_pso(m, mesh, sources, boundary.cells[1], boundary.cells[2], μ⁻, Λ⁻, σ, ϵ)

export solve_statics
"""
    solve_statics(bbox::Blackbox, null_basis::AbstractMatrix{<:Real},
        source_magnitudes::AbstractVector{<:Real})

Given a Blackbox and constraint matrix from `electrostatics_blackbox` or
`magnetostatics_blackbox`, return the scalar potential and charge or vector potential and
current, respectively.
"""
function solve_statics(bbox::Blackbox, null_basis::AbstractMatrix{<:Real},
    source_magnitudes::AbstractVector{<:Real})
    constrained_bbox = apply_transform(bbox, null_basis)
    field = null_basis * (collect(constrained_bbox.Y[1]) \ collect(constrained_bbox.P * source_magnitudes))
    source = bbox.Y[1] * field
    return field, source
end

export impedance_matrix
"""
    impedance_matrix(bbox::Blackbox, null_basis::AbstractMatrix{<:Real})

Given a Blackbox and constraint matrix from `electrostatics_blackbox` or
`magnetostatics_blackbox`, return the elastance matrix or inductance matrix, respectively.
"""
impedance_matrix(bbox::Blackbox, null_basis::AbstractMatrix{<:Real}) =
    impedance_matrices(apply_transform(bbox, null_basis))[1]

export admittance_matrix
"""
    admittance_matrix(bbox::Blackbox, null_basis::AbstractMatrix{<:Real})

Given a Blackbox and constraint matrix from `electrostatics_blackbox` or
`magnetostatics_blackbox`, return the capacitance matrix or inverse inductance matrix,
respectively.
"""
admittance_matrix(bbox::Blackbox, null_basis::AbstractMatrix{<:Real}) =
    admittance_matrices(apply_transform(bbox, null_basis))[1]

export electric_field
"""
    electric_field(m::Metric{N}, mesh::Mesh{N}, φ::AbstractVector{<:Real}) where N

Given the scalar potential from `solve_statics`, approximate the electric field at each
primal vertex.
"""
electric_field(m::Metric{N}, mesh::Mesh{N}, φ::AbstractVector{<:Real}) where N =
    sharp(m, mesh.primal.complex, -differential_operator(m, mesh, "d", 1, true, φ))

export magnetic_field
"""
    magnetic_field(m::Metric{N}, mesh::Mesh{N}, A::AbstractVector{<:Real}) where N

Given the scalar potential from `solve_statics`, approximate the magnetic field at each
dual vertex.
"""
magnetic_field(m::Metric{N}, mesh::Mesh{N}, A::AbstractVector{<:Real}) where N =
    sharp(m, mesh.dual.complex, differential_operator(m, mesh, "★d", 2, true, A))

export source_density
"""
    source_density(m::Metric{N}, mesh::Mesh{N, K},
        source::AbstractVector{<:Real}, k::Int) where {N, K}

Given the charge or current from `solve_statics`, find the charge density or current
density, respectively.
"""
function source_density(m::Metric{N}, mesh::Mesh{N, K},
    source::AbstractVector{<:Real}, k::Int) where {N, K}
    return differential_operator(m, mesh, "★★★", K-k+1, false, source)
end

export get_material
"""
    get_material(comp::CellComplex{N}, group::AbstractVector{Simplex{N, K}},
        value::Real, k::Int) where {N, K}

Return a vector representing the material distribution that is 0 everywhere except on the
simplices in `group` where it is `value`.
"""
function get_material(comp::CellComplex{N}, group::AbstractVector{Simplex{N, K}},
    value::Real, k::Int) where {N, K}
    material = zeros(length(comp.cells[k]))
    for c in subcomplex(comp, group).cells[k]
        material[findfirst(isequal(c), comp.cells[k])] = value
    end
    return material
end

"""
    get_material(comp::CellComplex, value::Real, k::Int)

Return a vector representing the material distribution that is `value` everywhere.
"""
get_material(comp::CellComplex, value::Real, k::Int) = ones(length(comp.cells[k])) * value

const speed_of_light = 299792458.0
const μ₀ = 4π/1e7
const ϵ₀ = 1/(μ₀ * speed_of_light^2)


#========================================================================

	Steady state solution for an Aiyagari model with a diffusion process

	Translated julia code based on code from Ben Moll's Website:
		http://www.princeton.edu/moll/HACTproject.htm

		For Julia 1.0.0
========================================================================#
using LinearAlgebra, SparseArrays, Plots

# Model parameters
γ = 2 # CRRA utility
α = 1/3 # parameter for cobb-douglas production function
δ = 0.05 # captal depreciation
ρ = 0.05 # discount rate
A_prod =0.3 # term in production function

# Parameters for productivity and the poisson process q
z_1 = .2
z_2 = 2*z_1
z = [z_1 z_2]
λ_1 = 1 # prob of transitioning from state 1 to 2
λ_2 = 1  # state 2 to 1
λ = [λ_1 λ_2]
z_ave = (z_1*λ_2 + z_2*λ_1)/(λ_1 + λ_2) # Average value of z

H = 500 # Number of elements in grid space for a
a_min = 0.0
a_max = 20.0

# Grid spaces for variables
a = LinRange(a_min, a_max, H)
a = convert(Array,a)
da = (a_max-a_min)/(H-1)

aa = [a a]
zz = ones(H,1)*z #check later

# Paramters for the simulation
maxit = 100
Ir = 100
crit = 10^(-6)
critS = 1e-5
Δ = 1000 #step size for HJB

# Spaces for the finite difference terms
Vaf, Vab, c= [zeros(H,2) for i in 1:3]
V_r = zeros(H,2,H)

# Create r grid space
r_min = -0.0499
r_max = 0.049
r_grid = LinRange(r_min, r_max, Ir)

# Set up an initial guess
r=r_grid[1]
KD_1 = (α*A_prod/(r+δ))^(1/(1-α))*z_ave
w =(1-α)*A_prod*(KD_1/z_ave)^α

v0 = zeros(H,2)
v0[:,1]=(w*z[1] .+ max(r,0.01).*a).^(1-γ)/(1-γ)/ρ
v0[:,2]=(w*z[2] .+ max(r,0.01).*a).^(1-γ)/(1-γ)/ρ

# Spaces for other terms
w_r =[]
KD= zeros(1,Ir)
S=[]
adot=[]
V_n=[]
g_r=[]

# Construct the matrix to summarize the evolution of z over time
# this matrix will not change over the finite differences and is based on the poisson process

Aswitch = [-sparse(I,H,H)*λ[1] sparse(I,H,H)*λ[2] ; sparse(I,H,H)*λ[1] -sparse(I,H,H)*λ[2]]

# The main loop, this makes sure markets clear

for ir in 1:Ir
	global r =r_grid[ir]
	global v0 = v0

	println("Main Loop iteration $(ir)")

	KD[ir]=(α*A_prod/(r+δ))^(1/(1-α))*z_ave
	global w = (1-α)*A_prod*KD[ir].^α * z_ave^(-α)

	push!(w_r,w)

	if ir >1
		global v0 = V_r[:,:,ir-1]
	end

	global v=v0

# Inner loop, value function iteration

	for n in 1:maxit
		V=v
		push!(V_n, V)
		# Forward difference
		Vaf[1:H-1,:] = (V[2:H,:]-V[1:H-1,:])/da
		Vaf[H,:] = (w*z .+ r.*a_max).^(-γ)

		# Backward difference
		Vab[2:H,:] = (V[2:H,:]-V[1:H-1,:])/da
		Vab[1,:] = (w*z .+ r.*a_min).^(-γ)

		# caculate consumption and savings
		# First the forward difference case
		cf = Vaf.^(-1/γ)
		sf = w*zz + r.*aa -cf

		# consumption and savings for the backward difference
		cb = Vab.^(-1/γ)
		sb = w*zz + r.*aa -cb

		#Consumption and V' at the steady state
		c0 = w*zz + r.*aa
		Va0 = c0.^(-γ)

		# Setup to the Upwind scheme
		If = sf .> 0
		Ib = sb .< 0
		I0 = (1 .- If .- Ib)

		#Now implement the upwind scheme
		Va_Upwind = Vaf.*If + Vab.*Ib + Va0.*I0

		global c= Va_Upwind.^(-1/γ)
		u = c.^(1-γ)/(1-γ)

		X = -min.(sb,0)/da
		Y = -max.(sf,0)/da + min.(sb,0)/da
		Z = max.(sf,0)/da

		A1 = spdiagm(0=>Y[:,1], -1 => X[2:H,1], 1=> Z[1:H-1,1])
		A2 = spdiagm(0=>Y[:,2], -1 => X[2:H,2], 1=> Z[1:H-1,2])
		AA = [A1 spzeros(H,H); spzeros(H,H) A2]

		global A= AA + Aswitch

		B = (1/Δ + ρ)*sparse(I,2*H,2*H)-A

		u_stacked = [u[:,1]; u[:,2]]
		V_stacked = [V[:,1]; V[:,2]]

		b = u_stacked + V_stacked/Δ

		V_stacked = B\b

		V = [V_stacked[1:H] V_stacked[H+1:2*H]]

		V_change = V-v

		global v = V

		dist = findmax(V_change)[1]

		if n>1 && dist < crit
			println("Value Function Converge Iteration = $(n)")
			break
		end
	end
	# Now solve the Fokker-Planck equation
	AT = A'
	b = zeros(2*H,1)

	# Dirty fix to avoid matrix singularities
	i_fix=1
	b[i_fix] =.1
	row = [zeros(1,i_fix-1) 1 zeros(1,H*2-i_fix)]
	AT[i_fix,:] = row

	# Now solve the linear system
	gg = AT\b
	g_sum = gg'*ones(2*H,1)*da
	gg = gg./g_sum

	global g = [gg[1:H] gg[H+1:2*H]]

	push!(g_r, g)
	push!(adot, w*zz+r.*aa-c)
	V_r[:,:,ir] = v

	push!(S, g[:,1]'*a*da + g[:,2]'*a*da)
end

# Graphs
S_max = findmax(S)[1]
amin1=a_min-0.02
aaa=LinRange(amin1,S_max,Ir)
rrr=LinRange(-0.06,0.06,Ir)
KD_new = (α*A_prod./max.(rrr.+δ,0)).^(1/(1-α)).*z_ave

plot(S,r_grid, xlims=(amin1,0.6),ylims=(-0.06,0.06), color=:blue,
	label="\$S(r)\$", legend=:bottomright, xlabel="K",ylabel="r",grid=false)
plot!(KD_new,rrr, label="\$F_{k}(K,1)-\\delta\$")
plot!(zeros(Ir,1).+a_min,rrr,label="\$a=\\underline{a}\$",color=:yellow,line=:dash)
plot!(aaa,ones(Ir,1).*ρ,label="\$r=\\rho\$",color=:purple,line=:dash)
plot!(aaa,ones(Ir,1).*(-δ),label="\$r=-\\delta\$",color=:green, line=:dash)
png("Asset_supply")


### A Pluto.jl notebook ###
# v0.12.21

using Markdown
using InteractiveUtils

# This Pluto notebook uses @bind for interactivity. When running this notebook outside of Pluto, the following 'mock version' of @bind gives bound variables a default value (instead of an error).
macro bind(def, element)
    quote
        local el = $(esc(element))
        global $(esc(def)) = Core.applicable(Base.get, el) ? Base.get(el) : missing
        el
    end
end

# ╔═╡ bc0bf1e8-8a36-11eb-3cc2-ff079d49df5b
begin
	using WaterLily, PlutoUI
	using LinearAlgebra: norm2
	include("../examples/TwoD_plots.jl")
end;

# ╔═╡ 5a2b2292-8bd9-11eb-1478-75b6bd59dc7b
md"Define the size $L$ and angle $\alpha$ of a square cylinder"

# ╔═╡ 63399f40-8bdd-11eb-35a9-1b23580343f8
@bind L PlutoUI.Slider(10.:30., default=16.)

# ╔═╡ 7e80b328-8bde-11eb-160f-73a67b8db0a2
Text(string("L=",L))

# ╔═╡ 7ac5d8a4-8a43-11eb-2ef0-e3c0aa4737b0
@bind α PlutoUI.Slider(range(0.,π/4,step=π/16), default=0.)

# ╔═╡ f956022e-8bde-11eb-311b-2dfb01626549
Text(α==0 ? "α=0" : string("α=π/",floor(Int,π/α)))

# ╔═╡ 1a9e4234-8bda-11eb-21f0-894c61269b18
md"Click `Start` (and then `stop`) to run (and then pause) the simulation."

# ╔═╡ 2c015c6a-8e81-11eb-01a1-f5b928f308e6
@bind tick PlutoUI.Clock(interval=0.01)

# ╔═╡ d48df7f8-8bd9-11eb-2f0b-1d3f58b046df
md"Define the `Simulation` by setting the size and the signed distance function of the box. Note that you need to use `norm2`, as it plays nicely with ForwardDiff."

# ╔═╡ d35fa7f0-9246-11eb-340e-156c8daaaa97
n,m = 3*2^6,2^7;

# ╔═╡ d9335082-9246-11eb-2c7c-2f3a03365bf9
# n-dimensional box SDF. See https://www.iquilezles.org/
body = AutoBody() do x,t
	x = [cos(α) sin(α); -sin(α) cos(α)] * (x .- m/2) # transform to body coords
	x = abs.(x) .- L                                 # position relative to corner
	norm2(max.(x, 0.))+min(maximum(x),0.)            # SDF
end;

# ╔═╡ 9a7c1caa-9248-11eb-284c-ef6bec18bf65
sim = Simulation((n+2,m+2),[1.,0.],L; body, ν=L/100.);

# ╔═╡ b8e4b74e-8a3a-11eb-27a0-87eea85497d9
begin
	tick
	sim_step!(sim,sim_time(sim)+0.25)
	@inside sim.flow.σ[I] = WaterLily.curl(3,I,sim.flow.u)*sim.L/sim.U
	as_png(flood(sim.flow.σ,clims=(-10,10)))
end

# ╔═╡ Cell order:
# ╟─5a2b2292-8bd9-11eb-1478-75b6bd59dc7b
# ╟─63399f40-8bdd-11eb-35a9-1b23580343f8
# ╟─7e80b328-8bde-11eb-160f-73a67b8db0a2
# ╟─7ac5d8a4-8a43-11eb-2ef0-e3c0aa4737b0
# ╟─f956022e-8bde-11eb-311b-2dfb01626549
# ╟─1a9e4234-8bda-11eb-21f0-894c61269b18
# ╟─2c015c6a-8e81-11eb-01a1-f5b928f308e6
# ╟─b8e4b74e-8a3a-11eb-27a0-87eea85497d9
# ╟─d48df7f8-8bd9-11eb-2f0b-1d3f58b046df
# ╠═bc0bf1e8-8a36-11eb-3cc2-ff079d49df5b
# ╠═d35fa7f0-9246-11eb-340e-156c8daaaa97
# ╠═d9335082-9246-11eb-2c7c-2f3a03365bf9
# ╠═9a7c1caa-9248-11eb-284c-ef6bec18bf65


export quantile_residuals, pearson_residuals

function quantile_residuals(obs::Vector{T}, gas::Model{D, T};
                            initial_params::Matrix{T} = stationary_initial_params(gas)) where {D, T}

    n = length(obs)
    len_ini_par = length(initial_params)
    quant_res = Vector{T}(undef, n - len_ini_par)
    params_fitted = score_driven_recursion(gas, obs; initial_params = initial_params)

    for t in axes(params_fitted[len_ini_par + 1:end - 1, :], 1)
        dist = update_dist(D, params_fitted[len_ini_par + 1:end - 1, :], t)
        # Continuous distributions PIT
        if supertype(D).parameters[2] == Continuous
            prob = cdf(dist, obs[t + len_ini_par])
            quant_res[t] = quantile(Normal(0, 1), prob)
        else # Discrete distributions PIT
            lb = cdf(dist, obs[t + len_ini_par] - 1)
            ub = cdf(dist, obs[t + len_ini_par])
            prob = rand(Uniform(lb, ub))
            quant_res[t] = quantile(Normal(0, 1), prob)
        end

        # Treat possible infinity values
        if quant_res[t] == Inf
            quant_res[t] = 1e5
        elseif quant_res[t] == -Inf
            quant_res[t] = -1e5
        end
    end

    return quant_res
end

function pearson_residuals(obs::Vector{T}, gas::Model{D, T};
                             initial_params::Matrix{T} = stationary_initial_params(gas)) where {D, T}

    n = length(obs)
    len_ini_par = length(initial_params)
    pearson = Vector{T}(undef, n - len_ini_par)
    params_fitted = score_driven_recursion(gas, obs; initial_params = initial_params)

    for t in axes(params_fitted[len_ini_par + 1:end - 1, :], 1)
        dist = update_dist(D, params_fitted[len_ini_par + 1:end - 1, :], t)
        pearson[t] = (obs[t + len_ini_par] - mean(dist))/sqrt(var(dist))
    end

    return pearson
end


function autocorr(x::Vector, lag::Integer = 30)
    ## plot autocorrelation using Winston
    acVals = ones(lag)
    for ii=1:lag
        acVals[ii] = cor(x[1:(end-ii)], x[(1+ii):end])
    end
    Winston.plot(acVals, "-b")
    Winston.oplot(acVals, ".b")
    Winston.ylim(-0.1, 1)
    ## return acVals
end

function autocorr!(x::Vector, lag::Integer = 30)
    ## add autocorrelation plot to existing plot in blue
    acVals = ones(lag)
    for ii=1:lag
        acVals[ii] = cor(x[1:(end-ii)], x[(1+ii):end])
    end
    Winston.oplot(acVals, "-b")
    Winston.oplot(acVals, ".b")
end

function autocorr_red!(x::Vector, lag::Integer = 30)
    ## add autocorrelation plot to existing plot in red
    acVals = ones(lag)
    for ii=1:lag
        acVals[ii] = cor(x[1:(end-ii)], x[(1+ii):end])
    end
    Winston.oplot(acVals, "-r")
    Winston.oplot(acVals, ".r")
end


##
## Bifurcations in Logistic Map
##
## Tomáš Kalvoda, 2015
##

using ProgressMeter, Images, Color

lm(x, a) = max(0.0, min(a*x*(1-x), 1.0))
tobin(x, delta, nbins) = max(1, min(convert(Int64, ceil(x/delta)), nbins))

function cmpreq(b1, b2)
    for j = 1:length(b1)
        if (b1[j] == 0 && b2[j] > 0) || (b1[j] > 0 && b2[j] == 0)
            return false
        end
    end
    return true
end

function fillbins(bins, x, delta, steps, a)
    for j = 1:steps
        bins[tobin(x, delta, length(bins))] = 1
        x = lm(x, a)
    end
    return x
end

function accpts(a, nbins, steps)
    delta = 1.0/nbins
    maxrun = 30
    r = 1

    x = 0.5
    acc1 = zeros(Int, nbins)
    acc2 = zeros(Int, nbins)
    x = fillbins(acc1, x, delta, steps, a)
    x = fillbins(acc2, x, delta, steps, a)

    while !(cmpreq(acc1, acc2)) && r <= maxrun
        acc1 = zeros(Int, nbins)
        acc2 = zeros(Int, nbins)
        x = fillbins(acc1, x, delta, steps, a)
        x = fillbins(acc2, x, delta, steps, a)
        r += 1
    end

    return reverse(acc2)
end

function getmat(nbins, steps, asteps)
    a = asteps
    mat = accpts(a, nbins, steps)

    p = Progress(convert(Int64, 4.0/asteps), 1)

    while a < 4.0
        a += asteps
        mat = [mat accpts(a, nbins, steps) ]
        next!(p)
    end
    return mat
end

function getimage(nbins, steps, asteps)
    mat = getmat(nbins, steps, asteps)
    mat = map(rgb -> RGB{Float64}(1-rgb,1-rgb,1-rgb), 1/maximum(mat)*mat)
    imwrite(convert(Image, mat), "out.png")
end


# ---
# title: 1240. Tiling a Rectangle with the Fewest Squares
# id: problem1240
# author: Tian Jun
# date: 2020-10-31
# difficulty: Hard
# categories: Dynamic Programming, Backtracking
# link: <https://leetcode.com/problems/tiling-a-rectangle-with-the-fewest-squares/description/>
# hidden: true
# ---
#
# Given a rectangle of size `n` x `m`, find the minimum number of integer-sided
# squares that tile the rectangle.
#
#
#
# **Example 1:**
#
# ![](https://assets.leetcode.com/uploads/2019/10/17/sample_11_1592.png)
#
#
#
#     Input: n = 2, m = 3
#     Output: 3
#     Explanation: 3 squares are necessary to cover the rectangle.
#     2 (squares of 1x1)
#     1 (square of 2x2)
#
# **Example 2:**
#
# ![](https://assets.leetcode.com/uploads/2019/10/17/sample_22_1592.png)
#
#
#
#     Input: n = 5, m = 8
#     Output: 5
#
#
# **Example 3:**
#
# ![](https://assets.leetcode.com/uploads/2019/10/17/sample_33_1592.png)
#
#
#
#     Input: n = 11, m = 13
#     Output: 6
#
#
#
#
# **Constraints:**
#
#   * `1 <= n <= 13`
#   * `1 <= m <= 13`
#
#
## @lc code=start
using LeetCode

## add your code here:
## @lc code=end


module ModularIndices

using Base: tail
import Base.getindex
export Mod

include("TupleVector.jl")

const IndexArrayTypes = Union{AbstractRange{Int}, AbstractVector{Int}, TupleVector}

"""
    Mod(idx)

Construct a modular (periodic) index.

# Examples
```jldoctest
julia> A = 1:3
1:3

julia> A[Mod(4)]
1

julia> A[Mod(2:4)]
3-element Array{Int64,1}:
 2
 3
 1

```

"""
struct Mod{S <: Union{IndexArrayTypes, Int}}
    ind::S
end

# Allow non-allocating construction
Mod(i₁::Int, i₂::Int, iₓ::Int...) = Mod(TupleVector((i₁, i₂, iₓ...)))

# The `mod_range` function is modified from https://github.com/JuliaLang/julia/pull/32628
# This does the actual modular arithmetic.
@inline mod_range(i::Int, r::Base.OneTo) = mod1(i, last(r))
@inline mod_range(i::Int, r::AbstractUnitRange) = mod(i-first(r), length(r)) + first(r)
@inline mod_range(i::Int, r::StepRange) = abs(r.step)==1 ? mod(i-minimum(r), length(r)) + minimum(r) :
    throw(ArgumentError("step must be +1 or -1"))

@inline mod_range(i::IndexArrayTypes, r) = TupleVector(ntuple(j -> mod_range(i[j], r), length(i)))

# By construction, `Mod` indices are be in bounds
@inline Base.checkindex(::Type{Bool}, inds::AbstractUnitRange, index::Mod) = true

# The key bit: peel off `Mod`'s and compute the mod given the axis information (in `inds`)
# For example, A[Mod(5)] == A[mod_range(5, axes(A, 1))]
@inline function Base.to_indices(A, inds, I::Tuple{Mod, Vararg{Any}})
    return to_indices(A, inds, (mod_range(I[1].ind, inds[1]), tail(I)...))
end

end # module


module sp

using StaticArrays: SVector
import StaticArrays: push, setindex
import Base: *, +, ^, promote_rule, convert, show, isless, size, getindex

immutable QuasiStatic{N, T} <: AbstractVector{T}
    data::SVector{N, T}
    length::Int

    function QuasiStatic{N, T}() where {N, T}
        data = SVector{N, T}(ntuple(_ -> T(), Val{N}))
        new{N, T}(data, 0)
    end
    QuasiStatic{N, T}(data::SVector{N, T}, length=N) where {N, T} = new{N, T}(data, length)
end

function QuasiStatic{N}(data::SVector{M, T}) where {N, M, T}
    q = QuasiStatic{N, T}()
    for d in data
        q = push(q, d)
    end
    q
end

QuasiStatic(data::SVector{M, T}, length=M) where {M, T} = QuasiStatic{M, T}(data, length)

@inline getindex(v::QuasiStatic, i) = (@boundscheck (i >= 1 && i <= v.length) || throw(BoundsError()); v.data[i])
size(v::QuasiStatic) = (v.length,)
function push(v::QuasiStatic{N}, x) where {N}
    if v.length >= N
        error("Capacity reached")
    end
    QuasiStatic(setindex(v.data, x, v.length + 1), v.length + 1)
end

abstract type PolynomialLike end
abstract type TermLike <: PolynomialLike end
abstract type MonomialLike <: TermLike end
abstract type VariableLike <: MonomialLike end

immutable Variable{Name} <: VariableLike
end

show(io::IO, v::Variable{Name}) where Name = print(io, Name)

immutable Monomial{N, V} <: MonomialLike
    exponents::NTuple{N, Int64}
end

exponents(m::Monomial) = m.exponents
isless(m1::Monomial, m2::Monomial) = variables(m1) < variables(m2) && exponents(m1) < exponents(m2)
format_exponent(e) = e == 1 ? "" : "^$e"

function show(io::IO, m::Monomial)
    for (i, v) in enumerate(variables(m))
        if m.exponents[i] != 0
            print(io, v, format_exponent(m.exponents[i]))
        end
    end
end

variables(::Type{Monomial{N, V}}) where {N, V} = V
variables(m::Monomial) = variables(typeof(m))

convert(::Type{<:Monomial}, v::Variable{Name}) where {Name} = Monomial{1, (Name,)}((1,))

*(v1::Variable{N1}, v2::Variable{N2}) where {N1, N2} = Monomial{2, (N1, N2)}((1, 1))
*(v1::Variable{N}, v2::Variable{N}) where N = Monomial{1, (N,)}((2,))

immutable Term{T, MonomialType <: Monomial} <: TermLike
    coefficient::T
    monomial::MonomialType
end

(::Type{TermType})() where {T, N, V, TermType <: Term{T, Monomial{N, V}}} = TermType(0, Monomial{N, V}(ntuple(_ -> 0, Val{N})))

format_coefficient(c) = c == 1 ? "" : string(c)
show(io::IO, t::Term) = print(io, format_coefficient(t.coefficient), t.monomial)

convert(T::Type{<:Term}, m::Monomial) = T(1, m)
convert(T::Type{<:Term}, v::Variable) = T(Monomial(v))

@generated function convert(::Type{Term{T, Mono1}}, t::Term{T, Mono2}) where {T, Mono1, Mono2}
    args = Any[0 for v in variables(Mono1)]
    for (j, var) in enumerate(variables(Mono2))
        I = find(v -> v == var, variables(Mono1))
        if isempty(I)
            throw(InexactError())
        elseif length(I) > 1
            error("Duplicate variables in destination $Mono1")
        end
        args[I[1]] = :(t.monomial.exponents[$j])
    end
    quote
        Term{$T, $Mono1}(t.coefficient,
            Monomial{$(length(args)), $(variables(Mono1))}($(Expr(:tuple, args...))))
    end
end

immutable Polynomial{T <: Term, V <: AbstractVector{T}} <: PolynomialLike
    terms::V
end

Polynomial(terms::V) where {T <: Term, V <: AbstractVector{T}} = Polynomial{T, V}(terms)
convert(T::Type{<:Polynomial}, t::Term) = T(QuasiStatic(SVector(t)))
convert(T::Type{<:Polynomial}, m::Monomial) = T(Term(m))
convert(T::Type{<:Polynomial}, v::Variable) = T(Term(v))

# convert(T::Type{Polynomial{T, V1}}, p::Polynomial{T, V2}) where {T, V1, V2} = T(convert(V1, p.terms))

convert(T::Type{Polynomial{T1, V1}}, p::Polynomial{T2, V2}) where {T1, T2, V1, V2} = T(convert(V1, p.terms))

function show(io::IO, p::Polynomial)
    if !isempty(p.terms)
        print(io, p.terms[1])
        for i in 2:length(p.terms)
            print(io, " + ", p.terms[i])
        end
    end
end

function promote_rule(::Type{<:MonomialLike}, ::Type{<:MonomialLike})
    Monomial
end

function promote_rule(::Type{<:TermLike}, ::Type{<:TermLike})
    Term
end

@generated function promote_rule(::Type{Term{T, Mono1}}, ::Type{Term{T, Mono2}}) where {T, Mono1, Mono2}
    vars = Tuple(sort(collect(union(Set(variables(Mono1)),
                                    Set(variables(Mono2))))))
    quote
        Term{T, Monomial{$(length(vars)), $(vars)}}
    end
end

function promote_rule(::Type{<:PolynomialLike}, ::Type{<:PolynomialLike})
    Polynomial
end

@generated function promote_rule(::Type{Polynomial{T1, V1}}, ::Type{Polynomial{T2, V2}}) where {T1, T2, V1, V2}
    termtype = promote_rule(T1, T2)
    quote
        Polynomial{$termtype, Vector{$termtype}}
    end
end

function (+)(v1::Variable{Name}, v2::Variable{Name}) where {Name}
    Term(v1) + Term(v2)
end

function (+)(t1::Term{T, Mono}, t2::Term{T, Mono}) where {T, Mono}
    if t1.monomial < t2.monomial
        Polynomial(QuasiStatic(SVector(t1, t2)))
    elseif t1.monomial > t2.monomial
        Polynomial(QuasiStatic(SVector(t2, t1)))
    else
        terms = QuasiStatic{2, Term{T, Mono}}()
        terms = push(terms, Term{T, Mono}(t1.coefficient + t2.coefficient, t1.monomial))
        Polynomial(terms)
        # Polynomial([Term{T, Mono}(t1.coefficient + t2.coefficient, t1.monomial)])
    end
end

@generated function (+)(p1::Polynomial{T1, QuasiStatic{N1, T1}}, p2::Polynomial{T2, QuasiStatic{N2, T2}}) where {T1, N1, T2, N2}
    T = promote_type(T1, T2)
    Q = QuasiStatic{N1 + N2, T}
    quote
        terms = $Q()
        i1 = 1
        i2 = 1
        while i1 <= length(p1.terms) && i2 <= length(p2.terms)
            t1 = convert($T, p1.terms[i1])
            t2 = convert($T, p2.terms[i2])
            if t1.monomial < t2.monomial
                terms = push(terms, t1)
                i1 += 1
            elseif t1.monomial > t2.monomial
                terms = push(terms, t2)
                i2 += 1
            else
                terms = push(terms, $T(t1.coefficient + t2.coefficient,
                             t1.monomial))
                i1 += 1
                i2 += 1
            end
        end
        for i in i1:length(p1.terms)
            terms = push(terms, p1.terms[i])
        end
        for i in i2:length(p2.terms)
            terms = push(terms, p2.terms[i])
        end
        Polynomial(terms)
    end
end

@generated function (*)(m1::Monomial{N1, V1}, m2::Monomial{N2, V2}) where {N1, V1, N2, V2}
    vars = Tuple(sort(collect(union(Set(V1), Set(V2)))))
    args = []
    for (i, v) in enumerate(vars)
        i1 = findfirst(V1, v)
        i2 = findfirst(V2, v)
        if i1 != 0 && i2 != 0
            push!(args, :(m1.exponents[$i1] + m2.exponents[$i2]))
        elseif i1 != 0
            push!(args, :(m1.exponents[$i1]))
        else
            @assert i2 != 0
            push!(args, :(m2.exponents[$i2]))
        end
    end
    Expr(:call, :(Monomial{$(length(args)), $vars}), Expr(:tuple, args...))
end

(*)(t1::Term, t2::Term) = Term(t1.coefficient * t2.coefficient, t1.monomial * t2.monomial)
(+)(m1::Monomial, m2::Monomial) = Term(m1) + Term(m2)

(+)(t1::PolynomialLike, t2::PolynomialLike) = +(promote(t1, t2)...)
(*)(t1::TermLike, t2::TermLike) = *(promote(t1, t2)...)
*(x::Number, m::Monomial) = Term(x, m)
*(x::Number, v::Variable) = x * Monomial(v)
^(v::Variable{Name}, x::Integer) where Name = Monomial{1, (Name,)}((x,))

end


# left.jl
# "special cases" related left vector multiplication like x = y'*A
# The subtlety is that y' is a AdjointAbsVec or a TransposeAbsVec
# which is a subtype of AbstractMatrix but it is really "vector like"
# so we want handle it like a vector (Issue#99)
# So this is an exception to the left multiplication rule by a AbstractMatrix
# that usually makes a WrappedMap.

# x = y'*A ⇐⇒ x' = (A'*y)
"""
    *(x::LinearAlgebra.AdjointAbsVec, A::LinearMap)::AdjointAbsVec

Compute the right-action of the linear map `A` on the adjoint vector `x`
and return an adjoint vector.

## Examples
```jldoctest; setup=(using LinearAlgebra, LinearMaps)
julia> A=LinearMap([1.0 2.0; 3.0 4.0]); x=[1.0, 1.0]; x'A
1×2 Adjoint{Float64,Array{Float64,1}}:
 4.0  6.0
```
"""
Base.:(*)(y::LinearAlgebra.AdjointAbsVec, A::LinearMap) = adjoint(A' * y')

"""
    *(x::LinearAlgebra.TransposeAbsVec, A::LinearMap)::TransposeAbsVec

Compute the right-action of the linear map `A` on the transpose vector `x`
and return a transpose vector.

## Examples
```jldoctest; setup=(using LinearAlgebra, LinearMaps)
julia> A=LinearMap([1.0 2.0; 3.0 4.0]); x=[1.0, 1.0]; transpose(x)*A
1×2 Transpose{Float64,Array{Float64,1}}:
 4.0  6.0
```
"""
Base.:(*)(y::LinearAlgebra.TransposeAbsVec, A::LinearMap) = transpose(transpose(A) * transpose(y))

# multiplication with vector/matrix
const AdjointAbsVecOrMat{T} = Adjoint{T,<:AbstractVecOrMat}
const TransposeAbsVecOrMat{T} = Transpose{T,<:AbstractVecOrMat}

function mul!(x::AbstractMatrix, y::AdjointAbsVecOrMat, A::LinearMap)
    check_dim_mul(x, y, A)
    _unsafe_mul!(x', A', y')
    return x
end

function mul!(x::AbstractMatrix, y::AdjointAbsVecOrMat, A::LinearMap, α::Number, β::Number)
    check_dim_mul(x, y, A)
    _unsafe_mul!(x', conj(α)*A', y', true, conj(β))
    return x
end

function mul!(x::AbstractMatrix, y::TransposeAbsVecOrMat, A::LinearMap)
    check_dim_mul(x, y, A)
    _unsafe_mul!(transpose(x), transpose(A), transpose(y))
    return x
end

function mul!(x::AbstractMatrix, y::TransposeAbsVecOrMat, A::LinearMap, α::Number, β::Number)
    check_dim_mul(x, y, A)
    _unsafe_mul!(transpose(x), α*transpose(A), transpose(y), true, β)
    return x
end


export variables, nvariables, exponents, degree, isconstant, powers, constantmonomial, mapexponents

"""
    monomialtype(p::AbstractPolynomialLike)

Return the type of the monomials of `p`.

    termtype(::Type{PT}) where PT<:AbstractPolynomialLike

Returns the type of the monomials of a polynomial of type `PT`.
"""
monomialtype(::Union{M, Type{M}}) where M<:AbstractMonomial = M
monomialtype(::Union{PT, Type{PT}}) where PT <: APL = monomialtype(termtype(PT))
monomialtype(::Union{AbstractVector{PT}, Type{<:AbstractVector{PT}}}) where PT <: APL = monomialtype(PT)

"""
    variables(p::AbstractPolynomialLike)

Returns the tuple of the variables of `p` in decreasing order. It could contain variables of zero degree, see the example section.

### Examples

Calling `variables(x^2*y)` should return `(x, y)` and calling `variables(x)` should return `(x,)`.
Note that the variables of `m` does not necessarily have nonzero degree.
For instance, `variables([x^2*y, y*z][1])` is usually `(x, y, z)` since the two monomials have been promoted to a common type.
"""
function variables end

"""
    nvariables(p::AbstractPolynomialLike)

Returns the number of variables in `p`, i.e. `length(variables(p))`. It could be more than the number of variables with nonzero degree (see the Examples section of [`variables`](@ref)).

### Examples

Calling `nvariables(x^2*y)` should return at least 2 and calling `nvariables(x)` should return at least 1.
"""
nvariables(::Union{AbstractVariable, Type{<:AbstractVariable}}) = 1

"""
    exponents(t::AbstractTermLike)

Returns the exponent of the variables in the monomial of the term `t`.

### Examples

Calling `exponents(x^2*y)` should return `(2, 1)`.
"""
exponents(t::AbstractTerm) = exponents(monomial(t))
exponents(v::AbstractVariable) = (1,)

"""
    degree(t::AbstractTermLike)

Returns the *total degree* of the monomial of the term `t`, i.e. `sum(exponents(t))`.

    degree(t::AbstractTermLike, v::AbstractVariable)

Returns the exponent of the variable `v` in the monomial of the term `t`.

### Examples

Calling `degree(x^2*y)` should return 3 which is ``2 + 1``.
Calling `degree(x^2*y, x)` should return 2 and calling `degree(x^2*y, y)` should return 1.

"""
degree(t::AbstractTermLike) = sum(exponents(t))

degree(t::AbstractTermLike, var::AbstractVariable) = degree(monomial(t), var)
degree(v::AbstractVariable, var::AbstractVariable) = (v == var ? 1 : 0)
#_deg(v::AbstractVariable) = 0
#_deg(v::AbstractVariable, power, powers...) = v == power[1] ? power[2] : _deg(v, powers...)
#degree(m::AbstractMonomial, v::AbstractVariable) = _deg(v, powers(t)...)

function degree(m::AbstractMonomial, v::AbstractVariable)
    i = findfirst(isequal(v), variables(m))
    if i === nothing || iszero(i)
        0
    else
        exponents(m)[i]
    end
end

"""
    isconstant(t::AbstractTermLike)

Returns whether the monomial of `t` is constant.
"""
isconstant(t::AbstractTermLike) = all(iszero.(exponents(t)))
isconstant(v::AbstractVariable) = false

"""
    powers(t::AbstractTermLike)

Returns an tuple of the powers of the monomial of `t`.

### Examples

Calling `powers(3x^4*y) should return `((x, 4), (y, 1))`.
"""
powers(t::AbstractTermLike) = tuplezip(variables(t), exponents(t))

"""
    constantmonomial(p::AbstractPolynomialLike)

Returns a constant monomial of the monomial type of `p` with the same variables as `p`.

    constantmonomial(::Type{PT}) where {PT<:AbstractPolynomialLike}

Returns a constant monomial of the monomial type of a polynomial of type `PT`.
"""
function constantmonomial end

"""
    mapexponents(f, m1::AbstractMonomialLike, m2::AbstractMonomialLike)

If ``m_1 = \\prod x^{\\alpha_i}`` and ``m_2 = \\prod x^{\\beta_i}`` then it returns the monomial ``m = \\prod x^{f(\\alpha_i, \\beta_i)}``.

### Examples

The multiplication `m1 * m2` is equivalent to `mapexponents(+, m1, m2)`, the unsafe division `_div(m1, m2)` is equivalent to `mapexponents(-, m1, m2)`, `gcd(m1, m2)` is equivalent to `mapexponents(min, m1, m2)`, `lcm(m1, m2)` is equivalent to `mapexponents(max, m1, m2)`.
"""
mapexponents(f, m1::AbstractMonomialLike, m2::AbstractMonomialLike) = mapexponents(f, monomial(m1), monomial(m2))

Base.one(::Type{TT}) where {TT<:AbstractMonomialLike} = constantmonomial(TT)
Base.one(t::AbstractMonomialLike) = constantmonomial(t)
# See https://github.com/JuliaAlgebra/MultivariatePolynomials.jl/issues/82
# By default, Base do oneunit(v::VT) = VT(one(v)).
# This tries to convert a monomial to a variable which does not work.
# The issue here is there is no way to represent the multiplicative identity
# using the variable type. The best we can do is return a monomial even
# if it does not exactly match the definition of oneunit.
Base.oneunit(v::AbstractVariable) = one(v)
Base.oneunit(VT::Type{<: AbstractVariable}) = one(VT)


using Test
using Random
using Distributions
using RandomTree

Random.seed!(0)

function treetest(tree)
    @test typeof(tree) <: FiniteTree
    @test isa(tree, FiniteTree)

    mdeg = maxdegree(tree)

    for i in 1:20
        degseq = RandomTree.degrees(tree)
        walk_seq = cumsum(broadcast(-, degseq, 1))
        @test all(x-> x>=0 && x <= mdeg, degseq)
        @test all(x-> x>=0, walk_seq[1:end-1])
        @test walk_seq[end] == -1
    end
end

@testset "RandomTree.jl" begin
@testset "tree.jl" begin
    @testset "CayleyTree" begin
        tree = CayleyTree(50)
        treetest(tree)
    end

    @testset "BinaryTree" begin
        tree = BinaryTree(50)
        treetest(tree)
    end

    @testset "CatalanTree" begin
        tree = CatalanTree(50)
        treetest(tree)
    end

    @testset "MotzkinTree" begin
        tree = MotzkinTree(50)
        treetest(tree)
    end

    @testset "DAryTree" begin
        for d in 2:5
            tree = DAryTree(50, d)
            treetest(tree)
        end
    end

    @testset "RandomRecursiveTree" begin
        tree = RandomRecursiveTree(50)
        treetest(tree)
    end

    @testset "FullDAryTree" begin
        tree = FullDAryTree(5, 1)
        @test length(tree) == 6
        @test collect(degrees(tree)) == [1, 1, 1, 1, 1, 0]
        tree = FullDAryTree(3, 2)
        @test length(tree) == 2^4-1
        @test collect(degrees(tree)) == [2, 2, 2, 0, 0, 2, 0, 0, 2, 2, 0, 0, 2, 0, 0]
        treetest(tree)
    end

    @testset "Degree sequence" begin
        testseq = [
                    [[0, 0, 1, 2, 1], 3, [1, 2, 1, 0, 0]],
                    [[0, 0, 0, 2, 2], 4, [2, 2, 0, 0, 0]],
                    [[1, 1, 1, 1, 0], 1, [1, 1, 1, 1, 0]],
                    ]

        for case in testseq
            @test RandomTree.find_startindex(case[1]) == case[2]
        end

        for case in testseq
            @test RandomTree.fix_degseq(case[1]) == case[3]
        end

        dist = Geometric(1/2)
        n = 100
        for i in 1:200
            degcount = RandomTree.degrees_count(n, dist, n)
            @test all(x-> x>= 0, degcount)
            @test sum((i-1)*degcount[i] for i in 1:length(degcount)) == n-1
        end

        for d in 2:5
            dist = Binomial(d, 1/d)
            n = 100
            for i in 1:200
                degcount = RandomTree.degrees_count(n, dist, d)
                @test length(degcount) <= d+1
                @test all(x-> x>= 0, degcount)
                @test sum((i-1)*degcount[i] for i in 1:length(degcount)) == n-1
            end
        end

        @test RandomTree.degrees(10, [4,4,1,1]) == vec([0 0 0 0 1 1 1 1 2 3])
        @test RandomTree.degrees(10, [4,3,3]) == vec([0 0 0 0 1 1 1 2 2 2])
    end
end

@testset "walker.jl" begin
    tree = CayleyTree(5)

    @testset "DepthWalker" begin
        walker = DepthWalker(tree)
        @test walk(tree, walker) == [2, 1, 1, 0, 0]
        @test result(walker) ==  [0, 1, 2, 3, 1]

        testseq= [
                    [[1, 2, 1, 0, 0], [0, 1, 2, 3, 2]],
                    [[2, 1, 0, 1, 0], [0, 1, 2, 1, 2]],
                    [[2, 1, 1, 0, 0], [0, 1, 2, 3, 1]],
                    ]

        for (degseq, depth_seq) in testseq
            walker = DepthWalker(tree)
            @test degseq == walk(degseq, walker)
            @test result(walker) == depth_seq
        end
    end

    @testset "SubtreeSizeWalker" begin
        walker = SubtreeSizeWalker(tree)
        @test walk(tree, walker) == [2, 1, 1, 0, 0]
        @test result(walker) ==  [5, 3, 2, 1, 1]

        testseq= [
                    [[1, 2, 1, 0, 0], [5, 4, 2, 1, 1]],
                    [[2, 1, 0, 1, 0], [5, 2, 1, 2, 1]],
                    [[2, 1, 1, 0, 0], [5, 3, 2, 1, 1]],
                    ]

        for (degseq, subtree_size_seq) in testseq
            walker = SubtreeSizeWalker(tree)
            @test degseq == walk(degseq, walker)
            @test result(walker) == subtree_size_seq
        end
    end

    @testset "GraphWalker" begin
        testseq= [
                    [[1, 1, 2, 0, 0], [(1, 2), (2, 3), (3, 4), (3, 5)]],
                    [[2, 1, 1, 0, 0], [(1, 2), (2, 3), (3, 4), (1, 5)]],
                    ]
        for (degseq, ret_edges) in testseq
            walker = GraphWalker(tree)
            @test size(walker.tree_digraph) == size(tree)
            @test degseq == walk(degseq, walker)
            @test RandomTree.edges(result(walker)) == ret_edges
        end
    end
end

@testset "simulator" begin
    SIZE = 101

    trees = [CayleyTree(SIZE), BinaryTree(SIZE)]

    @testset "KcutSimulator" begin
        for tree = trees
            k = 2
            sim = KcutSimulator(tree, k)

            @test repr(sim) != ""

            @test sim.tree === tree
            @test sim.k == k

            r1, r2 = simulation(sim)
            @test  SIZE >= r1 >= r2 >0

            for ret = simulation(sim, 20)
                r1, r2 = ret
                @test  SIZE >= r1 >= r2 >0
            end
        end
    end

    @testset "LogProductSimulator" begin
        # For binary tree of size 5, this is always log(15)
        sim = LogProductSimulator(BinaryTree(5))
        @test simulation(sim) ≈ log(5)+log(3)

        for tree = trees
            for pow = 1:3
                sim = LogProductSimulator(tree, pow)

                @test repr(sim) != ""

                @test sim.tree === tree

                ret = simulation(sim)
                @test ret isa Real
                @test ret > log(SIZE)^pow
            end
        end
    end

    @testset "SizePowerSimulator" begin
        sim = SizePowerSimulator(BinaryTree(5))
        @test simulation(sim) ≈ sqrt(5)+sqrt(3)+3

        for tree = trees
            for pow = 1:3
                sim = SizePowerSimulator(tree, pow)

                @test repr(sim) != ""

                @test sim.tree === tree

                ret = simulation(sim)
                @test ret isa Real
                @test ret > SIZE^pow
            end
        end
    end

    @testset "HeightSimulator" begin
        for tree = trees
            sim = HeightSimulator(tree)

            @test repr(sim) != ""

            ret = simulation(sim)
            @test ret >= 1 && ret <= size(tree)

            for ret = simulation(sim, 20)
                ret = simulation(sim)
                @test ret >= 1 && ret <= size(tree)
            end
        end
    end

    @testset "TotalPathSimulator" begin
        for tree = trees
            sim = TotalPathSimulator(tree)

            @test repr(sim) != ""

            ret = simulation(sim)
            @test ret >= size(tree)-1 && ret <= size(tree)^2

            for ret = simulation(sim, 20)
                ret = simulation(sim)
                @test ret >= 1 && ret <= size(tree)^2
            end
        end
    end
end

@testset "FixedGraph" begin
    tgraph = RandomTree.FixedTreeGraph(6, [(1, 2); (2, 3); (3, 4); (1, 5); (4, 6)])

    sortedge!(tgraph)
    @test edges(tgraph) == [(1, 5), (1, 2), (2, 3), (3, 4), (4, 6)]

    position_array = RandomTree.positions(tgraph)

    @test position_array == [1 2; 3 1; 4 1; 5 1; 0 0; 0 0]
    @test collect(degrees(tgraph)) == [2, 1, 1, 1, 0, 0]
end
end


using Test, StructuralDynamicsODESolvers

include("problems.jl")
include("api.jl")
include("solvers.jl")


export DegreeDominatingSet

struct DegreeDominatingSet end

"""
    update_dominated!(degree_queue, v, dominated, in_dom_set)

Check if a vertex is already dominated.
If not, make it dominated and update `degree_queue` by decreasing
the priority of the vertices adjacent to `v` by 1.
"""
function update_dominated!(
    g::AbstractGraph{T},
    degree_queue::PriorityQueue,
    v::Integer,
    dominated::BitArray{1},
    in_dom_set::BitArray{1}
    ) where T <: Integer

    @inbounds if !dominated[v]
        dominated[v] = true
        if !in_dom_set[v]
            degree_queue[v] -= 1
        end
        @inbounds @simd for u in neighbors(g, v)
            if !in_dom_set[u]
                degree_queue[u] -= ifelse(in_dom_set[u], 0, 1)
            end
        end
    end
end

"""
    dominating_set(g, DegreeDominatingSet())

Obtain a [dominating set](https://en.wikipedia.org/wiki/Dominating_set) using a greedy heuristic.

### Implementation Notes
A vertex is said to be dominated if it is in the dominating set or adjacent to a vertex
in the dominating set.
Initialise the dominating set to an empty set and iteratively choose the vertex that would
dominate the most undominated vertices.

### Performance
Runtime: ``\\mathcal{O}((|V|+|E|)*log(|V|))``
Memory: ``\\mathcal{O}(|V|)``
Approximation Factor: `ln(maximum(degree(g)))+2`
"""
function dominating_set(
    g::AbstractGraph{T},
    alg::DegreeDominatingSet
    ) where T <: Integer

    nvg = nv(g)
    in_dom_set = falses(nvg)
    dominated = falses(nvg)
    degree_queue = PriorityQueue(Base.Order.Reverse, enumerate(degree(g) .+ 1))
    length_ds = 0

    while !isempty(degree_queue) && peek(degree_queue)[2] > 0
        v = dequeue!(degree_queue)
        in_dom_set[v] = true
        length_ds += 1

        update_dominated!(g, degree_queue, v, dominated, in_dom_set)
        for u in neighbors(g, v)
            update_dominated!(g, degree_queue, u, dominated, in_dom_set)
        end
    end

    return LightGraphs.findall!(in_dom_set, Vector{T}(undef, length_ds))
end


using Test
using Qlab: savitzky_golay_filter

# test the innput validity checks
# polynomial order greater than window size
@test_throws AssertionError savitzky_golay_filter(1:10, 5, 5)
# window size must be odd
@test_throws AssertionError savitzky_golay_filter(1:10, 10, 2)
# boundary_mode must be one :nearest or :interpolation
@test_throws AssertionError savitzky_golay_filter(1:10, 11, 2; boundary_mode=:silly)

# a window size of 1 with a zeroth order polynomial should return the data
@test savitzky_golay_filter([1.2], 1, 0) == [1.2]
@test savitzky_golay_filter([1.2, 3.4, 5.6], 1, 0) == [1.2, 3.4, 5.6]

# a line smoothed with a polynomial order of 1 should return itself or `boundary_mode==:interpolation`
@test savitzky_golay_filter(1:10, 3, 1; boundary_mode=:interpolation) ≈ 1:10

# take a low order polynomial and smooth with a higher order polynomial to return the exact value
xpts = range(1,10; length=81)
data = 1.2*xpts.^3 + 3.4*xpts.^2 + 5.6*xpts .+ 7.8

@test savitzky_golay_filter(data, 5, 4) ≈ data
@test savitzky_golay_filter(data, 11, 4) ≈ data

# derivatives are easy to analytically calculate
# we need to scale savitzky_golay_filter by the spacing
deriv1 = 3*1.2*xpts.^2 + 2*3.4*xpts .+ 5.6
@test savitzky_golay_filter(data, 5, 4; deriv_order=1) / (xpts[2] - xpts[1]) ≈ deriv1
deriv2 = 2*3*1.2*xpts .+ 2*3.4
@test savitzky_golay_filter(data, 5, 4; deriv_order=2) / (xpts[2] - xpts[1])^2 ≈ deriv2

# test the :nearest boundary condition
# take 2 points and use a 3 point constant smoothing fit
# first point will be average of [1, 1, 2] = 4/3 and second point will be of [1, 2, 2] = 5/3
@test savitzky_golay_filter([1, 2], 3, 0; boundary_mode=:nearest) ≈ [4/3, 5/3]


# ---
# title: 967. Numbers With Same Consecutive Differences
# id: problem967
# author: Indigo
# date: 2021-02-19
# difficulty: Medium
# categories: Depth-first Search, Breadth-first Search
# link: <https://leetcode.com/problems/numbers-with-same-consecutive-differences/description/>
# hidden: true
# ---
#
# Return all **non-negative** integers of length `n` such that the absolute
# difference between every two consecutive digits is `k`.
#
# Note that **every** number in the answer **must not** have leading zeros
# **except** for the number `0` itself. For example, `01` has one leading zero
# and is invalid, but `0` is valid.
#
# You may return the answer in **any order**.
#
#
#
# **Example 1:**
#
#
#
#     Input: n = 3, k = 7
#     Output: [181,292,707,818,929]
#     Explanation: Note that 070 is not a valid number, because it has leading zeroes.
#
#
# **Example 2:**
#
#
#
#     Input: n = 2, k = 1
#     Output: [10,12,21,23,32,34,43,45,54,56,65,67,76,78,87,89,98]
#
#
# **Example 3:**
#
#
#
#     Input: n = 2, k = 0
#     Output: [11,22,33,44,55,66,77,88,99]
#
#
# **Example 4:**
#
#
#
#     Input: n = 2, k = 1
#     Output: [10,12,21,23,32,34,43,45,54,56,65,67,76,78,87,89,98]
#
#
# **Example 5:**
#
#
#
#     Input: n = 2, k = 2
#     Output: [13,20,24,31,35,42,46,53,57,64,68,75,79,86,97]
#
#
#
#
# **Constraints:**
#
#   * `2 <= n <= 9`
#   * `0 <= k <= 9`
#
#
## @lc code=start
using LeetCode

function nums_same_consec_diff(n::Int, k::Int)
    (k == 0) && return [(10 ^ n - 1) ÷ 9 * i for i in 1:9]
    function nums_same_consec_diff(n::Int, k::Int, pre::Int)
        (pre < 0 || pre > 9) && return String[]
        (n == 0) && return [""]
        n1, n2 = pre - k, pre + k
        r1 = nums_same_consec_diff(n - 1, k, n1)
        r2 = nums_same_consec_diff(n - 1, k, n2)
        [[string(n1) * r for r in r1]; [string(n2) * r for r in r2]]
    end
    res = Int[]
    for i in 1:9
        tmp = nums_same_consec_diff(n - 1, k, i)
        append!(res, parse(Int, string(i) * t) for t in tmp)
    end
    res
end
## @lc code=end


using Plots, LaTeXStrings,JLD
function neighbor_checking(network_,dim,i,j)
    neighbors_list=[]
    if i+1!=dim+1
        push!(neighbors_list,network_[i+1,j])
    end
    if i-1!=0
        push!(neighbors_list,network_[i-1,j])
    end
    if j+1!=dim+1
        push!(neighbors_list,network_[i,j+1])
    end
    if j-1!=0
        push!(neighbors_list, network_[i,j-1])
    end
    return neighbors_list
end
function on_or_blocked(network_,dim,i,j,p)
    if p>rand()
        network_[i,j]=1
    else
        network_[i,j]=-1
    end
    return network_
end
p=0.55
dim=50
network_=zeros(Int,dim,dim)
network_[rand(1:dim),rand(1:dim)]=1 #the first entry
for step in 1:(dim^2)
    operation_list=[]
    for i in 1:dim
        for j in 1:dim
            if network_[i,j]==0 && (1 in neighbor_checking(network_,dim,i,j))
                push!(operation_list,[i,j])
            end
        end
    end
    if length(operation_list)>0
        for i in operation_list
            network_=on_or_blocked(network_,dim,i[1],i[2],p)
        end
    elseif length(operation_list)==0
        break
    end
end
heatmap(network_)
savefig("C:\\Users\\Narges\\Documents\\GitHub\\computational_physics\\chapter4\\Fig\\4.7_p=0.5.png")