@testset "Simulator: Rotations" begin
	# 3D
	a = cos(0.25 * π)
	env = ContactImplicitMPC.Environment{R3,FrictionCone}(x -> 1.0 * x[1], x -> [1.0; 0.0])

	R = rotation(env, [0.0; 0.0])
	λ = [0.0; 0.0; 1.0]
	@test norm(R' * λ - [-a; 0.0; a]) < 1.0e-8

	λ = [1.0; 0.0; 0.0]
	@test norm(R' * λ - [a; 0.0; a]) < 1.0e-8

	λ = [0.0; 1.0; 0.0]
	@test norm(R' * λ - [0.0; 1.0; 0.0]) < 1.0e-8

	env = ContactImplicitMPC.Environment{R3,FrictionCone}(x -> x' * x, x -> 2.0 * x)

	R = rotation(env, [0.0; 0.0])
	λ = [0.0; 0.0; 1.0]
	@test norm(R' * λ - [0.0; 0.0; 1.0]) < 1.0e-8

	λ = [1.0; 0.0; 0.0]
	@test norm(R' * λ - [1.0; 0.0; 0.0]) < 1.0e-8

	λ = [0.0; 1.0; 0.0]
	@test norm(R' * λ - [0.0; 1.0; 0.0]) < 1.0e-8

	a = 1 / 3
	b = 2 / 3
	R = rotation(env, [1.0; 1.0])
	λ = [0.0; 0.0; 1.0]
	@test norm(R' * λ - [-b; -b; a]) < 1.0e-8

	R' * λ
	λ = [1.0; 0.0; 0.0]
	@test norm(R' * λ - [b; -a; b]) < 1.0e-8

	λ = [0.0; 1.0; 0.0]
	@test norm(R' * λ - [-a; b; b]) < 1.0e-8

	# 2D
	a = cos(0.25 * π)
	x1 = [1.0; 0.0]
	x2 = [0.0; 1.0]
	env = ContactImplicitMPC.Environment{R2,FrictionCone}(x -> 1.0 * x[1], x -> 1.0)

	R = rotation(env, [0.0])
	λ = [0.0; 1.0]
	@test norm(R' * λ - [-a; a]) < 1.0e-8

	λ = [1.0; 0.0]
	@test norm(R' * λ - [a; a]) < 1.0e-8
end


################################################################################
# Squencial processing of multiple counts matrices
################################################################################

"""
Multiple Counts Distributed Multinomial Regression by running independent poisson gamma lasso regression to each column of counts,
picks the minimum AICc segement of each path, and returns a coefficient matrix (coefs) representing point estimates
for the entire multinomial (includes the intercept if one was included).
Unlike dmr(), mcdmr() takes a vector of counts matrices and sequencially
  1. regresses each counts matrix on the covars genrating an srproj Z (includes total counts m)
  2. accumulated srproj z into Z
  3. uses [covars Z] for subsequent regressions
Returns the accumulated Z matrix and a vector of coefficient matrices
"""
function mcdmr(covars::AbstractMatrix{T},multicounts::Vector,projdir::Int;
    verbose=true, kwargs...) where {T<:AbstractFloat}
  L = length(multicounts)
  n,p = size(covars)
  verbose && @info("fitting mcdmr to $L counts matrices")
  multicoefs = Vector{DMRCoefs}(undef,L)
  Z = Array{T}(undef,n,0)
	for l=1:L
		verbose && @info("fitting dmr to counts matrix #$l on $p covars + $(size(Z,2)) previous SR projections ...")
		# get l'th counts matrix
		counts = multicounts[l]
		# fit dmr
		multicoefs[l] = dmr([covars Z],counts; verbose=verbose, kwargs...)
		# collapse counts into low dimensional SR projection
		z = srproj(multicoefs[l],counts,projdir)
		# append z to Z
	  Z = [Z z]
	end
  Z, multicoefs
end


Lar = LinearAlgebraicRepresentation

"""
	extrudeSimplicial(model::LAR, pattern::Array)::LAR

Algorithm for multimensional extrusion of a simplicial complex.
Can be applied to 0-, 1-, 2-, ... simplicial models, to get a 1-, 2-, 3-, .... model.
The pattern `Array` is used to specify how to decompose the added dimension.

A `model` is a LAR model, i.e. a pair (vertices,cells) to be extruded, whereas pattern is an array of `Int64`, to be used as lateral measures of the *extruded* model. `pattern` elements are assumed as either *solid* or *empty* measures, according to their (+/-) sign.

# Example
```julia
julia> V = [[0,0] [1,0] [2,0] [0,1] [1,1] [2,1] [0,2] [1,2] [2,2]];

julia> FV = [[1,2,4],[2,3,5],[3,5,6],[4,5,7],[5,7,8],[6,8,9]];

julia> pattern = repeat([1,2,-3],outer=4);

julia> model = (V,FV);

julia> W,FW = extrudeSimplicial(model, pattern);

julia> Plasm.view(W,FW)
```
"""
function extrudeSimplicial(model::Lar.LAR, pattern)
	V = [model[1][:,k] for k=1:size(model[1],2)]
    FV = model[2]
    d, m = length(FV[1]), length(pattern)
    coords = collect(cumsum(append!([0], abs.(pattern))))
    offset, outcells, rangelimit, i = length(V), [], d*m, 0
    for cell in FV
    	i += 1
        tube = [v+k*offset for k in range(0, length=m+1) for v in cell]
        cellTube = [tube[k:k+d] for k in range(1, length=rangelimit)]
        if i==1 outcells = reshape(cellTube, d, m)
        else outcells = vcat(outcells, reshape(cellTube, d, m)) end
    end
    cellGroups = []
    for i in 1:size(outcells, 2)
        if pattern[i]>0
            cellGroups = vcat(cellGroups, outcells[:, i])
        end
    end
    outVertices = [vcat(v, [z]) for z in coords for v in V]
    cellGroups = convert(Array{Array{Int, 1}, 1}, cellGroups)
    outModel = outVertices, cellGroups
    hcat(outVertices...), cellGroups
end
function extrudeSimplicial(model::Union{Any,Lar.Cells}, pattern)
	V,FV = model
    d, m = length(FV[1]), length(pattern)
    coords = collect(cumsum(append!([0], abs.(pattern))))
    offset, outcells, rangelimit, i = length(V), [], d*m, 0
    for cell in FV
    	i += 1
        tube = [v+k*offset for k in range(0, length=m+1) for v in cell]
        cellTube = [tube[k:k+d] for k in range(1, length=rangelimit)]
        if i==1 outcells = reshape(cellTube, d, m)
        else outcells = vcat(outcells, reshape(cellTube, d, m)) end
    end
    cellGroups = []
    for i in 1:size(outcells, 2)
        if pattern[i]>0
            cellGroups = vcat(cellGroups, outcells[:, i])
        end
    end
    outVertices = [vcat(v, [z]) for z in coords for v in V]
    cellGroups = convert(Array{Array{Int, 1}, 1}, cellGroups)
    outModel = outVertices, cellGroups
    hcat(outVertices...), cellGroups
end

"""
	simplexGrid(shape::Array)::LAR

Generate a simplicial complex decomposition of a cubical grid of ``d``-cuboids, where ``d`` is the length of `shape` array. Vertices (0-cells) of the grid have `Int64` coordinates.

# Examples
```julia
julia> simplexGrid([0]) # 0-dimensional complex
# output
([0], Array{Int64,1}[])

julia> V,EV = simplexGrid([1]) # 1-dimensional complex
# output
([0 1], Array{Int64,1}[[1, 2]])

julia> V,FV = simplexGrid([1,1]) # 2-dimensional complex
# output
([0 1 0 1; 0 0 1 1], Array{Int64,1}[[1, 2, 3], [2, 3, 4]])

julia> V,CV = simplexGrid([10,10,1]) # 3-dimensional complex
# output
([0 1 … 9 10; 0 0 … 10 10; 0 0 … 1 1], Array{Int64,1}[[1, 2, 12, 122], [2, 12, 122, 123], [12, 122, 123, 133], [2, 12, 13, 123], [12, 13, 123, 133], [13, 123, 133, 134], [2, 3, 13, 123], [3, 13, 123, 124], [13, 123, 124, 134], [3, 13, 14, 124]  …  [119, 229, 230, 240], [109, 119, 120, 230], [119, 120, 230, 240], [120, 230, 240, 241], [109, 110, 120, 230], [110, 120, 230, 231], [120, 230, 231, 241], [110, 120, 121, 231], [120, 121, 231, 241], [121, 231, 241, 242]])

julia> V
# output
3×242 Array{Int64,2}:
 0  1  2  3  4  5  6  7  8  9  10  0  1  2  3  …   1   2   3   4   5   6   7   8   9  10
 0  0  0  0  0  0  0  0  0  0   0  1  1  1  1     10  10  10  10  10  10  10  10  10  10
 0  0  0  0  0  0  0  0  0  0   0  0  0  0  0      1   1   1   1   1   1   1   1   1   1

julia> using Plasm

julia> hpc = Plasm.lar2exploded_hpc(V,CV) # exploded visualization of the grid

julia> Plasm.view(hpc)

julia> V,HV = simplexGrid([1,1,1,1]) # 4-dim cellular complex from the 4D simplex
# output
([0 1 … 0 1; 0 0 … 1 1; 0 0 … 1 1; 0 0 … 1 1], Array{Int64,1}[[1, 2, 3, 5, 9], [2, 3, 5, 9, 10], [3, 5, 9, 10, 11], [5, 9, 10, 11, 13], [2, 3, 5, 6, 10], [3, 5, 6, 10, 11], [5, 6, 10, 11, 13], [6, 10, 11, 13, 14], [3, 5, 6, 7, 11], [5, 6, 7, 11, 13]  …  [4, 6, 10, 11, 12], [6, 10, 11, 12, 14], [3, 4, 6, 7, 11], [4, 6, 7, 11, 12], [6, 7, 11, 12, 14], [7, 11, 12, 14, 15], [4, 6, 7, 8, 12], [6, 7, 8, 12, 14], [7, 8, 12, 14, 15], [8, 12, 14, 15, 16]])
```
"""
function simplexGrid(shape)
    model = [[]], [[1]]
    for item in shape
        model = extrudeSimplicial(model, fill(1, item))
    end
    V, CV = model
    V = convert(Array{Float64,2}, V)
    return V, CV
end

"""
	simplexFacets(simplices::Cells)::Cells

Compute the `(d-1)`-skeleton (set of `facets`) of a simplicial `d`-complex.

# Example
```julia
julia> V,FV = Lar.simplexGrid([1,1]) # 2-dimensional complex
# output
([0 1 0 1; 0 0 1 1], Array{Int64,1}[[1, 2, 3], [2, 3, 4]])

julia> Plasm.view(V,FV)

julia> W,CW = Lar.extrudeSimplicial((V,FV), [1])
([0.0 1.0 … 0.0 1.0; 0.0 0.0 … 1.0 1.0; 0.0 0.0 … 1.0 1.0],
Array{Int64,1}[[1,2,3,5],[2,3,5,6],[3,5,6,7],[2,3,4,6],[3,4,6,7],[4,6,7,8]])

julia> FW = Lar.simplexFacets(CW)
18-element Array{Any,1}:
[[1,3,5],[5,6,7],[3,5,7],[3,6,7],[4,6,7],[4,7,8],[4,6,8],
[6,7,8],[3,5,6],[2,3,5],[2,3,4],[3,4,7],[1,2,3],[2,4,6],[2,5,6],
[1,2,5],[2,3,6],[3,4,6]]

julia> Plasm.view(W,FW)
```

# Example

```julia
julia> V,(VV,EV,FV,CV) = Lar.cuboidGrid([3,3,3],true)

julia> TV = Lar.simplexFacets(CV)

julia> Plasm.view(V,TV)

```
"""
function simplexFacets(simplices) # TODO: solve bug
    out = []
    d = length(simplices[1])
    for simplex in simplices
    	non_oriented_facets = [ append!(simplex[1:k-1],simplex[k+1:d]) for k=1:d ]
    	if (-1)^d == 1
    		oriented_facets = non_oriented_facets
    	else
    		oriented_facets = [ length(f)==2 ? [f[2],f[1]] : [f[2],f[1],f[3:end]]
    			for f in non_oriented_facets ]
    	end
        append!(out, oriented_facets)
    end
    out = collect(Set(out))
    return convert(Lar.Cells, out)
end

"""
	quads2triangles(quads::Cells)::Cells

Convert an array of *quads* with type `::Lar.Cells` into an array of *triangles*
with the same type.

# Examples

The transformation from quads to triangles works for any 2-complex, embedded in any dimensional space

## 2D example
```
V,FV = Lar.cuboidGrid([4,5])
triangles = Lar.quads2triangles(FV::Lar.Cells)::Lar.Cells
using Plasm
Plasm.view((V,[triangles]))
```
## 3D example
```
V,(VV,EV,FV,CV) = Lar.cuboidGrid([4,5,3],true)
triangles = Lar.quads2triangles(FV::Lar.Cells)::Lar.Cells
using Plasm
Plasm.view((V,[triangles]))
```
"""
function quads2triangles(quads::Lar.Cells)::Lar.Cells
	pairs = [[ Int[v1,v2,v3], Int[v3,v2,v4]] for (v1,v2,v3,v4) in quads ]
	return cat(pairs)
end


path = dirname(@__FILE__)
file = h5open("$path/reference/kalman_filter_args.h5", "r")
y = read(file, "data")

T, R, C    = read(file, "TTT"), read(file, "RRR"), read(file, "CCC")
Q, Z, D, E = read(file, "QQ"), read(file, "ZZ"), read(file, "DD"), read(file, "EE")
s_0, P_0   = read(file, "z0"), read(file, "P0")
close(file)

# Basic Chand Recursion (all arguments, no presample)
out = chand_recursion(y, T, R, C, Q, Z, D, E, s_0, P_0)
@testset "Basic Chand Recursion (no presample, all)" begin
    h5open("$path/reference/kalman_filter_out.h5", "r") do h5
        @test read(h5, "log_likelihood") ≈ out[1]
        @test read(h5, "marginal_loglh") ≈ out[2]
        @test read(h5, "s_TT") ≈ out[3]
        @test read(h5, "P_TT") ≈ out[4]
    end
end

# Chand Recursion (no initial conditions)
out = chand_recursion(y, T, R, C, Q, Z, D, E)
@testset "Chand Recursion (no initial conditions)" begin
      h5open("$path/reference/kalman_filter_out.h5", "r") do h5
          @test read(h5, "log_likelihood") ≈ out[1]
          @test read(h5, "marginal_loglh") ≈ out[2]
          @test read(h5, "s_TT") ≈ out[3]
          @test read(h5, "P_TT") ≈ out[4]
      end
  end

# Chand Recursion (presample)
out = chand_recursion(y, T, R, C, Q, Z, D, E, Nt0 = 4)
@testset "Chand Recursion (presample)" begin
    h5open("$path/reference/kalman_filter_out_presample.h5", "r") do h5
        @test read(h5, "log_likelihood") ≈ out[1]
        @test read(h5, "marginal_loglh") ≈ out[2]
        @test read(h5, "s_TT") ≈ out[3]
        @test read(h5, "P_TT") ≈ out[4]
    end
end


# Copyright (c) 2021 Idiap Research Institute, http://www.idiap.ch/
#  Niccolò Antonello <nantonel@idiap.ch>

sym=Dict("$(Char(i))"=>i for i=97:122)
A = WFST(sym)
W = typeofweight(A)
add_arc!(A,1,2,"a","a",1)
add_arc!(A,1,3,"a","a",2)
add_arc!(A,2,2,"b","b",3)
add_arc!(A,3,3,"b","b",3)
add_arc!(A,2,4,"c","c",5)
add_arc!(A,3,4,"d","d",6)
initial!(A,1)
final!(A,4)
println(A)
@test is_deterministic(A) == false
@test is_acceptor(A)
D = determinize_fsa(A)
#println(D)
@test is_deterministic(D)
@test size(D) == (3,4)
# TODO add more tests


import Base.givens

function givens{T<:AbstractFloat}(u::Array{T,1})

  if length(u) != 2
    error("givens: u should contain just two elements")
  end

  x = u[1]
  y = u[2]

  if y == 0.0
    c = 1.0
    s = 0.0
  elseif norm(y) >= norm(x)
    tau = -x/y
    s = 1.0/(sqrt(1.0+tau'tau))
    c = s*tau
  else
    tau = -y/x
    c = 1.0/(sqrt(1.0+tau'tau))
    s = c*tau
  end

  g = [c' s; -s' c]

  return g

end

function givens{T<:AbstractFloat}(u::Array{Complex{T},1})

  if length(u) != 2
    error("givens: u should contain just two elements")
  end

  x = u[1]
  y = u[2]

  if y == 0.0
    c = 1.0
    s = 0.0
  elseif norm(y) >= norm(x)
    tau = -x/y
    s = 1.0/(sqrt(1.0+tau'tau))
    c = s*tau
  else
    tau = -y/x
    c = 1.0/(sqrt(1.0+tau'tau))
    s = c*tau
  end

  g = [c' s; -s' c]

  return g

end


# Verifiers: tools to verify if an expression is a valid tensor expression of certain type

# test for a valid index
const prime = Symbol("'")
function isindex(ex)
    if isa(ex, Symbol) || isa(ex, Int)
        return true
    elseif isa(ex, Expr) && ex.head == prime && length(ex.args) == 1
        return isindex(ex.args[1])
    else
        return false
    end
end

# test for a simple tensor object indexed by valid indices
istensor(ex) = false
function istensor(ex::Expr)
    if ex.head == :ref || ex.head == :typed_hcat
        if length(ex.args) == 1
            return true
        elseif isa(ex.args[2], Expr) && ex.args[2].head == :parameters
            return all(isindex, ex.args[2].args) || all(isindex, ex.args[3:end])
        else
            return all(isindex, ex.args[2:end])
        end
    elseif ex.head == :typed_vcat && length(ex.args) == 3
        length(ex.args) == 3 || return false
        if isa(ex.args[2], Expr) && (ex.args[2].head == :row || ex.args[2].head == :tuple)
            all(isindex, ex.args[2].args) || return false
        else
            isindex(ex.args[2]) || return false
        end
        if isa(ex.args[3], Expr) && (ex.args[3].head == :row || ex.args[3].head == :tuple)
            all(isindex, ex.args[3].args) || return false
        else
            isindex(ex.args[3]) || return false
        end
        return true
    end
    return false
end

# test for a generalized tensor, i.e. with scalar multiplication and conjugation
isgeneraltensor(ex) = false
function isgeneraltensor(ex::Expr)
    if istensor(ex)
        return true
    elseif ex.head == :call && ex.args[1] == :+ && length(ex.args) == 2
        # unary plus
        return isgeneraltensor(ex.args[2])
    elseif ex.head == :call && ex.args[1] == :- && length(ex.args) == 2
        # unary minus
        return isgeneraltensor(ex.args[2])
    elseif ex.head == :call && ex.args[1] == :conj && length(ex.args) == 2
        # conjugation
        return isgeneraltensor(ex.args[2])
    elseif ex.head == :call && ex.args[1] == :adjoint && length(ex.args) == 2
        # adjoint
        return isgeneraltensor(ex.args[2])
    elseif ex.head == prime && length(ex.args) == 1
        # adjoint
        return isgeneraltensor(ex.args[1])
    elseif ex.head == :call && ex.args[1] == :transpose && length(ex.args) == 2
        # conjugation
        return isgeneraltensor(ex.args[2])
    elseif ex.head == :call && ex.args[1] == :*
        # scalar multiplication
        count = 0
        for i = 2:length(ex.args)
            if isgeneraltensor(ex.args[i])
                count += 1
            elseif !isscalarexpr(ex.args[i])
                return false
            end
        end
        return count == 1
    elseif ex.head == :call && ex.args[1] == :/ && length(ex.args) == 3
        # scalar multiplication
        if isscalarexpr(ex.args[3]) && isgeneraltensor(ex.args[2])
            return true
        end
    elseif ex.head == :call && ex.args[1] == :\ && length(ex.args) == 3
        # scalar multiplication
        if isscalarexpr(ex.args[2]) && isgeneraltensor(ex.args[3])
            return true
        end
    end
    return false
end

function hastraceindices(ex)
    obj, leftind, rightind, = decomposegeneraltensor(ex)
    allind = vcat(leftind, rightind)
    return length(allind) != length(unique(allind))
end

# test for a scalar expression, i.e. no indices
function isscalarexpr(ex::Expr)
    if ex.head == :call && ex.args[1] == :scalar
        return istensorexpr(ex.args[2])
    elseif ex.head in (:ref, :typed_vcat, :typed_hcat)
        return false
    else
        return all(isscalarexpr, ex.args)
    end
end
isscalarexpr(ex::Symbol) = true
isscalarexpr(ex::Number) = true
isscalarexpr(ex) = true

# test for a tensor contraction expression
function istensorcontraction(ex)
    if isa(ex, Expr) && ex.head == :call && ex.args[1] == :*
        return count(istensorexpr, ex.args[2:end]) >= 2
    end
    return false
end

# test for a tensor expression, i.e. something that can be evaluated to a tensor
function istensorexpr(ex)
    if isgeneraltensor(ex)
        return true
    elseif isa(ex, Expr) && ex.head == :call && (ex.args[1] == :+ || ex.args[1] == :-)
        return all(istensorexpr, ex.args[2:end]) # all arguments should be tensor expressions (we are not checking matching indices yet)
    elseif isa(ex, Expr) && ex.head == :call && ex.args[1] == :*
        count = 0
        for i = 2:length(ex.args)
            if istensorexpr(ex.args[i])
                count += 1
            elseif !isscalarexpr(ex.args[i])
                return false
            end
        end
        return count > 0
    elseif isa(ex, Expr) && ex.head == :call && ex.args[1] == :/ && length(ex.args) == 3
        return istensorexpr(ex.args[2]) && isscalarexpr(ex.args[3])
    elseif isa(ex, Expr) && ex.head == :call && ex.args[1] == :\ && length(ex.args) == 3
        return istensorexpr(ex.args[3]) && isscalarexpr(ex.args[2])
    elseif isa(ex, Expr) && ex.head == :call && ex.args[1] == :conj && length(ex.args) == 2
        return istensorexpr(ex.args[2])
    elseif isa(ex, Expr) && ex.head == :call && ex.args[1] == :adjoint && length(ex.args) == 2
        return istensorexpr(ex.args[2])
    elseif isa(ex, Expr) && ex.head == prime
        return istensorexpr(ex.args[1])
    end
    return false
end

# test for assignment (copy into existing tensor) or definition (create new tensor)
isassignment(ex) = false
isdefinition(ex) = false
isassignment(ex::Expr) = ex.head == :(=) || ex.head == :(+=) || ex.head == :(-=)
isdefinition(ex::Expr) = (ex.head == :(:=) || ex.head == :(≔)) && istensor(ex.args[1])


using Bridge, Distributions, StaticArrays
using Test
#import Bridge: b, σ, a, transitionprob
n = 1000
TT = 5
tt = 0.0:TT/n:TT
m = 1000
S = SVector{2,Float64}
M = SArray{Tuple{2,2},Float64,2,4}
B = SMatrix{2,2,Float64}([-1 0.1; -0.2 -1])
mu = 0.1*S([0.2, 0.3])
sigma = SMatrix{2,2,Float64}(2*[-0.212887  0.0687025
  0.193157  0.388997 ])
a = sigma*sigma'

P = LinPro(B, mu, sigma)

u = S([1.0, 0.0])
v = S([.5, 0.0])

@test (norm(Matrix(-P.lambda*B' - B*P.lambda - a))) < eps()

t = 0.5
T = 2.0

@test Bridge.bderiv(t, v, P) == B
@test Bridge.σderiv(t, v, P) == zero(sigma)

dt = 1e-6
@test norm( (Bridge.V(t+dt, T, v, P)-Bridge.V(t, T, v, P))/dt - Bridge.dotV(t, T, v, P) ) < 10*dt

n2 = 150
tt = range(t, stop=T, length=n2)
K = SamplePath(tt, zeros(M, length(tt)))
Bridge.gpHinv!(K, P)
V = SamplePath(tt, zeros(S, length(tt)))
Bridge.gpHinv!(K, P) # warm up
Bridge.gpV!(V, P, v)
Mu = SamplePath(tt, zeros(S, length(tt)))
Mu2 = SamplePath(tt, zeros(S, length(tt)))

solve!(Bridge.R3(), Bridge.b, Mu, u, P)
solve!(BS3(), Bridge.b, Mu2, u, P)
@test (@allocated Bridge.gpHinv!(K, P)) <= 32
@test (@allocated Bridge.gpV!(V, P, v)) == 0
@test norm(K.yy[1]*Bridge.H(t, T, P) - I) < 10/n2^3
@test norm(V.yy[1] - Bridge.V(t, T, v, P)) < 10/n2^3

@test norm(Mu.yy[end] - Bridge.mu(t, u, T, P)) < 10/n2^3
@test norm(Mu2.yy[end] - Bridge.mu(t, u, T, P)) < 10/n2^3

@test norm(Bridge.K(t, T, P) - solve(Bridge.R3(), Bridge._dK, tt, zero(M), P)) < 10/n2^3

# Normal(mu, lambda) is the stationary distribution. check by starting in stationary distribution and evolve 20 time units
X = Bridge.mat(S[solve(EulerMaruyama(), mu + cholupper(P.lambda)*randn(S), sample(tt, Wiener{S}()),P).yy[end] - mu for i in 1:m])

@test supnorm(cov(X, dims=2) - Matrix(P.lambda)) < .1

n = 1000
TT = 0.5
tt = 0.0:TT/n:TT

X = solve(EulerMaruyama(), mu, sample(tt, Wiener{S}()),P)
@test supnorm(quvar(X)-TT*a) < 0.06

theta = 0.7
@test_skip  begin X = Bridge.mat(S[thetamethod(mu + chol(P.lambda)*randn(S), sample(tt, Wiener{S}()), P, theta).yy[end] - mu for i in 1:m])
  supnorm(cov(X,2) - Matrix(P.lambda)) < 1.0
end

Pt = Bridge.Ptilde(Bridge.CSpline(tt[1], tt[end], 1.0, 0.0, 0.0, 1.0), sigma)

@test Bridge.gamma(t, v, Pt) ≈ inv(sigma*sigma')


"""
Struct for Brownian Motion

		bmProcess=BrownianMotion(σ::num1,μ::num2) where {num1,num2 <: Number}

Where:\n
		σ	=	volatility of the process.
		μ	=	drift of the process.
"""
mutable struct BrownianMotion{num <: Number, num1 <: Number, numtype <: Number} <: AbstractMonteCarloEngine{numtype}
    σ::num
    μ::num1
    function BrownianMotion(σ::num, μ::num1) where {num <: Number, num1 <: Number}
        if σ <= 0
            error("Volatility must be positive")
        else
            zero_typed = zero(num) + zero(num1)
            return new{num, num1, typeof(zero_typed)}(σ, μ)
        end
    end
end

export BrownianMotion;

function simulate!(X, mcProcess::BrownianMotion, mcBaseData::MonteCarloConfiguration{type1, type2, type3, SerialMode, type5}, T::numb) where {numb <: Number, type1 <: Integer, type2 <: Integer, type3 <: StandardMC, type5 <: Random.AbstractRNG}
    Nsim = mcBaseData.Nsim
    Nstep = mcBaseData.Nstep
    σ = mcProcess.σ
    μ = mcProcess.μ
    @assert T > 0
    dt = T / Nstep
    mean_bm = μ * dt
    stddev_bm = σ * sqrt(dt)
    isDualZero = mean_bm * stddev_bm * 0
    view(X, :, 1) .= isDualZero
    zero_typed = predict_output_type_zero_(σ, μ)
    Z = Array{typeof(get_rng_type(zero_typed))}(undef, Nsim)
    @inbounds for j = 1:Nstep
        randn!(mcBaseData.rng, Z)
        @. @views X[:, j+1] .= X[:, j] + mean_bm + stddev_bm * Z
    end

    nothing
end

function simulate!(X, mcProcess::BrownianMotion, mcBaseData::MonteCarloConfiguration{type1, type2, type3, SerialMode, type5}, T::numb) where {numb <: Number, type1 <: Integer, type2 <: Integer, type3 <: AntitheticMC, type5 <: Random.AbstractRNG}
    Nsim = mcBaseData.Nsim
    Nstep = mcBaseData.Nstep
    σ = mcProcess.σ
    μ = mcProcess.μ
    @assert T > 0
    dt = T / Nstep
    mean_bm = μ * dt
    stddev_bm = σ * sqrt(dt)
    isDualZero = mean_bm * stddev_bm * 0
    view(X, :, 1) .= isDualZero
    Nsim_2 = div(Nsim, 2)
    zero_typed = predict_output_type_zero_(σ, μ)
    Z = Array{typeof(get_rng_type(zero_typed))}(undef, Nsim_2)
    Xp = @views X[1:Nsim_2, :]
    Xm = @views X[(Nsim_2+1):end, :]
    @inbounds for j = 1:Nstep
        randn!(mcBaseData.rng, Z)
        @. @views Xp[:, j+1] .= Xp[:, j] + mean_bm + stddev_bm * Z
        @. @views Xm[:, j+1] .= Xm[:, j] + mean_bm - stddev_bm * Z
    end

    nothing
end


cross(o::Vector{Float64}, a::Vector{Float64}, b::Vector{Float64}) = (a[1]-o[1])*(b[2]-o[2]) - (a[2]-o[2])*(b[1]-o[1])

function convex(points::Vector{Vector{Float64}})
    lower = Vector{Float64}[]
    upper = Vector{Float64}[]
    lt(a,b) = a[1] == b[1] ? a[2] < b[2] : a[1] < b[1]
    sort!(points, lt=lt)
    for i=1:length(points)
        while length(lower) >= 2 && cross(lower[end-2], lower[end-1], points[i]) <= 0
            pop!(lower)
        end
        push!(lower, points[i])
    end
    for i=length(points):-1:1
        while length(upper) >= 2 && cross(upper[end-2], upper[end-1], points[i]) <= 0
            pop!(upper)
        end
        push!(upper, points[i])
    end
    pop!(upper)
    pop!(lower)
    coords = [lower, upper]
    push!(coords, coords[1])
    GeoJSON.Feature(GeoJSON.Polygon(Vector{Vector{Float64}}[coords]))
end


struct LagrangeTensorProductBasis{dim,NF,NF1D,T} <: AbstractBasis{dim,NF}
    basis::LagrangeBasis{NF1D,T}
    points::Matrix{T}
    function LagrangeTensorProductBasis(dim,order;start = -1.0,stop = 1.0)
        @assert 1 <= dim <= 3
        basis1d = LagrangeBasis(order, start = start, stop = stop)
        NF1D = number_of_basis_functions(basis1d)
        NF = NF1D^dim
        T = type_of_interpolation_points(basis1d)
        points = tensor_product_points(dim,basis1d.points')
        new{dim,NF,NF1D,T}(basis1d,points)
    end
end

function Base.show(io::IO, basis::LagrangeTensorProductBasis{dim,NF,NF1D,T}) where {dim,NF,NF1D,T}
    p = order(basis)
    str = "LagrangeTensorProductBasis\n\tDimension  : $dim\n\tOrder      : $p\n\tNum. Funcs.: $NF"
    print(io,str)
end

function order(basis::LagrangeTensorProductBasis)
    return order(basis.basis)
end

function (B::LagrangeTensorProductBasis{1})(x)
    return B.basis(x)
end

function (B::LagrangeTensorProductBasis{1})(x::V) where {V<:AbstractVector}
    @assert length(x) == 1
    return B(x[1])
end

function (B::LagrangeTensorProductBasis{2})(x,y)
    return kron(B.basis(x),B.basis(y))
end

function (B::LagrangeTensorProductBasis{2})(x::V) where {V<:AbstractVector}
    @assert length(x) == 2
    return B(x[1],x[2])
end

function (B::LagrangeTensorProductBasis{3})(x,y,z)
    return kron(B.basis(x),B.basis(y),B.basis(z))
end

function (B::LagrangeTensorProductBasis{3})(x::V) where {V<:AbstractVector}
    @assert length(x) == 3
    return B(x[1],x[2],x[3])
end

function gradient(B::LagrangeTensorProductBasis{1},x)
    return derivative(B.basis,x)
end

function gradient(B::LagrangeTensorProductBasis{1},x::V) where {V<:AbstractVector}
    @assert length(x) == 1
    return gradient(B,x[1])
end

function gradient(B::LagrangeTensorProductBasis{2},dir::Z,x,y) where {Z<:Integer}
    @assert 1 <= dir <= 2
    if dir == 1
        dNx = derivative(B.basis,x)
        Ny = B.basis(y)
        return kron(dNx,Ny)
    else
        Nx = B.basis(x)
        dNy = derivative(B.basis,y)
        return kron(Nx,dNy)
    end
end

function gradient(B::LagrangeTensorProductBasis{2},x,y)
    col1 = gradient(B,1,x,y)
    col2 = gradient(B,2,x,y)
    return hcat(col1,col2)
end

function gradient(B::LagrangeTensorProductBasis{2},dir,x::V) where {V<:AbstractVector}
    @assert length(x) == 2
    @assert 1 <= dir <= 2
    return gradient(B,dir,x[1],x[2])
end

function gradient(B::LagrangeTensorProductBasis{2},x::V) where {V<:AbstractVector}
    @assert length(x) == 2
    return gradient(B,x[1],x[2])
end

function gradient(B::LagrangeTensorProductBasis{3},dir,x,y,z)
    @assert 1 <= dir <= 3
    if dir == 1
        dNx = derivative(B.basis,x)
        Ny = B.basis(y)
        Nz = B.basis(z)
        return kron(dNx,Ny,Nz)
    elseif dir == 2
        Nx = B.basis(x)
        dNy = derivative(B.basis,y)
        Nz = B.basis(z)
        return kron(Nx,dNy,Nz)
    else
        Nx = B.basis(x)
        Ny = B.basis(y)
        dNz = derivative(B.basis,z)
        return kron(Nx,Ny,dNz)
    end
end

function gradient(B::LagrangeTensorProductBasis{3},x,y,z)
    Nx = B.basis(x)
    Ny = B.basis(y)
    Nz = B.basis(z)

    dNx = derivative(B.basis,x)
    dNy = derivative(B.basis,y)
    dNz = derivative(B.basis,z)

    return hcat(kron(dNx,Ny,Nz),kron(Nx,dNy,Nz),kron(Nx,Ny,dNz))
end

function gradient(B::LagrangeTensorProductBasis{3},dir,x::V) where {V<:AbstractVector}
    @assert length(x) == 3
    @assert 1 <= dir <= 3
    return gradient(B,dir,x[1],x[2],x[3])
end

function gradient(B::LagrangeTensorProductBasis{3},x::V) where {V<:AbstractVector}
    @assert length(x) == 3
    return gradient(B,x[1],x[2],x[3])
end

function hessian(B::LagrangeTensorProductBasis{1},x)
    return hessian(B.basis,x)
end

function hessian(B::LagrangeTensorProductBasis{1},x::V) where {V<:AbstractVector}
    @assert length(x) == 1
    return hessian(B,x[1])
end

function hessian(B::LagrangeTensorProductBasis{2},x,y)

    d2Nx = hessian(B.basis,x)
    d1Nx = derivative(B.basis,x)
    d0Nx = B.basis(x)

    d2Ny = hessian(B.basis,y)
    d1Ny = derivative(B.basis,y)
    d0Ny = B.basis(y)

    Nxx = kron(d2Nx,d0Ny)
    Nxy = kron(d1Nx,d1Ny)
    Nyy = kron(d0Nx,d2Ny)

    return hcat(Nxx,Nxy,Nyy)
end

function hessian(B::LagrangeTensorProductBasis{2},x::V) where {V<:AbstractVector}
    @assert length(x) == 2
    return hessian(B,x[1],x[2])
end

function number_of_1d_points(p::M) where {M<:AbstractMatrix}
    m,n = size(p)
    @assert m == 1
    return n
end

function tensor_product_points(N,points)
    @assert 1 <= N <= 3

    np = number_of_1d_points(points)

    if N == 1
        return points
    elseif N == 2
        row1 = repeat(points,inner=(1,np))
        row2 = repeat(points,outer=(1,np))
        return vcat(row1,row2)
    else
        np2 = np^2
        row1 = repeat(points,inner=(1,np2))
        row2 = repeat(repeat(points,inner=(1,np)),outer=(1,np))
        row3 = repeat(points,outer=(1,np2))
        return vcat(row1,row2,row3)
    end
end


#! /usr/bin/julia

# Rosetta Code, Digital root

function digitalroot{S<:Integer,T<:Integer}(n::S, bs::T=10)
    -1 < n && 1 < bs || throw(DomainError())
    ds = n
    pers = 0
    while bs <= ds
        ds = sum(digits(ds, bs))
        pers += 1
    end
    return (pers, ds)
end

for i in {627615, 39390, 588225, 393900588225, big(2)^100}
    (pers, ds) = digitalroot(i)
    println(i, " has persistence ", pers, " and digital root ", ds)
end


import Base: atan2, cross, dot, normalize
export norm2, perp, project, projectN, reflect, reflectN, rotate
export projectNextrema, voronoi_region, overlapping, ininterval, minmaxV, wrap1

norm2(v::SVector{2}) = dot(v,v)
perp(v::SVector{2}) = SVector(v[2],-v[1])
cross(v1::SVector{2}, v2::SVector{2}) = v1[1]*v2[2] - v1[2]*v2[1]
project(v1::SVector{2}, v2::SVector{2}) = (dot(v1,v2) / norm2(v2)) * v2
projectN(v1::SVector{2}, v2::SVector{2}) = dot(v1,v2) * v2
reflect(v1::SVector{2}, ax::SVector{2}) = v1 - 2*project(v1,ax)
reflectN(v1::SVector{2}, ax::SVector{2}) = v1 - 2*projectN(v1,ax)
function rotate(v::SVector{2}, θ)
    c = cos(θ)
    s = sin(θ)
    SVector(v[1]*c - v[2]*s, v[1]*s + v[2]*c)
end
atan2(v::SVector{2}) = atan2(v[2], v[1])
function projectNextrema{T}(pts::AbstractVector{SVector{2,T}}, n::SVector{2,T})
    dmin = T(Inf)
    dmax = -T(Inf)
    for p in pts
        d = dot(p,n)
        d < dmin && (dmin = d)
        d > dmax && (dmax = d)
    end
    SVector(dmin, dmax)
end
function voronoi_region(p::SVector{2}, v::SVector{2})
    d = dot(p,v)
    d < 0 && return -1
    d > norm2(v) && return 1
    return 0
end
overlapping(I1::SVector{2}, I2::SVector{2}) = I1[1] <= I2[2] && I2[1] <= I1[2]
ininterval(x, I::SVector{2}) = I[1] <= x <= I[2]
minmaxV(x, y) = x < y ? SVector(x,y) : SVector(y,x)  # Base.minmax returns tuple
wrap1(i, N) = i < 1 ? N : i > N ? 1 : i      # marginally faster than mod1


baseprice = (3,1,2,5)
currentprice = (5,3,1,6)
baseqty = (25,50,30,15)
currentqty = (28,60,30,12)

@test FisherIndex(baseprice, currentprice, baseqty, currentqty)==1.55835405667883


# The following method was removed due to the fact that it is a type piracy from Distributions.jl
# Too bad;

#Distributions.convolve(d1::Distributions.Gamma,d2::Distributions.Gamma) = UnivariateGammaConvolution([d1.α,d2.α],[d1.θ,d2.θ])
Distributions.convolve(d1::Distributions.Gamma,d2::UnivariateGammaConvolution) = UnivariateGammaConvolution([d1.α,d2.α...], [d1.θ,d2.θ...])
Distributions.convolve(d1::UnivariateGammaConvolution,d2::Distributions.Gamma) = Distribution.convolve(d2,d1)
# Products of UnivariateGammaConvolution

"""
    Distributions.product_distribution(d1,d2)

Implements the special case of the product of two gamma or univariate gamma convolutions distributions, and
output as the result a multivariate gamma convolution with independent margins.
"""
function Distributions.product_distribution(dists::AbstractVector{<:UnivariateGammaConvolution})

    # number of gammas :
    T = Base.promote_eltype(dists)
    d = length(dists)
    ns = [length(d.α) for d in dists]
    n = sum(ns)
    α = zeros(T,n)
    θ = zeros(T,(n,d))
    current_n = 1
    for i in 1:d
        α[current_n:(current_n-1+ns[i])] = dists[i].α
        θ[current_n:(current_n-1+ns[i]),i] = dists[i].θ
        current_n +=ni
    end
    return MultivariateGammaConvolution(α,θ)
end


export   LinearTransform,
         MinMaxTransform,
         updateLinearTransform,
         transform!,
         transform,
         itransform!,
         itransform

 """
 $(TYPEDEF)

 This type holds an affine transformation of the form x-> μ + Lx, where μ is an offset and L is a linear operator.
 We typically use this type to normalize samples to have zero mean and unit covariance of element-wise standard deviation.

 ## Fields

 $(TYPEDFIELDS)

 """
struct LinearTransform
    Nx::Int64

    # Flag for using diagonal or dense transformation
    μ::Array{Float64,1}

    L::Union{Diagonal, LowerTriangular}

    diag::Bool
end

# Identity transformation
LinearTransform(Nx::Int64) = LinearTransform(Nx, zeros(Nx), Diagonal(ones(Nx)), true)

function LinearTransform(X::AbstractMatrix{Float64}; diag::Bool=true, factor::Float64=1.0)
    # The entry `factor` allows to set the entry-wise standard deviation to a value different from 1
    Nx, Ne = size(X)
    μ = deepcopy(mean(X; dims = 2)[:,1])

    if diag == true || Nx == 1
        σ = std(X; dims = 2)[:,1]
        L = deepcopy((1.0/factor)*Diagonal(σ))
        diag = true
    else #create a dense transformation from the Cholesky factor
        @assert Nx>1 "Only works for Nx>1, otherwise use first method"
        L = deepcopy((1.0/factor)*cholesky(cov(X')).L)
    end

    return LinearTransform(Nx, μ, L, diag)
end

function updateLinearTransform!(Lin::LinearTransform, X::AbstractMatrix{Float64}; diag::Bool=true)
    Nx, Ne = size(X)
    Lin.μ .= copy(mean(X; dims = 2)[:,1])

    if diag == true || Nx == 1
        σ = copy(std(X; dims = 2)[:,1])
        Lin.L .= Diagonal(σ)

    else #create a dense transformation from the Cholesky factor
        @assert Nx>1 "Only works for Nx>1, otherwise use diagonal scaling"
        Lin.L = copy(cholesky(cov(X')).L)
    end
end

# For scalar vectors
function transform!(L::LinearTransform, xout::AbstractVector{Float64}, xin::AbstractVector{Float64})
    @assert size(xout,1) == size(xin,1) "Input and output dimensions don't match"
    @assert size(xin,1) == L.Nx "Input dimension is incorrect"

    copy!(xout, xin)
    xout -= L.μ

    ldiv!(L.L, xout)
end

function transform!(L::LinearTransform, x::AbstractVector{Float64})
    @assert size(x,1) == L.Nx "Input dimension is incorrect"
    x -= L.μ
    ldiv!(L.L, x)
    return x
end

transform(L::LinearTransform, x::AbstractVector{Float64}) = transform!(L, zero(x), x)

function itransform!(L::LinearTransform, xout::AbstractVector{Float64}, xin::AbstractVector{Float64})
    @assert size(xout) == size(xin) "Input and output dimensions don't match"
    @assert size(xin,1) == L.Nx "Input dimension is incorrect"

    copy!(xout, xin)
    mul!(xout, L.L, xout)
    xout += L.μ

    return xout
end

function itransform!(L::LinearTransform, x::AbstractVector{Float64}, idx::Union{UnitRange{Int64}, Array{Int64,1}})
    @assert size(x,1) == L.Nx "Input dimension is incorrect"

    if typeof(L.L)<:Diagonal
        mul!(x, Diagonal(view(L.L.diag,idx)), x)
    else
        error("Not yet implemented")
    end

    # mul!(X, L.L, X)
    x += L.μ

    return x
end

function itransform!(L::LinearTransform, x::AbstractVector{Float64})
    @assert size(x,1) == L.Nx "Input dimension is incorrect"

    mul!(x, L.L, x)
    x += L.μ

    return x
end

itransform(L::LinearTransform, x::AbstractVector{Float64}) = itransform!(L, zero(x), x)

# For ensemble matrix
function transform!(L::LinearTransform, Xout::AbstractArray{Float64}, Xin::AbstractMatrix{Float64})
    @assert size(Xout,1) == size(Xin,1) "Input and output dimensions don't match"
    @assert size(Xin,1) == L.Nx "Input dimension is incorrect"

    copy!(Xout, Xin)
    Xout .-= L.μ

    ldiv!(L.L, Xout)
    # return Xout
end

function transform!(L::LinearTransform, X::AbstractMatrix{Float64}, idx::Union{UnitRange{Int64}, Array{Int64,1}})
    @assert size(X,1) == length(idx) "Input dimension is incorrect"
    X .-= view(L.μ,idx)
    if typeof(L.L)<:Diagonal
        ldiv!(Diagonal(view(L.L.diag,idx)), X)
    else
        error("Not yet implemented")
    end
    return X
end

function transform!(L::LinearTransform, X::AbstractMatrix{Float64})
    @assert size(X,1) == L.Nx "Input dimension is incorrect"
    X .-= L.μ
    ldiv!(L.L, X)
    return X
end

transform(L::LinearTransform, X::AbstractMatrix{Float64}) = transform!(L, zero(X), X)

transform(X::AbstractMatrix{Float64}; diag::Bool = true) = transform!(LinearTransform(X; diag = diag), zero(X), X)

function itransform!(L::LinearTransform, Xout::AbstractMatrix{Float64}, Xin::AbstractMatrix{Float64})
    @assert size(Xout) == size(Xin) "Input and output dimensions don't match"
    @assert size(Xin,1) == L.Nx "Input dimension is incorrect"

    copy!(Xout, Xin)
    mul!(Xout, L.L, Xout)
    Xout .+= L.μ

    return Xout
end

function itransform!(L::LinearTransform, X::AbstractMatrix{Float64}, idx::Union{UnitRange{Int64}, Array{Int64,1}})
    @assert size(X,1) == L.Nx "Input dimension is incorrect"

    if typeof(L.L)<:Diagonal
        mul!(X, Diagonal(view(L.L.diag,idx)), X)
    else
        error("Not yet implemented")
    end

    # mul!(X, L.L, X)
    X .+= L.μ

    return X
end

function itransform!(L::LinearTransform, X::AbstractMatrix{Float64})
    @assert size(X,1) == L.Nx "Input dimension is incorrect"

    mul!(X, L.L, X)
    X .+= L.μ

    return X
end

itransform(L::LinearTransform, X::AbstractMatrix{Float64}) = itransform!(L, zero(X), X)

# Define a min-max transformation to use with the package c3
struct MinMaxTransform
    Nx::Int64

    L::Diagonal #(maximum(X[i,:] - minimum(X[i,:]))

    b::Array{Float64,1} #minimum(X[i,:])/(maximum(X[i,:] - minimum(X[i,:]))
end

function MinMaxTransform(X::AbstractMatrix{Float64})
    Nx = size(X,1)
     b = zeros(Nx)
     L = Diagonal(zeros(Nx))

     @inbounds for i=1:Nx
         xi = view(X,i,:)
         Mi = maximum(xi)
         mi = minimum(xi)
         @assert Mi>mi "Minimum and maximum are equal"
         L.diag[i] = (Mi - mi)/2
         b[i] = (Mi + mi)/(Mi - mi)
     end
    return MinMaxTransform(Nx, L, b)
end

function transform!(L::MinMaxTransform, Xout::AbstractMatrix{Float64}, Xin::AbstractMatrix{Float64})
    @assert size(Xout,1) == size(Xin,1) "Input and output dimensions don't match"
    @assert size(Xin,1) == L.Nx "Input dimension is incorrect"

    copy!(Xout, Xin)
    ldiv!(L.L, Xout)
    Xout .-= L.b

    # return Xout
end

function transform!(L::MinMaxTransform, X::AbstractMatrix{Float64})
    @assert size(X,1) == L.Nx "Input dimension is incorrect"
    ldiv!(L.L, X)
    X .-= L.b
    return X
end

transform(L::MinMaxTransform, X::AbstractMatrix{Float64}) = transform!(L, zero(X), X)

function itransform!(L::MinMaxTransform, Xout::AbstractMatrix{Float64}, Xin::AbstractMatrix{Float64})
    @assert size(Xout,1) == size(Xin,1) "Input and output dimensions don't match"
    @assert size(Xin,1) == L.Nx "Input dimension is incorrect"

    copy!(Xout, Xin)
    Xout .+= L.b
    mul!(Xout, L.L, Xout)
    return Xout
end

function itransform!(L::MinMaxTransform, X::AbstractMatrix{Float64})
    X .+= L.b
    mul!(X, L.L, X)
    return X
end

itransform(L::MinMaxTransform, X::AbstractMatrix{Float64}) = itransform!(L, zero(X), X)


using GR

r = 1
θ, φ = meshgrid(range(0, stop=2π, length=200), range(0, stop=2π, length=100))

x = r * sin.(θ) .* sin.(φ)
y = r * sin.(θ) .* cos.(φ)
z = r * cos.(θ)

surface(x, y, z)


##########################################################################################
# Uncertain values represented by theoretical distributions.
# Base stats can be estimated directly from the distributions, so no need for resampling.
# These methods definitions are just for compatibility with other uncertain values, which
# may not have analytically determined statistics associated with them.
##########################################################################################
StatsBase.mode(uv::TheoreticalDistributionScalarValue, n::Int) = mode(uv)
Statistics.mean(uv::TheoreticalDistributionScalarValue, n::Int) = mean(uv)
Statistics.median(uv::TheoreticalDistributionScalarValue, n::Int) = median(uv)
Statistics.middle(uv::TheoreticalDistributionScalarValue, n::Int) = middle(uv)
Statistics.quantile(uv::TheoreticalDistributionScalarValue, q, n::Int) = quantile(uv, q)
Statistics.std(uv::TheoreticalDistributionScalarValue, n::Int) = std(uv)
Statistics.var(uv::TheoreticalDistributionScalarValue, n::Int) = var(uv)

StatsBase.mode(uv::UncertainScalarTheoreticalFit, n::Int) = mode(uv)
Statistics.mean(uv::UncertainScalarTheoreticalFit, n::Int) = mean(uv)
Statistics.median(uv::UncertainScalarTheoreticalFit, n::Int) = median(uv)
Statistics.middle(uv::UncertainScalarTheoreticalFit, n::Int) = middle(uv)
Statistics.quantile(uv::UncertainScalarTheoreticalFit, q, n::Int) = quantile(uv, q)
Statistics.std(uv::UncertainScalarTheoreticalFit, n::Int) = std(uv)
Statistics.var(uv::UncertainScalarTheoreticalFit, n::Int) = var(uv)


module  ProximalFunctionTest

using Test
using ProximalBase
using Random
using LinearAlgebra
using Distributions
import JuMP, Ipopt

Random.seed!(1)

@testset "prox zero" begin
  f = ProxZero()

  p = 10
  x = randn(p)
  z = zeros(p)

  @test prox!(f, z, x) == x
  @test prox(f, x) == z
end

@testset "proximal_l1" begin
  @testset "shrink to zero" begin
    x = randn(10)
    lambda = maximum(abs, x) + 0.1
    g = ProxL1(lambda)

    hat_x = randn(10)
    prox!(g, hat_x, x)
    @test maximum(abs, hat_x) == 0.

    @test value(g, x) ≈ lambda * sum(abs, x)
  end

  @testset "shrink towards zero" begin
    x = [1.0, 1.4, -3.2]
    lambda = 0.1
    g = ProxL1(lambda)

    hat_x = randn(3)
    prox!(g, hat_x, x)
    @test  hat_x ≈ [0.9, 1.3, -3.1]

    @test value(g, x) ≈ lambda * sum(abs, x)
    @test value(g, hat_x) ≈ lambda * sum(abs, hat_x)
  end

  @testset "shrink with vector" begin
    x = [1.0, 1.4, -3.2]
    lambda = [0.1, 1.5, 1]
    g = ProxL1(1., lambda)

    hat_x = randn(3)
    prox!(g, hat_x, x)
    @test hat_x ≈ [0.9, 0., -2.2]
    @test prox(g, x) ≈ [0.9, 0., -2.2]

    @test value(g, x) ≈ dot(lambda, abs.(x))
    @test value(g, hat_x) ≈ dot(lambda, abs.(hat_x))
  end

end

@testset "proximal_l1_fused" begin

    @testset "random" begin

    m = JuMP.Model(JuMP.with_optimizer(Ipopt.Optimizer, print_level=0))

    JuMP.@variable(m, z1)
    JuMP.@variable(m, z2)
    JuMP.@variable(m, t[1:3] >= 0)
    JuMP.@constraint(m, z1 <= t[1] )
    JuMP.@constraint(m, -t[1] <= z1 )
    JuMP.@constraint(m, z2 <= t[2] )
    JuMP.@constraint(m, -t[2] <= z2 )
    JuMP.@constraint(m, z1 - z2 <= t[3])
    JuMP.@constraint(m, -t[3] <= z1 - z2 )

    for i=1:1000
      x1 = randn()
      x2 = randn()
      λ1 = rand(Uniform(0.,1.))
      λ2 = rand(Uniform(0.,1.))

      JuMP.@objective(m, Min, ((x1-z1)^2+(x2-z2)^2)/2. + λ1 * (t[1]+t[2]) + λ2 * t[3])
      JuMP.optimize!(m)

      zp1, zp2 = ProximalBase.proxL1Fused(x1, x2, λ1, λ2)

      @test abs(JuMP.value.(z1) - zp1) + abs(JuMP.value.(z2) - zp2)  ≈ 0. atol=2e-4
    end

    m = JuMP.Model(JuMP.with_optimizer(Ipopt.Optimizer, print_level=0))
    JuMP.@variable(m, z1)
    JuMP.@variable(m, z2)
    JuMP.@variable(m, t >= 0)
    JuMP.@constraint(m, z1 - z2 <= t)
    JuMP.@constraint(m, -t <= z1 - z2 )

    for i=1:1000
      x1 = randn()
      x2 = randn()
      λ = rand(Uniform(0.,1.))

      JuMP.@objective(m, Min, ((x1-z1)^2+(x2-z2)^2)/2. + λ * t)
      JuMP.optimize!(m)

      zp1, zp2 = ProximalBase.proxL1Fused(x1, x2, 0., λ)

      @test abs(JuMP.value.(z1) - zp1) + abs(JuMP.value.(z2) - zp2)  ≈ 0. atol=1e-4
    end
  end # random testset

  @testset "nonrandom" begin
      x1 = randn()
      x2 = randn()
      λ1 = rand(Uniform(0.,1.))

      zp1, zp2 = ProximalBase.proxL1Fused(x1, x2, λ1, 0.)
      @test ProximalBase.shrink(x1, λ1) ≈ zp1
      @test ProximalBase.shrink(x2, λ1) ≈ zp2

      λ1 = 1000.
      zp1, zp2 = ProximalBase.proxL1Fused(x1, x2, λ1, 0.)
      @test ProximalBase.shrink(x1, λ1) ≈ 0.
      @test ProximalBase.shrink(x2, λ1) ≈ 0.
  end
end # proximal_l1

@testset "proximal_l2" begin

  @testset "shrink to zero" begin
    p = 10
    x = randn(p)
    hat_x = randn(p)

    lambda = norm(x) + 0.1
    g = ProxL2(lambda)
    # check the norm
    @test value(g, x) ≈ lambda * norm(x)
    ln = 0.
    for j=1:p
      ln += x[j]^2
    end
    @test value(g, x) ≈ lambda * sqrt(ln)

    # check shrinkage
    prox!(g, hat_x, x, 1.)
    @test hat_x ≈ zeros(Float64, p)
    @test norm(hat_x) ≈ 0.
  end

  @testset "shrink not to zero" begin
    p = 2
    x = [1., 2.]
    hat_x = randn(p)

    lambda = 1.
    # check the norm
    g = ProxL2(lambda)

    @test value(g, x) ≈ lambda * norm(x)
    ln = 0.
    for j=1:p
      ln += x[j]^2
    end
    @test value(g, x) ≈ lambda * sqrt(ln)

    # check shrinkage
    prox!(g, hat_x, x, 1.)

    @test hat_x ≈ (1. - 1. / sqrt(5.))*x
    @test norm(hat_x) ≈ sqrt(5.) - 1.
  end

end

@testset "proximal_l2sq" begin

  @testset "prox operator" begin
    x = [1., 2.]
    lambda = 1.

    # norm value
    g = ProxL2Sq(0.)
    @test value(g, x) == 0.

    g = ProxL2Sq(1.)
    @test value(g, x) == sum(abs2, x)

    g = ProxL2Sq(2.)
    @test value(g, x) ≈ 2. * norm(x)^2

    # prox
    hat_x = similar(x)

    g = ProxL2Sq(0.)
    @test prox!(g, hat_x, x) ≈ x

    g = ProxL2Sq(1.)
    @test prox!(g, hat_x, x) ≈ x / 3.

    g = ProxL2Sq(2.)
    @test prox!(g, hat_x, x) ≈ x / 5.

  end

end

@testset "proximal_nuclear" begin

  y = randn(30, 10)
  y = y' * y

  U, S, V = svd(y)
  for k=1:10
    S[k] = max(S[k] - 0.1, 0.)
  end
  tx = U * (Diagonal(S)*V')

  x = randn(10, 10)
  g = ProxNuclear(0.1)
  prox!(g, x, y)

  @test x ≈ tx
end

@testset "proximal_l1l2" begin

  @testset "shrink to zero" begin
    p = 10
    x = AtomIterate(p, 2)
    hat_x = AtomIterate(p, 2)

    x .= randn(p)
    hat_x .= randn(p)

    lambda = norm(x) + 0.1

    # check the norm
    g = ProxL2(lambda, [1., 1.])
    @test value(g, x) ≈ lambda * norm(x[1:5]) + lambda * norm(x[6:10])

    # check shrinkage
    prox!(g, hat_x, x, 1.)

    @test hat_x.storage ≈ zeros(Float64, p)
    @test norm(hat_x) ≈ 0.
  end

  @testset "shrink not to zero" begin
    p = 10
    x = AtomIterate(p, 2)
    hat_x = AtomIterate(p, 2)

    x .= randn(p)
    hat_x .= randn(p)

    lambda = min(norm(x[1:5]), norm(x[6:10])) - 0.2

    # check the norm
    g = ProxL2(lambda, [1., 1.])
    @test value(g, x) ≈ lambda * norm(x[1:5]) + lambda * norm(x[6:10])

    # check shrinkage
    prox!(g, hat_x, x, 1.)

    @test hat_x.storage[1:5] ≈  (1. - lambda/norm(x[1:5])) * x[1:5]
    @test hat_x.storage[6:10] ≈ (1. - lambda/norm(x[6:10])) * x[6:10]
  end

end

@testset "proximal_logdet" begin
  p = 10

  out = zeros(p,p)

  # identity
  Σ = Symmetric(Matrix{Float64}(I, p, p))
  V = Matrix{Float64}(I, p, p)
  g = ProxGaussLikelihood(Σ)
  @test prox(g, V) ≈ Matrix{Float64}(I, p, p)
  @test prox!(g, out, V) ≈ Matrix{Float64}(I, p, p)
  @test value(g, V) ≈ p

  γ = 0.5
  ρ = 2.
  sol = Matrix{Float64}(I, p, p) * (-(1. - ρ)+sqrt((1. - ρ)^2 + 4. * ρ)) / (2. * ρ)
  @test prox!(g, out, V, γ) ≈ sol

end

end


#=
   DONALD+GERALD=ROBERT in ConstraintSolver.jl

   Model created by Hakan Kjellerstrand, hakank@gmail.com
   See also my Julia page: http://www.hakank.org/julia/

=#

using ConstraintSolver, JuMP
const CS = ConstraintSolver
include("constraints_utils.jl")

function donald_gerald_robert()
    model = Model(optimizer_with_attributes(CS.Optimizer, "all_solutions"=>true,"logging"=>[]))
    @variable(model, 0 <= x[1:10] <= 9, Int)
    d, o, n, a, l, g, e, r, b, t = x

    @constraint(model, x in CS.AllDifferent())
    @constraint(model, d != 0)
    @constraint(model, g != 0)
    @constraint(model, r != 0)
    @constraint(model,
                              100000*d + 10000*o + 1000*n + 100*a + 10*l + d +
                              100000*g + 10000*e + 1000*r + 100*a + 10*l + d ==
                              100000*r + 10000*o + 1000*b + 100*e + 10*r + t

                              # Different approach:
                              # sum([100000*d,10000*o,1000*n,100*a,10*l + d]) +
                              # sum([100000*g,10000*e,1000*r, 100*a, 10*l, d]) ==
                              # sum([100000*r,10000*o, 1000*b, 100*e, 10*r, t])

        )

    # Solve the problem
    optimize!(model)

    status = JuMP.termination_status(model)
    println("status:$status")
    if status == MOI.OPTIMAL
        x = convert.(Integer,JuMP.value.(x))
        println(x)
        num_sols = MOI.get(model, MOI.ResultCount())
        println("num_sols:$num_sols\n")
    end
end

donald_gerald_robert()


module ModParameters

export Param, setParameters

const BCDirichlet,BCRiemann = 1,2 # Constants for different boundary types

using TOML

# Parameters
struct Param
   nD::Int             # Dimension of the system
   LatticeType::Int    # DmQn lattice model
   Δx::Float64         # Discretized space interval
   Δt::Float64         # Discretized time interval
   α ::Float64         # Thermal diffusivity
   ω ::Float64         # Relaxation coefficient
   TotalTime::Float64  # Total simulation time
   nT::Int             # Total timesteps

   Temp::Vector{Float64} # Left boundary temperature

   nI::Int
   nJ::Int
   BCtype::Array{Int,1}         # Type of boundary conditions
   BClines::Array{Float64,2}

   DoPlot::Bool

   function Param(BClines::Array{Float64,2},
      nD::Int=2,LatticeType::Int=3,dx::Float64=1.0,dt::Float64=1.0,
      α::Float64=0.25,TotalTime::Float64=400.0,
      Temp::Vector{Float64}=[1.0, 0.0, 0.0, 0.0],
      BCtype::Vector{Int}=[1,1,1,1], nI::Int=5, nJ::Int=5,
      DoPlot::Bool=false)
      @assert(0 ≤ TotalTime, "Simulation time must be positive!")

      new(nD,LatticeType,dx,dt,α,inv(nD*dt*α/dx^2 + 0.5),TotalTime,
         Int(TotalTime/dt),Temp,nI,nJ,BCtype,BClines,DoPlot)
   end
end

"""
   setParameters()

Read parameters from PARAM.toml and construct the parameter list.
"""
function setParameters()

   paramIn = TOML.parsefile("PARAM.toml")
   nD = paramIn["Parameters"]["nD"]
   LatticeType = paramIn["Parameters"]["LatticeType"]
   dx = paramIn["Parameters"]["dx"]
   dt = paramIn["Parameters"]["dt"]
   α = paramIn["Parameters"]["α"]
   TotalTime = paramIn["Parameters"]["TotalTime"]
   Temp = paramIn["Parameters"]["Temp"]
   DoPlot = paramIn["Plots"]["DoPlot"]

   if LatticeType == "D2Q4"
      LT = 1
   elseif LatticeType == "D2Q5"
      LT = 2
   elseif LatticeType == "D2Q9"
      LT = 3
   end

   nI = paramIn["Mesh"]["nI"]
   nJ = paramIn["Mesh"]["nJ"]
   nBCtype = length(paramIn["Mesh"]["BCtype"])
   if nBCtype != size(Temp,1) # This needs to be modified!
      error("Number of boundary values does not match! Check Temp in PARAM!")
   end
   BCtype = Vector{Int}(undef,nBCtype)
   for i = 1:nBCtype
      if paramIn["Mesh"]["BCtype"][i] == "Dirichlet"
         BCtype[i] = 1
      elseif paramIn["Mesh"]["BCtype"][i] == "Riemann"
         BCtype[i] = 2
      end
   end

   BClines = hcat(paramIn["Mesh"]["BClines"]...)

   param = Param(BClines,nD,LT,dx,dt,α,TotalTime,Temp,BCtype,nI,nJ,DoPlot)

   return param
end

end


using celerite
using Test
using LinearAlgebra
using Profile
using Random

include("test_solver.jl")
include("test_cholesky.jl")


using CUSPARSE
using CUDArt
using Base.Test

m = 5
n = 5
k = 10
blockdim = 5

for (func,typ) in ((:test_geam_csr,:CudaSparseMatrixCSR),
                   (:test_geam_csc,:CudaSparseMatrixCSC))
    @eval begin
        function $func(elty)
            A = sparse(rand(elty,m,n))
            B = sparse(rand(elty,m,n))
            alpha = rand(elty)
            beta = rand(elty)
            C = alpha * A + beta * B
            d_A = $typ(A)
            d_B = $typ(B)
            d_C = CUSPARSE.geam(alpha,d_A,beta,d_B,'O','O','O')
            h_C = to_host(d_C)
            @test_approx_eq(C,h_C)
            d_C = CUSPARSE.geam(d_A,beta,d_B,'O','O','O')
            h_C = to_host(d_C)
            C = A + beta * B
            @test_approx_eq(C,h_C)
            d_C = CUSPARSE.geam(alpha,d_A,d_B,'O','O','O')
            h_C = to_host(d_C)
            C = alpha * A + B
            @test_approx_eq(C,h_C)
            d_C = CUSPARSE.geam(d_A,d_B,'O','O','O')
            h_C = to_host(d_C)
            C = A + B
            @test_approx_eq(C,h_C)
            d_C = d_A + d_B
            h_C = to_host(d_C)
            C = A + B
            @test_approx_eq(C,h_C)
            d_C = d_A - d_B
            h_C = to_host(d_C)
            C = A - B
            @test_approx_eq(C,h_C)
            B = sparse(rand(elty,k,n))
            d_B = $typ(B)
            @test_throws(DimensionMismatch,CUSPARSE.geam(d_B,d_A,'O','O','O'))
        end
    end
end

function test_geam_mix(elty)
    A = spdiagm(rand(elty,m))
    B = spdiagm(rand(elty,m)) + sparse(diagm(rand(elty,m-1),1))
    d_A = CudaSparseMatrixCSR(A)
    d_B = CudaSparseMatrixCSC(B)
    d_C = d_B + d_A
    h_C = to_host(d_C)
    @test h_C ≈ A + B
    d_C = d_A + d_B
    h_C = to_host(d_C)
    @test h_C ≈ A + B
    d_C = d_A - d_B
    h_C = to_host(d_C)
    @test h_C ≈ A - B
    d_C = d_B - d_A
    h_C = to_host(d_C)
    @test h_C ≈ B - A
end

types = [Float32,Float64,Complex64,Complex128]
for elty in types
    tic()
    test_geam_csr(elty)
    test_geam_csc(elty)
    test_geam_mix(elty)
    println("geam took ", toq(), " for ", elty)
end


export upsample_nearest, ∇upsample_nearest,
    upsample_linear, ∇upsample_linear,
    upsample_bilinear, ∇upsample_bilinear,
    upsample_trilinear, ∇upsample_trilinear,
    pixel_shuffle

"""
    upsample_nearest(x, scale::NTuple{S,Int})
    upsample_nearest(x; size::NTuple{S,Int})

Upsamples the array `x` by integer multiples along the first `S` dimensions.
Subsequent dimensions of `x` are not altered.

Either the `scale` factors or the final output `size` can be specified.

See also [`upsample_bilinear`](@ref), for two dimensions of an `N=4` array.

# Example
```jldoctest
julia> upsample_nearest([1 2 3; 4 5 6], (2, 3))
4×9 Matrix{$Int}:
 1  1  1  2  2  2  3  3  3
 1  1  1  2  2  2  3  3  3
 4  4  4  5  5  5  6  6  6
 4  4  4  5  5  5  6  6  6

julia> ans == upsample_nearest([1 2 3; 4 5 6]; size=(4, 9))  # equivalent
true

julia> upsample_nearest([1 2 3; 4 5 6], (2,))
4×3 Matrix{$Int}:
 1  2  3
 1  2  3
 4  5  6
 4  5  6

julia> ans == upsample_nearest([1 2 3; 4 5 6], size=(4,))
true
```
"""
function upsample_nearest(x::AbstractArray; size::NTuple{S,Int}) where S
    xsize = Base.size(x)[1:S]
    all(size .% xsize .== 0) || throw(ArgumentError("expected output size divisible by input size"))
    return upsample_nearest(x, size .÷ xsize)
end

function upsample_nearest(x::AbstractArray{T,N}, scales::NTuple{S, <:Integer}) where {T,N,S}
    S in 1:N || throw(ArgumentError("can't upsample ndims(x)=$N with scale=$scales"))
    outsize = ntuple(d -> d<=S ? scales[d] * size(x,d) : size(x,d), N)
    out = similar(x, T, outsize)
    writesize = ntuple(N+S) do d
        d > 2S && return size(x, d-S)
        isodd(d) ? scales[cld(d,2)] : size(x, cld(d,2))
    end
    readsize = ntuple(N+S) do d
        d > 2S && return size(x, d-S)
        isodd(d) ? 1 : size(x, cld(d,2))
    end
    reshape(out, writesize) .= reshape(x, readsize)
    out
end

function ∇upsample_nearest(x::AbstractArray{T,N}, scales::NTuple{S, <:Integer}) where {T,N,S}
    outsize = ntuple(N) do d
        d > S && return size(x,d)
        rem(size(x,d), scales[d]) == 0 || throw(ArgumentError("expected input array evenly divisible by scale=$scales, got size(x)=$(size(x))"))
        div(size(x,d), scales[d])
    end
    tempsize = ntuple(N+S) do d
        d > 2S && return size(x, d-S)
        s = scales[cld(d,2)]
        isodd(d) ? s : div(size(x, cld(d,2)),s)
    end
    mid = sum(reshape(x, tempsize), dims=ntuple(d -> 2d-1, S))
    reshape(mid, outsize)
end

function rrule(::typeof(upsample_nearest), x::AbstractArray, s::Tuple)
    Ω = upsample_nearest(x, s)
    upsample_nearest_pullback(Δ) = (NoTangent(), ∇upsample_nearest(unthunk(Δ), s), NoTangent())
    return Ω, upsample_nearest_pullback
end

# utility function
@inline function compute_source_index_and_lambda(
    ratio, # 0 < ratio < 1
    output_index,
    input_size,
    output_size
)
    real_input_index = ratio*output_index
    input_index0 = floor(Int, real_input_index) # typecast to int was here in C++
    offset = (input_index0 < input_size - 1) ? 1 : 0
    input_index1 = input_index0 + offset
    lambda1 = real_input_index - input_index0
    lambda0 = 1 - lambda1
    return input_index0, input_index1, lambda0, lambda1
end

###########
# linear
###########
"""
    upsample_linear(x::AbstractArray{T,3}, scale::Real)
    upsample_linear(x::AbstractArray{T,3}; size::Integer)

Upsamples the first dimension of the array `x` by the upsample provided `scale`,
using linear interpolation. As an alternative to using `scale`, the resulting array `size`
can be directly specified with a keyword argument.

The size of the output is equal to
`(scale*S1, S2, S3)`, where `S1, S2, S3 = size(x)`.
"""
function upsample_linear(x::AbstractArray{<:Any,3}, scale::Real)
    outsize = floor(Int, scale * Base.size(x)[1])
    return upsample_linear(x; size=outsize)
end

function upsample_linear(x::AbstractArray{T,3}; size::Integer) where T
    w,c,n = Base.size(x)
    if w == size
        return x
    end
    y = similar(x, T, size, c, n)
    return upsample_linear_wcn!(y, x)
end

function upsample_linear(x::AbstractArray{T,3}; size::Integer) where T<:Integer
    y = float.(x)
    res = upsample_linear(y; size=size)
    return round.(T, res)
end

function upsample_linear_wcn!(output::AbstractArray{T,3}, input::AbstractArray{T,3}) where T
    size(input)[2:3] == size(output)[2:3] || error("Number of input and output channels and batches must match. Got input $(size(input)) and output $(size(output))")
    in_w, channels, batches = size(input)
    # treat batch and channel dimension as one for better parallelization granularity
    channels *= batches
    out_w, _, _ = size(output)
    output_slice_size = out_w

    # T() and // so that we can handle rationals (super slow)
    width_scale  = T((in_w - 1) // (out_w - 1))

    @inline idx(c, w) = c * in_w + w + 1

    @inbounds Threads.@threads for c in 0:channels-1
        for ow in 0:out_w-1
            iw0, iw1, w0lambda, w1lambda = compute_source_index_and_lambda(width_scale, ow, in_w, out_w)
            output_offset = c * output_slice_size + ow + 1
            output[output_offset] = (w0lambda * input[idx(c, iw0)] + # w0 * i00
                                     w1lambda * input[idx(c, iw1)])  # w1 * i01
        end
    end
    return output
end

"""
    ∇upsample_linear(Δ::AbstractArray{T,3}; size::Integer) where T

# Arguments
- `Δ`: Incoming gradient array, backpropagated from downstream layers
- `size`: Size of the image upsampled in the first place

# Outputs
- `dx`: Downsampled version of `Δ`
"""
function ∇upsample_linear(Δ::AbstractArray{T,3}; size::Integer) where T
    w, c, n = Base.size(Δ)
    out_w = size
    if w == out_w
        return Δ
    end
    dx = zero(similar(Δ, T, out_w, c, n))
    return ∇upsample_linear_wcn!(dx, Δ)
end

function ∇upsample_linear_wcn!(dx::AbstractArray{T,3}, Δ::AbstractArray{T,3}) where T
    size(dx)[2:3] == size(Δ)[2:3] || error("Number of input and output channels and batches must match. Got input $(size(input)) and output $(size(output))")
    in_w, channels, batches = size(dx)

    # treat batch and channel dimension as one for better parallelization granularity
    channels *= batches
    out_w, _, _ = size(Δ)
    output_slice_size = out_w

    width_scale  = T((in_w - 1) // (out_w - 1))

    @inline idx(c, w) = c * in_w + w + 1

    @inbounds Threads.@threads for c in 0:channels-1
        for ow in 0:out_w-1
            iw0, iw1, w0lambda, w1lambda = compute_source_index_and_lambda(width_scale, ow, in_w, out_w)
            output_offset = c * output_slice_size + ow + 1
            Δ_value = Δ[output_offset]
            dx[idx(c, iw0)] += w0lambda * Δ_value # i00
            dx[idx(c, iw1)] += w1lambda * Δ_value # i01
        end
    end
    return dx
end

function rrule(::typeof(upsample_linear), x; size)
    Ω = upsample_linear(x; size=size)
    function upsample_linear_pullback(Δ)
        (NoTangent(), ∇upsample_linear(unthunk(Δ); size=Base.size(x,1)))
    end
    return Ω, upsample_linear_pullback
end

###########
# bilinear
###########
"""
    upsample_bilinear(x::AbstractArray{T,4}, scale::NTuple{2,Real})
    upsample_bilinear(x::AbstractArray{T,4}; size::NTuple{2,Integer})

Upsamples the first 2 dimensions of the array `x` by the upsample factors stored in `scale`,
using bilinear interpolation. As an alternative to using `scale`, the resulting image `size`
can be directly specified with a keyword argument.

The size of the output is equal to
`(scale[1]*S1, scale[2]*S2, S3, S4)`, where `S1, S2, S3, S4 = size(x)`.

# Examples

```jldoctest
julia> x = reshape(Float32[1 2 3; 4 5 6], (2,3,1,1))
2×3×1×1 Array{Float32, 4}:
[:, :, 1, 1] =
 1.0  2.0  3.0
 4.0  5.0  6.0

julia> upsample_bilinear(x, (2, 3))
4×9×1×1 Array{Float32, 4}:
[:, :, 1, 1] =
 1.0  1.25  1.5  1.75  2.0  2.25  2.5  2.75  3.0
 2.0  2.25  2.5  2.75  3.0  3.25  3.5  3.75  4.0
 3.0  3.25  3.5  3.75  4.0  4.25  4.5  4.75  5.0
 4.0  4.25  4.5  4.75  5.0  5.25  5.5  5.75  6.0

julia> ans == upsample_bilinear(x; size=(4, 9))  # specify ouput size instead
true

julia> upsample_bilinear(x, (2.5, 3.5))  # non-integer scaling factors are allowed
5×10×1×1 Array{Float32, 4}:
[:, :, 1, 1] =
 1.0   1.22222  1.44444  1.66667  1.88889  …  2.33333  2.55556  2.77778  3.0
 1.75  1.97222  2.19444  2.41667  2.63889     3.08333  3.30556  3.52778  3.75
 2.5   2.72222  2.94444  3.16667  3.38889     3.83333  4.05556  4.27778  4.5
 3.25  3.47222  3.69444  3.91667  4.13889     4.58333  4.80556  5.02778  5.25
 4.0   4.22222  4.44444  4.66667  4.88889     5.33333  5.55556  5.77778  6.0
```
"""
function upsample_bilinear(x::AbstractArray{<:Any,4}, scale::NTuple{2,Real})
    outsize = ntuple(i -> floor(Int, scale[i] * Base.size(x, i)), 2)
    return upsample_bilinear(x; size=outsize)
end

upsample_bilinear(x, scale::Real) = upsample_bilinear(x, (scale,scale))

function upsample_bilinear(x::AbstractArray{T,4}; size::NTuple{2,Integer}) where T
    w,h,c,n = Base.size(x)
    if (w,h) == size
        return x
    end
    y = similar(x, T, size..., c, n)
    return upsample_bilinear_whcn!(y, x)
end

function upsample_bilinear(x::AbstractArray{T,4}; size::NTuple{2,Integer}) where T<:Integer
    y = float.(x)
    res = upsample_bilinear(y; size=size)
    return round.(T, res)
end

# this is the core function which works on arrays of arbitrary size
# the implementation is a translation of https://github.com/pytorch/pytorch/blob/master/aten/src/ATen/native/cpu/UpSampleMoreKernel.cpp
# which implements open-cv style linear interpolation / upsampling
# for simplicity, corners are aligned and all logic for other behaviour has been stripped
# - whcn because there is also a cwhn implementation
# - the function is parallelized using @threads
# - RGB types could be supported via reinterpreting
# - integer types need to be converted to Float and back
# - rationals work, but are slow
function upsample_bilinear_whcn!(output::AbstractArray{T,4}, input::AbstractArray{T,4}) where T
    size(input)[3:4] == size(output)[3:4] || error("Number of input and output channels and batches must match. Got input $(size(input)) and output $(size(output))")
    in_w, in_h, channels, batches = size(input)
    # treat batch and channel dimension as one for better parallelization granularity
    channels *= batches
    out_w, out_h, _, _ = size(output)
    output_slice_size = out_h * out_w

    # T() and // so that we can handle rationals (super slow)
    width_scale  = T((in_w - 1) // (out_w - 1))
    height_scale = T((in_h - 1) // (out_h - 1))

    @inline idx(c, h, w) = c * in_h * in_w + h * in_w + w + 1

    @inbounds Threads.@threads for c in 0:channels-1
        for oh in 0:out_h-1
            ih0, ih1, h0lambda, h1lambda = compute_source_index_and_lambda(height_scale, oh, in_h, out_h)
            for ow in 0:out_w-1
                iw0, iw1, w0lambda, w1lambda = compute_source_index_and_lambda(width_scale, ow, in_w, out_w)
                output_offset = c * output_slice_size + oh * out_w + ow + 1
                output[output_offset] =
                    (h0lambda * w0lambda * input[idx(c, ih0, iw0)] + # h0 * w0 * i00
                     h0lambda * w1lambda * input[idx(c, ih0, iw1)] + # h0 * w1 * i01
                     h1lambda * w0lambda * input[idx(c, ih1, iw0)] + # h1 * w0 * i10
                     h1lambda * w1lambda * input[idx(c, ih1, iw1)])  # h1 * w1 * i11
            end
        end
    end
    return output
end

"""
    ∇upsample_bilinear(Δ::AbstractArray{T,4}; size::NTuple{2,Integer}) where T

# Arguments
- `Δ`: Incoming gradient array, backpropagated from downstream layers
- `size`: Lateral (W,H) size of the image upsampled in the first place

# Outputs
- `dx`: Downsampled version of `Δ`
"""
function ∇upsample_bilinear(Δ::AbstractArray{T,4}; size::NTuple{2,Integer}) where T
    w, h, c, n = Base.size(Δ)
    out_w, out_h = size
    if (w,h) == (out_w, out_h)
        return Δ
    end
    dx = zero(similar(Δ, T, out_w, out_h, c, n))
    return ∇upsample_bilinear_whcn!(dx, Δ)
end

function ∇upsample_bilinear_whcn!(dx::AbstractArray{T,4}, Δ::AbstractArray{T,4}) where T
    size(dx)[3:4] == size(Δ)[3:4] || error("Number of input and output channels and batches must match. Got input $(size(input)) and output $(size(output))")
    in_w, in_h, channels, batches = size(dx)

    # treat batch and channel dimension as one for better parallelization granularity
    channels *= batches
    out_w, out_h, _, _ = size(Δ)
    output_slice_size = out_h * out_w

    width_scale  = T((in_w - 1) // (out_w - 1))
    height_scale = T((in_h - 1) // (out_h - 1))

    @inline idx(c, h, w) = c * in_h * in_w + h * in_w + w + 1

    @inbounds Threads.@threads for c in 0:channels-1
        for oh in 0:out_h-1
            ih0, ih1, h0lambda, h1lambda = compute_source_index_and_lambda(height_scale, oh, in_h, out_h)
            for ow in 0:out_w-1
                iw0, iw1, w0lambda, w1lambda = compute_source_index_and_lambda(width_scale, ow, in_w, out_w)
                output_offset = c * output_slice_size + oh * out_w + ow + 1
                Δ_value = Δ[output_offset]
                dx[idx(c, ih0, iw0)] += h0lambda * w0lambda * Δ_value # i00
                dx[idx(c, ih0, iw1)] += h0lambda * w1lambda * Δ_value # i01
                dx[idx(c, ih1, iw0)] += h1lambda * w0lambda * Δ_value # i10
                dx[idx(c, ih1, iw1)] += h1lambda * w1lambda * Δ_value # i11
            end
        end
    end
    return dx
end

function rrule(::typeof(upsample_bilinear), x; size)
    Ω = upsample_bilinear(x; size=size)
    function upsample_bilinear_pullback(Δ)
        (NoTangent(), ∇upsample_bilinear(unthunk(Δ); size=(Base.size(x,1),Base.size(x,2))))
    end
    return Ω, upsample_bilinear_pullback
end

###########
# trilinear
###########
"""
    upsample_trilinear(x::AbstractArray{T,5}, scale::NTuple{3,Real})
    upsample_trilinear(x::AbstractArray{T,5}; size::NTuple{3,Integer})

Upsamples the first 3 dimensions of the array `x` by the upsample factors stored in `scale`,
using trilinear interpolation. As an alternative to using `scale`, the resulting image `size`
can be directly specified with a keyword argument.

The size of the output is equal to
`(scale[1]*S1, scale[2]*S2, scale[3]*S3, S4, S5)`, where `S1, S2, S3, S4, S5 = size(x)`.

# Examples

```julia
upsample_trilinear(x, (2, 3, 4))
upsample_trilinear(x; size=(4, 9, 11))  # specify ouput size instead
upsample_trilinear(x, (2.5, 3.5, pi))  # non-integer scaling factors are allowed
```
"""
function upsample_trilinear(x::AbstractArray{<:Any,5}, scale::NTuple{3,Real})
    outsize = ntuple(i -> floor(Int, scale[i] * Base.size(x, i)), 3)
    return upsample_trilinear(x; size=outsize)
end

upsample_trilinear(x, scale::Real) = upsample_trilinear(x, (scale,scale,scale))

function upsample_trilinear(x::AbstractArray{T,5}; size::NTuple{3,Integer}) where T
    w,h,d,c,n = Base.size(x)
    if (w,h,d) == size
        return x
    end
    y = similar(x, T, size..., c, n)
    return upsample_trilinear_whdcn!(y, x)
end

function upsample_trilinear(x::AbstractArray{T,5}; size::NTuple{3,Integer}) where T<:Integer
    y = float.(x)
    res = upsample_trilinear(y; size=size)
    return round.(T, res)
end

function upsample_trilinear_whdcn!(output::AbstractArray{T,5}, input::AbstractArray{T,5}) where T
    size(input)[4:5] == size(output)[4:5] || error("Number of input and output channels and batches must match. Got input $(size(input)) and output $(size(output))")
    in_w, in_h, in_d, channels, batches = size(input)
    # treat batch and channel dimension as one for better parallelization granularity
    channels *= batches
    out_w, out_h, out_d, _, _ = size(output)
    output_slice_size = out_h * out_w * out_d

    # T() and // so that we can handle rationals (super slow)
    width_scale  = T((in_w - 1) // (out_w - 1))
    height_scale = T((in_h - 1) // (out_h - 1))
    depth_scale  = T((in_d - 1) // (out_d - 1))

    @inline idx(c, d, h, w) = c * in_d * in_h * in_w + d * in_h * in_w + h * in_w + w + 1

    @inbounds Threads.@threads for c in 0:channels-1
        for od in 0:out_d-1
            id0, id1, d0lambda, d1lambda = compute_source_index_and_lambda(depth_scale, od, in_d, out_d)
            for oh in 0:out_h-1
                ih0, ih1, h0lambda, h1lambda = compute_source_index_and_lambda(height_scale, oh, in_h, out_h)
                for ow in 0:out_w-1
                    iw0, iw1, w0lambda, w1lambda = compute_source_index_and_lambda(width_scale, ow, in_w, out_w)
                    output_offset = c * output_slice_size + od * out_w * out_h + oh * out_w + ow + 1
                    output[output_offset] =
                        d0lambda * h0lambda * w0lambda * input[idx(c, id0, ih0, iw0)] + # d0 * h0 * w0 * i000
                        d0lambda * h0lambda * w1lambda * input[idx(c, id0, ih0, iw1)] + # d0 * h0 * w1 * i001
                        d0lambda * h1lambda * w0lambda * input[idx(c, id0, ih1, iw0)] + # d0 * h1 * w0 * i010
                        d0lambda * h1lambda * w1lambda * input[idx(c, id0, ih1, iw1)] + # d0 * h1 * w1 * i011
                        d1lambda * h0lambda * w0lambda * input[idx(c, id1, ih0, iw0)] + # d1 * h0 * w0 * i100
                        d1lambda * h0lambda * w1lambda * input[idx(c, id1, ih0, iw1)] + # d1 * h0 * w1 * i101
                        d1lambda * h1lambda * w0lambda * input[idx(c, id1, ih1, iw0)] + # d1 * h1 * w0 * i110
                        d1lambda * h1lambda * w1lambda * input[idx(c, id1, ih1, iw1)]   # d1 * h1 * w1 * i111
                end
            end
        end
    end
    return output
end

"""
    ∇upsample_trilinear(Δ::AbstractArray{T,5}; size::NTuple{3,Integer}) where T

# Arguments
- `Δ`: Incoming gradient array, backpropagated from downstream layers
- `size`: Lateral size & depth (W,H,D) of the image upsampled in the first place

# Outputs
- `dx`: Downsampled version of `Δ`
"""
function ∇upsample_trilinear(Δ::AbstractArray{T,5}; size::NTuple{3,Integer}) where T
    w, h, d, c, n = Base.size(Δ)
    out_w, out_h, out_d = size
    if (w,h,d) == (out_w, out_h, out_d)
        return Δ
    end
    dx = zero(similar(Δ, T, size..., c, n))
    return ∇upsample_trilinear_whdcn!(dx, Δ)
end

function ∇upsample_trilinear_whdcn!(dx::AbstractArray{T,5}, Δ::AbstractArray{T,5}) where T
    size(dx)[4:5] == size(Δ)[4:5] || error("Number of input and output channels and batches must match. Got dx $(size(dx)) and Δ $(size(Δ))")
    in_w, in_h, in_d, channels, batches = size(dx)
    # treat batch and channel dimension as one for better parallelization granularity
    channels *= batches
    out_w, out_h, out_d, _, _ = size(Δ)
    output_slice_size = out_h * out_w * out_d

    # T() and // so that we can handle rationals (super slow)
    width_scale  = T((in_w - 1) // (out_w - 1))
    height_scale = T((in_h - 1) // (out_h - 1))
    depth_scale  = T((in_d - 1) // (out_d - 1))

    @inline idx(c, d, h, w) = c * in_d * in_h * in_w + d * in_h * in_w + h * in_w + w + 1

    @inbounds Threads.@threads for c in 0:channels-1
        for od in 0:out_d-1
            id0, id1, d0lambda, d1lambda = compute_source_index_and_lambda(depth_scale, od, in_d, out_d)
            for oh in 0:out_h-1
                ih0, ih1, h0lambda, h1lambda = compute_source_index_and_lambda(height_scale, oh, in_h, out_h)
                for ow in 0:out_w-1
                    iw0, iw1, w0lambda, w1lambda = compute_source_index_and_lambda(width_scale, ow, in_w, out_w)
                    output_offset = c * output_slice_size + od * out_w * out_h + oh * out_w + ow + 1
                    Δ_value = Δ[output_offset]
                    dx[idx(c, id0, ih0, iw0)] += d0lambda * h0lambda * w0lambda * Δ_value  # /* i000 */
                    dx[idx(c, id0, ih0, iw1)] += d0lambda * h0lambda * w1lambda * Δ_value  # /* i001 */
                    dx[idx(c, id0, ih1, iw0)] += d0lambda * h1lambda * w0lambda * Δ_value  # /* i010 */
                    dx[idx(c, id0, ih1, iw1)] += d0lambda * h1lambda * w1lambda * Δ_value  # /* i011 */
                    dx[idx(c, id1, ih0, iw0)] += d1lambda * h0lambda * w0lambda * Δ_value  # /* i100 */
                    dx[idx(c, id1, ih0, iw1)] += d1lambda * h0lambda * w1lambda * Δ_value  # /* i101 */
                    dx[idx(c, id1, ih1, iw0)] += d1lambda * h1lambda * w0lambda * Δ_value  # /* i110 */
                    dx[idx(c, id1, ih1, iw1)] += d1lambda * h1lambda * w1lambda * Δ_value  # /* i111 */
                end
            end
        end
    end
    return dx
end

function rrule(::typeof(upsample_trilinear), x; size)
    Ω = upsample_trilinear(x; size=size)
    function upsample_trilinear_pullback(Δ)
        (NoTangent(), ∇upsample_trilinear(unthunk(Δ); size=(Base.size(x,1), Base.size(x,2), Base.size(x,3))))
    end
    return Ω, upsample_trilinear_pullback
end

"""
    pixel_shuffle(x, r::Integer)

Pixel shuffling operation, upscaling by a factor `r`.

For 4-arrays representing `N` images, the operation converts input `size(x) == (W, H, r^2*C, N)`
to output of size `(r*W, r*H, C, N)`. For `D`-dimensional data, it expects `ndims(x) == D+2`
with channel and batch dimensions, and divides the number of channels by `r^D`.

Used in super-resolution networks to upsample towards high resolution features.
Reference: Shi et. al., "Real-Time Single Image and Video Super-Resolution ...", CVPR 2016, https://arxiv.org/abs/1609.05158

# Examples

```jldoctest
julia> x = [10i + j + channel/10 for i in 1:2, j in 1:3, channel in 1:4, batch in 1:1]
2×3×4×1 Array{Float64, 4}:
[:, :, 1, 1] =
 11.1  12.1  13.1
 21.1  22.1  23.1

[:, :, 2, 1] =
 11.2  12.2  13.2
 21.2  22.2  23.2

[:, :, 3, 1] =
 11.3  12.3  13.3
 21.3  22.3  23.3

[:, :, 4, 1] =
 11.4  12.4  13.4
 21.4  22.4  23.4

julia> pixel_shuffle(x, 2)  # 4 channels used up as 2x upscaling of image dimensions
4×6×1×1 Array{Float64, 4}:
[:, :, 1, 1] =
 11.1  11.3  12.1  12.3  13.1  13.3
 11.2  11.4  12.2  12.4  13.2  13.4
 21.1  21.3  22.1  22.3  23.1  23.3
 21.2  21.4  22.2  22.4  23.2  23.4

julia> y = [i + channel/10 for i in 1:3, channel in 1:6, batch in 1:1]
3×6×1 Array{Float64, 3}:
[:, :, 1] =
 1.1  1.2  1.3  1.4  1.5  1.6
 2.1  2.2  2.3  2.4  2.5  2.6
 3.1  3.2  3.3  3.4  3.5  3.6

julia> pixel_shuffle(y, 2)  # 1D image, with 6 channels reduced to 3
6×3×1 Array{Float64, 3}:
[:, :, 1] =
 1.1  1.3  1.5
 1.2  1.4  1.6
 2.1  2.3  2.5
 2.2  2.4  2.6
 3.1  3.3  3.5
 3.2  3.4  3.6
```
"""
function pixel_shuffle(x::AbstractArray, r::Integer)
    ndims(x) > 2 || throw(ArgumentError("expected x with at least 3 dimensions"))
    d = ndims(x) - 2
    sizein = size(x)[1:d]
    cin, n = size(x, d+1), size(x, d+2)
    cin % r^d == 0 || throw(ArgumentError("expected channel dimension to be divisible by r^d = $(
        r^d), where d=$d is the number of spatial dimensions. Given r=$r, input size(x) = $(size(x))"))
    cout = cin ÷ r^d
    x = reshape(x, sizein..., ntuple(i->r, d)..., cout, n)
    perm = hcat(d+1:2d, 1:d) |> transpose |> vec  # = [d+1, 1, d+2, 2, ..., 2d, d]
    x = permutedims(x, (perm..., 2d+1, 2d+2))
    return reshape(x, map(s -> s*r, sizein)..., cout, n)
end


type AnnealingSoftmax <: BanditAlgorithm
  counts::Vector{Int64}
  values::Vector{Float64}
end

function AnnealingSoftmax(n_arms::Int64)
  AnnealingSoftmax(zeros(Int64, n_arms), zeros(n_arms))
end

function initialize(algo::AnnealingSoftmax, n_arms::Int64)
  algo.counts = zeros(Int64, n_arms)
  algo.values = zeros(n_arms)
end

function initialize(algo::AnnealingSoftmax, arms::Vector)
  initialize(algo, length(arms))
end

function select_arm(algo::AnnealingSoftmax)
  t = sum(algo.counts) + 1
  temperature = 1 / log(t + eps(1.0))
  z = sum(exp(algo.values ./ temperature))
  probs = exp(algo.values ./ temperature) ./ z
  return rand(Categorical(probs))
end

function update(algo::AnnealingSoftmax, chosen_arm::Int64, reward::Real)
  algo.counts[chosen_arm] += 1
  n = algo.counts[chosen_arm]

  value = algo.values[chosen_arm]
  algo.values[chosen_arm] = ((n - 1) / n) * value + (1 / n) * reward
end


### Gensys Implementation
### Algorithm by Christopher A. Sims
### Implementation by Gilberto Boaretto and Daniel Coutinho
### License: MIT

using LinearAlgebra

struct Sims
    Theta1
    Theta2
    Theta3
    eu
end

#Update on dec 26 2019: schur(G0,G1) is not the same as schur(G1,G0). The former generated is far more unstable then the later. This fixes it.

function gensys(G0,G1,Psi,Pi;verbose = true, tol = 1e-20)
    n = size(G0,1)
    decomp_1 = schur(G1,G0)
    # try
    #     schur(G0,G1)
    # catch
    #     eu = (-1,-1)
    #     Theta1 = zeros(n,n)
    #     Theta2 = zeros(n,n)
    #     Theta3 = zeros(n,n)
    #     ans = Sims(Theta1,Theta2,Theta3,eu)
    #     return ans
    #     @warn "Unknown error. Probably LAPACK Exception. Skipping."
    # end
    gen_eigen = abs.(decomp_1.alpha ./ decomp_1.beta)
    ordschur!(decomp_1, gen_eigen .< 1)
    Q,S,T,Z = decomp_1.Q,decomp_1.S,decomp_1.T,decomp_1.Z
    Q[findall(abs.(Q) .< eps())] .= 0
    S[findall(abs.(S) .< eps())] .= 0
    T[findall(abs.(T) .< eps())] .= 0
    Z[findall(abs.(Z) .< eps())] .= 0

    ns = findfirst(sort(gen_eigen) .> 1) #finding the number of stable roots: find first unstable root
    if isnothing(ns)
        ns=1
    else
        ns = ns -1
    end
    nu = n - ns

    S11 = S[1:ns,1:ns]
    S12 = S[1:ns,(ns+1):n]
    S22 = S[(ns+1):n,(ns+1):n]

    T11 = T[1:ns,1:ns]
    T12 = T[1:ns,(ns+1):n]
    T22 = T[(ns+1):n,(ns+1):n]

    Qt = Q'

    Q1 = Qt[1:ns,:]
    Q2 = Qt[(ns+1):n,:]

    Q2Pi = Q2*Pi #This is equation 2.25 in p. 46 Miao (2014)

    m = size(Q2Pi,2)

    svd_Q2Pi = svd(Q2Pi)
    svd_aux = svd_Q2Pi.S #S is a vector
    r = sum(abs.(svd_aux) .> tol) #SVD may give a really small value that is not zeros. If it is bellow tol, we consider it zero.

    #Checking existence and uniqueness see p. 46-47, Miao (2014)
    if m > r
        eu = [1;0]
        if verbose == true
            @warn string("No Unique Solution: ", ns, " stable roots ", nu, " unstable roots. SV: ", svd_aux)
        end
        U1 = svd_Q2Pi.U[:,1:r]
        Xi = Q1*Pi*pinv(Q2Pi) #bottom of p 46 #change in 11 dec 2019: pinv instead of manually multiplying the elements

        Aux1 = T12-Xi*T22

        larg1 = size(Aux1,2)
        Aux2 = [T11 Aux1;zeros(larg1,size(T11,2)) Matrix(I,larg1,larg1)]
        larg2 = size(Aux2,2)
        larg2 = larg2 - size(S11,1)

        ##Matrices on top of p. 46

        Theta1 = [S11 S12-Xi*T22;zeros(larg2,n)]
        Theta1 = Z*inv(Aux2)*Theta1*Z'
        Theta2 = [Q1 - Xi*Q2;zeros(larg2,n)]
        Theta2 = Z*inv(Aux2)*Theta2*Psi
        Theta3 = zeros(n,n)
        ans = Sims(Theta1,Theta2,Theta3,eu)
        return ans
    elseif m < r
        eu = [0;0]
        Theta1 = zeros(n,n)
        Theta2 = zeros(n,n)
        Theta3 = zeros(n,n)
        ans = Sims(Theta1,Theta2,Theta3,eu)
        return ans
        if verbose == true
            @warn string("No Solution: ", ns, " stable roots ", nu, " unstable roots")
        end
    else
        eu = [1;1]
        if verbose == true
            @info "Unique and Stable Solution"
        end
        U1 = svd_Q2Pi.U[:,1:r]
        Xi = Q1*Pi*pinv(Q2Pi) #bottom of p 46 #change in 11 dec 2019: pinv instead of manually multiplying the elements

        Aux1 = T12-Xi*T22

        larg1 = size(Aux1,2)
        Aux2 = [T11 Aux1;zeros(larg1,size(T11,2)) Matrix(I,larg1,larg1)]
        larg2 = size(Aux2,2)
        larg2 = larg2 - size(S11,1)

        ##Matrices on top of p. 46

        Theta1 = [S11 S12-Xi*S22;zeros(larg2,n)]
        Theta1 = Z*inv(Aux2)*Theta1*Z'
        Theta2 = [Q1 - Xi*Q2;zeros(larg2,n)]
        Theta2 = Z*inv(Aux2)*Theta2*Psi
        Theta3 = zeros(n,n)
        ans = Sims(Theta1,Theta2,Theta3,eu)
        return ans #Theta1,Theta2,eu
    end
end

function irf(Theta1,Theta2,t,impulse)
    ans = zeros((t+1),size(Theta1,1))
    for j in 0:t
        ans[(j+1),:] = Theta1^j*Theta2*impulse
    end
    return ans
end

function irf(model::Sims,t,impulse)
    irf(model.Theta1,model.Theta2,t,impulse)
end


@testset "contractions" begin

D = 10
d = 3
sites = 5
T = ComplexF64

ψ = randn(MPS{T}, sites, D, d)
ϕ = randn(MPS{T}, sites, D, d)
mpo_ψ = randn(MPO{T}, sites, D, d)
mpo = randn(MPO{T}, 2, 2, 2)

Id = fill(I(d), length(ψ))

Id_m = MPO(fill(ones(1,1,1,d), length(ϕ)))

@testset "dot products" begin
    @testset "is equal to itself" begin
        @test dot(ψ, ψ) ≈ dot(ψ, ψ)
    end

    @testset "change of arguments results in conjugation" begin
        @test dot(ψ, ϕ) ≈ conj(dot(ϕ, ψ))
        @test dot(ψ, Id, ϕ) ≈ conj(dot(ϕ, Id, ψ))
    end

    @testset "dot with identity equal to dot of two MPS" begin
        @test dot(ψ, Id, ϕ) ≈ dot(ψ, ϕ)
    end

    @testset "norm is 2-norm" begin
        @test norm(ψ) ≈ sqrt(abs(dot(ψ, ψ)))
    end

    @testset "renormalizations" begin
        ψ[end] *= 1 / norm(ψ)
        @test dot(ψ, ψ) ≈ 1

        ϕ[1] *= 1 / norm(ϕ)
        @test dot(ϕ, ϕ) ≈ 1
    end

    @testset "dot products of MPO" begin
        mpo1 = dot(mpo, mpo)

        @testset "has correct sisze" begin
            @test size(mpo1[1]) == (1, 2, 4, 2)
            @test size(mpo1[2]) == (4, 2, 1, 2)
        end
    end

end

@testset "left environment returns correct overlap" begin
    L = left_env(ϕ, ψ)
    @test L[end][1] ≈ dot(ϕ, ψ)
end

@testset "right environment returns correct overlap" begin
    R = right_env(ϕ, ψ)
    @test R[1][end] ≈ dot(ϕ, ψ)
end

@testset "Cauchy-Schwarz inequality of MPS is OK" begin
    @test abs(dot(ϕ, ψ)) <= norm(ϕ) * norm(ψ)
end

@testset "left_env correctly contracts MPS for a given configuration" begin
    D = 10
    d = 2
    sites = 5
    T = ComplexF64

    ψ = randn(MPS{T}, sites, D, d)
    state = 2 * (rand(sites) .< 0.5) .- 1

    C = I
    for (A, σ) ∈ zip(ψ, state) C *= A[:, idx(σ), :] end

    @test tr(C) ≈ left_env(ψ, map(idx, state))[]
end

@testset "right_env correctly contracts MPO with MPS for a given configuration" begin
    D = 10
    d = 2
    sites = 5
    T = Float64

    ψ = randn(MPS{T}, sites, D, d)
    W = randn(MPO{T}, sites, D, d)

    σ = 2 * (rand(sites) .< 0.5) .- 1

    ϕ = MPS(T, sites)
    for (i, A) ∈ enumerate(W)
        m = idx(σ[i])
        @cast B[x, s, y] := A[x, $m, y, s]
        ϕ[i] = B
    end

    @test dot(ψ, ϕ) ≈ right_env(ψ, W, map(idx, σ))[]
end

end


module ManifoldLearning

    import Base: show, summary
    import SparseArrays: AbstractSparseArray, SparseMatrixCSC, spzeros, spdiagm, findnz
    import Statistics: mean, std
    import StatsBase: StatsBase, fit, standardize
    import MultivariateStats: fit, outdim, projection, transform, KernelPCA, principalvars
    import LinearAlgebra: eigvals, mul!, svd, qr, Symmetric, eigen, eigen!, diagm, tr,
                          rmul!, I, schur, opnorm
    import LightGraphs: neighbors, nv, add_edge!, connected_components, vertices,
                        dijkstra_shortest_paths, induced_subgraph, weights
    import SimpleWeightedGraphs: SimpleWeightedGraph

    export

    # Transformation types
    AbstractDimensionalityReduction,
    Isomap,             # Type: Isomap model
    HLLE,               # Type: Hessian Eigenmaps model
    LLE,                # Type: Locally Linear Embedding model
    LTSA,               # Type: Local Tangent Space Alignment model
    LEM,                # Type: Laplacian Eigenmaps model
    DiffMap,            # Type: Diffusion maps model

    ## common interface
    outdim,             # the output dimension of the transformation
    fit,                # perform the manifold learning
    transform,          # the projection matrix
    eigvals,            # eigenvalues from the spectral analysis
    neighbors,          # the number of nearest neighbors used for building local
    vertices,           # vertices of largest connected component
    projection          # the projection matrix (deprecated)

    abstract type AbstractDimensionalityReduction end
    vertices(R::AbstractDimensionalityReduction) = Int[]

    const Projection{T <: Real} = AbstractMatrix{T}

    include("utils.jl")
    include("isomap.jl")
    include("hlle.jl")
    include("lle.jl")
    include("ltsa.jl")
    include("lem.jl")
    include("diffmaps.jl")

    show(io::IO, ::MIME"text/plain", R::T) where {T<:AbstractDimensionalityReduction} = summary(io, R)
    function show(io::IO, R::T) where {T<:AbstractDimensionalityReduction}
        summary(io, R)
        io = IOContext(io, :limit=>true)
        println(io)
        println(io, "connected component: ")
        Base.show_vector(io, vertices(R))
        println(io)
        println(io, "eigenvalues: ")
        Base.show_vector(io, eigvals(R))
        println(io)
        println(io, "projection:")
        Base.print_matrix(io, transform(R), "[", ",","]")
    end

    # deprecated functions
    @deprecate transform(DimensionalityReduction, X; k=k, d=d) fit(DimensionalityReduction, X; k=k, maxoutdim=d)
    @deprecate projection(DimensionalityReductionModel) transform(DimensionalityReductionModel)
end


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

const ∑ = sum

function innerproduct(P::Type{<:AbstractDiscreteOrthogonalPolynomial}, f, g)
    dom = domain(P)
    fn = x -> f(x) * g(x) * weight_function(P)(x)
    a, b = first(dom), last(dom)
    if !isinf(a) && !isinf(b)
        return ∑(fn(x)  for x in first(dom):last(dom))
    else
        ## what to do if infinite
    end
end

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

abstract type AbstractDiscreteWeightFunction{T,X,N} <: AbstractDiscreteOrthogonalPolynomial{T,X} end
abstract type DiscreteWeightFunction{T,X,N} <:  AbstractDiscreteWeightFunction{T,X,N} end
export DiscreteWeightFunction

"""
    DiscreteWeightFunction

For a discrete measure, `dλ = ∑ wᵢ δ(x - xᵢ)`, specified through two
vectors, `xs` and `ws`, a collection of monic orthogonal polynomials is
produced through Darboux's formula for `α_n` and `β_n` using the
3-term recurrence defined by `π_{n+1} = (x-α_n)⋅π_n - β_n⋅π_{n-1}` (`An=1`, `Bn=-α_n`, `Cn=β_n`)
and the discrete Stieltjes method [Guatschi §3.1](https://www.cs.purdue.edu/homes/wxg/Madrid.pdf).

# Example

Discrete Chebyshev by its weight function (uniform  on 0,1,…,N-1)

```jldoctest
julia> using Polynomials, SpecialPolynomials

julia> const SP = SpecialPolynomials;

julia> N = 9
9

julia> xs, ws = collect(0:N-1), ones(N);   # w(x) = ∑ wⱼ⋅δ(x-xⱼ)

julia> SP.@register0 DWF DiscreteWeightFunction

julia> SP.@register_discrete_weight_function(DWF, xs, ws)

julia> [SP.Bn.(DWF, 0:N-1) SP.Cn.(DWF, 0:N-1)]
9×2 Matrix{Float64}:
 -4.0  9.0
 -4.0  6.66667
 -4.0  5.13333
 -4.0  4.62857
 -4.0  4.12698
 -4.0  3.53535
 -4.0  2.83217
 -4.0  2.01026
 -4.0  1.06667

julia> i,j = 3,4; ## check  that ∫pᵢpⱼdw  = 0    for i,j=3,4

julia> sum(basis(DWF,i)(x) *  basis(DWF,j)(x) * w for  (x,w) in zip(xs, ws))
5.684341886080802e-14

julia> ## Gogin, Hirvensalo (https://doi.org/10.1007/s10958-017-3410-8) characterization
       D(k,N,x) =  sum((-1)^l * binomial(k+l,k) * binomial(N-l,k-l) *  SP.generalized_binomial(x,l) for l in 0:k)
D (generic function with 1 method)

julia> x = variable()
Polynomials.Polynomial(x)

julia> ps,qs = [D(k,N-1,x)  for  k in 0:N-1], [basis(DWF, k)(x) for k  in 0:N-1];

julia> all(qs .* [p[end] for p  in ps] .≈ ps)
true
```

"""
DiscreteWeightFunction

basis_symbol(::Type{<:AbstractDiscreteWeightFunction}) = "W"

xs_ws(::Type{<:AbstractDiscreteWeightFunction}) = throw(ArgumentError("No default method"))

# (Gautschi](https://www.cs.purdue.edu/homes/wxg/Madrid.pdf), section 3.1
# compute α_n = <tπ_n,π_n>/<π_n,π_n>, β_n = <π_n,π_n>/<π_{n-1},π_{n-1}>
# wherer <p,q> = ∑_1^N w_k p(k) q(k)
function discrete_stieltjes(W::Type{<:AbstractDiscreteWeightFunction})

    xs,ws = xs_ws(W)

    N  =  length(xs)
    n = N

    # k = 0 case
    πk_1, πk = zeros(N), ones(N)

    βk = β0 = norm_k = sum(ws)/1 # <π_0, π_0> = ∑ w_k 1 ⋅ 1
    αk = α0 = ∑(ws[k] * xs[k] for k in eachindex(ws)) / norm_k
    αs = [α0]
    βs = [β0]

    for _ ∈ 1:(n-1)

        πk1 = (xs .- αk) .* πk - βk * πk_1 # use just computed αk, βk to find π_{k+1} = (x-αk)⋅π_k - βk⋅π_{k-1}
        norm_k1 =  ∑(ws[k] * πk1[k]^2  for k in eachindex(ws)) # <π_{k+1}, π_{k+1}>

        # Darboux
        # α_{k+1} = <x ⋅ π_{k+1}, π_{k+1}> / <π_{k+1}, π_{k+1}>,
        # β_{k+1} = <π_{k+1}, π_{k+1}> / <π_k, π_k>,
        αk1 = ∑(ws[k] * xs[k] * πk1[k]^2 for k in eachindex(ws)) / norm_k1
        βk1 = norm_k1/norm_k

        push!(αs, αk1)
        push!(βs, βk1)

        αk, βk = αk1, βk1
        πk_1, πk = πk, πk1
        norm_k = norm_k1

    end

    (-αs, βs)
end

An(::Type{W}, n) where {W <: AbstractDiscreteWeightFunction} = one(eltype(W))

Bn(::Type{W}, k::Int) where {W <:AbstractDiscreteWeightFunction} = discrete_stieltjes(W)[1][k+1]
Cn(::Type{W}, k::Int) where {W <:AbstractDiscreteWeightFunction} = discrete_stieltjes(W)[2][k+1]

Polynomials.domain(::Type{<:DiscreteWeightFunction}) = Polynomials.Interval(-Inf, Inf)

innerproduct(W::Type{<:AbstractDiscreteWeightFunction}, f, g) = ∑(wk * f(xk) * g(xk) for (xk, wk) in zip(xs_ws(W)...))


using DataStructures

function getNbors(M::Matrix{Int}, pos::CartesianIndex{2}, expand::Int) :: Vector{CartesianIndex{2}}
  s = size(M) .* expand
  nbors :: Vector{CartesianIndex{2}} = []
  (pos[1] != 1   ) && push!(nbors, pos + CartesianIndex(-1,0))
  (pos[1] != s[1]) && push!(nbors, pos + CartesianIndex(1,0))
  (pos[2] != 1   ) && push!(nbors, pos + CartesianIndex(0,-1))
  (pos[2] != s[2]) && push!(nbors, pos + CartesianIndex(0,1))
  return nbors
end

getNbors(M::Matrix{Int}, pos::CartesianIndex{2}) = getNbors(M,pos,1)

function getWeights(M::Matrix{Int}, pos::CartesianIndex{2}) :: Int
  s = size(M)
  return ((M[(pos[1]-1)%s[1]+1, (pos[2]-1)%s[2]+1]
          + ((pos[1]-1)÷s[1]) + ((pos[2]-1)÷s[2])) - 1) % 9 +1
end

# Checks if neighbor belongs in the queue:
#  - If neighbor has not been visited, adds it to the queue
#  - If neighbor has been visited but with bigger weight, checks if it is still in the queue
#    and either adds it (if not present) or updates weight (if present).
function addNbor!(q::PriorityQueue{CartesianIndex{2},Int}, n::CartesianIndex{2}, w::Int, W::Matrix{Int})
  (W[n] > w) && begin (W[n] = w) ; (in(n,keys(q)) ? q[n] = w : enqueue!(q,n,w)) end
end

# Dijkstra-based algorithm to find the path with less risk.
function bestPath(M::Matrix{Int}, pstart::CartesianIndex, pend::CartesianIndex, expand::Int) :: Int
  W :: Matrix{Int} = 5*sum(M)*ones(size(M) .* expand) # Weight matrix
  q = PriorityQueue{CartesianIndex{2}, Int}()
  enqueue!(q, pstart, 0)
  W[pstart] = 0

  while !isempty(q)
    # Retrieve next position
    pos, weight = dequeue_pair!(q)

    # Have we arrived?
    pos == pend && return weight

    # Otherwise, enqueue new path options
    nbors = getNbors(M,pos,expand)
    map(n -> addNbor!(q,n,weight+getWeights(M,n),W), nbors)
  end
  return -1
end

function readInput() :: Matrix{Int}
  f = open("./inputs/day15.txt")
  data :: Matrix{Int} = zeros(100,100)

  i = 1
  for line in eachline(f)
    data[i,:] = map(x -> parse(Int,x), collect(line))
    i += 1
  end
  return data
end

M = readInput()

expand = 5
res = bestPath(M, CartesianIndex(1,1), CartesianIndex(100*expand,100*expand), expand)
println(res)


"""
	cubicbezier2D(curvePts::Array{Array{Float,1},1})

Produce the two ``coordinate functions`` for a ``cubic`` *Bézier curve* in ``2D``.
The input is the array `curvePts` of 4 control points with 2 coordinates.

# Example

```
julia> curvePts = [[1232.24, 1340.84],[1259.53, 1119.86],
	[1417.91, 1015.16],[1133.47, 1010.63]]

julia> bx,by = cubicbezier2D(curvePts);

julia> (bx(0), by(0))
(1232.24, 1340.84)

julia> (bx(1), by(1))
(1133.47, 1010.63)
```
"""
function cubicbezier2D( curvePts::Array{Array{Float64,1},1} )
	b1(u) = (1 - u)^3
	b2(u) = 3*u*(1 - u)^2
	b3(u) = 3*u^2*(1 - u)
	b4(u) = u^3
	cntrlverts = hcat(curvePts...)
	x = cntrlverts[1,:]
	y = cntrlverts[2,:]
	Bx = u -> x[1]*b1(u) + x[2]*b2(u) + x[3]*b3(u) + x[4]*b4(u)
	By = u -> y[1]*b1(u) + y[2]*b2(u) + y[3]*b3(u) + y[4]*b4(u)
	return Bx,By
end

"""
	const cmdsplit

Regex for splitting the `d` element of a `<path` element into
a sequence of graphics commnds.
"""
const cmdsplit = r"\s*([mMzZlLhHvVcCsSqQtTaA])\s*"

"""
	const digitRegEx

Regex for extracting numeric params from from a sequence of graphics
commnds of a `<path` element.
"""
const digitRegEx = r"\s*(-?[0-9]*\.?\d+)\s*"

"""
	pathparse(elements)

Parse graphics commands in SVG `<path` tagged element.

# Example

```julia

```
"""
function pathparse(data, cnrtlpolygon=false)
	tokens = split(data,'\"')
	pathstring = tokens[2]

	iterator = eachmatch(cmdsplit, pathstring)
	offsets = [m.offset for m in iterator]
	commands = [m.match for m in iterator]

	substrings = [pathstring[offsets[k]+1:offsets[k+1]-1]
		for k=1:length(offsets)-1]
	push!(substrings, pathstring[offsets[end]+1:end])

	params = Array{Float64,1}[]
	for substring in substrings
		numbers = []
		for match in eachmatch(digitRegEx, substring)
			push!(numbers,String(match.match))
		end
		numbers = map(x->parse(Float64,x),numbers)
		push!(params,numbers)
	end

	lines = Array{Float64,1}[]
	global startpoint, endpoint = 0.0, 0.0
	for (command, args) in zip(commands, params)
		if command == "M"
			global startpoint = args
		elseif command == "L"
			endpoint = args
			line = vcat([startpoint,endpoint]...)
			startpoint = endpoint
			push!(lines, line)
		elseif command == "C"
			contrlpt1 = args[1:2]
			contrlpt2 = args[3:4]
			endpoint = args[5:6]
			curvePts = [startpoint,contrlpt1,contrlpt2,endpoint]
			#println(curvePts)

			Bx,By = cubicbezier2D(curvePts)
			pts = [[Bx(u),By(u)] for u=0:.1:1]
			curvelines = [ vcat([pts[k],pts[k+1]]...) for k=1:length(pts)-1 ]
			for line in curvelines
				push!(lines, line)
			end

 			if cnrtlpolygon
				push!(lines,vcat(curvePts[1:2]...),vcat(curvePts[2:3]...),
					vcat(curvePts[3:4]...))
			end
			startpoint = endpoint
		end

	end
	return lines
end

"""
	lines2lar(lines)

LAR model construction from array of float quadruples.
Each `line` in input array stands for `x1,y1,x2,y2`.
"""
function lines2lar(lines)
	vertdict = OrderedDict{Array{Float64,1}, Int64}()
	EV = Array{Int64,1}[]
	idx = 0
	for h=1:size(lines,2)
		x1,y1,x2,y2 = lines[:,h]

		if ! haskey(vertdict, [x1,y1])
			idx += 1
			vertdict[[x1,y1]] = idx
		end
		if ! haskey(vertdict, [x2,y2])
			idx += 1
			vertdict[[x2,y2]] = idx
		end
		v1,v2 = vertdict[[x1,y1]],vertdict[[x2,y2]]
		push!(EV, [v1,v2])
	end
	V = hcat(collect(keys(vertdict))...)
	return V,EV
end

"""
	normalize(V::Lar.Points; flag=true::Bool)::Lar.Points

2D normalization transformation (isomorphic by defaults) of model
vertices to normalized coordinates ``[0,1]^2``. Used with SVG importing.
"""
function normalize(V::Lar.Points; flag=true)
	m,n = size(V)
	if m > n # V by rows
		V = convert(Lar.Points, V')
	end

	xmin = minimum(V[1,:]); ymin = minimum(V[2,:]);
	xmax = maximum(V[1,:]); ymax = maximum(V[2,:]);
	box = [[xmin; ymin] [xmax; ymax]]	# containment box
	aspectratio = (xmax-xmin)/(ymax-ymin)
	if flag
		if aspectratio > 1
			umin = 0; umax = 1
			vmin = 0; vmax = 1/aspectratio; ty = vmax
		elseif aspectratio < 1
			umin = 0; umax = aspectratio
			vmin = 0; vmax = 1; ty = vmax
		end
		T = Lar.t(0,ty) * Lar.s(1,-1) * Lar.s((umax-umin), (vmax-vmin)) *
			Lar.s(1/(xmax-xmin),1/(ymax-ymin)) * Lar.t(-xmin,-ymin)
	else
		# T = Lar.t(0, ymax-ymin) * Lar.s(1,-1)
		T = Lar.s(1,-1)
	end
	dim = size(V,1)
	W = T[1:dim,:] * [V;ones(1,size(V,2))]
	#V = map( x->round(x,digits=8), W )
	V = map(Lar.approxVal(8), W)

	if m > n # V by rows
		V = convert(Lar.Points, V')
	end
	return V
end

"""
	svg2lar(filename::String; flag=true)::Lar.LAR

Parse a SVG file to a `LAR` model `(V,EV)`.
Only  `<line >` and `<rect >` and `<path >` SVG primitives are currently translated.
TODO:  interpretation of transformations.
"""
function svg2lar(filename::String; flag=true)::Lar.LAR
	outlines = Array{Float64,1}[]
	matchall(r::Regex, s::AbstractString; overlap::Bool=false) =
		collect(( m.match for m=eachmatch(r,s,overlap=overlap) ));

	for line in eachline(filename)
		parts = split(lstrip(line), ' ')
		elements = [part for part in parts if part≠""]
		tag = elements[1]
		# SVG <line > primitives
		if tag == "<line"
			r = r"(.)(.)="
			regex = r"([0-9]*?\.[0-9]*)"
			prefixes = matchall(r,line)
			values = matchall(regex,line)
			outline = map(Meta.parse, values)
			#exprs = [ eval(Meta.parse(*(pre,val))) for (pre,val) in zip(prefixes, values) ]
			push!(outlines, outline)
		# SVG <rect > primitives
		elseif tag == "<rect"
			r = r"(.)(.)="
			regex = r"([0-9]*?\.[0-9]*)"
			prefixes = matchall(r,line)
			values = matchall(regex,line)
			x, y, width, height = map(Meta.parse, values)
			# regex = r"""(<rect x=")(.+?)(" y=")(.+?)(" )(.*?)( width=")(.+?)(" height=")(.+?)("/>)"""
			# coords = collect(match( regex , line)[k] for k in (4,6,8,10))
			# x, y, width, height = [ parse(Float64, string) for string in coords ]
			line1 = [ x, y, x+width, y ]
			line2 = [ x, y, x, y+height ]
			line3 = [ x+width, y, x+width, y+height ]
			line4 = [ x, y+height, x+width, y+height ]
			push!(outlines, line1, line2, line3, line4)
		# SVG <path  > primitives
		# see https://github.com/chebfun/chebfun/issues/1617
		elseif tag == "<path"
			dataregex = r"""d=(".*?")(.*?)"""
			data = string(match( dataregex , line)[1])
			polyline = pathparse(data)
			append!(outlines, polyline)
		end
	end
	lines = hcat(outlines...)
	lines = map( x->round(x,sigdigits=8), lines )
	# LAR model construction
	V,EV = lines2lar(lines)
	# normalization
	V = normalize(V,flag=flag)
	return V,EV
end