nload(buses::Vector{<:Bus}) = length([b for b in buses if isa(b, LoadBus)])

ngen(buses::Vector{<:Bus}) = length([b for b in buses if isa(b, GenBus)])

count_bus_type(grid::Grid) = (nload(buses(grid)), ngen(buses(grid)))

#######################
### CHECK FUNCTIONS ###
#######################
# Everything here uses connectivty matrices as input instead of grids because
# all functions are supposed to work both for buses and for substations,
# effectively giving us two representations of the network (fine and coarse)
"""
    laplacian(con_mat::AbstractMatrix{<:Real})

Return the Laplacian matrix for a graph defined by adjacency matrix `con_mat`.
"""
function laplacian(conmat::AbstractMatrix{Bool})
    lap = zeros(Int, size(conmat))
    degree = sum(conmat, dims=1) # this gives the node degrees
    n = length(degree)
    for j in 1:(n - 1), i in (j + 1):n
        if conmat[i,j] > 0
            lap[i,j] = -1
        end
        lap[j,j] = degree[j]
    end
    lap[n,n] = degree[n]
    return Symmetric(lap, :L)
end

function laplacian(con_mat::AbstractMatrix{T}) where T <: Real
    return laplacian(con_mat .> zero(T)) # treating the substation graph as a simple graph.
end

# Shorter code but worse performance.
# function laplacian(con_mat::AbstractMatrix{<:Bool})
#     return sum(con_mat, 1) .* eye(Int, size(con_mat)[1]) - con_mat
# end

"""
    total_links(con_mat::AbstractMatrix{Bool})

Return the total number of links in a system with connectivity matrix `con_mat`.
"""
function total_links(con_mat::AbstractMatrix{Bool})
    return (1 / 2) * sum(con_mat)
end

"""
    total_links(con_mat::AbstractMatrix{Integer})

Return the total number of links in a system with connectivity matrix `con_mat`. The graph
will be treated as a simple graph.
"""
function total_links(con_mat::AbstractMatrix{T}) where T <: Real
    return total_links(con_mat .> zero(T)) # treating the substation graph as a simple graph.
end

"""
    mean_node_deg(con_mat::AbstractMatrix)

Return the average nodal degree of a system with connectivity matrix `con_mat`.
"""
function mean_node_deg(con_mat::AbstractArray{Bool, 2})
    return mean(sum(con_mat, dims=1))
end

function mean_node_deg(con_mat::AbstractArray{Int, 2})
    return mean_node_deg(con_mat .> 0) # treating the substation graph as a simple graph.
end

"""
    cluster_coeff(con_mat::AbstractMatrix)

Return the clustering coefficient of a system with connectivity matrix `con_mat`.
"""
function cluster_coeff(conmat::AbstractMatrix{Bool})
    degree = sum(conmat, 1) # this gives the node degrees
    clust_coeff = Vector{Float64}(length(degree))
    for i in 1:length(degree)
        maxedges = degree[i] * (degree[i] - 1) / 2
        cumul = 0
        for j in 1:length(degree)
            if conmat[i,j]
                cumul += dot(conmat[i,:],conmat[:,j])
            end
        end
        push!(clust_coeff, maxedges > 0 ? cumul/maxedges : 0)
    end
    return mean(clust_coeff)
end

function cluster_coeff(con_mat::AbstractMatrix{Int})
    return cluster_coeff(con_mat .> 0) # treating the substation graph as a simple graph.
end

"""
    cluster_coeff_degw(con_mat::AbstractMatrix{<:Real})

Return the degree-weighted clustering coefficient of a system with connectivity
matrix `con_mat`.
"""
function cluster_coeff_degw(conmat::AbstractMatrix{Bool})
    degree = sum(conmat, 1) # this gives the node degrees
    clust_coeff = Vector{Float64}(length(degree))
    for i in 1:length(degree)
        maxedges = degree[i] * (degree[i] - 1) / 2
        cumul = 0
        for j in 1:length(degree)
            if conmat[i, j]
                cumul += dot(conmat[i,:], conmat[:,j])
            end
        end
        push!(clust_coeff, maxedges > 0 ? degree[i] * cumul / maxedges : 0)
    end
    return mean(clust_coeff) / mean(degree)
end

function cluster_coeff_degw(con_mat::AbstractMatrix{Int})
    return cluster_coeff(con_mat .> 0) # treating the substation graph as a simple graph.
end

"""
    test_connectivity(con_mat::AbstractMatrix{<:Integer}, verb=true)

Return `true` if the system with connectivity matrix `con_mat`is connected and `false`
otherwise. If `verb=true` the result will be printed onscreen together with the Fiedler
eigenvalue.
"""
function test_connectivity(con_mat::AbstractMatrix{<:Integer}, verb=true)
    ZERO_TOL = 1e-13 # tolerance for zero due to numerical noise
    lap = laplacian(con_mat)
    evals = eigvals(lap)
    sort!(evals)
    if length(evals) > 1 && evals[2] > ZERO_TOL
        fiedler = evals[2]
        if verb
            println("Fiedler eigenvalue: $fiedler. System is connected.")
        end
        return true
    else
        if verb
            println("Fiedler eigenvalue equals zero. Network is NOT connected!")
        end
        return false
    end
end

"""
    dijkstra(con_mat::AbstractMatrix{<:Real}, v1)

Compute minimum distances to each node in the network with connectivity matrix
con_mat, starting from node v1, using Dijkstra's algorithm.
"""
function dijkstra(con_mat::AbstractMatrix{<:Real}, v1)
    n = size(con_mat, 1)
    c = v1
    visited = [v1]
    dist = [i == v1 ? 0 : Inf for i in 1:n]
    r_dist = [Inf for d in dist]
    while length(visited) < n
        neigh = [i for i in 1:n if con_mat[c, i] > 0 && !(i in visited)]
        for i in neigh
            if dist[i] > (dist[c] + con_mat[c, i])
                dist[i] = (dist[c] + con_mat[c, i])
                r_dist[i] = dist[i]
            end
        end
        c = indmin(r_dist)
        push!(visited, c)
        r_dist[c] = Inf
    end
    return dist
end

"""
    floyd_warshall(weights::AbstractMatrix{<:Real})

Compute minimum distances to each node in the network using Floyd-Warshall's algorithm. Here
we assume that the weigths matrix is symmetric.
"""
function floyd_warshall(weights::AbstractMatrix{<:Real})
    s = size(weights)
    dist = zeros(Float64, s)
    for j in 2:s[1]
        for i in 1:(j - 1)
            if weights[i, j] > 0
                dist[i, j] = weights[i, j] # we are assuming that weights is symmetric as it
                dist[j, i] = weights[i, j] # should be in our case.
            else
                dist[i, j] = Inf
                dist[j, i] = Inf
            end
        end
    end

    for k in 1:s[1]
        for i in 1:s[1]
            for j in 1:s[1]
                if dist[i, j] > dist[i, k] + dist[k, j]
                    dist[i, j] = dist[i, k] + dist[k, j]
                end
            end
        end
    end
    return dist
end

"""
    mean_shortest_path(conmat::AbstractMatrix{<:Real})

Compute the mean shortest distance between nodes for a network represented by `conmat`.
The `conmat` matrix should specify the distance between each connected node and be equal to
zero whenever two nodes are not connected. Using the adjacency matrix will return the mean
shortest path in hops. `conmat` is assumed to be symmetric. Floyd-Warshall's algorithm is
used to compute the distances.
"""
function mean_shortest_path(conmat::AbstractMatrix{<:Real})
    n = size(conmat, 1)
    return sum(floyd_warshall(conmat))/(n * n)
end

"""
    mean_shortest_path(conmat::AbstractMatrix{<:Integer}, distmat::AbstractMatrix{Float64})

Compute the mean shortest distance between nodes for a network with connectivity matrix
`conmat` and distances between nodes given by `distmat`. Distances are computed using
Floyd-Warshall's algorithm.
"""
function mean_shortest_path(
        conmat::AbstractMatrix{<:Integer},
        distmat::AbstractMatrix{Float64}
    )
    return mean_shortest_path(conmat.*distmat)
end

# """
#     mean_shortest_path(conmat::AbstractMatrix{<:Real})
#
# Compute the mean shortest distance between nodes for a network represented by `conmat`.
# The `conmat` matrix should specify the distance between each connected node and be equal to
# zero whenever two nodes are not connected. Distances are computed using Dijkstra's algorithm.
# """
# function mean_shortest_path(conmat::AbstractMatrix{<:Real})
#     n = size(conmat, 1)
#     mdist = [mean(dijkstra(conmat, i)) for i in 1:n]
#     return mean(mdist)
# end

# """
#     mean_shortest_path(conmat::AbstractMatrix{<:Real}, distmat::AbstractMatrix{<:Real})
#
# Compute the mean shortest distance between nodes for a network with connectivity matrix
# `conmat` and distances between nodes given by `distmat`. Distances are computed using
# Dijkstra's algorithm.
# """
# function mean_shortest_path(
#         conmat::AbstractMatrix{<:Real},
#         distmat::AbstractMatrix{<:Real}
#     )
#     return mean_shortest_path(conmat.*distmat)
# end

"""
    robustness_line(con_mat::AbstractMatrix{<:Integer}, n=1000)

Return the average number of transmission lines that have to be randomly removed from a
system with connectivity matrix `con_mat` before it becomes disconnected. Average is
computed over n iterations.
"""
function robustness_line(con_mat::AbstractMatrix{Bool}, n=1000)
    s = size(con_mat, 1)
    conns = []
    for i in 1:(s - 1), j in (i + 1):s
        con_mat[j, i] > 0 ? push!(conns, (i, j)) : nothing
    end
    total = 0
    for rep in 1:n
        connscopy = copy(conns)
        conmat = copy(con_mat)
        step = 0
        while test_connectivity(conmat, false)
            k = rand(1:length(connscopy))
            ij = connscopy[k]
            conmat[ij[1],ij[2]] = false
            conmat[ij[2],ij[1]] = false
            deleteat!(connscopy, k)
            step += 1
        end
        total += step
    end
    return total / n
end

function robustness_line(con_mat::AbstractMatrix{Int}, n=1000)
    return robustness_line(con_mat .> 0 , n)
end

"""
    robustness_node(con_mat::AbstractMatrix{<:Integer}, n=1000)

Return the average number of nodes that have to be randomly removed from a system with
connectivity matrix `con_mat` before it becomes disconnected. Average is computed over n
iterations.
"""
function robustness_node(con_mat::AbstractMatrix{Bool}, n=1000)
    s = size(con_mat, 1)
    total = 0
    for rep in 1:n
        len = s
        conmat = copy(con_mat)
        step = 0
        while test_connectivity(conmat, false)
            k = rand(1:len)
            mask = ones(Bool, len)
            mask[k] = false
            conmat = conmat[mask, mask]
            step += 1
            len -= 1
        end
        total += step
    end
    return total / n
end

function robustness_node(con_mat::AbstractMatrix{Int}, n=1000)
    return robustness_node(con_mat .> 0, n)
end


const branin = let res_init=zeros(2), jac_init=zeros(2,2), x_init=[2.0, 0.0]

    function res(x, r)
        r[1] = 4.0 * (x[1] + x[2])
        r[2] = r[1] + (x[1] - x[2]) * ((x[1] - 2.0) ^ 2 + x[2] ^ 2 - 1.0)
        return r
    end

    function jac(x, jac)
        jac[1, 1] = 4.0
        jac[1, 2] = 4.0
        jac[2, 1] = 3.0 + (x[1] - 2.0) * (3.0 * x[1] - 2.0 * x[2] - 2.0) + x[2] * x[2]
        jac[2, 2] = 1.0 + 2.0 * (2.0*x[1] - x[2] * x[2]) - (x[1] - x[2])^2
        return jac
    end

    f(;scale=1, verbose=false, print_steps=false) = testone("branin", res, jac,
                                                            res_init, jac_init, x_init;
                                                            scale=scale, verbose=verbose,
                                                            print_steps=print_steps)
end


include("utils.jl")

@testset "Stiefel" begin
    @testset "Real" begin
        M = Stiefel(3,2)
        @testset "Basics" begin
            x = [1.0 0.0; 0.0 1.0; 0.0 0.0]
            @test representation_size(M) == (3,2)
            @test manifold_dimension(M) == 3
            @test_throws DomainError is_manifold_point(M, [1., 0., 0., 0.],true)
            @test_throws DomainError is_manifold_point(M, 1im*[1.0 0.0; 0.0 1.0; 0.0 0.0],true)
            @test !is_tangent_vector(M, x, [0., 0., 1., 0.])
            @test_throws DomainError is_tangent_vector(M, x, 1 * im * zero_tangent_vector(M,x), true)
        end

        types = [
            Matrix{Float64},
            MMatrix{3, 2, Float64},
            Matrix{Float32},
        ]
        @testset "Type $T" for T in types
            x = [1.0 0.0; 0.0 1.0; 0.0 0.0]
            y = exp(M,x, [0.0 0.0; 0.0 0.0; 1.0 1.0])
            z = exp(M,x,[ 0.0 0.0; 0.0 0.0; -1.0 1.0])
            pts = convert.(T, [x,y,z])
            v = inverse_retract(M,x,y,PolarInverseRetraction())
            @test !is_manifold_point(M,2*x)
            @test_throws DomainError !is_manifold_point(M,2*x,true)
            @test !is_tangent_vector(M,2*x,v)
            @test_throws DomainError !is_tangent_vector(M,2*x,v,true)
            @test !is_tangent_vector(M,x,y)
            @test_throws DomainError is_tangent_vector(M,x,y,true)
            test_manifold(
                M,
                pts,
                test_exp_log = false,
                default_inverse_retraction_method = PolarInverseRetraction(),
                test_injectivity_radius = false,
                test_is_tangent = true,
                test_project_tangent = true,
                test_vector_transport = false,
                test_forward_diff = false,
                test_reverse_diff = false,
                projection_atol_multiplier = 15.0,
                retraction_atol_multiplier = 10.0,
                is_tangent_atol_multiplier = 4*10.0^2,
                retraction_methods = [PolarRetraction(), QRRetraction()],
                inverse_retraction_methods = [PolarInverseRetraction(), QRInverseRetraction()]
            )

            @testset "inner/norm" begin
                v1 = inverse_retract(M, pts[1], pts[2], PolarInverseRetraction())
                v2 = inverse_retract(M, pts[1], pts[3], PolarInverseRetraction())

                @test real(inner(M, pts[1], v1, v2)) ≈ real(inner(M, pts[1], v2, v1))
                @test imag(inner(M, pts[1], v1, v2)) ≈ -imag(inner(M, pts[1], v2, v1))
                @test imag(inner(M, pts[1], v1, v1)) ≈ 0

                @test norm(M, pts[1], v1) isa Real
                @test norm(M, pts[1], v1) ≈ sqrt(inner(M, pts[1], v1, v1))
            end
        end
    end

    @testset "Complex" begin
        M = Stiefel(3,2,ℂ)
        @testset "Basics" begin
            @test representation_size(M) == (3,2)
            @test manifold_dimension(M) == 8
            @test !is_manifold_point(M, [1., 0., 0., 0.])
            @test !is_tangent_vector(M, [1.0 0.0; 0.0 1.0; 0.0 0.0], [0., 0., 1., 0.])
            x = [1.0 0.0; 0.0 1.0; 0.0 0.0]
            @test_throws DomainError is_manifold_point(M, [:a :b; :c :d; :e :f],true)
            @test_throws DomainError is_tangent_vector(M, x, [:a :b; :c :d; :e :f], true)
        end
        types = [
            Matrix{ComplexF64},
        ]
        @testset "Type $T" for T in types
            x = [0.5+0.5im 0.5+0.5im; 0.5+0.5im -0.5-0.5im; 0.0 0.0]
            y = exp(M, x, [0.0 0.0; 0.0 0.0; 1.0 1.0])
            z = exp(M, x, [0.0 0.0; 0.0 0.0; -1.0 1.0])
            pts = convert.(T, [x,y,z])
            v = inverse_retract(M,x,y,PolarInverseRetraction())
            @test !is_manifold_point(M,2*x)
            @test_throws DomainError !is_manifold_point(M,2*x,true)
            @test !is_tangent_vector(M,2*x,v)
            @test_throws DomainError !is_tangent_vector(M,2*x,v,true)
            @test !is_tangent_vector(M,x,y)
            @test_throws DomainError is_tangent_vector(M,x,y,true)
            test_manifold(
                M,
                pts,
                test_exp_log = false,
                default_inverse_retraction_method = PolarInverseRetraction(),
                test_injectivity_radius = false,
                test_is_tangent = true,
                test_project_tangent = true,
                test_vector_transport = false,
                test_forward_diff = false,
                test_reverse_diff = false,
                projection_atol_multiplier = 15.0,
                retraction_atol_multiplier = 10.0,
                is_tangent_atol_multiplier = 4*10.0^2,
                retraction_methods = [PolarRetraction(), QRRetraction()],
                inverse_retraction_methods = [PolarInverseRetraction(), QRInverseRetraction()]
            )

            @testset "inner/norm" begin
                v1 = inverse_retract(M, pts[1], pts[2], PolarInverseRetraction())
                v2 = inverse_retract(M, pts[1], pts[3], PolarInverseRetraction())

                @test real(inner(M, pts[1], v1, v2)) ≈ real(inner(M, pts[1], v2, v1))
                @test imag(inner(M, pts[1], v1, v2)) ≈ -imag(inner(M, pts[1], v2, v1))
                @test imag(inner(M, pts[1], v1, v1)) ≈ 0

                @test norm(M, pts[1], v1) isa Real
                @test norm(M, pts[1], v1) ≈ sqrt(inner(M, pts[1], v1, v1))
            end
        end
    end

    @testset "Quaternion" begin
        M = Stiefel(3,2,ℍ)
        @testset "Basics" begin
            @test representation_size(M) == (3,2)
            @test manifold_dimension(M) == 18
        end
    end
end


@testset "check gradient for Linear block " begin
    clear()
    # [1] prepare input data and its label
    T = Array{Float64}
    x = randn(256, 62)
    l = rand(64, 62)
    l = l ./ sum(l,dims=1)
    m = Linear(256, 64; type=T)

    x = Variable(x; type=T);
    l = Variable(l; type=T);

    # [2] forward and backward propagation
    outs = forward(m, x)
    LOSS1 = mse(outs, l)
    COST1 = cost(LOSS1)
    backward()

    # [3] with a samll change of a weight
    GRAD = m.w.delta[1]
    DELTA = 1e-6
    m.w.value[1] += DELTA

    # [4] forward and backward propagation
    outs = forward(m, x)
    LOSS2 = mse(outs, l)
    COST2 = cost(LOSS2)
    backward()

    # [5] check if the auto-grad is true or not
    dLdW = (COST2 - COST1)/DELTA;   # numerical gradient
    err  = abs((dLdW-GRAD)/(GRAD+eps(Float64)))*100;  # relative error in %
    err  = err < 1e-1 ? 0.0 : err;
    @test err<1.0
end


# Write a program that outputs all possibilities to put + or - or nothing
# between the numbers 1,2,…,9 (in this order) such that the result is 100. For
# example 1 + 2 + 3 - 4 + 5 + 6 + 78 + 9 = 100.
using Combinatorics

num = [1,2,3,4,5,6,7,8,9]

function comb(x::Vector{Int64}, s::Int64)
    k = [s,s+1]
    t = parse(Int64, join(x[k]))
    return t, k
end

function  eq_100(x::Vector{Int64})
	co = collect(combinations(1:8))

	for j in co
		for i in 1:8
			c,n = comb(x,i)
			nu = insert!(filter(x -> x ∉ n  , x),n[1],c)
			su = sum(.-(nu[j])) + sum(.+(filter(x -> x ∉ j  , nu)))
			if su == 100
				println(  map(x -> string.("-",x),nu[j]), string.(filter(x -> x ∉ j  , nu)) )
			end
		end
	end

end

eq_100(num)


using Test, LinearAlgebra
using WaveProp
using WaveProp.Nystrom
using WaveProp.Geometry
using WaveProp.Integration
using WaveProp.Mesh

@testset "Greens identity test" begin
    # construct interior solution
    Geometry.clear!()
    pde  = Helmholtz(dim=2,k=1)
    xout = SVector(3,3)
    u    = (x)   -> SingleLayerKernel(pde)(xout,x)
    dudn = (x,n) -> DoubleLayerKernel(pde)(xout,x,n)
    geo = Circle()
    Ω   = Domain(geo)
    M   = meshgen(Ω,h=0.1)
    mesh = NystromMesh(view(M,boundary(Ω));order=5)
    γ₀u   = γ₀(u,mesh)
    γ₁u   = γ₁(dudn,mesh)
    𝐒     = SingleLayerOperator(pde,mesh)
    𝐃     = DoubleLayerOperator(pde,mesh)
    e0    = WaveProp.Nystrom.error_interior_green_identity(𝐒,𝐃,γ₀u,γ₁u)
    norm(e0,Inf)/norm(γ₀u,Inf)
    shand = Kress()
    q     = SingularQuadratureRule(GaussLegendre(8),shand)
    δ𝐒    = singular_weights(𝐒,q)
    δ𝐃    = singular_weights(𝐃,q)
    SS    = 𝐒 + δ𝐒
    DD    = 𝐃 + δ𝐃
    e0    = WaveProp.Nystrom.error_interior_green_identity(SS,DD,γ₀u,γ₁u)
    norm(e0,Inf)/norm(γ₀u,Inf)
    # δS    = singular_weights_dim(𝐒)
    # δD    = singular_weights_dim(𝐃)
    # Sfull = Matrix(𝐒) + δS
    # Dfull = Matrix(𝐃) + δD
    # e1 = WaveProp.Nystrom.error_interior_green_identity(Sfull,Dfull,γ₀u,γ₁u)
    # @test norm(e1,Inf) < norm(e0,Inf)
    # S,D = Nystrom.single_double_layer(pde,mesh)
    # e2 = WaveProp.Nystrom.error_interior_green_identity(S,D,γ₀u,γ₁u)
    # @test e1 ≈ e2
end


using DynamicHMC, Turing, Test

@model gdemo(x, y) = begin
  s ~ InverseGamma(2,3)
  m ~ Normal(0,sqrt(s))
  x ~ Normal(m, sqrt(s))
  y ~ Normal(m, sqrt(s))
  return s, m
end

mf = gdemo(1.5, 2.0)

chn = sample(mf, DynamicNUTS(2000));

@test mean(chn[:s]) ≈ 49/24 atol=0.2
@test mean(chn[:m]) ≈ 7/6 atol=0.2


using UnitJuMP
using Test
import HiGHS

function test_simple_example()
    model = Model(HiGHS.Optimizer)
    set_silent(model)
    max_speed = 2u"m/s"
    a = 9.8u"ft/s^2"
    M = 0.5u"m/s"
    no_speed = 0.0u"m/s"
    data = 4
    @variables(model, begin
        x >= 0, u"m/s"
        y >= 0, u"s"
        z >= 0, u"km/s"
        v[1:4], u"m/s"
        w, Bin
    end)
    @constraints(model, begin
        x + a * y <= max_speed
        z + x <= max_speed
        x + 0.5 * a * y <= max_speed
        x <= M * w
        -3x + 2z <= no_speed
        x == 0.15 * max_speed
        con7[i = 1:4], v[i] <= max_speed
        con8, sum(v[i] for i in 1:3) <= max_speed, u"ft/s"
        -3 * data * x + 2z <= no_speed
    end)
    @objective(model, Max, x + 0.2 * z)
    optimize!(model)
    @test value(x) == 0.3u"m/s"
    @test value(y) == 0.0u"s"
    @test value(z) ≈ 0.00045u"km/s"
    @test value(x + 0.2 * z) ≈ 0.39u"m/s"
    return
end

test_simple_example()


function bott(eigvec::AbstractMatrix{<:Number}, tb::TBModel,
              outputRange::AbstractRange=Base.OneTo(size(eigvec,2)))

    # computes exp(iθx) & exp(iθy) with θx ≡ x/2π & θy ≡ y/2π and [x y] = tb.pos[:,1:2],
    # and then block-repeats it for cases with non-unity basis sizes (assumed split over a
    # similar block structure). This implicitly assumes x and y to be ∈[0,1]
    expx = repeat(cis.(tb.pos[:,1].*(2π)), tb.basisdim)
    expy = repeat(cis.(tb.pos[:,2].*(2π)), tb.basisdim)

    if outputRange != 1:size(eigvec, 2)
        eigvec = @view eigvec[:,outputRange]
    end

    X = eigvec'*(expx.*eigvec)
    Y = eigvec'*(expy.*eigvec)

    β = zeros(Float64, length(outputRange))
    @showprogress 2 "Computing Bott indices ..." for idx in eachindex(outputRange)
        # Not worth it to do views here, I think; BLAS is more important than GC...
        X′ = X[Base.OneTo(idx), Base.OneTo(idx)]
        Y′ = Y[Base.OneTo(idx), Base.OneTo(idx)]
        β[idx] = imag(sum(log.(eigvals(Y′*X′*Y′'*X′'))))/(2*pi)
    end

    return β
end


module CliffordAlgebras

export CliffordAlgebra
export order, signature, dimension, character
export basesymbol, show, show_multivector
export cayleytable, signaturetable

export MultiVector
export zero, iszero, one, isone
export ==, algebra, eltype
export baseindices, coefficients
export scalar, prune, extend
export grade, isgrade, reverse, ~
export conj, grin, dual, even, odd
export basevector, pseudoscalar
export vector, matrix

export +, -, *, /, \
export ∧, ∨, ⋅, ⨼, ⨽, ⋆
export ×₊, ×₋, ≀
export inv, adjoint
export polarize, norm, norm_sqr
export Λᵏ, exp, outermorphism

include("utils.jl")
include("algebra.jl")
include("multivector.jl")
include("arithmetic.jl")

end


relu6(x) = min(max(zero(x),x), eltype(x)(6))
using Flux, CuArrays
m = Dense(10,5) |> gpu
x = rand(10) |> gpu

@show m(x)

m = Chain(
  Dense(28^2, 64),
  BatchNorm(64)) |>gpu

x = rand(28^2,3) |> gpu
@show typeof(x)
@show typeof(m.layers[1].W.data)
@show typeof(BatchNorm.σ)
@show m(x)


include("../src/Model.jl")
using Plots

using ModelingToolkit
using Calculus
using ForwardDiff
using OrdinaryDiffEq
using DifferentialEquations
##

## Use Functor

n = 10000
p1 = LinRange(0.001, 10, n)
out = zeros(n)

@time for (c, i) in enumerate(p1)
    R= Model.positive_feedback(s=i)
    prob = SteadyStateProblem(R, 0.0)

    #prob = ODEProblem(R, R.x, (0.0,100.0))
    local sol = solve(prob, DynamicSS(Rosenbrock23()))
    out[c] = sol.u
end

plot(p1, out)
plot!(xaxis=:log)

##

@parameters t s kK kA m x_50 x_tot kp
@variables x(t)
@derivatives D'~t

s = Model.positive_feedback(x,s, kK, kA, m, x_50, x_tot, kp)

dx = s(x, nothing, nothing)


@ms using LinearAlgebra

"""
Instance data structure definition
"""
mutable struct Instance

    name::String # name of the instance

    n::Int # number of airdromes
    I::Int # departure airdrome
    F::Int # arrival airdrome
    k::Int # minimum number of airdromes to be visited
    m::Int # total number of groups
    R::Array{Int} # association airdrome-group
    coordinates::Array{Tuple{Int, Int}} # geographical coordinates of the airdromes
    D::Dict{Tuple{Int, Int}, Int} # rounded distance between the airdromes
    Δ::Int # maximum distance that can be traveled without landing to refuel
    feasible_arcs::Set{Tuple{Int, Int}} # feasible arcs regarding the threshold distance Δ

    """
    Instance object builder
    """
    function Instance()

        this = new()

        # initialization of the containers which will be filled dynamically
        this.R = Int[]
        this.coordinates = Tuple{Int, Int}[]
        this.D = Dict{Tuple{Int, Int}, Int}()
        this.feasible_arcs = Set{Tuple{Int, Int}}()

        # read data from the input file
        read_input_file!(this, args["input"])

        # compute integer distances using from coordinates and update the feasible arcs set
        compute_arc_weights!(this)

        return this

    end

end

"""
Input file reader
"""
function read_input_file!(inst::Instance, path::String)

    # the name of the instance is the name of the file
    base, ext = splitext(basename(path))
    inst.name = base

    # collect lines
    lines = readlines(path)

    # read data

    inst.n = parse(Int, lines[1])
    inst.I = parse(Int, lines[2])
    inst.F = parse(Int, lines[3])
    inst.k = parse(Int, lines[4])
    inst.m = parse(Int, lines[5])

    for r in split(lines[7])
        push!(inst.R, parse(Int, r))
    end

    inst.Δ = parse(Int, lines[9])

    for i in 11:10+inst.n
        x_str, y_str = split(lines[i])
        push!(inst.coordinates, (parse(Int, x_str), parse(Int, y_str)))
    end

end

"""
Computes the weight of each arc and concludes on their feasibility
"""
function compute_arc_weights!(inst::Instance)

    for i in 1:inst.n
        for j in i+1:inst.n # we use the symmetry of the distance

            # compute distance using LinearAlgebra
            inst.D[(i, j)] = round(Int, norm(inst.coordinates[i].-inst.coordinates[j]))
            inst.D[(j, i)] = inst.D[(i, j)]

            # conclude on the feasibility of the arc
            if inst.D[(i, j)] <= inst.Δ
                push!(inst.feasible_arcs, (i,j), (j,i))
            end

        end
    end

end


# This file contains the line dislocation from Paul Segall's book

export disp_antiplane_seg2, stress_antiplane_seg2
export disp_inplane_seg2, stress_inplane_seg2

function disp_antiplane_seg2(x1::T, x2::T, y1::T, d1::T, d2::T, s::T=one(T)) where T
    @assert abs(d1) > abs(d2) "d₁ denotes a deeper dislocation than d₂"
    -s / 2π * (
        atan((x1 - y1) / (x2 + d1)) - atan((x1 - y1) / (x2 - d1)) -
        atan((x1 - y1) / (x2 + d2)) + atan((x1 - y1) / (x2 - d2))
    )
end

function stress_antiplane_seg2(x1::T, x2::T, y1::T, d1::T, d2::T, G::T, s::T=one(T)) where T
    s13 = -s / 2π * G * (
        (d1 + x2)/((d1 + x2)^2 + (x1 - y1)^2) - (-(d1 - x2))/((d1 - x2)^2 + (x1 - y1)^2) -
        (d2 + x2)/((d2 + x2)^2 + (x1 - y1)^2) + (-(d2 - x2))/((d2 - x2)^2 + (x1 - y1)^2)
    )
    s23 = -s / 2π * G * (
        (-(x1 - y1))/((d1 + x2)^2 + (x1 - y1)^2) - (-(x1 - y1))/((d1 - x2)^2 + (x1 - y1)^2) -
        (-(x1 - y1))/((d2 + x2)^2 + (x1 - y1)^2) + (-(x1 - y1))/((d2 - x2)^2 + (x1 - y1)^2)
    )
    return s13, s23
end

function _disp_half_infinite_line_inplane(x1::T, x2::T, ξ1::T, ξ2::T, s1::T, s2::T, ν::T) where T
    θ1 = atan((x1 - ξ1) / (x2 - ξ2))
    θ2 = atan((x1 - ξ1) / (x2 + ξ2))
    r₁ = hypot(x1 - ξ1, x2 - ξ2)
    r₂ = hypot(x1 - ξ1, x2 + ξ2)
    r₁² = r₁ ^ 2
    r₂² = r₂ ^ 2
    r₁⁴ = r₁² ^ 2
    r₂⁴ = r₂² ^ 2

    u1 = (
        -s1/π/(1-ν) * ( (1-ν)/2*(θ2-θ1) + (x1-ξ1)*(x2-ξ2)/4r₁² - (x1-ξ1)*(x2+(3-4ν)*ξ2)/4r₂² + ξ2*x2*(x1-ξ1)*(x2+ξ2)/r₂⁴ ) +
        s2/π/(1-ν) * ( (1-2ν)/4*log(r₂/r₁) - (x2-ξ2)^2/4r₁² + (x2^2+ξ2^2-4(1-ν)*ξ2*(x2+ξ2))/4r₂² + x2*ξ2*(x2+ξ2)^2/r₂⁴ )
    )
    u2 = (
        -s1/π/(1-ν) * ( (1-2ν)/4*log(r₂/r₁) + (x2-ξ2)^2/4r₁² - ((x2+ξ2)^2-2*ξ2^2-2(1-2ν)*ξ2*(x2+ξ2))/4r₂² + x2*ξ2*(x2+ξ2)^2/r₂⁴ ) +
        s2/π/(1-ν) * ( (1-ν)/2*(θ1-θ2) + (x1-ξ1)*(x2-ξ2)/4r₁² - (x1-ξ1)*(x2+(3-4ν)*ξ2)/4r₂² - ξ2*x2*(x1-ξ1)*(x2+ξ2)/r₂⁴ )
    )
    return u1, u2
end

function disp_inplane_seg2(x1::T, x2::T, ξ1::T, ξ2::T, ζ1::T, ζ2::T, s1::T, s2::T, ν::T) where T
    @assert abs(ξ2) > abs(ζ2) "ξ₂ denotes a deeper dislocation than ζ₂"
    u1ξ, u2ξ = _disp_half_infinite_line_inplane(x1, x2, ξ1, ξ2, -s1, -s2, ν)
    u1ζ, u2ζ = _disp_half_infinite_line_inplane(x1, x2, ζ1, ζ2, s1, s2, ν)
    return u1ξ+u1ζ, u2ξ+u2ζ
end

function _stress_half_infinite_line_inplane(x1::T, x2::T, ξ1::T, ξ2::T, s1::T, s2::T, μ::T, ν::T) where T
    r₁ = hypot(x1 - ξ1, x2 - ξ2)
    r₂ = hypot(x1 - ξ1, x2 + ξ2)
    r₁² = r₁ ^ 2
    r₂² = r₂ ^ 2
    r₁⁴ = r₁² ^ 2
    r₂⁴ = r₂² ^ 2
    r₁⁶ = r₁² * r₁⁴
    r₂⁶ = r₂² * r₂⁴

    σ11 = (
        μ*s2/2π/(1-ν) * ( (x1-ξ1)*((x2-ξ2)^2-(x1-ξ1)^2)/r₁⁴ - (x1-ξ1)*((x2+ξ2)^2-(x1-ξ1)^2)/r₂⁴ + 4ξ2*(x1-ξ1)/r₂⁶*((2ξ2-x2)*(x2+ξ2)^2+(3x2+2ξ2)*(x1-ξ1)^2) ) +
        μ*s1/2π/(1-ν) * ( (x2-ξ2)*((x2-ξ2)^2+3(x1-ξ1)^2)/r₁⁴ - (x2+ξ2)*((x2+ξ2)^2+3(x1-ξ1)^2)/r₂⁴ + 2ξ2/r₂⁶*(6x2*(x2+ξ2)*(x1-ξ1)^2-(x2-ξ2)*(x2+ξ2)^3-(x1-ξ1)^4) )
    )

    σ22 = (
        -μ*s2/2π/(1-ν) * ( (x1-ξ1)*(3(x2-ξ2)^2+(x1-ξ1)^2)/r₁⁴ - (x1-ξ1)*(3(x2+ξ2)^2+(x1-ξ1)^2)/r₂⁴ - 4ξ2*x2*(x1-ξ1)/r₂⁶*(3(x2+ξ2)^2-(x1-ξ1)^2) ) +
        μ*s1/2π/(1-ν) * ( (x2-ξ2)*((x2-ξ2)^2-(x1-ξ1)^2)/r₁⁴ - (x2+ξ2)*((x2+ξ2)^2-(x1-ξ1)^2)/r₂⁴ - 2ξ2/r₂⁶*(6x2*(x2+ξ2)*(x1-ξ1)^2-(3x2+ξ2)*(x2+ξ2)^3+(x1-ξ1)^4)  )
    )

    σ12 = (
        μ*s2/2π/(1-ν) * ( (x2-ξ2)*((x2-ξ2)^2-(x1-ξ1)^2)/r₁⁴ - (x2+ξ2)*((x2+ξ2)^2-(x1-ξ1)^2)/r₂⁴ + 2ξ2/r₂⁶*(6x2*(x2+ξ2)*(x1-ξ1)^2-(x1-ξ1)^4+(ξ2-x2)*(x2+ξ2)^3) ) +
        μ*s1/2π/(1-ν) * ( (x1-ξ1)*((x2-ξ2)^2-(x1-ξ1)^2)/r₁⁴ - (x1-ξ1)*((x2+ξ2)^2-(x1-ξ1)^2)/r₂⁴ + 4ξ2*x2*(x1-ξ1)/r₂⁶*(3(x2+ξ2)^2-(x1-ξ1)^2) )
    )

    return σ11, σ22, σ12
end

function stress_inplane_seg2(x1::T, x2::T, ξ1::T, ξ2::T, ζ1::T, ζ2::T, s1::T, s2::T, G::T, ν::T) where T
    @assert abs(ξ2) > abs(ζ2) "ξ₂ denotes a deeper dislocation than ζ₂"
    σ11ξ, σ22ξ, σ12ξ = _stress_half_infinite_line_inplane(x1, x2, ξ1, ξ2, -s1, -s2, G, ν)
    σ11ζ, σ22ζ, σ12ζ = _stress_half_infinite_line_inplane(x1, x2, ζ1, ζ2, s1, s2, G, ν)
    return σ11ξ+σ11ζ, σ22ξ+σ22ζ, σ12ξ+σ12ζ
end


using LESbrary.Utils

@testset "fit_cubic" begin
    c = fit_cubic((0, 0), (1, 1), 0, 0)

    # We should get back 3x^2 - 2x^3
    @test c[1] ≈ 0 atol=1e-12
    @test c[2] ≈ 0 atol=1e-12
    @test c[3] ≈ 3
    @test c[4] ≈ -2
end


function  magnetic_field(B,x)

# evaluates the radial magnetic field as a function of x = cos(theta);
#
# input
# B   - amplitude of magnetic field (dimensionless)
# x   - cos(theta)
#
# output
# br     - radial field as a function of x

#  size of x grid
   nx = length(x);
   br = zeros(nx);

#  constant radial field
#   br[1:nx] .= B;

#  dipole radial field
#  br = B * x;
#
#  increase rms field toward pole
   Gamma = 2.5;
   B02 = B^2 / (1.0  + Gamma/3.0);
   br2 = B02 *(Gamma*x.*x .+ 1);
   br = sqrt.(br2);

   return br

end


using FastTransforms, Compat
using Compat.Test

if VERSION < v"0.7-"
    @testset "Test vector field transforms" begin
        # f = (θ,φ) -> cospi(θ) + sinpi(θ)*(1+cospi(2θ))*sinpi(φ) + sinpi(θ)^5*(cospi(5φ)-sinpi(5φ))
        ∇θf = (θ,φ) -> -sinpi(θ) + (cospi(θ)*(1+cospi(2θ)) - 2*sinpi(θ)*sinpi(2θ))*sinpi(φ) + 5*sinpi(θ)^4*cospi(θ)*(cospi(5φ)-sinpi(5φ))
        ∇φf = (θ,φ) -> (1+cospi(2θ))*cospi(φ) - 5*sinpi(θ)^4*(sinpi(5φ)+cospi(5φ))

        n = 6
        θ = (0.5:n-0.5)/n
        φ = (0:2n-2)*2/(2n-1)
        ∇θF = [∇θf(θ,φ) for θ in θ, φ in φ]
        ∇φF = [∇φf(θ,φ) for θ in θ, φ in φ]
        V1 = zero(∇θF)
        V2 = zero(∇φF)
        Pa = FastTransforms.plan_analysis(∇θF)
        mul!(V1, V2, Pa, ∇θF, ∇φF)
        P = SlowSphericalHarmonicPlan(V1)

        U1 = zero(V1)
        U2 = zero(V2)
        At_mul_B!(U1, U2, P, V1, V2)

        W1 = zero(U1)
        W2 = zero(U2)

        mul!(W1, W2, P, U1, U2)

        Ps = FastTransforms.plan_synthesis(W1)

        G1 = zero(∇θF)
        G2 = zero(∇φF)

        mul!(G1, G2, Ps, W1, W2)

        @test vecnorm(∇θF - G1)/vecnorm(∇θF) < n*eps()
        @test vecnorm(∇φF - G2)/vecnorm(∇φF) < n*eps()

        y = (1.0, 2.0, 3.0)
        for k in (10, 20, 40)
            ∇θf = (θ,φ) -> -2k*sin(k*((sinpi(θ)*cospi(φ) - y[1])^2 + (sinpi(θ)*sinpi(φ) - y[2])^2 + (cospi(θ) - y[3])^2))*( (sinpi(θ)*cospi(φ) - y[1])*(cospi(θ)*cospi(φ)) + (sinpi(θ)*sinpi(φ) - y[2])*(cospi(θ)*sinpi(φ)) - (cospi(θ) - y[3])*sinpi(θ) )
            ∇φf = (θ,φ) -> -2k*sin(k*((sinpi(θ)*cospi(φ) - y[1])^2 + (sinpi(θ)*sinpi(φ) - y[2])^2 + (cospi(θ) - y[3])^2))*( (sinpi(θ)*cospi(φ) - y[1])*(-sinpi(φ)) + (sinpi(θ)*sinpi(φ) - y[2])*(cospi(φ)) )
            n = 12k

            θ = (0.5:n-0.5)/n
            φ = (0:2n-2)*2/(2n-1)
            ∇θF = [∇θf(θ,φ) for θ in θ, φ in φ]
            ∇φF = [∇φf(θ,φ) for θ in θ, φ in φ]
            V1 = zero(∇θF)
            V2 = zero(∇φF)
            Pa = FastTransforms.plan_analysis(∇θF)
            mul!(V1, V2, Pa, ∇θF, ∇φF)
            P = SlowSphericalHarmonicPlan(V1)

            U1 = zero(V1)
            U2 = zero(V2)
            At_mul_B!(U1, U2, P, V1, V2)

            W1 = zero(U1)
            W2 = zero(U2)

            mul!(W1, W2, P, U1, U2)

            Ps = FastTransforms.plan_synthesis(W1)

            G1 = zero(∇θF)
            G2 = zero(∇φF)

            mul!(G1, G2, Ps, W1, W2)

            @test vecnorm(∇θF - G1)/vecnorm(∇θF) < n*eps()
            @test vecnorm(∇φF - G2)/vecnorm(∇φF) < n*eps()
        end
    end
else
    @testset "Test vector field transforms" begin
        # f = (θ,φ) -> cospi(θ) + sinpi(θ)*(1+cospi(2θ))*sinpi(φ) + sinpi(θ)^5*(cospi(5φ)-sinpi(5φ))
        n = 6
        θ = (0.5:n-0.5)/n
        φ = (0:2n-2)*2/(2n-1)
        ∇θf = (θ,φ) -> -sinpi(θ) + (cospi(θ)*(1+cospi(2θ)) - 2*sinpi(θ)*sinpi(2θ))*sinpi(φ) + 5*sinpi(θ)^4*cospi(θ)*(cospi(5φ)-sinpi(5φ))
        ∇φf = (θ,φ) -> (1+cospi(2θ))*cospi(φ) - 5*sinpi(θ)^4*(sinpi(5φ)+cospi(5φ))

        ∇θF = [∇θf(θ,φ) for θ in θ, φ in φ]
        ∇φF = [∇φf(θ,φ) for θ in θ, φ in φ]
        V1 = zero(∇θF)
        V2 = zero(∇φF)
        Pa = FastTransforms.plan_analysis(∇θF)
        mul!(V1, V2, Pa, ∇θF, ∇φF)
        P = SlowSphericalHarmonicPlan(V1)

        U1 = zero(V1)
        U2 = zero(V2)
        mul!(U1, U2, transpose(P), V1, V2)

        W1 = zero(U1)
        W2 = zero(U2)

        mul!(W1, W2, P, U1, U2)

        Ps = FastTransforms.plan_synthesis(W1)

        G1 = zero(∇θF)
        G2 = zero(∇φF)

        mul!(G1, G2, Ps, W1, W2)

        @test norm(∇θF - G1)/norm(∇θF) < n*eps()
        @test norm(∇φF - G2)/norm(∇φF) < n*eps()

        for k in (10, 20, 40)
            y = (1.0, 2.0, 3.0)
            n = 12k
            θ = (0.5:n-0.5)/n
            φ = (0:2n-2)*2/(2n-1)
            ∇θf = (θ,φ) -> -2k*sin(k*((sinpi(θ)*cospi(φ) - y[1])^2 + (sinpi(θ)*sinpi(φ) - y[2])^2 + (cospi(θ) - y[3])^2))*( (sinpi(θ)*cospi(φ) - y[1])*(cospi(θ)*cospi(φ)) + (sinpi(θ)*sinpi(φ) - y[2])*(cospi(θ)*sinpi(φ)) - (cospi(θ) - y[3])*sinpi(θ) )
            ∇φf = (θ,φ) -> -2k*sin(k*((sinpi(θ)*cospi(φ) - y[1])^2 + (sinpi(θ)*sinpi(φ) - y[2])^2 + (cospi(θ) - y[3])^2))*( (sinpi(θ)*cospi(φ) - y[1])*(-sinpi(φ)) + (sinpi(θ)*sinpi(φ) - y[2])*(cospi(φ)) )

            ∇θF = [∇θf(θ,φ) for θ in θ, φ in φ]
            ∇φF = [∇φf(θ,φ) for θ in θ, φ in φ]
            V1 = zero(∇θF)
            V2 = zero(∇φF)
            Pa = FastTransforms.plan_analysis(∇θF)
            mul!(V1, V2, Pa, ∇θF, ∇φF)
            P = SlowSphericalHarmonicPlan(V1)

            U1 = zero(V1)
            U2 = zero(V2)
            FastTransforms.mul!(U1, U2, transpose(P), V1, V2)

            W1 = zero(U1)
            W2 = zero(U2)

            mul!(W1, W2, P, U1, U2)

            Ps = FastTransforms.plan_synthesis(W1)

            G1 = zero(∇θF)
            G2 = zero(∇φF)

            mul!(G1, G2, Ps, W1, W2)

            @test norm(∇θF - G1)/norm(∇θF) < n*eps()
            @test norm(∇φF - G2)/norm(∇φF) < n*eps()
        end
    end
end


using LinearAlgebra
using Plots

temps = [ 71, 71, 68, 69, 68, 69, 68, 74, 77, 82, 85, 86,
 88, 86, 85, 86, 84, 79, 77, 75, 73, 71, 70, 70, 69, 69, 69,
 69, 67, 68, 68, 73, 76, 77, 82, 84, 84, 81, 80, 78, 79, 78,
 73, 72, 70, 70, 68, 67 ];

plot(temps, marker = :circle, legend = false, grid = false)
#savefig(temps,"temperature.pdf")


@symbol_func function K_W(cur_reactor::AbstractReactor)
  cur_kappa = kappa_P(cur_reactor)

  cur_epsilon = epsilon_P(cur_reactor)

  cur_denominator = cur_kappa ^ 2 - 1.0

  cur_denominator *= ( 2 / cur_reactor.pi )

  cur_denominator += 1

  cur_K = K_F(cur_reactor)

  cur_K /= 5.0

  cur_K /= cur_reactor.pi ^ 2

  cur_K *= cur_kappa

  cur_K /= cur_epsilon

  cur_K /= cur_denominator

  cur_K
end


function getTableauSPARKGLRK(s)
    g = getCoefficientsGLRK(s)

    ω = zeros(s, s+1)
    δ = zeros(0, s)

    for i in 1:s-1
        for j in 1:s
            ω[i,j] = g.b[j] * g.c[j]^(i-1)
        end
    end

    ω[s,s+1] = 1

    return TableauSPARK(Symbol("sparkglrk", s), g.o,
                        g.a, g.a, g.a, g.a,
                        g.a, g.a, g.a, g.a,
                        g.b, g.b, g.b, g.b,
                        g.c, g.c, g.c, g.c,
                        ω, δ)
end


include("../src/HMat2d.jl");

function laplacian2d(DA,DV,nx,ny,h)
    M = speye(nx*ny);
    for x=1:nx
        for y=1:ny
            s = x+(y-1)*nx;
            M[s,s] = DV[x,y];
            if x > 1
                M[s,s-1] = -(DA[x,y+1]+DA[x+1,y+1])/2/h^2;
            end
            M[s,s] += (DA[x,y+1]+DA[x+1,y+1])/2/h^2;
            if x < nx
                M[s,s+1] = -(DA[x+2,y+1]+DA[x+1,y+1])/2/h^2;
            end
            M[s,s] += (DA[x+2,y+1]+DA[x+1,y+1])/2/h^2;
            if y > 1
                M[s,s-nx] = -(DA[x+1,y]+DA[x+1,y+1])/2/h^2;
            end
            M[s,s] += (DA[x+1,y]+DA[x+1,y+1])/2/h^2;
            if y < ny
                M[s,s+nx] = -(DA[x+1,y+2]+DA[x+1,y+1])/2/h^2;
            end
            M[s,s] += (DA[x+1,y+2]+DA[x+1,y+1])/2/h^2;
        end
    end
    return M;
end

EPS = 1e-6;
MaxRank = 4;
minn = 4;

for iter = 1:10

    n=32;
    h=1/n;
    DA = abs(randn(n+2,n+2))+0.01;
    DV = zeros(n,n);
    A = full(laplacian2d(DA,DV,n,n,h));
    Z2C = Z2Cmapper([n,n],minn,reshape(1:n^2,n,n));
    A = inv(full(A[Z2C,Z2C]));
    AHS = HMat2dd2h(A, [n,n], [n,n], STANDARD, [0,0], [0,0], 1, EPS, MaxRank, minn);
    AHSinv = hcopy(AHS);
    hinv!(AHSinv);
    ASinv = hh2d(AHSinv);
    errS = ASinv*A-eye(n^2);
    @printf("Direct inversion of Standard HMat error: %.2e\n",norm(errS));

    X0 = hidentity(AHS);
    hscale!(1/norm(A),X0);
    AHNinv = hNewton(AHS,X0,18);
    ANinv = hh2d(AHNinv);
    errN = ANinv*A-eye(n^2);
    @printf("Newton inversion of Standard HMat error1: %.2e\n\n",norm(errN));

    AHW = HMat2dd2h(A, [n,n], [n,n], WEAK, [0,0], [0,0], 1, EPS, MaxRank, minn);
    AHWinv = hcopy(AHW);
    hinv!(AHWinv);
    Dtmp = hh2d(AHWinv);
    X0 = HMat2dd2h(Dtmp, [n,n], [n,n], STANDARD, [0,0], [0,0], 1, EPS, MaxRank, minn);
    AHNinv = hNewton(AHS,X0,10);
    ANinv = hh2d(AHNinv);
    errN = ANinv*A-eye(n^2);
    @printf("Newton inversion of Standard HMat error2: %.2e\n\n",norm(errN));

end


mutable struct AccumulatedIntegral{Tt,Ft,Func,Wt}
    t::Tt
    ∫f::Ft
    f::Func
    c::Vector{Tt} # Quadrature roots
    w::Vector{Wt} # Quadrature weights

    function AccumulatedIntegral(f::Func, ::Type{Ft}, ::Type{Tt}=Ft;
                                 k = 3,
                                 t::Tt=zero(Tt), init=zero(Ft)) where {Ft,Func,Tt}
        c,w = gausslegendre(k)
        new{Tt,Ft,Func,eltype(w)}(t, init, f, c, w)
    end
end

function (F::AccumulatedIntegral)(a,b)
    μ = (a+b)/2
    δ = (b-a)/2
    v = zero(F.∫f)
    for (c,w) in zip(F.c,F.w)
        v += w*F.f(δ*c + μ)
    end
    δ*v
end

(F::AccumulatedIntegral)(t) = F.∫f + F(F.t, t)

function set!(F::AccumulatedIntegral, t)
    F.∫f = F(t)
    F.t = t
end

function field(F, ndt; kwargs...)
    t = timeaxis(F, ndt)
    tau = atomic_units.(t)

    Ev = atomic_units.(F.(t))
    A = AccumulatedIntegral(t -> -atomic_units(F(24.2u"as"*t)), Float64;
                            t=first(tau), kwargs...)
    nt = length(t)
    Av = zeros(Float64, nt)
    for i = 1:nt
        Av[i] = A(tau[i])
        set!(A, tau[i])
    end

    tau,Ev,Av
end


function toy_rnn()
    batch_size  = 4
    input_size  = 1
    output_size = 1
    hidden_size = 10

    function toy_data(n)
        x = rand(Float32,  input_size,  n*batch_size)
        y = zeros(Float32, output_size, n*batch_size)
        for i in 1:n
            for t in 1:batch_size
                y[i*batch_size] += x[1,(i-1)*batch_size+t]
            end
        end
        (x, y)
    end

    function toy_net(xtrn, ytrn, xtst, ytst, iter)
        net = NNN.Network(batch_size)
        NNN.push!(net,
                  NNN.SimpleRecurrentLayer(batch_size, input_size, hidden_size;
                                           activation=NNN.LeakyRelu()),
                  NNN.SGD(;ε=0.05),
                  NNN.SGD(;ε=0.05))
        NNN.push!(net,
                  NNN.OutputLayer(batch_size, hidden_size, output_size;
                                  activation=NNN.IdentityAct(),
                                  loss=NNN.rnn_quadratic_cost),
                  NNN.SGD(;ε=0.05))

        NNN.train(xtrn, ytrn, net; iter=iter, batch_size=batch_size)
        errors = NNN.test(xtst, ytst, net; batch_size=batch_size)

        (net, errors)
    end

    xtrn, ytrn  = toy_data(1000);
    xtst, ytst  = toy_data(100);
    net, errors = toy_net(xtrn, ytrn, xtst, ytst, 5)
    (net, errors, xtst, ytst)
end

net, errors, xtst, ytst = toy_rnn()

facts("Simple RNN") do
    @fact sum(abs(NNN.forward!(net, xtst[:,1:4])[4] - ytst[:,4])) => less_than(1.0)
end


include("euler/euler.jl")
using .Game: dice_combinations
using .Math: fast_max

function compute(a::Tuple{Int, Int}, b::Tuple{Int, Int})::Float64
    limit = fast_max(a[1] * a[2], b[1] * b[2])
    looses = zeros(Int, limit)
    result = 0
    for i ∈ b[1]:b[1] * b[2] looses[i] = dice_combinations(i, b[1], b[2]) + looses[i - 1] end
    for i ∈ a[1]:a[1] * a[2] result += dice_combinations(i, a[1], a[2]) * looses[i - 1] end
    return round(result / a[2] ^ a[1] / b[2] ^ b[1], digits=7)
end


include("utils.jl")
using Distances

Random.seed!(42)

@testset "Distances" begin
    M = SymmetricPositiveDefinite(3)
    A(α) = [1.0 0.0 0.0; 0.0 cos(α) sin(α); 0.0 -sin(α) cos(α)]
    ptsF = [#
        [1.0 0.0 0.0; 0.0 1.0 0.0; 0.0 0.0 1],
        [2.0 0.0 0.0; 0.0 2.0 0.0; 0.0 0.0 1],
        A(π / 6) * [1.0 0.0 0.0; 0.0 2.0 0.0; 0.0 0.0 1] * transpose(A(π / 6)),
    ]
    dist = ManifoldML.RiemannianDistance(M)
    @test evaluate(dist, reshape(ptsF[1], 9), reshape(ptsF[3], 9)) ≈ distance(M, ptsF[1], ptsF[3])

    point_matrix = reduce(hcat, map(a -> reshape(a, 9), ptsF))
    dists = pairwise(dist, point_matrix)
    for i in 1:3, j in 1:3
        @test dists[i, j] ≈ distance(M, ptsF[i], ptsF[j]) atol = 1e-15
    end
end


### LLL method

### f = prod(fs) over Zp, find factors over Z
function mu(i,j,B,Bs)
    dot(B[i,:], Bs[j,:]) // dot(Bs[j,:], Bs[j,:] )
end
function swaprows!(A, i, j, tmp)
    tmp[:] = A[i,:]
    A[i,:] = A[j,:]
    A[j,:] = tmp
    nothing
end

function _reduce(i, T, B, U)
    j = i - 1
    while j > 0
        rij = round(T, digits=U[i,j])
        B[i,:] -= rij * B[j, :]
        U[i,:] -= rij * U[j, :]
        j -= 1
    end
end

# Find a short vector for B = [f1;f2;...;fn]
# short vector if f1 after the work is done
# http://algo.epfl.ch/_media/en/projects/bachelor_semester/rapportetiennehelfer.pdf .
# Algo 2
function LLL_basis_reduction!(B::Matrix{T}, c=4//3) where {T <: Integer} # c > 4/3

    m, n = size(B)

    Bstar =  Matrix{Rational{T}}(I, m, m)
    U = Matrix{Rational{T}}(I, m, m)

    tmpZ = zeros(T, 1, m)
    tmpQ = zeros(Rational{T}, 1, m)

    ONE, ZERO = one(Rational{T}), zero(Rational{T})

    ## initialize Bstar, U
    Bstar[1,:] = B[1,:]  # b^*_1 = b_1
    for i in 2:n
        Bstar[i,:] = B[i,:]
        for j in 1:(i-1)
            U[i,j] = mu(i,j,B,Bstar) #vecdot(bi, bstar_j) // vecdot(bstar_j, bstar_j)
            Bstar[i,:] -= U[i,j] * Bstar[j,:]
        end

        _reduce(i, T, B, U)
    end

    i = 1
    while i < m
        if norm(B[i,:],2) < c * norm(B[i+1,:],2)
            i += 1
        else
            # Modify Q and R in order to keep the relation B = QR after the swapping
            Bstar[i+1, :] += U[i+1,i] * Bstar[i, :]

            U[i, i] = mu(i, i+1, B, Bstar)
            U[i, i+1] = U[i+1, i] = ONE
            U[i+1, i+1] = ZERO

            Bstar[i, :] -= U[i,i] * Bstar[i+1, :]

            swaprows!(U, i, i+1, tmpQ)
            swaprows!(Bstar, i, i+1, tmpQ)
            swaprows!(B, i, i+1, tmpZ)

            for k in (i+2):m
                U[k,i] = mu(k, i, B, Bstar)
                U[k,i+1] = mu(k, i+1, B, Bstar)
            end

            if abs(U[i+1,i]) > 1//2
                _reduce(i+1, T, B, U)
            end
            i = max(i-1, 1)
        end
    end
end

## For a polynomial p, create a lattice of p, xp, x^2p, ... then find a short vector.
## This will be a potential factor of f
function short_vector(u::AbstractAlgebra.Generic.Poly, m, j, d)
    us = poly_coeffs(u)
    T = eltype(us)
    F = zeros(T, j, j)

    # coefficients of us*x^i
    for i in 0:(j-d-1)
        F[1 + i, (1 + i):(1+i+d)] = us
    end
    # coefficients of mx^i
    for i in 0:(d-1)
        F[1 + j - d + i, 1 + i] = m
    end

    LLL_basis_reduction!(F)
    x = variable(u)
    # short vector is first row
    as_poly(vec(F[1,:]), x)
end

## u is a factor over Zp
## we identify irreducible gstar of f
## return gstar, fstar = f / gstar (basically), and reduced Ts
function identify_factor(u::AbstractAlgebra.Generic.Poly, fstar::AbstractAlgebra.Generic.Poly, Ts::Vector, p::T, l, b, B) where {T}

    d, nstar = degree(u), degree(fstar)
    d < nstar || throw(DomainError())
    m = p^l
    j = d + 1
    gstar = u
    Tsp = as_poly_Zp.(Ts, p)

    while j <= nstar+1
        gstar = short_vector(u,m,j,d)

        gstarp = as_poly_Zp(gstar, p)
        inds = Int[]
        for (i,t) in enumerate(Tsp)
            divides(gstarp, Tsp[i])[1] && push!(inds, i)  # allocates
        end

        notinds = setdiff(1:length(Ts), inds)
        Ssp = Tsp[notinds]

        # find hstar = prod(Ts)
        hstar = as_poly(mod(b, m) * (isempty(Ssp) ? one(gstarp) : prod(Ssp)))

        pd = onenorm(primpart(gstar)) * onenorm(primpart(hstar))

        if pd < B
            gstar, hstar = primpart(gstar), primpart(hstar)
            Ts = Ts[notinds] # modify Ts (Ts[:])or make a copy?
            fstar = primpart(hstar)
            break
        end
        j = j + 1
    end

    primpart(gstar), fstar, Ts
end

"""
Use LLL basis reduction to identify factors of square free f from its factors over Zp
"""
function identify_factors_lll(f, facs, p, l, b, B)

    Ts = sort(facs, by=degree, rev=true)

    Gs = similar(Ts, 0)
    fstar = primpart(f)
    m = p^l

    while length(Ts) > 0
        u = popfirst!(Ts)
        if length(Ts) == 0
            push!(Gs, fstar)
            break
        end
        if degree(fstar) == degree(u)
             push!(Gs, fstar)
            break
        end
        #        return (u, fstar, Ts, p, l, b, B)
        gstar, fstar, Ts = identify_factor(u, fstar, Ts, p, l, b, B)
        push!(Gs, gstar)
    end
    Gs
end


@reexport module _NumberTheory

import Base: ==
import Nemo
import Primes

using Base.Iterators: cycle
using Brainstorm: flatten, takewhile
using Brainstorm._Math: checked_add, checked_sub
using AutoHashEquals: @auto_hash_equals
using DataStructures: SortedDict
using IterTools: chain, drop, imap, take
using Lists: List, ListNode
using Nemo: eulerphi, factor, fmpz, isprime, is_prime, sigma, sqrtmod
using Pipe: @pipe
using Primes: ismersenneprime, isrieselprime, primes, primesmask
using Reexport: @reexport

include("primes.jl")
include("factor.jl")
include("mod.jl")
include("decimal.jl")
include("fibonacci.jl")
include("hailstone.jl")
include("triangle.jl")
include("pentagon.jl")
include("hexagon.jl")
include("confrac.jl")
include("diophantine.jl")

end


using ComponentArrays
using Optim

rosen(u) = (1.0 - u.x)^2 + 100.0 * (u.y - u.x^2)^2

u₀ = ComponentArray(x=0.0, y=0.0)
lb = ComponentArray(x=-0.5, y=-0.5)
ub = ComponentArray(x=0.5, y=0.5)

df = TwiceDifferentiable(rosen, u₀; autodiff=:forward)
dfc = TwiceDifferentiableConstraints(lb, ub)

val = optimize(df, dfc, u₀, IPNewton())


module _Simple

using Brainstorm._Algorithm: binarysearch

include("binarysplitting.jl")

# Compute int(e * 10^digits)
# This is done using Taylor series with binary splitting
euler(digits::Integer) = begin
    # How many terms to compute
    #
    # http://www.johndcook.com/blog/2011/06/10/stirling-approximation/
    #
    #             k! > 10^digits
    # =>      log k! > digits
    # => k log k - k > digits
    #
    # with log n! = sum_k=1^n log k
    #            >= int_x=1^n log x dx
    #             = n log n - n + 1
    #             > n log n - n
    #
    n = binarysearch(termfunc, digits + 1, one(digits), digits + 100)[2]

    # calculate P(0, N) and Q(0, N)
    p, q = binarysplitting(zero(n), n)

    o = big(10)^digits # one
    o + (o * p) ÷ q
end

termfunc(k) = k * (ndigits(k) - 2)

compute(a, b) = big(1), big(b)

combine(am, mb) = begin
    p₁, q₁ = am
    p₂, q₂ = mb

    p = p₁ * q₂ + p₂
    q = q₁ * q₂

    p, q
end

end


""" [Ackerman2009](@cite) """
Dycoms_RF02_q_tot(::Type{FT}) where {FT} =
    z -> if z <= 795.0
        FT(9.45) / FT(1000.0)
    else
        (FT(5) - FT(3) * (FT(1) - exp(-(z - FT(795)) / FT(500)))) / FT(1000)
    end

""" [Ackerman2009](@cite) """
Dycoms_RF02_θ_liq_ice(::Type{FT}) where {FT} =
    z -> if z <= 795.0
        FT(288.3)
    else
        FT(295.0) + (z - FT(795))^FT(1.0 / 3.0)
    end

""" [Ackerman2009](@cite) """
function Dycoms_RF02_u(::Type{FT}) where {FT}
    return z -> FT(3) + FT(4.3) * z / FT(1000) - FT(5)
end

""" [Ackerman2009](@cite) """
function Dycoms_RF02_v(::Type{FT}) where {FT}
    return z -> FT(-9) + FT(5.6) * z / FT(1000) - FT(-5.5)
end

""" [Ackerman2009](@cite) """
Dycoms_RF02_tke(::Type{FT}) where {FT} =
    z -> if z <= 795.0
        FT(1) - z / FT(1000)
    else
        FT(0)
    end


using PyPlot, Images
using PyCall
@pyimport numpy

function DIC_sine_transform(img_name, lambda)

    # load in RGB data, convert to greyscale array
    raw_img = PyPlot.imread(img_name);
    img = convert(Image{Images.Gray},raw_img[:,:,1:3]);
    M = convert(Array,img);
    M = float(M);

    # take gradient of image, then process
    gX, gY = numpy.gradient(M);
    d1 = FFTW.r2r(gY,FFTW.RODFT00)
    nx, ny = size(d1)
    d3=zeros(nx,ny)
    d3=[(nx/pi)^2 * d1[j,i]/(lambda*j^2 + i^2) for i in 1:nx, j in 1:ny];
    dst = convert(Array{Float64,2},d3)
    e1 = FFTW.r2r(dst,FFTW.RODFT00)
    e1 = e1 / (2*nx)^2

    # clamp image range to 0 1, output as .png
    maxi = maximum(e1)
    mini = minimum(e1)
    clamped = 1 +  (-e1 + maxi) * 1 / ( mini-maxi);
    out_name = split(image_name,".")[1] * "_sin_l=$(@sprintf("%.2f", lambda))" * ".png"
    imwrite(grayim(clamped),out_name)

    return clamped

end


# Define abstract interface
"""
$(TYPEDEF)

Abstract supertype for magnetic moments.
"""
abstract type MagneticMoments{T} end

"""
$(SIGNATURES)

Return the mean magnetic moment [Am²] as `SVector{3,T}` for a given magnetic
field H [T/μ₀] (μ₀ being the vacuum permeability), location r [m] and time t [s].
"""
function meanMagneticMoment(magneticMoment::MagneticMoments{T},H::SVector{3,T},r::SVector{3,T},t::T) where {T}
    error("meanMagneticMoment is not implemented for magneticMoment::$(typeof(magneticMoment)).")
end

# Define specific subtypes
## Langevin model
struct Langevin{T} <: MagneticMoments{T}
    msat::T
    beta::T
end

"""
$(SIGNATURES)

Initialize `Langevin` type `MagneticMoments` with diameter `d` [m], temperature
`T` [K], relative saturation magnetization [T] `Mrel` = Msat*μ₀ (μ₀ being
the vacuum permeability) and numeric `elementType`.
"""
function Langevin(;elementType=Float64,d=25e-9,T=294,Mrel=0.6)
    k = 1.380650424e-23 #Boltzman constant
    μ₀ = 4*π*1e-7 #vacuum permeability
    M = Mrel / μ₀ #saturation magnetization of magnetic material the core is made of
    msat = M * π/6*d^3 #saturation magnetic moment of a single nanoparticle
    beta = msat /(k*T) #H measured in T/μ₀

    Langevin{elementType}(msat,beta)
end

function meanMagneticMoment(magneticMoment::Langevin{T},H::SVector{3,T},r::SVector{3,T},t::T) where {T}
    if norm(H)!=0
        x = magneticMoment.beta*norm(H)
        return magneticMoment.msat*(coth(x) - 1/x)*normalize(H)
    else
        return zero(SVector{3,T})
    end
end


module MyMatching

function matching(prop_prefs,resp_prefs,reverse)

    if reverse == false

        m_prefs = prop_prefs
        f_prefs = resp_prefs
        mm = length(m_prefs)
        nn = length(f_prefs)

        for o in 1:mm
            push!(m_prefs[o],0)
        end

        for k in 1:nn
            push!(f_prefs[k],0)
        end

        matched_m = zeros(Int64,mm)
        matched_f = zeros(Int64,nn)
        accepted_m = zeros(Int64,mm)
        order = ones(Int64,mm)

        while sum(accepted_m) < mm          #男性全員がマッチするまで

            for i in 1:mm

                if accepted_m[i] == 1     #男性iがマッチ済み

                else　　　#男性iがマッチ済みでない

                    if m_prefs[i][order[i]] == 0       #男性が独身を希望

                        accepted_m[i] = 1

                    else　　　　#男性が女性を希望

                        if findfirst(f_prefs[m_prefs[i][order[i]]],i) == 0      #男性の希望する女性の選択肢の中に男性がいない

                            order[i] +=1           #男性の志望順位を一つ下げる

                        else　　　　　　#男性が女性の評価対象

                            if matched_f[m_prefs[i][order[i]]] == 0       #男性の希望する女性のもとに誰もいないとき

                                matched_m[i] = m_prefs[i][order[i]]
                                matched_f[m_prefs[i][order[i]]] = i
                                accepted_m[i] = 1

                            else　　　　　　　　　#女性が別の男性とマッチ済み

                                if findfirst(f_prefs[m_prefs[i][order[i]]],i) < findfirst(f_prefs[m_prefs[i][order[i]]],matched_f[m_prefs[i][order[i]]])

                                    order[matched_f[m_prefs[i][order[i]]]] +=1         #取って代わられた男性の志望順位を一つ下げる
                                    accepted_m[i] = 1
                                    matched_m[matched_f[m_prefs[i][order[i]]]] = 0
                                    accepted_m[matched_f[m_prefs[i][order[i]]]] = 0
                                    matched_m[i] = m_prefs[i][order[i]]
                                    matched_f[m_prefs[i][order[i]]] = i

                                else　　　　　#マッチ済みの男性のほうがいい

                                    order[i] +=1

                                end

                            end

                        end

                    end

                end

            end

        end

        return matched_m, matched_f

    elseif reverse == true

        m_prefs = prop_prefs
        f_prefs = resp_prefs
        mm = length(m_prefs)
        nn = length(f_prefs)

        for o in 1:mm
            push!(m_prefs[o],0)
        end

        for k in 1:nn
            push!(f_prefs[k],0)
        end

        matched_m = zeros(Int64,mm)
        matched_f = zeros(Int64,nn)
        accepted_f = zeros(Int64,nn)
        order = ones(Int64,nn)

        while sum(accepted_f) < nn          #女性全員がマッチするまで

            for i in 1:nn

                if accepted_f[i] == 1     #女性iがマッチ済み

                else　　　#女性iがマッチ済みでない

                    if f_prefs[i][order[i]] == 0       #女性iが独身を希望

                        accepted_f[i] = 1

                    else　　　　#女性が男性を希望

                        if findfirst(m_prefs[f_prefs[i][order[i]]],i) == 0      #男性の希望する女性の選択肢の中に男性がいない

                            order[i] +=1           #男性の志望順位を一つ下げる

                        else　　　　　　#男性が女性の評価対象

                            if matched_m[f_prefs[i][order[i]]] == 0       #男性の希望する女性のもとに誰もいないとき

                                matched_f[i] = f_prefs[i][order[i]]
                                matched_m[f_prefs[i][order[i]]] = i
                                accepted_f[i] = 1

                            else　　　　　　　　　#女性が別の男性とマッチ済み

                                if findfirst(m_prefs[f_prefs[i][order[i]]],i) < findfirst(m_prefs[f_prefs[i][order[i]]],matched_m[f_prefs[i][order[i]]])

                                    order[matched_m[f_prefs[i][order[i]]]] +=1         #取って代わられた男性の志望順位を一つ下げる
                                    accepted_f[i] = 1
                                    matched_f[matched_m[f_prefs[i][order[i]]]] = 0
                                    accepted_f[matched_m[f_prefs[i][order[i]]]] = 0
                                    matched_f[i] = f_prefs[i][order[i]]
                                    matched_m[f_prefs[i][order[i]]] = i

                                else　　　　　#マッチ済みの男性のほうがいい

                                    order[i] +=1

                                end

                            end

                        end

                    end

                end

            end

        end

        return matched_m, matched_f

    end

end

function matching(prop_prefs,resp_prefs,caps,reverse)

    if reverse == false           #学生　→　大学

        mm = length(prop_prefs)
        nn = length(resp_prefs)
        for o in 1:mm
            push!(prop_prefs[o],0)
        end

    #resp_prefsに0を入れる必要はなかった

        indptr = Array{Int}(nn+1)
        indptr[1] = 1
        for k in 1:nn
            indptr[k+1] = indptr[k] + caps[k]
        end
        #indptrを関数内に内蔵！

        prop_matched = zeros(Int64,mm)
        resp_matched = zeros(Int64,sum(caps))
        prop_accepted = zeros(Int64,mm)
        order = ones(Int64,mm)

        while sum(prop_accepted) < mm       #学生全員がマッチするまで

            for i in 1:mm

                if prop_accepted[i] == 1    #学生iがマッチ済み

                else

                    if prop_prefs[i][order[i]] == 0   #学生iの第一志望が浪人

                        prop_accepted[i] = 1

                    else

                        if findfirst(resp_prefs[prop_prefs[i][order[i]]],i) == 0  #学生iが志望する大学の選択肢の中に学生iがいない

                            order[i] +=1    #学生iの志望順位を一つ下げる

                        else

                            if findfirst(resp_matched[indptr[prop_prefs[i][order[i]]]:indptr[prop_prefs[i][order[i]]+1]-1],0) !== 0
                               #学生iが志望する大学の受け入れ枠に空きがあるとき(0が受け入れ枠にある)

                                z = findfirst(resp_matched[indptr[prop_prefs[i][order[i]]]:indptr[prop_prefs[i][order[i]]+1]-1],0)
                                #0が左から何番目にあるか

                                resp_matched[indptr[prop_prefs[i][order[i]]]+z-1] = i     #0...0にiを左詰めで入れる
                                prop_matched[i] = prop_prefs[i][order[i]]
                                prop_accepted[i] = 1

                            else

                                passed = resp_matched[indptr[prop_prefs[i][order[i]]]:indptr[prop_prefs[i][order[i]]+1]-1]
                                #受け入れ保留されている学生たちのarray

                                ranking = [findfirst(resp_prefs[prop_prefs[i][order[i]]],passed[p]) for p in 1:length(passed)]
                                #彼らのランキング

                                worst = resp_prefs[prop_prefs[i][order[i]]][maximum(ranking)]
                                #保留されている中で最もランキングの低い学生の”名前”

                                if findfirst(resp_prefs[prop_prefs[i][order[i]]],i) > maximum(ranking)
                                   #最下位の人よりも低い

                                    order[i] += 1

                                else

                                    ww = findfirst(resp_matched[indptr[prop_prefs[i][order[i]]]:indptr[prop_prefs[i][order[i]]+1]-1],worst)
                                    #最下位の人が何番目にいるのか

                                    prop_accepted[worst] = 0
                                    prop_accepted[i] = 1
                                    prop_matched[worst] = 0
                                    prop_matched[i] = prop_prefs[i][order[i]]
                                    order[worst] += 1
                                    resp_matched[indptr[prop_prefs[i][order[i]]]+ww-1] = i

                                end

                            end

                        end

                    end

                end

            end

        end

        return prop_matched, resp_matched, indptr

    elseif reverse == true            #大学　→　学生

        mm = length(prop_prefs)
        nn = length(resp_prefs)
        for o in 1:nn
            push!(resp_prefs[o],0)
        end

        #今回はprop_prefsに0を入れる必要がない

        indptr = Array{Int}(nn+1)
        indptr[1] = 1
        for k in 1:nn
            indptr[k+1] = indptr[k] + caps[k]
        end
        #indptrを関数内に内蔵！

        prop_matched = zeros(Int64,mm)
        resp_matched = zeros(Int64,sum(caps))
        resp_accepted = zeros(Int64,nn)
        order = ones(Int64,nn)

        while sum(resp_accepted) < nn       #学生全員がマッチするまで

            for i in 1:nn

                if resp_accepted[i] == 1    #学生iがマッチ済み

                else

                    if resp_prefs[i][order[i]] == 0   #大学iの第一志望がいないとき

                        resp_accepted[i] = 1

                    elseif findfirst(prop_prefs[resp_prefs[i][order[i]]],i) == 0 && resp_prefs[i][order[i]] !== 0
                        #大学が志望する学生の選択肢の中に大学がない

                        order[i] += 1

                    else

                        if prop_matched[resp_prefs[i][order[i]]] == 0      #希望の学生が空いているとき

                            prop_matched[resp_prefs[i][order[i]]] = i
                            vacant = findfirst(resp_matched[indptr[i]:indptr[i+1]-1],0)   #大学の空いている番号
                            resp_matched[indptr[i]+vacant-1] = resp_prefs[i][order[i]]

                            if findfirst(resp_matched[indptr[i]:indptr[i+1]-1],0) == 0  #大学の定員が埋まったとき

                                resp_accepted[i] = 1

                            else

                                order[i] += 1

                            end

                        else

                            rival = prop_matched[resp_prefs[i][order[i]]]     #希望の学生がすでにマッチング済みの時の相手

                            if findfirst(prop_prefs[resp_prefs[i][order[i]]],rival) < findfirst(prop_prefs[resp_prefs[i][order[i]]],i)
                                #大学iはライバル大学よりも学生に好まれていないとき

                                order[i] += 1

                            else
                                #大学iがライバル大学よりも人気だったとき
                                losestu = findfirst(resp_matched[indptr[rival]:indptr[rival+1]-1],resp_prefs[i][order[i]])
                                #ライバルの定員のどこに学生が位置していたか
                                resp_matched[indptr[rival]+losestu-1] = 0
                                vacant = findfirst(resp_matched[indptr[i]:indptr[i+1]-1],0)   #大学の空いている番号
                                resp_matched[indptr[i]+vacant-1] = resp_prefs[i][order[i]]
                                resp_accepted[rival] = 0

                                if order[rival] < length(resp_prefs[rival])

                                    order[rival] += 1

                                end

                                prop_matched[resp_prefs[i][order[i]]] = i    # 最後に学生のマッチング相手を置き換える

                                if findfirst(resp_matched[indptr[i]:indptr[i+1]-1],0) == 0  #大学の定員が埋まったとき

                                    resp_accepted[i] = 1

                                else

                                    order[i] += 1

                                end

                            end

                        end

                    end

                end

            end

        end

        return prop_matched, resp_matched, indptr

    end

end

export matching

end


using XPORTA: POI, IEQ, traf, make_porta_tmp, rm_porta_tmp
using JSON, Dates

export rotate_facet, adjacent_facets, adjacency_decomposition

"""
    rotate_facet(
        F :: Vector{Int64},
        G :: Vector{Int64},
        xbar :: Vector{Int64}
    ) :: Vector{Int64}

Performs a rotation of facet `F` relative to non-included vertex `xbar` and returns
the rotated facet vector. `F` is a polytope facet, `G` is a subfacet of `F` and `xbar`
is a vertex not contained by `F`. By construction, the rotated facet contains
`xbar`.
"""
function rotate_facet(F::Vector{Int64}, G ::Vector{Int64}, xbar::Vector{Int64}) :: Vector{Int64}
    a_rot = (F[end] - F[1:(end-1)]' * xbar)*G[1:(end-1)] - (G[end] - G[1:(end-1)]' * xbar)*F[1:(end-1)]
    β_rot = (F[end] - F[1:(end-1)]' * xbar)*G[end] - (G[end] - G[1:(end-1)]' * xbar)*F[end]

    cat(a_rot, β_rot, dims=1)
end

"""
    adjacent_facets(
        vertices :: Vector{Vector{Int64}},
        F :: Vector{int64};
        dir = "./" :: String,
        cleanup = true ::Bool
    ) :: Vector{Vector{Int64}}

For the polytope represented by `vertices`, returns the set of facets
adjacent to `F`.

Facet vector `F` and the return facet vectors are assumed to ba in the normalized
subspace.

The  `dir` argument specifies where to where to write files and directories from
XPORTA.jl. If `cleanup` is `true`, then a  porta_tmp directory is created as a
subdirectory of `dir`.

If `cleanup` is `false`, the created  `porta_tmp` directory is not removed.
"""
function adjacent_facets(
    vertices::Vector{Vector{Int64}},
    F::Vector{Int64};
    dir = "./" :: String,
    cleanup=true :: Bool
) :: Vector{Vector{Int64}}
    # vertices of facet F
    F_vertices = filter(v -> F[1:(end-1)]' * v == F[end], vertices)

    # find the subfacets of facet F, these subfacets are labeled G
    poi_vertices = hcat(F_vertices...)'[:,:]
    G_ieq = traf(POI(vertices = poi_vertices), dir=dir, cleanup=cleanup)
    G_ineqs = convert.(Int64, G_ieq.inequalities)

    # polytope vertices not in F index 1 is farthest from F
    xbar_vertices = sort(
        filter(v -> F[1:(end-1)]' * v != F[end], vertices),
        by = v -> F[1:(end-1)]' * v, rev=true
    )

    adjacent_facets = Array{Vector{Int64},1}(undef,0)
    for G_row_id in 1:size(G_ineqs,1)
        G = G_ineqs[G_row_id,:]
        for xbar_id in 1:length(xbar_vertices)
            xbar = xbar_vertices[xbar_id]

            # rotate G about x_bar to get G_rot
            G_rot = rotate_facet(F, G, xbar)

            # If no polytope vertices violate G_rot facet then it is a facet of polytope P
            if (findfirst(v -> G_rot[1:(end-1)]' * v > G_rot[end], xbar_vertices) === nothing)
                push!(adjacent_facets, G_rot)

                # breaking misses adjacent facets, but I have not found any cases
                # where a canonical facet class has been missed
                break # TODO: does breaking catch all adjacent facet classes?
            end
        end
    end

    return adjacent_facets
end

"""
    adjacenecy_decomposition(
        vertices :: Vector{Vector{Int64}},
        BG_seed :: BellGame,
        scenario :: LocalSignaling;
        kwargs...
    ) :: Dict

Given a polytpe represented by `vertices`, returns the complete set of canonical
facets for prepare and measure scenario `scenario`. The adjacency_decomposition algorithm
requires a seeded vertex which is supplied with the `BG_seed` argument. Facets
are returned in the lexicographic normal form.

### Returned Dictionary Format

Returns a dictionary where the keys are canonical `BellGames` and the value is a
dictionary with keys

* "considered" => true, if the facet was considered in the algorithm.
* "skipped" => true, if the facet was skipped (not considered).
* "num_vertices" => number of vertices.
* "norm_facet" => facet vector representative (normalized rep) of canonical game

### Keyword  Arguments: `kwargs...`
* `skip_games = [] ::  Vector{BellGame}` - List of games to skip.
* `max_vertices = 100 :: Int64` - The maximum number of vertices to allow in target facets.
* `dir` = "./" :: String` - Directory in which to create `porta_tmp/` and `.json` files.
* `log = false :: Bool` - If true, the facet dictionary is  written to `.json` each iteration.
* `log_filename = "adjacency_decomposition_$(Dates.now).json" :: String`
"""
function adjacency_decomposition(
    vertices :: Vector{Vector{Int64}},
    BG_seed :: BellGame,
    scenario :: LocalSignaling;
    skip_games = Array{BellGame}(undef,0) :: Vector{BellGame},
    max_vertices = 100 :: Int64,
    dir = "./"  :: String,
    log = false :: Bool,
    log_filename = "adjacency_decomposition_$(Dates.now()).json" :: String,
) :: Dict
    # canonicalize facet
    canonical_BG_seed = LocalPolytope.generator_facet(BG_seed, scenario)
    canonical_skip_BGs = map( BG -> LocalPolytope.generator_facet(BG, scenario), skip_games)

    norm_facet_seed = convert(Vector{Int64}, canonical_BG_seed, rep = "normalized")
    num_BG_seed_vertices = length(
        filter(v -> norm_facet_seed[1:(end-1)]' * v == norm_facet_seed[end], vertices)
    )

    # holds considered and unconsidered facets
    facet_dict = Dict{BellGame, Dict}(
        canonical_BG_seed => Dict(
            "considered" => false,
            "skipped" => false,
            "norm_facet"  => norm_facet_seed,
            "generator_facet" => convert(Vector{Int64}, canonical_BG_seed),
            "num_vertices" => num_BG_seed_vertices
        )
    )

    # add skipped facets as considered
    for skip_BG in canonical_skip_BGs
        facet_dict[skip_BG] = Dict(
            "considered" => true,
            "norm_facet" => convert(Vector{Int64}, skip_BG, rep = "normalized"),
            "generator_facet" => convert(Vector{Int64}, skip_BG),
            "skipped" => true
        )
    end

    # create porta_tmp directory
    porta_tmp_dir = make_porta_tmp(dir)

    # loop until all facet are considered
    while !all(d -> d["considered"], collect(values(facet_dict)))
        # select the first uncosidered facet with fewest vertices
        target_BG = sort(
                filter(d -> facet_dict[d]["considered"] == false, collect(keys(facet_dict))),
                by=key -> facet_dict[key]["num_vertices"]
            )[1]

        # get target facet vector in normalized representation
        norm_facet = facet_dict[target_BG]["norm_facet"]

        # compute adjacent facets
        adj_facets = try
            adjacent_facets(vertices, norm_facet, dir=porta_tmp_dir, cleanup=false)
        # if an unexpected error occurs with XPORTA, mark facet as such and move on.
        catch error
            facet_dict[target_BG]["considered"] = true

            push!(facet_dict[target_BG],"error" => true)
            push!(facet_dict[target_BG],"error_msg" => sprint(showerror, error, backtrace()))

            continue
        end

        for adj_facet in adj_facets
            adj_game = convert(BellGame, adj_facet, scenario, rep="normalized")
            canonical_game = LocalPolytope.generator_facet(adj_game, scenario)

            # add new facet to  dictionary
            if !haskey(facet_dict, canonical_game)
                num_vertices = length(filter(v -> adj_facet[1:(end-1)]' * v == adj_facet[end], vertices))

                if num_vertices <= max_vertices
                    facet_dict[canonical_game] = Dict(
                        "considered" => false,
                        "skipped" => false,
                        "norm_facet" => adj_facet,
                        "generator_facet" => convert(Vector{Int64}, canonical_game),
                        "num_vertices" => num_vertices
                    )
                else
                    facet_dict[canonical_game] = Dict(
                        "considered" => true,
                        "skipped" => true,
                        "norm_facet" => adj_facet,
                        "generator_facet" => convert(Vector{Int64}, canonical_game),
                        "num_vertices" => num_vertices
                    )
                end
            end
        end

        # set current bell game "considered" to true
        facet_dict[target_BG]["considered"] = true

        if log
            open(dir*log_filename, "w") do io
                JSON.print(io, facet_dict)
            end
        end
    end

    # cleanup porta_tmp directory after completion
    rm_porta_tmp(dir)

    facet_dict
end


using Distributions, Random, Test

@testset "truncated exponential" begin
    d = Exponential(1.5)
    l = 1.2
    r = 2.7
    @test mean(d) ≈ mean(truncated(d; lower=-3.0)) # non-binding truncation
    @test mean(d) ≈ mean(truncated(d; lower=-3.0, upper=Inf))
    @test mean(truncated(d; lower=l)) ≈ mean(d) + l
    @test mean(truncated(d; lower=l, upper=Inf)) ≈ mean(d) + l
    # test values below calculated using symbolic integration in Maxima
    @test mean(truncated(d, 0, r)) ≈ 0.9653092084094841
    @test mean(truncated(d, l, r)) ≈ 1.82703493969601

    # all the fun corner cases and numerical quirks
    @test mean(truncated(Exponential(1.0); upper=0)) == 0                   # degenerate
    @test mean(truncated(Exponential(1.0); lower=-Inf, upper=0)) == 0
    @test mean(truncated(Exponential(1.0); upper=0+eps())) ≈ 0 atol = eps() # near-degenerate
    @test mean(truncated(Exponential(1.0); lower=-Inf, upper=0+eps())) ≈ 0 atol=eps()
    @test mean(truncated(Exponential(1.0), 1.0, 1.0+eps())) ≈ 1.0 # near-degenerate
    @test mean(truncated(Exponential(1e308), 1.0, 1.0+eps())) ≈ 1.0 # near-degenerate
end


module LinearDynamicsModels

using LinearAlgebra
using StaticArrays
using DifferentialDynamicsModels
using ForwardDiff
using Requires
using MacroTools

import DifferentialDynamicsModels: SteeringBVP
import DifferentialDynamicsModels: state_dim, control_dim, duration, propagate, instantaneous_control
export LinearDynamics, ZeroOrderHoldLinearization, FirstOrderHoldLinearization, linearize
export NIntegratorDynamics, DoubleIntegratorDynamics, TripleIntegratorDynamics
export LinearQuadraticSteering, NIntegratorSteering, DoubleIntegratorSteering, TripleIntegratorSteering

include("utils.jl")

# Continous-Time Linear Time-Invariant Systems
struct LinearDynamics{Dx,Du,TA<:StaticMatrix{Dx,Dx},TB<:StaticMatrix{Dx,Du},Tc<:StaticVector{Dx}} <: DifferentialDynamics
    A::TA
    B::TB
    c::Tc
end
Base.zero(::Type{LinearDynamics{Dx,Du,TA,TB,Tc}}) where {Dx,Du,TA,TB,Tc} = LinearDynamics(zero(TA), zero(TB), zero(Tc))

state_dim(::LinearDynamics{Dx,Du}) where {Dx,Du} = Dx
control_dim(::LinearDynamics{Dx,Du}) where {Dx,Du} = Du

(f::LinearDynamics{Dx,Du})(x::StaticVector{Dx}, u::StaticVector{Du}) where {Dx,Du} = f.A*x + f.B*u + f.c
function propagate(f::LinearDynamics{Dx,Du}, x::StaticVector{Dx}, SC::StepControl{Du}) where {Dx,Du}
    y = f.B*SC.u + f.c
    eᴬᵗ, ∫eᴬᵗy = integrate_expAt_B(f.A, y, SC.t)
    eᴬᵗ*x + ∫eᴬᵗy
end
function propagate(f::LinearDynamics{Dx,Du}, x::StaticVector{Dx}, RC::RampControl{Du}) where {Dx,Du}
    y = f.B*RC.uf + f.c
    eᴬᵗ, ∫eᴬᵗy = integrate_expAt_B(f.A, y, RC.t)
    z = f.B*(RC.u0 - RC.uf)
    _, _, ∫eᴬᵗztdt⁻¹ = integrate_expAt_Bt_dtinv(f.A, z, RC.t)
    eᴬᵗ*x + ∫eᴬᵗy + ∫eᴬᵗztdt⁻¹
end

# Discrete-Time Linear Time-Invariant Systems
include("linearization.jl")

# NIntegrators (DoubleIntegrator, TripleIntegrator, etc.)
function NIntegratorDynamics(::Val{N}, ::Val{D}, ::Type{T} = Rational{Int}) where {N,D,T}
    A = diagm(Val(D) => ones(SVector{(N-1)*D,T}))
    B = [zeros(SMatrix{(N-1)*D,D,T}); SMatrix{D,D,T}(I)]
    c = zeros(SVector{N*D,T})
    LinearDynamics(A, B, c)
end
NIntegratorDynamics(N::Int, D::Int, ::Type{T} = Rational{Int}) where {T} = NIntegratorDynamics(Val(N), Val(D), T)
DoubleIntegratorDynamics(D::Int, ::Type{T} = Rational{Int}) where {T} = NIntegratorDynamics(2, D, T)
TripleIntegratorDynamics(D::Int, ::Type{T} = Rational{Int}) where {T} = NIntegratorDynamics(3, D, T)

# TimePlusQuadraticControl BVPs
function SteeringBVP(f::LinearDynamics{Dx,Du}, j::TimePlusQuadraticControl{Du};
                     compile::Union{Val{false},Val{true}}=Val(false)) where {Dx,Du}
    compile === Val(true) ? error("Run `import SymPy` to enable SteeringBVP compilation.") :
                            SteeringBVP(f, j, EmptySteeringConstraints(), EmptySteeringCache())
end
const LinearQuadraticSteering{Dx,Du,Cache} = SteeringBVP{<:LinearDynamics{Dx,Du},<:TimePlusQuadraticControl{Du},EmptySteeringConstraints,Cache}
function LinearQuadraticSteering(A, B, c, R; compile::Union{Val{false},Val{true}}=Val(false))
    SteeringBVP(LinearDynamics(A, B, c), TimePlusQuadraticControl(R), compile=compile)
end
function NIntegratorSteering(N::Int, D::Int, R=SMatrix{D,D,Rational{Int}}(I); compile::Union{Val{false},Val{true}}=Val(false))
    SteeringBVP(NIntegratorDynamics(N, D), TimePlusQuadraticControl(R), compile=compile)
end
function DoubleIntegratorSteering(D::Int, R=SMatrix{D,D,Rational{Int}}(I); compile::Union{Val{false},Val{true}}=Val(false))
    SteeringBVP(DoubleIntegratorDynamics(D), TimePlusQuadraticControl(R), compile=compile)
end
function TripleIntegratorSteering(D::Int, R=SMatrix{D,D,Rational{Int}}(I); compile::Union{Val{false},Val{true}}=Val(false))
    SteeringBVP(TripleIntegratorDynamics(D), TimePlusQuadraticControl(R), compile=compile)
end

## Ad Hoc Steering
struct LinearQuadraticSteeringControl{Dx,Du,T,
                                      Tx0<:StaticVector{Dx},
                                      Txf<:StaticVector{Dx},
                                      Tf<:LinearDynamics{Dx,Du},
                                      Tj<:TimePlusQuadraticControl{Du},
                                      Tz<:StaticVector{Dx}} <: ControlInterval
    t::T
    x0::Tx0
    xf::Txf
    dynamics::Tf
    cost::Tj
    z::Tz
end
duration(lqsc::LinearQuadraticSteeringControl) = lqsc.t
function Base.zero(::Type{LinearQuadraticSteeringControl{Dx,Du,T,Tx0,Txf,Tf,Tj,Tz}}) where {Dx,Du,T,Tx0,Txf,Tf,Tj,Tz}
    LinearQuadraticSteeringControl(zero(T), zero(Tx0), zero(Txf), zero(Tf), zero(Tj), zero(Tz))
end
propagate(f::LinearDynamics, x::State, lqsc::LinearQuadraticSteeringControl) = (x - lqsc.x0) + lqsc.xf
function propagate(f::LinearDynamics, x::State, lqsc::LinearQuadraticSteeringControl, s::Number)
    f, j = lqsc.dynamics, lqsc.cost
    x0, A, B, c, R, z = lqsc.x0, f.A, f.B, f.c, j.R, lqsc.z
    eᴬˢ, ∫eᴬˢc = integrate_expAt_B(A, c, s)
    Gs = integrate_expAt_B_expATt(A, B*(R\B'), s)
    (x - x0) + eᴬˢ*x0 + ∫eᴬˢc + Gs*(eᴬˢ'\z)
end
function instantaneous_control(lqsc::LinearQuadraticSteeringControl, s::Number)
    A, B, R, z = lqsc.dynamics.A, lqsc.dynamics.B, lqsc.cost.R, lqsc.z
    eᴬˢ = exp(A*s)
    (R\B')*(eᴬˢ'\z)
end
function (j::TimePlusQuadraticControl)(lqsc::LinearQuadraticSteeringControl)
    @assert j == lqsc.cost
    cost(lqsc.dynamics, lqsc.cost, lqsc.x0, lqsc.xf, lqsc.t)
end

function (bvp::LinearQuadraticSteering{Dx,Du,EmptySteeringCache})(x0::StaticVector{Dx}, xf::StaticVector{Dx},
                                                                  c_max::T=eltype(x0)(1e6)) where {Dx,Du,T<:Number}    # TODO: handle c_max == Inf
    f, j = bvp.dynamics, bvp.cost
    A, B, c, R = f.A, f.B, f.c, j.R
    x0 == xf && return (cost=T(0), controls=LinearQuadraticSteeringControl(T(0), x0, xf, f, j, zeros(similar_type(typeof(c), T))))
    t = optimal_time(bvp, x0, xf, c_max)
    Q = B*(R\B')
    G = integrate_expAt_B_expATt(A, Q, t)
    eᴬᵗ, ∫eᴬᵗc = integrate_expAt_B(A, c, t)
    x̄ = eᴬᵗ*x0 + ∫eᴬᵗc
    z = eᴬᵗ'*(G\(xf - x̄))
    (cost=cost(f, j, x0, xf, t), controls=LinearQuadraticSteeringControl(t, x0, xf, f, j, z))
end

function cost(f::LinearDynamics{Dx,Du}, j::TimePlusQuadraticControl{Du},
              x0::StaticVector{Dx}, xf::StaticVector{Dx}, t) where {Dx,Du}
    A, B, c, R = f.A, f.B, f.c, j.R
    Q = B*(R\B')
    G = integrate_expAt_B_expATt(A, Q, t)
    eᴬᵗ, ∫eᴬᵗc = integrate_expAt_B(A, c, t)
    x̄ = eᴬᵗ*x0 + ∫eᴬᵗc
    t + (xf - x̄)'*(G\(xf - x̄))
end

function dcost(f::LinearDynamics{Dx,Du}, j::TimePlusQuadraticControl{Du},
               x0::StaticVector{Dx}, xf::StaticVector{Dx}, t) where {Dx,Du}
    A, B, c, R = f.A, f.B, f.c, j.R
    Q = B*(R\B')
    G = integrate_expAt_B_expATt(A, Q, t)
    eᴬᵗ, ∫eᴬᵗc = integrate_expAt_B(A, c, t)
    x̄ = eᴬᵗ*x0 + ∫eᴬᵗc
    z = eᴬᵗ'*(G\(xf - x̄))
    1 - 2*(A*x0 + c)'*z - z'*Q*z
end

function optimal_time(bvp::SteeringBVP{D,C,EmptySteeringConstraints,EmptySteeringCache},
                      x0::StaticVector{Dx},
                      xf::StaticVector{Dx},
                      t_max::T) where {Dx,Du,T<:Number,D<:LinearDynamics{Dx,Du},C<:TimePlusQuadraticControl{Du}}
    t = bisection(t -> dcost(bvp.dynamics, bvp.cost, x0, xf, t), t_max/100, t_max)
    t !== nothing ? t : golden_section(cost, t_max/100, t_max)
end

## Compiled Steering Functions (enabled by `import SymPy`; compiled `SteeringBVP`s return `BVPControl`s)
function __init__()
    @require SymPy="24249f21-da20-56a4-8eb1-6a02cf4ae2e6" include("sympy_bvp_compilation.jl")
end

end # module


import Celeste.Coordinates: angular_separation, match_coordinates

@testset "coordinates" begin

    @test angular_separation(180., 45., 0., -45.) == 180.
    @test angular_separation(0., 0., 0., 1.) == 1.

    ra1 = [2., 4., 5.]
    dec1 = [70., -80., 90.]
    ra2 = [5., 2., 0., 50.]
    dec2 = [89.9, 70.1, 0., -90.]
    idxs, dists = match_coordinates(ra1, dec1, ra2, dec2)

    @test length(idxs) == length(ra1)
    @test idxs[1] == 2 && dists[1] ≈ 0.1
    @test idxs[2] == 4 && dists[2] ≈ 10.
    @test idxs[3] == 1 && dists[3] ≈ 0.1
end


using LinearAlgebra
using Test

using ProximalOperators
using ProximalAlgorithms:
    DouglasRachfordIteration, DRLSIteration,
    ForwardBackwardIteration, PANOCIteration,
    PANOCplusIteration,
    NoAcceleration,PnpDrsIteration

@testset "DR/DRLS equivalence ($T)" for T in [Float32, Float64]
    A = T[
        1.0 -2.0 3.0 -4.0 5.0
        2.0 -1.0 0.0 -1.0 3.0
        -1.0 0.0 4.0 -3.0 2.0
        -1.0 -1.0 -1.0 1.0 3.0
    ]
    b = T[1.0, 2.0, 3.0, 4.0]

    m, n = size(A)

    R = real(T)

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

    f = LeastSquares(A, b)
    g = NormL1(lam)

    x0 = zeros(R, n)

    dr_iter = DouglasRachfordIteration(f=f, g=g, x0=x0, gamma=R(10) / opnorm(A)^2)
    drls_iter = DRLSIteration(f=f, g=g, x0=x0, gamma=R(10) / opnorm(A)^2, lambda=R(1), c=-R(Inf), max_backtracks=1, directions=NoAcceleration())

    for (dr_state, drls_state) in Iterators.take(zip(dr_iter, drls_iter), 10)
        @test isapprox(dr_state.x, drls_state.xbar)
    end
end

@testset "Pnp-DRS equivalence ($T)" for T in [Float32, Float64]
    A = T[
        1.0 -2.0 3.0 -4.0 5.0
        2.0 -1.0 0.0 -1.0 3.0
        -1.0 0.0 4.0 -3.0 2.0
        -1.0 -1.0 -1.0 1.0 3.0
    ]
    b = T[1.0, 2.0, 3.0, 4.0]

    m, n = size(A)

    R = real(T)

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

    f = LeastSquares(A, b)
    g = NormL1(lam)

    x0 = zeros(R, n)

    y = similar(x0)

    gamma=R(10) / opnorm(A)^2

    proxf!(xhat,uhat) = prox!(xhat,f,uhat,gamma) #in place prox updates xhat
    denoiser!(yhat,rhat) = prox!(yhat,g,rhat,gamma) #rhat^{k+1} = 2xhat^{k+1} - uhat^{k}

    dr_iter = DouglasRachfordIteration(f=f, g=g, x0=x0, gamma=gamma)

    pnp_dr_iter = PnpDrsIteration(proxf! = proxf!, denoiser! = denoiser!, uhat0 = x0, gamma = gamma)

    for (dr_state, pnp_dr_state) in Iterators.take(zip(dr_iter, pnp_dr_iter), 10)
        @test isapprox(dr_state.x, pnp_dr_state.uhat)
        @test isapprox(dr_state.y, pnp_dr_state.xhat)
        @test isapprox(dr_state.z, pnp_dr_state.yhat)
    end
end

@testset "FB/PANOC equivalence ($T)" for T in [Float32, Float64]
    A = T[
        1.0 -2.0 3.0 -4.0 5.0
        2.0 -1.0 0.0 -1.0 3.0
        -1.0 0.0 4.0 -3.0 2.0
        -1.0 -1.0 -1.0 1.0 3.0
    ]
    b = T[1.0, 2.0, 3.0, 4.0]

    m, n = size(A)

    R = real(T)

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

    f = LeastSquares(A, b)
    g = NormL1(lam)

    x0 = zeros(R, n)

    fb_iter = ForwardBackwardIteration(f=f, g=g, x0=x0, gamma=R(0.95) / opnorm(A)^2)
    panoc_iter = PANOCIteration(f=f, g=g, x0=x0, gamma=R(0.95) / opnorm(A)^2, max_backtracks=1, directions=NoAcceleration())

    for (fb_state, panoc_state) in Iterators.take(zip(fb_iter, panoc_iter), 10)
        @test isapprox(fb_state.z, panoc_state.z)
    end
end

@testset "PANOC/PANOCplus equivalence ($T)" for T in [Float32, Float64]
    A = T[
        1.0 -2.0 3.0 -4.0 5.0
        2.0 -1.0 0.0 -1.0 3.0
        -1.0 0.0 4.0 -3.0 2.0
        -1.0 -1.0 -1.0 1.0 3.0
    ]
    b = T[1.0, 2.0, 3.0, 4.0]

    m, n = size(A)

    R = real(T)

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

    f = LeastSquares(A, b)
    g = NormL1(lam)

    x0 = zeros(R, n)

    panoc_iter = PANOCIteration(f=f, g=g, x0=x0, gamma=R(0.95) / opnorm(A)^2)
    panocplus_iter = PANOCplusIteration(f=f, g=g, x0=x0, gamma=R(0.95) / opnorm(A)^2)

    for (panoc_state, panocplus_state) in Iterators.take(zip(panoc_iter, panocplus_iter), 10)
        @test isapprox(panoc_state.z, panocplus_state.z)
    end
end


## Biot savart law
# using LinearAlgebra
# using PyPlot
# using MPI_Tools
# using Interpolations

VecDist(X::Array) = [ X[2,1]-X[1,1], X[2,2]-X[1,2] , X[2,3]-X[1,3]]
#X is the 2 coordinates to take distance between in format of [x₁,y₁,z₁;x₂,y₂,z₂]

"""
This function takes in:
    "Point Path" which is an Nx3 set of coordinates that define a current loop
    "r" which is a 1x3 vector that is a point in space to evaluate the 3D B field at

    kwargs:
        "current" is a scalar multiplier. Default is 1 Amp
        "MinThreshold" is a minimum distance that the "r" point must be from any wire
            The function tends to inf near wires

"""
function BiotSav(PointPath,r;Current=1 ,MinThreshold = 0.01)
    PointPath = vcat(PointPath,PointPath[1,:]')
    NPts = length(PointPath[:,1])
    # dB = zeros(NPts-1,3)
    dB = [0. , 0., 0.]
    minDist = minimum(sqrt.(sum((PointPath .- repeat(transpose(r[:]),NPts,1)).^2,dims=2)))
    if minDist>= MinThreshold
        for I in 2:NPts
            dL = VecDist(PointPath[I-1:I,:])
            MeanPoint = sum(PointPath[I-1:I,:],dims=1)[:] ./ 2
            Rprime = r .- MeanPoint

            RDist = sqrt(sum(Rprime.^2))
            R̂ = Rprime/RDist

                if Current isa AbstractArray
                    # dB[I-1,:] = μ₀/(4*π) .* Current[I].*LinearAlgebra.cross(dL,R̂[:]) ./ (RDist)^2
                    dB .+= μ₀/(4*π) .* Current[I].*LinearAlgebra.cross(dL,R̂[:]) ./ (RDist)^2

                else
                    # dB[I-1,:] = μ₀/(4*π) .* Current.*LinearAlgebra.cross(dL,R̂[:]) ./ (RDist)^2
                    dB .+= μ₀/(4*π) .* Current.*LinearAlgebra.cross(dL,R̂[:]) ./ (RDist)^2
                end

        end
    # else
    #    plot(r[1],r[2],"r*")
    end
    sum(dB,dims=1)
end

function BiotSav(PointPath,dL,r,L)

    dB = [0. , 0., 0.]

        for I in 2:L
            Rprime = r .- PointPath[I]
            RDist = (sum(Rprime.^2))
            dB .+= 1e-7 .*LinearAlgebra.cross(dL[I-1],(Rprime/sqrt(RDist))) ./ (RDist)
        end

    dB
end

"""
Makes a point path of an ellipse with r₁,r₂ as two radii
normal direction is assumed to be in Z
kwargs: Center is the center of path [X,Y,Z]
NPts is the number of points along path

"""
function MakeEllip(r₁,r₂;Center = [0,0,0],NPts=100)
    X,Y,Z = ([r₁ .* cos.(I/NPts *2*π) for I in 1:NPts].+Center[1],
             [r₂ .* sin.(I/NPts *2*π) for I in 1:NPts].+Center[2],
             [0 for I in 1:NPts].+Center[3])

    Coords = hcat(X,Y,Z)

end

"""
Simple analytical formula for the field on axis of a Circle
    R - radius in meters
    z - axial offset in meters

    kwargs:
        I = current in coil

"""
function FieldOnAxis_Circ(R,z;I=1)
    Bz = μ₀*2*pi*R^2*I ./ (4*π* (z^2 + R^2)^(3/2))
end

# x = -.3:.01:.3
# y = x
# DefField = [BiotSav(MakeEllip(1,1;Center = [0,0,0],NPts=1000),[i,j,k];MinThreshold= 0.05)[3]
#             for i in x,
#                 j in y,
#                 k=0]
# X,Y = MPI_Tools.meshgrid(x,y)
# PyPlot.surf(X[:],Y[:],DefField[:],cmap="jet")

function MakeEllipTestPoints(r₁,r₂;Center = [0,0,0],NPts=6,Layers = 3)
    LL=1
    X,Y,Z = ([r₁*LL/(Layers+1) .* cos.(I/NPts *2*π) for I in 1:NPts].+Center[1],
             [r₂ *LL/(Layers+1) .* sin.(I/NPts *2*π) for I in 1:NPts].+Center[2],
             [0 for I in 1:NPts].+Center[3])
             Coords = hcat(X,Y,Z)
    for LL in 2:Layers
        X₁,Y₁,Z₁ = ([r₁*LL/(Layers+1) .* cos.(I/NPts *2*π) for I in 1:NPts].+Center[1],
                 [r₂ *LL/(Layers+1) .* sin.(I/NPts *2*π) for I in 1:NPts].+Center[2],
                 [0 for I in 1:NPts].+Center[3])
                 Coords = hcat(X,Y,Z)
        X = vcat(X,X₁)
        Y = vcat(Y,Y₁)
        Z = vcat(Z,Z₁)

     end
     return X,Y,Z
 end

"""
This function takes in a pointpath [X,Y,Z] array and maps the interior

Inputs are
PointPath [X,Y,Z] , N x 3 array
NumLayers - number of interior test points

"""
 function FieldMapPointPath(PointPath, NumLayers; WeightRadius = false,InvWeights = false, PlotAxes = nothing)

     xTP = PointPath[:,1]
     x   = PointPath[:,1]
     yTP = PointPath[:,2]
     y   = PointPath[:,2]
     x₁ = minimum(xTP)
     x₂ = maximum(xTP)
     y₁ = minimum(yTP)
     y₂ = maximum(yTP)
     xmid = (x₁+ x₂)/2
     ymid = (y₁+ y₂)/2

     for i in 1:(NumLayers-2)
         xTP = vcat(xTP,(x .- xmid) .* (i/NumLayers) .+ xmid)
         yTP = vcat(yTP,(y .- ymid) .* (i/NumLayers) .+ ymid)
     end
     TPList = hcat(xTP,yTP,zeros(size(yTP)))
     TP_Arr =[([TPList[i,1],TPList[i,2],TPList[i,3]]) for i in 1:length(xTP)]
     if WeightRadius
         Weights = PointPath[:,1]
     else
          Weights = ones(size( PointPath[:,1]))
      end
      if InvWeights
          Weights = 1 ./Weights
      end
      push!(Weights,Weights[1])
     BMag = [BiotSav(PointPath,TP_Arr[i];Current = Weights,MinThreshold =1e-8)[3] for i in 1:length(TP_Arr)]

     BMag = abs.(BMag)

     BPlot = findall(xx-> xx!=0,BMag)
     if PlotAxes!== nothing
        PyPlot.axes(PlotAxes)
     end

     surf(xTP[BPlot],yTP[BPlot],BMag[BPlot],cmap="jet")

 end

"""
Makes a point path for the BiotSav function
    Input is an Nx3 array of coordinates where it is structured as:
    [x₁, y₁, z₁
    ..
    ..
    xₙ, yₙ, zₙ]

Coords = [1 0 0
          2 0 0
          2 1 0
          1 1 0]
"""
function MakeRectPointPath(Coords;NElement = 50, PlotOn=false)
    Coords = vcat(Coords, reshape(Coords[1,:],1,3))
    xVec = Coords[:,1]
    yVec = Coords[:,2]
    zVec = Coords[:,3]
    xVecUp = [Coords[1,1]]
    yVecUp = [Coords[1,2]]
    zVecUp = [Coords[1,3]]
    for i in 1:(length(xVec)-1)
        UpSamp = LinRange(xVec[i],xVec[i+1],NElement)[:]
        xVecUp = vcat(xVecUp,UpSamp,Coords[i+1,1])
        UpSampy = LinRange(yVec[i],yVec[i+1],NElement)[:]
        yVecUp = vcat(yVecUp,UpSampy,Coords[i+1,2])
        UpSampz = LinRange(zVec[i],zVec[i+1],NElement)[:]
        zVecUp = vcat(zVecUp,UpSampz,Coords[i+1,3])
    end
    PointPath = hcat(xVecUp,yVecUp,zVecUp)
    if PlotOn
        scatter3D(xVecUp,yVecUp,zVecUp)
    end
    return PointPath

end

function WireNearField_B(r,Wire_R;I=1)
    μ₀ = 4*π*1e-7
    if r<Wire_R
        B = μ₀*r*I / (2*π*Wire_R^2)
    elseif r==Wire_R
        B = μ₀*I / (2*π*Wire_R)
    else
        B = μ₀*I / (2*π*r)
    end
    return B
end

function DerivField(r,Wire_R=0.001;I=1)
    μ₀ = 4*π*1e-7
    if r<Wire_R
        B = 0 ##Update later.
    elseif r==Wire_R
        B=0
    else
        B = -1*μ₀*I / (2*π*r^2)
    end
    return B
end

function BiotSavSpeedTest(NPathPoints, NTestPoints)
    PointPath =  MakeEllip(1. ,1. ;NPts = NPathPoints)
    TestPoints,W = PP_2_TestPoints(PointPath)
    PointPath = vcat(PointPath,PointPath[1,:]')
    dL = ([CircModDesign.VecDist(PointPath[I-1:I,:]) for I in 2:length(PointPath[:,1])])
    PointPath2 = ([PointPath[I,:] for I in 1:length(PointPath[:,1])])
    L = length(PointPath[:,1])
    dBMat = zeros(size(TestPoints))
   @time begin
    for I in 1:length(TestPoints[:,1])
    dBMat[I,:] = BiotSav(PointPath2,dL,TestPoints[I,:],L)
    end
   end
    dBMat
end


# # Little Group in 2D

# ## Preamble
using LatticeTools
using Formatting
using Plots

function display_matrix(io::IO, matrix::AbstractMatrix; prefix::AbstractString="")
    width = ceil(Int, maximum(length("$item")+1 for item in matrix)/4)*4
    for row in eachrow(matrix)
        for (icol, col) in enumerate(row)
            icol == 1 && print(io, prefix)
            printfmt(io, "{:>$(width)s}", "$col")
        end
        println(io)
    end
end

# ## Set up Lattice and Symmetry
unitcell = makeunitcell([1.0 0.0; 0.0 1.0]; SiteType=String);
addsite!(unitcell, "Ox", FractCoord([0,0], [0.5, 0.0]));
addsite!(unitcell, "Oy", FractCoord([0,0], [0.0, 0.5]));

lattice = makelattice(unitcell, [4 0; 0 4]);
tsym = FiniteTranslationSymmetry(lattice);
psym = project(PointSymmetryDatabase.get(13), [1 0 0; 0 1 0]);

# ## Little Group
lge = little_group_elements(tsym, 2, psym)
lg = little_group(tsym, 2, psym)
println("Little Group")
println("------------")
display_matrix(stdout, group_multiplication_table(lg))

lg_matrep = psym.matrix_representations[lge]
println("Matrix Representations: $lg_matrep")

# ## Finding Point Groups Isomorphic to the Little Group
little_symmetry_candidates = Tuple{PointSymmetry, Vector{Int}}[]
for i in 1:32
    ps = PointSymmetryDatabase.get(i)
    ϕ = group_isomorphism(lg, ps.group)
    if !isnothing(ϕ)
        push!(little_symmetry_candidates, (ps, ϕ))
    end
end
(psym2, ϕ) = first(little_symmetry_candidates)

lg_matrep2 = lg_matrep[ϕ]
println("Matrix Representations (Isomorphic): $lg_matrep2")

# ## Multiplication Tables
println("Parent Point Group")
println("------------------")
display_matrix(stdout, group_multiplication_table(psym2))
println("Little Group")
println("------------")
display_matrix(stdout, group_multiplication_table(lg_matrep))
println("Isomorphic Little Group")
println("-----------------------")
display_matrix(stdout, group_multiplication_table(lg_matrep2))

# ## Irreps and Little Groups

println("Irreps and Little Groups")
println("------------------------")
for tsic in get_irrep_components(tsym)
    idx = tsic.irrep_index
    kf = tsym.fractional_momenta[idx]
    k = lattice.unitcell.reducedreciprocallatticevectors * kf
    psym_little = little_symmetry(tsym, idx, psym)
    println("- irrep_index: $(idx)")
    println("  momentum: $(k)")
    println("  little_point_group: { name: \"$(psym_little.hermann_mauguin)\", order: $(group_order(psym_little)) }")
    println("  is_psym_compatible: $(iscompatible(tsym, idx, psym))")
    println("  is_psym_little_compatible: $(iscompatible(tsym, idx, psym_little))")
end


module Lattices
export Lattice, MakeLattice

    struct Lattice
        name::String
        l::Int16
        dim::Int8
        a::Array
        unit::Array
        N::Int16
        X::Array
        nnei::Int8
        neigh::Array
    end

    function armod(x,y)
        return mod.(x-1,y)+1
    end

    function MakeLattice(name,l)
        if(name=="Square")
            return MakeSquare(l)
        elseif(name=="Chain")
            return MakeChain(l)
        elseif(name=="Honeycomb")
            return MakeHoneycomb(l)
        elseif(name=="Checkerboard")
            return MakeCheckerboard(l)

            #elseif(name=="Pyrochlore")
            #elseif(name=="Kagome")
        else
            println("I didnt give a good name, making a 2x2 square")
            return MakeSquare(2)
        end

    end

    function MakeSquare(l)
        N=l^2;
        d=2;
        X=Array{Int16}(undef,N,2);
        nnei=4;
        neigh=Array{Int16}(undef,N,4);

        X[:,1]=armod.(collect(1:N),l);
        X[:,2]=ceil.(collect(1:(N))/l);
        a=[[1 0]
            [0 1]];
        unit=[0,0];

        neigh[:,1]=armod.(X[:,1].+1,l).+l*(X[:,2].-1);
        neigh[:,2]=armod.(X[:,1].+l.-1,l).+l*(X[:,2].-1);
        neigh[:,3]=X[:,1].+mod.(X[:,2],l)*l;
        neigh[:,4]=X[:,1].+mod.(X[:,2].+l.-2,l)*l;

        return Lattice("Square",l,d,a,unit,N,X,nnei,neigh)
      end

      function MakeCheckerboard(l)
          N=l^2;
          d=2;
          X=Array{Int16}(undef,N,2);
          nnei=6;
          neigh=Array{Int16}(undef,N,6);

          X[:,1]=armod.(collect(1:N),l);
          X[:,2]=ceil.(collect(1:(N))/l);
          a=[[1 0]
              [0 1]];
          unit=[0,0];

          neigh[:,1]=armod.(X[:,1].+1,l).+l*(X[:,2].-1);
          neigh[:,2]=armod.(X[:,1].+l.-1,l).+l*(X[:,2].-1);
          neigh[:,3]=X[:,1].+mod.(X[:,2],l)*l;
          neigh[:,4]=X[:,1].+mod.(X[:,2].+l.-2,l)*l;

          for ii in 1:N
              if isodd(X[ii,1])==isodd(X[ii,2])
                  neigh[ii,5]=armod(X[ii,1]+1,l)+l*mod.(X[ii,2],l);
                  neigh[ii,6]=armod(X[ii,1]+l-1,l)+l*mod.(X[ii,2]+l-2,l);
              else
                neigh[ii,5]=armod(X[ii,1]+1,l)+l*mod.(X[ii,2]+l-2,l);
                neigh[ii,6]=armod(X[ii,1]+l-1,l)+l*mod.(X[ii,2],l);
              end
          end

          return Lattice("Checkerboard",l,d,a,unit,N,X,nnei,neigh)
      end

    function MakeChain(l)
        N=l
        d=1
        nnei=2
        neigh=Array{Int16}(undef,N,2)
        a=[1];
        unit=[0];

        X=collect(1:l);

        neigh[:,1]=armod.(collect(2:(l+1)),l);
        neigh[:,2]=armod.(collect(l:(2*l-1)),l);

        return Lattice("Chain",l,d,a,unit,N,X,nnei,neigh)
    end

    function MakeHoneycomb(l)
        d=2;
        nnei=3;
        N=2*l^2;
        a=[[2*cos(pi/6) 0]
            [cos(pi/6) 1+sin(pi/6)]];
        unit=[[0 0]
                [cos(π/6) sin(π/6)]];

        return GeneralMultiUnit2D("Honeycomb",l,d,a,unit,nnei)
    end

    function GeneralMultiUnit2D(name,l,d,a,unit,nnei)
        ncell=size(a)[1];
        N=l*l*ncell;

        a1=repeat(transpose(a[1,:]),outer=[ncell,1]);
        a2=repeat(transpose(a[2,:]),outer=[ncell*l,1]);

        X=Array{Float64}(undef,N,2);
        # Here we are actually calculating the positions for every site
        for i in 1:l    #for the first row
            X[ncell*i-ncell+1:ncell*i,:]=unit+(i-1)*a1;
        end

        for j in 2:l    #copying the first row into the first layer
            X[(ncell*l*(j-1)+1):(ncell*l*(j-1)+ncell*l),:]=X[1:ncell*l,:]+(j-1)*a2;
        end

        neigh=Array{Int16}(undef,N,nnei)

        return Lattice(name,l,d,a,unit,N,X,nnei,neigh)
    end

end


ENV["JULIA_DEBUG"] = "all"
using HierarchicalTemporalMemory, Test

# Test iteration over a hypercube
startP= (50,80)
radius= 30
hc1= Hypercube(startP,radius,(200,200));
a= [convert(NTuple{2,Int},i) for i in hc1]
reference= [i for i in zip(
    repeat(startP[1]-radius : startP[1]+radius,outer=2*radius+1),
    repeat(startP[2]-radius : startP[2]+radius,inner=2*radius+1))]
@test a == reference


function dijkstra_test()
    (A,xy,labels) = load_matrix_network_metadata("airports")
    A = -A; # fix funny encoding of airport data
    lax = 247; rst = 355
    (d,pred) = dijkstra(A,lax)

    if maximum(d) != 540
        error("dijkstra failed")
    end

    if pred[end] != lax
        error("dijkstra failed")
    end
    return true
end