module MVCalculusTest
using BenchmarkTools, LinearAlgebra, Random, Test, WiSER

@testset "kron_axpy!" begin
Random.seed!(123)
m, n = 100, 50
p, q = 30, 20
A = randn(m, n)
X = randn(p, q)

Y1 = zeros(m * p, n * q)
kron_axpy!(A, X, Y1)
@test Y1 == kron(A, X)
bm = @benchmark kron_axpy!($A, $X, $Y1) setup=(fill!($Y1, 0))
display(bm); println()
@test allocs(bm) == 0

Y2 = zeros(n * p, m * q)
kron_axpy!(transpose(A), X, Y2)
@test Y2 == kron(transpose(A), X)
bm = @benchmark kron_axpy!($(transpose(A)), $X, $Y2) setup=(fill!($Y2, 0))
display(bm); println()
@test allocs(bm) == 0

Y3 = zeros(m * q, n * p)
kron_axpy!(A, transpose(X), Y3)
@test Y3 == kron(A, transpose(X))
bm = @benchmark kron_axpy!($A, $(transpose(X)), $Y3) setup=(fill!($Y3, 0))
display(bm); println()
@test allocs(bm) == 0

Y4 = zeros(n * q, m * p)
kron_axpy!(transpose(A), transpose(X), Y4)
@test Y4 == kron(transpose(A), transpose(X))
bm = @benchmark kron_axpy!($(transpose(A)), $(transpose(X)), $Y4) setup=(fill!($Y4, 0))
display(bm); println()
@test allocs(bm) == 0
end

@testset "kr_axpy!" begin
Random.seed!(123)
m, n, p = 100, 50, 30
A = randn(m, n)
X = randn(p, n)

Y1 = zeros(m * p, n)
kr_axpy!(A, X, Y1)
@test Y1 == reshape([A[i1, j] * X[i2, j] for i2 in 1:p, i1 in 1:m, j in 1:n], m * p, n)
bm = @benchmark kr_axpy!($A, $X, $Y1) setup=(fill!($Y1, 0))
display(bm); println()
@test allocs(bm) == 0
end

@testset "CopyMatrix" begin
Random.seed!(123)
n = 50
C = CopyMatrix(n)

@test size(C) == (n * n, (n * (n + 1)) >> 1)
# display(C); println()
# display(Matrix{Float64}(C)); println()
M = randn(n, n)
@test vec(LowerTriangular(M)) == C * vech(LowerTriangular(M))
@test vec(M)' * C == vech(M)'
@test C' * vec(M) == vech(M)

result1 = similar(vec(M)' * C)
bm = @benchmark mul!($result1, $(transpose(vec(M))), $C)
display(bm); println()
@test allocs(bm) == 0

result2 = similar(C' * vec(M))
bm = @benchmark mul!($result2, $(transpose(C)), $(vec(M)))
display(bm); println()
@test_skip allocs(bm) == 0
end

@testset "Ct_At_kron_A_KC" begin
Random.seed!(123)
q = 5
q◺ = WiSER.◺(q)
A = randn(q, q); A = A'A * LowerTriangular(randn(q, q))
H = Ct_At_kron_A_KC(A)
Cq  = CopyMatrix(q)
Kqq = commutation(q, q)
@test size(H) == (q◺, q◺)
@test issymmetric(H)
@test H == Cq' * kron(A', A) * Kqq * Cq
bm = @benchmark Ct_At_kron_A_KC!($H, $A) setup=(fill!($H, 0))
display(bm); println()
@test allocs(bm) == 0
end

@testset "Ct_A_kron_B_C" begin
Random.seed!(123)
q = 5
q◺ = WiSER.◺(q)
A = randn(q, q); A = A'A
B = randn(q, q); B = B'B
H = Ct_A_kron_B_C(A, B)
Cq  = CopyMatrix(q)
@test size(H) == (q◺, q◺)
@test issymmetric(H)
@test all(eigvals(Symmetric(H)) .≥ 0)
@test H == Cq' * kron(A, B) * Cq
bm = @benchmark Ct_A_kron_B_C!($H, $A, $B) setup=(fill!($H, 0))
display(bm); println()
@test allocs(bm) == 0
end

@testset "Ct_A_kr_B" begin
Random.seed!(123)
q, n = 5, 10
q◺ = ◺(q)
A = randn(q, n)
B = randn(q, n)
H = Ct_A_kr_B(A, B)
Cq  = CopyMatrix(q)
@test size(H) == (q◺, n)
@test H == Cq' * kr(A, B)
bm = @benchmark Ct_A_kr_B!($H, $A, $B) setup=(fill!($H, 0))
display(bm); println()
@test allocs(bm) == 0
end
end


function createMesh(L::Float64, dx::Float64)
    N = Int(round(L/dx))
    Nc, Ni = 4, (N-1)^2
    Nm, Ns = 2*(N-1), 2*(N-1)
    Nel = N^2
    NDOF = 2*(N+1)*(N+1)
    nodes = zeros(Int64, N+1, N+1)

    # Corners
    nodes[end,1], nodes[end,end] = (1,2)
    nodes[1,end], nodes[1,1] = (3,4)
    # Internal
    nodes[2:N,2:N] = reshape(5 : Ni+4, (N-1, N-1))
    # Left
    nodes[2:end-1,1] = Ni+5 : Ni+N+3
    # Bottom
    nodes[end,2:end-1] = Ni+N+4 : Ni+2*(N+1)
    # Right
    nodes[2:end-1,end] = Ni+2*N+3 : Ni+3*N+1
    # Top
    nodes[1,2:end-1] = Ni+3*N+2 : Ni+4*N

    # Generate connectivity of each element based on their node numbers.
    NC = zeros(Int64, Nel, 4)
    for j in 1:N
        for i in 1:N
            NC[i+(j-1)*N, :] = [nodes[end-j+1,i], nodes[end-j+1,i+1], nodes[end-j,i+1], nodes[end-j,i]]
        end
    end
    # Create square mesh over the domain.
    x, y = (0:dx:L, 0:dx:L)
    XY = zeros(Float64, (N+1)^2, 2)
    for j in 1:N+1
        for i in 1:N+1
            XY[i+(j-1)*(N+1), :] = [x[i], y[j]]
        end
    end

    elCoords = zeros(Float64, Nel, 4, 2)
    for j in 1:N
        for i in 1:N
            k = i+(j-1)*(N+1)
            ind = [k, k+1, k+N+2, k+N+1]
            elCoords[i+(j-1)*N, :, :] = XY[ind,:]
        end
    end
    DOFs = zeros(Int64, Nel, 4, 2)
    for i in 1:Nel
        DOFs[i, :, :] = reshape(hcat(2*NC[i,:]' .- 1, 2*NC[i,:]'), (4,2))
    end

    # Dependency matrix
    rowInd = 1 : 2*Nm
    colInd = vcat(repeat([3,4], N-1), repeat([7,8], N-1))
    data = ones(Int64, 2*Nm)
    D = sparse(rowInd, colInd, data)
    Nci = Nc + Ni
    Nim = Ni + Nm
    Ncim = Nc + Ni + Nm
    Nims = Ni + Nm + Ns
    Ncims = Nc + Nims
    Ns = [Nc, Ni, Nm, Ns, Nel, NDOF, Nci, Nim, Ncim, Nims, Ncims]

   return N, NC, XY, elCoords, DOFs, D, Ns;
end


__precompile__()

module HermiteQuadratureRules

using #Reduce,
      Compat,
      Base.Cartesian,
     # Distributions,
      SpecialFunctions

export  generate_weights,
        eval_on_nodes,
        herm_design,
        calc_weights,
        rule_lengths,
        gk_nodes,
        weighted_gk_weights

include("rules_grad_free.jl")
#include("rules.jl")
#include("distribution_derivatives.jl")

function __init__()
    setprecision(512)
#    Reduce.Rational(false)
    nothing
end

@generated function normal_moment_general(::Val{N}) where N
    expr = rcall(:(int(x^$N * exp(-x^2/2) / y, x, -infinity, infinity)))
    quote
        y = sqrt(big(2)π)
        $expr
    end
end
function normal_moment_general(N::Int)
    y = big(2)π
    rcall(:(int(x^N * exp(-x^2/2) / y, x, -infinity, infinity)))
end
@generated function normal_moment(::Val{N}) where N
    N%2 == 1 ? 0.0 : normal_moment_general(N)
end
@generated function poly_integral_tuple(::Val{N}) where N
    ntuple( i -> normal_moment_general(i) , Val(N))
end

function eval_poly(x::T, β) where T
    @boundscheck length(β) > 1
    @inbounds out = β[1]
    @inbounds for i ∈ 2:length(β)
        out = fma(out, x, β[i])
    end
    out
end

"""
Order is function val, and then derivatives.
"""
function herm_design(nodes, ::Type{T} = BigFloat) where T
    n = length(nodes)
    X = Matrix{T}(2n, 2n)
    buffer = T.(nodes)
    ∂buffer = similar(buffer)
    for iₙ ∈ 1:n
        X[2iₙ-1,1] = one(T)
        X[2iₙ,1] = zero(T)
    end
    for iₙ ∈ 1:n
        X[2iₙ-1,2] = buffer[iₙ]
        X[2iₙ,2] = one(T)
    end
    for j ∈ 2:2n-1
        ∂buffer .= buffer .* j
        buffer .*= nodes
        for iₙ ∈ 1:n
            X[2iₙ-1,j+1] = buffer[iₙ]
            X[2iₙ,j+1] = ∂buffer[iₙ]
        end
    end
    X
end
function herm_design_sym(nodes::AbstractVector{T}) where T
    n = length(nodes)
    X = Matrix{T}(4n+2, 4n+2)
    buffer = T.(nodes)
    ∂buffer = similar(buffer)
    for iₙ ∈ 1:2n+1
        X[2iₙ-1,1] = one(T)
        X[2iₙ,1] = zero(T)
    end
    for iₙ ∈ 1:n
        X[2iₙ-1,2] = -buffer[n-iₙ+1]
        X[2iₙ,2] = one(T)
    end
    X[2n+1,2] = zero(T)
    X[2n+2,2] = one(T)
    for iₙ ∈ 1:n
        X[2iₙ+1+2n,2] = buffer[iₙ]
        X[2iₙ+2+2n,2] = one(T)
    end
    for j ∈ 2:4n+1
        ∂buffer .= buffer .* j
        buffer .*= nodes
        for iₙ ∈ 1:n
            X[2iₙ-1,j+1] = (-1)^j* buffer[n-iₙ+1]
            X[2iₙ,j+1]  = (-1)^(j-1)*∂buffer[n-iₙ+1]
        end
        X[2n+1,j+1] = zero(T)
        X[2n+2,j+1] = one(T)
        for iₙ ∈ 1:n
            X[2iₙ+1+2n,j+1] = buffer[iₙ]
            X[2iₙ+2+2n,j+1] = ∂buffer[iₙ]
        end
    end
    X
end
function herm_design_sym_reversed_order(nodes::AbstractVector{T}) where T
    n = length(nodes)
    N = 4n+2
    X = Matrix{T}(N,N)
    buffer = Array{T}(uninitialized, n)
    ∂buffer = Array{T}(uninitialized, n)
    for j ∈ 1:N-2
        exponent = N - j
        @. ∂buffer = exponent * nodes ^ (exponent - 1)
        @. buffer = nodes ^ exponent
        for iₙ ∈ 1:n
            X[2iₙ-1,j] = (-1)^exponent * buffer[n-iₙ+1]
            X[2iₙ,j]  = (-1)^(exponent-1) * ∂buffer[n-iₙ+1]
        end
        X[2n+1,j] = zero(T)
        X[2n+2,j] = one(T)
        for iₙ ∈ 1:n
            X[2iₙ+1+2n,j] = buffer[iₙ]
            X[2iₙ+2+2n,j] = ∂buffer[iₙ]
        end
    end
    @. buffer = T(nodes)
    for iₙ ∈ 1:n
        X[2iₙ-1,N-1] = -buffer[n-iₙ+1]
        X[2iₙ,N-1] = one(T)
    end
    X[2n+1,N-1] = zero(T)
    X[2n+2,N-1] = one(T)
    for iₙ ∈ 1:n
        X[2iₙ+1+2n,N-1] = buffer[iₙ]
        X[2iₙ+2+2n,N-1] = one(T)
    end
    for iₙ ∈ 1:2n+1
        X[2iₙ-1,N] = one(T)
        X[2iₙ,N] = zero(T)
    end
    X
end

@generated function weight_mat(Xⁱ::AbstractMatrix{T}, ::Val{N}) where {N,T}
    quote
        weights = Matrix{BigFloat}($N, size(Xⁱ,2))
        for j ∈ 1:size(Xⁱ,2)
            @nexprs $N i -> begin
                weights[i,j] = normal_moment_general(Val{2(i-1)}()) * Xⁱ[2(i-1)+1,j]
            end
        end
        weights
    end
end
@generated function weight_vec(Xⁱ::AbstractMatrix{T}, ::Val{N}) where {N,T}
    quote
        weights = fill(zero(T), size(Xⁱ,2))
        for j ∈ 1:size(Xⁱ,2)
            @nexprs $N i -> begin
                weights[j] += normal_moment_general(Val{2(i-1)}()) * Xⁱ[2(i-1)+1,j]
            end
        end
        weights
    end
end
function weight_vec(Xⁱ::AbstractMatrix{T}) where T#Dynamic dispatch
    weight_vec(Xⁱ, Val{size(Xⁱ,1) ÷ 2}())::Vector{T}
end

function generate_weights(nodes)
    X = herm_design(nodes)
    weight_vec(inv(X))
end

function generate_weights_sym(nodes)
    X = herm_design_sym(nodes)
    weight_vec(inv(X))
end

function eval_on_nodes(f, nodes::AbstractVector{T}) where T
    out = Matrix{T}(2, length(nodes))
    for i ∈ eachindex(nodes)
        fn = f(nodes[i] + eps()*im )
        out[1,i] = imag(fn) / eps()
        out[2,i] = real(fn)
    end
    out
end
# function eval_on_nodes(f::Distributions.UnivariateDistribution, nodes::AbstractVector{T}) where T
#     out = Matrix{T}(2, length(nodes))
#     for i ∈ eachindex(nodes)
#         out[:,i] .= fg(f, nodes[i])
#     end
#     out
# end
# function eval_weighted_nodes(f::Distributions.UnivariateDistribution, nodes::AbstractVector{T}) where T
#     out = Matrix{T}(2, length(nodes))
#     for i ∈ eachindex(nodes)
#         out[:,i] .= fgw(f, nodes[i])
#     end
#     out
# end

# using Optim, ForwardDiff
# initial_n = BigFloat.(sort(rand(4)));
# node_weight_magnitude(nodes) = sum(x -> (x^8-1//(4length(nodes)+2) ), generate_weights_sym(nodes))
# td = TwiceDifferentiable(node_weight_magnitude, initial_n; autodiff = :forward)

# opt = optimize(td, initial_n, Newton())
end # module


__precompile__(true)

module GaussianFreeFields

import Contour,
       Interpolations,
       FFTW,
       SparseArrays,
       LinearAlgebra

export DGFF,
       fix_boundary_values,
       harmonicextension,
       inside,
       flowline,
       interpolate

function DGFF(n::Int)
    h = complex(zeros(n,n))
    for j=1:n
        for k=1:n
            h[j,k] = 1/sqrt(2.0)*(j+k == 2 ? 0 : (randn() + im*randn()) * 1/sqrt(sin((j-1)*pi/n)^2+ sin((k-1)*pi/n)^2))
        end
    end
    return real(n*1/sqrt(2)*FFTW.ifft(h))
end

function DGFF(m::Int,n::Int)
    h = complex(zeros(m,n))
    for j=1:m
        for k=1:n
            h[j,k] = 1/sqrt(2.0)*(j+k == 2 ? 0 :
                                  randn() + im*randn() *
                                  1/sqrt(sin((j-1)*pi/m)^2+(sin((k-1)*pi/n))^2))
        end
    end
    return real(n/sqrt(2)*FFTW.ifft(h))
end

"Return the array obtained by subtracting from `h` the
harmonic extension of the values around the edge of
the array `boundary_values`"
function fix_boundary_values(h::Array{Float64,2},
                             boundary_values::Array{Float64,2}=zeros(h))
    m,n = size(h)
    #sparse matrix with ones in top left, lower right positions
    cornersonly(m) = SparseArrays.sparse([1,m],[1,m],[1.0,1.0])
    #sparse matrix with ones along diagonal except top-left, lower-right
    eyenocorners(n) = SparseArrays.sparse(LinearAlgebra.I,n,n) - cornersonly(n)
    A = cornersonly(m)
    B = SparseArrays.sparse(LinearAlgebra.I,n,n)
    D_dense = zeros(n,n);
    for i=1:n-1
        D_dense[i,i+1] = -1;
        D_dense[i+1,i] = -1;
        D_dense[i,i] = 4;
    end
    D_dense[1,1] = 1
    D_dense[1,2] = 0
    D_dense[n,n-1] = 0
    D_dense[n,n] = 1
    D = SparseArrays.sparse(D_dense);
    E_dense = zeros(m,m)
    for i=1:m-1
        E_dense[i,i+1]=-1
        E_dense[i+1,i]=-1
    end
    E_dense[m,m-1] = 0
    E_dense[1,2] = 0
    E = SparseArrays.sparse(E_dense)
    Δ = kron(A,B) + kron(eyenocorners(m),D) + kron(E,eyenocorners(n))
    boundary = zeros(m*n)
    for i in 1:m-1
        boundary[n*i+1] = h[i+1,1] - boundary_values[i+1,1]
        boundary[n*i] = h[i,n] - boundary_values[i,n]
    end
    for i in 1:n-1
        boundary[i] = h[1,i] - boundary_values[1,i]
        boundary[end-n+i] = h[m,i] - boundary_values[m,i]
    end
    boundary[end] = h[m,n]

    return h - transpose(reshape(Array(LinearAlgebra.lu(Δ) \ boundary),n,m))
end

function harmonicextension(h::Array{T,2},vertices::Set) where T<:Number
    m,n = size(h)
    function neighbors(i::Int,j::Int)
        return [(i-1,j),(i+1,j),(i,j-1),(i,j+1)]
    end
    vertexmap = Dict(map(reverse,enumerate([(i,j) for i=1:m,j=1:n][:])));
    I = Int64[]
    J = Int64[]
    V = Float64[]
    b = zeros(m*n)
    for v in keys(vertexmap)
        if v in vertices
            k = vertexmap[v]
            push!(I,k); push!(J,k); push!(V,1)
            b[k] = h[k]
        else
            k = vertexmap[v]
            for nb in neighbors(v...)
                try
                    j = vertexmap[nb]
                    push!(I,k); push!(J,j); push!(V,1)
                    push!(I,k); push!(J,k); push!(V,-1)
                catch
                end
            end
        end
    end
    A = SparseArrays.sparse(I,J,V)
    return reshape(LinearAlgebra.lu(A) \ b,m,n)
end

# `inside` determines whether a point p is inside a curve γ
# by checking the number of points of intersection between γ
# and a ray emanating from p
function inside(p::Array{Float64,1},γ::Array{Float64,2})
    if γ[1,:] != γ[length(γ[:,1]),:]
        return false
    end
    cntr = 0; m = sqrt(2); # the slope is an arbitrary irrational number
    for i=1:length(γ[:,1])-1
        (x1,y1,x2,y2) = (γ[i,1],γ[i,2],γ[i+1,1],γ[i+1,2])
        if ((y2 - p[2] - m*(x2-p[1]))*(y1 - p[2] - m*(x1-p[1])) < 0)
            if (m*p[1]*x1 - p[2]*x1 - m*p[1]*x2 + p[2]*x2 - x2*y1 + x1*y2) /
                (m*x1 - m*x2 - y1 + y2)  - p[1] > 0
                cntr += 1
            end
        end
    end
    return isodd(cntr)
end

import Interpolations.interpolate
function interpolate(h::Array{T,2}) where T <: Real
    return interpolate(h,
                       Interpolations.BSpline(Interpolations.Linear()),
                       Interpolations.OnGrid())
end

import Base.getindex
getindex(h::Interpolations.BSplineInterpolation,
         z0::Complex) = h[real(z0),imag(z0)]

"""
Find the flow line of angle θ in the field h.
In other words, solve the DE η'(t) = exp(ih(η(t))/χ + iθ)".
"""
function flowline(h::Interpolations.BSplineInterpolation,
                  z0::Complex,
                  χ::Real,
                  θ::Real;
                  δ::Real=0.01,
                  S::Set{Complex}=Set{Complex}())
    (a,b) = size(h.coefs)
    η = [z0]
    while 1.0 ≤ real(η[end]) ≤ a && 1.0 ≤ imag(η[end]) ≤ b
        push!(η, η[end] + δ * exp(im*h[η[end]]/χ + im*θ))
        w = η[end]
        for z in S
            if abs2(z-w) < 1e-3
                return η
            end
        end
    end
    return η
end

flowline(h::Array{Float64,2},
         z0::Complex{Float64},
         χ::AbstractFloat,
         θ::AbstractFloat;
         δ::AbstractFloat=0.01,
         S::Set{Complex{Float64}}=Set()) = flowline(interpolate(h),z0,χ,θ,δ,S)

end # module


__precompile__(true)
# This file is part of IntegerSequences.
# Copyright Peter Luschny. License is MIT.

(@__DIR__) ∉ LOAD_PATH && push!(LOAD_PATH, (@__DIR__))

module Triangles

using Nemo, SeqUtils

export ModuleTriangles
export Triangle, ZTriangle, QTriangle, RecTriangle
export TriangularNumber, isTriangular, assertTriangular
export ShowAsΔ, ShowAsMatrix, Row, RowSums
export fromΔ, toΔ, TriangleToList

"""
* Triangle, ZTriangle, QTriangle, RecTriangle, TriangularNumber, isTriangular, assertTriangular, ShowAsΔ, ShowAsMatrix, Row, RowSums, fromΔ, toΔ, TriangleToList
"""
const ModuleTriangles = ""

#    T(0,0)                          row 0
#    T(1,0) T(1,1)                   row 1
#    T(2,0) T(2,1) T(2,2)            row 2
#    T(3,0) T(3,1) T(3,2) T(3,3)     row 3
#    col 0  col 1  col 2  col 3
#
# The two dimensional index is ``(0, 0)``-based and displayed row-wise as
# a lower triangular array. In particular note that a `Triangle` has rows
# and cols from ``0`` to dim ``-1``.

abstract type AbstractTriangle end

"""
A Triangle is a list of rows. The rows of the triangle are generated by a function `(n::Int, k::Int) → gen::fmpz` for ``0 ≤ k ≤ n`` and ``0 ≤ n <``dim.
"""
struct Triangle <: AbstractTriangle
    dim::Int
    gen::Function
    function Triangle(dim::Int, gen::Function)
        dim <= 0 && throw(ArgumentError("dim has to be a positive integer."))
        new(dim, (n::Int, k::Int) -> gen(n, k)::fmpz )
    end
end

"""
Return the first row of the triangle.
"""
function Base.iterate(T::Triangle)
    ([T.gen(0, 0)], 1)
end

"""
Return the next row of the triangle.
"""
function Base.iterate(T::Triangle, n)
    n >= T.dim && return nothing
    row = [T.gen(n, k) for k in 0:n]
    (row, n + 1)
end

Base.length(T::Triangle) = T.dim
Base.eltype(T::Triangle) = fmpz

# --------------------------------------------------

"""
A recursive triangle `RecTriangle` is a subtype of `AbstractTriangle`.
The rows of the triangle are generated by a function `gen(n, k, prevrow)` defined for ``n ≥ 0`` and ``0 ≤ k ≤ n``. The function returns value of type Nemo.fmpz.

The parameter prevrow is a function which returns the values of `row(n-1)` of the triangle and 0 if ``k < 0`` or ``k > n``. The function prevrow is provided
by an instance of `RecTriangle` and must not be defined by the user.
"""
struct RecTriangle <: AbstractTriangle
    dim::Int
    A::Array{fmpz}
    gen::Function  # generating function
    function RecTriangle(dim::Int, gen::Function)
        dim <= 0 && throw(ArgumentError("dim has to be a positive integer."))
        new( dim,
             fill(ZZ(0), dim),
             (n::Int, k::Int, f::Function) -> gen(n, k, f)::fmpz)
    end
end

function Base.iterate(T::RecTriangle)
    top = T.gen(0, 0, () -> undefined)
    T.A[1] = ZZ(top)
    ([top], 1)
end

"""
Return the next row of the triangle.
"""
function Base.iterate(T::RecTriangle, n)
    @inline prevrow(A, n) = (k) -> (k<0 || k>n) ? ZZ(0) : A[k+1]
    n == T.dim && return nothing
    F(n, k) = T.gen(n, k, prevrow(T.A, n))
    row = [F(n, k) for k in 0:n]
    for k in 1:n+1 T.A[k] = row[k] end
    (row, n + 1)
end

Base.length(R::RecTriangle) = R.dim
Base.eltype(R::RecTriangle) = fmpz

# --------------------------------------------------

"""
Return the``n``-th triangular number.
"""
TriangularNumber(n) = div(n * (n + 1), 2)

"""
Is ``n`` a triangular number?
"""
isTriangular(n) = n == TriangularNumber(isqrt(2n))

"""
Return the square root of ``2n`` or throw an ArgumentError if ``n`` is not a triangular number.
"""
function assertTriangular(n)
    dim = isqrt(2n)
    n ≠ TriangularNumber(dim) && throw(ArgumentError("This is not a triangular array!"))
    return dim
end

"""
Return a trianguler array with ``n`` rows set to ``0`` (type ZZ).
"""
ZTriangle(dim::Int) = zeros(ZZ, TriangularNumber(dim))

"""
Return a trianguler array with ``n`` rows set to ``0`` (type QQ).
"""
QTriangle(dim::Int) = zeros(QQ, TriangularNumber(dim))

"""
Return the triangle as a list of rows.
"""
TriangularArray(T::AbstractTriangle) = [row for row in T]

"""
Return the triangle as a list of integers.
"""
TriangleToList(T::AbstractTriangle) = [k for row in T for k in row]

"""
Return row ``n (0 ≤ n)`` of the lower triangular matrix T or the row ``n`` in reversed order if `reversed` is set true.
"""
function Row(T, n, reversed=false)
    s = length(T)
    (s == 0 || n < 0) && return []
    assertTriangular(s)

    t = TriangularNumber(n+1)
    s < t && error("This row is not in the matrix.")
    R = reversed ? range(t, step=-1, stop=t-n) : range(t-n, step=1, stop=t)
    return [T[k] for k in R]
end

"""
Return the row sums of a triangle, if `alternate=true` then the alternating row sums.
"""
function RowSums(T, alternate = false)
    n = length(T)
    n == 0 && return T
    len = assertTriangular(n)
    S = zeros(ZZ, len)
    lo = hi = 0
    step = 0

    while hi < n
        if alternate
            s = sum((-1)^k * T[lo+k+1] for k in 0:hi - lo)
        else
            s = sum(T[k+1] for k in lo:hi)
        end
        step += 1
        S[step] = s
        hi += 1
        lo, hi = hi, hi + step
    end
    S
end

"""
Display a lower triangular matrix.
"""
function ShowAsΔ(T::AbstractTriangle, separator=", ")
    for row in T
        for k in row print(k, separator) end
        println()
    end
end

"""
Display an array as a lower triangular matrix.
"""
function ShowAsΔ(T::AbstractArray, separator=", ")
    if typeof(T) == Array{Array{fmpz,1},1}
        for row in T
            for k in row print(k, separator) end
            println()
        end
    elseif typeof(T) == Array{fmpz,2}
        for row in 1:size(T,1)
            T[row,:]' |> Println
        end
    elseif typeof(T) == Array{fmpz,1}
        dim = assertTriangular(length(T))
        low = high = 1
        for r in 2:dim+1
            [T[j] for j = low:high] |> Println
            low, high = high + 1, high + r
        end
    else
        println(T)
    end
end

"""
Convert a lower triangular array to a square matrix.
"""
function fromΔ(T::AbstractTriangle)
    L = TriangleToList(T)
    dim = assertTriangular(length(L))

    A = fill(ZZ(0), dim, dim)
    k = 1
    for r in 1:dim
        for j = 1:r
            A[r, j] = L[k]
            k += 1
        end
    end
    A
end

"""
Convert a square matrix to a list using only the ``T(n,k)`` with ``0 ≤ k ≤ n``. Returns a ZArray.
"""
function toΔ(M)
    dim = size(M, 1)
    T = ZTriangle(dim)
    k = 1

    if typeof(M[1, 1]) == Nemo.fmpq
        for r in 1:dim, j = 1:r
            T[k] = numerator(M[r, j])
            k += 1
        end
    else
        for r in 1:dim, j = 1:r
            T[k] = M[r, j]
            k += 1
        end
    end
    T
end

"""
Print the triangle in matrix form.
"""
ShowAsMatrix(T) = println(fromΔ(T))

#START-TEST-########################################################

using Test

function test()
    BinTri(n) = [binomial(k, j) for k in 0:n - 1 for j in 0:k]

    X(n, k) = ((k > n || k < 0) && return 0;
        (n == 0 && k == 0) && return 1;
        4 * X(n - 1, k - 1) + (4 * n - 1) * X(n - 1, k); )

    T(n) = [X(j, k) for j in 0:n for k in 0:j]

    @testset "Triangles" begin
        t = T(6)
        a = [3465, 8784, 7136, 2304, 256]
        b = Row(t, 4)
        @test all(a .== b)

        # RowSums
        t = BinTri(8)
        a = [1, 2, 4, 8, 16, 32, 64, 128]
        b = RowSums(t)
        @test all(a .== b)

        a = [1, 0, 0, 0, 0, 0, 0, 0]
        b = RowSums(t, true)  # alternating sum
        @test all(a .== b)
    end
end

function demo()

    println("\n--- Using Triangle\n")

    function X(n, k)
        (k > n || k < 0) && return fmpz(0)
        (n == 0 && k == 0) && return fmpz(1)
        ZZ( 4 * X(n - 1, k - 1) + (4 * n - 1) * X(n - 1, k) )
    end

    TX(dim) = Triangle(dim, X)
    ShowAsΔ(TX(6), "\t")

    println(TriangleToList(TX(6)))
    ShowAsΔ(TriangleToList(TX(6)))

    TL = TriangleToList(TX(6))
    for k in 0:5
        Println(Row(TL, k))
    end

    println()
    ShowAsMatrix(TX(6))

    # The above triangle is much better implemented with a RecTriangle!

    println("\n--- Using RecTriangle\n")

    function r225478(n, k, prevrow::Function)
        (k == 0 && n == 0) && return fmpz(1)
        4 * prevrow(k - 1) + (4 * n - 1) * prevrow(k)
    end

    I225478(dim) = RecTriangle(dim, r225478)
    for row in I225478(6) Println(row) end

    println()
    ShowAsΔ(RecTriangle(6, r225478), "\t")

    println()
    TA225478 = TriangularArray(I225478(6))
    ShowAsΔ(TA225478)

    println()
    println("ΔToListToΔ (test)")
    ShowAsΔ(TriangleToList(I225478(6)))

    println()
    ShowAsMatrix(I225478(6))

    println()
    GC.gc()
    @time TriangularArray(TX(16))
    @time TriangularArray(I225478(16))
end

"""
TriangularArray(TX(16))
    0.063180 seconds (785.95 k allocations: 11.996 MiB)
TriangularArray(I225478(16))
    0.000264 seconds (653 allocations: 15.781 KiB)
"""
function perf()
    #GC.gc()
    #@time TriangularArray(TX(20))
    #@time TriangularArray(I225478(20))
 end

 function main()
     test()
     demo()
     perf()
 end

 main()

 end # module


# Dev code for free space solver for modified stokes
push!(LOAD_PATH, string(pwd(),"/src"))

using ModifiedStokesSolver
using Base.Test

@testset "PeriodicSolver" begin
    # Setup problem
    srand(1)
    L = 4.0
    lambda = 5.6

    sigma = L/20
    x0 = 0.12376938253
    y0 = 0.21938752311

    xi = 1/sqrt(2*sigma^2)
    # Reference solution derived by applying modified biharmonic to Gaussian, plus a constant field
    # ufuncr(r) = exp(-xi^2*r^2)
    ffuncr(r) = -4*xi^2*exp(-xi^2*r^2)*( lambda^2*(1-r^2*xi^2) + 4*xi^2*(2 - 4*r^2*xi^2 + r^4*xi^4) ) - lambda^2
    s1r(r) = -2*xi^2*exp(-xi^2*r^2)*(2*xi^2*r^2-1) + 1
    s2r(r) = 4*xi^4*exp(-xi^2*r^2)
    pr(r) = 2*xi^2*exp(-xi^2*r^2)*(lambda^2 + 8*xi^2 - 4*xi^4*r^2)

    r1(x,y) = x-x0
    r2(x,y) = y-y0
    r(x,y) = sqrt( (x-x0)^2 + (y-y0)^2 )

    c1 = rand()
    c2 = rand()
    ffunc1(x, y) = c1*ffuncr(r(x,y))
    ffunc2(x, y) = c2*ffuncr(r(x,y))

    ufunc1(x, y) = -1*(c1*s1r(r(x,y)) + s2r(r(x,y))*r1(x,y)*(c1*r1(x,y) + c2*r2(x,y)))
    ufunc2(x, y) = -1*(c2*s1r(r(x,y)) + s2r(r(x,y))*r2(x,y)*(c1*r1(x,y) + c2*r2(x,y)))
    pfunc(x, y) = -1*(pr(r(x,y))*(c1*r1(x,y) + c2*r2(x,y)))

    function solve(N)
        # Grid domain
        x = linspace(-L/2, L/2, N+1)
        x = x[1:end-1] # Note that intervals are closed/open!
        y = x
        X,Y = ndgrid(x, y)
        # Nonuniform test points
        M = 1000
        xt = L*(1/2-rand(M, 1))
        yt = L*(1/2-rand(M, 1))

        # Get rhs
        F1 = ffunc1.(X,Y)
        F2 = ffunc2.(X,Y)

        # Get reference solution
        U1ref = ufunc1.(X,Y)
        U2ref = ufunc2.(X,Y)

        # Compute solution with FFT method
        U1, U2, ut1, ut2, U1x, U1y, U2x, U2y = per_modstokes(F1, F2, L, lambda, xt, yt; zerok0=false, ifgrad=true)

        # Compute error on grid
        E1 = U1 - U1ref
        E2 = U2 - U2ref
        Unorm = norm(sqrt.( vec(U1).^2 + vec(U2).^2 ), Inf)
        relerr1 = norm(E1[:], Inf) / Unorm
        relerr2 = norm(E2[:], Inf) / Unorm
        relerr = max(relerr1, relerr2)

        # Compute error at nonuniform test points
        enu1 = ut1-ufunc1.(xt,yt)
        enu2 = ut2-ufunc2.(xt,yt)
        relerrnu1 = norm(enu1, Inf) / norm(ut1, Inf)
        relerrnu2 = norm(enu2, Inf) / norm(ut2, Inf)
        relerrnu = max(relerrnu1, relerrnu2)

        # Compute gradient with spectral differentiation and compare
        U1xd, U1yd = per_gradient(U1, L)
        U2xd, U2yd = per_gradient(U2, L)
        @test maximum(abs.( U1x-U1xd )) / maximum(abs.( U1x )) < 21e-14
        @test maximum(abs.( U1y-U1yd )) / maximum(abs.( U1y )) < 2e-14
        @test maximum(abs.( U2x-U2xd )) / maximum(abs.( U2x )) < 2e-14
        @test maximum(abs.( U2y-U2yd )) / maximum(abs.( U2y )) < 2e-14

        # Check Divergence
        Udiv = U1x+U2y
        diverr_rel = maximum(abs.(Udiv))/Unorm
        @test diverr_rel < 5e-14

        # Compute numerical derivatives to check that we actually satisfy the PDE
        # using finite differences
        h = X[2]-X[1]
        int = 2:N-1
        U1x = (U1[int+1,int]-U1[int-1,int])/(2*h)
        U2y = (U2[int,int+1]-U2[int,int-1])/(2*h)

        P = pfunc.(X,Y)
        Pnorm = norm( vec(P), Inf)

        Udiv = U1x+U2y
        diverr_rel = maximum(abs.(Udiv))/Unorm

        LP = (-4*P[int,int] + P[int+1,int] + P[int-1,int] + P[int,int+1] + P[int,int-1])/h^2
        Px = (P[int+1,int]-P[int-1,int])/(2*h)
        Py = (P[int,int+1]-P[int,int-1])/(2*h)
        LU1 = (-4*U1[int,int] + U1[int+1,int] + U1[int-1,int] + U1[int,int+1] + U1[int,int-1])/h^2
        LU2 = (-4*U2[int,int] + U2[int+1,int] + U2[int-1,int] + U2[int,int+1] + U2[int,int-1])/h^2

        pde1 = lambda^2*U1[int,int] - LU1 + Px - F1[int,int]
        pde2 = lambda^2*U2[int,int] - LU2 + Py - F2[int,int]

        pdeerr1_rel = maximum(abs.(pde1))/Unorm
        pdeerr2_rel = maximum(abs.(pde2))/Unorm
        pdeerr_rel = max(pdeerr1_rel, pdeerr2_rel)

        return relerr, relerrnu, diverr_rel, pdeerr_rel
    end

    # Solve above problem for a variety of N
    iters = 3;
    diverr_list = zeros(iters)
    pdeerr_list = zeros(iters)
    N_list = zeros(iters)
    for i=0:iters-1
        N = 64*2^i
        relerr, relerrnu, diverr_rel, pdeerr_rel = solve(N)
        @test relerr < 1e-13
        @test relerrnu < 1e-13
        N_list[i+1] = N
        diverr_list[i+1] = diverr_rel
        pdeerr_list[i+1] = pdeerr_rel
    end
    # Estimate convergence constant of FD test (should be 2)
    A = [ones(iters) -log.(N_list)]
    div_conv = (A\log.(diverr_list))[2]
    pde_conv = (A\log.(pdeerr_list))[2]
    @test abs(div_conv - 2) < 0.05
    @test abs(pde_conv - 2) < 0.05
end


@testset "NumField/NfRel/Conjugates.jl" begin
  Qx, x = QQ["x"]
  K, a = NumberField(x^2 - 2, "a")
  Kt, t = K["t"]
  g = t^3 + a*t + 5
  L, b = NumberField(g, "b")
  r, s = @inferred signature(L)
  @test (r, s) == (2, 2)
  plc = @inferred infinite_places(L)
  @test count(isreal, plc) == 2
  @test count(!isreal, plc) == 2

  Labs, LabstoL, = absolute_field(L)
  c = conjugates(b, 64)
  cc = conjugates(LabstoL\b, 64)

  for _c in c
    @test count(_cc -> overlaps(_cc, _c), cc) == count(_cc -> overlaps(_cc, _c), c)
  end

  Ly, y = L["y"]
  M, m = number_field(y^2 + y + b - 5, "c")
  r, s = @inferred signature(M)
  @test (r, s) == (4, 4)

  plc = @inferred infinite_places(M)
  @test count(isreal, plc) == 4
  @test count(!isreal, plc) == 4

  Mabs, MabstoM, = absolute_field(M)
  Mabsabs, MabsabstoMabs = absolute_field(Mabs)

  o = rand(M, -1:1)

  c = conjugates(o, 64)
  cc = conjugates(MabsabstoMabs\(MabstoM\o), 64)

  for _c in c
    @test count(_cc -> overlaps(_cc, _c), cc) == count(_cc -> overlaps(_cc, _c), c)
  end
end


struct Dogleg end

function (dl::Dogleg)(state, B, Δ)
    p = similar(state.x)
    ∇f = state.∇f
    norm_∇f = norm(∇f)
    prod = ∇f' * B * ∇f
    pᶜ = -norm_∇f^2 / prod*∇f
    norm_pᶜ = norm(pᶜ)
    if Δ ≤ norm_pᶜ
        p .= Δ / norm_pᶜ * pᶜ
    else
        isposdef(B) || error("Dogleg method only works for positive definite B")
        pᴮ = -inv(B)*∇f
        norm_pᴮ = norm(pᴮ)
        if Δ ≥ norm_pᴮ
            p .= pᴮ
        else
            pᴮpᶜ = pᴮ⋅pᶜ
            norm_pᴮ = norm_pᴮ
            r = (norm_pᴮ^2 - pᴮpᶜ) / (norm_pᶜ^2 + norm_pᴮ^2 - 2pᴮpᶜ)
            @. p = r*pᶜ + (1-r)*pᴮ
        end
    end
    return p
end


# SKCE

function CalibrationErrors.unsafe_skce_eval_targets(
    kernel::SqExponentialKernel{Euclidean},
    p::MvNormal,
    y::AbstractVector{<:Real},
    p̃::MvNormal,
    ỹ::AbstractVector{<:Real},
)
    # extract the parameters
    μ = p.μ
    Σ = p.Σ
    μ̃ = p̃.μ
    Σ̃ = p̃.Σ

    # compute inverse scaling matrices
    invA = LinearAlgebra.I + Σ
    invB = LinearAlgebra.I + Σ̃
    invC = invA + Σ̃

    res =
        kernel(y, ỹ) - mvnormal_gaussian_kernel(invA, μ, ỹ) -
        mvnormal_gaussian_kernel(invB, μ̃, y) + mvnormal_gaussian_kernel(invC, μ, μ̃)

    return res
end

function mvnormal_gaussian_kernel(invA, y, ỹ)
    return exp(-(LinearAlgebra.logdet(invA) + invquad_diff(invA, y, ỹ)) / 2)
end

# UCME

# predicted normal distributions with squared exponential kernel for the targets
function CalibrationErrors.unsafe_ucme_eval_targets(
    kernel::SqExponentialKernel{Euclidean},
    p::MvNormal,
    y::AbstractVector{<:Real},
    testp::MvNormal,
    testy::AbstractVector{<:Real},
)
    # compute inverse scaling matrix
    invA = LinearAlgebra.I + p.Σ

    return kernel(y, testy) - mvnormal_gaussian_kernel(invA, p.μ, testy)
end

# kernels with input transformations
# TODO: scale upfront?

function CalibrationErrors.unsafe_skce_eval_targets(
    kernel::TransformedKernel{
        SqExponentialKernel{Euclidean},<:Union{ScaleTransform,ARDTransform,LinearTransform}
    },
    p::MvNormal,
    y::AbstractVector{<:Real},
    p̃::MvNormal,
    ỹ::AbstractVector{<:Real},
)
    # obtain the transform
    t = kernel.transform

    return CalibrationErrors.unsafe_skce_eval_targets(
        SqExponentialKernel(), apply(t, p), t(y), apply(t, p̃), t(ỹ)
    )
end

function CalibrationErrors.unsafe_ucme_eval_targets(
    kernel::TransformedKernel{
        SqExponentialKernel{Euclidean},<:Union{ScaleTransform,ARDTransform,LinearTransform}
    },
    p::MvNormal,
    y::AbstractVector{<:Real},
    testp::MvNormal,
    testy::AbstractVector{<:Real},
)
    # obtain the transform
    t = kernel.transform

    # `testp` is irrelevant for the evaluation and therefore not transformed
    return CalibrationErrors.unsafe_ucme_eval_targets(
        SqExponentialKernel(), apply(t, p), t(y), testp, t(testy)
    )
end

## utilities

# internal `apply` avoids type piracy of transforms
function apply(t::ScaleTransform, d::MvNormal)
    s = first(t.s)
    return MvNormal(t(d.μ), s^2 * d.Σ)
end
function apply(t::ARDTransform, d::MvNormal)
    return MvNormal(t(d.μ), scale_cov(d.Σ, t.v))
end
# `X_A_Xt` only works with StridedMatrix: https://github.com/JuliaStats/PDMats.jl/issues/96
apply(t::LinearTransform, d::MvNormal) = Matrix(t.A) * d

# scale covariance matrix
# TODO: improve efficiency
scale_cov(Σ::PDMats.ScalMat, v) = PDMats.PDiagMat(v .^ 2 .* Σ.value)
scale_cov(Σ::PDMats.PDiagMat, v) = PDMats.PDiagMat(v .^ 2 .* Σ.diag)
scale_cov(Σ::PDMats.AbstractPDMat, v) = PDMats.X_A_Xt(Σ, LinearAlgebra.diagm(v))

invquad_diff(A::PDMats.ScalMat, x, y) = Distances.sqeuclidean(x, y) / A.value
function invquad_diff(
    A::PDMats.PDiagMat, x::AbstractVector{<:Real}, y::AbstractVector{<:Real}
)
    n = length(x)
    length(y) == n || throw(DimensionMismatch("x and y must be of the same length"))
    size(A) == (n, n) ||
        throw(DimensionMismatch("size of A is not consistent with x and y"))
    return sum(abs2(xi - yi) / wi for (xi, yi, wi) in zip(x, y, A.diag))
end
invquad_diff(A::PDMats.AbstractPDMat, x, y) = PDMats.invquad(A, x .- y)


using Base.Test
using Tensors

# Single mode QR

t = rand([Mode(k,2) for k in 1:4])
q,r = qr(t, 3)
@test_approx_eq_eps(norm(t - q*r), 0, 1e-12)

# Multiple modes QR

t = rand([Mode(k,2) for k in 1:4])
q,r = qr(t, Any[3,1], 5)
@test_approx_eq_eps(norm(t - q*r), 0, 1e-12)

# Single mode SVD

t = rand([Mode(k,2) for k in 1:4])
u,s,v = svd(t, 3)
@test_approx_eq_eps(norm(t - u*scale(s,v)), 0, 1e-12)

# Multiple modes SVD

t = rand([Mode(k,2) for k in 1:4])
u,s,v = svd(t, Any[4,2], 5)
@test_approx_eq_eps(norm(t - scale(u,s)*v), 0, 1e-12)


using Pkg
pkg"activate ."
pkg"add JuMP"
pkg"add Cbc"
pkg"add Juniper"
pkg"add Ipopt"

using JuMP, Cbc, Juniper
# Escolhendo o modelo
model = Model(Cbc.Optimizer)
# Definindo os parâmetros que, por sinal, coloquei aleatórios, pois não sei as entradas
m = 4
n = 4
c = rand(1:4, n,m)
a = rand(0:0, m)
b = rand(1000:1000,n)
c[pi[j],pi[i]] * x[pi[i],pi[g]]
# Implementando realmente o modelo
@variable(model,x[i = 1:n, g = 1:m], Bin)
@objective(model, Max, t)
@constraint(model, [i = 1:n], sum(x[i,g] for  g = 1:m) == 1) # 1º restrição
@constraint(model, [i = 1:n], sum(x[i,g] for i = 1:n-1, g = 1:m ) ≤ b[g]) # 2º restrição
@constraint(model, [i = 1:n], sum(x[i,g] for i = 1:n-1, g = 1:m ) ≥ a[g]) # 3º restrição
#@constraint(model, [i = 1:n - 1, g = 1:m], - sum(max(0,(c[i,j])) * x[i,g] + w[i,g] for j = i+1:n) <= 0) # 4º restrição
#@constraint(model, [i = 1:n - 1, g = 1:m], - sum(c[i,j]*x[j,g] +  sum(min(0,c[i,j]) * (1 - x[i,g]) + w[i,g]) for j = i+1:n)  <= 0) # 5º restrição

optimize!(model) # Otimizando o modelo

println("Valor da função objetivo = ", objective_value(model))
value.(x)


function dftReg(imgRef::AbstractArray{Complex{T},N},imgF::AbstractArray{Complex{T},N},usfac) where {T, N}
    dftReg(CPU1(),imgRef,imgF,usfac)
end
"
`dftReg{N}(imgRef::AbstractArray{Complex{Float64},N},imgF::AbstractArray{Complex{Float64},N},usfac)`

Main internal function : takes the Fourier transforms of the arrays to register as inputs (`imgRef`/`imgF`) and returns a dictionary containing the residual error and the shift along the dimensions of the arrays, with the level of subpixel precision provided by `usfac`"
function dftReg(resource::CPU1,imgRef::AbstractArray{Complex{T},N},imgF::AbstractArray{Complex{T},N},usfac) where {T, N}
    if usfac==0
        ## Compute error for no pixel shift
        CCmax = sum(imgRef.*conj(imgF))
        rfzero = sum(abs2, imgRef)
        rgzero = sum(abs2, imgF)
        error = 1 - CCmax*conj(CCmax)/(rgzero*rfzero)
        diffphase = atan(imag(CCmax),real(CCmax))
        output = Dict("error" => error)
    elseif usfac==1
        ## Whole-pixel shift - Compute crosscorrelation by an IFFT and locate the peak
        L = length(imgRef)
        CC = ifft(imgRef.*conj(imgF))
        loc = argmax(abs.(CC))
        CCmax=CC[loc]
        rfzero = sum(abs2, imgRef)/L
        rgzero = sum(abs2, imgF)/L
        error = abs(1 - CCmax*conj(CCmax)/(rgzero*rfzero))
        diffphase = atan(imag(CCmax),real(CCmax))

        indi = size(imgRef)
        ind2 = tuple([div(x,2) for x in indi]...)

        #locI = [ind2sub(indi,loc)...]
        locI = [Tuple(loc)...]

        shift = zeros(size(locI))
        for i in eachindex(locI)
            if locI[i]>ind2[i]
                shift[i]=locI[i]-indi[i]-1
            else shift[i]=locI[i]-1
            end
        end

        output = Dict("error"=>error,"shift"=>shift,"diffphase"=>diffphase)
    else
        ## Partial pixel shift

        ##First upsample by a factor of 2 to obtain initial estimate
        ##Embed Fourier data in a 2x larger array
        dimR = [size(imgRef)...]
        ranges = [(x+1-div(x,2)):(x+1+div(x-1,2)) for x in dimR]
        dimRL = map(x -> x*2, dimR)
        CC = zeros(Complex{Float64},tuple(dimRL...))
        CC[ranges...] = fftshift(imgRef).*conj(fftshift(imgF))

        ##  Compute crosscorrelation and locate the peak
        CC = ifft(ifftshift(CC))
        loc = argmax(abs.(CC))

        indi = size(CC)
        #locI = [ind2sub(indi,loc)...]
        locI = [Tuple(loc)...]
        CCmax = CC[loc]
        ## Obtain shift in original pixel grid from the position of the crosscorrelation peak

        ind2 = tuple([div(x,2) for x in indi]...)

        shift = zeros(size(locI))
        for i in eachindex(locI)
            if locI[i]>ind2[i]
                shift[i]=locI[i]-indi[i]-1
            else shift[i]=locI[i]-1
            end
        end
        shift = shift/2

        ## If upsampling > 2, then refine estimate with matrix multiply DFT
        if usfac > 2
            ### DFT Computation ###
            # Initial shift estimate in upsampled grid
            shift = round.(Integer,shift*usfac)/usfac
            dftShift = div(ceil(usfac*1.5),2) ## center of output array at dftshift+1
            ## Matrix multiplies DFT around the current shift estimate
            CC = conj(dftups(imgF.*conj(imgRef),ceil(Integer,usfac*1.5),usfac,dftShift.-shift*usfac))/(prod(ind2)*usfac^N)
            ## Locate maximum and map back to original pixel grid
            loc = argmax(abs.(CC))
            # locI = ind2sub(size(CC),loc)
            locI = Tuple(loc)
            CCmax = CC[loc]
            rg00 = dftups(imgRef.*conj(imgRef),1,usfac)[1]/(prod(ind2)*usfac^N)
            rf00 = dftups(imgF.*conj(imgF),1,usfac)[1]/(prod(ind2)*usfac^N)
            locI = map((x) -> x - dftShift - 1,locI)

            for i in eachindex(shift)
                shift[i]=shift[i]+locI[i]/usfac
            end

        else
            rg00 = sum(imgRef.*conj(imgRef))/(prod(indi))
            rf00 = sum(imgF.*conj(imgF))/(prod(indi))
        end
        error = 1 - CCmax*conj(CCmax)/(rg00*rf00)
        error = sqrt(abs.(error))
        diffphase = atan(imag(CCmax),real(CCmax))
        ## If its only one row or column the shift along that dimension has no effect. Set to zero.
        shift[[div(x,2) for x in size(imgRef)].==1].=0

        output = Dict("error"=>error,"shift"=>shift,"diffphase"=>diffphase)

    end
    output
end

"
`dftups{T,N}(inp::AbstractArray{T,N},no,usfac::Int=1,off=zeros(N))`

Upsampled DFT by matrix multiplication, computes an upsampled DFT in just a small region. `no` is the size of the region in pixels, `offset` the position in the full array, `usfac` the upsampling parameter."
function dftups(inp::AbstractArray{T,N},no,usfac::Int=1,offset=zeros(N)) where {T,N}
    sz = [size(inp)...]
    permV = 1:N
    for i in permV
        inp = permutedims(inp,[i;deleteat!(collect(permV),i)])
        kern = exp.((-1im*2*pi/(sz[i]*usfac))*((0:(no-1)).-offset[i])*transpose(ifftshift(0:(sz[i]-1)).-floor(sz[i]/2)))
        d = size(inp)[2:N]
        inp = kern * reshape(inp, Val(2))
        inp = reshape(inp,(no,d...))
    end
    permutedims(inp,collect(ndims(inp):-1:1))
end

"
`subPixShift(imgft::AbstractArray{Complex{Float64}},shift::Array{Float64,1})`
    Shift the image `imgft` (in Fourier space) by the amount provided in the vector `shift`."
function subPixShift(imgft::AbstractArray{Complex{T},N},shift::Array{Float64,2},diffphase::Array{Float64,1}) where {T,N}
    sz = [size(imgft)[1:(N-1)]...]
    nim = size(imgft)[N]
    Z=0
    resh = [repeat([1],inner=N-1)...,nim]
    for i in eachindex(sz)
        shifti = ifftshift((-div(sz[i],2)):(ceil(Integer,sz[i]/2)-1)).*shift[:,i]'/sz[i]
        reshTemp = copy(resh)
        reshTemp[i] = size(shifti)[1]
        Z = Z .- reshape(shifti,reshTemp...)
    end

    Greg = imgft .* exp.(2im*pi*Z)
    Greg = Greg .* reshape(exp.(1im*diffphase),resh...)
    Greg
end

function subPixShift(imgft::AbstractArray{Complex{T}},shift::Array{Float64,1},diffphase) where {T}
    sz = [size(imgft)...]
    N=0
    for i in eachindex(sz)
        shifti = ifftshift((-div(sz[i],2)):(ceil(Integer,sz[i]/2)-1))*shift[i]/sz[i]
        resh = (repeat([1],inner=[i-1])...,length(shifti))
        N = N .- reshape(shifti,resh)
    end

    Greg = imgft .* exp.(2im*pi*N)
    Greg = Greg .* exp(1im*diffphase)
    Greg
end

function stackDftReg(imgser::AbstractArray{T,N};ref::AbstractArray{T,N1}=reshape(selectdim(imgser,N,1),size(imgser)[1:(N-1)]),ufac::Int=10) where {T,N,N1}
    stackDftReg(CPU1(),imgser,ref=ref,ufac=ufac)
end
"
`stackDftReg{T,N,N1}(imgser::AbstractArray{T,N};ref::AbstractArray{T,N1}=reshape(slicedim(imgser,N,1),size(imgser)[1:(N-1)]),ufac::Int=10)`

`dftReg` applied to a full array. Each array along the last dimension of `imgser` is aligned to `ref` (by default the first image of the series, with precision `ufac`. Returns an array of `Dict` containing the translation information."
function stackDftReg(resource::CPU1,imgser::AbstractArray{T,N};ref::AbstractArray{T,N1}=reshape(selectdim(imgser,N,1),size(imgser)[1:(N-1)]),ufac::Int=10) where {T,N,N1}
    if ((N1 != (N - 1)) & (N1 != N))
        error("Reference image has the wrong dimensionality")
    end
    ref = fft(ref)
    imgF = fft(imgser,(1:N1...,))

    if N1 == (N-1)
        imgF = [reshape(selectdim(imgF,N,i),size(ref)) for i = 1:size(imgF)[N]]
        results = pmap(imgF) do im
            dftReg(ref,im,ufac)
        end
    else
        results = dftReg(ref,imgF,ufac)
    end
    return results
end

"
`alignFromDft{T,N}(img2reg::AbstractArray{T,N},dftRegRes::Array{Any,1})`

Given an array and a `Dict` of translations as returned by `dftReg`, returns the aligned array."
function alignFromDict(img2reg::AbstractArray{T,N},dftRegRes::Array{Dict{String,Any},1}) where {T,N}
    alignFromDict(CPU1(),img2reg,dftRegRes)
end

function alignFromDict(resource::CPU1,img2reg::AbstractArray{T,N},dftRegRes::Array{Dict{String,Any},1}) where {T,N}
    if (length(dftRegRes) != size(img2reg)[N])
        error("Alignment results and image stack dimensionalities don't match.")
    end

    img2regF = fft(img2reg,(1:(N-1)...,))
    szF = size(img2reg)[1:(N-1)]

    imRes = subPixShift(img2regF,shifts_from_res(dftRegRes),diffphase_from_res(dftRegRes))
    imRes = real(ifft(imRes,1:(N-1)))

    imRes
end

## Only a single Dict, means we expect one image of the same size
function alignFromDict(img2reg::AbstractArray{T,N},dftRegRes::Dict) where {T,N}
    if (length(dftRegRes["shift"]) != N)
        error("Alignment results and image dimensionalities don't match.")
    end
    frameft = real(ifft(subPixShift(fft(img2reg),dftRegRes["shift"],dftRegRes["diffphase"])))
end

function shifts_from_res(dftRegRes::Array{Dict{String,Any},1})
    shifts = zeros(length(dftRegRes),length(dftRegRes[1]["shift"]))
    for i in eachindex(dftRegRes)
        shifts[i,:] = dftRegRes[i]["shift"]
    end
    shifts
end

function diffphase_from_res(dftRegRes::Array{Dict{String,Any},1})
    dP = zeros(length(dftRegRes))
    for i in eachindex(dftRegRes)
        dP[i] = dftRegRes[i]["diffphase"]
    end
    dP
end


using Revise
using PyPlot
using Infiltrator
using LinearAlgebra
using Bem2d

"""
    plotparticular(g, rho, lambda, mu)

Plot fundamental particular fields
"""
function plotparticular(g, rho, lambda, mu)
    npts = 100
    x, y = obsgrid(-20000, -20000, 20000, 20000, npts)
    Up, Sp = gravityparticularfunctions(x, y, g, rho, lambda, mu)
    figure(figsize=(20, 10))
    subplot(2, 3, 1)
    contourf(reshape(x, npts, npts), reshape(y, npts, npts), reshape(Up[:, 1], npts, npts), 50)
    colorbar()
    gca().set_aspect("equal")
    title("ux")
    subplot(2, 3, 2)
    contourf(reshape(x, npts, npts), reshape(y, npts, npts), reshape(Up[:, 2], npts, npts), 50)
    colorbar()
    gca().set_aspect("equal")
    title("uy")
    subplot(2, 3, 4)
    contourf(reshape(x, npts, npts), reshape(y, npts, npts), reshape(Sp[:, 1], npts, npts), 50)
    colorbar()
    gca().set_aspect("equal")
    title("Sxx")
    subplot(2, 3, 5)
    contourf(reshape(x, npts, npts), reshape(y, npts, npts), reshape(Sp[:, 2], npts, npts), 50)
    colorbar()
    gca().set_aspect("equal")
    title("Syy")
    subplot(2, 3, 6)
    contourf(reshape(x, npts, npts), reshape(y, npts, npts), reshape(Sp[:, 3], npts, npts), 50)
    colorbar()
    gca().set_aspect("equal")
    title("Sxy")
end

"""
    gravitysquareparticular()

Experiments with gravity body force.
"""
function gravitysquareparticular()
    # TODO: It's strange that the top of the model has to be at zero.  Can we generalize this?
    # TODO: Make a version of gravityparticularfunctions() for BC generation

    close("all")

    fontsize = 20
    mu = 3e10
    lambda = 3e10
    nu = 0.25
    rho = 2700
    g = 9.81
    nels = 20
    npts = 100
    L = 1e4
    offset = 10
    x, y = obsgrid(-L+offset, -2*L+offset, L-offset, 0-offset, npts)

    plotparticular(g, rho, lambda, mu)

    # Define BEM geometry
    els = Elements(Int(1e5))
    x1, y1, x2, y2 = discretizedline(-L, -2*L, L, -2*L, nels) # Bottom
    addelsez!(els, x1, y1, x2, y2, "B")
    x1, y1, x2, y2 = discretizedline(L, -2*L, L, 0, nels) # Right hand side
    addelsez!(els, x1, y1, x2, y2, "R")
    x1, y1, x2, y2 = discretizedline(L, 0, -L, 0, nels) # Top
    addelsez!(els, x1, y1, x2, y2, "T")
    x1, y1, x2, y2 = discretizedline(-L, 0, -L, -2*L, nels) # Left hand side
    addelsez!(els, x1, y1, x2, y2, "L")

    # Common indexing
    idx = getidxdict(els) # Should this return "all" - YES TODO
    bcidxU = idx["B"] # Boundaries with *displacement* BCs
    bcidxT = [idx["R"] ; idx["T"]; idx["L"]] # Boundaries with *traction* BCs
    bcidxall = collect(1:1:els.endidx) # All boundaries

    # Gravity square problem with quadratic elements
    T_pU_qall, _ = PUTQ(slip2dispstress, els, bcidxU, bcidxall, mu, nu)
    _, H_pT_qall = PUTQ(slip2dispstress, els, bcidxT, bcidxall, mu, nu)
    TH = [T_pU_qall ; H_pT_qall] # Assemble combined linear operator

    # Particular solution and effective boundary conditions
    xnodes = transpose(els.xnodes[idx["B"], :])[:]
    ynodes = transpose(els.ynodes[idx["B"], :])[:]
    UB, _ = gravityparticularfunctions(xnodes, ynodes, g, rho, lambda, mu)
    bcs = zeros(6 * els.endidx)
    bcs[1:2:6*nels] = UB[:, 1] # Bottom boundary (x-component)
    bcs[2:2:6*nels] = UB[:, 2] # Bottom boundary (y-component)
    bcs *= -1 # This is neccesary for the right answer and is consistent with derivation

    # BEM solve to get particular solution
    Ueffparticular = inv(TH) * bcs

    # Evaluate and plot interior solution
    Uinteriorcomplementary, Sinteriorcomplementary = quaddispstress(slip2dispstress, x, y, els, bcidxall, quadstack(Ueffparticular[1:2:end]), quadstack(Ueffparticular[2:2:end]), mu, nu)
    Uinteriorparticular, Sinteriorparticular = gravityparticularfunctions(x, y, g, rho, lambda, mu)
    U = @. Uinteriorcomplementary + Uinteriorparticular
    S = @. Sinteriorcomplementary + Sinteriorparticular
    Umag = sqrt.(U[:, 1].^2 + U[:, 2].^2)

    figure()
    contourf(reshape(x, npts, npts), reshape(y, npts, npts), reshape(Umag, npts, npts), 50)
    colorbar()
    gca().set_aspect("equal")
    title("BEM - below ground")

    #####
    ##### Now with top of box not at zero
    #####
    # New observaiton coordinates
    x, y = obsgrid(-L+offset, 0, L-offset, 2*L-offset, npts)
    num = -2*L
    num = 0
    y = y .+ num
    elsbox = Elements(Int(1e5))
    x1, y1, x2, y2 = discretizedline(-L, 0+num, L, 0+num, nels) # Bottom
    addelsez!(elsbox, x1, y1, x2, y2, "B")
    x1, y1, x2, y2 = discretizedline(L, 0+num, L, 2*L+num, nels) # Right hand side
    addelsez!(elsbox, x1, y1, x2, y2, "R")
    x1, y1, x2, y2 = discretizedline(L, 2*L+num, -L, 2*L+num, nels) # Top
    addelsez!(elsbox, x1, y1, x2, y2, "T")
    x1, y1, x2, y2 = discretizedline(-L, 2*L+num, -L, 0+num, nels) # Left hand side
    addelsez!(elsbox, x1, y1, x2, y2, "L")

    # Common indexing
    idxbox = getidxdict(elsbox) # Should this return "all" - YES TODO
    bcidxU = idxbox["B"] # Boundaries with *displacement* BCs
    bcidxT = [idxbox["R"] ; idxbox["T"]; idxbox["L"]] # Boundaries with *traction* BCs
    bcidxall = collect(1:1:elsbox.endidx) # All boundaries

    T_B_BRTL, H_B_BRTL = PUTC(slip2dispstress, elsbox, idxbox["B"],
                              [idxbox["B"] ; idxbox["R"] ; idxbox["T"] ; idxbox["L"]],
                              mu, nu)
    T_R_BRTL, H_R_BRTL = PUTC(slip2dispstress, elsbox, idxbox["R"],
                              [idxbox["B"] ; idxbox["R"] ; idxbox["T"] ; idxbox["L"]],
                              mu, nu)
    T_L_BRTL, H_L_BRTL = PUTC(slip2dispstress, elsbox, idxbox["L"],
                              [idxbox["B"] ; idxbox["R"] ; idxbox["T"] ; idxbox["L"]],
                              mu, nu)
    T_T_BRTL, H_T_BRTL = PUTC(slip2dispstress, elsbox, idxbox["T"],
                              [idxbox["B"] ; idxbox["R"] ; idxbox["T"] ; idxbox["L"]],
                              mu, nu)
    THbox = [T_B_BRTL ; H_R_BRTL; H_T_BRTL ; H_L_BRTL]

    # Displacement BCs for bottom
    Ugabove, Sgabove = gravityparticularfunctions(elsbox.xcenter[idxbox["B"]],
                                                  elsbox.ycenter[idxbox["B"]],
                                                  g, rho, lambda, mu)
    bcsboxgravity = zeros(2 * elsbox.endidx)
    bcsboxgravity[1:2:2*nels] = Ugabove[:, 1]
    bcsboxgravity[2:2:2*nels] = Ugabove[:, 2]

    # Traction BCs for top and sides
    # Ug, Sg = gravityparticularfunctions(elsbox.xcenter[idxbox["T"]],
    #                                     elsbox.ycenter[idxbox["T"]],
    #                                     g, rho, lambda, mu)
    # Ttx = zeros(nels)
    # Tty = zeros(nels)
    # for i in 1:length(Sg[:, 1])
    #     nvec = [elsbox.xnormal[idxbox["T"][i]] ; elsbox.ynormal[idxbox["T"][i]]]
    #     temp = [Sg[i, 1] Sg[i, 3] ; Sg[i, 3] Sg[i, 2]] * nvec
    #     Ttx[i] = temp[1]
    #     Tty[i] = temp[2]
    # end
    # bcsboxgravity[4*nels+1:2:6*nels] = Ttx
    # bcsboxgravity[4*nels+2:2:6*nels] = Tty
    bcsboxgravity *= -1
    Ueffboxparticular = THbox \ bcsboxgravity

    # Forward solution on grid
    Ucomp, Scomp = constdispstress(slip2dispstress, x, y, elsbox,
                                   [idxbox["B"] ; idxbox["R"] ; idxbox["T"] ; idxbox["L"]],
                                   Ueffboxparticular[1:2:end], Ueffboxparticular[2:2:end], mu, nu)
    Uint, Sint = gravityparticularfunctions(x, y, g, rho, lambda, mu)
    Ugravityonly = @. Ucomp + Uint
    Sgravityonly = @. Scomp + Sint
    bemaboveumag = sqrt.(Ugravityonly[:, 1].^2 + Ugravityonly[:, 2].^2)

    figure();
    contourf(reshape(x, npts, npts), reshape(y, npts, npts),
             reshape(bemaboveumag, npts, npts), 50)
    colorbar()
    gca().set_aspect("equal")
    title("BEM - above ground")
end
gravitysquareparticular()


function add_born_sources!(issp::Int64, pac, pap)

	δx24I=pac.fc[:δx24I]; δz24I=pac.fc[:δz24I];
	δxI=pac.fc[:δxI]; δzI=pac.fc[:δzI];
	δt=pac.fc[:δt]
	δtI=pac.fc[:δtI]
	δmodKI=pac.δmod[:KI]; modK=pac.mod[:K];
	δmodrhovxI=pac.δmod[:rhovxI]; δmodrhovzI=pac.δmod[:rhovzI]
	nx=pac.ic[:nx]; nz=pac.ic[:nz]

	born_svalue_stack=pap[1].born_svalue_stack

	p1=pap[1].w1[:t][:p]
	p2=pap[2].w1[:t][:p]
	p1p=pap[1].w1[:tp][:p]
	p1pp=pap[1].w1[:tpp][:p]
	dpdx1=pap[1].w1[:dx][:p]
	dpdx2=pap[2].w1[:dx][:p]
	dpdz1=pap[1].w1[:dz][:p]
	dpdz2=pap[2].w1[:dz][:p]

	# secondary sources for Born modeling
	# adding born sources from pressure(:,:,1) to pressure(:,:,2)
	# upto until [it-2]
	# lambdaI scatterrer source term at [it-1]
	# p is at [it], pp is at [it-1], ppp is at [it-2]
	# dpdx is at [it-1] and dpdz is at [it-1]
	# modrhovxI scatterrer source term at [it-1]
	# modrhovzI scatterrer source term at [it-1]

	# compute strength of secondary sources to δmod
	born_stackKI!(born_svalue_stack,p1,p1p,p1pp,δmodKI,nx,nz,δtI,δt)
	born_stackrhovxI!(born_svalue_stack,dpdx1,δmodrhovxI,nx,nz,δx24I,δt)
	born_stackrhovzI!(born_svalue_stack,dpdz1,δmodrhovzI,nx,nz,δz24I,δt)
	born_stack!(p2,born_svalue_stack,modK,nx,nz,δt)
end
@inbounds @fastmath function born_stackKI!(born_svalue_stack,p1,p1p,p1pp,δmodKI,nx,nz,δtI,δt)
	for ix=1:nx
		@simd for iz=1:nz
			born_svalue_stack[iz,ix] += δt * ((-1.0 * (p1pp[iz,ix] + p1[iz,ix] - 2.0 * p1p[iz,ix]) * δmodKI[iz,ix] * δtI * δtI))
		end
	end
end
@inbounds @fastmath function born_stackrhovxI!(born_svalue_stack,dpdx1,δmodrhovxI,nx,nz,δx24I,δt)
	for ix=3:nx-1
		@simd for iz=1:nz
			born_svalue_stack[iz,ix] +=
				δt * ((27.e0*dpdx1[iz,ix] * δmodrhovxI[iz,ix] -27.e0*dpdx1[iz,ix-1] * δmodrhovxI[iz,ix-1] -dpdx1[iz,ix+1] * δmodrhovxI[iz,ix+1] +dpdx1[iz,ix-2] * δmodrhovxI[iz,ix-2] ) * (δx24I))
		end
	end

end
@inbounds @fastmath function born_stackrhovzI!(born_svalue_stack,dpdz1,δmodrhovzI,nx,nz,δz24I,δt)
	for ix=1:nx
		@simd for iz=3:nz-1
			born_svalue_stack[iz,ix] +=
				δt * ((27.e0*dpdz1[iz,ix] * δmodrhovzI[iz,ix] -27.e0*dpdz1[iz-1,ix] * δmodrhovzI[iz-1,ix] -dpdz1[iz+1,ix] * δmodrhovzI[iz+1,ix] +dpdz1[iz-2,ix] * δmodrhovzI[iz-2,ix] ) * (δz24I))
		end
	end
end
# add secondary sources to second pw
@inbounds @fastmath function born_stack!(p2,born_svalue_stack,modK,nx,nz,δt)
	for ix=1:nx
		@simd for iz=1:nz
			p2[iz,ix] += born_svalue_stack[iz,ix] * modK[iz,ix] * δt  #* δxI * δzI
		end
	end
end


using Test, DiscreteExteriorCalculus, DiscretePDEs
using LinearAlgebra: norm
using UniqueVectors: UniqueVector

function disks!(r1::Real, r2::Real, factor::Real, N::Int)
    @assert (N == 2) || (N == 3)
    file_name = joinpath(@__DIR__, "disks.geo")
    if N == 2
        geo_write!(file_name, characteristic_length_factor=factor,
            footer="""
            Circle(1) = {0, 0, 0, $r1, 0, 2*Pi};
            Circle(2) = {0, 0, 0, $r2, 0, 2*Pi};
            Curve Loop(1) = {2};
            Curve Loop(2) = {1};
            Plane Surface(1) = {1, 2};
            Physical Curve("Conductor") = {1};
            Physical Surface("Dielectric") = {1};
            """)
    elseif N == 3
        geo_write!(file_name, characteristic_length_factor=factor,
            footer="""
            Sphere(1) = {0, 0, 0, $r1, -Pi/2, Pi/2, 2*Pi};
            Sphere(2) = {0, 0, 0, $r2, -Pi/2, Pi/2, 2*Pi};
            BooleanDifference{ Volume{2}; Delete; }{ Volume{1}; Delete; }
            Physical Surface("Conductor") = {1};
            Physical Volume("Dielectric") = {2};
            """)
    end
    return file_name
end

function voltage(R, outer_radius, ϵ, N)
    @assert (N == 2) || (N == 3)
    if N == 2
        return (log(outer_radius) - log(norm(R)))/(2π * ϵ)
    elseif N == 3
        return (1/norm(R) - 1/outer_radius)/(4π * ϵ)
    end
end

initialize!()
for (N, factor) in [(2, .3), (3, .45)]
    r1, r2 = 1.5, 3
    file_name = disks!(r1, r2, factor, N)
    gmsh_open!(file_name)
    K = N+1
    mesh!(K)

    node_tags, points, tcomp = get_triangulated_complex(N, K)
    group_dict = get_physical_groups(node_tags, points)
    @test typeof(tcomp) <: TriangulatedComplex{N, K}
    comp = tcomp.complex
    orient!(comp)
    sources = [get_charge_source(comp, group_dict["Conductor"])]
    _, exterior = boundary_components_connected(comp)
    m = Metric(N)
    mesh = Mesh(tcomp, circumcenter(m))

    ϵ = 2
    ϵ_form = get_material(comp, group_dict["Dielectric"], ϵ, 2)
    bbox, null_basis = electrostatics_blackbox(m, mesh, sources, exterior, ϵ_form)

    elastance = impedance_matrix(bbox, null_basis)[1,1]
    charge = 2
    φ, q = solve_statics(bbox, null_basis, [charge])
    @testset "concentric $N-disks" begin
        @test isapprox(elastance[1,1], voltage(r1, r2, ϵ, N), rtol= N == 2 ? 1e-4 : 4e-2)
        @test bbox.Y[1] * φ == q
        inner_charges = q[[findfirst(isequal(c), comp.cells[1]) for c in sources[1]]]
        @test sum(inner_charges) ≈ charge
        @test all(abs.(diff(inner_charges)) .< charge * (N == 2 ? .0003 : .005))
        outer_charges = q[[findfirst(isequal(c), comp.cells[1]) for c in exterior.cells[1]]]
        @test sum(outer_charges) ≈ -charge
        @test all(abs.(diff(outer_charges)) .< charge * (N == 2 ? .0003 : .005))
        @test isapprox(φ, charge * voltage.([c.points[1].coords
            for c in comp.cells[1]], r2, ϵ, N), rtol= N == 2 ? 7e-4 : 4e-2)
    end
    if false # set to true to display plots with Gmsh
        ρ = source_density(m, mesh, q, 1)
        vec_E = [N == 2 ? [p..., 0] : p for p in electric_field(m, mesh, φ)]
        comp_points = UniqueVector([c.points[1] for c in comp.cells[1]])
        ordering = [findfirst(isequal(p), comp_points) for p in points]
        add_field!("Electric field", node_tags, vec_E[ordering])
        inds = N == 2 ? (1:length(points)) : findall([p.coords[1] > 0 for p in points])
        add_field!("Electric field magnitude", node_tags[inds], norm.(vec_E)[ordering][inds])
        dd_field!("Voltage", node_tags[inds], φ[ordering][inds])
        add_field!("Charge density", node_tags[inds], ρ[ordering][inds])
        gui!()
    end
    rm(file_name)
end


using Test
using StaticArrays, IntervalSets
import ClimaCore.DataLayouts: IJFH
import ClimaCore:
    Fields, Domains, Meshes, Topologies, Spaces, Operators, Geometry
using StaticArrays, IntervalSets, LinearAlgebra

using OrdinaryDiffEq

@testset "2D field dx/dt = ∇⋅∇  ODE solve" begin
    FT = Float64

    domain = Domains.RectangleDomain(
        FT(-π)..FT(π),
        FT(-π)..FT(π),
        x1periodic = true,
        x2periodic = true,
    )

    mesh = Meshes.EquispacedRectangleMesh(domain, 5, 5)
    grid_topology = Topologies.GridTopology(mesh)

    Nq = 6
    quad = Spaces.Quadratures.GLL{Nq}()
    points, weights = Spaces.Quadratures.quadrature_points(Float64, quad)
    space = Spaces.SpectralElementSpace2D(grid_topology, quad)

    # 2D field
    # dx/dt = ∇⋅∇ x

    # Standard heat equation:
    # ∂_t f(x1,x2) =  ∇⋅( α ∇ f(x1,x2) ) + g(x1,x2), α > 0

    # Advection Equation
    # ∂_t f + c ∂_x f  = 0
    # the solution translates to the right at speed c,
    # so if you you have a periodic domain of size [0, 1]
    # at time t, the solution is f(x - c * t, y)

    f(x, t) = sin(x.x1) * exp(-t)
    y0 = f.(Fields.coordinate_field(space), 0.0)

    function rhs!(dydt, y, _, t)

        ∇y = Operators.slab_gradient(y)
        dydt .= .-Operators.slab_weak_divergence(∇y)

        Spaces.horizontal_dss!(dydt)
        Spaces.variational_solve!(dydt)
    end

    # Solve the ODE operator
    prob = ODEProblem(rhs!, y0, (0.0, 1.0))
    sol = solve(prob, Tsit5(), reltol = 1e-8, abstol = 1e-8)

    # Reconstruct the result Field at the last timestep
    y1 = sol(1.0)

    @show y1 .- f.(Fields.coordinate_field(space), 1.0)

    @test y1 ≈ f.(Fields.coordinate_field(space), 1.0) rtol = 5e-5
end


@doc raw"""
    PolyaGammaPoisson(y::Real, c::Real, λ::Real)

A bivariate distribution, used as hierachical prior as:
```math
    p(\omega, n) = \operatorname{PG}(\omega|y + n, c)\operatorname{Po}(n|\lambda).
```

Random samples as well as statistics from the distribution will returned as `NamedTuple` : `(;ω, n)`.

This structured distributions is needed for example for the [`PoissonLikelihood`](https://juliagaussianprocesses.github.io/GPLikelihoods.jl/dev/#GPLikelihoods.PoissonLikelihood).
"""
struct PolyaGammaPoisson{Ty,Tc,Tλ} <: AbstractNTDist
    y::Ty # Intermediate first parameter for PG(y + n, c)
    c::Tc # Second parameter for PG
    λ::Tλ # Poisson Parameter
end

Distributions.Poisson(d::PolyaGammaPoisson) = Poisson(d.λ)

Distributions.length(::PolyaGammaPoisson) = 2

function ntrand(rng::AbstractRNG, d::PolyaGammaPoisson)
    n = rand(rng, Poisson(d))
    ω = rand(rng, PolyaGamma.(n + d.y, d.c))
    return (; ω, n)
end

function MeasureBase.logdensity_def(d::PolyaGammaPoisson, x::NamedTuple)
    logpdf_n = logpdf(Poisson(d), x.n)
    logpdf_ω = logpdf(PolyaGamma(d.y + x.n, d.c), x.ω)
    return logpdf_ω + logpdf_n
end

function tvmean(ds::AbstractVector{<:PolyaGammaPoisson})
    n = getfield.(ds, :λ)
    ω = map(ds, n) do d, n
        mean(PolyaGamma(d.y + n, d.c))
    end
    return TupleVector((; ω, n))
end

function ntmean(d::PolyaGammaPoisson)
    return (; ω=mean(PolyaGamma(d.y + d.λ, d.c)), n=d.λ)
end

function Distributions.kldivergence(q::PolyaGammaPoisson, p::PolyaGammaPoisson)
    (p.c == 0 && p.y == q.y) || error("No solution for this prior. qΩ = $q, pΩ = $p")
    return kldivergence(PolyaGamma(q.y + q.λ, q.c), PolyaGamma(q.y + q.λ, 0)) +
           kldivergence(Poisson(q), Poisson(p))
end


# Solve the n-queens problem to test optional constraints.

"Creates the constraints matrix for the n-queens problem."
function make_queens_matrix(n)
  slash_diagonal_for(row, column) = row + column - 1
  backslash_diagonal_for(row, column) = n + row - column

  constraints_for(row, column) = Set(Any[
    (:row, row),
    (:column, column),
    (:slash, slash_diagonal_for(row, column)),
    (:backslash, backslash_diagonal_for(row, column))
  ])

  CoverMatrix(
    Dict([(row, column) => constraints_for(row, column) for row in 1:n, column in 1:n]),
    optionals = union(
      Set(Any[(:slash, x) for x in 1:(2n-1)]),
      Set(Any[(:backslash, x) for x in 1:(2n-1)])
    )
  )
end

"See [OEIS sequence A000170](http://oeis.org/A000170)."
const oeis_A000170 = [1, 0, 0, 2, 10, 4, 40, 92, 352, 724]

let
  for (queen_count, position_count) in enumerate(oeis_A000170)
    @test exact_cover_count(make_queens_matrix(queen_count)) == position_count
  end
end


using Optim
using Parameters
using LinearAlgebra
using GeometryTypes

### If C=0 we have original Zinchenko 1997 stabilization scheme
@with_kw struct KineticStabilizer
    C::AbstractFloat = 0.4
    ftol::AbstractFloat = 1e-6
end

function F(v,points,faces,zc::KineticStabilizer)

    Cp = zc.C

    s = 0
    for ti in 1:length(faces)
        v1,v2,v3 = faces[ti]

        a = norm(points[v2] - points[v1])
        b = norm(points[v3] - points[v2])
        c = norm(points[v1] - points[v3])

        xava = dot(points[v2]-points[v1],v[v2]-v[v1])
        xbvb = dot(points[v3]-points[v2],v[v3]-v[v2])
        xcvc = dot(points[v1]-points[v3],v[v1]-v[v3])

        ### This part is responsible for first part in the sum
        s += v2>v1 ? 4 * xava^2 : 0
        s += v3>v2 ? 4 * xbvb^2 : 0
        s += v1>v3 ? 4 * xcvc^2 : 0

        ### This part is needed for ordinary things
        Cdelta = 1/4*sqrt(1 - 2* (a^4 + b^4 + c^4)/(a^2 + b^2 + c^2)^2)
        A = 1/4/Cdelta/(a^2 + b^2 + c^2)^3 * ( a^2*(a^2 + b^2 + c^2) - a^4 - b^4 - c^4)
        B = 1/4/Cdelta/(a^2 + b^2 + c^2)^3 * ( b^2*(a^2 + b^2 + c^2) - a^4 - b^4 - c^4)
        C = 1/4/Cdelta/(a^2 + b^2 + c^2)^3 * ( c^2*(a^2 + b^2 + c^2) - a^4 - b^4 - c^4)
        DCdelta = -A*xava - B*xbvb - C*xcvc

        s += Cp*DCdelta^2/Cdelta^2
    end

    return s
end

function gradF!(v,points,faces,storage,zc::KineticStabilizer)

    Cp = zc.C

    for i in 1:length(storage)
        storage[i] = Point(0,0,0)
    end

    for i in 1:length(faces)
        v1,v2,v3 = faces[i]
        a = norm(points[v2] - points[v1])
        b = norm(points[v3] - points[v2])
        c = norm(points[v1] - points[v3])

        Cdelta = 1/4*sqrt(1 - 2 * (a^4 + b^4 + c^4)/(a^2 + b^2 + c^2)^2)
        A = 1/4/Cdelta/(a^2 + b^2 + c^2)^3 * ( a^2*(a^2 + b^2 + c^2) - a^4 - b^4 - c^4)
        B = 1/4/Cdelta/(a^2 + b^2 + c^2)^3 * ( b^2*(a^2 + b^2 + c^2) - a^4 - b^4 - c^4)
        C = 1/4/Cdelta/(a^2 + b^2 + c^2)^3 * ( c^2*(a^2 + b^2 + c^2) - a^4 - b^4 - c^4)

        xa = points[v2] - points[v1]
        xb = points[v3] - points[v2]
        xc = points[v1] - points[v3]

        xava = dot(points[v2]-points[v1],v[v2]-v[v1])
        xbvb = dot(points[v3]-points[v2],v[v3]-v[v2])
        xcvc = dot(points[v1]-points[v3],v[v1]-v[v3])

        DCdelta = -A*xava - B*xbvb - C*xcvc

        A_ = 2*Cp*DCdelta*A/Cdelta^2
        B_ = 2*Cp*DCdelta*B/Cdelta^2
        C_ = 2*Cp*DCdelta*C/Cdelta^2

        storage[v1] += (A_ - 8*xava)*xa - C_*xc
        storage[v2] += (B_ - 8*xbvb)*xb - A_*xa
        storage[v3] += (C_ - 8*xcvc)*xc - B_*xb
    end
end

function stabilize!(v,points,faces,n,zc::KineticStabilizer)
    #h2 = hvec(points,faces,n)

    ### Let's try to make it simpler

    function f(x::Vector)
        # vv = reshape(x,3,div(length(x),3))
        return F(x,points,faces,zc)
    end

    function g!(storage::Vector,x::Vector)

        # vv = reshape(x,size(points)...)
        # gradf = reshape(storage,size(points)...)
        gradF!(x,points,faces,storage,zc)
        for i in 1:size(v,2)
            P = I - n[i]*n[i]'
            storage[i] = P*storage[i]
        end
    end

    res = optimize(f,g!,v,method=Optim.ConjugateGradient(),ftol=zc.ftol)

    ### One needs to be a little bit carefull about what types would Julia return
    return res
    #v[:,:] = reshape(res.minimum, size(v)...)[:,:]
end


const badevs = Ref{Int}(0)
using Printf
function schurtest(A::Matrix{T}, tol; normal=false) where {T<:Complex}
    n = size(A,1)
    ulp = eps(real(T))
    if real(T) <: BlasFloat
        S = GenericSchur.gschur(A)
    else
        S = schur(A)
    end
    # test 1: S.T is upper triangular
    @test all(tril(S.T,-1) .== 0)
    # test 2: norm(A - S.Z * S.T * S.Z') / (n * norm(A) * ulp) < tol
    @test norm(A - S.Z * S.T * S.Z') / (n * norm(A) * ulp) < tol
    # test 3: S.Z is unitary: norm(I - S.Z * S.Z') / (n * ulp) < tol
    @test norm(I - S.Z * S.Z') / (n * ulp) < tol
    # test 4: S.values are e.v. of T
    @test all(csort(S.values) .== csort(diag(S.T)))

    # It is tempting to check eigenvalues against LAPACK eigvals(A)
    # for suitable types, but the comparison is misleading without
    # detailed analysis because many of the test matrices are severly
    # non-normal.

    if normal
        # verify that Schur diagonalizes normal matrices
        @test norm(triu(S.T,1)) / (n * norm(A) * ulp) < tol
    end
    # CHECKME: a few hard cases fail; is overflow protection sufficient?
    fom = norm(A) / (floatmin(real(T))/eps(real(T)))
    FOM = norm(A) / (floatmax(real(T))*eps(real(T)))
    if (fom > 25) && (FOM < 0.04)
        As = A
        Ss = S
        vtol = tol
    else
        vtol = (T == Complex{Float16} ? 200 : 20)
        # test work-arounds
        if fom <= 25
            As = A * (floatmax(real(T))*eps(real(T)))
            Ss = GenericSchur.gschur(As)
        elseif FOM >= 0.04
            As =  A * (floatmin(real(T))/eps(real(T)))
            Ss = GenericSchur.gschur(As)
        end
    end
    VR = eigvecs(Ss)
    @test norm(As * VR - VR * diagm(0 => Ss.values)) / (n * norm(As) * ulp) < vtol
    VL = eigvecs(Ss, left=true)
    @test norm(As' * VL - VL * diagm(0 => conj.(Ss.values))) / (n * norm(As) * ulp) < vtol
    if normal
        # check orthonormality of vectors where appropriate
        @test norm(VR*VR'-I) / (n * ulp) < vtol
        @test norm(VL*VL'-I) / (n * ulp) < vtol
    end
end

"""
generate a random unitary matrix

Just something good enough for present purposes.
Unitary matrices are normal, so Schur decomposition is diagonal for them.
(This is not only another test, but sometimes actually useful.)
"""
function randu(::Type{T},n) where {T<:Complex}
    if T ∈ [Float16,Float32,Float64]
        A = randn(T,n,n)
        F = qr(A)
        return Matrix(F.Q)
    else
        # don't have normal deviates for other types, but need appropriate QR
        A = randn(ComplexF64,n,n)
        F = qr(convert.(T,A))
        return Matrix(F.Q)
    end
end

Random.seed!(1234)

for T in [Complex{BigFloat},Complex{Float16}]
@testset "group $T" begin
unfl = floatmin(real(T))
ovfl = one(real(T)) / unfl
ulp = eps(real(T))
ulpinv = one(real(T)) / ulp
rtulp = sqrt(ulp)
rtulpi = one(real(T)) / rtulp

ens = [4,32]
@testset "random general" begin
    for n in ens
        A = rand(T,n,n)
        schurtest(A,10)
    end
end
@testset "random unitary" begin
    for n in ens
        A = randu(T,n)
        schurtest(A,10,normal=true)
    end
end

end # group testset
end # type loop (non-BlasFloat)

@testset "Godunov (complex)" begin
    setprecision(BigFloat, 80) do
        A,v = godunov(Complex{BigFloat})
        S = schur(A)
        @test isapprox(csort(S.values),v,atol=2e-5)
        vnew = eigvals(A)
        @test isapprox(csort(vnew),v,atol=2e-5)
    end
end

for T in [ComplexF64, ComplexF32]
@testset "group $T" begin

unfl = floatmin(real(T))
ovfl = one(real(T)) / unfl
ulp = eps(real(T))
ulpinv = one(real(T)) / ulp
rtulp = sqrt(ulp)
rtulpi = one(real(T)) / rtulp

# sizes of matrices to test
ens = [4,32]

itypemax = 10
# On author's dev system, tests pass with tol=5 for most and 6 for all,
# but generalization is risky.
# (LAPACK uses 20, FWIW.)
tols = fill(20,itypemax)

simrconds = [1.0, rtulpi, 0.0]
magns = [1.0, ovfl*ulp, unfl*ulpinv]

# test matrix from zdrves:
# conds=0 means index 3 for simrconds (i.e. irrelevant)
# type magn mode conds
# 1 1 0 0
# 2 1 0 0
# 3 1 0 0
# 4 1 4 0
# 4 1 3 0
# 4 1 1 0
# 4 2 4 0
# 4 3 4 0
# 6 1 4 1
# 6 1 3 1
# 6 1 1 1
# 6 1 5 1
# 6 1 4 2
# 6 1 3 2
# 6 1 1 2
# 6 1 5 2
# 6 2 5 2
# 6 3 5 2
# 9 1 4 0
# 9 2 3 0
# 9 3 1 0

# Other stuff in zdrves: pointless or not even used
# 1- zero
# 2- identity
# 4- diagonal, ev specified
# 5- symmetric, ev specified, not used in zdrves

@testset "Jordan" begin
    itype=3
    kmagn = [1]
    nj = length(kmagn)
    for n in ens
        for j in 1:nj
            anorm = magns[kmagn[j]]
            A = diagm(1 => ones(T,n-1)) + anorm * I
            schurtest(A,tols[itype])
        end
    end
end

@testset "general, ev specified" begin
    itype=6
    kmagn =  [1,1,1,1,1,1,1,1,2,3]
    kmode =  [4,3,1,5,4,3,1,5,5,5]
    kconds = [1,1,1,1,2,2,2,2,2,2]
    nj = length(kmode)
    for n in ens
        for j in 1:nj
#               CALL ZLATME( N, 'D', ISEED, WORK, IMODE, COND, CONE,
#     $                      'T', 'T', 'T', RWORK, 4, CONDS, N, N, ANORM,
#     $                      A, LDA, WORK( 2*N+1 ), IINFO )
            A = zeros(T,n,n)
            simrcond = simrconds[kconds[j]]
            imode = kmode[j]
            anorm = magns[kmagn[j]]
            rcond = ulpinv
            verbosity[] > 1 &&
                println("latme: n=$n $anorm, $imode, $rcond, $simrcond")
            latme!(A,anorm,imode,rcond,simrcond)
            schurtest(A,tols[itype])
        end
    end
end

@testset "diagonal, random ev" begin
    itype=7
    kmode = [6]
    kmagn = [1]
    nj = length(kmagn)
    for n in ens
        for j in 1:nj
     #           CALL ZLATMR( N, N, 'D', ISEED, 'N', WORK, 6, ONE, CONE,
     # $                      'T', 'N', WORK( N+1 ), 1, ONE,
     # $                      WORK( 2*N+1 ), 1, ONE, 'N', IDUMMA, 0, 0,
     # $                      ZERO, ANORM, 'NO', A, LDA, IWORK, IINFO )
            A = zeros(T,n,n)
            imode = kmode[j]
            anorm = magns[kmagn[j]]
            rcond = 1.0
            latmr!(A,anorm,imode,rcond,kl=0,ku=0)
            schurtest(A,tols[itype])
        end
    end
end

@testset "symmetric, random ev" begin
    itype=8
    kmagn = [1]
    kmode = [6]
    nj = length(kmagn)
    for n in ens
        for j in 1:nj
     # note sym='H'
     #           CALL ZLATMR( N, N, 'D', ISEED, 'H', WORK, 6, ONE, CONE,
     # $                      'T', 'N', WORK( N+1 ), 1, ONE,
     # $                      WORK( 2*N+1 ), 1, ONE, 'N', IDUMMA, N, N,
     # $                      ZERO, ANORM, 'NO', A, LDA, IWORK, IINFO )
            A = zeros(T,n,n)
            imode = kmode[j]
            anorm = magns[kmagn[j]]
            rcond = 1.0
            latmr!(A,anorm,imode,rcond,sym='H',kl=n,ku=n)
            schurtest(A,tols[itype])
        end
    end
end

@testset "general, random ev" begin
    itype=9
    kmagn = [1,2,3]
    # kmode = [4,3,1] # data statement is a lie
    kmode = [6,6,6]
    nj = length(kmagn)
    for n in ens
        for j in 1:nj
     #           CALL ZLATMR( N, N, 'D', ISEED, 'N', WORK, 6, ONE, CONE,
     # $                      'T', 'N', WORK( N+1 ), 1, ONE,
     # $                      WORK( 2*N+1 ), 1, ONE, 'N', IDUMMA, N, N,
     # $                      ZERO, ANORM, 'NO', A, LDA, IWORK, IINFO )
            # zdrves zeros first 2 rows/cols & last row w/o explanation
            # presumably to test balancing, which we don't do, so skip it.
            A = zeros(T,n,n)
            imode = kmode[j]
            anorm = magns[kmagn[j]]
            rcond = 1.0
            (verbosity[] > 1) &&
                println("latmr: n=$n $anorm, $imode, $rcond")
            latmr!(A,anorm,imode,rcond)
            schurtest(A,tols[itype])
        end
    end
end

@testset "triangular, random ev" begin
    # trivial, not even run in zdrves
    itype=10
    kconds = [1]
    kmagn = [1]
    kmode = [1]
    nj = length(kmagn)
    for n in ens
        for j in 1:nj
     # note: kl=0
     #           CALL ZLATMR( N, N, 'D', ISEED, 'N', WORK, 6, ONE, CONE,
     # $                      'T', 'N', WORK( N+1 ), 1, ONE,
     # $                      WORK( 2*N+1 ), 1, ONE, 'N', IDUMMA, N, 0,
     # $                      ZERO, ANORM, 'NO', A, LDA, IWORK, IINFO )
#
            A = zeros(T,n,n)
            imode = 6
            anorm = magns[kmagn[j]]
            rcond = 1.0
            latmr!(A,anorm,imode,rcond,kl=0)
            schurtest(A,tols[itype])
        end
    end
end

end # group testset
end # type loop (BlasFloat)


@testset "airspace.jl" begin
    p1 = Point(0.0, 0.0)
    p2 = Point(0.0, 10.0)
    p3 = Point(10.0, 10.0)
    p4 = Point(10.0, 0.0)

    ap = Airspace("test", [p1, p2, p3, p4, p1])

    mp = midpoint(p3, p4)
    @test ap.name == "test"
    @test ap.polygon[1] == p1
    @test ap.polygon[end] == p1
    @test ap.bounding_box[3].ϕ ≈ mp.ϕ atol = 0.0001

    @test isinside(Point(5.0, 5.0), ap.bounding_box)
    @test isinside(Point(15.0, 5.0), ap.bounding_box) == false
    @test isinside(Point(5.0, 15.0), ap.bounding_box) == false

    @test intersection_point(Point(15.0, 5.0), Point(-90.0, 0.0),
    ap.polygon).ϕ ≈ 10.03742304591071 atol = 0.0001
    @test intersection_point(Point(5.0, 5.0), Point(-90.0, 0.0),
    ap.polygon).ϕ ≈ 0.0 atol = 0.0001

    @test intersection_point(Point(15.0, 5.0), Point(-90.0, 0.0), ap).ϕ ≈
    10.03742304591071 atol = 0.0001
    @test intersection_point(Point(5.0, 5.0), Point(-90.0, 0.0), ap).ϕ ≈
    0.0 atol = 0.0001
    @test isnan(intersection_point(Point(5.0, 15.0), Point(-90.0, 0.0), ap).ϕ)

    ap_triangle = Airspace("triangle", [p1, p2, p3, p1])
    @test isinside(Point(9.0, 1.0), ap_triangle.bounding_box) == true
    @test isinside(Point(9.0, 1.0), ap_triangle.polygon) == false

    @test isnan(intersection_point(Point(0.0, -1.0), Point(11.0, 10.0), ap_triangle).ϕ)
    @test intersection_point(Point(11.0, -1.0), Point(-1.0, 11.0), ap_triangle).ϕ ≈ 5.0703 atol = 0.001
    @test intersection_point(Point(-1.0, 11.0), Point(11.0, -1.0), ap_triangle).λ ≈ 10.000   atol = 0.001
end


# ---
# title: 704. Binary Search
# id: problem704
# author: Tian Jun
# date: 2020-10-31
# difficulty: Easy
# categories: Binary Search
# link: <https://leetcode.com/problems/binary-search/description/>
# hidden: true
# ---
#
# Given a **sorted** (in ascending order) integer array `nums` of `n` elements
# and a `target` value, write a function to search `target` in `nums`. If
# `target` exists, then return its index, otherwise return `-1`.
#
#
# **Example 1:**
#
#
#
#     Input: nums = [-1,0,3,5,9,12], target = 9
#     Output: 4
#     Explanation: 9 exists in nums and its index is 4
#
#
#
# **Example 2:**
#
#
#
#     Input: nums = [-1,0,3,5,9,12], target = 2
#     Output: -1
#     Explanation: 2 does not exist in nums so return -1
#
#
#
#
# **Note:**
#
#   1. You may assume that all elements in `nums` are unique.
#   2. `n` will be in the range `[1, 10000]`.
#   3. The value of each element in `nums` will be in the range `[-9999, 9999]`.
#
#
## @lc code=start
using LeetCode

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


module TestMappedArrays

using Test
using SIMDIndexing

include("../examples/vdot.jl")
using MappedArrays

arr1 = randn(2^20)
arr2 = randn(2^20)
f1(x) = x*x + x
f2(x) = x*x*x + 1
ma1 = mappedarray(f1, arr1)
ma2 = mappedarray(f2, arr2)

@test vdot(ma1, ma2) ≈ vdot(f1.(arr1), f2.(arr2))

end  # module


module TestFunctionsTests

import ..Algopt # ensure we are using the correct Algopt

using Test

using Algopt.TestFunctions:
Rosenbrock

@testset "Algopt.TestFunctions.Rosenbrock" begin
    ros = Rosenbrock()
    @test 0 == ros.f(ros.argmin)
    @test [0, 0] == ros.∇f(ros.argmin)

    ros = Rosenbrock(a=100, b=16)
    @test 0 == ros.f(ros.argmin)
    @test [0, 0] == ros.∇f(ros.argmin)
end

end # module


# # Wiring diagrams in TikZ
#
#md # [![](https://img.shields.io/badge/show-nbviewer-579ACA.svg)](@__NBVIEWER_ROOT_URL__/generated/graphics/tikz_wiring_diagrams.ipynb)
#
# Catlab can draw morphism expressions as TikZ pictures. To use this feature,
# LaTeX must be installed and the Julia package
# [TikzPictures.jl](https://github.com/sisl/TikzPictures.jl) must be loaded.
#
# For best results, it is recommended to load the packages
# [Convex.j](https://github.com/JuliaOpt/Convex.jl) and
# [SCS.jl](https://github.com/JuliaOpt/SCS.jl). When available they are used to
# optimize the layout of the outer ports.

using Catlab.WiringDiagrams, Catlab.Graphics

import Convex, SCS
import TikzPictures

# ## Examples

# ### Symmetric monoidal category

using Catlab.Doctrines

A, B, C, D = Ob(FreeSymmetricMonoidalCategory, :A, :B, :C, :D)
f, g = Hom(:f, A, B), Hom(:g, B, A);

# To start, here are a few very simple examples.

to_tikz(f, labels=true)
#-
to_tikz(f⋅g, labels=true)
#-
to_tikz(f⊗g, labels=true, orientation=TopToBottom)

# Here is a more complex example, involving generators with compound domains and
# codomains.

h, k = Hom(:h, C, D),  Hom(:k, D, C)
m, n = Hom(:m, B⊗A, A⊗B), Hom(:n, D⊗C, C⊗D)
q = Hom(:l, A⊗B⊗C⊗D, D⊗C⊗B⊗A)

to_tikz((f⊗g⊗h⊗k)⋅(m⊗n)⋅q⋅(n⊗m)⋅(h⊗k⊗f⊗g))

# Identities and braidings appear as wires.

to_tikz(id(A), labels=true)
#-
to_tikz(braid(A,B), labels=true, labels_pos=0.25)
#-
to_tikz(braid(A,B) ⋅ (g⊗f) ⋅ braid(A,B))

# The isomorphism $A \otimes B \otimes C \to C \otimes B \otimes A$ induced by
# the permutation $(3\ 2\ 1)$ is a composite of braidings and identities.

σ = (braid(A,B) ⊗ id(C)) ⋅ (id(B) ⊗ braid(A,C) ⋅ (braid(B,C) ⊗ id(A)))

to_tikz(σ, arrowtip="Stealth", arrowtip_pos="-0.1pt",
        labels=true, labels_pos="0.1pt")

# By default, anchor points are added along identity and braiding wires to
# reproduce the expression structure in the layout. The anchors can be disabled
# to get a more "unbiased" layout.

to_tikz(σ, anchor_wires=false, arrowtip="Stealth", arrowtip_pos="-0.1pt",
        labels=true, labels_pos="0.1pt")

# ### Biproduct category

A, B, C = Ob(FreeBiproductCategory, :A, :B, :C)
f = Hom(:f, A, B)

to_tikz(mcopy(A), labels=true)
#-
to_tikz(delete(A), labels=true)
#-
to_tikz(mcopy(A)⋅(f⊗f)⋅mmerge(B), labels=true)
#-
to_tikz(mcopy(A⊗B), orientation=TopToBottom, labels=true)
#-
to_tikz(mcopy(A⊗B⊗C), orientation=TopToBottom, labels=true)

# ### Compact closed category

# The unit and co-unit of a compact closed category appear as caps and cups.

A, B = Ob(FreeCompactClosedCategory, :A, :B)

to_tikz(dunit(A), arrowtip="Stealth", labels=true)
#-
to_tikz(dcounit(A), arrowtip="Stealth", labels=true)

# In a self-dual compact closed category, such as a bicategory of relations,
# every morphism $f: A \to B$ has a transpose $f^\dagger: B \to A$ given by
# bending wires:

A, B = Ob(FreeBicategoryRelations, :A, :B)
f = Hom(:f, A, B)

to_tikz((dunit(A) ⊗ id(B)) ⋅ (id(A) ⊗ f ⊗ id(B)) ⋅ (id(A) ⊗ dcounit(B)))

# ### Abelian bicategory of relations

# In an abelian bicategory of relations, such as the category of linear
# relations, the duplication morphisms $\Delta_X: X \to X \otimes X$ and
# addition morphisms $\blacktriangledown_X: X \otimes X \to X$ belong to a
# bimonoid. Among other things, this means that the following two morphisms are
# equal.

X = Ob(FreeAbelianBicategoryRelations, :X)

to_tikz(plus(X) ⋅ mcopy(X))
#-
to_tikz((mcopy(X)⊗mcopy(X)) ⋅ (id(X)⊗braid(X,X)⊗id(X)) ⋅ (plus(X)⊗plus(X)))

# ## Custom styles

# The visual appearance of wiring diagrams can be customized using the builtin
# options or by redefining the TikZ styles for the boxes or wires.

A, B, = Ob(FreeSymmetricMonoidalCategory, :A, :B)
f, g = Hom(:f, A, B), Hom(:g, B, A)

pic = to_tikz(f⋅g, styles=Dict(
  "box" => ["draw", "fill"=>"{rgb,255: red,230; green,230; blue,250}"],
))
#-
X = Ob(FreeAbelianBicategoryRelations, :X)

to_tikz(plus(X) ⋅ mcopy(X), styles=Dict(
  "junction" => ["circle", "draw", "fill"=>"red", "inner sep"=>"0"],
  "variant junction" => ["circle", "draw", "fill"=>"blue", "inner sep"=>"0"],
))

# By default, the boxes are rectangular (`:rectangle`). Other available shapes
# include circles (`:circle`), ellipses (`:ellipse`), triangles (`:triangle`,
# `:invtriangle`), and trapezoids (`:trapezium`, `:invtrapezium`).

to_tikz(f⋅g, box_shape=:circle)
#-
to_tikz(f⋅g, box_shape=:triangle, rounded_boxes=false)
#-
to_tikz(f⋅g, box_shape=:invtriangle, rounded_boxes=false)
#-
to_tikz(f⋅g, box_shape=:invtriangle, orientation=TopToBottom, rounded_boxes=false)
#-
to_tikz(f⋅g, box_shape=:trapezium)
#-
to_tikz(f⋅g, box_shape=:invtrapezium)

# ## Output formats

# The function `to_tikz` returns an object of type `TikZ.Document`, representing
# a TikZ picture and its TikZ library dependencies as an abstract syntax tree.
# When displayed interactively, this object is compiled by LaTeX to PDF and then
# converted to SVG.

# To generate the LaTeX source code, use the builtin pretty-printer. This
# feature does not require LaTeX or TikzPictures.jl to be installed.

import Catlab.Graphics: TikZ

doc = to_tikz(f⋅g)
TikZ.pprint(doc)


function ptrace_ref(A::Matrix, out, sysdims::NTuple)
    # Function adapted from a [MATLAB function](http://www.dr-qubit.org/Matlab_code.html) by Toby Cubitt and licensed under GPL2.
    # First, calculate systems, dimensions, etc.
    out = collect(out)
    nsub = length(sysdims) # Number of subsytems
    rdims = reverse(sysdims) # Dimensions of subsystems in reverse order
    keep = setdiff(1:nsub,out) # Indices of subsystems to keep
    dimstrace = prod(sysdims[out])::Int # Total dim of subsystems to trace out
    dimskeep = prod(sysdims[keep])::Int # Total dim of subsystems to keep
    # Reshape density matrix into tensor with one row and one column index
    # for each subsystem, permute traced subsystem indices to the end,
    perm = (nsub + 1) .- [reverse(keep); reverse(keep).-nsub; out; out.-nsub]
    B = permutedims(reshape(A,rdims...,rdims...),perm)
    # Reshape again so that first two indices are row and column
    # multi-indices for kept subsystems and third index is a flattened index
    # for traced subsystems
    C = reshape(B,dimskeep,dimskeep,dimstrace^2)
    # Sum third index over "diagonal" entries
    return dropdims(sum(C[:,:,1:dimstrace+1:dimstrace^2],3),dims=3), sysdims[keep]
end

function ptrace_ref(x::Vector, out::NTuple{no,Int}, sysdims::NTuple{ns,Int}) where {no,ns}
    # Make sure the input arguments make sense
    issorted(out) || throw(ArgumentError("subsystem indices $out must be sorted"))
    no>ns && throw(ArgumentError("more subsystem indices ($no) than number of subsystems ($ns)"))
    for i = 1:no
        out[i]>ns && throw(ArgumentError("subsystem index $(out[i]) larger than number of subsystems"))
    end
    # First, calculate systems, dimensions, etc.
    keep = sorted_setdiff(ntuple(identity,Val(ns)),out) # Subsystems to keep
    rsysdims = revtuple(sysdims) # Dimensions of subsystems in reverse order
    keepdims = gettuple(sysdims,keep) # Dimensions of subsystems to keep
    outdims  = gettuple(sysdims,out) # Dimensions of subsystems to trace out
    # Reshape state vector to "reverse" ket on traced subsystems into a bra
    perm = (ns + 1) .- [collect(revtuple(keep)); collect(out)]
    y = permutedims(reshape(x,rsysdims...),perm)
    y = reshape(y,prod(keepdims),prod(outdims))
    # Take outer product
    return y*y', keepdims
end


using SymbolicUtils: Sym, FnType, Term, symtype, Contextual, EmptyCtx
using SymbolicUtils
using Test

@testset "@syms" begin
    let
        @syms a b::Float64 f(::Real) g(p, h(q::Real))::Int

        @test a isa Sym{Number}
        @test a.name === :a

        @test b isa Sym{Float64}
        @test b.name === :b

        @test f isa Sym{FnType{Tuple{Real}, Number}}
        @test f.name === :f

        @test g isa Sym{FnType{Tuple{Number, FnType{Tuple{Real}, Number}}, Int}}
        @test g.name === :g

        @test f(b) isa Term
        @test symtype(f(b)) === Number
        @test_throws ErrorException f(a)

        @test g(b, f) isa Term
        @test_throws ErrorException g(b, a)

        @test symtype(g(b, f)) === Int
    end
end

@testset "hashing" begin
    @syms a b f(x, y)
    @test hash(a) == hash(a)
    @test hash(a) != hash(b)
    @test hash(a+1) == hash(a+1)
    @test hash(sin(a+1)) == hash(sin(a+1))
    @test hash(f(1,a)) == hash(f(1, a))

    c = a
    g = f
    @syms a f(x, y)
    @test hash(a) == hash(c)
    @test hash(g(a, b)) == hash(f(a,b))
    @test hash(f(a, b)) == hash(f(c,b))
    @test hash(sin(a+1)) == hash(sin(c+1))
end

@testset "Base methods" begin
    @syms w::Complex z::Complex a::Real b::Real x

    @test isequal(w + z, Term{Complex}(+, [w, z]))
    @test isequal(z + a, Term{Number}(+, [z, a]))
    @test isequal(a + b, Term{Real}(+, [a, b]))
    @test isequal(a + x, Term{Number}(+, [a, x]))
    @test isequal(a + z, Term{Number}(+, [a, z]))

    # promote_symtype of identity
    @test isequal(Term(identity, [w]), Term{Complex}(identity, [w]))
    @test isequal(+(w), w)
    @test isequal(+(a), a)

    @test isequal(rem2pi(a, RoundNearest), Term{Real}(rem2pi, [a, RoundNearest]))

    # bool
    for f in [(==), (!=), (<=), (>=), (<), (>)]
        @test isequal(f(a, 0), Term{Bool}(f, [a, 0]))
        @test isequal(f(0, a), Term{Bool}(f, [0, a]))
        @test isequal(f(a, a), Term{Bool}(f, [a, a]))
    end

    @test symtype(cond(true, 4, 5)) == Int
    @test symtype(cond(a < 0, b, w)) == Union{Real, Complex}
    @test_throws MethodError w < 0
    @test isequal(w == 0, Term{Bool}(==, [w, 0]))
end

@testset "err test" begin
    @syms t()
    @test_throws ErrorException t(2)
end

@testset "Contexts" begin
    @syms a b c

    @test @rule(~x::Contextual((x, ctx) -> ctx==EmptyCtx()) => "yes")(1) == "yes"
    @test @rule(~x::Contextual((x, ctx) -> haskey(ctx, x)) => true)(a, Dict(a=>1))
    @test @rule(~x::Contextual((x, ctx) -> haskey(ctx, x)) => true)(b, Dict(a=>1)) === nothing
    @test_throws UndefVarError @rule(~x => __CTX__)(a, "test")
    @test @rule(~x => @ctx)(a, "test") == "test"
    @test @rule(~x::Contextual((x, ctx) -> haskey(ctx, x)) => (@ctx)[~x])(a, Dict(a=>1)) === 1
    @test @rule(~x::Contextual((x, ctx) -> haskey(ctx, x)) => (@ctx)[~x])(b, Dict(a=>1)) === nothing
    @test simplify(a+a, "test", rules=RuleSet([@rule ~x => @ctx])) == "test"
end

@testset "substitute" begin
    @syms a b
    @test substitute(a, Dict(a=>1)) == 1
    @test isequal(substitute(sin(a+b), Dict(a=>1)), sin(1+b))
    @test substitute(a+b, Dict(a=>1, b=>3)) |> simplify == 4
end

@testset "printing" begin
    @syms a b
    @test repr(a+b) == "a + b"
    @test repr(-a) == "-a"
    @test repr(-a + 3) == "(-a) + 3"
end


using KNITRO, Compat.Test

#    min  9 - 8x1 - 6x2 - 4x3
#         + 2(x1^2) + 2(x2^2) + (x3^2) + 2(x1*x2) + 2(x1*x3)
#    subject to  c[0]:  x1 + x2 + 2x3 <= 3
#                x1 >= 0
#                x2 >= 0
#                x3 >= 0
#    initpt (0.5, 0.5, 0.5)
#
#    Solution is x1=4/3, x2=7/9, x3=4/9, lambda=2/9  (f* = 1/9)
#
#  The problem comes from Hock and Schittkowski, HS35.

function eval_f(x::Vector{Float64})
    linear_terms = 9.0 - 8.0*x[1] - 6.0*x[2] - 4.0*x[3]
    quad_terms = 2.0*x[1]^2 + 2.0*x[2]^2 + x[3]^2 + 2.0*x[1]*x[2] + 2.0*x[1]*x[3]
    return linear_terms + quad_terms
end

function eval_g(x::Vector{Float64}, cons::Vector{Float64})
    cons[1] = x[1] + x[2] + 2.0*x[3]
end

function eval_grad_f(x::Vector{Float64}, grad::Vector{Float64})
    grad[1] = -8.0 + 4.0*x[1] + 2.0*x[2] + 2.0*x[3]
    grad[2] = -6.0 + 2.0*x[1] + 4.0*x[2]
    grad[3] = -4.0 + 2.0*x[1]            + 2.0*x[3]
end

function eval_jac_g(x::Vector{Float64}, jac::Vector{Float64})
    jac[1] = 1.0
    jac[2] = 1.0
    jac[3] = 2.0
end

function eval_h(x::Vector{Float64}, lambda::Vector{Float64},
                sigma::Float64, hess::Vector{Float64})
    hess[1] = sigma*4.0
    hess[2] = sigma*2.0
    hess[3] = sigma*2.0
    hess[4] = sigma*4.0
    hess[5] = sigma*2.0
end

function eval_hv(x::Vector{Float64}, lambda::Vector{Float64},
                 sigma::Float64, hv::Vector{Float64})
    hv[1] = sigma*4.0*hv[1] + sigma*2.0*hv[2] + sigma*2.0*hv[3]
    hv[2] = sigma*2.0*hv[1] + sigma*4.0*hv[2]
    hv[3] = sigma*2.0*hv[1]                   + sigma*2.0*hv[3]
end

objGoal = KTR_OBJGOAL_MINIMIZE
objType = KTR_OBJTYPE_QUADRATIC

n = 3
x_L = zeros(n)
x_U = [KTR_INFBOUND,KTR_INFBOUND,KTR_INFBOUND]

m = 1
c_Type = [KTR_CONTYPE_LINEAR]
c_L = [-KTR_INFBOUND]
c_U = [3.0]

jac_con = Int32[0,0,0]
jac_var = Int32[0,1,2]
hess_row = Int32[0,0,0,1,2]
hess_col = Int32[0,1,2,1,2]

x       = [0.5,0.5,0.5]
lambda  = zeros(n+m)
obj     = [0.0]

kp = createProblem()
@test applicationReturnStatus(kp) == :Uninitialized
loadOptionsFile(kp,joinpath(dirname(@__FILE__),"knitro.opt"))
@test applicationReturnStatus(kp) == :Uninitialized

# --- test getters and setters ---
hessopt = zeros(Int32, 1)
getOption(kp, "hessopt", hessopt)
@test hessopt[1] == 1
setOption(kp, "hessopt", 0)
getOption(kp, "hessopt", hessopt)
@test hessopt[1] == 0
setOption(kp, "hessopt", 1)
getOption(kp, "hessopt", hessopt)
@test hessopt[1] == 1
# ---------------------------------

initializeProblem(kp, objGoal, objType, x_L, x_U, c_Type, c_L, c_U,
                  jac_var, jac_con, hess_row, hess_col)
@test applicationReturnStatus(kp) == :Initialized
setCallbacks(kp, eval_f, eval_g, eval_grad_f, eval_jac_g, eval_h, eval_hv)
@test applicationReturnStatus(kp) == :Initialized
solveProblem(kp)

# --- test optimal solutions ---
@testset "Test optimal solutions" begin
    @test applicationReturnStatus(kp) == :Optimal
    @test isapprox(kp.x, [1.3333333, 0.7777777, 0.4444444], atol=1e-5)
    @test isapprox(kp.obj_val[1], 0.1111111, atol=1e-5)
end

@testset "Test getters" begin
    @test get_number_FC_evals(kp) >= 0
    @test get_number_GA_evals(kp) >= 0
    @test get_number_H_evals(kp) >= 0
    @test get_number_HV_evals(kp) >= 0
    @test get_number_iters(kp) >= 0
    @test get_number_cg_iters(kp) >= 0
    @test isapprox(get_abs_feas_error(kp), 0.0, atol=1e-5)
    @test isapprox(get_rel_feas_error(kp), 0.0, atol=1e-5)
    @test isapprox(get_abs_opt_error(kp), 0.0, atol=1e-5)
    @test isapprox(get_rel_opt_error(kp), 0.0, atol=1e-5)
end

# start at a different starting point
x = [2.0,2.0,2.0]
lambda = ones(n+m)
restartProblem(kp, x, lambda)
@test applicationReturnStatus(kp) == :Initialized
solveProblem(kp)
@testset "Test optimal solutions (at a different starting point)" begin
    @test applicationReturnStatus(kp) == :Optimal
    @test isapprox(kp.x,  [1.3333333, 0.7777777, 0.4444444], atol=1e-5)
    @test isapprox(kp.obj_val[1], 0.1111111, atol=1e-5)
end

freeProblem(kp)


using StochasticDiffEq, DiffEqDevTools, Test, LinearAlgebra, Random

Random.seed!(100)
const σ_const = 0.87
const μ = 1.01

u0 = rand(2)
A = [-2.0 1.0;1.0 -2.0]
B = Diagonal([σ_const for i in 1:2])

function f_nondiag(u,p,t)
  return A*u + 1.01*u
end

function f_nondiag_iip(du,u,p,t)
  mul!(du,A,u)
  du .+= 1.01u
end

function f_analytic_nondiag(u0,p,t,W)
 tmp = (A+1.01I-(B^2))*t + B*sum(W)
 exp(tmp)*u0
end

function g_nondiag(u,p,t)
  du = zeros(2,2)
  du[1,1] = σ_const*u[1]
  du[1,2] = σ_const*u[1]
  du[2,1] = σ_const*u[2]
  du[2,2] = σ_const*u[2]
  return du
end

function g_nondiag_iip(du,u,p,t)
  du[1,1] = σ_const*u[1]
  du[1,2] = σ_const*u[1]
  du[2,1] = σ_const*u[2]
  du[2,2] = σ_const*u[2]
end

coeff = 2*σ_const^2 #To not compute the same coefficient in the sde.
function ggprime(u, p, t)
  return coeff*u
end

function ggprime(du, u, p, t)
  du .= coeff*u
end

prob = SDEProblem(SDEFunction(f_nondiag,g_nondiag,analytic=f_analytic_nondiag),g_nondiag,u0,(0.0,1.0),noise_rate_prototype=zeros(2,2))
probiip = SDEProblem(SDEFunction(f_nondiag_iip,g_nondiag_iip,analytic=f_analytic_nondiag),g_nondiag_iip,u0,(0.0,1.0),noise_rate_prototype=zeros(2,2))

## Just solve to test compatibility
IEM = solve(probiip,ImplicitEM())
IEM = solve(probiip,ImplicitEulerHeun())
IEM = solve(probiip,ISSEM())
IEM = solve(probiip,ISSEulerHeun())

## use BenchmarkTools to benchmark speed here if desired.
dt = 0.002
seed = 1
# solEMiip = solve(probiip, EM(), dt=dt, seed=seed)
# solRKMil = solve(probiip, RKMilCommute(), dt=dt, seed=seed)
solPCEuler = solve(prob, PCEuler(ggprime), dt=dt, seed=seed)
solPCEuleriip = solve(probiip, PCEuler(ggprime), dt=dt, seed=seed)

@test solPCEuler.u ≈ solPCEuleriip.u atol=1e-10

## plot to see performance of PCEuler
# first_elem(x) = x[1]
# using PyPlot
# figure()
# plot(solEMiip.t, first_elem.(solEMiip.u_analytic), label="True", "--")
# plot(solEMiip.t, first_elem.(solEMiip.u), label="EM")
# plot(solRKMil.t, first_elem.(solRKMil.u), label="RKMil")
# plot(solPCEuleriip.t, first_elem.(solPCEuleriip.u), label="PC")
# legend()

##
dts = (1/2) .^ (10:-1:5) #14->7 good plot
trajectories = 50
simEM = test_convergence(dts,probiip,EM(),trajectories=trajectories)
simPCEuler = test_convergence(dts,probiip,PCEuler(ggprime),trajectories=trajectories)
#simRKMil = test_convergence(dts,probiip,RKMilCommute(),trajectories=trajectories)
@test all(simPCEuler.errors[:l2] .< simEM.errors[:l2])

## Plotting script to see the order 1 scaling of PCEuler
# figure()
# loglog(dts, simEM.errors[:l2], label="EM")
# loglog(dts, simRKMil.errors[:l2], label="RKMil")
# loglog(dts, simPCEuler.errors[:l2], label="PC")
# ylabel("l2 error")
# xlabel("dt")
# legend()

f_morenoise = (du,u,p,t) -> du.=1.01u
g_morenoise = function (du,u,p,t)
  du[1,1] = 0.3u[1]
  du[1,2] = 0.6u[1]
  du[1,3] = 0.9u[1]
  du[1,4] = 0.12u[2]
  du[2,1] = 1.2u[1]
  du[2,2] = 0.2u[2]
  du[2,3] = 0.3u[2]
  du[2,4] = 1.8u[2]
end
prob = SDEProblem(f_morenoise,g_morenoise,ones(2),(0.0,1.0),
                  noise_rate_prototype=zeros(2,4))

sol =solve(prob,dt=1/2^(3),EM())
sol =solve(prob,dt=1/2^(3),ISSEM())
sol =solve(prob,dt=1/2^(3),ImplicitEM())
sol =solve(prob,dt=1/2^(3),EulerHeun())
sol =solve(prob,dt=1/2^(3),ImplicitEulerHeun())
sol =solve(prob,dt=1/2^(3),ISSEulerHeun())