""" Diagrams of a given shape.
"""
module ShapeDiagrams
export AbstractFunctor, AbstractLaxator, LaxMonoidalFunctor,
  Span, Cone, Cospan, DecoratedCospan, Cocone, Diagram, left, right, leg, nlegs, apex, base,
  decorator, decoration, undecorate

using ...Theories: dom, codom

abstract type AbstractFunctor end
abstract type AbstractLaxator end

struct LaxMonoidalFunctor{Ftr <: AbstractFunctor, Lxr <: AbstractLaxator} <: AbstractFunctor
  F::Ftr
  L::Lxr
end

""" Span of morphisms in a category.
"""
struct Span{Left,Right}
  left::Left
  right::Right

  function Span(left::Left, right::Right; strict::Bool=true) where {Left,Right}
    if strict && dom(left) != dom(right)
      error("Domains of legs in span do not match: $left vs $right")
    end
    new{Left,Right}(left, right)
  end
end

apex(span::Span) = dom(span.left) # == dom(span.right)
left(span::Span) = span.left
right(span::Span) = span.right

""" Cone of morphisms in a category.
"""
struct Cone{S,T}
  apex::S # We store this separately because legs might be empty
  legs::Vector{T}

  function Cone(apex::S,legs::Vector{T}, strict::Bool=true) where {S,T}
    if strict && !all(dom(leg) == apex for leg in legs)
      error("Domain of legs in cone do not match")
    end
    new{S,T}(apex,legs)
  end
end

apex(cone::Cone) = cone.apex
leg(cone::Cone,n) = cone.legs[n]
legs(cone::Cone) = cone.legs
nlegs(cone::Cone) = length(cone.legs)

""" Cospan of morphisms in a category.
"""
struct Cospan{Left,Right}
  left::Left
  right::Right

  function Cospan(left::Left, right::Right; strict::Bool=true) where {Left,Right}
    if strict && codom(left) != codom(right)
      error("Codomains of legs in cospan do not match: $left vs $right")
    end
    new{Left,Right}(left, right)
  end
end

base(cospan::Cospan) = codom(cospan.left) # == codom(cospan.right)
left(cospan::Cospan) = cospan.left
right(cospan::Cospan) = cospan.right

""" Decorate Cospan of morphisms for representing open networks.
"""
struct DecoratedCospan{Decorator <: AbstractFunctor,Decoration}
  cospan::Cospan
  decorator::Decorator
  decoration::Decoration
end

decorator(m::DecoratedCospan) = m.decorator
decoration(m::DecoratedCospan) = m.decoration
undecorate(m::DecoratedCospan) = m.cospan
base(m::DecoratedCospan) = base(m.cospan)
left(m::DecoratedCospan) = left(m.cospan)
right(m::DecoratedCospan) = right(m.cospan)

""" Cocone of morphisms in a category.
"""
struct Cocone{S,T}
  base::S # We store this separately because legs might be empty
  legs::Vector{T}

  function Cocone(base::S,legs::Vector{T}, strict::Bool=true) where {S,T}
    if strict && !all(codom(leg) == base for leg in legs)
      error("Codomain of legs in cocone do not match")
    end
    new{S,T}(base,legs)
  end
end

base(cocone::Cocone) = cocone.base
leg(cocone::Cocone,n) = cocone.legs[n]
nlegs(cocone::Cocone) = length(cocone.legs)

struct Diagram{Ob,Hom}
  obs::Vector{Ob}
  homs::Vector{Tuple{Int64,Int64,Hom}}

  function Diagram(obs::Vector{Ob}, homs::Vector{Tuple{Int64,Int64,Hom}}) where {Ob, Hom}
    for (s,t,f) in homs
      @assert obs[s] == dom(f)
      @assert obs[t] == codom(f)
    end
    new{Ob,Hom}(obs,homs)
  end
end

end


# The SteinMaternRadialKernel is given by
#
# k(x, y) = (1 + gamma * r) * exp{-gamma * r}
#
# where r = ||x-y||. This is a norm for the order
# 2 Sobolev space H_2. This is the Matern covariance
# associated with v = 3/2.
#
# See http://www.gaussianprocess.org/gpml/chapters/RW4.pdf
# or http://arxiv.org/pdf/1204.6448.pdf for more details.

mutable struct SteinMaternRadialKernel <: SteinMaternWeightedKernel
    gamma::Float64
end

# default is use gamma sqrt{3}
SteinMaternRadialKernel() = SteinMaternRadialKernel(sqrt(3.0))

function Q(ker::SteinMaternRadialKernel, x::Array{Float64,1}, y::Array{Float64,1})
    1.0
end

function gradxQ(ker::SteinMaternRadialKernel, x::Array{Float64,1}, y::Array{Float64,1})
    d = length(x)
    zeros(d)
end

function gradxyQ(ker::SteinMaternRadialKernel, x::Array{Float64,1}, y::Array{Float64,1})
    0.0
end


# using DataStructures

function readinput(filename)
  lines = readlines(filename)
  M = zeros(Int, length(lines),length(lines[1]))
  for (i,l) in enumerate(lines)
    for (j,c) in enumerate(split(l,""))
      M[i,j] = parse(Int,String(c))
    end
  end
  M
end

function expanded_vals(n)
  S = [
    0 1 2 3 4
    1 2 3 4 5
    2 3 4 5 6
    3 4 5 6 7
    4 5 6 7 8
  ]
  N = zeros(Int, 5, 5)
  fill!(N,n)
  (x -> x>9 ? x-9 : x).(N + S)
end

function expandedmat(M)
  H = size(M,1)
  W = size(M,2)
  M2 = zeros(Int, H*5, W*5)
  for x in 1:H, y in 1:W
    M2[x:H:end,y:W:end] = expanded_vals(M[x,y])
  end
  M2
end

function cheapestpath(M)
  q = [(0,1,1)]
  costs = Dict()
  while true
    cost,x,y = q[1]
    if x==size(M,1) && y==size(M,2)
      return cost
      break
    end
    popfirst!(q)
    for (dx,dy) ∈ [(-1,0), (1,0), (0,1), (0,-1)]
      if x+dx in 1:size(M,1) && y+dy in 1:size(M,2)
        ncost = cost + M[x+dx,y+dy]
        if (x+dx,y+dy) ∈ keys(costs) && costs[(x+dx,y+dy)] <= ncost
          continue
        end
        costs[(x+dx,y+dy)] = ncost
        push!(q,(ncost,x+dx,y+dy))
      end
    end
    q = sort(q)
  end
end

function cheapestpath2(M,scalar)
  H = size(M,1)
  W = size(M,2)
  q = [(0,1,1)]
  costs = Dict()
  while true
    cost,x,y = q[1]
    if x==H*scalar && y==W*scalar
      return cost
    end
    popfirst!(q)
    # val = Msmall[mod(x,size(Msmall,1)),mod(y,size(Msmall,2))] + trunc(Int,x/size(Msmall,1)) + trunc(Int,y/size(Msmall,2))
    @show mod(x,)
    for (dx,dy) ∈ [(-1,0), (1,0), (0,1), (0,-1)]
      if x+dx in 1:H*scalar && y+dy in 1:W*scalar
        ncost = cost + M[x+dx,y+dy]
        if (x+dx,y+dy) ∈ keys(costs) && costs[(x+dx,y+dy)] <= ncost
          continue
        end
        costs[(x+dx,y+dy)] = ncost
        push!(q,(ncost,x+dx,y+dy))
      end
    end
    q = sort(q)
  end
end

M = readinput("inputs/11-15/15ex.txt")
# @show cheapestpath(M)

M2 = expandedmat(M)
# @show cheapestpath2(M2,M)
@show cheapestpath2(M,1)

using BenchmarkTools
# @btime cheapestpath(M)


using Test
using LinearAlgebra: norm

using ConjugateGradientMethod

@testset "Conjugate Gradient Method" begin
    for i in 1:10
        n = rand(1:50)
        # Generates a random symmetric positive definite matrix
        A = begin
            A = rand(Float64, n, n)
            A * A'
        end
        b = rand(Float64, n)

        x = A \ b
        @test conjugate_gradient(A, b) ≈ x
        @test conjugate_directions(A, b) ≈ x
    end
end


##

using FreeTypeAbstraction
using AprilTags
using Images, ImageView
using FileIO
using Optim

using ImageDraw, Colors, FixedPointNumbers

##

calibfiles = [
  ENV["HOME"]*"/data/cameracalib/CommonData/IMG_2909.jpg";
  ENV["HOME"]*"/data/cameracalib/CommonData/IMG_2910.jpg";
  ENV["HOME"]*"/data/cameracalib/CommonData/IMG_2911.jpg";
  ENV["HOME"]*"/data/cameracalib/CommonData/IMG_2913.jpg";
  ENV["HOME"]*"/data/cameracalib/CommonData/IMG_2914.jpg";
  ENV["HOME"]*"/data/cameracalib/CommonData/IMG_2916.jpg";
  ENV["HOME"]*"/data/cameracalib/CommonData/IMG_2920.jpg";
]

##

INITTEST = load(calibfiles[1])

f_width  = size(INITTEST,1) + 0.0
f_height = f_width  + 0.0
c_width  = size(INITTEST,2) / 2 + 0.0
c_height = size(INITTEST,1) / 2 + 0.0

s = 0.0

taglength = 0.03

##

arr = Vector{typeof(INITTEST)}()

for i in 1:length(calibfiles)
  push!(arr, load(calibfiles[i]))
end

## quick test

detector = AprilTagDetector()
tags = detector.(arr)

#

ARR = []

for tag in tags
  push!(ARR, [(;id=tg.id,p=tg.p, H=tg.H) for tg in tag])
end

##

resid = calcCalibResidualAprilTags!( arr, ARR, f_height=f_height, taglength=taglength )

##

obj = (f_w, f_h, c_w, c_h) -> calcCalibResidualAprilTags!(arr, ARR,
                                                          # tagList=40:40;
                                                          f_width=f_w,
                                                          f_height=f_h,
                                                          c_width=c_w,
                                                          c_height=c_h,
                                                          taglength=taglength )

obj_ = (fc_wh) -> obj(fc_wh...)

## current best guess

f_width_  = f_width
f_height_ = f_height
c_width_  = c_width
c_height_ = c_height

##

# check that it works
obj_([f_width_, f_height_, c_width_, c_height_])

##

result = optimize(obj_, [f_width_; f_height_; c_width_; c_height_ ], BFGS(), Optim.Options(iterations = 100, show_trace=true, x_tol=1e-8))

##

f_width_  = result.minimizer[1]
f_height_ = result.minimizer[2]
c_width_  = result.minimizer[3]
c_height_ = result.minimizer[4]

##

# iPhone 8 rear camera (coarse calibration)
# f_height_ = 3346.1894
# f_width_ = 3346.1894
# c_height_ = 2021.11068
# c_width_ = 1471.0241

# f_height_ = 3371.2553294118493
# f_width_ = 3353.696574041437
# c_height_ = 2007.7796750349364
# c_width_ = 1496.4523912712611

# f_height_ = 3370.4878918701756
# f_width_  = 3352.8348099534364
# c_height_ = 2005.641610450976
# c_width_  = 1494.8282013012076

# f_height_ = 3431.554669353193
# f_width_  = 3411.8526620805414
# c_height_ = 2016.7496065830883
# c_width_  = 1501.0475003688337

minim = obj(f_width_, f_height_, c_width_, c_height_)

## draw what is going on

SEL = 4

cimg_ = deepcopy(arr[SEL])
calcCornerProjectionsAprilTags!(cimg_, ARR[SEL],
                                # tagList=18:18,
                                taglength=0.0315,
                                f_width=f_width_,
                                f_height=f_height_,
                                c_width=c_width_,
                                c_height=c_height_,
                                dodraw=true )
#

##

ImageView.imshow(cimg_)

##

freeDetector!(detector)

##


function solve()
    n = parse(Int, readline())
    as = zeros(Int, n)
    bs = zeros(Int, n)
    for i=1:n
        a, b = [parse(Int, x) for x in split(readline())]
        as[i] = a
        bs[i] = b
    end
    sort!(as)
    sort!(bs)
    if n % 2 == 0
        m = n ÷ 2
        ma = (as[m] + as[m+1])
        mb = (bs[m] + bs[m+1])
        mb - ma + 1
    else
        m = (n+1) ÷ 2
        ma = as[m]
        mb = bs[m]
        mb - ma + 1
    end
end

println(solve())


# this file contains code for the simulated annealing TSP heuristic

"""
.. simulated_annealing(distmat; ...) ..

Use a simulated annealing strategy to return a closed tour. The temperature
decays exponentially from init_temp to final_temp.

Optional arguments:

- steps: number of steps to take; defaults to 50n^2 where n is number of cities

- num_starts: number of times to run the simulated annealing algorithm, each time
	starting with a random path. Defaults to 1.

- init_temp: initial temperature which controls initial chance of accepting an
	inferior tour.

- final_temp: final temperature which controls final chance of accepting an
	inferior tour; lower values roughly correspond to a longer period of 2-opt.

- init_path: path to start the annealing from. A Nullable{Vector{Int}}. An empty
	Nullable corresponds to picking a random path; if the Nullable contains a value
	then this path will be used. Defaults to a random path.
"""
function simulated_annealing{T <: Real}(distmat::Matrix{T}; steps = 50*length(distmat),
										num_starts = 1,
										init_temp = exp(8), final_temp = exp(-6.5),
										init_path::Nullable{Vector{Int}} = Nullable{Vector{Int}}())

	# check inputs
	check_square(distmat, "Must pass a square distance matrix to simulated_annealing.")

	# cooling rate: we multiply by a constant mult each step
	cool_rate = (final_temp / init_temp)^(1 / (steps - 1))

	# do SA with a single starting location
	function sahelper()
		temp = init_temp / cool_rate # divide by cool_rate so when we first multiply we get init_temp
		n = size(distmat, 1)
		path = isnull(init_path) ? randpath(n) : copy(get(init_path))
		cost_cur = pathcost(distmat, path)

		for i in 1:steps
			temp *= cool_rate

			# take a step
			# keep first and last cities fixed
			first, last = rand(2:n), rand(2:n)
			if first > last
				first, last = last, first
			end
			cost_other = pathcost_rev(distmat, path, first, last)
			@fastmath accept = cost_other < cost_cur ? true : rand() < exp((cost_cur - cost_other) / temp)
			# should we accept?
			if accept
				reverse!(path, first, last)
				cost_cur = cost_other
			end
		end

		return path, cost_cur
	end

	path, cost = sahelper()
	for _ in 2:num_starts
		otherpath, othercost = sahelper()
		if othercost < cost
			cost = othercost
			path = otherpath
		end
	end

	return path, cost
end

function randpath(n)
	path = 1:n |> collect |> shuffle
	push!(path, path[1]) # loop
	return path
end


using Test

function Λ1(x, T)
    return (T-x)/T
end

#add basis for the final value
function Λ2(x,T)
    return x/T
end

function Λ(x, T)
    return max(0.0, sqrt(T)/2 - abs(x/sqrt(T) - sqrt(T)/2))
end
#T = 3
#plot([Λ(x, T) for x=0:0.001:T])

function Λ(x, l, k, T)
    x = mod(x, T)   #ok
    2^(-l/2)*Λ((x)*(2)^(l) - k*T, T)
end

function c(k::Fs, ξ, t, u, v, T)
    c = 0.0
    for n in k.nhb
        i,j = Faber(n)
        c += Λ(t, i, j, T)*ξ[n]
    end
    return c + Λ1(t, T)*u + Λ2(t, T)*v
end

L = 1
T = 10.0
ϕ = Fs(1, L, T)
ξ = fill(1.0, 2<<L - 1)
u = -1.0
v = -1.0

S = System(L, T, ξ)

fs_expansion(S, t, u, v)
c(ϕ, ξ, t, u, v, T)
for k in S.ϕ
    @test MCintegration(k) < k.ub
    @test MCintegration(k) > k.lb
    @test k.lb + range = k.ub
end

fs_expansion()
fs_expansion(ϕ, ξ, u, v, L, T)

for i
function MCintegration(k::Fs)
end

using Test
for i in 1:2^20
    @test i >= 1 << binlog(i)
    @test i < 2 << binlog(i)
    @test binlog(i) == Faber(i)[1]
end

function Λ(x, l, k, T)
   x = mod(x, T)   #ok
   2^(-l/2)*Λ((x)*1<<(l) -T*(k),T)
end

using Plots

T=1
dt= 0:0.0001:T
plot(dt, [Λ(t, 2, 0, T) for t in dt])

function num_int(i1,j1,i2,j2, T, dt)
    t = 0:dt:T
    r = 0
    for i in t[2:end]
        r += Λ(i + dt/2, i1, j1, T)*Λ(i + dt/2, i2, j2, T)*dt
    end
    return r
end

function num_int(i1,j1, T, dt)
    t = 0:dt:T
    r = 0
    for i in t[2:end]
        r += Λ(i + dt/2, i1, j1, T)*dt
    end
    return r
end

function num_int_bound1(n, T, dt)
    i1,j1 = Faber(n)
    t = 0:dt:T
    r = 0.0
    for i in t[2:end]
        r += Λ(i + dt/2, i1, j1, T)*(i + dt/2)/T*dt
    end
    return r
end

function num_int_bound2(n, T, dt)
    i1,j1 = Faber(n)
    t = 0:dt:T
    r = 0.0
    for i in t[2:end]
        r += Λ(i + dt/2, i1, j1, T)*(1 -(i + dt/2)/T)*dt
    end
    return r
end

num_int(0, 0, 2, 2 , 100, 0.00001)
num_int(0, 0 , 10, 0.00001)

generate_matrix(2, 10)
generate_vector(2, 10)

Λ(2.5, 1, 0, 1)
sqrt(10)*2^-(1/2 + 1)

bound2(3, 100)
num_int_bound2(3, 100.0, 0.001)


####
#### Chebyshev on [-1,1]
####

@testset "Chebyshev" begin
    @test_throws ArgumentError Chebyshev(InteriorGrid(), 0)
    @test_throws ArgumentError Chebyshev(EndpointGrid(), 1)

    for grid_kind in (InteriorGrid(), EndpointGrid())
        for N in (grid_kind ≡ InteriorGrid() ? 1 : 2):10

            basis = Chebyshev(grid_kind, N)
            @test is_function_basis(basis)
            @test is_function_basis(typeof(basis))
            @test dimension(basis) == N

            for i in 1:100
                x = rand_in_domain(i, -1, 1)
                bx = @inferred basis_at(basis, x)

                @test length(bx) == N
                @test eltype(bx) == Float64
                @test collect(bx) ≈ [chebyshev_cos(x, i) for i in 1:N]

                θ = rand(N)
                @test linear_combination(basis, θ, x) ≈
                    sum(chebyshev_cos(x, i) * θ for (i,θ) in enumerate(θ))
                @test derivative(linear_combination(basis, θ), x) ≈
                    sum(chebyshev_cos_deriv(x, i) * θ for (i,θ) in enumerate(θ))
            end

            g = @inferred grid(basis)
            @test length(g) == N
            if grid_kind ≡ InteriorGrid()
                @test all(x -> isapprox(chebyshev_cos(x, N + 1), 0, atol = 1e-14), g)
            else
                @test all(x -> isapprox(chebyshev_cos_deriv(x, N), 0, atol = 1e-13),
                          g[2:(end-1)])
                @test g[1] == -1
                @test g[end] == 1
            end
        end
    end
end


"""
    function dinic_impl(residual_graph, source, target, capacity_matrix)

Compute the maximum flow between the `source` and `target` for `residual_graph`
with edge flow capacities in `capacity_matrix` using
[Dinic\'s Algorithm](https://en.wikipedia.org/wiki/Dinic%27s_algorithm).
Return the value of the maximum flow as well as the final flow matrix.
"""
function dinic_impl end
@traitfn function dinic_impl(
        residual_graph::::lg.IsDirected,               # the input graph
        source::Integer,                       # the source vertex
        target::Integer,                       # the target vertex
        capacity_matrix::AbstractMatrix{T}    # edge flow capacities
    ) where {T}
    n = lg.nv(residual_graph)                     # number of vertexes
    flow_matrix = zeros(T, n, n)           # initialize flow matrix
    P = zeros(Int, n)                      # Sharable parent vector

    flow = 0

    while true
        augment = blocking_flow!(residual_graph, source, target, capacity_matrix, flow_matrix, P)
        augment == 0 && break
        flow += augment
    end
    return flow, flow_matrix
end

"""
    blocking_flow!(residual_graph, source, target, capacity_matrix, flow-matrix, P)

Like `blocking_flow`, but requires a preallocated parent vector `P`.
"""
function blocking_flow! end
@traitfn function blocking_flow!(
        residual_graph::::lg.IsDirected,     # the input graph
        source::Integer,                     # the source vertex
        target::Integer,                     # the target vertex
        capacity_matrix::AbstractMatrix{T},  # edge flow capacities
        flow_matrix::AbstractMatrix,         # the current flow matrix
        P::AbstractVector{Int}               # Parent vector to store Level Graph
    ) where {T}
    n = lg.nv(residual_graph)                # number of vertexes
    fill!(P, -1)
    P[source] = -2

    Q = [source]
    sizehint!(Q, n)

    while length(Q) > 0                   # Construct the Level Graph using BFS
        u = pop!(Q)
        for v in lg.outneighbors(residual_graph, u)
            if P[v] == -1 && capacity_matrix[u, v] > flow_matrix[u, v]
                P[v] = u
                pushfirst!(Q, v)
            end
        end
    end

    P[target] == -1 && return 0                    # BFS couldn't reach the target

    total_flow = 0

    for bv in lg.inneighbors(residual_graph, target)    # Trace all possible routes to source
        flow = typemax(T)
        v = target
        u = bv
        while v != source
            if u == -1                    # Vertex unreachable from source
                flow = 0
                break
            else
                flow = min(flow, capacity_matrix[u, v] - flow_matrix[u, v])
                v = u
                u = P[u]
            end
        end

        flow == 0 && continue                      # Flow cannot be augmented along path

        v = target
        u = bv
        while v != source             # Augment flow along path
            flow_matrix[u, v] += flow
            flow_matrix[v, u] -= flow
            v = u
            u = P[u]
        end

        total_flow += flow
    end
    return total_flow
end

"""
    blocking_flow(residual_graph, source, target, capacity_matrix, flow-matrix)

Use BFS to identify a blocking flow in the `residual_graph` with current flow
matrix `flow_matrix`and then backtrack from `target` to `source`,
augmenting flow along all possible paths.
"""
blocking_flow(
    residual_graph::lg.AbstractGraph,  # the input graph
    source::Integer,                   # the source vertex
    target::Integer,                   # the target vertex
    capacity_matrix::AbstractMatrix,   # edge flow capacities
    flow_matrix::AbstractMatrix,       # the current flow matrix
    ) = blocking_flow!(
            residual_graph,
            source,
            target,
            capacity_matrix,
            flow_matrix,
            zeros(Int, lg.nv(residual_graph)))


module SSGE

using LinearAlgebra
using ForwardDiff
using Distances
using Statistics

include("CovKer.jl")
include("Nystroem.jl")

"""
    SSGEstimator(x_samples, num_eig_func, cov_kernel)

Stores the eigenfunctions `ψ` and the projection matrix `β`, with which
the Stein gradient can be approximated. These are constructed from the points
sampled `x_samples` with `num_eig_func` eigenfunctions regarding the
the covariance kernel `cov_kernel`.
The procedure is described in

Shi, Jiaxin and Sun, Shengyang and Zhu, Jun,
*A Spectral Approach to Gradient Estimation for Implicit Distributions*,
Proceedings of the 35th International Conference on Machine Learning,
4651--4660,
2018

See also: [`(ssge::SSGEstimator)(x)`](@ref)
"""
struct SSGEstimator{F <: Real, V <: AbstractVector{F}, M <: AbstractMatrix{F}, C <: CovKer}
    ψ::Nystroem{F, V, M, C}
    β::M
end

function SSGEstimator(x_samples::M, num_eig_func::Integer, cov_kernel::C) where {
        F <: Real, M <: AbstractMatrix{F}, C <: CovKer}
    ψ = Nystroem(x_samples, num_eig_func, cov_kernel)

    ∇ψ(x) = ForwardDiff.jacobian(ψ, x)

    β =  ∇ψ(x_samples[:,1])
    for i in 2:ψ.num_samples
       β = β + ∇ψ(x_samples[:,i])
    end
    β = -1/ψ.num_samples .* β

    SSGEstimator{F, typeof(ψ.eig_val_inv), M, C}(ψ, β)
end

function SSGEstimator(x_samples::M, r_bar::F) where {
    F <: Real, M <: AbstractMatrix{F}}
    ψ = Nystroem(x_samples, r_bar)

    ∇ψ(x) = ForwardDiff.jacobian(ψ, x)

    β =  ∇ψ(x_samples[:,1])
    for i in 2:ψ.num_samples
    β = β + ∇ψ(x_samples[:,i])
    end
    β = -1/ψ.num_samples .* β

    SSGEstimator{F, typeof(ψ.eig_val_inv), M, typeof(ψ.cov_kernel)}(ψ, β)
end

"""
    (ssge::SSGEstimator)(x)

Estimates the corresponding Stein gradient of `ssge` at point `x`.
"""
(ssge::SSGEstimator)(x::V) where{V <: AbstractVector} =
    (ssge.ψ(x)' * ssge.β)'

end # module


import jInv.ForwardShare.getSensTMatVec

function getSensTMatVec(v::Vector,m::Vector,pFor::BasicFWIparam)

    # extract pointers
    Mesh  = pFor.Mesh
    omega = pFor.omega
    gamma = pFor.gamma
    Q     = pFor.Sources
    P     = pFor.Receivers
    LU    = pFor.Ainv
    U     = pFor.Fields

    nsrc = size(Q,2); nfreq = length(omega); nrec = size(P,2)

    # reshape v by receivers x sources x frequencies
    v    = reshape(v,nrec,nsrc,nfreq)

    # allocate space for result
    JTv  = zeros(length(m))
    # derivative of mass matrix
    An2cc = getNodalAverageMatrix(Mesh)
    dM    = repeat(1 .- 1im*vec(gamma),1,nsrc)
    for i=1:nfreq
        tf1=time_ns();
        Lam  = LU[i]\(P*v[:,:,i])
        # println(size(An2cc))
        # println(size(Lam))
        # println(size(U[:,:,i]))
        # println(size(dM))
        # println(size(omega[i]))
        JTvi =  omega[i]^2*dM.*(An2cc*(U[:,:,i].*Lam))
        JTv +=  sum(real(JTvi),dims=2)
        tf2=time_ns();
        # println("Runtime of sensT freq:");
        # println((tf1 - tf2)/1.0e9);
    end
    return vec(JTv)
end

function getSensTMatVecZs(v::Vector,m::Vector,pFor::BasicFWIparam)

        # extract pointers
        Mesh  = pFor.Mesh
        omega = pFor.omega
        gamma = pFor.gamma
        Q     = pFor.Sources
        P     = pFor.Receivers
        LU    = pFor.Ainv
        U     = pFor.Fields

        nsrc = size(Q,2); nfreq = length(omega); nrec = size(P,2)

        # reshape v by receivers x sources x frequencies
        v    = reshape(v,nrec,nsrc,nfreq)

        # allocate space for result
        JTv  = zeros(length(m))
        # derivative of mass matrix
        An2cc = getNodalAverageMatrix(Mesh)
        dM    = repeat(1 .- 1im*vec(gamma),1,1)
        for i=1:nfreq
            tf1 = time_ns();
            for s=1:nsrc
                t1 = time_ns();
                Lam  = LU[i]\(P*v[:,s,i])
                JTv += omega[i]^2*dM.*(An2cc*((LU[i]\Vector(Q[:,s])).*Lam))[:,1];
                t2 = time_ns();
                # println("Runtime of sensT iteration:");
                # println((t1 - t2)/1.0e9);
            end
            tf2=time_ns();
            # println("Runtime of sensT freq:");
            # println((tf1 - tf2)/1.0e9);

        end
        return vec(JTv)
end


exponentialsample(λ) = -log(rand()) / λ

function denseperiodic(f, dur, fs; magnitude=20.0)
	N_pat = Int(round(dur * fs))
	dn_pat = Int(round(fs/f))
	train = zeros(N_pat)
	idx = 1
	while idx <= N_pat
		train[idx] = magnitude
		idx += dn_pat
	end
	train
end

function sparseperiodic(f, dur)
	numspikes = Int(floor(dur*f))
	dt = 1.0/f
	[dt*(k-1) for k ∈ 1:numspikes]
end

function sparsepoisson(λ, dur)
	ts = Array{Float64, 1}(undef, 0)
	t = 0.0
	while t < dur
		push!(ts, t)
		t += exponentialsample(λ)
	end
	ts
end

function sparserefractorypoisson(λ, dur, deaddur)
	ts = Array{Float64, 1}(undef, 0)
	t = 0.0
	while t < dur
		push!(ts, t)
		sample = exponentialsample(λ)
		while sample < deaddur
			sample = exponentialsample(λ)
		end
		t += sample
	end
	ts
end

function densepoisson(λ, dur, fs; magnitude=20.0)
	ts = sparsepoisson(λ, dur)
	output = zeros(Int64(round(dur*fs))+1)
	for t ∈ ts
		output[Int64(round(t*fs))+1] = magnitude
	end
	output
end

function channeldup(times::Array{T,1}, channels) where T <: Number
	numspikes = length(times)
	numchan = length(channels)
	spikes = Array{Spike,1}(undef,numspikes*numchan)
	count = 1
	for i ∈ 1:numspikes
		for channel ∈ channels
			spikes[count] = Spike(channel, times[i])
			count += 1
		end
	end
	spikes
end

function channelscatter(times::Array{T,1}, available_channels) where T <: Number
	n = length(times)
	spikes = Array{Spike, 1}(undef, n)
	if length(available_channels) > n
		channels = shuffle(available_channels)[1:n]
		for i ∈ 1:n
			spikes[i] = Spike(channels[i], times[i])
		end
	else
		for i ∈ 1:n
			spikes[i] = Spike(rand(available_channels), times[i])
		end
	end
	spikes
end

function stochasticblock(times::Array{T,1}, p, channels) where T <: Number
	n = length(times)
	spikes = Array{Spike,1}(undef,0)
	for time ∈ times
		for channel ∈ channels
			if rand() < p
				push!(spikes, Spike(channel, time))
			end
		end
	end
	spikes
end

function genrandinput(λ, dur, nchannels)
	times = sparsepoisson(λ, dur)
	channelscatter(times, 1:nchannels)
end

function gendupinput(λ, dur, channels)
	times = sparsepoisson(λ, dur)
	channeldup(times, channels)
end


# Setup plotting environment
using Pkg;
Pkg.add("Plots");
using Plots;
Pkg.add("PyPlot");
Pkg.add("PyCall");
Pkg.add("LaTeXStrings");
pyplot();

# Define a Gaussian shaped function
# Note - we put loop inside function so it can operate on Array object and
# so it can execute efficiently. Calling function for each element of Array
# is more expensive.
ndist(x,μ,σ)=[1/√(2*π*σ^2)*exp(-((xx-μ)^2)/(2*σ^2)) for xx in x]

# Define a list of values and apply Gaussian function to them, showing timing.
x=collect(-10:0.1:10);
μ=0.;σ=π;@time yy=ndist(x,μ,σ)
plot(x,yy)

Pkg.add("Statistics")
using Statistics;
print(Statistics.std(yy))

Pkg.add("InteractiveUtils")
using InteractiveUtils;
@code_native ndist(x,μ,σ);


"""imaginary time evolution solution to maximum cut problem."""
using Yao

@const_gate ZZ = mat(kron(Z,Z))

function maxcut_circuit(W::AbstractMatrix, τ)
    nbit = size(W, 1)
    ab = chain(nbit)
    for i=1:nbit,j=i+1:nbit
        if W[i,j] != 0
            #push!(ab, put(nbit, (i,j)=>0.5*W[i,j]*ZZ))
            push!(ab, put(nbit, (i,j)=>rot(ZZ, -im*W[i,j]*τ)))
        end
    end
	return ab
end

function maxcut_h(W::AbstractMatrix)
    nbit = size(W, 1)
    ab = Add{nbit}()
    for i=1:nbit,j=i+1:nbit
        if W[i,j] != 0
            #push!(ab, put(nbit, (i,j)=>0.5*W[i,j]*ZZ))
            push!(ab, put(nbit, (i,j)=>0.5*W[i,j]*ZZ))
        end
    end
	return ab
end

using Random, Test
@testset "iqaoa circuit" begin
	Random.seed!(2)
	nbit = 5
	# exact solution is [(1,3,4), (2,5)]
	W = [0 5 2 1 0;
		 5 0 3 2 0;
		 2 3 0 0 0;
		 1 2 0 0 4;
		 0 0 0 4 0]

	c = maxcut_circuit(W, 10)
	h = maxcut_h(W)
	reg = uniform_state(nbit) |> c

	# check the correctness of result
	opt_pl = reg |> probs
	config = argmax(opt_pl)-1
	@show BitStr64{nbit}(config), opt_pl[config+1]
	@test config == bit"01101" || config == bit"10010"
	@test isapprox(expect(h, reg |> normalize!), -5.5)
end


using Statistics: std #We have some problem in the calculation of the std

function _discrete_fourier_transform(series, word_length, norm, inverse_sqrt_win_size, normalise)
    _length = length(series)
    output_length = ceil(Int, word_length/2)
    start = norm ? 1 : 0

    _std = 1

    if normalise
        s = std(series)  # some issue with std statement ??
        if s!=0
            _std = s
        end
    end

    dft = [ sum( [ [  series[n] * cos(2 * π * (n-1) * i / _length),
                     -series[n] * sin(2 * π * (n-1) * i / _length) ]
                                                   for n=1:_length ] ) # sum end
                                                   for i = start:(start + output_length-1) ] #end of dft

    dft = vcat(dft...)

    if normalise
        dft = dft * (inverse_sqrt_win_size/ _std)
    end
    return dft
end

# self._create_word(), self._add_to_pyramid(),  self.levels,  self._add_to_bag(), self.bigrams,
# self.save_words, self.words.append, self.window_size, self.word_length,

function transform(self, X, y=None)
    # check_is_fitted()
    # X = check_X(X, enforce_univariate=True)
    # X = tabularize(X, return_array=True)
    # bags = pd.DataFrame()
    # dim = []
    for i in range(X.shape[0])
        dfts = _mft(X[i, :])
        bag = Dict() # {} This is how they use the ditionary in the python
        last_word = -1
        repeat_words = 0
        words = []
        for window in range(dfts.shape[0])  # use size(dfts, 1)
            word = _create_word(dfts[window])
            append!(words, word)
            # repeat_word = (_add_to_pyramid(bag, word, last_word,      # adding aproproate if eles statement
            #                                     window -
            #                                     Int(repeat_words/2))
            #                if levels > 1 else
            #                _add_to_bag(bag, word, last_word))
            if repeat_word
                repeat_words += 1
            else
                last_word = word.word
                repeat_words = 0
            end
            if bigrams
                if ((window - window_size) >= 0) && (window > 0)
                    # bigram = words[window - window_size]\  # Adding the biagram finding its library
                    #    .create_bigram(word, word_length)
                    if levels > 1
                        bigram = (bigram, 0)
                    end
                    bag[bigram] = bag.get(bigram, 0) + 1
                end
            end
        if save_words
            words.append(words)
        end
    end
        dim.append(pd.Series(bag))
    bags[0] = dim
    end
end

function _mft(series, word_length, window_size, norm, inverse_sqrt_win_size, series_length )
    start_offset = norm ? 2 : 0
    lengthl = (word_length + word_length) ÷ 2

    phis = [  [cos(2 * π * (-((i * 2) + start_offset) / 2) / window_size),
               -sin(2 * π * (-((i * 2) + start_offset) / 2) / window_size) ]
                                          for i = 0:((lengthl÷2)-1) ] #end of dft
    phis = vcat(phis...)
    endl = max(1, series_length - window_size + 1)
    stds = _calc_incremental_mean_std(series, endl, window_size)
    transformed = zeros(Float64, endl, lengthl)
    mft_data = []  # Find way to do catching
    @inbounds for i = 0:endl-1
        if i > 0
            @inbounds for n = 1:2:lengthl
                real = mft_data[n] + series[i + window_size] - series[i]
                imag = mft_data[n + 1]
                mft_data[n] = real * phis[n] - imag * phis[n + 1]
                mft_data[n + 1] = real * phis[n + 1] + phis[n] * imag
            end
        else
            mft_data = _discrete_fourier_transform(series[1:window_size], word_length,
                                               norm, inverse_sqrt_win_size, false) #normalise=false
        end
        normalising_factor = ( (1 / ( stds[i + 1] > 0 ? stds[i + 1] : 1) ) * inverse_sqrt_win_size)
        transformed[i + 1, :] = mft_data * normalising_factor
    end
    return transformed
end

function _calc_incremental_mean_std(series, endl, window_size)
    means = zeros(endl)
    stds = zeros(endl)
    window = series[1:window_size]
    series_sum = sum(window)
    square_sum = sum(window.*window)

    r_window_length = 1 / window_size
    means[1] = series_sum * r_window_length
    buf = square_sum * r_window_length - means[1]^2
    stds[1] = buf > 0 ? sqrt(buf) : 0

    @inbounds for w =1:(endl-1)
        series_sum += series[w + window_size] - series[w]
        means[w + 1] = series_sum * r_window_length
        square_sum += series[w + window_size]^2 - series[w]^2
        buf = square_sum * r_window_length - means[w + 1]^2
        stds[w + 1] = buf > 0 ? sqrt(buf) : 0
    end
    return stds
end
# self.series_length  self.window_size, self.n_instances, self.alphabet_size,  self.word_length

function _mcb(X, window_size, alphabet_size, word_length, norm, inverse_sqrt_win_size, normalise)
    n_instances, series_length = size(X)
    num_windows_per_inst = ceil(series_length / window_size)
    dft = zeros(Float64, n_instances, num_windows_per_inst, word_length)
    @inbounds for k=1:n_instances
        @inbounds for i=1:num_windows_per_inst-1
            dft[k, i, :] = _discrete_fourier_transform(view(X, k, ((i-1)*window_size + 1):(i*window_size)),
                                                word_length, norm, inverse_sqrt_win_size, normalise)
        end
        dft[k, end, :] = _discrete_fourier_transform(view(X, k, (series_length-window_size + 1):series_length),
                                                word_length, norm, inverse_sqrt_win_size, normalise)
    end
    total_num_windows = n_instances * num_windows_per_inst
    breakpoints = zeros(Float64, word_length, alphabet_size)
    @inbounds for letter in range(word_length)                        # sortring algo, making a array and finding the aproproate ramge to place
        sort!(dft[((letter-1)*total_num_windows + 1):(letter*total_num_windows)])
        bin_index = 0
        target_bin_depth = total_num_windows / alphabet_size
        @inbounds for bp in range(alphabet_size - 1)
            bin_index += target_bin_depth
            breakpoints[letter][bp] = dft[int(bin_index + (letter-1)*total_num_windows)]  # Check indexing here
        end
        breakpoints[letter][alphabet_size] = maxintfloat(Float64)   # check this out in the system
    return breakpoints
end

# self.window_size, self.window_size
#=
function _mcb_dft(series, num_windows_per_inst, window_size, series_length,
                        word_length, norm, inverse_sqrt_win_size, normalise)
    # Splits individual time series into windows and returns the DFT for
    # each
    Z = zeros(Float64, num_windows_per_inst, window_size)
    @inbounds for i=1:num_windows_per_inst-1
        Z[i, :] = _discrete_fourier_transform(series[((i-1)*window_size + 1):(i*window_size)],
                                            word_length, norm, inverse_sqrt_win_size, normalise)
    end
    Z[end, :] = _discrete_fourier_transform(series[ (series_length-window_size + 1):series_length],
                                            word_length, norm, inverse_sqrt_win_size, normalise)
    return Z # find the way to have two indiaces for he same loops @inoans may help
end
=#
# self.word_length, self.alphabet_size, self.breakpoints

function _create_word(self, dft)
    word = _BitWord()                       # check out this function
    for i in range(self.word_length)
        for bp in range(self.alphabet_size)
            if dft[i] <= self.breakpoints[i][bp]
                word.push(bp)
                break
            end
        end
    end
    return word
end

# self.remove_repeat_words

function _add_to_bag(self, bag, word, last_word)
    if self.remove_repeat_words && word.word == last_word
        return false
    end
    bag[word.word] = bag.get(word.word, 0) + 1   # check out this get fucntion
    return true
end

# self.levels, self.remove_repeat_words, self.window_size,  self.series_length, self.level_weights

function _add_to_pyramid(self, bag, word, last_word, window_ind)
    if self.remove_repeat_words && word.word == last_word  # check if python `and` and `&&` from the julia are same
        return false
    end
    start = 0
    for i in range(self.levels)
        num_quadrants = pow(2, i)                # Check out this fucntion
        quadrant_size = self.series_length / num_quadrants
        pos = window_ind + Int((self.window_size / 2))
        quadrant = start + (pos / quadrant_size)
        bag[(word.word, quadrant)] = (bag.get((word.word, quadrant), 0)
                                      + self.level_weights[i])
        start += num_quadrants
    end
    return True
end

function BOSSfit(X,  y, min_window, alphabet_size, word_length, norm, inverse_sqrt_win_size,
                 normalise, series_length, max_win_len_prop, max_ensemble_size)

    max_window_searches = series_length ÷ 4
    max_window = series_length * max_win_len_prop
    win_inc = (max_window - min_window) ÷ max_window_searches
    inverse_sqrt_win_size = 1 / sqrt(window_size)
    for normalise in [true, false]
        for win_size = min_window:win_inc:(max_window + 1)
            boss = _mcb(X, win_size, alphabet_size, word_length, norm, inverse_sqrt_win_size, normalise)
            _individual_train_acc(outputfrom_boss, y, )
end

function _train_predict(self, train_num)
    test_bag = self.transformed_data[train_num]
    best_dist = sys.float_info.max
    nn = None

    for n, bag in enumerate(self.transformed_data)
        if n == train_num
            continue
        end
        dist = boss_distance(test_bag, bag, best_dist)
        if dist < best_dist
            best_dist = dist
            nn = self.class_vals[n]
        end
    end
    return nn
end

function _individual_train_acc(self, boss, y, train_size, lowest_acc)
    correct = 0
    required_correct = int(lowest_acc * train_size)

    for i in range(train_size)
        if correct + train_size - i < required_correct
            return -1
        end
        c = boss._train_predict(i)

        if c == y[i]
            correct += 1
        end
    end
    return correct / train_size
end

"""
julia> linespace(2,4,5, Array{Int})
5-element Array{Int64,1}:
 2
 2
 3
 3
 4
"""
linespace(start,stop,_length, _type) =  convert(_type, floor.(LinRange(start,stop,_length)))


export RandomVertexCover

struct RandomVertexCover end

"""
    vertex_cover(g, RandomVertexCover())

Find a set of vertices such that every edge in `g` has some vertex in the set as
atleast one of its end point.

### Implementation Notes
Performs [Approximate Minimum Vertex Cover](https://en.wikipedia.org/wiki/Vertex_cover#Approximate_evaluation) once.
Returns a vector of vertices representing the vertices in the Vertex Cover.

### Performance
Runtime: O(|V|+|E|)
Memory: O(|E|)
Approximation Factor: 2
"""
function vertex_cover(
    g::AbstractGraph{T},
    alg::RandomVertexCover
    ) where T <: Integer

    nvg::T = nv(g)
    cover = Vector{T}()
    in_cover = falses(nvg)

    @inbounds for e in shuffle(collect(edges(g)))
        if !(in_cover[e.src] || in_cover[e.dst])
            in_cover[e.src] = in_cover[e.dst] = true
            push!(cover, e.src)
            push!(cover, e.dst)
        end
    end

    return [v for v in vertices(g) if in_cover[v]]
end


# Rounding of vectors to a given number of significant digits
vcode(v::Verts,prec=10^5) = zeros!(map(round, v*prec)/prec)
vcode(v::Vector,prec=10^5) = zeros!(map(round, v*prec)/prec)

# random 2D point with given number of digits.
# A single 2D random point, codified in floating point format, and with a fixed (quite small)
# number of digits, is returned by the \texttt{rand_point2d()} function, with no input
# parameters.
rand_point2d(prec=10^15) = vcode(rand(2,1))

# Generation of a random line segment.
# A single random segment, scaled about its centroid by the \texttt{scaling} parameter, is
# returned by the \texttt{redge()} function, as a tuple ot two random points in the unit
# square.
function rand_edge(scaling=1)
	v1,v2 = (rand_point2d(), rand_point2d())
    c = (v1+v2)/2
    pos = rand_point2d()
    v1 = (v1-c)*scaling + pos
    v2 = (v2-c)*scaling + pos
    (vcode(v1), vcode(v2))
end

# Generation of random lines
# Return a LAR pair (V,EV)
function randomLines(lineNumber,scaling=1)
    L = hcat([hcat(rand_edge(scaling)...)  for k=1:lineNumber]...)
    xmin, ymin = min(L[1,:]...),min(L[2,:]...)
    v = [-xmin,-ymin]
    lines = [L[:,k]+v  for k=1:size(L,2)]
	V = hcat(lines...)
	EV = reshape(collect(1:2*lineNumber),(2,lineNumber))
	V,EV
end

# Generation of containment boxes of LAR cells
# Input: a LAR pair V,EV (or V,FV etc)
# Output:
# Containment boxes
function containment2DBoxes(V,EV)
	boxes = Vector{Float64}[]
	for k=1:size(EV,2)
		(v1,v2) = EV[:,k]
		(x1,y1),(x2,y2) = V[:,v1],V[:,v2]
		box = [min(x1,x2),min(y1,y2),max(x1,x2),max(y1,y2)]
		boxes = push!(boxes,vcode(box))
	end
	boxes
end

function containment2DBoxes(V,EV)
	boxes = Vector{Float64}[]
	for k=1:size(EV,2)
		(v1,v2) = EV[:,k]
		(x1,y1),(x2,y2) = V[:,v1],V[:,v2]
		box = [min(x1,x2),min(y1,y2),max(x1,x2),max(y1,y2)]
		boxes = push!(boxes,vcode(box))
	end
	boxes
end

# generate an HPC value from data of `Verts` type
polyline(verts::Verts) = p.POLYLINE(PyObject(map(PyObject,verts')))

# produce the 5 vertices of a closed rectangular polyline
# input is a box, i.e. a quadruple xmin,ymin,xmax,ymax()
function box2rect(box)
    x1,y1,x2,y2 = box
    verts = [[x1,y1] [x2,y1] [x2,y2] [x1,y2] [x1,y1]]
end

# linesFromLineArray(V,EV) = Any[[V[:,EV[:,k][1]] V[:,EV[:,k][2]]  ] for k=1:size(EV,2)]
# Implemented indirect indexing via multidimensional array
function linesFromLineArray(V,EV)
	result = Array(Float64, size(V,1), 2, size(EV,2))
	for k = 1 : size(EV,2)
		for j = 1 : 2
			for i = 1 : size(V,1)
				result[i,j,k] = V[i,EV[j,k]]
			end
		end
	end
	result
end

# Computation of the 1D centroid of a list of 2D boxes. The direction
# is chosen depending on the value of the `xy in Set([1,2])` parameter.
function centroid(boxes::Array{Float64,2},xyz)
	m = size(boxes,1)
	n = Int(m/2)
	average = zeros(m)
	for h=1:m
		average[h] = mean(boxes[h,:])
	end
	median = (average[xyz] + average[xyz+n])/2
end

function centroid(boxes::Array{Array{Float64,1},1},xyz)
	myboxes = hcat(boxes...)
	return centroid(myboxes::Array{Float64,2},xyz)
end

# Splitting the input above and below a median threshold
function splitOnThreshold(boxes,subset,coord)
	if subset==Set{Int64}()
		return Set{Int64}(), Set{Int64}()
	end
    theBoxes = [boxes[:,k] for k in subset]
    threshold = centroid(theBoxes,coord)
    ncoords = Int(floor(length(boxes[:,1])/2))
    a = coord % ncoords +1
    b = Int(a + ncoords)
    below,above = Int[],Int[]
    for k in subset
        if boxes[:,k][a] <= threshold
        	push!(below, k) end
    end
    for k in subset
        if boxes[:,k][b] >= threshold
        	push!(above, k) end
    end
    Set{Int64}(below), Set{Int64}(above)
end

function geomPartitionate(boxes,buckets)
    geomInters = [Set{Int64}() for h=1:length(boxes)]
    for bucket in buckets
        for k in bucket
            geomInters[k] = union(geomInters[k],bucket)
        end
    end
    for (h,inters) in enumerate(geomInters)
        geomInters[h] = setdiff(geomInters[h],Set([h]))
    end
    geomInters
end

# local function
function splitting(bucket,below,above, finalBuckets,splittingStack)
	a = (length(below)<4) & (length(above)<4)
	b = length(setdiff(bucket,below)) < 7
	c = length(setdiff(bucket,above)) < 7
	if ( a | b | c )
		push!(finalBuckets, below)
		push!(finalBuckets, above)
		println("pushed finalBuckets")
	else
		push!(splittingStack, below)
		push!(splittingStack, above)
		println("pushed splittingStack")
	end
end

function lar2boxes(V,CV::Array{Int64,2})
	m = size(V,1)*2
	boxes = zeros(Float64,(m,size(CV,2)))
	for k=1:size(CV,2)
	cell = CV[:,k]
		verts = hcat([V[:,v] for v in cell]...)
		pmin,pmax = Float64[],Float64[],Float64[]
		for h=1:size(verts,1)
			coords = verts[h,:]
			push!(pmin,minimum(coords))
			push!(pmax,maximum(coords))
		end
		box = vcat(pmin,pmax)
		boxes[:,k] = box
	end
	return boxes
end

function lar2boxes(V,CV::Array{Array{Int64,1},1})
	m = size(V,1)*2
	n = length(CV)
	boxes = zeros(Float64,(m,n))
	for k=1:n
		cell = CV[k]
		verts = hcat([V[:,v] for v in cell]...)
		pmin,pmax = Float64[],Float64[]
		for h=1:size(verts,1)
			coords = verts[h,:]
			push!(pmin,minimum(coords))
			push!(pmax,maximum(coords))
		end
		box = vcat(pmin,pmax)
		boxes[:,k] = box
	end
	return boxes
end

# Intersection of two line segments
function segmentIntersect(boxes,lineArray,lineStorage)
	V,EV = lineArray
    function segmentIntersect0(h)
    	h1,h2 = EV[:,h]
        p1,p2 = V[:,h1],V[:,h2]
        line1 = vcode([p1 p2])
        #line1 = [p1 p2]
        (x1,y1),(x2,y2) = p1,p2
        B1,B2,B3,B4 = boxes[:,h]
        function segmentIntersect1(k)
			k1,k2 = EV[:,k]
			p3,p4 = V[:,k1],V[:,k2]
			line2 = vcode([p3 p4])
			(x3,y3),(x4,y4) = p3,p4
			b1,b2,b3,b4 = boxes[:,k]
            if ! |(b3<B1 , B3<b1 , b4<B2 , B4<b2)
                m23 = [p2 p3]
                m14 = [p1 p4]
                m = m23 - m14
                v = p3-p1
                a=m[1,1]; b=m[1,2]; c=m[2,1]; d=m[2,2]
                det = a*d-b*c
                if abs(det) > 10^-6.
                    m_inv = inv(m)
                    alpha, beta = m_inv*v
                    if (-0.0<=alpha<=1.0) & (-0.0<=beta<=1.0)
                    	if 0.0 != vcode([alpha])[1] != 1.0
                        	push!(lineStorage[line1], alpha)
                        end
                    	#if 0.0 != vcode([alpha])[1] != 1.0
                    	if 0.0 != vcode([beta])[1] != 1.0
	                        push!(lineStorage[line2], beta)
	                    end
                        return p1+alpha*(p2-p1)
                    end
                end
            end
            #return nothing
        end
    end
end

# Brute force intersection within the buckets
function lineBucketIntersect(boxes,lineArray, h,bucket, lineStorage)
    intersect0 = segmentIntersect(boxes,lineArray,lineStorage)
    intersectionPoints = Array{Float64,1}[]
    intersect1 = intersect0(h)
    for line in bucket
    	#@show line
        point = intersect1(line)
        if point != nothing
            push!(intersectionPoints, vcode(point))
        end
    end
    intersectionPoints
end

function boxBucketing(boxes::Array{Float64,2})
	nboxes = size(boxes,2)
	dim = Int(size(boxes,1)/2)
	trees,boxes1D = Any[],Any[]
	# preparation of d interval-trees
	for d=1:dim
		intervals = [ (boxes[d,k], boxes[d+dim,k], k) for k=1:nboxes ];
		push!(boxes1D, intervals)
		tree1D = IntervalTree{Float64, IntervalValue{Float64, Int64}}();
		for triple in intervals
			push!(tree1D, IntervalValue{Float64, Int64}(triple...));
		end
		push!(trees, tree1D)
	end
	# execution of spatial queries
	buckets,queries = Any[],Any[]
	for k=1:nboxes
		sets = Any[]
		for d=1:dim
			push!(sets, Set(Int64[]))
			query = boxes1D[d][k]
			for item in intersect(trees[d],query[1:2])
				push!(sets[d], item.value)
			end
		end
		push!(buckets,intersect(sets...))
	end
	buckets = [sort(collect(bucket)) for bucket in buckets]
	return buckets
end

# Accelerate intersection of lines
function lineIntersection(lineArray)
	V,EV = lineArray
	lineStorage = Dict{Array{Float64,2},Array{Float64,1}}()
	for k = 1:size(EV,2)
		v1,v2 = EV[:,k]
		p1,p2 = V[:,v1],V[:,v2]
		key = vcode([p1 p2])
		lineStorage[key] = Int[]
	end
	boxes = lar2boxes(lineArray...)
	buckets = boxBucketing(boxes)
    for (h,bucket) in enumerate(buckets)
    	#@show h,bucket
        pointBucket = lineBucketIntersect(boxes,lineArray, h,bucket, lineStorage)
    end
    frags = keys(lineStorage)
    params = [sort([x for x in Set(vcode(group))]) for group in values(lineStorage)]
    lineFrags = Dict(zip(frags,params))
end

function check4DoubleEdges(EV)
	for k=1:(size(EV,2)-1)
		if EV[:,k]==EV[:,k+1]

		end
	end
end

function larModelCheck(V,EV)
	vertexIndex,k = Dict{Int64,Int64}(),0
	W = sort(collect(Set(EV)))
	for (k,v) in enumerate(W)
		vertexIndex[v] = k
	end
	EW = deepcopy(EV)
	for k=1:size(EV,2)
		EW[1,k] = vertexIndex[EV[1,k]]
		EW[2,k] = vertexIndex[EV[2,k]]
	end
	vkeys = p.TRANS(sort(collect(vertexIndex)))'[:,1]
	V = hcat([V[:,k] for k in vkeys]...)
	EV = hcat([sort(EW[:,k]) for k=1:size(EW,2)]...)
	EV = sortcols(EV, by=x->(x[1],x[2]))
	EV = sortrows(EV)
	#EV = check4Edges(EV)
	V,EV
end

# Transform a lineArray (Array of pairs of 2D points) into a 1-complex (V,EV)
function lines2lar(lineArray,prec=10^15)
	lineFrags = lineIntersection(lineArray)

	verts = zeros(2,1)
	index = 0
	EV = Array{Any,1}()
	for k in 1:length(lineFrags)
		ps = collect(keys(lineFrags))[k]
		alphas = vcat([[0.],collect(values(lineFrags))[k],[1.]]...)
		pts = [ (ps[:,1]*(1.-alphas[i])+ps[:,2]*alphas[i]) for i=1:length(alphas) ]
		verts = hcat(verts,hcat(pts...))
		for h=(index+1):(index+length(pts)-1)
			push!(EV, [h,h+1])
		end
		index += length(pts)
	end
	V,EV = verts[:,2:end],hcat([EV[k] for k=1:length(EV)]...)

	W,close,clusters,vmap = p.pruneVertices(V')
	if typeof(W) == Array{Any,1}
		Z = Array{Float64,2}(hcat(W...))
	else
		Z = Array{Float64,2}(W)
	end
	vmap = vmap + 1
	EW = [[vmap[EV[1,k]],vmap[EV[2,k]]] for k in 1:size(EV,2)]
	EZ = hcat(EW...)
	V,EV = biconnectedComponents(Z,EZ)
	if size(V,2)==2 V=V' end
	V,EV = larModelCheck(V,EV)
end

# Adjacency lists of 1-complex vertices
function vertices2vertices(V,EV)
	lar = Lar()
	lar.EV = [EV[:,k] for k=1:size(EV,2)]
	ev = cellComplex(lar.EV)
	vv = ev'*ev
	triples = hcat(findnz(vv)...)'
	VV = [Int64[] for k=1:size(vv,1)]
	for k=1:size(triples,2)
		row,col,datum = triples[:,k]
		if row != col
			push!(VV[col],row)
		end
	end
	return VV
end

# Main procedure for biconnected components
function biconnectedComponents(W,EV)
    V = 1:length(W)
    count = 0
    stack,out = [],[]
    visited = [false for v in V]
    parent = [-1 for v in V]
    d = [-1 for v in V]
    low = [-1 for v in V]
    VV = vertices2vertices(W,EV)
    for u in V
        if !visited[u]
            DFV_visit( VV,out,count,visited,parent,d,low,stack, u )
        end
    end
    out = [component for component in out if length(component) > 1]
    EV = [[u,v] for (u,v) in vcat(out...)]
    W,hcat(EV...)
end

# Hopcroft-Tarjan algorithm
function DFV_visit( VV,out,count,visited,parent,d,low,stack,u )
    visited[u] = true
    count += 1
    d[u] = count
    low[u] = d[u]
    if u > length(VV) return() end
    for v in VV[u]
        if !visited[v]
            push!(stack, (u,v))   # possible bug: []
            #println("stack = $stack")
            parent[v] = u
            DFV_visit( VV,out,count,visited,parent,d,low,stack, v )
            if low[v] >= d[u]
                push!(out,outputComp(stack,u,v))
                #println("\tout = $out")
                #println("\tv = $v")
                if v == length(VV) return() end
            end
            low[u] = min( low[u], low[v] )
        else
            if ! (parent[u]==v) & (d[v] < d[u])
                push!(stack, (u,v))   # possible bug: []
                low[u] = min( low[u], d[v] )
            end
        end
    end
end

# Output of biconnected components
function outputComp(stack,u,v)
    out = []
    while true
        elem = pop!(stack)
        push!(out, elem)
        if elem == (u,v)
        	break
        end
    end
    out
end

function larFromLines(datafile) # lineArray'
	V = reshape(datafile',(size(datafile',1)÷2,size(datafile',2)*2))
	len = length(datafile)
	EV = collect(reshape(1:(len÷2), 2,(len÷4)))
	W,EW = lines2lar((V,EV))
	chains = boundary(W,EW)
	operator = boundaryOp(EW,chains)
	FW = [sort(collect(Set(vcat([EW[:,abs(e)] for e in face]...)))) for face in chains]
	W,FW,EW
end


#=
Created on 05/01/2021 20:10:45
Last update: -

@author: Michiel Stock
michielfmstock@gmail.com

Example of some interesting matrices
- Strang matrix
=#

# STRANG MATRIX

struct Strang <: AbstractMatrix{Int}
    n::Int
end

Base.size(S::Strang) = (S.n, S.n)
Base.getindex(S::Strang, i, j) = i==j ? 2 : (abs(i - j) == 1 ?  -1 : 0)

S = Strang(1000)

@time sum(S)

Base.sum(S::Strang) = 2

@time sum(S)

v = randn(1000)

@time S * v

function Base.:*(S::Strang, v::Vector)
    n = length(v)
    @assert size(S, 2) == n
    x = similar(v)
    for i in 1:n
        x[i] = v[i]
        i > 1 && (x[i] += v[i-1])
        i < n && (x[i] += v[i+1])
    end
    return x
end

@time S * v

# Vandermonde matrix

struct Vandermonde{T,VT} <: AbstractMatrix{T}
    α::VT
    m::Int
    Vandermonde(α::AbstractVector{T}, m) where {T} = new{T,typeof(α)}(α,m)
end

Vandermonde(α::Vector{<:Number}) = Vandermonde(α, length(α))

Base.size(V::Vandermonde) = (length(V.α), V.m)
Base.getindex(V::Vandermonde, i, j) = V.α[i]^(j-1)

α = [1, 2, 3, 4]

V = Vandermonde(α, 4)

using LinearAlgebra

LinearAlgebra.det(V::Vandermonde) = ((xi - xj) for (i, xi) in enumerate(V.α), (j, xj) in enumerate(V.α) if i < j) |> prod


g = evolving_graph(Char, Int, is_directed = false)
add_edge!(g, 'a', 'b', 1)
add_edge!(g, 'b', 'g', 1)
add_edge!(g, 'b', 'f', 1)
add_edge!(g, 'c', 'e', 2)
add_edge!(g, 'e', 'g', 2)
add_edge!(g, 'a', 'b', 2)
add_edge!(g, 'b', 'f', 2)
add_edge!(g, 'f', 'd', 2)
add_edge!(g, 'a', 'b', 3)
add_edge!(g, 'c', 'f', 3)
add_edge!(g, 'e', 'g', 3)
add_edge!(g, 'g', 'f', 3)

p = shortest_temporal_path(g, 'a', 1, 'e', 3)
display(p)

@test shortest_temporal_distance(g, 'c', 2, 'a', 2) == Inf

g = int_evolving_graph(6, 3)
add_edge!(g, 1, 2, 1)
add_edge!(g, 1, 3, 2)
add_edge!(g, 4, 5, 2)
add_edge!(g, 2, 3, 3)
add_edge!(g, 5, 6, 3)

p = shortest_temporal_path(g, 1, 1, 3, 3)
@test length(p) - 1 == shortest_temporal_distance(g, 1, 1, 3, 3)
display(p)


function get_nv(clique)
    length(unique(vcat(clique...)))
end

function get_ne(clique)
    edges = Set([])
    for clique in clique
        for edge in collect(combinations(clique, 2))
            push!(edges, Set(edge))
        end
    end
    return length(edges)
end

function get_nb_lc(clique, cliquetree)
    dstmx = zeros(Int, length(clique), length(clique))
    for i in 1:length(clique) - 1
        for j in i + 1:length(clique)
            value = length(intersect(clique[i], clique[j]))
            dstmx[i, j] = value
            dstmx[j, i] = value
        end
    end
    nb_lc = sum([dstmx[src, dst] * (2 * dstmx[src, dst] + 1) for (src, dst) in cliquetree])
    return nb_lc
end

save_clique(clique::Vector{Vector{Int}}, path::AbstractString) = writedlm(path, clique)

save_cliquetree(cliquetree::Vector{Vector{Int}}, path::AbstractString) = writedlm(path, cliquetree)

function read_cliquetree(path)
    cliquetree = readdlm(path, '\t', Int)
    cliquetree = [cliquetree[i, :] for i in 1:size(cliquetree, 1)]
    return cliquetree
end

function read_clique(path)
    lines = split(read(open(path, "r"), String), '\n')[begin:end-1]
    clique = [parse.(Int, split(line, "\t")) for line in lines]
    return clique
end


# -*- coding: utf-8 -*-
# ---
# jupyter:
#   jupytext:
#     formats: ipynb,jl:hydrogen
#     text_representation:
#       extension: .jl
#       format_name: hydrogen
#       format_version: '1.3'
#       jupytext_version: 1.10.3
#   kernelspec:
#     display_name: Julia 1.6.4
#     language: julia
#     name: julia-1.6
# ---

# %%
using Distributions
using Plots
using Optim

traceon = Optim.Options(
    store_trace = true,
    extended_trace = true
)

# model
Distributions.TDist(μ, ρ, ν) = LocationScale(μ, ρ, TDist(ν))

# test data
X = [-0.01, 0.01, 1.0]

# estimations with different initial values
r1 = optimize(x -> -loglikelihood(TDist(x[1], eps() + 10^x[2], eps() + 10^x[3]), X), [0.33, 0.0, 0.16], LBFGS(), traceon)
r2 = optimize(x -> -loglikelihood(TDist(x[1], eps() + 10^x[2], eps() + 10^x[3]), X), [0.33, 0.0, 0.18], LBFGS(), traceon)
r3 = optimize(x -> -loglikelihood(TDist(x[1], eps() + 10^x[2], eps() + 10^x[3]), X), [0.33, 0.0, 0.27], LBFGS(), traceon);

# %%
function plot_result(r, X)
    x = r.initial_x
    m = r.minimizer
    l = -minimum(r)
    t = "initial = $x\nfinal = $(round.(m; digits=3))\nfinal loglikelihood = $(round(l; digits=3))"
    xtick = axes(X, 1)
    normal = fit_mle(Normal, X)

    plot(; legend=:topleft)
    scatter!(xtick, X; label="data", xtick, xlim=(first(xtick)-0.5, last(xtick)+0.5))
    hline!([m[1]]; label="result of t-dist. model", lw=1.5)
    hline!([mean(normal)]; label="result of normal dist. model", ls=:dash, lw=1.5)
    title!(t; titlefontsize=10)
end

R1 = plot_result(r1, X)
R2 = plot_result(r2, X)
R3 = plot_result(r3, X)
plot(R1, R2, R3; size=(800, 700), xtick=1:3)

# %%
function plot_trace(r)
    x = r.initial_x
    c = hcat(x, (t.metadata["x"] for t in r.trace)...)
    l = -minimum(r)
    m = r.minimizer
    t = "initial = $x\nfinal = $(round.(m; digits=3))\nfinal loglikelihood = $(round(l; digits=3))"

    plot(; legend=:topleft)
    scatter3d!([x[1]], [x[2]], [x[3]]; label="initial", c=:blue, ms=3)
    plot!(c[1,:], c[2,:], c[3,:]; label="path", c=:darkcyan)
    scatter3d!([m[1]], [m[2]], [m[3]]; label="final", c=:yellow, ms=3)
    plot!(; xlabel="μ", ylabel="log₁₀ρ", zlabel="log₁₀ν")
    title!(t; titlefontsize=10)
end

P1 = plot_trace(r1)
P2 = plot_trace(r2)
P3 = plot_trace(r3)
plot(P1, P2, P3; size=(800, 700), tickfontsize=6)

# %%
function f(μ, log10ν, X)
    log10rhos = range(-15, 10; length=200)
    -maximum(loglikelihood(TDist(μ, 10^log10ρ, 10^log10ν), X) for log10ρ in log10rhos)
end

function plot_trace2d(r)
    x = r.initial_x
    c = hcat(x, (t.metadata["x"] for t in r.trace)...)
    l = -minimum(r)
    m = r.minimizer
    t = "initial = $x\nfinal = $(round.(m; digits=3))\nfinal loglikelihood = $(round(l; digits=3))"

    mus = range(-0.1, 0.4; length=200)
    log10nus = range(-2.5, 8.0; length=200)
    z = f.(mus', log10nus, Ref(X))

    plot(; legend=:topleft, colorbar=false)
    plot!(; xlim=extrema(mus), ylim=extrema(log10nus))
    heatmap!(mus, log10nus, z; clim=(-1, 5), c=reverse(cgrad(:CMRmap)))
    scatter!([x[1]], [x[3]]; label="initial", ms=4, c=:blue)
    plot!(c[1,:], c[3,:]; label="path", c=:cyan, lw=1.5)
    scatter!([m[1]], [m[3]]; label="final", ms=4, c=:yellow)
    plot!(; xlabel="μ", ylabel="log₁₀ν")
    title!(t; titlefontsize=10)
end

Q1 = plot_trace2d(r1)
Q2 = plot_trace2d(r2)
Q3 = plot_trace2d(r3)
plot(Q1, Q2, Q3; size=(800, 700))

# %%

0024/TDist model is not log-convex - LBFGS.jl


using Printf

for n in (0, 5, 50, 9000)
    @printf("%6i → %s\n", n, string(n, base=2))
end

# with pad
println("\nwith pad")
for n in (0, 5, 50, 9000)
    @printf("%6i → %s\n", n, string(n, base=2, pad=20))
end


# ---
# title: 135. Candy
# id: problem135
# author: Qling
# date: 2020-12-09
# difficulty: Hard
# categories: Greedy
# link: <https://leetcode.com/problems/candy/description/>
# hidden: true
# ---
#
# There are _N_ children standing in a line. Each child is assigned a rating
# value.
#
# You are giving candies to these children subjected to the following
# requirements:
#
#   * Each child must have at least one candy.
#   * Children with a higher rating get more candies than their neighbors.
#
# What is the minimum candies you must give?
#
# **Example 1:**
#
#
#
#     Input: [1,0,2]
#     Output: 5
#     Explanation: You can allocate to the first, second and third child with 2, 1, 2 candies respectively.
#
#
# **Example 2:**
#
#
#
#     Input: [1,2,2]
#     Output: 4
#     Explanation: You can allocate to the first, second and third child with 1, 2, 1 candies respectively.
#                  The third child gets 1 candy because it satisfies the above two conditions.
#
#
#
## @lc code=start
using LeetCode

function candy(ratings::Vector{Int})::Int
    sizes = length(ratings)
    if sizes < 2
        return sizes
    end

    nums = ones(Int, sizes)
    ## from left to right
    for i in 2: sizes
        if ratings[i] > ratings[i - 1]
            nums[i] = nums[i - 1] + 1
        end
    end

    ## from right to left
    for i in sizes: -1: 2
        if ratings[i] < ratings[i - 1]
            nums[i - 1] = max(nums[i - 1], nums[i] + 1)
        end
    end

    return sum(nums)
end
## @lc code=end


using LSystems
using Base.Test

# Algae-growth from https://en.wikipedia.org/wiki/L-system#Example_1:_Algae
algae_start = "A"
algae_transition = Dict([('A', "AB"), ('B', "A")])

s = ""
for c in @task lindenmayer(algae_start, algae_transition, 0)
    s *= string(c)
end
@test "A" == s

s = ""
for c in @task lindenmayer(algae_start, algae_transition, 1)
    s *= string(c)
end
@test "AB" == s

s = ""
for c in @task lindenmayer(algae_start, algae_transition, 5)
    s *= string(c)
end
@test "ABAABABAABAAB" == s

s = ""
for c in @task lindenmayer(algae_start, algae_transition, 7)
    s *= string(c)
end
@test "ABAABABAABAABABAABABAABAABABAABAAB" == s

# Pythagoras tree from https://en.wikipedia.org/wiki/L-system#Example_2:_Pythagoras_tree
pythagoras_start = "0"
pythagoras_transition = Dict([('1', "11"), ('0', "1[0]0")])

s = ""
for c in @task lindenmayer(pythagoras_start, pythagoras_transition, 3)
    s *= string(c)
end
@test "1111[11[1[0]0]1[0]0]11[1[0]0]1[0]0" == s

# Koch curve from https://en.wikipedia.org/wiki/L-system#Example_4:_Koch_curve
koch_curve_start = "F"
koch_curve_transition = Dict([('F', "F+F-F-F+F")])

s = ""
for c in @task lindenmayer(koch_curve_start, koch_curve_transition, 2)
    s *= string(c)
end
@test "F+F-F-F+F+F+F-F-F+F-F+F-F-F+F-F+F-F-F+F+F+F-F-F+F" == s


abstract AbstractMeta
abstract AbstractΨMeta <: AbstractMeta

"""A `ψMeta` object contains all the meta-information to identify
an oriented wavelet within a filter bank. Fields:
* `γ` log-scale. `2^(-γ)` is proportional to center frequency
* `θ` orientation, i.e. angle (in 2d) or sign of center frequency (in 1d)
* `χ` chroma. `χ` is `mod(γ, n_filters_per_octave)`
* `j` octave. `j` is `div(γ, n_filters_per_octave)`
* `bandwidth` ∈]0,1] the width at -3dB, expressed in fraction of signal length
* `centerfrequency` ∈]0,1] is expressed in fraction of signal length
* `qualityfactor` ∈[1,max_qualityfactor] is equal to `centerfrequency/bandwidth`
* `scale` is the FWTM (full width at tenth maximum) in spatial domain"""
immutable ΨMeta1D <: AbstractΨMeta
    γ::Int16
    θ::Int8
    χ::Int8
    bandwidth::Float64
    centerfrequency::Float64
    j::Int8
    qualityfactor::Float64
    scale::Float64
end

immutable ΨMeta2D <: AbstractΨMeta
    γ::Int16
    θ::Int8
    χ::Int8
    aspectratio::Float64
    bandwidth::Float64
    centerfrequency::Float64
    j::Int8
    qualityfactor::Float64
    scale::Float64
end

"""A `ΦMeta` object contains all the meta-information to identify a low-pass
filter within a wavelet filter bank. Fields:
* `bandwidth` ∈]0,1] the width at -3dB, expressed in fraction of signal length
* `scale` ∈]0,1] is the FWTM (full width at tenth maximum) in spatial domain."""
immutable ΦMeta <: AbstractMeta
    bandwidth::Float64
    scale::Float64
end

get_γ(meta::AbstractΨMeta) = meta.γ
get_θ(meta::AbstractΨMeta) = meta.θ
get_χ(meta::AbstractΨMeta) = meta.χ
get_aspectratio(meta::ΨMeta2D) = meta.aspectratio
get_bandwidth(meta::AbstractMeta) = meta.bandwidth
get_centerfrequency(meta::AbstractΨMeta) = meta.centerfrequency
get_j(meta::AbstractΨMeta) = meta.j
get_qualityfactor(meta::AbstractΨMeta) = meta.qualityfactor
get_scale(meta::AbstractMeta) = meta.scale


a = 2
b = 3
c = 5

mf = BellMF(a, b, c)

# Evaluation tests
@assert mf.eval(c) == 1
@assert mf.eval(c - a) == mf.eval(c + a) == 0.5

# Mean finding tests
@assert mf.mean_at(1) == mf.mean_at(0.6) == mf.mean_at(0.3) == mf.mean_at(0) == c


# julia 1.2
push!(LOAD_PATH,pwd())

using PyPlot
# import src
include("src.jl")

# init a model
MC = (
    L = exp.([-0.4, 0.4]), # labor endow states
    P = [0.8 0.2 ; 0.2 0.8],  #transition matrix
    π = [0.5,0.5],  # stationary distribution
)
M = src.ModelBasic( N = 1.0,
BOTTOM_VAL = -6.66E36, AGG_SEED = 20200417,
AGG_SIMU_SIZE = 10000, AGG_DROP_FIRST_SIZE = 1000,
Na = 200, alim = (0.0,4.5),
lgrid = MC.L, P = MC.P, π = MC.π,
θ = 0.36, δ = 0.08, β = 0.96, μ = 2.0 )

# search ss
@time src.solve_ss_gs!(M, 1.0 , max_iter_ss = 500, max_iter_hh = 500,
rtol_ss = 1E-3, rtol_hh = 1E-3, step_size_ss = 0.7, step_size_hh = 0.7,
print_err_ss = true, print_err_hh = false, if_golden = true )

# visualization
plot(M.agrid, M.HHPolicy.aOpt[:,1], linewidth = 0.75)
plot(M.agrid, M.HHPolicy.aOpt[:,2], "--", linewidth = 0.75)
plot(M.agrid, M.agrid, "-.", linewidth = 0.75)
legend(["l - low","l - high", L"$\pi/4$"])


using CryptoTools
using Test
using Random

@testset "Shamir" begin
    scheme = ShamirSecretSharing()
    secret = BigInt(1234931)
    shares = split(scheme, secret, 6, 3)
    @test join(scheme, shares) == secret
    @test join(scheme, shares[1:3]) == secret
    @test join(scheme, shares[4:6]) == secret
end


#  Pseudorandom numbers from a truncated Gaussian distribution.

#  This implements the ziggurat algorithm from
#  N. Chopin, "Fast simulation of truncated Gaussian distributions",
#  Stat Comput (2011) 21:275-288
#
#  The code is based on the implementation by Guillaume Dollé, Vincent Mazet
#  available from http:#miv.u-strasbg.fr/mazet/rtnorm/. In particular, the
#  rtchopin_twosided and rtchopin_onesided are derived from their code.
#  Giada Spaccapanico Proietti adapted these functions to the Julia environment using
#  the random number generators implemented in the `Distributions` package.
#

#------------------------------------------------------------
# Pseudorandom numbers from a truncated Gaussian distribution
# The Gaussian has parameters μ (default 0) and σ (default 1)
# and is truncated on the interval [lower,upper].

include("constants.jl")

function Distributions.rand(
    rng::Distributions.AbstractRNG,
    d::Distributions.Truncated{Distributions.Normal{T},Distributions.Continuous},
) where {T<:Real}
    lower = d.lower
    upper = d.upper
    mu = d.untruncated.μ
    sigma = d.untruncated.σ

    a = (lower - mu) / sigma
    b = (upper - mu) / sigma

    # return(lower[0])

    if (abs(a) > abs(b))

        r = -Distributions.rand(rng, Distributions.TruncatedNormal(mu, sigma, -b, -a))

    elseif (a == -Inf)
        r = Distributions.rand(rng, Distributions.Normal(0, 1))
        # If a in the right tail (a > xmax), use rejection algorithm with a truncated exponential proposal
    elseif (a > _XMAX)
        if (b == Inf)
            r = c_rtexp_onesided(rng, a)
        else
            r = c_rt_exp_c(rng, a, b)
        end
        # If a in the left tail (a < xmin), use rejection algorithm with a Gaussian proposal
    elseif (a < _XMIN)

        if (b == Inf)
            r = c_rtnaive_onesided(rng, a)
        else
            r = c_rtnaive(rng, a, b)
        end
        # In other cases (xmin < a < xmax), use Chopin's algorithm
    else

        if (b == Inf)
            r = rtchopin_onesided(rng, a)
        else
            r = rtchopin_twosided(rng, a, b)
        end
    end

    return sigma * r + mu
end

#------------------------------------------------------------
# Compute y_l from y_k #IDX_OK
function yl(k::Int64)
    if (k == 1) # 1 was 0
        return 0.053513975472   # y_l of the leftmost rectangle
    elseif (k == _N - 1) #_N was _N - 1
        return 0.000914116389555  # y_l of the rightmost rectangle
    elseif (k <= 1954) # 1954 was 1953
        return _YU[k-1]
    else
        return _YU[k+1]
    end
end

#------------------------------------------------------------
# Rejection algorithm with a truncated exponential proposal
function c_rt_exp_c(rng::Distributions.AbstractRNG, a::Float64, b::Float64)

    stop = false
    twoasq = 2 * a^2
    expab = _exp_c(-a * (b - a)) - 1
    z = 0.0
    while (!stop)
        z = _log_c(1 + Random.rand(rng) * expab)
        e = -_log_c(Random.rand(rng))
        stop = (twoasq * e > z^2)
    end
    return a - z / a
end

#------------------------------------------------------------
# One-sided rejection algorithm with a truncated exponential proposal
function c_rtexp_onesided(rng::Distributions.AbstractRNG, a::Float64)

    stop = false
    lambda = 0.5 * (a + sqrt(a^2 + 4))
    z = 0.0
    while (!stop)
        z = a - _log_c(Random.rand(rng)) / lambda
        stop = (-2 * _log_c(Random.rand(rng)) > (z - lambda)^2)
    end
    return z
end

#------------------------------------------------------------
# Naive accept reject (two-sided)
function c_rtnaive(rng::Distributions.AbstractRNG, a::Float64, b::Float64)
    stop = false
    r = 0.0
    while (!stop)
        r = Distributions.rand(rng, Distributions.Normal(0, 1))
        stop = (r >= a) && (r <= b)
    end
    return r
end

#------------------------------------------------------------
# Naive accept reject  (one-sided)
function c_rtnaive_onesided(rng::Distributions.AbstractRNG, a::Float64)
    stop = false
    r = 0.0
    while (!stop)
        r = Distributions.rand(rng, Distributions.Normal(0, 1))
        stop = (r >= a)
    end

    return r
end

#------------------------------------------------------------
# Rejection algorithm with a truncated Rayleigh proposal
function rtrayleigh(rng::Distributions.AbstractRNG, a::Float64)

    stop = false
    asq = (a^2) * 0.5
    v = 0.0
    x = 0.0
    while (!stop)
        v = Random.rand(rng)
        x = asq - _log_c(Random.rand(rng))
        stop = (v^2 * x < a)
    end

    return sqrt(2 * x)
end

#------------------------------------------------------------
# Chopin's one-sided ziggurat algorithm
function rtchopin_onesided(rng::Distributions.AbstractRNG, a::Float64)

    stop = false

    if (a < _XMIN)  #if a is too small, use simple reject algorithm

        r = c_rtnaive_onesided(rng, a)
        stop = true

    elseif (a > _XMAX) # if a is too large, use Devroye's algorithm

        r = c_rtexp_onesided(rng, a)
        stop = true

    else
        i = Int64(_I0 + floor(a * _INVH)) #was without +1
        ka = _NCELL[i] + 1#was without +1
        while (!stop)
            # sample integer between ka and N
            k = Int64(floor(Random.rand(rng) * (_N - ka + 1)) + ka)  # was without +1
            if (k >= _N)  #right tail (last box on the right) # was ==
                r = c_rtexp_onesided(rng, _XCONST[_N])
                stop = true
            elseif (k <= ka + 1)
                sim = _XCONST[k] + (_XCONST[k+1] - _XCONST[k]) * Random.rand(rng)
                if (sim >= a)
                    simy = _YU[k] * Random.rand(rng)
                    if ((simy < yl(k)) || (sim * sim + 2.0 * _log_c(simy) + _ALPHA) < 0)
                        r = sim
                        stop = true
                    end
                end
            else

                u = Random.rand(rng)
                simy = _YU[k] * u
                d = _XCONST[k+1] - _XCONST[k]
                ylk = yl(k)
                if (simy < ylk)  # That's what happens most of the time
                    r = _XCONST[k] + u * d * _YU[k] / ylk
                    stop = true
                else

                    sim = _XCONST[k] + d * Random.rand(rng)

                    # Otherwise, check you're below the pdf curve
                    if ((sim * sim + 2 * _log_c(simy) + _ALPHA) < 0)

                        r = sim
                        stop = true
                    end
                end
            end
        end # end while loop
    end

    return r
end #IDXOK

#------------------------------------------------------------
# Chopin's two-sided ziggurat algorithm
function rtchopin_twosided(rng::Distributions.AbstractRNG, a::Float64, b::Float64)

    kmin = 5                                 # if kb-ka < kmin then use a rejection algorithm
    stop = false

    # Compute ka
    i = Int64(_I0 + floor(a * _INVH)) # was without +1
    ka = _NCELL[i] + 1 #was without +1

    # Compute kb
    if (b >= _XMAX)
        kb = _N
    else
        i = Int64(_I0 + floor(b * _INVH))  # was without +1
        kb = _NCELL[i] + 1 # was without +1
    end

    # If |b-a| is small, use rejection algorithm with a truncated exponential proposal
    if (abs(kb - ka) < kmin)
        r = c_rt_exp_c(rng, a, b)
        stop = true
    end

    while (!stop)

        # Sample integer between ka and kb
        k = Int64(ceil(Distributions.rand(rng, Distributions.Uniform(ka, kb))))  # was without +1

        if (k >= _N) # was ==

            # Right tail
            lbound = _XCONST[_N]
            z = -_log_c(Random.rand(rng))
            e = -_log_c(Random.rand(rng))
            z = z / lbound

            if ((z^2 <= 2 * e) && (z < b - lbound))

                # Accept this proposition, otherwise reject
                r = lbound + z
                stop = true
            end

        elseif ((k <= ka + 1) || (k >= kb - 1 && b < _XMAX))

            # Two leftmost and rightmost regions
            sim = _XCONST[k] + (_XCONST[k+1] - _XCONST[k]) * Random.rand(rng)

            if ((sim >= a) && (sim <= b))

                # Accept this proposition, otherwise reject
                simy = _YU[k] * Random.rand(rng)
                if ((simy < yl(k)) || (sim * sim + 2 * _log_c(simy) + _ALPHA) < 0)

                    r = sim
                    stop = true
                end
            end
        else # All the other boxes

            u = Random.rand(rng)
            simy = _YU[k] * u
            d = _XCONST[k+1] - _XCONST[k]
            ylk = yl(k)
            if (simy < ylk)  # That's what happens most of the time

                r = _XCONST[k] + u * d * _YU[k] / ylk
                stop = true
            else

                sim = _XCONST[k] + d * Random.rand(rng)

                # Otherwise, check you're below the pdf curve
                if ((sim * sim + 2 * _log_c(simy) + _ALPHA) < 0)

                    r = sim
                    stop = true
                end
            end
        end
    end

    return r
end


"""
    galerkin_tensor(
        B::AbstractBSplineBasis,
        (D₁::Derivative, D₂::Derivative, D₃::Derivative),
        [T = Float64],
    )

Compute 3D banded tensor appearing from quadratic terms in Galerkin method.

As with [`galerkin_matrix`](@ref), it is also possible to combine different
functional bases by passing, instead of `B`, a tuple `(B₁, B₂, B₃)` of three
`AbstractBSplineBasis`.
For now, the first two bases, `B₁` and `B₂`, must have the same length.

The tensor is efficiently stored in a [`BandedTensor3D`](@ref) object.
"""
function galerkin_tensor end

function galerkin_tensor(
        Bs::NTuple{3,AbstractBSplineBasis},
        deriv::DerivativeCombination{3},
        ::Type{T} = Float64,
    ) where {T}
    _check_bases(Bs)
    dims = length.(Bs)
    b = order(first(Bs)) - 1   # band width
    if length(Bs[1]) != length(Bs[2])
        throw(DimensionMismatch("the first two bases must have the same lengths"))
    end
    δl, δr = num_constraints(Bs[3]) .- num_constraints(Bs[1])
    A = BandedTensor3D{T}(undef, dims, Val(b), bandshift=(0, 0, δl))
    galerkin_tensor!(A, Bs, deriv)
end

galerkin_tensor(B::AbstractBSplineBasis, args...) =
    galerkin_tensor((B, B, B), args...)

"""
    galerkin_tensor!(
        A::BandedTensor3D,
        B::AbstractBSplineBasis,
        (D₁::Derivative, D₂::Derivative, D₃::Derivative),
    )

Compute 3D Galerkin tensor in-place.

See [`galerkin_tensor`](@ref) for details.
"""
function galerkin_tensor! end

function galerkin_tensor!(
        A::BandedTensor3D, Bs::NTuple{3,AbstractBSplineBasis},
        deriv::DerivativeCombination{3},
    )
    _check_bases(Bs)

    Ns = size(A)
    if Ns != length.(Bs)
        throw(DimensionMismatch("wrong dimensions of Galerkin tensor"))
    end

    Bi, Bj, Bl = Bs
    Ni, Nj, Nl = Ns
    @assert Ni == Nj  # verified at construction of BandedTensor3D

    # Orders and knots are assumed to be the same (see _check_bases).
    k = order(Bi)
    ts = knots(Bi)

    if bandwidth(A) != k - 1
        throw(ArgumentError("BandedTensor3D must have bandwidth = $(k - 1)"))
    end

    δl, _ = num_constraints(Bl) .- num_constraints(Bi)

    if bandshift(A) != (0, 0, δl)
        throw(ArgumentError("BandedTensor3D must have bandshift = (0, 0, $δl)"))
    end

    same_12 = Bi == Bj && deriv[1] == deriv[2]
    same_13 = Bi == Bl && deriv[1] == deriv[3]
    same_23 = Bj == Bl && deriv[2] == deriv[3]

    # Quadrature information (weights, nodes).
    quadx, quadw = _quadrature_prod(Val(3k - 3))

    fill!(A, 0)
    T = eltype(A)
    Al = @MMatrix zeros(T, 2k - 1, 2k - 1)
    nlast = last(eachindex(ts))

    @inbounds for n in eachindex(ts)
        n == nlast && break
        tn, tn1 = ts[n], ts[n + 1]
        tn1 == tn && continue  # interval of length = 0

        metric = QuadratureMetric(tn, tn1)
        xs = metric .* quadx

        is = nonzero_in_segment(Bi, n)
        js = nonzero_in_segment(Bj, n)
        ls = nonzero_in_segment(Bl, n)

        bis = eval_basis_functions(Bi, is, xs, deriv[1])
        bjs = same_12 ?
            bis : eval_basis_functions(Bj, js, xs, deriv[2])
        bls = same_13 ?
            bis : same_23 ?
            bjs : eval_basis_functions(Bl, ls, xs, deriv[3])

        # We compute the submatrix A[:, :, l] for each `l`.
        for (nl, l) in enumerate(ls)
            fill!(Al, 0)
            inds = BandedTensors.band_indices(A, l)
            # @assert is ⊆ inds && js ⊆ inds
            # @assert size(Al, 1) == size(Al, 2) == length(inds)
            δi = searchsortedlast(inds, is[1]) - 1
            δj = searchsortedlast(inds, js[1]) - 1
            for nj in eachindex(js), ni in eachindex(is)
                Al[ni + δi, nj + δj] =
                    metric.α * ((bis[ni] .* bjs[nj] .* bls[nl]) ⋅ quadw)
            end
            A[:, :, l] += Al
        end
    end

    A
end


# struct LogitGating{T<:Array{Union{Nothing, Number}}} <: NonRandomGating
#     α::T
#     LogitGating{T}(α::T) where {T<:Array{Union{Nothing, Number}}} = new{T}(α::T)
# end

# function LogitGating(α::T; check_args=true) where {T <: Array{Union{Nothing, Number}}}
#     check_args # && @check_args(LogitGating, isa.(α, Number))
#     return LogitGating{T}(α)
# end

#### Outer constructors
# LogitGating(α::Array{Union{Nothing, Real}}) = LogitGating(promote(α)...)
# LogitGating(α::Array{Union{Nothing, Integer}}) = LogitGating(float(α))

# SHOULD BE ARRAY TYPE

function LogitGating(α, x; check_args = true)
    check_args && @check_args(LogitGating, size(α)[2] == size(x)[2])
    ax = x * α'
    rowsum = rowlogsumexp(ax) # logsumexp.(ax[row,:] for row in 1:size(ax)[1])
    return ax .- rowsum
    # return (x * α') .- logsumexp.((x * α')[row,:] for row in 1:size((x * α'))[1])
end


GetGeodesicODE(Metric::Function, InitialPos::AbstractVector{<:Number}, approx::Bool=false; kwargs...) = GetGeodesicODE(Metric, InitialPos, Val(approx); kwargs...)
function GetGeodesicODE(Metric::Function, InitialPos::AbstractVector{<:Number}, approx::Val{false}; kwargs...)
    function GeodesicODE!(du,u,p,t)
        n = length(u)÷2
        du[1:n] = u[(n+1):end]
        du[(n+1):end] = ChristoffelTerm(ChristoffelSymbol(Metric,u[1:n]; kwargs...),du[1:n])
    end
end
function GetGeodesicODE(Metric::Function, InitialPos::AbstractVector{<:Number}, approx::Val{true}; kwargs...)
    Γ = ChristoffelSymbol(Metric, InitialPos; kwargs...)
    function ApproxGeodesicODE!(du,u,p,t)
        n = length(u)÷2
        du[1:n] = u[(n+1):end]
        du[(n+1):end] = ChristoffelTerm(Γ, du[1:n])
    end
end

# accuracy ≈ 6e-11
function ComputeGeodesic(Metric::Function, InitialPos::AbstractVector, InitialVel::AbstractVector, Endtime::Number=50.;
                        Boundaries::Union{Function,Nothing}=nothing, tol::Real=1e-11, meth::OrdinaryDiffEqAlgorithm=GetMethod(tol), approx::Bool=false, kwargs...)
    @assert length(InitialPos) == length(InitialVel)
    prob = ODEProblem(GetGeodesicODE(Metric, InitialPos, approx), vcat(InitialPos,InitialVel), (0.0,Endtime))
    if isnothing(Boundaries)
        return solve(prob, meth; reltol=tol, abstol=tol, kwargs...)
    else
        return solve(prob, meth; reltol=tol, abstol=tol, callback=DiscreteCallback(Boundaries,terminate!), kwargs...)
    end
end

"""
    ComputeGeodesic(DM::DataModel, InitialPos::AbstractVector, InitialVel::AbstractVector, Endtime::Number=50.;
                                    Boundaries::Union{Function,Nothing}=nothing, tol::Real=1e-11, approx::Bool=false, meth=Tsit5())
Constructs geodesic with given initial position and velocity.
It is possible to specify a boolean-valued function `Boundaries(u,t,int)`, which terminates the integration process when it returns `true`.

By setting the keyword `approx=true`, the ChristoffelSymbols are assumed to be constant and only computed once at the initial position. This simplifies the computation immensely but may also constitute an inaccurate approximation depending on the magnitude of the Ricci curvature.
"""
ComputeGeodesic(DM::AbstractDataModel, args...; kwargs...) = ComputeGeodesic(FisherMetric(DM), args...; kwargs...)

"""
    GeodesicLength(DM::DataModel,sol::AbstractODESolution, Endrange::Number=sol.t[end]; FullSol::Bool=false, tol=1e-14)
    GeodesicLength(Metric::Function,sol::AbstractODESolution, Endrange::Number=sol.t[end]; FullSol::Bool=false, tol=1e-14)
Calculates the length of a geodesic `sol` using the `Metric` up to parameter value `Endrange`.
```math
L[\\gamma] \\coloneqq \\int_a^b \\mathrm{d} t \\, \\sqrt{g_{\\gamma(t)} \\big(\\dot{\\gamma}(t), \\dot{\\gamma}(t)\\big)}
```
"""
GeodesicLength(DM::AbstractDataModel, args...; kwargs...) = GeodesicLength(FisherMetric(DM), args...; kwargs...)
function GeodesicLength(Metric::Function, sol::AbstractODESolution, Endrange::Number=sol.t[end]; FullSol::Bool=false, tol::Real=1e-14, kwargs...)
    @assert length(sol.u[1]) % 2 == 0
    n = length(sol.u[1])÷2
    function Integrand(t)
        FullGamma = sol(t)
        InnerProduct(Metric(FullGamma[1:n]), FullGamma[(n+1):end]) |> sqrt
    end
    return Integrate1D(Integrand, (sol.t[1],Endrange); FullSol=FullSol, tol=tol, kwargs...)
end

"""
    GeodesicCrossing(DM::DataModel, sol::AbstractODESolution, Conf::Real=ConfVol(1); tol=1e-15)
Gives the parameter value of the geodesic `sol` at which the confidence level `Conf` is crossed.
"""
function GeodesicCrossing(DM::AbstractDataModel, sol::AbstractODESolution, Conf::Real=ConfVol(1); tol::Real=1e-15)
    start = sol.t[end]/2
    if (tol < 1e-15)
        start *= one(BigFloat)
        println("GeodesicCrossing: Conf value not programmed as BigFloat yet.")
    end
    A = loglikelihood(DM,sol(0.)[1:2]) - (1/2)*quantile(Chisq(Int(length(sol(0.))/2)),Conf)
    f(t) = A - loglikelihood(DM,sol(t)[1:2])
    find_zero(f,start,Order1B();xatol=tol)
end

"""
    DistanceAlongGeodesic(Metric::Function,sol::AbstractODESolution,L::Number; tol=1e-14)
Calculates at which parameter value of the geodesic `sol` the length `L` is reached.
"""
function DistanceAlongGeodesic(Metric::Function, sol::AbstractODESolution, L::Number; tol::Real=1e-14)
    L < 0 && throw(BoundsError("DistanceAlongGeodesic: L=$L"))
    # Use interpolated Solution of integral for improved accuracy
    GeoLength = GeodesicLength(Metric,sol,sol.t[end], FullSol=true, tol=tol)
    Func(x) = L - GeoLength(x)
    dFunc(x) = ForwardDiff.derivative(Func,x)
    find_zero((Func,dFunc),sol.t[end]/2*one(typeof(L)),Roots.Newton(),xatol=tol)
end

# Input Array of Geodesics, Output Array of its endpoints
function Endpoints(Geodesics::AbstractVector{<:AbstractODESolution})
    Endpoints = Vector{Vector{Float64}}(undef,0)
    Numb = Int(length(Geodesics[1].u[1])/2)
    for Curve in Geodesics
        T = Curve.t[end]
        push!(Endpoints,Curve(T)[1:Numb])
    end;    Endpoints
end

"""
    EvaluateEach(geos::AbstractVector{<:AbstractODESolution}, Ts::AbstractVector) -> Vector
Evalues a family `geos` of geodesics on a set of parameters `Ts`. `geos[1]` is evaluated at `Ts[1]`, `geos[2]` is evaluated at `Ts[2]` and so on.
The second half of the values respresenting the velocities is automatically truncated.
"""
function EvaluateEach(sols::AbstractVector{<:AbstractODESolution}, Ts::AbstractVector{<:Number})
    length(sols) != length(Ts) && throw(ArgumentError("Dimension Mismatch."))
    n = Int(length(sols[1].u[1])/2)
    Res = Vector{Vector{Float64}}(undef,0)
    for i in 1:length(Ts)
        F = sols[i]
        push!(Res,F(Ts[i])[1:n])
    end
    Res
end

# """
#     Truncated(sol::AbstractODESolution) -> Function
# Given a geodesic `sol`, the second half of the components which represent the velocity are truncated off.
# The result gives the position of the geodesic as a function of the parameter.
# However, since it is no longer of type `ODESolution`, one no longer has access to the fields `sol.t`, `sol.u` and so on.
# """
# Truncated(sol::AbstractODESolution) = (t->sol(t)[1:Int(length(sol.u[1])/2)])

################################################################ MERGE THESE FUNCTION VARIATIONS
# function ConstLengthGeodesics(DM::DataModel,Metric::Function,MLE::AbstractVector,Conf::Float64=ConfVol(1),N::Int=100)
#     angles = [2*pi*n/N      for n in 1:N]
#     sols = Vector{ODESolution}(undef,0)
#     for α in angles
#         InitialVelocity = [cos(α),sin(α)]
#         push!(sols,ConfidenceBoundaryViaGeodesic(DM,Metric,[MLE...,InitialVelocity...],Conf))
#         num = α*N/(2*pi) |> round |> Int
#         println("Calculated Geodesic $num of $N.")
#     end
#     Region = Endpoints(sols);   plot(Unpack(Region))
#     sols
# end

# ADAPT FOR PLANES
function ConstLengthGeodesics(DM::AbstractDataModel, Metric::Function, MLE::AbstractVector, Conf::Real=ConfVol(1), N::Int=100; tol::Real=6e-11)
    angles = [2π*n/N      for n in 1:N]
    Initials = [ [MLE...,cos(alpha),sin(alpha)] for alpha in angles]
    solving = 0
    function Constructor(Initial)
        solving += 1
        println("Computing Geodesic $(solving) / $N\t")
        ConfidenceBoundaryViaGeodesic(DM,Metric,Initial,Conf; tol=tol)
    end
    pmap(Constructor,Initials)
end

function BoundaryViaGeodesic(DM::AbstractDataModel, InitialPos::AbstractVector, InitialVel::AbstractVector,
                                    Confnum::Real=1, Endtime::Real=500.0; dof::Int=length(MLE(DM)), kwargs...)
    WilksCond = (1/2)*quantile(Chisq(dof),ConfVol(Confnum))
    BoundaryFunc(u,t,int) = LogLikeMLE(DM) - loglikelihood(DM, u[1:end÷2]) > WilksCond
    ComputeGeodesic(FisherMetric(DM), InitialPos, InitialVel, Endtime; Boundaries=BoundaryFunc, kwargs...)
end

###############################################################

function pConstParamGeodesics(Metric::Function,MLE::AbstractVector,Endtime::Number=10.,N::Int=100;
    Boundaries::Union{Function,Nothing}=nothing, tol::Real=1e-13, parallel::Bool=true)
    ConstParamGeodesics(Metric,MLE,Endtime,N;Boundaries=Boundaries, tol=tol, parallel=parallel)
end

function ConstParamGeodesics(Metric::Function,MLE::AbstractVector,Endtime::Number=10.,N::Int=100;
    Boundaries::Union{Function,Nothing}=nothing, tol::Real=1e-13, parallel::Bool=false)
    Initials = [ [cos(alpha),sin(alpha)] for alpha in range(0,2π;length=N)];    solving = 0
    Map = parallel ? pmap : map
    function Constructor(Initial)
        solving += 1
        println("Computing Geodesic $(solving) / $N")
        ComputeGeodesic(Metric,MLE,Initial,Endtime;tol=tol,Boundaries=Boundaries)
    end
    Map(Constructor,Initials)
end

GeodesicBoundaryFunction(Cube::HyperCube) = (u,p,t) -> u[1:end÷2] ∉ Cube
function GeodesicBoundaryFunction(M::ModelMap)
    function ModelMapBoundaries(u,p,t)
        S = !IsInDomain(M, u[1:end÷2])
        S && @warn "Geodesic ran into boundaries specified by ModelMap at $(u[1:end÷2])."
        return S
    end
end

# Also add Plane method!
function RadialGeodesics(DM::AbstractDataModel, Cube::HyperCube; N::Int=50, tol::Real=1e-9, Boundaries::Union{Function,Nothing}=nothing, parallel::Bool=false, kwargs...)
    @assert length(Cube) == 2 && MLE(DM) ∈ Cube
    widths = CubeWidths(Cube);    Map = parallel ? pmap : map;    Metric(x) = FisherMetric(DM, x)
    initialvels = [widths .* [cos(α), sin(α)] for α in range(0, 2π*(1-1/N); length=N)]
    CB = DiscreteCallback(GeodesicBoundaryFunction(Cube))
    CB = !isnothing(Boundaries) ? CallbackSet(CB, DiscreteCallback(Boundaries,terminate!)) : CB
    CB = Predictor(DM) isa ModelMap ? CallbackSet(CB, DiscreteCallback(GeodesicBoundaryFunction(Predictor(DM)),terminate!)) : CB
    solving = 0
    function Constructor(InitialVel)
        solving += 1
        println("Computing Geodesic $(solving) / $N")
        # Already added Boundaries(u,p,t) function to callbacks if any was passed via kwarg
        ComputeGeodesic(Metric, MLE(DM), InitialVel, 10.0; tol=tol, Boundaries=nothing, callback=CB, kwargs...)
    end
    Map(Constructor, initialvels)
end

"""
    GeodesicBetween(DM::DataModel, P::AbstractVector{<:Number}, Q::AbstractVector{<:Number}; tol::Real=1e-10, meth=Tsit5())
    GeodesicBetween(Metric::Function, P::AbstractVector{<:Number}, Q::AbstractVector{<:Number}; tol::Real=1e-10, meth=Tsit5())
Computes a geodesic between two given points on the parameter manifold and an expression for the metric.

By setting the keyword `approx=true`, the ChristoffelSymbols are assumed to be constant and only computed once at the initial position. This simplifies the computation immensely but may also constitute an inaccurate approximation depending on the magnitude of the Ricci curvature.
"""
GeodesicBetween(DM::AbstractDataModel, args...; kwargs...) = GeodesicBetween(FisherMetric(DM), args...; kwargs...)
function GeodesicBetween(Metric::Function, P::AbstractVector{<:Number}, Q::AbstractVector{<:Number}, Endtime::Real=10.0; tol::Real=1e-9, meth::OrdinaryDiffEqAlgorithm=GetMethod(tol), approx::Bool=false, kwargs...)
    length(P) != length(Q) && throw("GeodesicBetween: Points not of same dim.")
    dim = length(P)
    function bc!(resid, u, p, t)
        resid[1:dim] = u[1][1:dim] .- P
        resid[(dim+1):2dim] = u[end][1:dim] .- Q
    end
    # Slightly perturb initial direction:
    initial = vcat(P, ((Q - P) ./ Endtime) .+ 1e-8 .*(rand(dim) .- 0.5))
    BVP = BVProblem(GetGeodesicODE(Metric, P, approx), bc!, initial, (0.0, Endtime))
    solve(BVP, Shooting(meth); reltol=tol, abstol=tol, kwargs...)
end

"""
    GeodesicDistance(DM::DataModel, P::AbstractVector{<:Number}, Q::AbstractVector{<:Number}; tol::Real=1e-10)
    GeodesicDistance(Metric::Function, P::AbstractVector{<:Number}, Q::AbstractVector{<:Number}; tol::Real=1e-10)
Computes the length of a geodesic connecting the points `P` and `Q`.
"""
GeodesicDistance(DM::AbstractDataModel, args...; kwargs...) = GeodesicDistance(FisherMetric(DM), args...; kwargs...)
function GeodesicDistance(Metric::Function, P::AbstractVector{<:Number}, Q::AbstractVector{<:Number}; tol::Real=1e-10, meth::OrdinaryDiffEqAlgorithm=GetMethod(tol), kwargs...)
    GeodesicLength(Metric, GeodesicBetween(Metric, P, Q; tol=tol, meth=meth, kwargs...))
end

ParamVol(sol::AbstractODESolution) = sol.t[end] - sol.t[1]
GeodesicEnergy(DM::DataModel,sol::AbstractODESolution,Endrange::Number=sol.t[end];FullSol::Bool=false,tol::Real=1e-14) = GeodesicEnergy(x->FisherMetric(DM,x),sol,Endrange;tol=tol)
function GeodesicEnergy(Metric::Function,sol::AbstractODESolution,Endrange=sol.t[end]; FullSol::Bool=false,tol::Real=1e-14)
    @assert length(sol.u[1]) % 2 == 0
    n = length(sol.u[1])÷2
    function Integrand(t)
        FullGamma = sol(t)
        InnerProduct(Metric(FullGamma[1:n]), FullGamma[(n+1):end])
    end
    Integrate1D(Integrand, [sol.t[1],Endrange]; FullSol=FullSol, tol=tol)
end

"""
    ExponentialMap(Metric::Function, point::AbstractVector{<:Number}, tangent::AbstractVector{<:Number}; tol::Real=1e-9)
Computes the differential-geometric exponential map ``\\mathrm{exp}_p(v)`` which returns the endpoint that is reached by a geodesic ``\\gamma:[0,1] \\longrightarrow \\mathcal{M}`` with initial direction ``v \\in T_p \\mathcal{M}``.
"""
function ExponentialMap(Metric::Function, point::AbstractVector{<:Number}, tangent::AbstractVector{<:Number}; tol::Real=1e-9, meth::OrdinaryDiffEqAlgorithm=GetMethod(tol), FullSol::Bool=false, kwargs...)
    if FullSol
        ComputeGeodesic(Metric, point, tangent, 1.0; tol=tol, meth=meth, kwargs...)
    else
        ComputeGeodesic(Metric, point, tangent, 1.0; tol=tol, meth=meth, save_everystep=false, save_start=false, save_end=true, kwargs...).u[end][1:end÷2]
    end
end
ExponentialMap(DM::AbstractDataModel, args...; kwargs...) = ExponentialMap(FisherMetric(DM), args...; kwargs...)

"""
    LogarithmicMap(Metric::Function, P::AbstractVector{<:Number}, Q::AbstractVector{<:Number}; tol::Real=1e-9)
Computes the inverse of the differential-geometric exponential map, i.e. ``\\mathrm{ln}_p(q) \\equiv (\\mathrm{exp}^{-1})_p(q)`` which returns a (possibly non-unique!) initial direction ``v \\in T_p \\mathcal{M}`` for a geodesic ``\\gamma:[0,1] \\longrightarrow \\mathcal{M}`` that goes from ``p \\in \\mathcal{M}`` to ``q \\in \\mathcal{M}``.
"""
function LogarithmicMap(Metric::Function, P::AbstractVector{<:Number}, Q::AbstractVector{<:Number}; tol::Real=1e-9, meth::OrdinaryDiffEqAlgorithm=GetMethod(tol), FullSol::Bool=false, kwargs...)
    if FullSol
        GeodesicBetween(Metric, P, Q, 1.0; tol=tol, meth=meth, kwargs...)
    else
        GeodesicBetween(Metric, P, Q, 1.0; tol=tol, meth=meth, save_everystep=false, save_start=true, save_end=false, kwargs...).u[1][((end÷2)+1):end]
    end
end
LogarithmicMap(DM::AbstractDataModel, args...; kwargs...) = LogarithmicMap(FisherMetric(DM), args...; kwargs...)

function KarcherMeanStep(Metric::Function, points::AbstractVector{<:AbstractVector{<:Number}}, initialmean::AbstractVector{<:Number}=sum(points)/length(points); kwargs...)
    dirs = map(x->LogarithmicMap(Metric, initialmean, x; kwargs...), points)
    ExponentialMap(Metric, initialmean, sum(dirs) / length(dirs))
end
function KarcherMean(Metric::Function, points::AbstractVector{<:AbstractVector{<:Number}}, initialmean::AbstractVector{<:Number}=sum(points)/length(points); tol::Real=1e-8, meth::OrdinaryDiffEqAlgorithm=GetMethod(tol), maxiter::Int=10, kwargs...)
    @assert ConsistentElDims(points) == length(initialmean)
    oldmean = initialmean
    for iter in 1:maxiter
        println("Karcher Mean iteration $iter.")
        newmean = KarcherMeanStep(Metric, points, oldmean; tol=tol, meth=meth, kwargs...)
        if norm(newmean-oldmean) < 20tol
            return newmean
        elseif iter < maxiter
            oldmean = newmean
            continue;
        else
            @warn "KarcherMean: Hit maxiter. Returning last result."
            return newmean
        end
    end
end

# function MBAMBoundaries(u, t, int, DM; componentlim::Real=1e4, singularlim::Real=1e-8)::Bool
#     if !all(x->abs(x) < componentlim, u)
#         @warn "Terminated because a position / velocity coordinate > $componentlim at: $u."
#         return true
#     elseif svdvals(FisherMetric(DM,u[1:end÷2]))[end] < singularlim
#         @warn "Terminated because Fisher metric became singular (i.e. < $singularlim) at: $u."
#         return true
#     else
#         return false
#     end
# end

"""
Return `true` when integration of ODE should be terminated.
"""
function MBAMBoundaries(DM::AbstractDataModel; componentlim::Real=1e4, singularlim::Real=1e-8)
    function MBAMBoundary(u,t,int)
        if !all(x->abs(x) < componentlim, u)
            @warn "Terminated because a position / velocity coordinate > $componentlim at: $u."
            return true
        elseif svdvals(FisherMetric(DM, u[1:end÷2]))[end] < singularlim
            @warn "Terminated because Fisher metric became singular (i.e. < $singularlim) at: $u."
            return true
        else
            return false
        end
    end
end

function MBAM(DM::AbstractDataModel; Boundaries::Union{Function,Nothing}=nothing, tol::Real=1e-5, meth::OrdinaryDiffEqAlgorithm=GetMethod(tol), componentlim::Real=1e4, singularlim::Real=1e-8, kwargs...)
    InitialVel = normalize(LeastInformativeDirection(DM,MLE(DM)))
    CB = DiscreteCallback(MBAMBoundaries(DM; componentlim=componentlim, singularlim=singularlim), terminate!)
    CB = !isnothing(Boundaries) ? CallbackSet(CB, DiscreteCallback(Boundaries, terminate!)) : CB
    CB = Predictor(DM) isa ModelMap ? CallbackSet(CB, DiscreteCallback(GeodesicBoundaryFunction(Predictor(DM)),terminate!)) : CB
    # Already added Boundaries(u,p,t) function to callbacks if any was passed via kwarg
    ComputeGeodesic(DM, MLE(DM), InitialVel, 1e4; Boundaries=nothing, callback=CB, tol=tol, meth=meth, kwargs...)
    # if isnothing(Boundaries)
    #     MBAMboundary(u,t,int) = MBAMBoundaries(u,t,int,DM)
    #     return ComputeGeodesic(DM, MLE(DM), InitialVel, 1e4; Boundaries=MBAMboundary, tol=tol, meth=meth, kwargs...)
    # else
    #     CombinedBoundaries(u,t,int)::Bool = Boundaries(u,t,int) || MBAMBoundaries(u,t,int,DM)
    #     return ComputeGeodesic(DM, MLE(DM), InitialVel, 1e4; Boundaries=CombinedBoundaries, tol=tol, meth=meth, kwargs...)
    # end
end