using PersistentCohomology
using PersistentCohomology: Span

using Test

using SparseArrays, StructArrays
using LinearAlgebra
using GaloisFields

@testset "vietorisrips" begin

    m = [0.0 0.2 0.9 0.0
         0.2 0.0 0.5 0.0
         0.9 0.5 0.0 0.0
         0.0 0.0 0.0 0.0]

    s = sparse(m)

    cplx = vietorisrips(s, 2)
    @test keys(cplx[1]) == StructArray([(1,), (2,), (3,), (4,)],)
    @test values(cplx[1]) == [0.0, 0.0, 0.0, 0.0]

    @test keys(cplx[2]) == StructArray([(2, 1), (3, 1), (3, 2)])
    @test values(cplx[2]) == [0.2, 0.9, 0.5]

    @test keys(cplx[3]) == StructArray([(3, 2, 1)])
    @test values(cplx[3]) == [0.9]

    @test length(cplx) == 3

    cplx = vietorisrips(s, 1)
    @test !issparse(cplx[1])
    @test keys(cplx[1]) == StructArray([(1,), (2,), (3,), (4,)],)
    @test values(cplx[1]) == [0.0, 0.0, 0.0, 0.0]

    @test keys(cplx[2]) == StructArray([(2, 1), (3, 1), (3, 2)])
    @test values(cplx[2]) == [0.2, 0.9, 0.5]

    @test length(cplx) == 2
end

@testset "persistentcocycles" begin
    GF = @GaloisField 3
    ts = [-pi, -pi/2, 0, pi/2]
    pts = map(t -> [cos(t), sin(t)], ts)

    m = round.([norm(pt1-pt2) for pt1 in pts, pt2 in pts], digits = 4)
    cplx = vietorisrips(sparse(m), 2)
    @test keys(cplx[1]) == StructArray(([1, 2, 3, 4],))
    @test keys(cplx[2]) == StructArray([(2, 1), (3, 1), (3, 2), (4, 1), (4, 2), (4, 3)])
    @test keys(cplx[3]) == StructArray([(3, 2, 1), (4, 2, 1), (4, 3, 1), (4, 3, 2)])

    short = m[1, 2]
    long = m[1, 3]

    cocycles, spans = persistentcocycles(GF, cplx, 1)

    @test spans[1][1] == Span{Float64}(0, Inf)
    @test findnz(values(cocycles[1][1])) == ([1, 2, 3, 4], GF.([1, 1, 1, 1]))
    @test spans[1][2] == Span{Float64}(0, short)
    @test findnz(values(cocycles[1][2])) == ([2], GF.([1]))
    @test spans[1][3] == Span{Float64}(0, short)
    @test findnz(values(cocycles[1][3])) == ([3], GF.([1]))
    @test spans[1][4] == Span{Float64}(0, short)
    @test findnz(values(cocycles[1][4])) == ([4], GF.([1]))
    @test length(cocycles[1]) == 4

    @test spans[2][1] == Span{Float64}(short, long)
    @test findnz(values(cocycles[2][1])) == ([6], GF.([1]))
    @test issparse(cocycles[2][1])
    @test findnz(cocycles[2][1]) == (StructVector([(4, 3)]), GF.([1]))
    @test length(cocycles[2]) == 1
end


using BenchmarkTools
using DataFrames
using CSV
using Statistics
using StatsBase: sample, quantile
using EvoTrees
using EvoTrees: sigmoid, logit
using Plots
using GraphRecipes

# prepare a dataset
features = rand(10_000) .* 5
X = reshape(features, (size(features)[1], 1))
Y = sin.(features) .* 0.5 .+ 0.5
Y = logit(Y) + randn(size(Y))
Y = sigmoid(Y)
𝑖 = collect(1:size(X,1))

# train-eval split
𝑖_sample = sample(𝑖, size(𝑖, 1), replace = false)
train_size = 0.8
𝑖_train = 𝑖_sample[1:floor(Int, train_size * size(𝑖, 1))]
𝑖_eval = 𝑖_sample[floor(Int, train_size * size(𝑖, 1))+1:end]

X_train, X_eval = X[𝑖_train, :], X[𝑖_eval, :]
Y_train, Y_eval = Y[𝑖_train], Y[𝑖_eval]

# linear
params1 = EvoTreeRegressor(
    loss=:linear, metric=:mse,
    nrounds=100, nbins = 100,
    λ = 0.5, γ=0.1, η=0.05,
    max_depth = 4, min_weight = 1.0,
    rowsample=0.5, colsample=1.0)

@time model = grow_gbtree(X_train, Y_train, params1, X_eval = X_eval, Y_eval = Y_eval, print_every_n = 25)
# @btime model = grow_gbtree($X_train, $Y_train, $params1, X_eval = $X_eval, Y_eval = $Y_eval)

# @btime model = grow_gbtree($X_train, $Y_train, $params1, X_eval = $X_eval, Y_eval = $Y_eval, print_every_n = 25, metric=:mae)
@time pred_train_linear = predict(model, X_train)
# @time pred_eval_linear = predict(model, X_eval)
# mean(abs.(pred_train_linear .- Y_train))
# sqrt(mean((pred_train_linear .- Y_train) .^ 2))

x_perm = sortperm(X_train[:,1])
plot(X_train, Y_train, ms = 1, mcolor = "gray", mscolor = "lightgray", background_color = RGB(1, 1, 1), seriestype=:scatter, xaxis = ("feature"), yaxis = ("target"), legend = true, label = "")
plot!(X_train[:,1][x_perm], pred_train_linear[x_perm], color = "navy", linewidth = 1.5, label = "Linear")

params1 = EvoTreeRegressor(
    loss=:linear, metric=:mse,
    nrounds=100, nbins = 100,
    λ = 0.0, γ=0.0, η=0.1,
    max_depth = 2, min_weight = 1.0,
    rowsample=0.5, colsample=1.0)

anim = @animate for i=1:20
    params1.nrounds = (i-1)*5+1
    model = grow_gbtree(X_train, Y_train, params1, X_eval = X_eval, Y_eval = Y_eval, print_every_n = Inf)
    pred_train_linear = predict(model, X_train)
    x_perm = sortperm(X_train[:,1])
    plot(X_train, Y_train, ms = 1, mcolor = "gray", mscolor = "lightgray", background_color = RGB(1, 1, 1), seriestype=:scatter, xaxis = ("feature"), yaxis = ("target"), legend = true, label = "")
    plot!(X_train[:,1][x_perm], pred_train_linear[x_perm], color = "navy", linewidth = 1.5, label = "Linear")
end
gif(anim, "anim_fps1.gif", fps = 1)

# plot tree
function treemat(tree)
    mat = zeros(Int, length(tree), length(tree))
    for i in 1:length(tree)
        if tree[i].split
            mat[i,tree[i].left] = 1
            mat[tree[i].left, i] = 1
            mat[i,tree[i].right] = 1
            mat[tree[i].right, i] = 1
        end
        mat = sparse(mat)
    end
    return mat
end

# plot tree
function nodenames(tree)
    names = []
    for i in 1:length(tree)
        if tree[i].split
            push!(names, "feat: " * string(tree[i].feat) * "\n< " * string(round(tree[i].cond, sigdigits=3)))
        else
            push!(names, "pred: " * string(round(tree[i].pred[1], sigdigits=3)))
        end
    end
    return names
end

tree1 = model.trees[2].nodes
mat = treemat(tree1)
nodes = nodenames(tree1)
p = graphplot(mat, method=:tree, node_weights=ones(length(tree1)) .* 10, names = nodes, linecolor=:grey, nodeshape=:ellipse, nodecolor="#66ffcc")
savefig(p, "tree1.svg")


module BenchFillArrays

using SIMDIndexing

include("../examples/vdot.jl")
# using LinearAlgebra
using FillArrays
using BenchmarkTools

suite = BenchmarkGroup()

arr1 = randn(2^20)
arr2 = Ones{Float64}(length(arr1))

# Bump SIMD width to 16.  It looks like `@simd` macro (or LLVM?) likes
# to unroll loops a lot.  So I have to increase the vector size to
# make the performance comparable.
N = 16

# There is no way `vdot` is faster; but it must not be _slower_.
suite["vdot"] = @benchmarkable vdot($arr1, $arr2, $(Vec{N, Float64}))
suite["sum"] = @benchmarkable sum($arr1)
# suite["dot"] = @benchmarkable dot($arr1, $arr2)

function main()
    global results = run(suite; verbose = true)
    show(IOContext(stdout, :compact => false), results)

    println()
    println()
    println("Benchmark result is stored in $(@__MODULE__).results")

    println("vdot vs sum")
    display(judge(minimum.((results["vdot"], results["sum"]))...))
end

end  # module
BenchFillArrays.suite


module CLagrangianFESpacesTests

using Test
using Gridap.ReferenceFEs
using Gridap.Geometry
using Gridap.TensorValues
using Gridap.Arrays
using Gridap.FESpaces
using Gridap.CellData

domain = (0,1,0,1)
partition = (2,2)
grid = CartesianGrid(domain,partition)

x = collect1d(get_node_coordinates(grid))

V = CLagrangianFESpace(Float64,grid)
matvecdata = ([],[],[])
matdata = ([],[],[])
vecdata = ([],[])
test_single_field_fe_space(V,matvecdata,matdata,vecdata)

@test get_cell_dofs(V) === get_cell_nodes(grid)
@test V.dof_to_node == collect(1:num_nodes(grid))
@test V.dof_to_comp == ones(Int,num_nodes(grid))
@test V.node_and_comp_to_dof == V.dof_to_node

u(x) = x[1]+x[2]
uh = interpolate(u,V)
uhx = get_free_values(uh)
ux = u.(x)
@test uhx ≈ ux

V = CLagrangianFESpace(VectorValue{2,Float64},grid)
test_single_field_fe_space(V,matvecdata,matdata,vecdata)

@test get_cell_dofs(V) == [[1,3,7,9,2,4,8,10], [3,5,9,11,4,6,10,12], [7,9,13,15,8,10,14,16], [9,11,15,17,10,12,16,18]]
@test V.dof_to_node == [1, 1, 2, 2, 3, 3, 4, 4, 5, 5, 6, 6, 7, 7, 8, 8, 9, 9]
@test V.dof_to_comp == [1, 2, 1, 2, 1, 2, 1, 2, 1, 2, 1, 2, 1, 2, 1, 2, 1, 2]
@test V.node_and_comp_to_dof == VectorValue{2,Int}[(1,2),(3,4),(5,6),(7,8),(9,10),(11,12),(13,14),(15,16),(17,18)]

u(x) = VectorValue(x[1]+x[2],x[2])
uh = interpolate(u,V)
uhx = get_free_values(uh)
ux = u.(x)
@test uhx ≈ collect1d(reinterpret(ux))

end # module


# @with_kw struct D_Linear
#     #C_e.jl
#     D_ce::Function = z -> zeros(length(z))

#     #Phie.jl
#     D_ϕe_neg::Function = (Lneg,σ_eff_Neg,κ_eff_Neg,ν_neg,ν_pos,pt,CC_A) -> @. (Lneg*(σ_eff_Neg/κ_eff_Neg)*(1-cosh(ν_neg*pt/Lneg)) - pt*ν_neg*sinh(ν_neg))/(CC_A*(κ_eff_Neg+σ_eff_Neg)*sinh(ν_neg)*ν_neg) + (Lneg*(cosh(ν_neg)-cosh(ν_neg*(Lneg-pt)/Lneg)/(CC_A*κ_eff_Neg*(κ_eff_Neg+σ_eff_Neg)*sinh(ν_neg)*ν_neg))) #Lee. Eqn. 4.22
#     D_ϕe_sep::Function = (Lneg,σ_eff_Neg,κ_eff_Neg,ν_neg,ν_pos,pt,CC_A) -> @. (CellData.Neg.L - pt)/(CC_A*κ_eff_Sep) + (CellData.Neg.L*((1-σ_eff_Neg/κ_eff_Neg)*tanh(ν_neg/2)-ν_neg))/(CC_A*(κ_eff_Neg+σ_eff_Neg)*ν_neg) #Lee. Eqn. 4.23
#     D_ϕe_pos::Function = (Lneg,σ_eff_Neg,κ_eff_Neg,ν_neg,ν_pos,pt,CC_A) -> @. -CellData.Sep.L/(CC_A*κ_eff_Sep) + (CellData.Neg.L*((1-σ_eff_Neg/κ_eff_Neg)*tanh(ν_neg/2)-ν_neg))/(CC_A*(κ_eff_Neg+σ_eff_Neg)*ν_neg) - (CellData.Pos.L*(1+(σ_eff_Pos/κ_eff_Pos)*cosh(ν_pos)) + CellData.Pos.L*cosh((CellData.Neg.L+CellData.Sep.L-pt)*ν_pos/CellData.Pos.L) + CellData.Pos.L*((σ_eff_Pos/κ_eff_Pos)*cosh((Ltot-pt)*ν_pos/CellData.Pos.L)))/(CC_A*(κ_eff_Pos+σ_eff_Pos)*sinh(ν_pos)*ν_pos) + (CellData.Neg.L + CellData.Sep.L - pt)/(CC_A*((κ_eff_Pos+σ_eff_Pos)))  #Lee. Eqn. 4.24

#     #Flux.jl
#     D_j::Function = (Lneg,σ_eff_Neg,κ_eff_Neg,ν_neg,ν_pos,pt,CC_A) -> @. ν*(σ_eff*cosh(ν*z)+κ_eff*cosh(ν*(z-1)))/(as*F*Electrode.L*CC_A*(κ_eff+σ_eff)*sinh(ν))

#     #C_se.jl
#     D_cse::Function = z -> zeros(length(z))

#     #Phi_s.jl
#     D_ϕs::Function = (Lneg,σ_eff_Neg,κ_eff_Neg,ν_neg,ν_pos,pt,CC_A) -> @. -Electrode.L*(κ_eff*(cosh(ν)-cosh(z-1)*ν))/(CellData.Const.CC_A*σ_eff*(comb_cond_eff)*ν*sinh(ν))-Electrode.L*(σ_eff*(1-cosh(z*ν)+z*ν*sinh(ν)))/(CellData.Const.CC_A*σ_eff*(comb_cond_eff)*ν*sinh(ν)) #Transfer Function - eq. 4.19

#     #Phi_se.jl
#     D_ϕse::Function = (Lneg,σ_eff_Neg,κ_eff_Neg,ν_neg,ν_pos,pt,CC_A) -> @. Electrode.L/(CC_A*ν*sinh(ν))*((1/κ_eff)*cosh(ν*z)+(1/σ_eff)*cosh(ν*(z-1))) #Transfer Function - eq. 4.14

# end


module egrss
using LinearAlgebra

export generators, symv, gemv, potrf, ldl, trmv, trsv, trtri, trtri2, trnrms, full, tril

"""
    generators(t, p)

Computes generator representation of p'th order spline kernel matrix generated
by a strictly monotonic vector t of length n.

Ut,Vt = generators(t,p) returns two matrices Ut and Vt of size p-by-n
(with p > 0) such that K = tril(Ut'*Vt) + triu(Vt'*Ut,1) is the kernel
matrix with elements

    K[i,j] = sum_{k=0}^{p-1} (-1)^k/(factorial(p-1-k)*factorial(p+k)
                *(t[i]*t[j])^(p-1-k)*min(t[i],t[j])^(2*k+1)

where t is a nonnegative vector.

# Example

```
t = range(0,1,101)
Ut, Vt = generators(t, 2)
```
"""
function generators(t, p)
    Ut = repeat(t',p,1).^collect(p-1:-1:0)./map(factorial,p-1:-1:0)
    Vt = ((-1).^(0:p-1)).*(repeat(t',p,1).^collect(p:2*p-1)./map(factorial,p:2*p-1))
    return Ut,Vt
end

"""
    full(Ut, Vt[, d])

Forms dense symmetric matrix from generator representation.

K = full(Ut,Vt) forms the symmetric matrix

    K = tril(Ut'*Vt) + triu(Vt'*Ut,1).

K = full(Ut,Vt,d) forms the symmetric matrix

    K = tril(Ut'*Vt) + triu(Vt'*Ut,1) + diag(d).
"""
function full(Ut, Vt, d=Nothing)
    K = tril(Ut'*Vt,-1)
    K = K + K' + Diagonal(sum(Ut.*Vt,dims=1)[:])
    if d !== Nothing
        if isa(d,Number)
            K = K + Diagonal(d*ones(eltype(d),size(Ut,2)))
        else
            K = K + Diagonal(d[:])
        end
    end
    return Symmetric(K)
end

"""
    full_tril(Ut, Wt[, d])

Forms dense lower triangular matrix from generator representation.

L = full_tril(Ut,Wt) forms the lower triangular matrix

    L = tril(Ut'*Wt)

L = full_tril(Ut,Wt,d) forms the lower triangular matrix

    L = tril(Ut'*Wt,-1) + diag(d).
"""
function full_tril(Ut, Wt, c=Nothing)
    if c === Nothing
        return LowerTriangular(tril(Ut'*Wt))
    else
        if isa(c,Number)
            c = c*ones(eltype(c),size(Ut,2))
        end
        return LowerTriangular(tril(Ut'*Wt,-1) + Diagonal(c[:]))
    end
end

"""
    gemv(Ut, Vt, Pt, Qt, x)

Computes matrix-vector product A*x where A is an extended generator
representable semiseparable matrix given by

   A = tril(Ut'*Vt) + triu(Pt'*Qt,1).

# Example

```
b = gemv(Ut,Vt,Pt,Qt,x)
```
"""
function gemv(Ut,Vt,Pt,Qt,x)
    p, m = size(Ut)
    q, n = size(Qt)
    min_mn = min(m,n)

    b = [x[1:min_mn]; zeros(eltype(Ut),max(0,m-n))]
    v = zeros(eltype(Ut),size(Ut,1))
    q = Qt[:,1:min_mn]*x[1:min_mn]

    for k = 1:min_mn
        v = v + Vt[:,k]*b[k]
        q = q - Qt[:,k]*b[k]
        b[k] = Ut[:,k]'*v + Pt[:,k]'*q
    end

    if n > m
        b = b + Pt'*(Qt[:,min_mn+1:end]*x[min_mn+1:end])
    elseif m > n
        b[min_mn+1:end] = Ut[:,min_mn+1:end]'*(Vt*x)
    end

    return b
end

"""

Computes matrix-vector product A*x where A is a symmetric and extended generator
representable semiseparable matrix given by

    A = tril(Ut'*Vt) + triu(Vt'*Ut,1)

# Example

```
b = symv(Ut,Vt,x)
```
"""
function symv(Ut,Vt,x)
    p, n = size(Ut)
    b = copy(x)
    z = zeros(eltype(Ut),p)
    y = Ut*x
    for k = 1:n
        z = z + Vt[:,k]*b[k]
        y = y - Ut[:,k]*b[k]
        b[k] = Ut[:,k]'*z + Vt[:,k]'*y
    end
    return b
end

"""
    potrf(Ut, Vt[, d])

Computes implicit Cholesky factorization of the sum of a symmetric extended
generator representable semiseparable matrix and a diagonal matrix.

Wt = potrf(Ut,Vt) computes a matrix Wt such that L = tril(Ut'*Wt) is
the Cholesky factor of the symmetric matrix A = tril(Ut'*Vt)+triu(Vt'*Ut,1),
i.e., A = L*L'.

Wt,c = potrf(Ut,Vt,d) computes a matrix Wt and a vector c such that
L = tril(Ut'*Wt,-1) + diag(c) is the Cholesky factor of the symmetric matrix
A = tril(Ut'*Vt) + triu(Vt'*Ut,1) + diag(d), i.e., A = L*L'. The input d must
either be a vector of length size(Ut,2) or a scalar; in the latter case diag(d)
is interpreted as the identity matrix scaled by d.
"""
function potrf(Ut,Vt,d=Nothing)
    p, n = size(Ut)
    Wt = copy(Vt)
    P = zeros(eltype(Ut),p,p)
    if d === Nothing
        for k = 1:n
            Wt[:,k] = Wt[:,k] - P*Ut[:,k]
            Wt[:,k] = Wt[:,k]/sqrt(Ut[:,k]'*Wt[:,k])
            P = P + Wt[:,k]*Wt[:,k]'
        end
        return Wt
    else
        if isa(d,Number)
            c = d*ones(eltype(d),n)
        else
            c = copy(d)
        end
        for k = 1:n
            Wt[:,k] = Wt[:,k] - P*Ut[:,k]
            c[k] = sqrt(Ut[:,k]'*Wt[:,k] + c[k])
            Wt[:,k] = Wt[:,k]/c[k]
            P = P + Wt[:,k]*Wt[:,k]'
        end
        return Wt, c
    end
end

"""
    ldl(Ut, Vt[, d])

Computes implicit LDL factorization of the sum of a symmetric extended
generator representable semiseparable matrix and a diagonal matrix.

Wt,c = ldl(Ut,Vt) computes a matrix Wt and a vector c such that the LDL
factorization of the symmetric matrix A = tril(Ut'*Vt)+triu(Vt'*Ut,1) + diag(d)
is given by L = tril(Ut'*Wt,-1)+I and D = diag(c), i.e., A = L*D*L'.

Wt,c = ldl(Ut,Vt,d) computes a matrix Wt and a vector c such that the LDL
factorization of the symmetric matrix A = tril(Ut'*Vt)+triu(Vt'*Ut,1) is given
by L = tril(Ut'*Wt,-1)+I and D = diag(c), i.e., A = L*D*L'.
"""
function ldl(Ut,Vt,d=Nothing)
    p, n = size(Ut)
    Wt = copy(Vt)
    P = zeros(eltype(Ut),p,p)
    if d === Nothing
        c = zeros(eltype(Ut),n);
        for k = 1:n
            Wt[:,k] = Wt[:,k] - P*Ut[:,k]
            c[k] = Ut[:,k]'*Wt[:,k]
            P = P + (Wt[:,k]*Wt[:,k]')/c[k]
            Wt[:,k] = Wt[:,k]/c[k]
        end
    else
        if isa(d,Number)
            c = d*ones(eltype(d),n)
        else
            c = copy(d)
        end
        for k = 1:n
            Wt[:,k] = Wt[:,k] - P*Ut[:,k]
            c[k] = Ut[:,k]'*Wt[:,k] + c[k]
            P = P + (Wt[:,k]*Wt[:,k]')/c[k]
            Wt[:,k] = Wt[:,k]/c[k]
        end
    end
    return Wt, c
end

"""
Computes matrix-vector product L*x or L'*x where L is a lower triangular
extended generator representable semiseparable or quasi-separable matrix
defined in terms of matrices Ut and Wt, which are of size p-by-n (with p > 0),
and in the quasi-separable case, a vector c of length n.

b = trmv(Ut,Wt,x) computes b = L*x with L = tril(Ut'*Wt).

b = trmv(Ut,Wt,x,'N') is the same as b = trmv(Ut,Wt,x).

b = trmv(Ut,Wt,x,'T') computes b = L'*x with L = tril(Ut'*Wt).

b = trmv(Ut,Wt,c,x) computes b = L*x with L = tril(Ut'*Wt,-1) + diag(c).

b = trmv(Ut,Wt,c,x,'N') is the same as b = trmv(Ut,Wt,c,x).

b = trmv(Ut,Wt,c,x,'T') computes b = L'*x with L = tril(Ut'*Wt,-1) + diag(c).
"""
function trmv(Ut,Wt,args...)
    p, n = size(Ut)

    if length(args) == 3
        c = args[1]
        x = args[2]
        trans = args[3]
    elseif length(args) == 2
        if args[2] isa Char
            c = Nothing
            x = args[1]
            trans = args[2]
        else
            c = args[1]
            x = args[2]
            trans = 'N'
        end
    elseif length(args) == 1
        c = Nothing
        x = args[1]
        trans = 'N'
    else
        error("Invalid number of input arguments.")
    end

    b = copy(x)
    z = zeros(eltype(Ut),p)
    if c === Nothing
        if trans == 'N'
            for k = 1:n
                z = z + Wt[:,k]*x[k]
                b[k] = Ut[:,k]'*z
            end
        elseif trans == 'T'
            for k = n:-1:1
                z = z + Ut[:,k]*x[k]
                b[k] = Wt[:,k]'*z
            end
        else
            error("Expected character 'N or 'T'.")
        end
    else
        if trans == 'N'
            for k = 1:n
                b[k] = c[k]*b[k] + Ut[:,k]'*z
                z = z + Wt[:,k]*x[k]
            end
        elseif trans == 'T'
            for k = n:-1:1
                b[k] = c[k]*b[k] + Wt[:,k]'*z
                z = z + Ut[:,k]*x[k]
            end
        else
            error("Expected character 'N or 'T'.")
        end
    end

    return b
end

"""
Solves the equation L*x = b or L'*x = b where L is a lower triangular extended
generator representable semiseparable or quasi-separable matrix defined in terms
of matrices Ut and Wt, which are of size p-by-n (with p > 0), and in the
quasi-separable case, a vector c of length n.

x = trsv(Ut,Wt,b) solves L*x = b with L = tril(Ut'*Wt).

x = trsv(Ut,Wt,b,'N') is the same as x = trsv(Ut,Wt,b).

x = trsv(Ut,Wt,b,'T') solves L'*x = b with L = tril(Ut'*Wt).

x = trsv(Ut,Wt,c,b) solves L*x = b with L = tril(Ut'*Wt,-1) + diag(c).

x = trsv(Ut,Wt,c,b,'N') is the same as x = trsv(Ut,Wt,c,b).

x = trsv(Ut,Wt,c,b,'T') solves L'*x = b with L = tril(Ut'*Wt,-1) + diag(c).
"""
function trsv(Ut,Wt,args...)
    p, n = size(Ut)

    if length(args) == 3
        c = args[1]
        b = args[2]
        trans = args[3]
    elseif length(args) == 2
        if args[2] isa Char
            c = Nothing
            b = args[1]
            trans = args[2]
        else
            c = args[1]
            b = args[2]
            trans = 'N'
        end
    elseif length(args) == 1
        c = Nothing
        b = args[1]
        trans = 'N'
    else
        error("Invalid number of input arguments.")
    end

    x = copy(b)
    z = zeros(eltype(Ut),p);
    if c === Nothing
        if trans == 'N'
            for k = 1:n
                x[k] = (x[k]-Ut[:,k]'*z)/(Wt[:,k]'*Ut[:,k])
                z = z + Wt[:,k]*x[k]
            end
        elseif trans == 'T'
            for k = n:-1:1
                x[k] = (x[k]-Wt[:,k]'*z)/(Wt[:,k]'*Ut[:,k])
                z = z + Ut[:,k]*x[k]
            end
        else
            error("Expected character 'N or 'T'.")
        end
    else
        if trans == 'N'
            for k = 1:n
                x[k] = (x[k]-Ut[:,k]'*z)/c[k]
                z = z + Wt[:,k]*x[k]
            end
        elseif trans == 'T'
            for k = n:-1:1
                x[k] = (x[k]-Wt[:,k]'*z)/c[k]
                z = z + Ut[:,k]*x[k]
            end
        else
            error("Expected character 'N or 'T'.")
        end
    end

    return x
end

"""
    trtri(Ut, Wt, c)

Computes an implicit inverse of a lower triangular matrix of the form

    L = tril(Ut'*Wt,-1) + diag(c).

Yt,Zt = trtri(Ut,Wt,c) computes Yt and Zt such that

    L^{-1} = tril(Yt'*Zt,-1) + diag(1./c).
"""
function trtri(Ut,Wt,c)

    p, n = size(Ut)
    Yt = copy(Ut)
    Zt = copy(Wt)

    for k = 1:p
        Yt[k,:] = trsv(Ut,Wt,c,Yt[k,:]')';
    end

    Zt[:,end] .*= 0.0;
    for k = 1:p
        Zt[k,:] = trsv(Ut,Wt,c,Zt[k,:]','T')';
    end
    Zt = (Zt[:,1:end-1]*Ut[:,1:end-1]' - I)\Zt;

    return Yt,Zt
end

"""
    trtri2(Ut, Wt[, c])

Computes the explicit inverse of a lower triangular matrix of either the form
L = tril(Ut'*Wt) or L = tril(Ut'*Wt,-1) + diag(c).

"""
function trtri2(Ut,Wt,c=Nothing)
    p, n = size(Ut)
    Linv = zeros(eltype(Ut),n,n)
    e = zeros(eltype(Ut),n)
    e[1] = 1
    if c === Nothing
        for k = 1:n
            Linv[k:end,:] = trsv(Ut,Wt,e[1:end+1-k])
        end
    else
        for k = 1:n
            Linv[k:end,k] = trsv(Ut[:,k:end],Wt[:,k:end],c[k:end],e[1:end+1-k])
        end
    end
    return LowerTriangular(Linv)
end

"""
    trnrms(Ut, Wt, c)

Computes the squared column norms of a lower triangular matrix of the form
L = tril(Ut'*Wt,-1) + diag(c) where Ut and Wt are of size p-by-n (with p > 0),
and c is a positive vector of length n.

b = trnrms(Ut,Wt,c) computes a vector b and with entries
b(i) = norm(L(:,i))^2 where L = tril(Ut'*Wt,-1) + diag(c).
"""
function trnrms(Ut,Wt,c)
    p, n = size(Ut)
    colnrms = zeros(eltype(Ut),n)
    P = zeros(eltype(Ut),p,p);
    for k = n:-1:1
        colnrms[k] = c[k]^2 + Wt[:,k]'*P*Wt[:,k];
        P = P + Ut[:,k]*Ut[:,k]';
    end
    return colnrms
end

end


function knuthshuffle!(r::AbstractRNG, v::AbstractVector)
    for i in length(v):-1:2
        j = rand(r, 1:i)
        v[i], v[j] = v[j], v[i]
    end
    return v
end
knuthshuffle!(v::AbstractVector) = knuthshuffle!(Base.Random.GLOBAL_RNG, v)

v = collect(1:20)
println("# v = $v\n   -> ", knuthshuffle!(v))


# a variety of rankings
#
# Please refer to http://en.wikipedia.org/wiki/Ranking#Strategies_for_assigning_rankings
# to see the definitions of a variety of ranking strategies
#
# The implementations here follow this wikipedia page.
#

function _check_randparams(rks, x, p)
    n = length(rks)
    length(x) == length(p) == n || raise_dimerror()
    return n
end

# Ordinal ranking ("1234 ranking") -- use the literal order resulted from sort
function ordinalrank!(rks::RealArray, x::RealArray, p::IntegerArray)
    n = _check_randparams(rks, x, p)

    if n > 0
        i = 1
        while i <= n
            rks[p[i]] = i
            i += 1
        end
    end

    return rks
end

"""
    ordinalrank(x)

Return the ordinal ranking ("1234" ranking) of a real-valued array.
All items in `x` are given distinct, successive ranks based on their
position in `sort(x)`.
"""
ordinalrank(x::RealArray) = ordinalrank!(Array(Int, size(x)), x, sortperm(x))

# Competition ranking ("1224" ranking) -- resolve tied ranks using min
function competerank!(rks::RealArray, x::RealArray, p::IntegerArray)
    n = _check_randparams(rks, x, p)

    if n > 0
        p1 = p[1]
        v = x[p1]
        rks[p1] = k = 1

        i = 2
        while i <= n
            pi = p[i]
            xi = x[pi]
            if xi == v
                rks[pi] = k
            else
                rks[pi] = k = i
                v = xi
            end
            i += 1
        end
    end

    return rks
end

"""
    competerank(x)

Return the standard competition ranking ("1224" ranking) of a real-valued
array. Items that compare equal are given the same rank, then a gap is left
in the rankings the size of the number of tied items - 1.
"""
competerank(x::RealArray) = competerank!(Array(Int, size(x)), x, sortperm(x))

# Dense ranking ("1223" ranking) -- resolve tied ranks using min
function denserank!(rks::RealArray, x::RealArray, p::IntegerArray)
    n = _check_randparams(rks, x, p)

    if n > 0
        p1 = p[1]
        v = x[p1]
        rks[p1] = k = 1

        i = 2
        while i <= n
            pi = p[i]
            xi = x[pi]
            if xi == v
                rks[pi] = k
            else
                rks[pi] = (k += 1)
                v = xi
            end
            i += 1
        end
    end

    return rks
end

"""
    denserank(x)

Return the dense ranking ("1223" ranking) of a real-valued array. Items that
compare equal receive the same ranking, and the next subsequent rank is
assigned with no gap.
"""
denserank(x::RealArray) = denserank!(Array(Int, size(x)), x, sortperm(x))

# Tied ranking ("1 2.5 2.5 4" ranking) -- resolve tied ranks using average
function tiedrank!(rks::RealArray, x::RealArray, p::IntegerArray)
    n = _check_randparams(rks, x, p)

    if n > 0
        v = x[p[1]]

        s = 1  # starting index of current range
        e = 2  # pass-by-end index of current range
        while e <= n
            cx = x[p[e]]
            if cx != v
                # fill average rank to s : e-1
                ar = (s + e - 1) / 2
                for i = s : e-1
                    rks[p[i]] = ar
                end
                # switch to next range
                s = e
                v = cx
            end
            e += 1
        end

        # the last range (e == n+1)
        ar = (s + n) / 2
        for i = s : n
            rks[p[i]] = ar
        end
    end

    return rks
end

# order (aka. rank), resolving ties using the mean rank
"""
    tiedrank(x)

Return the tied ranking, also called fractional or "1 2.5 2.5 4" ranking,
of a real-valued array. Items that compare equal receive the mean of the
rankings they would have been assigned under ordinal ranking.
"""
tiedrank(x::RealArray) = tiedrank!(Array(Float64, size(x)), x, sortperm(x))


function math_expr(op, op1, op2)
    expr = Expr(:call, op, op1, op2)
    return expr
end

ex = math_expr(:+, 1, Expr(:call, :*, 4, 5))

eval(ex)

function make_expr2(op, opr1, opr2)
    opr1f, opr2f = map(x -> isa(x, Number) ? 2*x : x, (opr1, opr2))
    retexpr = Expr(:call, op, opr1f, opr2f)
    return retexpr
end

make_expr2(:+, 1, 2)

ex = make_expr2(:+, 1, Expr(:call, :*, 5, 8))

eval(ex)


# # Unicycle Car Model
#
#md # [![](https://img.shields.io/badge/show-nbviewer-579ACA.svg)](@__NBVIEWER_ROOT_URL__/models/Sherlock-Benchmark-10-Unicycle.ipynb)

module Unicycle  #jl

using NeuralNetworkAnalysis, MAT
using NeuralNetworkAnalysis: UniformAdditivePostprocessing

# This benchmark is that of a unicycle model of a car [^DCS19] taken from Benchmark
# 10 of the Sherlock tool. It models the dynamics of a car involving 4
# variables, specifically the $x$ and $y$ coordinates on a 2 dimensional
# plane, as well as velocity magnitude (speed) and steering angle.

# ## Model
#
# The model is a four-dimensional system given by the following equations:
#
# ```math
# \left\{ \begin{array}{lcl}
# \dot{x}_1 &=& x_4 \cos(x_3) \\
# \dot{x}_2 &=& x_4 \sin(x_3) \\
# \dot{x}_3 &=& u_2 \\
# \dot{x}_4 &=& u_1 + w
# \end{array} \right.
# ```
# where $w$ is a bounded error in the range $[−1e−4, 1e−4]$. A neural
# network controller was trained for this system. The trained network has 1
# hidden layer with 500 neurons. Note that the output of the neural network
# $f(x)$ needs to be normalized in order to obtain $(u_1, u_2)$, namely
# $u_i = f(x)_i − 20$. The sampling time for this controller is 0.2s.

# We model the error $w$ as a nondeterministically assigned constant.
@taylorize function unicycle!(dx, x, p, t)
    x₁, x₂, x₃, x₄, w, u₁, u₂ = x

    dx[1] = x₄ * cos(x₃)
    dx[2] = x₄ * sin(x₃)
    dx[3] = u₂
    dx[4] = u₁ + w
    dx[5] = zero(x[5])
    dx[6] = zero(x[6])
    dx[7] = zero(x[7])
    return dx
end

controller = read_nnet_mat(@modelpath("Sherlock-Benchmark-10-Unicycle",
                                      "controllerB_nnv.mat");
                           act_key="act_fcns");

# ## Specification
#
# The verification problem here is that of reachability. For an initial set of,
# $x_1 ∈ [9.5,9.55], x_2 ∈ [−4.5,−4.45], x_3 ∈ [2.1,2.11], x_4 ∈ [1.5,1.51]$,
# its is required to prove that the system reaches the set
# $x_1 ∈ [−0.6,0.6], x_2 ∈ [−0.2,0.2], x_3 ∈ [−0.06,0.06], x_4 ∈ [−0.3,0.3]$
# within a time window of 10s.

X₀ = Hyperrectangle(low=[9.5, -4.5, 2.1, 1.5, -1e-4],
                    high=[9.55, -4.45, 2.11, 1.51, 1e-4])
U₀ = ZeroSet(2)
vars_idx = Dict(:state_vars=>1:4, :input_vars=>[5], :control_vars=>6:7)
ivp = @ivp(x' = unicycle!(x), dim: 7, x(0) ∈ X₀ × U₀)

period = 0.2  # control period

control_postprocessing = UniformAdditivePostprocessing(-20.0)

prob = ControlledPlant(ivp, controller, vars_idx, period;
                       postprocessing=control_postprocessing)

## Safety specification: [x[1], x[2], x[3], x[4]] ∈ ±[0.6, 0.2, 0.06, 0.3] for some t ≤ T
T = 10.0
T_warmup = 2 * period  # shorter time horizon for dry run

target_states = cartesian_product(Hyperrectangle(zeros(4), [0.6, 0.2, 0.06, 0.3]),
                                  Universe(3))
predicate = X -> X ⊆ target_states
predicate_sol = sol -> any(predicate(R) for F in sol for R in F);

## sufficient check: only look at the final time point
predicate_R_tend = R -> overapproximate(R, Zonotope, tend(R)) ⊆ target_states
predicate_R_all = R -> R ⊆ target_states
predicate_sol_suff = sol -> predicate_R_all(sol[end]);

# ## Results

alg = TMJets(abstol=1e-15, orderT=10, orderQ=1)
alg_nn = Ai2()
splitter = BoxSplitter([3, 1, 8, 1])

function benchmark(; T=T, silent::Bool=false)
    ## We solve the controlled system:
    silent || println("flowpipe construction")
    res_sol = @timed solve(prob, T=T, alg_nn=alg_nn, alg=alg,
                                 splitter=splitter)
    sol = res_sol.value
    silent || print_timed(res_sol)

    ## Next we check the property for an overapproximated flowpipe:
    silent || println("property checking")
    res_pred = @timed predicate_sol_suff(sol)
    silent || print_timed(res_pred)
    if res_pred.value
        silent || println("The property is satisfied.")
    else
        silent || println("The property may be violated.")
    end
    return sol
end;

benchmark(T=T_warmup, silent=true)  # warm-up
res = @timed benchmark()  # benchmark
sol = res.value
println("total analysis time")
print_timed(res);

# We also compute some simulations:

import DifferentialEquations

println("simulation")
res = @timed simulate(prob, T=T; trajectories=10, include_vertices=false)
sim = res.value
print_timed(res);

# Finally we plot the results:

using Plots
import DisplayAs

solz = overapproximate(sol, Zonotope)
Tint = try convert(Int, T) catch; T end

function plot_helper(fig, vars; show_simulation::Bool=true)
    if vars[1] == 0
        target_states_projected = project(target_states, [vars[2]])
        time = Interval(0, T)
        target_states_projected = cartesian_product(time, target_states_projected)
    else
        target_states_projected = project(target_states, vars)
    end
    plot!(fig, solz, vars=vars, color=:yellow, lab="")
    plot!(fig, target_states_projected, color=:cyan, alpha=0.5, lab="target states")
    if show_simulation
        plot_simulation!(fig, sim; vars=vars, color=:black, lab="")
    end
end

vars = (1, 2)
fig = plot(xlab="x₁", ylab="x₂")
plot_helper(fig, vars)
## savefig("Unicycle-x1-x2.png")
fig = DisplayAs.Text(DisplayAs.PNG(fig))

#-

fig = plot(xlab="x₁", ylab="x₂")
plot_helper(fig, vars; show_simulation=false)
xlims!(0, 1)
ylims!(-0.5, 0.5)
plot!(fig, overapproximate(sol[end], Zonotope, tend(solz[end])), vars=vars,
      color=:orange, lab="reach set at t = $Tint")
## savefig("Unicycle-close-x1-x2.png")
fig = DisplayAs.Text(DisplayAs.PNG(fig))

#-

vars=(3, 4)
fig = plot(xlab="x₃", ylab="x₄", leg=:bottomright)
plot_helper(fig, vars)
## savefig("Unicycle-x3-x4.png")
fig = DisplayAs.Text(DisplayAs.PNG(fig))

#-

fig = plot(xlab="x₃", ylab="x₄", leg=:bottomright)
plot_helper(fig, vars; show_simulation=false)
xlims!(-0.1, 0.1)
ylims!(-0.4, 0)
plot!(fig, overapproximate(sol[end], Zonotope, tend(solz[end])), vars=vars,
      color=:orange, lab="reach set at t = $Tint")
## savefig("Unicycle-close-x3-x4.png")
fig = DisplayAs.Text(DisplayAs.PNG(fig))

# ## References

# [^DCS19]: Souradeep Dutta, Xin Chen, and Sriram Sankaranarayanan. *Reachability
#           analysis for neural feedback systems using regressive polynomial rule
#           inference.* In [Proceedings of the 22nd ACMInternational Conference on Hybrid
#           Systems: Computation and Control, HSCC 2019, Montreal,QC, Canada,
#           April 16-18, 2019., pages 157–168, 2019](https://dl.acm.org/doi/abs/10.1145/3302504.3311807).

end  #jl
nothing  #jl


using LinearAlgebra, Test
using TensorToolbox

include("doctest.jl") #check for erros in README examples

include("tensorTest.jl")
include("ttensorTest.jl")
include("ktensorTest.jl")
include("dimtreeTest.jl")
include("htensorTest.jl")
include("TTtensorTest.jl")

println("\n\n")


# ---
# title: 894. All Possible Full Binary Trees
# id: problem894
# author: Indigo
# date: 2021-02-01
# difficulty: Medium
# categories: Tree, Recursion
# link: <https://leetcode.com/problems/all-possible-full-binary-trees/description/>
# hidden: true
# ---
#
# A _full binary tree_  is a binary tree where each node has exactly 0 or 2
# children.
#
# Return a list of all possible full binary trees with `N` nodes.  Each element
# of the answer is the root node of one possible tree.
#
# Each `node` of each tree in the answer **must** have `node.val = 0`.
#
# You may return the final list of trees in any order.
#
#
#
# **Example 1:**
#
#
#
#     Input: 7
#     Output: [[0,0,0,null,null,0,0,null,null,0,0],[0,0,0,null,null,0,0,0,0],[0,0,0,0,0,0,0],[0,0,0,0,0,null,null,null,null,0,0],[0,0,0,0,0,null,null,0,0]]
#     Explanation:
#     ![](https://s3-lc-upload.s3.amazonaws.com/uploads/2018/08/22/fivetrees.png)
#
#
#
#
# **Note:**
#
#   * `1 <= N <= 20`
#
#
## @lc code=start
using LeetCode

function all_possible_fbt(N::Int)
    cache_res = [TreeNode{Int}[] for i in 1:N]
    cache_res[1] = [TreeNode(0)]
    if N % 2 == 0
        return TreeNode[]
    end
    for n in 3:2:N
        for i in 1:2:n-1
            lres = cache_res[i]
            rres = cache_res[n-i-1]
            append!(cache_res[n], TreeNode(0, lr, rr) for lr in lres for rr in rres)
        end
    end
    return cache_res[N]
end
## @lc code=end


@testset "48.rotate-image.jl" begin
    @test rotate_48([
        1 2 3
        4 5 6
        7 8 9
    ]) == [7 4 1
        8 5 2
        9 6 3]
    @test rotate_48([
        5 1 9 11
        2 4 8 10
        13 3 6 7
        15 14 12 16]) == [
        15 13 2 5
        14 3 4 1
        12 6 8 9
        16 7 10 11]
    @test rotate_48(fill(1, 1, 1)) == fill(1, 1, 1)
    @test rotate_48([
        1 2
        3 4]) == [
        3 1
        4 2]
end


# ---
# title: 718. Maximum Length of Repeated Subarray
# id: problem718
# author: Indigo
# date: 2021-06-29
# difficulty: Medium
# categories: Array, Hash Table, Binary Search, Dynamic Programming
# link: <https://leetcode.com/problems/maximum-length-of-repeated-subarray/description/>
# hidden: true
# ---
#
# Given two integer arrays `A` and `B`, return the maximum length of an subarray
# that appears in both arrays.
#
# **Example 1:**
#
#
#
#     Input:
#     A: [1,2,3,2,1]
#     B: [3,2,1,4,7]
#     Output: 3
#     Explanation:
#     The repeated subarray with maximum length is [3, 2, 1].
#
#
#
#
# **Note:**
#
#   1. 1 <= len(A), len(B) <= 1000
#   2. 0 <= A[i], B[i] < 100
#
#
#
#
## @lc code=start
using LeetCode

function longest_common_substring(itr1, itr2)
    m, n = length(itr1) + 1, length(itr2) + 1
    dp = fill(0, m, n)
    for i in 2: m, j in 2: n
        (itr1[i - 1] == itr2[j - 1]) && (dp[i, j] = dp[i - 1, j - 1] + 1)
    end
    return maximum(dp)
end

find_length718(nums1::Vector{Int}, nums2::Vector{Int}) = longest_common_substring(nums1, nums2)
## @lc code=end


struct Interval
    min::Float64
    max::Float64
end

Base.minimum(iv::Interval) = iv.min
Base.maximum(iv::Interval) = iv.max

Base.in(x::Real, iv::Interval) = (x <= iv.max) & (x >= iv.min)

Base.issubset(iv1::Interval, iv2::Interval) = (iv1.max <= iv2.max) & (iv1.min >= iv2.min)

Base.isempty(iv::Interval) = iv.max < iv.min

const ∅ = Interval(1, -1)

Base.:(==)(iv1::Interval, iv2::Interval) = ((iv1.max == iv2.max) & (iv1.min == iv2.min)) | ( isempty(iv1) & isempty(iv2) )

Base.intersect(iv1::Interval, iv2::Interval) = Interval(max(iv1.min,iv2.min), min(iv1.max,iv2.max))

function Base.show(io::IO, mime::MIME"text/plain", iv::Interval)
    if isempty(iv)
        print(io,"∅")
    else
        print(io,"\u301a$(iv.min), $(iv.max)\u301b")
    end
end


"""
    struct JohnsonSoff1985

Type to dispatch on data from the following paper:

* W.R. Johnson, Gerhard Soff, _"The lamb shift in hydrogen-like atoms, 1 ⩽ Z ⩽ 110"_, Atomic
  Data and Nuclear Data Tables, Volume 33, Issue 3, 1985, Pages 405-446, <https://doi.org/10.1016/0092-640X(85)90010-5>

See also: [`rms(::Type{JohnsonSoff1985}, ::Real)`](@ref).
"""
struct JohnsonSoff1985 end

"""
    rms(JohnsonSoff1985, A::Real) -> Float64

Returns the root-mean-square radius (in atomic units) of a nucleus based on the fit from the
[`JohnsonSoff1985`](@ref) paper

```math
\\rm{rms}(A) = (0.836 A^{1/3} + 0.570)~\\rm{fm}
```

where `A` is the atomic mass number of the isotope.

```jldoctest
julia> using AtomicMiscellany: rms, JohnsonSoff1985

julia> rms(JohnsonSoff1985, 238)
0.00010867476884785438
```

!!! note "Accuracy and validity"

    The paper states that the fit is for ``A > 9``, and it appears that the largest ``A``
    value in the data used for the fit was about ``250``. The implementation, however, will
    work with any positive ``A`` value.

    The accuracy is stated to be ``0.05~\\rm{fm}`` i.e. ``9.5 \\times 10^{-7}~\\rm{a.u.}``.
"""
function rms(::Type{JohnsonSoff1985}, A::Real)
    A >= 0 || throw(DomainError(A, "Atomic mass number A must be strictly positive"))
    rms_fm = 0.836 * A^(1/3) + 0.570 # equation (20) from the paper
    # convert to atomic units
    rms_fm * (1e-15 / bohr_in_m)
end

# According to CODATA 2018: 5.29177210903(80)×10−11
const bohr_in_m = 5.29177210903e-11


function ismonotonic(A::AA{T,1}) where T<:AF
    return (all(diff(A) .>= zero(T)) | all(diff(A) .<= zero(T)))
end

function strip_columns(A::AA{T,2}, cols::AA{Bool,1}) where T<:AF
    @assert length(cols) == size(A,2)
    return A[:, .!cols]
end

# credit: traktofon @ https://discourse.julialang.org/t/findnearest-function/4143/4
function searchsortednearest(a::AbstractVector{T}, x::T) where T
    idx = searchsortedfirst(a,x)
    if (idx==1); return idx; end
    if (idx>length(a)); return length(a); end
    if (a[idx]==x); return idx; end
    if (abs(a[idx]-x) < abs(a[idx-1]-x))
        return idx
    else
        return idx-1
    end
end

function searchsortednearest(x::T, a::AbstractVector{T}) where T
    return searchsortednearest(a, x)
end

"""
    calc_rms(A)

Calculate the RMS of given data about the observed mean.

# Arguments
- `A::AbstractArray{Float64,1}`: 1D array containing data for RMS calculation
"""
function calc_rms(A::AA{T,1}) where T<:AF
    return sqrt(sum((A.-mean(A)).^2)/length(A))
end

function strip_nans_by_column(A::AA{T,1}) where T<:AF
    nans = isnan.(A)
    return A[.!vec(nans)]
end

function strip_nans_by_column(A::AA{T,2}) where T<:AF
    nancols = all(isnan.(A), dims=1)
    return A[:,.!vec(nancols)]
end

function strip_nans_by_row(A::AA{T,2}) where T<:AF
    nanrows = all(isnan.(A), dims=2)
    return A[.!vec(nanrows),:]
end

function findnearest(A::AA{T,1}, t) where T<:Real
    return findmin(abs.(A.-t))[2]
end


function cost_matrices_with_dummy(truck_cost_mtx, drone_cost_mtx)
    Ct = [
        truck_cost_mtx          truck_cost_mtx[:, 1];
        truck_cost_mtx[1, :]'    0.0
    ]

    Cd = [
        drone_cost_mtx          drone_cost_mtx[:, 1];
        drone_cost_mtx[1, :]'    0.0
    ]

    return Ct, Cd
end

function _cost_matrices_with_dummy(x, y, speed_of_truck, speed_of_drone)
    n_nodes = length(x)
    @assert length(x) == length(y)

    dist = zeros(Float64, n_nodes, n_nodes)
    for i in 1:n_nodes
        for j in 1:n_nodes
            dist[i, j] = (x[i]-x[j])^2 + (y[i]-y[j])^2 |> sqrt
        end
    end

    Ct = speed_of_truck .* dist
    Cd = speed_of_drone .* dist

    return Ct, Cd
end

function cost_matrices_with_dummy(x, y, speed_of_truck, speed_of_drone)
    xx = copy(x)
    yy = copy(y)
    push!(xx, x[1])
    push!(yy, y[1])
    return _cost_matrices_with_dummy(xx, yy, speed_of_truck, speed_of_drone)
end

function travel_cost(path::Vector{Int}, C::Matrix{T}) where T
    # @show path
    sum = zero(T)
    for i in 1:length(path)-1
        sum += C[path[i], path[i+1]]
    end
    return sum
end

function objective_value(truck_route, drone_route, Ct, Cd)
    combined_nodes = intersect(truck_route, drone_route)
    obj_val = 0.0
    for i in 1:length(combined_nodes)-1
        j1 = combined_nodes[i]
        j2 = combined_nodes[i+1]

        t_idx1 = findfirst(x -> x == j1, truck_route)
        t_idx2 = findfirst(x -> x == j2, truck_route)
        t_cost = travel_cost(truck_route[t_idx1:t_idx2], Ct)

        d_idx1 = findfirst(x -> x == j1, drone_route)
        d_idx2 = findfirst(x -> x == j2, drone_route)
        d_cost = travel_cost(drone_route[d_idx1:d_idx2], Cd)

        obj_val += max(t_cost, d_cost)
    end
    return obj_val
end


using Distributions
using JSON

mu = 0
s = 0.19
d = Logistic( mu,s )

x = [ -5, -2.5, 0, 2.5, 5 ]

dmgf(t) = mgf(d, t )
y = map( dmgf, x )
println( y )

data = Dict([
	("mu", mu),
	("s", s),
	("data", x),
	("expected", y)
])

outfile = open("./test/fixtures/number.json", "w")
JSON.json(data)

write( outfile, JSON.json(data) )


# # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # #
#
# Description
# ==============================================================================
#
#   Compute the precession as in equinox-based IAU-2006 theory.
#
# # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # #
#
# References
# ==============================================================================
#
#   [1] Vallado, D. A (2013). Fundamentals of Astrodynamics and Applications.
#       Microcosm Press, Hawthorn, CA, USA.
#
#   [2] Wallace, P. T., Capitaine, N (2006). Precession-nutation procedures
#       consistent with IAU 2006 resolutions. Astronomy & Astrophysics.
#
#   [3] IERS (2010). Transformation between the International Terrestrial
#       Reference System and the Geocentric Celestial Reference System. IERS
#       Technical Note No. 36, Chapter 5.
#
# # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # #

export precession_iau2006

"""
    precession_iau2006(jd_tt::Number)

Compute the precession angles [rad] according to equinox-based IAU-2006 theory
in the Julia day `jd_tt` [Terrestrial Time].

This algorithm was obtained from **[1]**(p. 49).

# References

- **[1]**: IERS (2010). Transformation between the International Terrestrial
    Reference System and the Geocentric Celestial Reference System. IERS
    Technical Note No. 36, Chapter 5.
"""
function precession_iau2006(jd_tt::Number)
    # Compute the Julian Centuries from `jd_tt`.
    t_tt = (jd_tt - JD_J2000) / 36525

    # Auxiliary variables
    # ===================

    a2d = 1 / 3600
    d2r = π / 180
    a2r = a2d * d2r

    Ψ_a = @evalpoly(
        t_tt,
        0,
        +5038.481_507,
        -1.079_006_9,
        -0.001_140_45,
        +1.328_51e-4,
        -9.51e-8
    )
    Ψ_a = mod(Ψ_a * a2r, 2π)

    ω_a = @evalpoly(
        t_tt,
        +84381.406,
        -0.025_754,
        +0.051_262_3,
        -0.007_725_03,
        -4.67e-7,
        +3.337e-7
    )
    ω_a = mod(ω_a * a2r, 2π)

    χ_a = @evalpoly(
        t_tt,
        0,
        +10.556_403,
        -2.381_429_2,
        -0.001_211_97,
        +1.706_63e-4,
        -5.60e-8
    )
    χ_a = mod(χ_a * a2r, 2π)

    return Ψ_a, ω_a, χ_a
end


using GridPotentialFlow
using Plots
Δx = 0.01
Lx = 2.0
xlim = (-Lx/2,Lx/2)
ylim = (-Lx/2,Lx/2)
g = PhysicalGrid(xlim,ylim,Δx);
c = Lx/2 # chord length
α = -π/3 # angle of attack
plate = Plate(c,4*cellsize(g));
Tr = RigidTransform((0.0,0.0),α)
Tr(plate)
Δs = dlengthmid(plate);

σLE_list = [0.0,0.05,0.1];
pfb_list = [PotentialFlowBody(plate,edges=[1,length(plate)],σ=[SuctionParameter(σLE),SuctionParameter(0.0)]) for σLE in σLE_list];

Δt = 2e-2
vLE = Vortex(plate.x[1]+3Δt*plate.len*cos(plate.α+π/2),plate.y[1]+3Δt*plate.len*sin(plate.α+π/2),0.0);
vTE = Vortex(plate.x[end]+3Δt*cos(α+π/2),plate.y[end]+3Δt*sin(α+π/2),0.0);

function createsheddedvortices(plate,oldvortices)

    vLE = Vortex(2/3*plate.x[1]+1/3*oldvortices[end-1].x,2/3*plate.y[1]+1/3*oldvortices[end-1].y,0.0)
    vTE = Vortex(2/3*plate.x[end]+1/3*oldvortices[end].x,2/3*plate.y[end]+1/3*oldvortices[end].y,0.0)

    return vLE, vTE
end

model_list = [VortexModel(g,bodies=[pfb],vortices=[vLE,vTE],U∞=(1.0,0.0)) for pfb in pfb_list];

T = 0:Δt:0.2
for t in T
    for vm in model_list
        X = getvortexpositions(vm)
        Ẋ = vortexvelocities!(vm)
        X .= X .+ Ẋ*Δt
        setvortexpositions!(vm, X)
        vLEnew, vTEnew = createsheddedvortices(plate,vm.vortices[end-1:end])
        pushvortices!(vm,vLEnew,vTEnew)
    end
end

colors = [:red,:blue,:green];
plot(plate,fillcolor=:black,fillrange=0,fillalpha=0.25,linecolor=:black,linewidth=2,xlabel="x",ylabel="y")
for i in 1:length(model_list)
    plot!(model_list[i].vortices.x[4:2:end],model_list[i].vortices.y[4:2:end],color=colors[i],marker=:circle,markersize=2)
    plot!(model_list[i].vortices.x[3:2:end],model_list[i].vortices.y[3:2:end],color=colors[i],marker=:circle,markersize=2)
    scatter!(model_list[i].vortices.x[1:2],model_list[i].vortices.y[1:2],color=colors[i],marker=:circle,markersize=2,label="σLE=$(σLE_list[i])")
end
plot!()

plot(xlabel="body point index",ylabel="f̃")
for i in 1:length(model_list)
    sol = solve(model_list[i]);
    plot!(sol.f./model_list[i].system.f₀,linecolor=colors[i],label="σLE=$(σLE_list[i])")
end
plot!()

# This file was generated using Literate.jl, https://github.com/fredrikekre/Literate.jl


#-----------------------------------------------------------------------------------------------------
# Functions for "Basis"

# The "Basis" object is used to label product states:

# 1. A single digit starts from 0, and ends with (base-1)
# 2. The index number starts from 1, and ends with (base^len)
# 3. Digits are read from left to right, i.e., the left digits have greater weights
#-----------------------------------------------------------------------------------------------------
"""
    Basis{T<:AbstractVector{<:Integer}}

Type that contains 3 fields:
1. bits: vector of integer represent the product states
2. base: dimension of local Hilbert space
3. len : size of the product state
"""
struct Basis{
    Tbits <: Union{Vector{Int64}, SubArray{Int64, 1, Array{Int64, 1}} },
    Tbase <: Integer,
    Tlen <: Integer
}
    bits::Tbits
    base::Tbase
    len::Tlen
end
# Initiations of "Basis"
basis(base::Integer, len::Integer) = Basis(zeros(Int64, len), base, len)
basis(bits::Vector{<:Integer}, base::Integer) = Basis(bits, base, length(bits))

#-----------------------------------------------------------------------------------------------------
# Convertion between index numbers and bits lists:

# number = base * (base * (...(base * bits[1] + bits[2])...) + bits[L-1]) + bits[L] + 1

# 1. List of bits -> index number
# 2. Index number -> list of bits
#-----------------------------------------------------------------------------------------------------
# List of bits -> index number
function list2num(bits::AbstractVector{<:Integer}, base::Integer, len::Integer)
    num = bits[1]
    for i = 2:len
        num = num * base + bits[i]
    end
    num + 1
end
# Index number -> list of bits
function num2list!(bits::AbstractVector{<:Integer}, index::Integer, base::Integer, len::Integer)
    number = index - 1
    for i = len:-1:2
        number, bits[i] = divrem(number, base)
    end
    bits[1] = number
end

#-----------------------------------------------------------------------------------------------------
# Functions defined on "Basis":

# 1. index  : Get the index of a given "Basis"
# 2. change!: Change the digits of a "Basis", given:
#    a. list of digits
#    b. index number
# 3. copy   : Copy a "Basis"
# 4. view   : Get a view on part of the "Basis", the view is also a "Basis".
#-----------------------------------------------------------------------------------------------------
index(b::Basis) = list2num(b.bits, b.base, b.len)
change!(b::Basis, index::Integer) = num2list!(b.bits, index, b.base, b.len)
function change!(b::Basis, bits::Vector{<:Integer})
    b.bits .= bits
end
copy(b::Basis) = Basis(copy(b.bits), b.base, b.len)
view(b::Basis, inds::AbstractVector) = Basis(view(b.bits, inds), b.base, length(inds))


# This file is a part of BAT.jl, licensed under the MIT License (MIT).

using BAT
using Compat.Test
using StatsBase
using DoubleDouble

@testset "onlineuvstats" begin
    n = 10
    data  = [(-1)^x*exp(x-n/2) for x in 1:n]
    w = abs.(data)

    T = Float64
    wK = ProbabilityWeights
    mdata = mean(data, wK(w))
    vdata = var(data, wK(w); corrected=true)
    maxdata = maximum(data)
    mindata = minimum(data)

    @testset "BAT.OnlineUvMean" begin
        @test typeof(@inferred BAT.OnlineUvMean()) <: BAT.OnlineUvMean{Float64}
        @test typeof(@inferred BAT.OnlineUvMean{Float32}()) <: BAT.OnlineUvMean{Float32}
        ouvm = OnlineUvMean()

        res = append!(ouvm, data, w)
        @test res[] ≈ mdata

        numMeans = 3
        means = Array{BAT.OnlineUvMean{T}}(numMeans)
        for i in indices(means, 1)
            means[i] = OnlineUvMean()
        end

        for i in indices(data, 1)
            x = (i % numMeans) + 1
            means[x] = push!(means[x], data[i], w[i]);
        end

        @test merge!(means...)[] ≈ mdata

    end

    @testset "BAT.OnlineUvVar" begin
        @test typeof(@inferred BAT.OnlineUvVar()) <: BAT.OnlineUvVar{Float64, ProbabilityWeights}
        @test typeof(@inferred BAT.OnlineUvVar{Float32, FrequencyWeights}()) <: BAT.OnlineUvVar{Float32, FrequencyWeights}

        for wKind in [ProbabilityWeights, FrequencyWeights, AnalyticWeights, Weights]
            res = @inferred BAT.OnlineUvVar{Float64, wKind}()
            res = append!(res, data, w)
            @test res[] ≈ var(data, wKind(w); corrected=(wKind != Weights))
        end

        numVars = 3
        vars = Array{BAT.OnlineUvVar{T, wK}}(numVars)
        for i in indices(vars, 1)
            vars[i] = OnlineUvVar()
        end

        for i in indices(data, 1)
            x = (i % numVars) + 1
            vars[x] = push!(vars[x], data[i], w[i]);
        end

        merge!(vars...)
        @test vars[1][] ≈ var(data, wK(w); corrected=true)
        @test push!(vars[1], data[1], zero(Float64))[] ≈ var(data, wK(w); corrected=true)
    end

    @testset "BAT.BasicUvStatistics" begin
        @test typeof(@inferred BAT.BasicUvStatistics{Float32, FrequencyWeights}()) <: BAT.BasicUvStatistics{Float32, FrequencyWeights}

        res = BAT.BasicUvStatistics{T, wK}()
        res = append!(res, data, w)

        @test res.mean[] ≈ mdata
        @test res.var[] ≈ vdata # 100
        @test res.maximum ≈ maxdata
        @test res.minimum ≈ mindata

        numStats = 3
        stats = Array{BAT.BasicUvStatistics{T, wK}}(numStats)
        for i in indices(stats, 1)
            stats[i] = BasicUvStatistics{T, wK}()
        end

        for i in indices(data, 1)
            x = (i % numStats) + 1
            stats[x] = push!(stats[x], data[i], w[i]);
        end

        res = merge(stats...)
        @test res.mean[] ≈ mdata
        @test res.var[] ≈ vdata
        @test res.maximum ≈ maxdata
        @test res.minimum ≈ mindata

        res = BAT.BasicUvStatistics{T, wK}()
        res = append!(res, data)
        @test res.mean[] ≈ mean(data)
        @test res.var[] ≈ cov(data)
    end

end


export solve!

# Convert from sets used within Convex to MOI sets
function get_MOI_set(cone, length_inds)
    if cone == :SDP
        set = MOI.PositiveSemidefiniteConeTriangle(Int(sqrt(.25 + 2 * length_inds) - .5))
    elseif cone == :Zero
        set = MOI.Zeros(length_inds)
    elseif cone == :Free
        set = MOI.Reals(length_inds)
    elseif cone == :NonNeg
        set = MOI.Nonnegatives(length_inds)
    elseif cone == :NonPos
        set = MOI.Nonpositives(length_inds)
    elseif cone == :SOC
        set = MOI.SecondOrderCone(length_inds)
    elseif cone == :SOCRotated
        set = MOI.RotatedSecondOrderCone(length_inds)
    elseif cone == :ExpPrimal
        set = MOI.ExponentialCone()
    elseif cone == :ExpDual
        set = MOI.DualExponentialCone()
    else
        error("Cone $cone not found somehow; please file an issue.")
    end
    return set
end

# add_terms! turns a matrix or vector `X` into a collection of `VectorAffineTerm`'s, which it adds to a list (the first argument).
# This replicates the behavior of the relation `A[row_indices, col_indices] = X`, where `X` could be a vector or matrix.
# Here, `row_indices` corresponds to the output index of a `VectorAffineTerm`
# Instead of `col_indices`, here we have their wrapped counterparts, `var_indices` (a vector of `MOI.VariableIndex`'s).
function add_terms!(terms::Vector{MOI.VectorAffineTerm{T}}, matrix::SparseMatrixCSC, row_indices, var_indices) where T
    CIs = CartesianIndices((row_indices, 1:length(var_indices)))
    I, J, V = findnz(matrix)
    for  k = eachindex(I, J, V)
        term = MOI.VectorAffineTerm{T}(CIs[I[k],J[k]][1], MOI.ScalarAffineTerm{T}(V[k], var_indices[CIs[I[k],J[k]][2]]))
        push!(terms, term)
    end
end

function add_terms!(terms::Vector{MOI.VectorAffineTerm{T}}, vector::SparseVector, row_indices, var_indices) where T
    CIs = CartesianIndices((row_indices, 1:length(var_indices)))
    I, V = findnz(vector)
    for k = eachindex(I, V)
        term =  MOI.VectorAffineTerm{T}(CIs[I[k]][1], MOI.ScalarAffineTerm{T}(V[k], var_indices[CIs[I[k]][2]]))
        push!(terms, term)
    end
end

function add_terms!(terms::Vector{MOI.VectorAffineTerm{T}}, matrix::Matrix, row_indices, var_indices) where T
    CIs = CartesianIndices((row_indices, 1:length(var_indices)))
    for ci = CartesianIndices(size(matrix))
        term = MOI.VectorAffineTerm{T}(CIs[ci][1], MOI.ScalarAffineTerm{T}(matrix[ci], var_indices[CIs[ci][2]]))
        push!(terms, term)
    end
end

function add_terms!(terms::Vector{MOI.VectorAffineTerm{T}}, vector::Vector, row_indices, var_indices) where T
    CIs = CartesianIndices((row_indices, 1:length(var_indices)))
    for k = 1:length(vector)
        term = MOI.VectorAffineTerm{T}(CIs[k][1], MOI.ScalarAffineTerm{T}(vector[k], var_indices[CIs[k][2]]))
        push!(terms, term)
    end
end

# This function generates a MOI set and VectorAffineFunction corresponding to a constraint that
# Convex represents with a `ConicConstr` object.
# The constraint is of the form Ax + b ∈ set
# where `A` is a matrix, `x` a vector of variables, and `b` a constant vector.
# MOI represents this type of constraint as a VectorAffineFunction, where `A` is represented
# by a collection of `VectorAffineTerm`'s.
function make_MOI_constr(conic_constr::ConicConstr, var_to_indices, id_to_variables, T)
    total_constraint_size = sum(conic_constr.sizes)
    constr_index = 0
    # We create a vector of `VectorAffineTerm`'s to build the constraint.
    terms =  MOI.VectorAffineTerm{T}[]
    b = spzeros(T, total_constraint_size)

    for i = 1:length(conic_constr.objs)
        sz = conic_constr.sizes[i]
        for (id, val) in conic_constr.objs[i]
            if id == objectid(:constant)
                for l in 1:sz
                    b[constr_index + l] = ifelse(val[1][l] == 0, val[2][l],  val[1][l])
                end
            else
                var_indices = var_to_indices[id]
                if id_to_variables[id].sign == ComplexSign()
                    l = length(var_indices) ÷ 2
                    # We create MOI terms and add them to `terms`.
                    # Real part:
                    add_terms!(terms, val[1], constr_index + 1 : constr_index + sz, var_indices[1:l])

                    # Imaginary part:
                    add_terms!(terms, val[2], constr_index + 1 : constr_index + sz, var_indices[l+1 : end])

                else
                    # Only a real part:
                    add_terms!(terms, val[1], constr_index + 1 : constr_index + sz, var_indices)
                end
            end
        end
        constr_index += sz
    end
    set = get_MOI_set(conic_constr.cone, total_constraint_size)
    constr_fn =  MOI.VectorAffineFunction{T}(terms, b)
    return set, constr_fn
end

# This function gets MOI variable indices for each variable used as a key
# inside a `ConicObj` used in the problem.
function get_variable_indices!(model, conic_constraints, id_to_variables)
    var_to_indices = Dict{UInt64,Vector{MOI.VariableIndex}}()
    for conic_constr in conic_constraints
        for i = 1:length(conic_constr.objs)
            for (id, val) in conic_constr.objs[i]
                if !haskey(var_to_indices, id) && id != objectid(:constant)
                    var = id_to_variables[id]
                    if var.sign == ComplexSign()
                        var_size = 2 * length(var)
                    else
                        var_size = length(var)
                    end
                    var_to_indices[id] = MOI.add_variables(model, var_size)
                end
            end
        end
    end
    return var_to_indices
end

# This function traverses the problem tree and loads
# an MOI model with the corresponding problem instance.
# The problem is written in a simple epigraph form.
function load_MOI_model!(model, problem::Problem{T}) where {T}
    if length(problem.objective) != 1
        error("Objective must be a scalar")
    end

    unique_conic_forms = UniqueConicForms()
    objective, objective_var_id = conic_form!(problem, unique_conic_forms)
    conic_constraints = unique_conic_forms.constr_list
    conic_constr_to_constr = unique_conic_forms.conic_constr_to_constr
    id_to_variables = unique_conic_forms.id_to_variables

    # `var_to_indices` maps from variable id to the MOI `VariableIndex`'s corresponding to the variable
    var_to_indices = get_variable_indices!(model, conic_constraints, id_to_variables)

    # the objective: maximize or minimize a scalar variable
    objective_index = var_to_indices[objective_var_id][] # get the `MOI.VariableIndex` corresponding to the objective
    MOI.set(model, MOI.ObjectiveFunction{MOI.SingleVariable}(), MOI.SingleVariable(objective_index))
    MOI.set(model, MOI.ObjectiveSense(), problem.head == :maximize ? MOI.MAX_SENSE : MOI.MIN_SENSE)

    # Constraints: Generate a MOI function and a MOI sets for each `ConicConstr` object in the problem
    MOI_constr_fn = Union{MOI.VectorAffineFunction{T},MOI.SingleVariable}[]
    MOI_sets = Any[]
    for conic_constr in conic_constraints
        set, constr_fn = make_MOI_constr(conic_constr, var_to_indices, id_to_variables, T)
        push!(MOI_sets, set)
        push!(MOI_constr_fn, constr_fn)
    end

    # Add integral and boolean constraints
    for var_id in keys(var_to_indices)
        variable = id_to_variables[var_id]
        if :Int in variable.sets
            var_indices = var_to_indices[var_id]
            for idx = eachindex(var_indices)
                push!(MOI_constr_fn, MOI.SingleVariable(var_indices[idx]))
                push!(MOI_sets, MOI.Integer())
            end
        end
        if :Bin in variable.sets
            var_indices = var_to_indices[var_id]
            for idx in eachindex(var_indices)
                push!(MOI_constr_fn, MOI.SingleVariable(var_indices[idx]))
                push!(MOI_sets, MOI.ZeroOne())
            end
        end
    end

    # Add all the constraints to the model and collect the corresponding MOI indices
    constraint_indices = MOI.add_constraints(model, MOI_constr_fn, MOI_sets)

    return id_to_variables, conic_constr_to_constr, conic_constraints, var_to_indices, constraint_indices
end

function solve!(problem::Problem{T}, optimizer::MOI.ModelLike;
    check_vexity = true,
    verbose = true,
    warmstart = false) where {T}

    if check_vexity
        vex = vexity(problem)
    end

    model = MOIB.full_bridge_optimizer(
        MOIU.CachingOptimizer(
            MOIU.UniversalFallback(MOIU.Model{T}()),
            optimizer
        ),
        T
    )

    id_to_variables, conic_constr_to_constr, conic_constraints, var_to_indices, constraint_indices = load_MOI_model!(model, problem)

    if warmstart
        warmstart_variables!(model, var_to_indices, id_to_variables, T, verbose)
    end

    MOI.optimize!(model)
    problem.model = model

    # populate the status, primal variables, and dual variables (when possible)
    moi_populate_solution!(model, problem, id_to_variables, conic_constr_to_constr, conic_constraints, var_to_indices, constraint_indices)

    if problem.status != MOI.OPTIMAL && verbose
        @warn "Problem status $(problem.status); solution may be inaccurate."
    end
end

function warmstart_variables!(model, var_to_indices, id_to_variables, T, verbose)
    if MOI.supports(model, MOI.VariablePrimalStart(), MOI.VariableIndex)
        for (id, var_inds) in pairs(var_to_indices)
            x = id_to_variables[id]
            value = x.value
            value === nothing && continue
            value_vec = packvec(value, sign(x) == ComplexSign())
            value_vec = convert(Vector{T}, value_vec)
            MOI.set(model, MOI.VariablePrimalStart(), var_inds, value_vec)
        end
    elseif verbose
        @warn "Skipping variable warmstart; the solver does not support it."
    end
end

function moi_populate_solution!(model::MOI.ModelLike, problem, id_to_variables, conic_constr_to_constr, conic_constraints, var_to_indices, constraint_indices)
    status = MOI.get(model, MOI.TerminationStatus())
    problem.status = status

    ## This can throw if the problem was not solved correctly
    # objective = MOI.get(model, MOI.ObjectiveValue())
    # problem.optval = objective
    ## Instead, we use a `getproperty` override to access the objective value only when the user asks.
    ## That way `moi_populate_solution!` will complete successfully.

    dual_status = MOI.get(model, MOI.DualStatus())
    primal_status = MOI.get(model, MOI.PrimalStatus())

    if primal_status != MOI.NO_SOLUTION
        for (id, var_indices) in var_to_indices
            var = id_to_variables[id]
            vectorized_value =  MOI.get(model, MOI.VariablePrimal(), var_indices)
            var.value = unpackvec(vectorized_value, size(var), var.sign == ComplexSign())
        end
    else
        for (id, var_indices) in var_to_indices
            var = id_to_variables[id]
            var.value = nothing
        end
    end

    if dual_status != MOI.NO_SOLUTION
        for (idx, conic_constr) in enumerate(conic_constraints)
            haskey(conic_constr_to_constr, conic_constr) || continue
            constr = conic_constr_to_constr[conic_constr]
            MOI_constr_indices = constraint_indices[idx]
            dual_value_vectorized = MOI.get(model, MOI.ConstraintDual(), MOI_constr_indices)
            iscomplex = sign(constr.lhs) == ComplexSign() || sign(constr.rhs) == ComplexSign()
            constr.dual = unpackvec(dual_value_vectorized, constr.size, iscomplex)
        end
    else
        for (idx, conic_constr) in enumerate(conic_constraints)
            haskey(conic_constr_to_constr, conic_constr) || continue
            constr = conic_constr_to_constr[conic_constr]
            constr.dual = nothing
        end
    end

end

# This is type unstable!
function unpackvec(v::AbstractVector, size::Tuple{Int,Int}, iscomplex::Bool)
    if iscomplex && length(v) == 2
        return v[1] + im * v[2]
    elseif iscomplex
        l = length(v) ÷ 2
        # use views?
        return reshape(v[1:l], size) + im * reshape(v[l+1 : end], size)
    elseif length(v) == 1
        return v[]
    else
        return reshape(v, size)
    end
end

# In `packvec`, we use `real` even in the `iscomplex == false` branch
# for type stability. Note here `iscomplex` refers to the `Sign` of the variable
# associated to the values here, not whether or not the variable actually holds
# a complex number at this point. Hence, one could have `iscomplex==true` and
# `isreal(value)==true` or even `value isa Real` could hold.
function packvec(value::Number, iscomplex::Bool)
    iscomplex ? [real(value), imag(value)] : [real(value)]
end

function packvec(value::AbstractArray, iscomplex::Bool)
    value = reshape(value, length(value))
    if iscomplex
        return [real(value); imag(value)]
    else
        return real(value)
    end
end


import SparseArrays.SparseMatrixCSC
import SparseArrays: findnz, dropzeros!, spdiagm
import StaticArrays: SVector
using NearestNeighbors: BallTree, KDTree
using Distances: PeriodicEuclidean

# q: total number of subdivisions, leading to a number dofs given by 2^{qd}
function subdivision_2d(q)
    ##################################################################
    # Construct the domains
    ##################################################################
    n = 2 ^ q
    N = n ^ 2
    Δx = Δy = 1 / (n + 1)
    # The evaluation points of the finite difference discretization.
    domains = array2domains(mapreduce(identity, hcat, [[x; y] for x in Δx .* (1/2 : (n - 1/2)) for y in Δy .* (1/2 : (n - 1/2))]))

    ##################################################################
    # Construct the domain decomposition
    ##################################################################

    # Note that the ids of all bu the finest scale domains are arbitrary
    next_id = maximum(id.(domains)) + 1
    for k = (q - 1) : -1 : 1
        new_domains = Vector{eltype(domains)}(undef, 2 ^ (2 * k))
        # return assignment to a node on the coarser scale
        # TODO: Still need to test that ij -> ji is doing the right thing
        function return_assignment(domain)
            i, j = Int.(div.(center(domain), 1 / 2^k) .+ 1)
            return LinearIndices(zeros(2^k, 2^k))[i, j]
        end
        # go through all domains on the next finer level
        for domain in domains
            parent_index = return_assignment(domain)
            # eithercreate the domain if it doesn't exist yet
            if !isassigned(new_domains, parent_index)
                new_domains[parent_index] = Domain([domain], next_id)
                next_id += 1
            # or else add the new child
            else
                new_domains[parent_index] = add_children(new_domains[parent_index], [domain])
            end
        end
        domains = new_domains
    end
    scales = 1 ./ (2 .^ (1 : q))

    ##################################################################
    # Construct the basis functions
    ##################################################################
    basis_functions = compute_basis_functions(domains)

    return domains, scales, basis_functions
end

# function for selecting the aggregation centers in the square
function aggregation_centers_square(ρh)
    if ρh < Inf
        ticks = ρh : (ρh) : (1 - ρh)
    else
        ticks = [zero(ρh)]
    end
    # if ticks is empty, add a single entry to it, resulting in all nodes being summarized in the same supernode
    if isempty(ticks)
        ticks = [zero(ρh)]
    end
    return SVector{2}.([[x; y] for x in ticks for y in ticks])
end

# Create a finite difference Laplacian problem on a quadratic mesh using sudivision, with dirichlet boundary conditions.
# q: total number of subdivisions, leading to a number dofs given by 2^{qd}
# α: the coefficient function. an edge between x and y will have conductivity (α(x) + α(y)) / 2
# Possibly remore the implicit choice of ρ
function FD_Laplacian_subdivision_2d(q, ρ = 2.0, α = x -> 1)
    n = 2 ^ q
    N = n ^ 2
    Δx = Δy = 1 / (n + 1)

    # actual multiscale domains are provided by subdivision_2d. fine_domains only
    # there fore the

    ##################################################################
    # Construct the Laplace operator
    ##################################################################
    # Create the sparsity pattern of the Laplace operator
    A = spdiagm(-n => -ones(N-n), -1 => -ones(N-1) .* (mod.(1 : (N - 1), n) .!= 0), 0 => 4 * ones(N), 1 => -ones(N-1) .* (mod.(1 : (N - 1), n) .!= 0), n => -ones(N-n)) # / Δx^2

    # Important to multiply A.nzval and not A, to prevent dropping of structural nonzeros.
    A.nzval .*= 0
   ##################################################################
    # Construct the domain decomposition
    ##################################################################
    domains, scales, basis_functions = subdivision_2d(q)

    # collect the finest Domains and sorts them to be in accordance with the construction of the matrix dofs
    fine_domains = sort(gather_descendants(domains), by=id)

    for (i, j, val) in zip(findnz(A)...)
        # only look at edges
        if i != j
            val = (α(center(fine_domains[i])) + α(center(fine_domains[j]))) / 2 / Δx^2
            A[i, i] += val
            A[j, j] += val
            A[i, j] -= val
            A[j, i] -= val
        end
    end
    # # adding zero order term for now
    # for i = 1 : size(A, 1)
    #     A[i, i] += 1.0
    # end

    # constructing the tree function
    function tree_function(x)
        return KDTree(x)
    end

    ##################################################################
    # Constructing the multicolor ordering
    ##################################################################
    # The supernodes corresponding to different columns of 𝐋
    multicolor_ordering = construct_multicolor_ordering(basis_functions, ρ * scales, tree_function)

    return A, domains, scales, basis_functions, multicolor_ordering, fine_domains, tree_function
end

# Create a finite difference Laplacian problem on a quadratic mesh using sudivision, with dirichlet boundary conditions.
# q: total number of subdivisions, leading to a number dofs given by 2^{qd}
# α: the coefficient function. an edge between x and y will have conductivity (α(x) + α(y)) / 2
# Possibly remore the implicit choice of ρ
function FD_periodic_Laplacian_subdivision_2d(q, ρ = 2.0, α = x -> 1)
    n = 2 ^ q
    N = n ^ 2
    Δx = Δy = 1 / (n + 1)
    h = Δx
    # actual multiscale domains are provided by subdivision_2d. fine_domains only
    # there fore the

    ##################################################################
    # Construct the Laplace operator
    ##################################################################
    # Create the sparsity pattern of the Laplace operator
    lin_inds = LinearIndices((n, n))
    row_inds = Int[]
    col_inds = Int[]
    S = Float64[]
    for i in 1 : n, j in 1 : n
        # adding self-interaction 2
        push!(col_inds, lin_inds[mod(i - 1, n) + 1, mod(j - 1, n) + 1])
        push!(row_inds, lin_inds[mod(i - 1, n) + 1, mod(j - 1, n) + 1])
        push!(S, 4.0)
        # interaction to next element in i direction
        push!(col_inds, lin_inds[mod(i - 1, n) + 1, mod(j - 1, n) + 1])
        push!(row_inds, lin_inds[mod(i - 1 + 1, n) + 1, mod(j - 1, n) + 1])
        push!(S, -1.0)

        # interaction to previous element in i direction
        push!(col_inds, lin_inds[mod(i - 1, n) + 1, mod(j - 1, n) + 1])
        push!(row_inds, lin_inds[mod(i - 1 - 1, n) + 1, mod(j - 1, n) + 1])
        push!(S, -1.0)

        # interaction to next element in j direction
        push!(col_inds, lin_inds[mod(i - 1, n) + 1, mod(j - 1, n) + 1])
        push!(row_inds, lin_inds[mod(i - 1, n) + 1, mod(j - 1 + 1, n) + 1])
        push!(S, -1.0)

        # interaction to previous element in j direction
        push!(col_inds, lin_inds[mod(i - 1, n) + 1, mod(j - 1, n) + 1])
        push!(row_inds, lin_inds[mod(i - 1, n) + 1, mod(j - 1 - 1, n) + 1])
        push!(S, -1.0)
    end
    A = sparse(row_inds, col_inds, S) / (h^2)

    # Important to multiply A.nzval and not A, to prevent dropping of structural nonzeros.
    A.nzval .*= 0
   ##################################################################
    # Construct the domain decomposition
    ##################################################################
    domains, scales, basis_functions = subdivision_2d(q)

    # collect the finest Domains and sorts them to be in accordance with the construction of the matrix dofs
    fine_domains = sort(gather_descendants(domains), by=id)

    for (i, j, val) in zip(findnz(A)...)
        # only look at edges
        if i < j
            val = (α(center(fine_domains[i])) + α(center(fine_domains[j]))) / 2 / Δx^2
            A[i, i] += val
            A[j, j] += val
            A[i, j] -= val
            A[j, i] -= val
        end
    end

    # adding zero order term for now
    for i = 1 : size(A, 1)
        A[i, i] += 1.0 + rand()
    end

    # constructing the tree function
    function tree_function(x)
        return BallTree(x, PeriodicEuclidean([1.0, 1.0]))
    end

    ##################################################################
    # Constructing the multicolor ordering
    ##################################################################
    # The supernodes corresponding to different columns of 𝐋
    multicolor_ordering = construct_multicolor_ordering(basis_functions, ρ * scales, tree_function)

    return A, domains, scales, basis_functions, multicolor_ordering, fine_domains, tree_function
end


# Basic algebra
#---------------
Base.copy(t::AbstractTensorMap) = Base.copyto!(similar(t), t)

Base.:-(t::AbstractTensorMap) = mul!(similar(t), t, -one(eltype(t)))
function Base.:+(t1::AbstractTensorMap, t2::AbstractTensorMap)
    T = promote_type(eltype(t1), eltype(t2))
    return axpy!(one(T), t2, copyto!(similar(t1, T), t1))
end
function Base.:-(t1::AbstractTensorMap, t2::AbstractTensorMap)
    T = promote_type(eltype(t1), eltype(t2))
    return axpy!(-one(T), t2, copyto!(similar(t1, T), t1))
end

Base.:*(t::AbstractTensorMap, α::Number) =
    mul!(similar(t, promote_type(eltype(t), typeof(α))), t, α)
Base.:*(α::Number, t::AbstractTensorMap) =
    mul!(similar(t, promote_type(eltype(t), typeof(α))), α, t)
Base.:/(t::AbstractTensorMap, α::Number) = *(t, one(α)/α)
Base.:\(α::Number, t::AbstractTensorMap) = *(t, one(α)/α)

LinearAlgebra.normalize!(t::AbstractTensorMap, p::Real = 2) = rmul!(t, inv(norm(t, p)))
LinearAlgebra.normalize(t::AbstractTensorMap, p::Real = 2) =
    mul!(similar(t), t, inv(norm(t, p)))

Base.:*(t1::AbstractTensorMap, t2::AbstractTensorMap) =
    mul!(similar(t1, promote_type(eltype(t1),eltype(t2)), codomain(t1)←domain(t2)), t1, t2)
Base.exp(t::AbstractTensorMap) = exp!(copy(t))

# Special purpose constructors
#------------------------------
Base.zero(t::AbstractTensorMap) = fill!(similar(t), 0)
function Base.one(t::AbstractTensorMap)
    domain(t) == codomain(t) || throw(SectorMismatch("no identity if domain and codomain are different"))
    TensorMap(I, eltype(t), domain(t), domain(t))
end
function one!(t::AbstractTensorMap)
    domain(t) == codomain(t) || throw(SectorMismatch("no identity if domain and codomain are different"))
    for (c,b) in blocks(t)
        copyto!(b, I)
    end
    return t
end

# Equality and approximality
#----------------------------
function Base.:(==)(t1::AbstractTensorMap, t2::AbstractTensorMap)
    (codomain(t1) == codomain(t2) && domain(t1) == domain(t2)) || return false
    for c in blocksectors(t1)
        block(t1, c) == block(t2, c) || return false
    end
    return true
end

function Base.isapprox(t1::AbstractTensorMap, t2::AbstractTensorMap; atol::Real=0, rtol::Real=Base.rtoldefault(eltype(t1), eltype(t2), atol))
    d = norm(t1 - t2)
    if isfinite(d)
        return d <= max(atol, rtol*max(norm(t1), norm(t2)))
    else
        return false
    end
end

# In-place methods
#------------------
# Wrapping the blocks in a StridedView enables multithreading if JULIA_NUM_THREADS > 1
# Copy, adjoint! and fill:
function Base.copyto!(tdest::AbstractTensorMap, tsource::AbstractTensorMap)
    codomain(tdest) == codomain(tsource) && domain(tdest) == domain(tsource) || throw(SpaceMismatch())
    for c in blocksectors(tdest)
        copyto!(StridedView(block(tdest, c)), StridedView(block(tsource, c)))
    end
    return tdest
end
function Base.fill!(t::AbstractTensorMap, value::Number)
    for (c,b) in blocks(t)
        fill!(b, value)
    end
    return t
end
function LinearAlgebra.adjoint!(tdest::AbstractTensorMap, tsource::AbstractTensorMap)
    codomain(tdest) == domain(tsource) && domain(tdest) == codomain(tsource) || throw(SpaceMismatch())
    for c in blocksectors(tdest)
        adjoint!(StridedView(block(tdest, c)), StridedView(block(tsource, c)))
    end
    return tdest
end

# Basic vector space methods: addition and scalar multiplication
LinearAlgebra.rmul!(t::AbstractTensorMap, α::Number) = mul!(t, t, α)
LinearAlgebra.lmul!(α::Number, t::AbstractTensorMap) = mul!(t, α, t)

function LinearAlgebra.mul!(t1::AbstractTensorMap, t2::AbstractTensorMap, α::Number)
    (codomain(t1)==codomain(t2) && domain(t1) == domain(t2)) || throw(SpaceMismatch())
    for c in blocksectors(t1)
        mul!(StridedView(block(t1, c)), StridedView(block(t2, c)), α)
    end
    return t1
end
function LinearAlgebra.mul!(t1::AbstractTensorMap, α::Number, t2::AbstractTensorMap)
    (codomain(t1)==codomain(t2) && domain(t1) == domain(t2)) || throw(SpaceMismatch())
    for c in blocksectors(t1)
        mul!(StridedView(block(t1, c)), α, StridedView(block(t2, c)))
    end
    return t1
end
function LinearAlgebra.axpy!(α::Number, t1::AbstractTensorMap, t2::AbstractTensorMap)
    (codomain(t1)==codomain(t2) && domain(t1) == domain(t2)) || throw(SpaceMismatch())
    for c in blocksectors(t1)
        axpy!(α, StridedView(block(t1, c)), StridedView(block(t2, c)))
    end
    return t2
end
function LinearAlgebra.axpby!(α::Number, t1::AbstractTensorMap, β::Number, t2::AbstractTensorMap)
    (codomain(t1)==codomain(t2) && domain(t1) == domain(t2)) || throw(SpaceMismatch())
    for c in blocksectors(t1)
        axpby!(α, StridedView(block(t1, c)), β, StridedView(block(t2, c)))
    end
    return t2
end

# inner product and norm only valid for spaces with Euclidean inner product
function LinearAlgebra.dot(t1::AbstractTensorMap{S}, t2::AbstractTensorMap{S}) where {S<:EuclideanSpace}
    (codomain(t1) == codomain(t2) && domain(t1) == domain(t2)) || throw(SpaceMismatch())
    iter = blocksectors(t1)
    if isempty(iter)
        return zero(eltype(t1))*zero(eltype(t2))
    else
        return sum(dim(c)*dot(block(t1,c), block(t2,c)) for c in blocksectors(t1))
    end
end

LinearAlgebra.norm(t::AbstractTensorMap{<:EuclideanSpace}, p::Real) =
    isempty(blocks(t)) ? zero(real(eltype(t))) : _norm(blocks(t), p)
function _norm(blockiterator, p::Real)
    if p == Inf
        maximum(norm(b, p) for (c,b) in blockiterator)
    elseif p == -Inf
        minimum(norm(b, p) for (c,b) in blockiterator)
    elseif p == 1
        sum(dim(c)*norm(b, p) for (c,b) in blockiterator)
    else
        s = sum(dim(c)*norm(b, p)^p for (c,b) in blockiterator)
        return s^inv(oftype(s, p))
    end
end

# TensorMap trace
function LinearAlgebra.tr(t::AbstractTensorMap)
    domain(t) == codomain(t) ||
        throw(SpaceMismatch("Trace of a tensor only exist when domain == codomain"))
    return sum(dim(c)*tr(b) for (c,b) in blocks(t))
end

# TensorMap multiplication:
function LinearAlgebra.mul!(tC::AbstractTensorMap, tA::AbstractTensorMap,  tB::AbstractTensorMap, α = true, β = false)
    if !(codomain(tC) == codomain(tA) && domain(tC) == domain(tB) && domain(tA) == codomain(tB))
        throw(SpaceMismatch())
    end
    for c in blocksectors(tC)
        if hasblock(tA, c) # then also tB should have such a block
            A = block(tA, c)
            B = block(tB, c)
            C = block(tC, c)
            if isbitstype(eltype(A)) && isbitstype(eltype(B)) && isbitstype(eltype(C))
                @unsafe_strided A B C mul!(C, A, B, α, β)
            else
                mul!(StridedView(C), StridedView(A), StridedView(B), α, β)
            end
        elseif β != one(β)
            rmul!(block(tC, c), β)
        end
    end
    return tC
end

# TensorMap inverse
function Base.inv(t::AbstractTensorMap)
    domain(t) == codomain(t) ||
        throw(SpaceMismatch("Inverse of a tensor only exist when domain == codomain; check pinv"))
    if sectortype(t) === Trivial
        return TensorMap(inv(block(t, Trivial())), domain(t)←codomain(t))
    else
        data = empty(t.data)
        for (c,b) in blocks(t)
            data[c] = inv(b)
        end
        return TensorMap(data, domain(t)←codomain(t))
    end
end
function LinearAlgebra.pinv(t::AbstractTensorMap; kwargs...)
    if sectortype(t) === Trivial
        return TensorMap(pinv(block(t, Trivial()); kwargs...), domain(t)←codomain(t))
    else
        data = empty(t.data)
        for (c,b) in blocks(t)
            data[c] = pinv(b; kwargs...)
        end
        return TensorMap(data, domain(t)←codomain(t))
    end
end
function Base.:(\)(t1::AbstractTensorMap, t2::AbstractTensorMap)
    codomain(t1) == codomain(t2) ||
        throw(SpaceMismatch("non-matching codomains in t1 \\ t2"))
    if sectortype(t1) === Trivial
        data = block(t1, Trivial()) \ block(t2, Trivial())
        return TensorMap(data, domain(t1)←domain(t2))
    else
        cod = codomain(t1)
        data = SectorDict(c=>block(t1,c) \ block(t2,c) for c in blocksectors(codomain(t1)))
        return TensorMap(data, domain(t1)←domain(t2))
    end
end
function Base.:(/)(t1::AbstractTensorMap, t2::AbstractTensorMap)
    domain(t1) == domain(t2) ||
        throw(SpaceMismatch("non-matching domains in t1 / t2"))
    if sectortype(t1) === Trivial
        data = block(t1, Trivial()) / block(t2, Trivial())
        return TensorMap(data, codomain(t1)←codomain(t2))
    else
        data = SectorDict(c=>block(t1,c) / block(t2,c) for c in blocksectors(domain(t1)))
        return TensorMap(data, codomain(t1)←codomain(t2))
    end
end

# TensorMap exponentation:
function exp!(t::TensorMap)
    domain(t) == codomain(t) ||
        error("Exponentional of a tensor only exist when domain == codomain.")
    for (c,b) in blocks(t)
        copyto!(b, LinearAlgebra.exp!(b))
    end
    return t
end

# # concatenate tensors
# function concatenate(t1::AbstractTensorMap{S}, ts::AbstractTensorMap{S}...; direction::Symbol) where {S}
#     if direction = :domain
#         numin(t1) == 1 || throw(SpaceMismatch("concatenation along domain"))
#         for t2 in ts
#             domain(t1) == domain(t2) || throw(SpaceMismatch())
#         end
#         t = t1
#         for t2 in ts
#             cat(t1)

function catdomain(t1::AbstractTensorMap{S,N₁,1}, t2::AbstractTensorMap{S,N₁,1}) where {S,N₁}
    codomain(t1) == codomain(t2) || throw(SpaceMismatch())

    V1, = domain(t1)
    V2, = domain(t2)
    isdual(V1) == isdual(V2) || throw(SpaceMismatch("cannot hcat tensors whose domain has non-matching duality"))

    V = V1 ⊕ V2
    t = TensorMap(undef, promote_type(eltype(t1), eltype(t2)), codomain(t1), V)
    for c in sectors(V)
        block(t, c)[:,1:dim(V1, c)] .= block(t1, c)
        block(t, c)[:,dim(V1, c) .+ (1:dim(V2, c))] .= block(t2, c)
    end
    return t
end
function catcodomain(t1::AbstractTensorMap{S,1,N₂}, t2::AbstractTensorMap{S,1,N₂}) where {S,N₂}
    domain(t1) == domain(t2) || throw(SpaceMismatch())

    V1, = codomain(t1)
    V2, = codomain(t2)
    isdual(V1) == isdual(V2) || throw(SpaceMismatch("cannot hcat tensors whose domain has non-matching duality"))

    V = V1 ⊕ V2
    t = TensorMap(undef, promote_type(eltype(t1),eltype(t2)), V, domain(t1))
    for c in sectors(V)
        block(t, c)[1:dim(V1, c),:] .= block(t1, c)
        block(t, c)[dim(V1,c) .+ (1:dim(V2, c)),:] .= block(t2, c)
    end
    return t
end

# tensor product of tensors
function ⊗(t1::AbstractTensorMap{S}, t2::AbstractTensorMap{S}) where S
    cod1, cod2 = codomain(t1), codomain(t2)
    dom1, dom2 = domain(t1), domain(t2)
    cod = cod1 ⊗ cod2
    dom = dom1 ⊗ dom2
    t = TensorMap(zeros, promote_type(eltype(t1), eltype(t2)), cod, dom)
    if sectortype(S) === Trivial
        d1 = dim(cod1)
        d2 = dim(cod2)
        d3 = dim(dom1)
        d4 = dim(dom2)
        m1 = reshape(t1[], (d1, 1, d3, 1))
        m2 = reshape(t2[], (1, d2, 1, d4))
        m = reshape(t[], (d1, d2, d3, d4))
        m .= m1 .* m2
    else
        for (f1l, f1r) in fusiontrees(t1)
            for (f2l, f2r) in fusiontrees(t2)
                for (fl, coeff1) in merge(f1l, f2l)
                    for (fr, coeff2) in merge(f1r, f2r)
                        fl.coupled == fr.coupled || continue
                        d1 = dim(cod1, f1l.uncoupled)
                        d2 = dim(cod2, f2l.uncoupled)
                        d3 = dim(dom1, f1r.uncoupled)
                        d4 = dim(dom2, f2r.uncoupled)
                        m1 = reshape(t1[f1l,f1r], (d1, 1, d3, 1))
                        m2 = reshape(t2[f2l,f2r], (1, d2, 1, d4))
                        m = reshape(t[fl, fr], (d1, d2, d3, d4))
                        m .+= coeff1 .* coeff2 .* m1 .* m2
                    end
                end
            end
        end
    end
    return t
end


using DynamicHMC,
    LinearAlgebra,
    LogDensityProblems,
    MCMCDiagnosticTools,
    Optim,
    Pathfinder,
    Random,
    Statistics,
    StatsFuns,
    Test,
    TransformVariables

Random.seed!(1)

struct RegressionProblem{X,Y}
    x::X
    y::Y
end

function (prob::RegressionProblem)(θ)
    σ = θ.σ
    α = θ.α
    β = θ.β
    x = prob.x
    y = prob.y
    lp = normlogpdf(σ) + logtwo
    lp += normlogpdf(α)
    lp += sum(normlogpdf, β)
    y_hat = muladd(x, β, α)
    lp += sum(eachindex(y_hat, y)) do i
        return normlogpdf(y_hat[i], σ, y[i])
    end
    return lp
end

function mean_and_mcse(f, θs)
    zs = map(f, θs)
    ms = mean(zs)
    ses = map(mcse, eachrow(reduce(hcat, zs)))
    return ms, ses
end

function compare_estimates(f, xs1, xs2, α=0.05)
    nparams = length(first(xs1))
    α /= nparams  # bonferroni correction
    p = α / 2
    m1, s1 = mean_and_mcse(f, xs1)
    m2, s2 = mean_and_mcse(f, xs2)
    zs = @. (m1 - m2) / sqrt(s1^2 + s2^2)
    @test all(norminvcdf(p) .< zs .< norminvccdf(p))
end

@testset "DynamicHMC integration" begin
    A = Diagonal(rand(5))
    B = randn(5, 2)
    D = exp(Symmetric(randn(2, 2)))
    M⁻¹ = Pathfinder.WoodburyPDMat(A, B, D)

    @testset "Pathfinder.WoodburyLeftInvFactor" begin
        L = Pathfinder.WoodburyLeftInvFactor(M⁻¹)
        v = randn(5)
        V = randn(5, 2)
        @test L.A === M⁻¹
        @test size(L) === size(M⁻¹)
        Lmat = @inferred Matrix(L)
        @test Lmat isa Matrix
        @test Lmat * Lmat' ≈ inv(M⁻¹)
        @test L * v ≈ Lmat * v
        @test L * V ≈ Lmat * V
        @test AbstractMatrix{Float32}(L) isa Pathfinder.WoodburyLeftInvFactor{Float32}
        for i in axes(L, 1), j in axes(L, 2)
            @test L[i, j] ≈ Lmat[i, j]
        end
    end

    @testset "DynamicHMC.GaussianKineticEnergy" begin
        κ = DynamicHMC.GaussianKineticEnergy(M⁻¹)
        @test κ.M⁻¹ === M⁻¹
        @test κ.W === Pathfinder.WoodburyLeftInvFactor(M⁻¹)
        κdense = DynamicHMC.GaussianKineticEnergy(Symmetric(Matrix(M⁻¹)))
        κdense2 = DynamicHMC.GaussianKineticEnergy(Symmetric(Matrix(M⁻¹)), Matrix(κ.W))
        p = randn(5)
        q = randn(5)

        @test size(κ) == size(κdense)
        @test DynamicHMC.kinetic_energy(κ, p, q) ≈ DynamicHMC.kinetic_energy(κdense, p, q)
        @test DynamicHMC.calculate_p♯(κ, p, q) ≈ DynamicHMC.calculate_p♯(κdense, p, q)
        @test DynamicHMC.∇kinetic_energy(κ, p, q) ≈ DynamicHMC.∇kinetic_energy(κdense, p, q)
        @test DynamicHMC.rand_p(MersenneTwister(42), κ, q) ≈
            DynamicHMC.rand_p(MersenneTwister(42), κdense2, q)
    end

    @testset "DynamicHMC.mcmc_with_warmup" begin
        ndraws = 1_000
        x = 0:0.01:1
        y = sin.(x) .+ randn.() .* 0.2 .+ x
        X = [x x .^ 2 x .^ 3]
        prob = RegressionProblem(X, y)
        trans = as((σ=asℝ₊, α=asℝ, β=as(Array, size(X, 2))))
        P = TransformedLogDensity(trans, prob)
        ∇P = ADgradient(:ForwardDiff, P)
        rng = Random.GLOBAL_RNG

        result_hmc1 = mcmc_with_warmup(rng, ∇P, ndraws; reporter=NoProgressReport())

        logp(x) = LogDensityProblems.logdensity(P, x)
        ∇logp(x) = LogDensityProblems.logdensity_and_gradient(∇P, x)[2]
        result_pf = pathfinder(logp, ∇logp; dim=LogDensityProblems.dimension(P))

        @testset "Initial point" begin
            result_hmc2 = mcmc_with_warmup(
                rng,
                ∇P,
                ndraws;
                initialization=(; q=result_pf.draws[:, 1]),
                reporter=NoProgressReport(),
            )
            @test result_hmc2.κ.M⁻¹ isa Diagonal
            compare_estimates(identity, result_hmc2.chain, result_hmc1.chain)
        end

        @testset "Initial point and metric" begin
            result_hmc3 = mcmc_with_warmup(
                rng,
                ∇P,
                ndraws;
                initialization=(;
                    q=result_pf.draws[:, 1],
                    κ=GaussianKineticEnergy(result_pf.fit_distribution.Σ),
                ),
                warmup_stages=default_warmup_stages(; M=Symmetric),
                reporter=NoProgressReport(),
            )
            @test result_hmc3.κ.M⁻¹ isa Symmetric
            compare_estimates(identity, result_hmc3.chain, result_hmc1.chain)
        end

        @testset "Initial point and final metric" begin
            result_hmc4 = mcmc_with_warmup(
                rng,
                ∇P,
                ndraws;
                initialization=(;
                    q=result_pf.draws[:, 1],
                    κ=GaussianKineticEnergy(result_pf.fit_distribution.Σ),
                ),
                warmup_stages=default_warmup_stages(; middle_steps=0, doubling_stages=0),
                reporter=NoProgressReport(),
            )
            @test result_hmc4.κ.M⁻¹ === result_pf.fit_distribution.Σ
            compare_estimates(identity, result_hmc4.chain, result_hmc1.chain)
        end
    end
end


@doc raw"""
    subgradient_method(M, F, ∂F, x)

perform a subgradient method ``x_{k+1} = \mathrm{retr}(x_k, s_k∂F(x_k))``,

where ``\mathrm{retr}`` is a retraction, ``s_k`` can be specified as a function but is
usually set to a constant value. Though the subgradient might be set valued,
the argument `∂F` should always return _one_ element from the subgradient, but
not necessarily deterministic.

# Input
* `M` – a manifold ``\mathcal M``
* `F` – a cost function ``F:\mathcal M→ℝ`` to minimize
* `∂F`– the (sub)gradient ``\partial F: \mathcal M→ T\mathcal M`` of F
  restricted to always only returning one value/element from the subgradient.
  This function can be passed as an allocation function `(M, y) -> X` or
  a mutating function `(M, X, y) -> X`, see `evaluation`.
* `x` – an initial value ``x ∈ \mathcal M``

# Optional
* `evaluation` – ([`AllocatingEvaluation`](@ref)) specify whether the subgradient works by
   allocation (default) form `∂F(M, y)` or [`MutatingEvaluation`](@ref) in place, i.e. is
   of the form `∂F!(M, X, x)`.
* `stepsize` – ([`ConstantStepsize`](@ref)`(1.)`) specify a [`Stepsize`](@ref)
* `retraction` – (`default_retraction_method(M)`) a `retraction(M,x,ξ)` to use.
* `stopping_criterion` – ([`StopAfterIteration`](@ref)`(5000)`)
  a functor, see[`StoppingCriterion`](@ref), indicating when to stop.
* `return_options` – (`false`) – if activated, the extended result, i.e. the
  complete [`Options`](@ref) re returned. This can be used to access recorded values.
  If set to false (default) just the optimal value `x_opt` if returned
...
and the ones that are passed to [`decorate_options`](@ref) for decorators.

# Output
* `x_opt` – the resulting (approximately critical) point of the subgradient method
OR
* `options` - the options returned by the solver (see `return_options`)
"""
function subgradient_method(
    M::AbstractManifold, F::TF, ∂F::TdF, x; kwargs...
) where {TF,TdF}
    x_res = allocate(x)
    copyto!(M, x_res, x)
    return subgradient_method!(M, F, ∂F, x_res; kwargs...)
end
@doc raw"""
    subgradient_method!(M, F, ∂F, x)

perform a subgradient method ``x_{k+1} = \mathrm{retr}(x_k, s_k∂F(x_k))`` in place of `x`

# Input
* `M` – a manifold ``\mathcal M``
* `F` – a cost function ``F:\mathcal M→ℝ`` to minimize
* `∂F`- the (sub)gradient ``\partial F:\mathcal M→ T\mathcal M`` of F
  restricted to always only returning one value/element from the subgradient.
  This function can be passed as an allocation function `(M, y) -> X` or
  a mutating function `(M, X, y) -> X`, see `evaluation`.
* `x` – an initial value ``x ∈ \mathcal M``

for more details and all optional parameters, see [`subgradient_method`](@ref).
"""
function subgradient_method!(
    M::AbstractManifold,
    F::TF,
    ∂F!!::TdF,
    x;
    retraction::TRetr=default_retraction_method(M),
    stepsize::Stepsize=ConstantStepsize(1.0),
    stopping_criterion::StoppingCriterion=StopAfterIteration(5000),
    return_options=false,
    evaluation::AbstractEvaluationType=AllocatingEvaluation(),
    kwargs..., #especially may contain debug
) where {TF,TdF,TRetr}
    p = SubGradientProblem(M, F, ∂F!!; evaluation=evaluation)
    o = SubGradientMethodOptions(M, x, stopping_criterion, stepsize, retraction)
    o = decorate_options(o; kwargs...)
    resultO = solve(p, o)
    if return_options
        return resultO
    else
        return get_solver_result(resultO)
    end
end
function initialize_solver!(p::SubGradientProblem, o::SubGradientMethodOptions)
    o.x_optimal = o.x
    o.∂ = zero_vector(p.M, o.x)
    return o
end
function step_solver!(p::SubGradientProblem, o::SubGradientMethodOptions, iter)
    get_subgradient!(p, o.∂, o.x)
    s = get_stepsize(p, o, iter)
    retract!(p.M, o.x, o.x, -s * o.∂, o.retraction_method)
    (get_cost(p, o.x) < get_cost(p, o.x_optimal)) && (o.x_optimal = o.x)
    return o
end
get_solver_result(o::SubGradientMethodOptions) = o.x_optimal


print("Please enter a number to factorialize: ")

n = parse(Int64, readline())

counter = n
total = 1

while counter > 1
	global total *= counter
	global counter -= 1
end

println("$n ! = $total")


@testset "argmin_x" begin

#test argmin_x.jl for CSC sparse matrix format
A=sprandn(100,100,0.01)+sparse(I, 100, 100);
A=A'*A;
while rank(Array(A))<100
  A=A+sparse(I, 100, 100)
end
xt=randn(100)
b=A*xt;

maxit=100;
pp=2
p=2
TF=Float64
log_PARSDMM = log_type_PARSDMM(zeros(maxit,pp),zeros(maxit,p),zeros(maxit,p),zeros(maxit),zeros(maxit),zeros(maxit),
zeros(maxit),zeros(maxit,p),zeros(maxit,p),zeros(maxit),zeros(maxit),TF(0),TF(0),TF(0),TF(0),TF(0),TF(0),TF(0));

#test for zero initial guess
x_min_solver="CG_normal"
(x,iter,relres)=argmin_x(A,b,zeros(100),x_min_solver,1e-5,1,log_PARSDMM)
@test norm(A*x-b)/norm(b) <= 1e-5
@test isapprox(norm(A*x-b)/norm(b),relres,rtol=1e-5)

#test for good initial guess
x_min_solver="CG_normal"
(x2,iter2,relres2)=argmin_x(A,b,xt+randn(100).*1e-4,x_min_solver,1e-5,1,log_PARSDMM)
@test iter2<iter
@test norm(A*x2-b)/norm(b) <= 1e-5
@test isapprox(norm(A*x2-b)/norm(b),relres2,rtol=1e-5)

#test if 10*eps(Float32/Float64) is achieved
x_min_solver="CG_normal"
(x,iter,relres)=argmin_x(A,b,zeros(100),x_min_solver,10*eps(),1,log_PARSDMM)
@test norm(A*x-b)/norm(b) <= 20*eps()

#test argmin_x.jl for CDS sparse matrix format
A=sprandn(100,100,0.01)+sparse(I, 100, 100);
A=A'*A;
while rank(Array(A))<100
  A=A+sparse(I, 100, 100)
end
xt=randn(100)
b=A*xt;

(R,offset) = mat2CDS(A)

x_min_solver="CG_normal"
y=zeros(Float64,length(b))
(x,iter,relres)=argmin_x(A,b,zeros(100),x_min_solver,1e-5,1,log_PARSDMM,offset,y)

@test norm(A*x-b)/norm(b) <= 1e-5
@test norm(A*x-b)/norm(b) <= 2.0*relres

#test for more accurate solutions with CDS
y=zeros(Float64,length(b))
(x,iter,relres)=argmin_x(A,b,zeros(100),x_min_solver,1e-10,1,log_PARSDMM,offset,y)

@test norm(A*x-b)/norm(b) <= 1e-10
@test norm(A*x-b)/norm(b) <= 2.0*relres

end


export WeightingOp

"""
  WeightingOp(weights::Vector{T}, rep::Int=1) where T

generates a `LinearOperator` which multiplies an input vector index-weise with `weights`

# Arguments
* `weights::Vector{T}` - weights vector
* `rep::Int=1`         - number of sub-arrays that need to be multiplied with `weights`
"""
function WeightingOp(weights::Vector{T}, rep::Int=1) where T
  weights_cat = repeat(weights,rep)
  return opDiagonal(weights_cat)
end