test_str = "939
7,13,x,x,59,x,31,19"

test_list = split(test_str, '\n')

list = readlines("day13_input.txt")

function departure_time(list)
    earliest = parse(Int, list[1])
    buses = map(x-> parse(Int, x), split(replace(list[2], ",x" => ""), ','))
    next_departures = Dict(map(x->(div(earliest, x) + 1 ) * x => x, buses))
    departure = minimum(keys(next_departures))
    return (departure - earliest) * next_departures[departure]
end

@show departure_time(test_list)
@show departure_time(list)
@show departure_time(test_list) == 295
@show departure_time(list) == 246

function time_stamp(line)
    buses = map(x-> parse(Int, x), split(replace(line, "x" => "-1"), ','))
    imax = argmax(buses .+ Array(0:-1:1-length(buses)))
    bmax = buses[imax]
    n = 1
    t = n*bmax + 1 - imax
    len = length(buses)
    while !all(≤(0), map(i->mod(t+i-1, buses[i]), 1:len))
        n += 1
        t = n*bmax + 1 - imax
    end
    return t
end

@show time_stamp(test_list[2])

@show time_stamp("17,x,13,19") == 3417
@show time_stamp("67,7,59,61") == 754018
@show time_stamp("67,x,7,59,61") == 779210
@show time_stamp("67,7,x,59,61") == 1261476
@show time_stamp("1789,37,47,1889") == 1202161486
#@show time_stamp(list[2]) # Too slow

# Implementing algorithm for solving linear Diophantine equations as
# explained in https://www.math.uwaterloo.ca/~wgilbert/Research/GilbertPathria.pdf
# but there is some bug. It works on all test examples, but not on the problem list.

function time_stamp_diophantine(line)
    buses = map(x-> parse(Int, x), split(replace(line, "x" => "0"), ','))
    running = [b for b=buses if b > 0]
    delays = [1-i for i=1:length(buses) if buses[i] > 0]
    offset = (mod.(running+delays,running))[2:end]
    len=length(running)
    T = fill(0, len, len)
    for i=1:len
        T[i,i] = 1
    end
    A = fill(0, len-1, len)
    for i=1:len-1
        A[i,1] = running[1]
        A[i,i+1] = - running[i+1]
    end
    R = deepcopy(A')
    for i=2:len
        while R[i,i-1] ≠ 0
            if abs(R[i-1,i-1]) > abs(R[i, i-1])
                a = div(R[i-1,i-1], R[i, i-1])
                if mod(R[i-1,i-1], R[i, i-1]) == 0
                    a -= 1
                end
                R[i-1,:] .-= a * R[i, :]
                T[i-1,:] .-= a * T[i,:]
            else
                a = div(R[i, i-1], R[i-1,i-1])
                R[i,:] .-= a * R[i-1,:]
                T[i,:] .-= a * T[i-1,:]
            end
        end
    end
    k = BigInt.((R[1:end-1,:]')\offset)
    k = [k; BigInt(floor(-sum([T[i,1]*k[i]/T[len,1] for i=1:len-1])))]
    n1 = sum([T[i,1]*k[i] for i=1:len])
#    while !all(>(0), [sum([T[i,j]*k[i] for i=1:len]) for j=1:len])
    while n1 < 0
        k[len] += sign(T[len,1])
        n1 = sum([T[i,1]*k[i] for i=1:len])
    end

    t = n1*running[1]
    return running, offset, len, A, T, R, k, n1, t
end

@show time_stamp_diophantine(test_list[2])[end] == 1068781
@show time_stamp_diophantine("7,13,x,x,59,x,31,19")[end] == 1068781
@show time_stamp_diophantine("17,x,13,19")[end] == 3417
@show time_stamp_diophantine("67,7,59,61")[end]  == 754018
@show time_stamp_diophantine("67,x,7,59,61")[end]  == 779210
@show time_stamp_diophantine("67,7,x,59,61")[end]  == 1261476
@show time_stamp_diophantine("1789,37,47,1889")[end]  == 1202161486
@show time_stamp_diophantine(list[2])[end] == 939490236001473

function test_result(line)
    running, offset, len, A, T, R, k, n1, t = time_stamp_diophantine(line)

    check = all(>(0), [sum([T[i,j]*k[i] for i=1:len]) for j=1:len])
    timings = (T'k) .* running
    check_offset = timings[2:end] - fill(timings[1], len-1)
    difference = all(==(0), check_offset + offset)
    return check, difference, T*A'==R
end

# Using the algorithm for the Chinese Remainder Theorem

function time_stamp_CRG(line)
    buses = map(x-> parse(Int, x), split(replace(line, "x" => "0"), ','))
    running = [b for b=buses if b > 0]
    offset = [mod(b+1-i,b) for (i,b) in enumerate(buses) if b>0]
    N = prod(running)
    d = div.(N,running)
    result = sum(offset .* d .* invmod.(d,running))
    return mod(result, N)
end

# There is an option to use Mods.jl package, which already has an implemention
# of the Chinese Remainder Theorem, as I learned from other Humans of Julia on
# Discord, but it is more obscure and slower, I prefer the direct implementation
# above.

using Mods

function time_stamp_ModsCRT(line)
    buses = map(x-> parse(Int, x), split(replace(line, "x" => "0"), ','))
    running = [b for b=buses if b > 0]
    x_delays = [i-1 for i=1:length(buses) if buses[i] > 0]
    mod_prep = Mod[Mod(b,d) for (d,b) in zip(running, x_delays)]
    rem = Mods.CRT(mod_prep...)
    return modulus(rem) - rem.val
end

@show time_stamp_CRG(test_list[2]) == 1068781
@show time_stamp_CRG(list[2]) == 939490236001473
@show time_stamp_ModsCRT(test_list[2]) == 1068781
@show time_stamp_ModsCRT(list[2]) == 939490236001473

#=
```julia
julia> @btime time_stamp_ModsCRT(list[2])
  23.110 μs (45 allocations: 5.47 KiB)
939490236001473

julia> @btime time_stamp_CRG(list[2])
  15.109 μs (24 allocations: 5.28 KiB)
939490236001473
```
 =#

nothing

#=

If `x=buses` (with `x=-1` if bus = 'x'), time `t` is given by the condition
that `t+i-1` is an integer multiple of `x[i]`, i.e. `mod(t+i-1,x) == 0` or
`mod(t, x) = 1-i` or `mod(t,x) = x-i+1`

In case `t` is not so big, we can just search for this condition
by stepping up `t` by multiples of `x[1]`. That was my first approach,
which works fast for the test set. But for the real list, that is not
computationally feasible. So we do differently.

The conditions

    `t + i - 1 = nᵢ xᵢ,   ∀ i=1:length(x) s.th. xᵢ > 0`

are equivalent to
    `t = n₁x₁ = n₂x₂ - 1 = ⋯ = nᵢxᵢ - i + 1,     ∀ i, with xᵢ > 0`

And we know that `x₁ > 0`. Hence we just iterate over `i` to find

    `For i = 1:
        n₁¹ = 1
    For i = 2:
        n₂² such that n₂²x₂ - 1 = jn₁¹x₁
        n₁² = j₂²n₁¹
    For i = 3:
        n₃³ such that n₃³x₂ - 2 = jn₁²x₁
        n₁² = jn₁²
    `n₁ such that t = n₁ x₁`
    `n₂ such that n₂x₂ - 1 = `

```julia
n = 1
for i = 2:length(x)
    j = 1
    while mod(j*n*buses[i]-i+1, buses[1]) > 0
        j += 1
    end
    n *= j
end
```

But this is slow in some cases. So we notice the problem can also be written as
    `t = 0 (mod x₁); t = x₂-1 (mod x₂); … t = xᵢ+1-i (mod xᵢ); …`

which is in the context of the Chinese Remainder Problem (see e.g.
[Chinese Remainder Theorem](https://en.wikipedia.org/wiki/Chinese_remainder_theorem))
and [Chinese Remainder Theorem with algorithm](https://shainer.github.io/crypto/math/2017/10/22/chinese-remainder-theorem.html))

An algorithm, in python, from the latter homepage, is the following

def ChineseRemainderGauss(x, a):
    """
    Return t such that mod(t,xᵢ) = aᵢ, ∀i=1,…,n, if 0 ≤ aᵢ< xᵢ
    and the xᵢ are pairwise coprime.
    """
    from math import prod
    P = prod(x)
    result = 0

    for i in range(len(n)):
        ai = a[i]
        xi = x[i]
        bi = P // xi

        result += ai * bi * invmod(bi, xi)

    return result % P

See also https://shainer.github.io/crypto/math/2017/10/22/chinese-remainder-theorem.html

 =#


using Plots

func1(R) = R/2
func2(R) = R/2 + [1 ; 0]
func3(R) = R/2 + [0 ; 1]
P = 200
num = 500000
x = []
y = []
for i in 1:num
    point = [rand() ; rand()]
    for j in 1:P
        point = rand([func1(point), func2(point), func3(point)])
    end
    push!(x, point[1])
    push!(y, point[2])
end

scatter(x,y,markersize = 0.000001, legend = false, border=:none, dpi=500)
savefig("C:\\Users\\Yaghoub\\Documents\\GitHub\\A-few-fractals-in-Julia\\Fractals-Fig\\SRT.png")


function inner_product(a1, a2)
  somme = 0
  for (word1, count1) in a1
    for (word2, count2) in a2
      if word2 == word1
        somme += count1 * count2
        break
      end
    end
  end
  return somme
end

function len(a)
  return √(innter_product(a, a))
end

function θ(a1, a2)
  cosθ = inner_product(a1, a2) / √(inner_product(a1, a1) * inner_product(a2, a2))
  return acos(cosθ)
end


using StructuresKit

E = 29000.0
σy = 50.0                   #steel yield stress
σy1 = 50.0                  #steel yield stress
σu = 65.3                   #steel ultimate stress
σf=σy * (1 + 0.10)      #steel fracture stress#
ϵy = σy / E              #steel yield strain
ϵy1 = σy / E * 10    #steel strain at end of yield plateau
ϵu = 0.18             #steel ultimate strain
ϵf = 0.21             #steel fracture strain
n = 11

σ, ϵ  = MaterialModels.steel(σy, σy1, σu, σf, ϵy, ϵy1, ϵu, ϵf, n)

using Plots
plot(σ, ϵ, markershape = :o)

shape_name = "W14X90"

shape_info = CrossSection.AISC(shape_name)

n_Wshape=(4, 2, 4, 4, 4)

xcoords, ycoords = CrossSection.wshape_nodes(shape_info, n_Wshape)

plot(xcoords, ycoords, markershape = :o)

# using TriangleMesh

mesh_size = 0.01
mesh = CrossSection.triangular_mesh(xcoords, ycoords, mesh_size)

Ai, cxi, cyi = CrossSection.triangular_mesh_properties(mesh)

cx, cy = CrossSection.centroid_from_cells(Ai, cxi, cyi)

Ix = CrossSection.moment_of_inertia_from_cells(Ai, cyi, cy)
Iy = CrossSection.moment_of_inertia_from_cells(Ai, cxi, cx)

# #discretize cross-section with a triangular mesh
# function triangular_mesh(xcoords, ycoords, mesh_size)

#     num_nodes = length(xcoords)
#     num_segments = num_nodes

#     # n_point, n_point_marker, n_point_attribute, n_segment, n_holes
#     poly = TriangleMesh.Polygon_pslg(num_nodes, 1, 0, num_segments, 0)

#     node = [xcoords ycoords]
#     set_polygon_point!(poly, node)

#     node_marker = ones(Int, num_nodes, 1)
#     set_polygon_point_marker!(poly, node_marker)

#     segments = zeros(Int, num_segments, 2)
#     for i=1:num_segments

#         if i == num_segments
#             segments[i, 1:2] = [i, 1]
#         else
#             segments[i, 1:2] = [i, i+1]
#         end

#     end

#     set_polygon_segment!(poly, segments)

#     segment_markers = ones(Int, num_segments)
#     set_polygon_segment_marker!(poly, segment_markers)

#     #switches from https://www.cs.cmu.edu/~quake/triangle.html
#     switches = "penvVa" * string(mesh_size) * "D"

#     mesh = create_mesh(poly, switches)

#     return mesh

# end

#calculate triangle areas

# # https://keisan.casio.com/has10/SpecExec.cgi?path=05000000.Mathematics%252F01000500.Plane%2520geometry%252F10010300.Area%2520of%2520a%2520triangle%2520with%2520three%2520points%252Fdefault.xml&charset=utf-8
# function triangle_area(x1, y1, x2, y2, x3, y3)

#     A = abs((x1*y2 + x2*y3 + x3*y1 - y1*x2 - y2*x3 - y3*x1)/2)

# end

# #https://www.mathopenref.com/coordcentroid.html
# function triangle_centroid(x1, y1, x2, y2, x3, y3)

#     cx = (x1 + x2 + x3)/3
#     cy = (y1 + y2 + y3)/3

#     return cx, cy

# end

# function triangulation_properties(mesh)

#     #calculate cell area and centroid
#     Ai = zeros(Float64, mesh.n_cell)
#     cxi = zeros(Float64, mesh.n_cell)
#     cyi = zeros(Float64, mesh.n_cell)

#     for i = 1:mesh.n_cell

#         p1 = mesh.cell[1, i]
#         p2 = mesh.cell[2, i]
#         p3 = mesh.cell[3, i]

#         x1 = mesh.point[1, p1]
#         y1 = mesh.point[2,p1]
#         x2 = mesh.point[1, p2]
#         y2 = mesh.point[2,p2]
#         x3 = mesh.point[1, p3]
#         y3 = mesh.point[2,p3]

#     Ai[i] = triangle_area(x1, y1, x2, y2, x3, y3)

#     cxi[i], cyi[i] = triangle_centroid(x1, y1, x2, y2, x3, y3)

#     end

#     return Ai, cxi, cyi

# end

# #calculate cross-sectional area from cells

# # function area_from_cells(Ai)

# #     A = sum(Ai)

# # end

# # #calculate cross-section centroid from cells

# # function centroid_from_cells(Ai, cxi, cyi)

# #     A = area_from_cells(Ai)

# #     cx = sum(cxi .* Ai) / A
# #     cy = sum(cyi .* Ai) / A

# #     return cx, cy

# # end

# cx, cy = CrossSection.centroid_from_cells(Ai, cxi, cyi)

# # function moment_of_inertia_from_cells(Ai, ci, c)

# #     I = sum(((c .- ci) .^2 .* Ai))

# #     return I

# # end

# Ix = moment_of_inertia_from_cells(Ai, cyi, cy)
# Iy = moment_of_inertia_from_cells(Ai, cxi, cx)

# #calculate triangle centroids

# # for i = 1:mesh.n_cell

# #     p1 = mesh.cell[1, i]
# #     p2 = mesh.cell[2, i]
# #     p3 = mesh.cell[3, i]

# #     x1 = mesh.point[1, p1]
# #     y1 = mesh.point[2,p1]
# #     x2 = mesh.point[1, p2]
# #     y2 = mesh.point[2,p2]
# #     x3 = mesh.point[1, p3]
# #     y3 = mesh.point[2,p3]

# #    cx[i] = triangle_area(x1, y1, x2, y2, x3, y3)

# # end

# zcoords = zeros(Float64, mesh.n_point)
# coordinates = [mesh.point[1,:] mesh.point[2,:] zcoords]

# connectivity = mesh.cell'

# using Makie
# scene = Makie.poly(coordinates, connectivity, color=:lightgray, shading=:true, show_axis=:true, overdraw=:false, strokecolor = (:black, 0.6), strokewidth = 1)

# poly = polygon_Lshape()
# mesh = create_mesh(poly, info_str="my mesh", voronoi=true, delaunay=true, set_area_max=true)

# node = [1.0 0.0 ; 0.0 1.0 ; -1.0 0.0 ; 0.0 -1.0 ;
#         0.25 0.25 ; -0.25 0.25 ; -0.25 -0.25 ; 0.25 -0.25]

#         # size is number_segments x 2
# seg = [1 2 ; 2 3 ; 3 4 ; 4 1 ; 5 6 ; 6 7 ; 7 8 ; 8 5]

# # all points get marker 1
# node_marker = [ones(Int,4,1) ; 2*ones(Int,4,1)]
# # last segment gets a different marker
# seg_marker = [ones(Int,4) ; 2*ones(Int,4)]

# # size is number_points x number_attr
# node_attr = rand(8,2)

# # size is number_holes x 2
# hole = [0.5 0.5]

# polygon_struct_from_points

# shape_mesh, cross_section_edges = Mesh.open_cross_section_tessellation(xcoords, ycoords)

# connectivity = zeros(Int, length(shape_mesh), 3)
# for i=1:length(shape_mesh)
#     connectivity[i, :] = shape_mesh[i]
# end


@testset "Testing matrix_eqn.jl" begin
    rough_kwargs = Dict(:atol => 1e-7, :rtol => 1e-7)

    @testset "simple test where X is all zero" begin
        A = Matrix(I, 4, 4) .* .95
        B = zeros(4, 4)

        X = solve_discrete_lyapunov(A, B)

        @test X == (B)
    end

    @testset "simple test where X is same as B" begin
        A = fill(0.5, 2, 2)
        B = [.5 -.5; -.5 .5]

        X = solve_discrete_lyapunov(A, B)

        @test X == (B)
    end

    @testset "simple_ones lyap" begin
        A = zeros(4, 4)
        B = ones(4, 4)

        sol = solve_discrete_lyapunov(A, B)

        @test sol == (B)
    end

    @testset "scalars lyap" begin
        a, b = 0.5, 0.75
        sol = solve_discrete_lyapunov(a, b)

        @test sol == ones(1, 1)
    end

    @testset "testing ricatti golden_num_float" begin
        val = solve_discrete_riccati(1.0, 1.0, 1.0, 1.0)
        gold_ratio = (1 + sqrt(5)) / 2.
        @test isapprox(val[1], gold_ratio; rough_kwargs...)
    end

    @testset "testing ricatti golden_num_2d" begin
        If64 = Matrix{Float64}(I, 2, 2)
        A, B, R, Q = If64, If64, If64, If64
        gold_diag = If64 .* (1 + sqrt(5)) ./ 2.
        val = solve_discrete_riccati(A, B, Q, R)
        @test isapprox(val, gold_diag; rough_kwargs...)
    end

    @testset "test tjm 1" begin
        A = [0.0 0.1 0.0
             0.0 0.0 0.1
             0.0 0.0 0.0]
        B = [1.0 0.0
             0.0 0.0
             0.0 1.0]
        Q = [10^5 0.0 0.0
             0.0 10^3 0.0
             0.0 0.0 -10.0]
        R = [0.0 0.0
             0.0 1.0]
        X = solve_discrete_riccati(A, B, Q, R)
        Y = diagm(0 => [1e5, 1e3, 0.0])

        @test isapprox(X, Y; rough_kwargs...)
    end

    @testset "test tjm 2" begin
        A = [0.0 -1.0
             0.0 2.0]
        B = [1.0 0.0
             1.0 1.0]
        Q = [1.0 0.0
             0.0 0.0]
        R = [4.0 2.0
             2.0 1.0]
        X = solve_discrete_riccati(A, B, Q, R)
        Y = zeros(2, 2)
        Y[1, 1] = 1
        @test isapprox(X, Y; rough_kwargs...)
    end

    @testset "test tjm 3" begin
        r = 0.5
        Im = Matrix{Float64}(I, 2, 2)
        A = [2.0+r^2 0.0
             0.0     0.0]
        B = Im
        R = [1.0 r
             r   r*r]
        Q = Im - A' * A + A' * ((R + Im) \ A)
        X = solve_discrete_riccati(A, B, Q, R)
        Y = Im
        @test isapprox(X, Y; rough_kwargs...)
    end

end  # @testset


"""
Betina Robust Sampled based Formulation

Further information:
  - Fernandes, B., Street, A., ValladA˜ £o, D., e Fernandes, C. (2016). An adaptive robust portfolio optimization model with loss constraints based on data-driven polyhedral uncertainty sets. European Journal of Operational Research, 255(3):961 – 970. ISSN 0377-2217. URL.

"""
struct RobustBetina <: AbstractSampleBased
    sampled_returns::Array{Float64,2}
    number_of_assets::Int
    number_of_samples::Int
end

function RobustBetina(;
    sampled_returns::Array{Float64,2}
)
    number_of_samples, number_of_assets = size(sampled_returns)

    return RobustBetina(
        sampled_returns, number_of_assets, number_of_samples
    )
end

function _portfolio_return_latex_RobustBetina_dual()
    return """
        ```math
        \\max_{\\theta} \\quad  \\theta \\\\
        s.t.  \\quad \\theta \\leq r_s ' w \\quad \\forall s = 1:\\mathcal{S} \\\\
        ```
        """
end

"""
    portfolio_return!(model::JuMP.Model, w, formulation::RobustBetina)

Returns worst case return in Betina's uncertainty set, defined by the following dual problem:

$(_portfolio_return_latex_RobustBetina_dual())
"""
function portfolio_return!(model::JuMP.Model, w, formulation::RobustBetina)
    # auxilary variables
    θ = @variable(model, θ)
    # convex hull
    @constraint(model, sum(formulation.sampled_returns * w , dims=2) .>= θ)

    return θ
end


using GaussQuadrature, GLMakie

function cubed_sphere_warp(
    a,
    b,
    c,
    R = max(abs(a), abs(b), abs(c)),
)

    function f(sR, ξ, η)
        X, Y = tan(π * ξ / 4), tan(π * η / 4)
        ζ1 = sR / sqrt(X^2 + Y^2 + 1)
        ζ2, ζ3 = X * ζ1, Y * ζ1
        ζ1, ζ2, ζ3
    end

    fdim = argmax(abs.((a, b, c)))
    if fdim == 1 && a < 0
        # (-R, *, *) : formulas for Face I from Ronchi, Iacono, Paolucci (1996)
        #              but for us face II of the developed net of the cube
        x1, x2, x3 = f(-R, b / a, c / a)
    elseif fdim == 2 && b < 0
        # ( *,-R, *) : formulas for Face II from Ronchi, Iacono, Paolucci (1996)
        #              but for us face III of the developed net of the cube
        x2, x1, x3 = f(-R, a / b, c / b)
    elseif fdim == 1 && a > 0
        # ( R, *, *) : formulas for Face III from Ronchi, Iacono, Paolucci (1996)
        #              but for us face IV of the developed net of the cube
        x1, x2, x3 = f(R, b / a, c / a)
    elseif fdim == 2 && b > 0
        # ( *, R, *) : formulas for Face IV from Ronchi, Iacono, Paolucci (1996)
        #              but for us face I of the developed net of the cube
        x2, x1, x3 = f(R, a / b, c / b)
    elseif fdim == 3 && c > 0
        # ( *, *, R) : formulas for Face V from Ronchi, Iacono, Paolucci (1996)
        #              and the same for us on the developed net of the cube
        x3, x2, x1 = f(R, b / c, a / c)
    elseif fdim == 3 && c < 0
        # ( *, *,-R) : formulas for Face VI from Ronchi, Iacono, Paolucci (1996)
        #              and the same for us on the developed net of the cube
        x3, x2, x1 = f(-R, b / c, a / c)
    else
        error("invalid case for cubed_sphere_warp(::EquiangularCubedSphere): $a, $b, $c")
    end

    return x1, x2, x3
end

N = 5
ξ¹, ω¹ = GaussQuadrature.legendre(N, GaussQuadrature.both)
ξ² = copy(ξ¹)
ξ³ = copy(ξ¹)

Ne = 12
vertices = collect(range(-1, 1, length = Ne + 1))
x¹ = zeros(N, Ne)
x² = copy(x¹)
x³ = copy(x¹)

for e in 1:Ne
    @. x¹[:, e] = (ξ¹ + 1) / 2 * (vertices[e+1] - vertices[e]) + vertices[e]
    @. x²[:, e] = (ξ¹ + 1) / 2 * (vertices[e+1] - vertices[e]) + vertices[e]
    @. x³[:, e] = (ξ¹ + 1) / 2 * (vertices[e+1] - vertices[e]) + vertices[e]
end

Nex = Ney = Ne
face1 = [(x¹[1], x²[j, ex], x³[k, ey]) for j in 1:N, k in 1:N, ex in 1:Nex, ey in 1:Ney]
warpedface1 = [cubed_sphere_warp(face1[i]...) for i in eachindex(face1)]

face2 = [(x¹[end], x²[j, ex], x³[k, ey]) for j in 1:N, k in 1:N, ex in 1:Nex, ey in 1:Ney]
warpedface2 = [cubed_sphere_warp(face2[i]...) for i in eachindex(face2)]

face3 = [(x¹[j, ey], x²[1], x³[k, ex]) for j in 1:N, k in 1:N, ex in 1:Nex, ey in 1:Ney]
warpedface3 = [cubed_sphere_warp(face3[i]...) for i in eachindex(face1)]

face4 = [(x¹[j, ey], x²[end], x³[k, ex]) for j in 1:N, k in 1:N, ex in 1:Nex, ey in 1:Ney]
warpedface4 = [cubed_sphere_warp(face4[i]...) for i in eachindex(face1)]

face5 = [(x¹[j, ex], x²[k, ey], x³[1]) for j in 1:N, k in 1:N, ex in 1:Nex, ey in 1:Ney]
warpedface5 = [cubed_sphere_warp(face5[i]...) for i in eachindex(face1)]

face6 = [(x¹[j, ex], x²[k, ey], x³[end]) for j in 1:N, k in 1:N, ex in 1:Nex, ey in 1:Ney]
warpedface6 = [cubed_sphere_warp(face6[i]...) for i in eachindex(face1)]

# Now Plot
lw = 1  # linewidth
ms = 25 # marker size

fig = Figure(resolution = (1400, 1200))
ax = LScene(fig[1, 1])
# fig, ax, sc = scatter(warpedface1, color = :red, markersize = 0.0, show_axis = false)
# scatter(warpedface1, color = :red, markersize = 0.0, show_axis = false)
r = range(-1, 1, length = Ne + 1)

xa = zeros(Ne + 1, Ne + 1)
xb = zeros(Ne + 1, Ne + 1)
xc = zeros(Ne + 1, Ne + 1)

xa .= r
xb .= r'
xc .= 1

a = [cubed_sphere_warp(a, b, c)[1] for (a, b, c) in zip(xa, xb, xc)] .* 1.00
b = [cubed_sphere_warp(a, b, c)[2] for (a, b, c) in zip(xa, xb, xc)] .* 1.00
c = [cubed_sphere_warp(a, b, c)[3] for (a, b, c) in zip(xa, xb, xc)] .* 1.00

wireframe!(ax,
    a, b, c,
    show_axis = false,
    linewidth = lw)

wireframe!(ax,
    -c, a, b,
    show_axis = false,
    linewidth = lw * 5)

scatter!(ax, warpedface1, color = :red, markersize = ms * 0.75,)
scatter!(ax, warpedface3, color = :blue, markersize = ms,)

rotate_cam!(fig.scene.children[1], (π/12, π, 0))


function OPF_dual(optimizer,set_generators,set_nodes,set_demands,generators,demands,capacity,varcost,load_node,utility_node,links,links_rev,F_max_dict,B_dict,MapG,MapD)

m=Model(optimizer)

@variable(m, μ_G[g in set_generators]>= 0)
@variable(m, μ_D[d in set_demands]>= 0)
@variable(m, η_lower[link in links]>= 0)
@variable(m, η_upper[link in links]>= 0)
@variable(m, λ[set_nodes])
@variable(m, γ)

@objective(m, Min,     sum(μ_D[d]*demands[d,load_node]  for d in set_demands) +
                       sum(μ_G[g]*generators[g,capacity]  for g in set_generators)+
                       sum(F_max_dict[j]*(η_lower[j]+η_upper[j])   for j in links)
                       )

@constraint(m, constraint1[d in set_demands], -demands[d,utility_node]+ μ_D[d]+λ[MapD[d][2]]>= 0)

@constraint(m, constraint2[g in set_generators], generators[g,varcost]+ μ_G[g]-λ[MapG[g][2]]>= 0)

#@constraint(m, constraint3[n in set_nodes_ref],     sum(B_dict[j]*(λ[j[1]]-λ[j[2]]+η_upper[j] -η_lower[j]) for j in links if n == j[1])
#                                                     +γ == 0)

@constraint(m, constraint3[n in set_nodes_ref],     sum(B_dict[j]*(λ[j[1]]-λ[j[2]]+η_upper[j] -η_lower[j]) for j in links if n == j[1])
                                                      +sum(B_dict[j]*(-η_upper[j] +η_lower[j]) for j in links_rev if n == j[2] )+γ == 0)

#@constraint(m, constraint4[n in set_nodes_noref],  sum(B_dict[j]*(λ[j[1]]-λ[j[2]]+η_upper[j] -η_lower[j]) for j in links if n == j[1])
#                                                      == 0)

@constraint(m, constraint4[n in set_nodes_noref],  sum(B_dict[j]*(λ[j[1]]-λ[j[2]]+η_upper[j] -η_lower[j]) for j in links if n == j[1])
                                                      +sum(B_dict[j]*(-η_upper[j] +η_lower[j]) for j in links_rev if n == j[2] )== 0)

@time optimize!(m)

status = termination_status(m)
println("The solution status is: $status")

syscost_det=objective_value(m)

println("System Cost:",syscost_det)

Dual_constraint2=zeros(length(set_generators))

for g in set_generators
   Dual_constraint2[g]= JuMP.dual(constraint2[g])
end
println("Production level of generator g:", Dual_constraint2)

Dual_constraint1=zeros(length(set_demands))

for d in set_demands
   Dual_constraint1[d]= JuMP.dual(constraint1[d])
end
println("Consumption level of demand d: ", Dual_constraint1)

λ_value=zeros(length(set_nodes))
for n in set_nodes
  λ_value[n]=JuMP.value.(λ[n])
end
println("Electricity Price:",λ_value)

#Review this flow lines
#Review all the constraints

f_value=zeros(n_link)
global i=1

for j in links
global jj=j
if set_nodes_ref==jj[1]
global f_value[i]= B_dict[j]*(JuMP.dual(constraint3[j[1]])-JuMP.dual(constraint4[j[2]]))
elseif set_nodes_ref==jj[2]
global f_value[i]= B_dict[j]*(JuMP.dual(constraint4[j[1]])-JuMP.dual(constraint3[j[2]]))
else
global f_value[i]= B_dict[j]*(JuMP.dual(constraint4[j[1]])-JuMP.dual(constraint4[j[2]]))
end
println("Power Flow lines $j:", f_value[i])
global i=1+i
end

return (syscost_det,Dual_constraint2,Dual_constraint1,λ_value,f_value)
end

#cd("C:\\Users\\braya\\.julia\\dev\\DTU_BrayamValqui_SP2021\\src")
#include("Singlefile_OPF_dual.jl")


# Simulate a 5-area (~20 Hz) beta network instantiated by an AR(3) model =============================
# cd("Homework_3_Lect_5&6")

xDim=5 # state dimensionå
order=3# p=3 order of the model
T=3# Trial duration in seconds
nTrials=200
Fs=200# sampling rate in per second
dt=1/Fs
burnin=1000 # for transient removal
df=0.001#frequency resolution to be used in the parametric AR estimation
nF=1/df+1#number of frequencies evaluated in the parametric AR estimation

N=T*Fs

# The AR(3) matrices
A = zeros(xDim,xDim,order)
a=sqrt(2)
A[1,1,1] =  0.95*a
A[1,1,2] = -0.9025
A[2,1,2] =  0.5
A[3,1,3] = -0.4
A[4,1,2] = -0.5
A[4,4,1] =  0.25*a
A[4,5,1] =  0.25*a
A[5,4,1] = -0.25*a
A[5,5,1] =  0.25*a

# EXERCISE (1a): Stability/Stationarity Build the companion matrix (augmented state AR(1)) and check stability
m,n,p = size(A)
pn = (p-1)*m
Ac = [reshape(A,m,p*n); eye(pn) zeros(pn,m)]# companion matrix

# Sampling from the VAR(3) model
# The noise covariance (Using DIAGONAL COVARIANCE MATRIX FOR SIMPLICITY ...)
SIGMA =diag([0.60.5 0.3 0.3 0.6])
mu = zeros(xDim,1)
for r=1:nTrials
    r
    E=mvnrnd(mu,SIGMA,N+burnin)" #Multivariate GWN sequence
    X(:,1:order,r)=E(:,1:order)
    for k=order+1:N+burnin
        x=0
        for j=1:order
            x=x+squeeze(A(:,:,j))*squeeze(X(:,k-j,r))
        end
        X(:,k,r)=x+E(:,k)
    end
end
X=X(:,burnin+1:end,:)

# Computing the spectral matrix for the sampled VAR(p) data via multitaper ===========
NFFT=N
bandWidth = ...
removeTemporalMean=true
RemoveEnsembleMean=true
nTapers=[]
clear S
xDim = 5
for j=1:xDim
    x=squeeze(X(j,:,:))
    for k=j+1:xDim
        y=squeeze(X(k,:,:))
        [Sxx, Syy, Sxy, ~, ~, F, nTapers]= multitaperSpectrum(x,y,Fs,bandWidth,NFFT,removeTemporalMean,RemoveEnsembleMean,nTapers)
        S(j,j,:)=Sxx
        S(j,k,:)=Sxy #Cross-spectrum
        S(k,j,:)=conj(Sxy)
    end
end
S(j,j,:)=Syy

# To compute the partial coherence =======================
# S is the spectral matrix S(channel j, channel k, frequency), j,k = 1, 2, ... xDim
for j=1:xDim
    for k=j+1:xDim
        i1=setdiff([1:xDim],j)
        i2=setdiff([1:xDim],k)
        for f=1:length(F)
            Mjk = det(S(i1,i2,f))
            Mjj = det(S(i1,i1,f))
            Mkk = det(S(i2,i2,f))
            Cp(j,k,f) = abs(Mjk)/real(sqrt(Mjj*Mkk))#to avoid numerical issues, force it to be real
        end
    end
end

# Estimating VAR(p) from data ===================
[Ah,SIGMAh,Eh] = var_maxent(X,order)
DSIG = det(SIGMA)# residuals covariance matrix determinant
if DSIG <= 0
    fprintf(2,"  WARNING: residuals covariance not positive definite\n")
end
M=N*nTrials
L= -(M/2)*log(DSIG) #max loglikelihood
aic = -2*L + 2*order*xDim^2*(M/(M-order-1)) # Note AIC without correction = -2*L + 2*order*xDim^2
bic = -2*L + order*log(M)

# Transfer function H(f) and spectral matrix S(f) from fitted model
# F=[0:nF-1]*df*Fs/2
delta=1
F=linspace(0,0.5,nF)# use normalized frequency (cycles/sample) first
j=0
for f=F
    j=j+1
    H = eye(xDim) # identity matrix
    for m=1:order
        H=H-squeeze(Ah(:,:,m))*exp(-1i*m*2*pi*f)
    end
    H = inv(H)
    #S(:,:,j) = H*SIGMAh*ctranspose(H)
    S(:,:,j) = H*SIGMAh*H"
    #Note: the transpose " " " will also recognize the complex data and perform the conjugate transpose
    #Note also, however, that ." implements only the transpose, not the conjugate transpose
end
S = 2 * delta^2 * 1/(N*delta) * S #Normalize the spectral matrix
for k = 1:xDim, S(k,k,:) = real(S(k,k,:))end #make sure the diagonal has real numbers (power spectrum)
F=F*Fs# Change frequency to Hz


using ValidatedNumerics
using FactCheck

A =
    [
        @interval( 2, 4)    @interval(-2, 1) ;
        @interval(-1, 2)    @interval( 2, 4)
    ]

b =
    [
        @interval(-2, 2),
        @interval(-2, 2)
    ]

facts("Linear algebra with intervals tests") do

    @fact A * b -->
        [
            @interval(-12, 12),
            @interval(-12, 12)
        ]

    # Example from Moore et al., Introduction to Interval Analysis (2009), pg. 88:

    @fact A \ b -->
        [
            @interval(-5, 5),
            @interval(-4, 4)
        ]

end


import IterativeSolvers: gmres!, bicgstabl!
import LinearAlgebra: ldiv!

abstract type AbstractLinearSolver end
abstract type DirectSolver <: AbstractLinearSolver end
abstract type IterativeSolver <: AbstractLinearSolver end

struct LU  <:DirectSolver end
struct GMRES  <: IterativeSolver end
struct BICGSTAB  <: IterativeSolver end

linsolve(A,b ; args...) = linsolve(A,b,LU() ; args...)

function linsolve(A,b,solver::AbstractLinearSolver ; args...)
    x = similar(b)
    linsolve!(x,A,b,solver; args...)
    return x
end

linsolve!(x,A,b ; args...) = linsolve!(x,A,b,LU() ; args...)

function linsolve!(x,A,b , ::LU ; args...)
  ldiv!(x, lu(A), b; args...)
end

function linsolve!(x,A,b,::GMRES ; args...)
    gmres!(x,A,b; args...)
end

function linsolve!(x,A,b,::BICGSTAB ; args...)
    bicgstabl!(x,A,b; args...)
end

# methods for sensitivty analysis
function linsolve!(x,x_adj,A,b,b_adj,::LU ; args...)
    F = lu(A)
    ldiv!(x, F, b; args...)
    ldiv!(x_adj, transpose(F), b_adj; args...)
end

function linsolve(A,b,b_adj,::LU ;  args...)
    x = similar(b) ; x_adj = similar(b_adj)
    linsolve!(x,x_adj,A,b,b_adj,LU(); args...)
    return x , x_adj
end


using ModelingToolkit, OrdinaryDiffEq, Test

@parameters t g e b
@variables v(t) w(t) F(t)
@derivatives D'~t
single_neuron_eqs = [
    D(v) ~ min(max(-2 - v, v), 2 - v) - w + F, # add the flux term
    D(w) ~ e * (v - g * w + b)
]
n1 = ODESystem(single_neuron_eqs, t, [v,w,F],[g,e,b], name = :n1)
n2 = ODESystem(single_neuron_eqs, t, [v,w,F],[g,e,b], name = :n2)
@parameters D Dk
connections = [
    0 ~ n1.F - D * Dk * max(n1.v - n2.v, 0)
    0 ~ n2.F - D * max(n2.v - n1.v, 0)
]
connected = ODESystem(connections,t,[],[D,Dk],systems=[n1,n2],name=:connected)

u0 = [
    n1.v => -2,
    n1.w => -2/3,
    n1.F => 0,
    n2.v => -2,
    n2.w => -.7,
    n2.F => 0,
]
tspan = (0.0, 1750.0)
p0 = [
    n1.g => 0.8,
    n1.e => 0.04,
    n1.b => 0,
    n2.g => 0.8,
    n2.e => 0.04,
    n2.b => 0.2,
    D => 0.047,
    Dk => 1
]

prob = ODEProblem(connected, u0, tspan, p0)
sol = solve(prob,Rodas5(),initializealg=BrownFullBasicInit())
@test prob.u0 == sol[1]
sol = solve(prob,Rodas5(),initiailizealg=ShampineCollocationInit())
@test prob.u0 == sol[1]


# import Base.copy
import Base.-
import Base./

export
    Vector2D,
    set!

mutable struct Vector2D{T <: AbstractFloat}
    x::T
    y::T

    function Vector2D{T}() where {T <: AbstractFloat}
        new(0.0, 0.0)
    end

    function Vector2D{T}(x::T, y::T) where {T <: AbstractFloat}
        new(x, y)
    end
end

# copy specializations
copy(v::Vector2D) = Vector2D{Float64}(v.x, v.y)

# setters/getters set_point
function set!(v::Vector2D{T}, x::T, y::T) where {T <: AbstractFloat}
    v.x = x
    v.y = y;
end

function set!(v::Vector2D{T}, from::Vector2D{T}) where {T <: AbstractFloat}
    v.x = from.x
    v.y = from.y;
end

function length(x::T, y::T) where {T <: AbstractFloat}
    sqrt(x * x + y * y)
end

function length(v::Vector2D{T}) where {T <: AbstractFloat}
    sqrt(v.x * v.x + v.y * v.y)
end

function length_sq(x::T, y::T) where {T <: AbstractFloat}
    x * x + y * y
end

# Scratch vectors
scv = Vector2D{Float64}()
scv2 = Vector2D{Float64}()

function add!(v1::Vector2D{T}, v2::Vector2D{T}, out::Vector2D{T}) where {T <: AbstractFloat}
    set!(out, v1.x + v2.x, v1.y + v2.y)
end

function sub!(v1::Vector2D{T}, v2::Vector2D{T}, out::Vector2D{T}) where {T <: AbstractFloat}
    set!(out, v1.x - v2.x, v1.y - v2.y)
end

function scale!(v::Vector2D{T}, s::T) where {T <: AbstractFloat}
    set!(v, v.x * s, v.y * s)
end

function scale!(v::Vector2D{T}, s::T, out::Vector2D{T}) where {T <: AbstractFloat}
    set!(out, v.x * s, v.y * s)
end

function div!(v::Vector2D{T}, value::T) where {T <: AbstractFloat}
    set!(v, v.x / value, v.y / value)
end

# distance between two vectors
function distance(v1::Vector2D{T}, v2::Vector2D{T}) where {T <: AbstractFloat}
    sub!(v1, v2, scv)
    length(scv)
end

# returns the angle in radians between this vector and the x axis
function angle(v::Vector2D{T}) where {T <: AbstractFloat}
    atan(v.y, v.x)
end

# Returns  multiplied to a length of 1.
# If the point is 0, it returns (1, 0)
function normalize!(v::Vector2D{T}) where {T <: AbstractFloat}
    len = length(v)
    if length(v) == 0.0
        set!(scv, 1.0, 0.0)
        return scv
    end

    div!(v, len);
end

# Calculates dot product of two points.
function dot(v1::Vector2D{T}, v2::Vector2D{T}) where {T <: AbstractFloat}
    v1.x * v2.x + v1.y * v2.y
end

# Calculates cross product of two points.
function cross(v1::Vector2D{T}, v2::Vector2D{T}) where {T <: AbstractFloat}
    v1.x * v2.y - v1.y * v2.x
end

# returns the angle in radians between two vector directions
function angle_between(v1::Vector2D{T}, v2::Vector2D{T}) where {T <: AbstractFloat}
    set!(scv, v1)
    set!(scv2, v2)

    normalize!(scv) #a2
    normalize!(scv2) #b2

    angle = atan(cross(scv, scv2), dot(scv, scv2))

    if abs(angle) < EPSILON
        return 0.0
    else
        return angle
    end
end

# Calculates perpendicular of v, rotated 90 degrees counter-clockwise -- cross(v, perp(v)) >= 0
function perpindicular(x::T, y::T) where {T <: AbstractFloat}
    Vector2D(-y, x)
end

function ccw_perpindicular!(v::Vector2D{T}) where {T <: AbstractFloat}
    set!(v, -v.y, v.x);
end

# Calculates perpendicular of v, rotated 90 degrees clockwise -- cross(v, rperp(v)) <= 0
function cw_perpindicular!(v::Vector2D{T}) where {T <: AbstractFloat}
    set!(v, v.y, -v.x);
end

function set_direction!(v::Vector2D{T}, degrees::T) where {T <: AbstractFloat}
    set!(v, cos(deg2rad(degrees)), sin(deg2rad(degrees)));
end


# This code is a julia translation of the following project:
# https://bitbucket.org/happyalu/mcep_alpha_calc

# mcepalpha computes appropriate α for a given sampling frequency.
function mcepalpha(fs::Real;
                   start::Float64=0.0,
                   stop::Float64=1.0,
                   step::Float64=0.001,
                   numpoints::Integer=1000)
    α_candidates = start:step:stop
    mel = melscale_vector(fs, numpoints)
    distances = [rms_distance(mel, warping_vector(α, numpoints)) for
                 α in α_candidates]
    return α_candidates[indmin(distances)]
end

function melscale_vector(fs::Real, len::Integer)
    step = (fs / 2.0) / len
    melscalev = 1000.0/log(2)*log(1 + step.*(1:len)./1000.0)
    return melscalev / melscalev[end]
end

function warping_vector(α::Float64, len::Integer)
    step = π / len
    ω = step .* (1:len)
    num = (1-α*α) * sin(ω)
    den = (1+α*α) * cos(ω) - 2*α
    warpfreq = atan(num./den)
    warpfreq[warpfreq .< 0] += π
    return warpfreq / warpfreq[end]
end

rms_distance(v1, v2) = sumabs2(v1 - v2) / length(v1)


function mean(x::Vector{T}) where {T}
    m = sum(x) / length(x)
    return m
end

function std(x::Vector{T}) where {T}
    m = mean(x)
    z = x .- m
    m = sqrt( sum(z.^2) / (length(x) - 1) )
    return m
end

function center(x::Vector)
    m = mean(x)
    y = copy(x)
    y .-= m
    return y
end

function cov(x::Vector{T}) where {T}
    return std(x)
end

function cov(x::Vector{T}, y::Vector{T}) where {T}
    mx = mean(x)
    my = mean(y)
    L = length(x)

    return 1 / (L - 1) * dot(x .- mx, y .- my)
end

function cov(x::Matrix{T}) where {T}
    # covariance in column way
    L = size(x,1)
    return 1/(L-1) * (x' * x)
end


# Utility functions for common algebraic operations.

"""
    rotateTensorToVectorZ(tensor, vector)

Rotate `tensor` with a rotation matrix that aligns the 3rd direction with `vector`, which is
equivalent to change the basis from (i,j,k) to (i′,j′,k′) where k′ ∥ vector.
Reference: [Tensor rotation](https://math.stackexchange.com/questions/2303869/tensor-rotation)
"""
function rotateTensorToVectorZ(tensor::AbstractMatrix{T}, v::AbstractVector{T}) where T
   k = SVector{3, T}(0.0, 0.0, 1.0)
   axis = v × k::SVector{3, T}
   if axis[1] == axis[2] == 0
      return tensor
   else
      normalize!(axis)
      angle = acos(v ⋅ k / hypot(v[1], v[2], v[3]))
      R = getRotationMatrix(axis, angle)
      return R * tensor * R'
   end
end

"""
    getRotationMatrix(axis::AbstractVector, angle) --> SMatrix{3,3}

Create a rotation matrix for rotating a 3D vector around a unit `axis` by an `angle` in
radians.
Reference: [Rotation matrix from axis and angle](https://en.wikipedia.org/wiki/Rotation_matrix#Rotation_matrix_from_axis_and_angle)

# Example

```julia
using LinearAlgebra
v = [-0.5, 1.0, 1.0]
v̂ = normalize(v)
angle = deg2rad(-74)
R = getRotationMatrix(v̂, angle)
```
"""
function getRotationMatrix(v::AbstractVector{<:AbstractFloat}, θ)
   sinθ, cosθ = sincos(eltype(v)(θ))
   tmp = 1 - cosθ
   m =  @SMatrix [
        cosθ+v[1]^2*tmp         v[1]*v[2]*tmp-v[3]*sinθ v[1]*v[3]*tmp+v[2]*sinθ;
        v[1]*v[2]*tmp+v[3]*sinθ cosθ+v[2]^2*tmp         v[2]*v[3]*tmp-v[1]*sinθ;
        v[1]*v[3]*tmp-v[2]*sinθ v[3]*v[2]*tmp+v[1]*sinθ cosθ+v[3]^2*tmp]
end

"""
    getRotationMatrix(e1::Matrix, e2::Matrix) --> SMatrix{3,3}

Obtain the rotation matrix from orthgonal base vectors `e1` to `e2`, such that a vector
``\\mathbf{u}_1`` in `e1` can be expressed as ``\\mathbf{u}_1 = M\\cdot \\mathbf{u}_2``,
where ``M`` is the rotation matrix and ``\\mathbf{u}_2`` is the same vector in `e2`.

# Example

```julia
e1 = [1.0 0.0 0.0; 0.0 1.0 0.0; 0.0 0.0 1.0]
e2 = [0.0 1.0 0.0; 1.0 0.0 0.0; 0.0 0.0 1.0]
R = getRotationMatrix(e1, e2)
```
"""
function getRotationMatrix(e1::AbstractMatrix, e2::AbstractMatrix)
   @views begin
      r11 = e1[:,1] ⋅ e2[:,1]
      r12 = e1[:,1] ⋅ e2[:,2]
      r13 = e1[:,1] ⋅ e2[:,3]
      r21 = e1[:,2] ⋅ e2[:,1]
      r22 = e1[:,2] ⋅ e2[:,2]
      r23 = e1[:,2] ⋅ e2[:,3]
      r31 = e1[:,3] ⋅ e2[:,1]
      r32 = e1[:,3] ⋅ e2[:,2]
      r33 = e1[:,3] ⋅ e2[:,3]
   end
   R = @SMatrix [r11 r12 r13; r21 r22 r23; r31 r32 r33]
end


function delta(n)
# DELTA calculates the sun's declination angle
# y = DELTA(jd) gets the Julian day (jd) and calculate declination of the sun

d=(asin(-sin(23.45/180*pi).*cos(360/365.25*(n+10)/180*pi)));

return d

end


using Iterators, Base.Test

# count
# -----

i = 0
for j = count(0, 2)
	@test j == i*2
	i += 1
	i <= 10 || break
end

# take
# ----

i = 0
for j = take(0:2:8, 10)
	@test j == i*2
	i += 1
end
@test i == 5

i = 0
for j = take(0:2:100, 10)
	@test j == i*2
	i += 1
end
@test i == 10

# drop
# ----

i = 0
for j = drop(0:2:10, 2)
	@test j == (i+2)*2
	i += 1
end
@test i == 4

# cycle
# -----

i = 0
for j = cycle(0:3)
	@test j == i % 4
	i += 1
	i <= 10 || break
end

# repeated
# --------

i = 0
for j = repeated(1, 10)
	@test j == 1
	i += 1
end
@test i == 10
i = 0
for j = repeated(1)
	@test j == 1
	i += 1
	i <= 10 || break
end

# chain
# -----

@test collect(chain(1:2:5, 0.2:0.1:1.6)) == [1:2:5, 0.2:0.1:1.6]

# product
# -------

x1 = 1:2:10
x2 = 1:5
@test collect(product(x1, x2)) == vec([(y1, y2) for y1 in x1, y2 in x2])

# distinct
# --------

x = [5, 2, 2, 1, 2, 1, 1, 2, 4, 2]
@test collect(distinct(x)) == unique(x)

# partition
# ---------

@test collect(partition(take(count(1), 6), 2)) == [(1,2), (3,4), (5,6)]
@test collect(partition(take(count(1), 4), 2, 1)) == [(1,2), (2,3), (3,4)]
@test collect(partition(take(count(1), 8), 2, 3)) == [(1,2), (4,5), (7,8)]

# imap
# ----

function test_imap(expected, input...)
  result = collect(imap(+, input...))
  @test result == expected
end

# Empty arrays
test_imap(
  {},
  []
)

# Simple operation
test_imap(
  {1,2,3},
  [1,2,3]
)

# Multiple arguments
test_imap(
  {5,7,9},
  [1,2,3],
  [4,5,6]
)

# Different-length arguments
test_imap(
  {2,4,6},
  [1,2,3],
  count(1)
)

# groupby
# -------

function test_groupby(input, expected)
  result = collect(groupby(input, x -> x[1]))
  @test result == expected
end

# Empty arrays
test_groupby(
  [],
  {}
)

# Singletons
test_groupby(
  ["xxx"],
  {["xxx"]}
)

# Typical operation
test_groupby(
  ["face", "foo", "bar", "book", "baz"],
  {["face", "foo"], ["bar", "book", "baz"]}
)

# Trailing singletons
test_groupby(
  ["face", "foo", "bar", "book", "baz", "xxx"],
  {["face", "foo"], ["bar", "book", "baz"], ["xxx"]}
)

# Leading singletons
test_groupby(
  ["xxx", "face", "foo", "bar", "book", "baz"],
  {["xxx"], ["face", "foo"], ["bar", "book", "baz"]}
)

# Middle singletons
test_groupby(
  ["face", "foo", "xxx", "bar", "book", "baz"],
  {["face", "foo"], ["xxx"], ["bar", "book", "baz"]}
)

# subsets

@test collect(subsets({})) == {{}}

@test collect(subsets([:a])) == {Symbol[], Symbol[:a]}

@test collect(subsets([:a, :b, :c])) ==
      {Symbol[], Symbol[:a], Symbol[:b], Symbol[:a, :b], Symbol[:c],
       Symbol[:a, :c], Symbol[:b, :c], Symbol[:a, :b, :c]}

# every n'th element
@test collect(everynth([], 10)) == []
@test_throws ArgumentError everynth([], 0)
@test collect(everynth(10:20, 3)) == [12,15,18]
@test collect(everynth(10:20, 1)) == [10:20]


# Newton-like methods for NEPs

using LinearAlgebra
using Printf
using Random

export newton
export resinv
export augnewton
export quasinewton
export newtonqr
export implicitdet

#############################################################################
"""
    λ,v = newton([eltype],nep::NEP;[errmeasure,][tol,][maxit,][λ,][v,][c,][logger,][armijo_factor=1,][armijo_max])

Applies Newton-Raphsons method on the system of
nonlinear equations with `n+1` unknowns:
```math
M(λ)v=0
```
```math
c^Hv-1=0
```
The vector `c` is the
orthogonalization vector.  If `c=0` the current approximation will be used for the orthogonalization. See [`augnewton`](@ref) for other parameters.

# Example
```julia-repl
julia> using LinearAlgebra
julia> nep=nep_gallery("dep0");
julia> λ,v=newton(nep);
julia> minimum(svdvals(compute_Mder(nep,λ)))
1.9997125567227177e-16
```

# References
* Nichtlineare Behandlung von Eigenwertaufgaben, Z. Angew. Math. Mech. 30 (1950) 281-282.
* A. Ruhe, Algorithms for the nonlinear eigenvalue problem, SIAM J. Numer. Anal. 10 (1973) 674-689

"""
    newton(nep::NEP;params...)=newton(ComplexF64,nep;params...)
    function newton(::Type{T},
                    nep::NEP;
                    errmeasure::ErrmeasureType = DefaultErrmeasure(nep),
                    tol::Real=eps(real(T))*100,
                    maxit::Int=10,
                    λ::Number=zero(T),
                    v::Vector=randn(size(nep,1)),
                    c::Vector=v,
                    logger=0,
                    armijo_factor::Real=1,
                    armijo_max::Int=5) where {T<:Number}

        @parse_logger_param!(logger)

        # Ensure types λ and v are of type T
        λ=T(λ)
        v=Vector{T}(v)
        c=Vector{T}(c)

        err=Inf;
        v[:] = v/dot(c,v);

        for k=1:maxit
            err=estimate_error(errmeasure,λ,v)

            push_iteration_info!(logger,k,err=err,λ=λ,v=v,continues=true);
            if (err< tol)
                push_info!(logger,"")
                return (λ,v)
            end

            # Compute NEP matrix and derivative
            M = compute_Mder(nep,λ)
            Md = compute_Mder(nep,λ,1)

            # Create jacobian
            J = [M Md*v; c' 0];
            F = [M*v; c'*v-1];

            # Compute update
            delta=-J\F;  # Hardcoded backslash

            Δv=Vector{T}(delta[1:size(nep,1)]);
            Δλ=T(delta[size(nep,1)+1]);

            (Δλ,Δv,j,scaling)=armijo_rule(nep,errmeasure,err,
                                          λ,v,Δλ,Δv,real(T(armijo_factor)),armijo_max)
            if (j>0)
                push_info!(logger," Armijo scaling=$scaling")
            else
                push_info!(logger,"")
            end

            # Update eigenvalue and eigvec
            v[:] += Δv
            λ = λ+Δλ
        end

        msg="Number of iterations exceeded. maxit=$(maxit)."
        throw(NoConvergenceException(λ,v,err,msg))
    end

    ############################################################################
"""
    λ,v = resinv([eltype],nep::NEP;[errmeasure,][tol,][maxit,][λ,][v,][c,][logger,][armijo_factor=1,][armijo_max,][linsolvecreator])

Applies residual inverse iteration method for nonlinear eigenvalue problems.
The kwarg `linsolvecreator`
is a function which specifies how the linear system is created.
The function calls `compute_rf` for the computation
of the Rayleigh functional.
See [`augnewton`](@ref) for other parameters.

# Example
The example shows how to specify if the method should run in real
or complex mode (or any other `Number` type).
```julia-repl
julia> nep=nep_gallery("qdep0");
julia> λ,v=resinv(nep,λ=-2,v=ones(size(nep,1)))
julia> typeof(λ)
Complex{Float64}
julia> norm(compute_Mlincomb(nep,λ,v))
6.688224435370382e-12
julia> λ,v=resinv(Float64,nep,λ=-2,v=ones(size(nep,1)))
julia> typeof(λ)
Float64
julia> norm(compute_Mlincomb(nep,λ,v))
5.939894690000396e-12
```

# References
*  A. Neumaier, Residual inverse iteration for the nonlinear eigenvalue problem, SIAM J. Numer. Anal. 22 (1985) 914-923

"""
    resinv(nep::NEP;params...)=resinv(ComplexF64,nep;params...)
    function resinv(::Type{T},
                    nep::NEP;
                    errmeasure::ErrmeasureType = DefaultErrmeasure(nep),
                    tol::Real=eps(real(T))*100,
                    maxit::Int=100,
                    λ::Number=zero(T),
                    v::Vector=randn(real(T),size(nep,1)),
                    c::Vector=v,
                    logger=0,
                    inner_solver= @default_compute_rf_inner_solver(nep),
                    linsolvercreator=DefaultLinSolverCreator(),
                    armijo_factor::Real=1,
                    armijo_max::Int=5) where T

        @parse_logger_param!(logger)

        # Ensure types λ and v are of type T
        λ::T=T(λ)
        v=Vector{T}(v)
        c=Vector{T}(c)
        n=size(v,1);

        push_info!(logger,"Precomputing linsolver")
        local linsolver::LinSolver=create_linsolver(linsolvercreator,nep,λ)

        # If c is zero vector we take eigvec approx as left vector in
        # generalized Rayleigh functional
        use_v_as_rf_vector=false;
        if (norm(c)==0)
            use_v_as_rf_vector=true;
        end

        push_info!(logger,2,
                   "use_v_as_rf_vector=$use_v_as_rf_vector");

        σ::T=λ;
        err=Inf;

        for k=1:maxit
            # Normalize
            v[:] = v/norm(v);

            err=estimate_error(errmeasure,λ,v)

            if (use_v_as_rf_vector)
                c[:]=v;
            end

            push_iteration_info!(logger,k,err=err,λ=λ,v=v,continues=true);

            if (err< tol)
                push_info!(logger,"")
                return (λ,v)
            end

            # Compute eigenvalue update
            λ_vec = compute_rf(T, nep, v, inner_solver, y=c, λ=λ, target=σ)
            local λ1::T = closest_to(λ_vec,  λ)
            Δλ=λ1-λ

            # Compute eigenvector update
            Δv = -lin_solve(linsolver,compute_Mlincomb(nep,λ1,reshape(v,n,1))) #M*v);

            (Δλ,Δv,j,scaling)=armijo_rule(nep,errmeasure,err,
                                          λ,v,Δλ,Δv,real(T(armijo_factor)),armijo_max)
            if (j>0)
                push_info!(logger," Armijo scaling=$scaling")
            else
                push_info!(logger,"")
            end

            # Update the eigenpair
            λ+=Δλ
            v[:] += Δv;

        end

        msg="Number of iterations exceeded. maxit=$(maxit)."
        throw(NoConvergenceException(λ,v,err,msg))
    end

    # New augnewton
"""
    augnewton([eltype], nep::NEP; [errmeasure,][tol,][maxit,][λ,][v,][c,][logger,][linsolvercreator,][armijo_factor,][armijo_max])

Run the augmented Newton method. The method is equivalent to `newton()`
in exact arithmetic,  but works only with operations on vectors of
length `n`.

The following keyword arguments are in common for many NEP-solvers:

* `logger` is either a [`Logger`](@ref) object or an `Int`. If it is an `Int`, a `PrintLogger(logger)` will be instantiated. `logger=0` prints nothing, `logger=1` prints more, etc.

* `errmeasure` determines how error is measured. It is either a function handle or an object of the type `Errmeasure`.  If it is a function handle, it should take `(λ,v)` as input and return a real scalar (the error). See [`Errmeasure`](@ref) and [`ErrmeasureType`](@ref) for further description.

* `tol` is a scalar which determines termination. If `errmeasure` is less than `tol` the eigenpair is marked as converged.

* The scalar `λ` and the vector `v` are starting approximations.

* `maxit` determines the maximum number of iterations. The error `NoConvergenceException` is thrown if this is exceeded.

*  The `linsolvecreator` specifies how the linear system should be solved. See [`LinSolver`](@ref) for further information.

* `armijo_factor` specifies if an Armijo rule should be applied, and its value specifies the scaling factor of the step length (per reduction step). The variable `armijo_max` specifies the maximum number of step length reductions.

# Example
This illustrates the equivalence between `newton` and `augnewton`.
```julia-repl
julia> nep=nep_gallery("dep1")
julia> λ1,v1=newton(nep,maxit=20,v=ones(size(nep,1)),λ=0)
julia> λ2,v2=augnewton(nep,maxit=20,v=ones(size(nep,1)),λ=0)
julia> λ1-λ2
0.0 + 0.0im
```
# References
* Nichtlineare Behandlung von Eigenwertaufgaben, Z. Angew. Math. Mech. 30 (1950) 281-282.
* A. Ruhe, Algorithms for the nonlinear eigenvalue problem, SIAM J. Numer. Anal. 10 (1973) 674-689
"""
    augnewton(nep::NEP;kwargs...)=augnewton(ComplexF64,nep::NEP;kwargs...)
    function augnewton(::Type{T},
                       nep::NEP;
                       errmeasure::ErrmeasureType = DefaultErrmeasure(nep),
                       tol::Real=eps(real(T))*100,
                       maxit::Int=30,
                       λ::Number=zero(T),
                       v::Vector=randn(real(T),size(nep,1)),
                       c::Vector=v,
                       logger=0,
                       linsolvercreator=DefaultLinSolverCreator(),
                       armijo_factor::Real=one(real(T)),
                       armijo_max::Int=5) where {T<:Number}

        @parse_logger_param!(logger)

        # Ensure types λ and v are of type T
        λ=T(λ)
        v=Vector{T}(v)
        c=Vector{T}(c)

        err=Inf;
        # If c is zero vector we take eigvec approx as normalization vector
        use_v_as_normalization_vector=false;
        if norm(c) == 0
            use_v_as_normalization_vector=true;
            c[:] = v / norm(v)^2
        end
        v[:] = v/dot(c,v);
        local linsolver::LinSolver
        local tempvec = Vector{T}(undef, size(nep,1))

        push_info!(logger,2,
                   "use_v_as_normalization_vector=$use_v_as_normalization_vector");

        for k=1:maxit
            err=estimate_error(errmeasure,λ,v)
            push_iteration_info!(logger,k,err=err,λ=λ,v=v,continues=true);
            if (err< tol)
                push_info!(logger,"")
                return (λ,v)
            end
            # tempvec =  (M(λ_k)^{-1})*M'(λ_k)*v_k
            # α = 1/(c'*(M(λ_k)^{-1})*M'(λ_k)*v_k);

            z::AbstractVector=compute_Mlincomb(nep,λ,v,[T(1.0)],1)

            linsolver = create_linsolver(linsolvercreator,nep,λ)
            tempvec[:] = Vector{T}(lin_solve(linsolver, z, tol=tol));

            if (use_v_as_normalization_vector)
                c[:] = v /norm(v)^2
            end
            α = T(1)/ dot(c,tempvec);

            Δλ=-α
            Δv=α*tempvec-v;

            (Δλ,Δv,j,scaling)=armijo_rule(nep,errmeasure,err,
                                          λ,v,Δλ,Δv,real(T(armijo_factor)),armijo_max)

            if (j>0)
                push_info!(logger," Armijo scaling=$scaling")
            else
                push_info!(logger,"")
            end

            λ+=Δλ
            v[:]+=Δv

        end

        msg="Number of iterations exceeded. maxit=$(maxit)."
        throw(NoConvergenceException(λ,v,err,msg))
    end

"""
    quasinewton([T=ComplexF64],nep,[errmeasure,][tol,][maxit,][λ,][v][ws][logger][linsolvercreator,][armijo_factor,][armijo_max])

An implementation of the quasi-Newton approach referred to as quasi-Newton 2 in the reference.
The method involves one linear system solve per iteration corresponding with the
matrix ``M(λ)``, where ``λ`` is constant.
The vector `ws` is a representation of the normalization, in the sense that ``c^T=w_s^TM(λ)``,
where all iterates satisfy ``c^Tx_i=1``.
See [`augnewton`](@ref) for other parameters.

# Example
```julia-repl
julia> nep=nep_gallery("pep0")
julia> λ,v=quasinewton(nep,λ=1.0,v=ones(size(nep,1)));
julia> norm(compute_Mlincomb(nep,λ,v))/norm(v)
5.448264607410413e-12
```

# References
* Jarlebring, Koskela, Mele, Disguised and new Quasi-Newton methods for nonlinear eigenvalue problems, Numer. Algorithms, 79:311-335, 2018. [preprint](https://arxiv.org/abs/1702.08492)
"""
    quasinewton(nep::NEP;params...)=quasinewton(ComplexF64,nep;params...)
    function quasinewton(::Type{T},
                         nep::NEP;
                         errmeasure = DefaultErrmeasure(nep),
                         tol::Real=eps(real(T))*100,
                         maxit::Int=100,
                         λ::Number=zero(T),
                         v::Vector=randn(real(T),size(nep,1)),
                         ws::Vector=v,
                         logger=0,
                         linsolvercreator=DefaultLinSolverCreator(),
                         armijo_factor::Real=1,
                         armijo_max::Int=5) where T

        @parse_logger_param!(logger)

        # Ensure types λ and v are of type T
        λ=T(λ)
        v=Vector{T}(v)
        ws=Vector{T}(ws) # Left vector such that c'=w'M(λ) where c normalization

        n = size(nep,1)
        u = zeros(T,n)
        w = zeros(T,n)

        err=Inf;

        local linsolver::LinSolver;
        push_info!(logger,"Precomputing linsolver")
        linsolver = create_linsolver(linsolvercreator,nep,λ)

        for k=1:maxit
            err=estimate_error(errmeasure,λ,v)
            push_iteration_info!(logger,k,err=err,λ=λ,v=v,continues=true);
            if (err< tol)
                push_info!(logger,"")
                return (λ,v)
            end

            # Compute u=M(λ)v and w=M'(λ)v
            u[:] = compute_Mlincomb(nep,λ,v,[T(1)],0);
            w[:] = compute_Mlincomb(nep,λ,v,[T(1)],1);

            # Intermediate quantities
            Δλ=-dot(ws,u)/dot(ws,w);
            z=Δλ*w+u;
            # Throws an error if lin_solve returns incorrect type.
            local Δv::Vector{T}= -lin_solve(linsolver, z, tol=tol)

            normΔv=norm(Δv);
            push_info!(logger,2," norm(Δv)=$normΔv",continues=true)

            (Δλ,Δv,j,scaling)=armijo_rule(nep,errmeasure,err,
                                          λ,v,Δλ,Δv,real(T(armijo_factor)),armijo_max)

            if (j>0)
                push_info!(logger," Armijo scaling=$scaling")
            else
                push_info!(logger,"");
            end

            # Update eigenpair
            λ += Δλ
            v[:] += Δv; # eigvec update

        end

        msg="Number of iterations exceeded. maxit=$(maxit)."
        throw(NoConvergenceException(λ,v,err,msg))
    end

"""
    λ,v = newtonqr([eltype],nep::NEP;[errmeasure,][tol,][maxit,][λ,][v,][c,][logger])

This function implements the Newton-QR method as formulated in the reference. The method ivolves the computation of a rank-revealing QR factorization
of ``M(λ)``, with the idea that on convergence the the last diagonal element ``R[n,n]`` of the upper-triangular matrix ``R`` becomes zero as a result of ``M(λ)``
becoming singular. Since the computation of a QR factorization is expensive, it is advisable to use this method for problems of small size or problems with
a certain structure that makes the QR computation less expensive.
See [`augnewton`](@ref) for other parameters.

# Example
```julia-repl
julia> nep=nep_gallery("pep0")
julia> λ,v=newtonqr(nep,v=ones(size(nep,1)));
julia> norm(compute_Mlincomb(nep,λ,v))/norm(v)
8.440206093655014e-15
```

# References
* Kublanovskaya, V. N., (1970).  On an approach to the solution of the generalized latent value problem for λ-matrices, SIAM J. Numer. Anal. 7, 532–537
* Güttel, S., & Tisseur, F. (2017). The nonlinear eigenvalue problem. Acta Numerica, 26, 1-94. doi:10.1017/S0962492917000034
"""
    newtonqr(nep::NEP;params...)=newtonqr(ComplexF64,nep;params...)
    function newtonqr(::Type{T},
                      nep::NEP;
                      errmeasure::ErrmeasureType = DefaultErrmeasure(nep),
                      tol::Real=eps(real(T))*100,
                      maxit::Int=100,
                      λ::Number=zero(T),
                      v::Vector=randn(real(T),size(nep,1)),
                      c::Vector=v,
                      logger=0) where T

        @parse_logger_param!(logger)

        # Ensure types λ and v are of type T
        λ=T(λ)
        v=Vector{T}(v)
        c=Vector{T}(c)

        n = size(nep,1);
        local err
        local w

        en = zeros(n);
        en[n] = 1;

        for k=1:maxit
            A = compute_Mder(nep,λ);
            Q,R,PI = qr(A, Val(true)) #QR factorization with pivoting.
            Q = Matrix(Q)

            P = Matrix{T}(I, n, n)[:,PI] #The permutation matrix corresponding to the pivoted QR.

            p = R[1:n-1,1:n-1]\R[1:n-1,n];
            v = P*[-p;T(1)];#Right eigenvector
            w = Q*en;#Left eigenvector

            #err = abs(R[n,n])/norm(compute_Mder(nep,λ),2); # Frobenius norm
            err=estimate_error(errmeasure,λ,v);

            push_iteration_info!(logger,k,err=err,λ=λ,v=v);
            if(err < tol)
                return λ,v,w;
            end

            d = dot(Q[:,n],compute_Mlincomb(nep,λ,reshape(v,n,1),[T(1)],1));
            #d = dot(Q[:,n],compute_Mder(nep,λ,1)*P*[-p;T(1.0)]);
            λ = λ - R[n,n]/d;
        end

        msg="Number of iterations exceeded. maxit=$(maxit)."
        throw(NoConvergenceException(λ,v,err,msg))
    end

"""
    λ,v = implicitdet([eltype],nep::NEP;[errmeasure,][tol,][maxit,][λ,][v,][c,][logger])

This function implements the Implicit determinant method as formulated Algorithm 4.3 in the reference. The method applies Newton-Raphson to the equation
``det(M(λ))/det(G(λ)) = 0``, where ``G(λ)`` is a saddle point matrix with ``M(λ)``
in the (1,1) block. The (2,1) and (1,2) blocks of ``G(λ)`` are set to
``c^H`` and ``c`` respectively. Note that ``G(λ) `` can be non-singular even when ``M(λ) ``
is singular. See reference for more information.
See [`augnewton`](@ref) for other parameters.

# Example
```julia-repl
julia> nep=nep_gallery("pep0")
julia> λ,v=implicitdet(nep,v=ones(size(nep,1)));
julia> norm(compute_Mlincomb(nep,λ,v))/norm(v)
2.566371972986362e-14
```

# References
* Spence, A., & Poulton, C. (2005). Photonic band structure calculations using nonlinear eigenvalue techniques, J. Comput. Phys., 204 (2005), pp. 65–8
* Güttel, S., & Tisseur, F. (2017). The nonlinear eigenvalue problem. Acta Numerica, 26, 1-94. doi:10.1017/S0962492917000034
"""
    implicitdet(nep::NEP;params...)=implicitdet(ComplexF64,nep;params...)
    function implicitdet(::Type{T},
                         nep::NEP;
                         errmeasure::ErrmeasureType = DefaultErrmeasure(nep),
                         tol=eps(real(T))*100,
                         maxit=100,
                         λ=zero(T),
                         v=randn(real(T),size(nep,1)),
                         c=v,
                         logger=0) where T

        @parse_logger_param!(logger)

        n = size(nep,1);
        v = Vector{T}(vcat(v,one(T)))
        vp = zeros(T,n+1)
        c = Vector{T}(c);
        b = c;
        P = Matrix{T}(I, n+1, n+1)

        local err

        for k=1:maxit

            A = compute_Mder(nep,λ);
            AA = [A b;c' 0];#The matrix G(λ)

            F = lu(AA);

            v[:] = F\([zeros(T,n);T(1)]);
            vp[:] = F\([-1*compute_Mder(nep,λ,1)*v[1:n];0]);

            #err = estimate_error(errmeasure,λ,v[1:n]);
            err = abs(v[n+1])/norm(compute_Mder(nep,λ),2); # Frobenius norm based error
            push_iteration_info!(logger,k,err=err,λ=λ,v=v);
            if(err < tol)
                return λ,v[1:n];
            end

            λ = λ - v[n+1]/vp[n+1];#Newton update for the equation det(M(λ))/det(G(λ)) = 0
        end

        msg="Number of iterations exceeded. maxit=$(maxit)."
        throw(NoConvergenceException(λ,v,NaN,msg))
    end

    # Armijo rule implementation
    function armijo_rule(nep,errmeasure,err0,λ,v,Δλ,Δv,armijo_factor,armijo_max)
        j=0
        if (armijo_factor<1)
            # take smaller and smaller steps until errmeasure is decreasing
            while (estimate_error(errmeasure,λ+Δλ,v+Δv)>err0 && j<armijo_max)
                j=j+1;
                Δv=Δv*armijo_factor;
                Δλ=Δλ*armijo_factor;
            end
        end
        return  (Δλ,Δv,j,armijo_factor^j)
    end


#An Extended Kalman Filter (EKF) example taken from Markus Gesmann's
#Mages' blog titled "Extended Kalman filter example in R" [1] which in turn
#was taken from a blog post titled "Fun with (Extended Kalman) Filters"
#by Dominic Steinitz [2].
#The web addresses are:
#[1] http://www.magesblog.com/2015/01/extended-kalman-filter-example-in-r.html
#[2] https://idontgetoutmuch.wordpress.com/2014/09/09/fun-with-extended-kalman-filters-4/

#Here we import the required modules
using StateSpace
using Distributions
using Gadfly
using DataFrames
using Colors

################################################################################
#Section: Generate noisy Observations
#-------------------------------------------------------------------------------

#Here we set the parameters with their true values
r = 0.2 #r is the growth rate
k = 100.0 #k is the carrying capacity
p0 = 0.1 * k # p0 is the initial population
Δt = 0.1 # Δt is the change in time.

#Define the logistic growth function to set the observations
function logisticGrowth(r, p, k, t)
    k * p * exp(r*t) / (k + p * (exp(r*t) - 1))
end
logisticGrowth(state) = logisticGrowth(state[1], state[2], k, Δt)

#Set the measurement noise variance
measurement_noise_variance = 25.0

#create the noisy observations (zero mean Gaussian noise)
numObs = 100
true_values = Vector{Float64}(numObs)
population_measurements = Vector{Float64}(numObs)
for i in 1:numObs
    true_values[i] = logisticGrowth(r, p0, k, i*Δt)
    population_measurements[i] = true_values[i] + randn() * sqrt(measurement_noise_variance)
end

#Since we're going to assume that our state consists of a growth rate, r,
#as well as a population rate, p, we need measurements of the growth rate.
#The problem is that we don't actually observe the growth rate directly.
#So we will set these measurements to zero.
growth_rate_measurements = zeros(numObs)

#Then we put these measurements together
measurements = [growth_rate_measurements population_measurements]'
#End Section: Generate noisy Observations
################################################################################

################################################################################
#Section: Describe Extended Kalman Filter parameters
#-------------------------------------------------------------------------------
#The state consists of the growth rate, r, and the population number, p. We'll
#assume that the growth rate is constant throughout time. And we'll assume that
#the population update follows the logistic growth pattern. So let's create that
#process function
function process_fcn(state)
    predict_growth_rate = state[1]
    predict_population = logisticGrowth(state)
    new_state = [predict_growth_rate, predict_population]
    return new_state
end
#Here we assume that there is no evolution noise so well create a zero matrix
process_noise_mat = diagm([0.001, 0.001])

#Now we need to describe the observation model. We'll assume that we don't
#observe the growth rate but we do observe the population. So the observation
#function is:
function observation_fcn(state)
    growth_rate_observation = 0.0
    population_observation = 1.0 * state[2]
    observation = [growth_rate_observation, population_observation]
    return observation
end

#Now we need to set the observation noise. We already set the measurement noise
#for the population earlier. We'lll arbitrarily make the variance for the
#growth rate (it doesn't matter because we don't explicity observe it):
observation_noise_mat = diagm([1.0, measurement_noise_variance])

#Create instance of our EKF model
nonLinSSM = NonlinearGaussianSSM(process_fcn, process_noise_mat, observation_fcn, observation_noise_mat)
#End Section: Describe Extended Kalman Filter parameters
################################################################################

################################################################################
#Section: Set initial guess of the state
#-------------------------------------------------------------------------------
initial_guess = MvNormal([0.5, 10], diagm([1.0,20.0]))
################################################################################

################################################################################
#Section: Execute the Extended Kalman Filter
#-------------------------------------------------------------------------------
filtered_state = filter(nonLinSSM, measurements, initial_guess)
#End Section: Execute  the Extended Kalman Filter
################################################################################

################################################################################
#Section: Plot Filtered results
#-------------------------------------------------------------------------------
#Here we are plotting the filtered results with Gadfly. See the Gadfly
#documentation for information about how plotting works if you are unfamiliar.
#Website: http://gadflyjl.org/

x_data = 1:numObs
population_array = Vector{Float64}(numObs+1)
confidence_array = Vector{Float64}(numObs+1)
population_array[1] = initial_guess.μ[2]
confidence_array[1] = 2*sqrt(initial_guess.Σ.mat[2,2])
for i in x_data
    current_state = filtered_state.state[i]
    population_array[i+1] = current_state.μ[2]
    confidence_array[i+1] = 2*sqrt(current_state.Σ.mat[2,2])
end
df_fs = DataFrame(
    x = [0;x_data],
    y = population_array,
    ymin = population_array - confidence_array,
    ymax = population_array + confidence_array,
    f = "Filtered values"
    )

n = 3
getColors = distinguishable_colors(n, Color[LCHab(70, 60, 240)],
                                   transform=c -> deuteranopic(c, 0.5),
                                   lchoices=Float64[65, 70, 75, 80],
                                   cchoices=Float64[0, 50, 60, 70],
                                   hchoices=linspace(0, 330, 24))
population_state_plot = plot(
    layer(x=0:numObs, y=[p0;measurements[2,:]'], Geom.point, Theme(default_color=getColors[2])),
    layer(x=0:numObs, y=[p0;true_values], Geom.line, Theme(default_color=getColors[3])),
    layer(df_fs, x=:x, y=:y, ymin=:ymin, ymax=:ymax, Geom.line, Geom.ribbon),
    Guide.xlabel("Measurement Number"), Guide.ylabel("Population"),
    Guide.manual_color_key("Colour Key",["Filtered Estimate", "Measurements","True Value "],[getColors[1],getColors[2],getColors[3]]),
    Guide.title("Extended Kalman Filter Example")
    )
display(population_state_plot)
#End Section: Plot Filtered results
################################################################################

################################################################################
#Section: Plot Filtered results
#-------------------------------------------------------------------------------
#Here we are plotting the filtered results with Gadfly. See the Gadfly
#documentation for information about how plotting works if you are unfamiliar.
#Website: http://gadflyjl.org/

x_data = 1:numObs
growth_rate_array = Vector{Float64}(numObs+1)
confidence_array = Vector{Float64}(numObs+1)
growth_rate_array[1] = initial_guess.μ[1]
confidence_array[1] = initial_guess.Σ.mat[1,1]
for i in x_data
    current_state = filtered_state.state[i]
    growth_rate_array[i+1] = current_state.μ[1]
    confidence_array[i+1] = 2*sqrt(current_state.Σ.mat[1,1])
end
df_fs = DataFrame(
    x = [0;x_data],
    y = growth_rate_array,
    ymin = growth_rate_array - confidence_array,
    ymax = growth_rate_array + confidence_array,
    f = "Filtered values"
    )

growth_rate_state_plot = plot(
    layer(x=0:numObs, y=ones(numObs+1)*r, Geom.line, Theme(default_color=getColors[3])),
    layer(df_fs, x=:x, y=:y, ymin=:ymin, ymax=:ymax, Geom.line, Geom.ribbon),
    Guide.xlabel("Measurement Number"), Guide.ylabel("Growth Rate"),
    Guide.manual_color_key("Colour Key",["Filtered Estimate", "Measurements","True Value "],[getColors[1],getColors[2],getColors[3]]),
    Guide.title("Extended Kalman Filter Example")
    )
display(growth_rate_state_plot)
#End Section: Plot Filtered results
################################################################################


immutable Skellam <: DiscreteUnivariateDistribution
    μ1::Float64
    μ2::Float64

    function Skellam(μ1::Float64, μ2::Float64)
        μ1 > 0.0 && μ2 > 0.0 || error("μ1 and μ2 must be positive.")
        new(μ1, μ2)
    end

    @compat Skellam(μ1::Real, μ2::Real) = Skellam(Float64(μ1), Float64(μ2))

    Skellam(μ::Real) = Skellam(μ, μ)

    Skellam() = new(1.0, 1.0)
end

@distr_support Skellam -Inf Inf

### Parameters

params(d::Skellam) = (d.μ1, d.μ2)

### Statistics

mean(d::Skellam) = d.μ1 - d.μ2

var(d::Skellam) = d.μ1 + d.μ2

skewness(d::Skellam) = mean(d) / (var(d)^1.5)

kurtosis(d::Skellam) = 1.0 / var(d)

### Evaluation

function logpdf(d::Skellam, x::Int)
    μ1, μ2 = params(d)
    - (μ1 + μ2) + (x / 2.0) * log(μ1 / μ2) + log(besseli(x, 2.0 * sqrt(μ1) * sqrt(μ2)))
end

pdf(d::Skellam, x::Int) = exp(logpdf(d, x))

function mgf(d::Skellam, t::Real)
    μ1, μ2 = params(d)
    exp(-(μ1 + μ2) + μ1 * exp(t) + μ2 * exp(-t))
end

function cf(d::Skellam, t::Real)
    μ1, μ2 = params(d)
    exp(-(μ1 + μ2) + μ1 * cis(t) + μ2 * cis(-t))
end

### Sampling

rand(d::Skellam) = rand(Poisson(d.μ1)) - rand(Poisson(d.μ2))


function constantdiffusionproblem(stepper; nx=128, Lx=2π, kappa=1e-2, nsteps=1000, dev=CPU())
   τ = 1/kappa  # time-scale for diffusive decay
  dt = 1e-9 * τ # dynamics are resolved

  prob = Problem(nx=nx, Lx=Lx, kappa=kappa, dt=dt, stepper=stepper, dev=dev)
  g = prob.grid

  # a gaussian initial condition c(x, t=0)
  c0ampl, σ = 0.01, 0.2
  c0func(x) = @. c0ampl*exp(-x^2/(2σ^2))
  c0 = c0func.(g.x)

  # analytic solution for for 1D heat equation with constant κ
  tfinal = nsteps*dt
  σt = sqrt(2*kappa*tfinal + σ^2)
  cfinal = @. c0ampl*σ/σt * exp(-g.x^2/(2*σt^2))

  set_c!(prob, c0)
  tcomp = @elapsed stepforward!(prob, nsteps)
  updatevars!(prob)

  prob, c0, cfinal, nsteps, tcomp
end

function varyingdiffusionproblem(stepper; nx=128, Lx=2π, kappa=1e-2, nsteps=1000, dev=CPU())
   τ = 1/kappa  # time-scale for diffusive decay
  dt = 1e-9 * τ # dynamics are resolved

  kappa = kappa*ones(nx) # this is actually a constant diffusion but defining it
                         # as an array makes stepforward! call function calcN!
                         # instead of just the linear coefficients L*sol

  prob = Problem(nx=nx, Lx=Lx, kappa=kappa, dt=dt, stepper=stepper, dev=dev)
  g = prob.grid

  # a gaussian initial condition c(x, t=0)
  c0ampl, σ = 0.01, 0.2
  c0func(x) = @. c0ampl*exp(-x^2/(2σ^2))
  c0 = c0func.(g.x)

  # analytic solution for for 1D heat equation with constant κ
  tfinal = nsteps*dt
  σt = sqrt(2*kappa[1]*tfinal + σ^2)
  cfinal = @. c0ampl*σ/σt * exp(-g.x^2/(2*σt^2))

  set_c!(prob, c0)
  tcomp = @elapsed stepforward!(prob, nsteps)
  updatevars!(prob)

  prob, c0, cfinal, nsteps, tcomp
end

function constantdiffusiontest(stepper, dev::Device=CPU(); kwargs...)
  prob, c0, c1, nsteps, tcomp = constantdiffusionproblem(stepper; kwargs...)
  normmsg = "$stepper: relative error ="
  @printf("% 40s %.2e (%.3f s)\n", normmsg, norm(c1-prob.vars.c)/norm(c1), tcomp)
  isapprox(c1, prob.vars.c, rtol=nsteps*rtol_timesteppers)
end

function varyingdiffusiontest(stepper, dev::Device=CPU(); kwargs...)
  prob, c0, c1, nsteps, tcomp = varyingdiffusionproblem(stepper; kwargs...)
  normmsg = "$stepper: relative error ="
  @printf("% 40s %.2e (%.3f s)\n", normmsg, norm(c1-prob.vars.c)/norm(c1), tcomp)
  isapprox(c1, prob.vars.c, rtol=nsteps*rtol_timesteppers)
end


module Skew

mvnrnd(n,mu,C) = sqrtm(C)*randn(size(C,1),n) .+ mu

# Generate n random samples from SkewNormal(Omega,alpha).
#    Omega = d-by-d "correlation" matrix (a covariance matrix with ones on the diagonal).
#    alpha = length d vector of skew parameters.
# (This implements Proposition 1 of Azzalini & Capitanio (1999).)
function skewrndNd(n,Omega,alpha)
    delta = (Omega*alpha)/sqrt(1+dot(alpha,Omega*alpha))
    x = mvnrnd(n,0,[1 delta'; delta Omega])
    return sign(x[1,:]).*x[2:end,:]
end

# Generate n random samples from SkewNormal(xi,C,alpha).
#    xi = length d vector of location parameters (it is not necessarily the mean of the resulting samples).
#    C = d-by-d posdef matrix (it is not necessarily the covariance matrix of the resulting samples).
#    alpha = length d vector of skew parameters.
# (This implements the location-scale extension in Section 5.1 of Azzalini & Capitanio (1999).)
function skewrndNd(n,xi,C,alpha)
    s = sqrt(diag(C))
    Omega = (1.0./s) .* C .* (1.0./s)'
    return xi .+ s.*skewrndNd(n,Omega,alpha)
end

# Compute the mean of Z ~ SkewNormal(Omega,alpha).
# (See Equation (4) of Azzalini & Capitanio (1999).)
mu(Omega,alpha) = (delta = (Omega*alpha)/sqrt(1+dot(alpha,Omega*alpha)); sqrt(2/pi)*delta)

# Compute the covariance matrix of Z ~ SkewNormal(Omega,alpha).
# (See Equation (4) of Azzalini & Capitanio (1999).)
Cov(Omega,alpha) = (m = mu(Omega,alpha); Omega-m*m')

# Generate n random samples from SkewNormal(Omega,alpha) and transform them so that each entry has zero mean and unit variance.
skewrndNormalized(n,Omega,alpha) = (skewrndNd(n,Omega,alpha) .- mu(Omega,alpha))./sqrt(diag(Cov(Omega,alpha)))

end


### Edge and Gradient Related Image Operations ###

# Edge/gradient filters

function sobel()
    f = [ -1.0  0.0  1.0
          -2.0  0.0  2.0
          -1.0  0.0  1.0 ]
    return f', f
end

function prewitt()
    f = [ -1.0  0.0  1.0
          -1.0  0.0  1.0
          -1.0  0.0  1.0 ]
    return f', f
end

# Consistent Gradient Operators
# Ando Shigeru
# IEEE Trans. Pat. Anal. Mach. Int., vol. 22 no 3, March 2000
#
# TODO: These coefficients were taken from the paper It would be nice
#       to resolve the optimization problem and use higher precision
#       versions, which might allow better separable approximations of
#       ando4 and ando5.

function ando3()
    f = [ -0.112737  0.0  0.112737
          -0.274526  0.0  0.274526
          -0.112737  0.0  0.112737 ]
    return f', f
end

# Below, the ando4() and ando5() functions return filters with
# the published filter values.  The ando4_sep() and ando5_sep()
# functions return separable approximations to the corresponding
# filters, estimated using the projection of the actual values on the
# eigenvector corresponding to the largest eigenvalue of the SVD of
# the original filter.

function ando4()
    f = [ -0.022116 -0.025526  0.025526  0.022116
          -0.098381 -0.112984  0.112984  0.098381
          -0.098381 -0.112984  0.112984  0.098381
          -0.022116 -0.025526  0.025526  0.022116 ]
    return f', f
end

function ando4_sep()
    f = [-0.022175974729759376 -0.025473821998749126 0.025473821998749126 0.022175974729759376
         -0.09836750569692418  -0.11299599504060115  0.11299599504060115  0.09836750569692418
         -0.09836750569692418  -0.11299599504060115  0.11299599504060115  0.09836750569692418
         -0.022175974729759376 -0.025473821998749126 0.025473821998749126 0.022175974729759376]
    return f', f
end

function ando5()
    f = [ -0.003776 -0.010199  0.0  0.010199  0.003776
          -0.026786 -0.070844  0.0  0.070844  0.026786
          -0.046548 -0.122572  0.0  0.122572  0.046548
          -0.026786 -0.070844  0.0  0.070844  0.026786
          -0.003776 -0.010199  0.0  0.010199  0.003776 ]
    return f', f
end

function ando5_sep()
    f = [-0.0038543900766123762 -0.0101692999709622   0.0  0.0101692999709622   0.0038543900766123762
         -0.026843218687756566  -0.07082229291692607  0.0  0.07082229291692607  0.026843218687756566
         -0.046468878396946627  -0.12260200818803602  0.0  0.12260200818803602  0.046468878396946627
         -0.026843218687756566  -0.07082229291692607  0.0  0.07082229291692607  0.026843218687756566
         -0.0038543900766123762 -0.0101692999709622   0.0  0.0101692999709622   0.0038543900766123762]
    return f', f
end

# Image gradients in the X and Y direction
function imgradients(img::AbstractArray, method::String="ando3", border::String="replicate")
    sx,sy = spatialorder(img)[1] == "x" ? (1,2) : (2,1)
    s = (method == "sobel"     ? sobel() :
         method == "prewitt"   ? prewitt() :
         method == "ando3"     ? ando3() :
         method == "ando4"     ? ando4() :
         method == "ando5"     ? ando5() :
         method == "ando4_sep" ? ando4_sep() :
         method == "ando5_sep" ? ando5_sep() :
         error("Unknown gradient method: $method"))

    grad_x = imfilter(img, s[sx], border)
    grad_y = imfilter(img, s[sy], border)

    return grad_x, grad_y
end

# Magnitude of gradient, calculated from X and Y image gradients
magnitude(grad_x::AbstractArray, grad_y::AbstractArray) = hypot(grad_x, grad_y)

# Phase (angle of steepest gradient ascent), calculated from X and Y gradient images
function phase{T}(grad_x::AbstractArray{T}, grad_y::AbstractArray{T})
    EPS = sqrt(eps(eltype(T)))
    # Set phase to zero when both gradients are close to zero
    reshape([atan2(-grad_y[i], grad_x[i]) * ((abs(grad_x[i]) > EPS) | (abs(grad_y[i]) > EPS))
             for i=1:length(grad_x)], size(grad_x))
end

function phase(grad_x::AbstractImageDirect, grad_y::AbstractImageDirect)
    img = copy(grad_x, phase(data(grad_x), data(grad_y)))
    img["limits"] = (-float(pi),float(pi))
    img
end

# Orientation of the strongest edge at a point, calculated from X and Y gradient images
# Note that this is perpendicular to the phase at that point, except where
# both gradients are close to zero.

function orientation{T}(grad_x::AbstractArray{T}, grad_y::AbstractArray{T})
    EPS = sqrt(eps(eltype(T)))
    # Set orientation to zero when both gradients are close to zero
    # (grad_y[i] should probably be negated here, but isn't for consistency with earlier releases)
    reshape([atan2(grad_x[i], grad_y[i]) * ((abs(grad_x[i]) > EPS) | (abs(grad_y[i]) > EPS))
             for i=1:length(grad_x)], size(grad_x))
end

function orientation(grad_x::AbstractImageDirect, grad_y::AbstractImageDirect)
    img = copy(grad_x, orientation(data(grad_x), data(grad_y)))
    img["limits"] = (-float(pi),float(pi))
    img
end

# Return both the magnitude and phase in one call
magnitude_phase(grad_x::AbstractArray, grad_y::AbstractArray) = (magnitude(grad_x,grad_y), phase(grad_x,grad_y))

# Return the magnituded and phase of the gradients in an image
function magnitude_phase(img::AbstractArray, method::String="ando3", border::String="replicate")
    grad_x, grad_y = imgradients(img, method, border)
    return magnitude_phase(grad_x, grad_y)
end

# Return the x-y gradients and magnitude and phase of gradients in an image
function imedge(img::AbstractArray, method::String="ando3", border::String="replicate")
    grad_x, grad_y = imgradients(img, method, border)
    mag = magnitude(grad_x, grad_y)
    orient = orientation(grad_x, grad_y)
    return (grad_x, grad_y, mag, orient)
end

# Thin edges
thin_edges{T}(img::AbstractArray{T,2}, gradientangles::AbstractArray, border::String="replicate") =
    thin_edges_nonmaxsup(img, gradientangles, border)
thin_edges_subpix{T}(img::AbstractArray{T,2}, gradientangles::AbstractArray, border::String="replicate") =
    thin_edges_nonmaxsup_subpix(img, gradientangles, border)

# Code below is related to non-maximal suppression, and was ported to Julia from
# http://www.csse.uwa.edu.au/~pk/research/matlabfns/Spatial/nonmaxsup.m
# (Please conserve the original copyright below.)

# NONMAXSUP - Non-maxima suppression
#
# Usage:
#          (im,location) = nonmaxsup(img, gradientangles, radius);
#
# Function for performing non-maxima suppression on an image using
# gradient angles.  Gradient angles are assumed to be in radians.
#
# Input:
#   img - image to be non-maxima suppressed.
#
#   gradientangles - image containing gradient angles around each pixel in radians
#                    (-pi,pi)
#
#   radius  - Distance in pixel units to be looked at on each side of each
#             pixel when determining whether it is a local maxima or not.
#             This value cannot be less than 1.
#             (Suggested value about 1.2 - 1.5)
#
# Returns:
#   im        - Non maximally suppressed image.
#   location  - `Graphics.Point` image holding subpixel locations of edge
#               points.
#
# Notes:
#
# This function uses bilinear interpolation to estimate
# intensity values at ideal, real-valued pixel locations on each side of
# pixels to determine if they are local maxima.

# Copyright (c) 1996-2013 Peter Kovesi
# Centre for Exploration Targeting
# The University of Western Australia
#
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
# in the Software without restriction, subject to the following conditions:
#
# The above copyright notice and this permission notice shall be included in all
# copies or substantial portions of the Software.
#
# The Software is provided "as is", without warranty of any kind.

# December  1996 - Original version
# September 2004 - Subpixel localization added
# August    2005 - Made Octave compatible
# October   2013 - Final thinning applied to binary image for Octave
#                  compatbility (Thanks to Chris Pudney)
# June      2014 - Ported (and modified significantly) to Julia (Kevin Squire)

import Base.Graphics.Point

if !applicable(zero, Point)
    import Base.zero
    zero(Point) = Point(0.0,0.0)
end

# Used to encode the sign, integral, and fractional components of
# an offset from a coordinate
immutable CoordOffset
    s::Int      # sign
    i::Int      # integer part
    f::Float64  # fractional part
end

CoordOffset(x::Float64) = ((frac,i) = modf(x); CoordOffset(sign(frac), int(i), abs(frac)))
(-)(off::CoordOffset) = CoordOffset(-off.s,-off.i, off.f)
(*)(x::Number, off::CoordOffset) = x*(off.i + off.s*off.f)
(*)(off::CoordOffset, x::Number) = x*(off.i + off.s*off.f)
(+)(x::Number, off::CoordOffset) = x + off.i + off.s*off.f
(+)(off::CoordOffset, x::Number) = x + off.i + off.s*off.f

# Precalculate x and y offsets relative to centre pixel for each orientation angle
function _calc_discrete_offsets(θ, radius, transposed)

    θ_count = iround(2π/θ)
    θ = 2π/θ_count
    angles = (0:θ_count)*θ

    # x and y offset of points at specified radius and angles
    # from each reference position.

    if transposed
        # θ′ = -π/2 - θ
        xoffs = [CoordOffset(-x) for x in  radius*sin(angles)]
        yoffs = [CoordOffset( y) for y in  radius*cos(angles)]
    else
        xoffs = [CoordOffset( x) for x in  radius*cos(angles)]
        yoffs = [CoordOffset(-y) for y in  radius*sin(angles)]
    end

    return θ, xoffs, yoffs
end

_discretize_angle(angle::FloatingPoint, invθ) =
    angle < 0 ? iround((angle + 2π)*invθ)+1 : iround(angle*invθ)+1

# Interpolate the value of an offset from a particular pixel
#
# Returns (interpolated value, min_value of adjacent pixels in direction of offset)
#
# The second value is made available to eliminate double edges; if the value at
# (x,y) is less than the value or values adjacent to it in the direction of the
# gradient (xoff,yoff), then it is not a local maximum

function _interp_offset(img::AbstractArray, x::Integer, y::Integer, xoff::CoordOffset, yoff::CoordOffset, Ix, Iy, pad)
    fx = Ix[x + xoff.i + pad]
    fy = Iy[y + yoff.i + pad]
    cx = Ix[x + xoff.i + xoff.s + pad]
    cy = Iy[y + yoff.i + yoff.s + pad]

    tl = img[fy,fx]    # Value at bottom left integer pixel location.
    tr = img[fy,cx]    # bottom right
    bl = img[cy,fx]    # top left
    br = img[cy,cx]    # top right

    upperavg = tl + xoff.f * (tr - tl)  # Now use bilinear interpolation to
    loweravg = bl + xoff.f * (br - bl)  # estimate value at x,y

    min_adjacent = (fx == x) & (fy == y) ? min(tr,bl) : tl

    return (upperavg + yoff.f * (loweravg - upperavg), min_adjacent)
end

# Core edge thinning algorithm using nonmaximal suppression
function thin_edges_nonmaxsup_core!{T}(out::AbstractArray{T,2}, location::AbstractArray{Point,2},
                                       img::AbstractArray{T,2}, gradientangles, radius, border, theta)
    calc_subpixel = !isempty(location)

    # Error checking
    size(img) == size(gradientangles) == size(out) || error("image, gradient angle, and output image must all be the same size")
    calc_subpixel && size(location) != size(img) && error("subpixel location has a different size than the input image")
    radius < 1.0 && error("radius must be >= 1")

    # Precalculate x and y offsets relative to centre pixel for each orientation angle
    transposed = spatialorder(img)[1] == "x"
    θ, xoffs, yoffs = _calc_discrete_offsets(theta, radius, transposed)
    iθ = 1/θ

    # Indexes to use for border handling
    pad = iceil(radius)
    Ix = Images.padindexes(img, 2, pad, pad, border)
    Iy = Images.padindexes(img, 1, pad, pad, border)

    # Now run through the image interpolating grey values on each side
    # of the centre pixel to be used for the non-maximal suppression.

    (height,width) = size(img)

    for x = 1:width, y = 1:height
        (c = img[y,x]) == 0 && continue  # For thresholded images

        or = _discretize_angle(gradientangles[y,x],iθ)   # Disretized orientation
        v1, n1 = _interp_offset(img, x, y, xoffs[or], yoffs[or], Ix, Iy, pad)

        if (c > v1) & (c >= n1) # We need to check the value on the other side...
            v2, n2 = _interp_offset(img, x, y, -xoffs[or], -yoffs[or], Ix, Iy, pad)

            if (c > v2) & (c >= n2)  # This is a local maximum.
                                     # Record value in the output image.
                if calc_subpixel
                    # Solve for coefficients of parabola that passes through
                    # [-1, v2]  [0, img] and [1, v1].
                    # v = a*r^2 + b*r + c

                    # c = img[y,x]
                    a = (v1 + v2)/2 - c
                    b = a + c - v2

                    # location where maxima of fitted parabola occurs
                    r = -b/2a
                    location[y,x] = transposed ? Point(y + r*yoffs[or], x + r*xoffs[or]) :
                                                 Point(x + r*xoffs[or], y + r*yoffs[or])

                    if T<:FloatingPoint
                        # Store the interpolated value
                        out[y,x] = a*r^2 + b*r + c
                    else
                        out[y,x] = c
                    end
                else
                    out[y,x] = c
                end
            end
        end
    end

    out
end

# Main function call when subpixel location of edges is not needed
function thin_edges_nonmaxsup!{A<:AbstractArray,B<:AbstractArray}(out::A, img::A, gradientangles::B, border::String="replicate";
                                                                  radius::Float64=1.35, theta=pi/180)
    properties(out) != properties(img) && error("Input and output arrays must have the same properties")
    thin_edges_nonmaxsup_core!(data(out), Array(Point,(0,0)), img, gradientangles, radius, border, theta)
    out
end

function thin_edges_nonmaxsup{T}(img::AbstractArray{T,2}, gradientangles::AbstractArray, border::String="replicate";
                                 radius::Float64=1.35, theta=pi/180)
    (height,width) = size(img)
    out = zeros(T, height, width)
    thin_edges_nonmaxsup_core!(out, Array(Point,(0,0)), img, gradientangles, radius, border, theta)
    copy(img, out)
end

# Main function call when subpixel location of edges is desired
function thin_edges_nonmaxsup_subpix!{A<:AbstractArray, B<:AbstractArray, C<:AbstractArray}(out::A, location::B, img::A, gradientangles::C,
                                     border::String="replicate"; radius::Float64=1.35, theta=pi/180)
    properties(out) != properties(img) && error("Input and output arrays must have the same properties")
    eltype(location) != Point && error("Preallocated subpixel location array/image must have element type Graphics.Point")

    thin_edges_nonmaxsup_core!(data(out), data(location), img, gradientangles, radius, border, theta)
    img, location
end

function thin_edges_nonmaxsup_subpix{T}(img::AbstractArray{T}, gradientangles::AbstractArray,
                                        border::String="replicate"; radius::Float64=1.35, theta=pi/180)
    (height,width) = size(img)
    out = zeros(T, height, width)
    location = zeros(Point, height, width)
    thin_edges_nonmaxsup_core!(out, location, img, gradientangles, radius, border, theta)

    copy(img, out), copy(img, location)
end


using DiffEqOperators, SparseArrays, LinearAlgebra

x = 0.0:0.01:π
x_ = x[2:end-1]
dx = diff(x)
y = sin.(2x)
y_ = y[2:(end-1)]
dy = [2cos.(2x_),-4sin.(2x_),-8cos.(2x_),16y_,32cos.(2x_)]

# test concretization against regular grid operator
for dor in 1:4, aor in 2:2:6
    Dr = CenteredDifference(dor,aor,dx[1],length(x)-2)
    Dir = CenteredDifference(dor,aor,dx,length(x)-2)

    @test sparse(Dr)≈sparse(Dir)
    @test Array(Dr)≈Array(Dir)

end

# test irregular grid operator with regular grid
for dor in 1:4, aor in 2:6

    D1 = CenteredDifference(dor,aor,dx,length(x)-2)

    #take derivative
    yprime1 = D1*y

    #test result
    @test yprime1 ≈ dy[dor] atol = 10.0^(1-aor)#2test operator with known derivative of exp(kx)

    #TODO: implement specific tests for the left and right boundary regions, waiting until after update
end

# test irregular grid

x = sin.(0.0:0.05:π)
x = cumsum(x)
x = x/x[end]*π
x_ = x[2:end-1]
dx = diff(x)
y  = sin.(2x)
y_ = y[2:(end-1)]
dy = [2cos.(2x_), -4sin.(2x_), -8cos.(2x_), 16y_, 32cos.(2x_)]

for dor in 1:4, aor in 4:10

    D1 = CenteredDifference(dor,aor,dx,length(x)-2)

    #take derivative
    yprime1 = D1*y

    #test result
    tol = 2*10.0^(2-aor)*maximum(dx)^(2-dor)
    # error estimate is fairly difficult for high order derivatives and small dx.

    # err = norm(yprime1.-dy[dor])
    # @show err
    # @show tol

    @test yprime1 ≈ dy[dor] atol = tol #2test operator with known derivative of exp(kx)

    #TODO: implement specific tests for the left and right boundary regions, waiting until after update
end


export pivotedqr

function pivotedqr(ψ::AbstractMatrix{T}, u::AbstractVector{T}; invert::Bool=true, verbose::Bool = true, ϵrel::Float64 = 1e-1, maxterms::Int64=typemax(Int64)) where {T}

    m, n = size(ψ)

    # Compute a pivoted QR decomposition of ψ, use the pivot for the sensor placement

    F = qr(ψ, Val(true))

    residue = copy(u)
    idxset = Int64[]
    dict = collect(1:n)
    ϵu = norm(u)
    ϵhist = [ϵu]
    ϵrel *= ϵu

    @inbounds for k=1:n
        # Update set of selected basis
        push!(idxset, F.p[k])

        # Update candidate dictionary
        filter!(x-> x != F.p[k], dict)

        # Compute residual
        c = view(ψ,:,idxset)\u
        residue = u - view(ψ,:,idxset)*c

        # Calculate stopping critera
        ϵ = norm(residue)
        if verbose == true
            push!(ϵhist, copy(ϵ))
        end
        if ϵ < ϵrel || k == maxterms
            break
        end

    end

    # Solve the system with the set of indices
    if verbose == true
        if invert == true
            c = zeros(length(idxset))
            c .= view(ψ,:, idxset)\u
            return idxset, c, ϵhist
        else
            return idxset, ϵhist
        end
    else
        if invert == true
            c = zeros(length(idxset))
            c .= view(ψ,:, idxset)\u
            return idxset, c
        else
            return idxset
        end
    end
end


### A Pluto.jl notebook ###
# v0.12.7

using Markdown
using InteractiveUtils

# ╔═╡ 6fd616f6-1ea1-11eb-3814-8bfb4a096c49
using Pkg, DrWatson

# ╔═╡ 6ffe2628-1ea1-11eb-24ea-57f985146a72
begin
	@quickactivate "StatisticsWithJuliaPlutoNotebooks"
	using Distributions, Random, StatsBase, StatsPlots
	Random.seed!(1)
end;

# ╔═╡ ed174bc4-1ea0-11eb-1e2f-a32874cec549
md"## Listing 3.11"

# ╔═╡ 70179b62-1ea1-11eb-13ce-7fb4d54bad4b
dist1 = TriangularDist(0,10,5)

# ╔═╡ 75c334f0-2134-11eb-132d-dfcb59e14474
dist2 = DiscreteUniform(1,5)

# ╔═╡ 75c37638-2134-11eb-150e-a5788c7f72a7
theorMean1, theorMean2 = mean(dist1), mean(dist2)

# ╔═╡ 75c3fa78-2134-11eb-0259-699e1c89a023
begin
	N = 10^6
	data1 = rand(dist1,N)
	data2 = rand(dist2,N)
	estMean1, estMean2 = mean(data1), mean(data2)
end

# ╔═╡ de66cfee-2134-11eb-2c7a-add4bee67341
density(data1, lab="Triangukar density")

# ╔═╡ a315bbee-2134-11eb-0249-17f3c6af3543
density!(data2, lab="Discrete Uniform density")

# ╔═╡ 75cedfbc-2134-11eb-3b3c-5bdc69e4735a
Text("Symmetric Triangular Distiribution on [0,10] has mean $theorMean1
	(estimated: $estMean1)")

# ╔═╡ 75cf627a-2134-11eb-0dc9-e5e3d869f4a8
Text("Discrete Uniform Distiribution on {1,2,3,4,5} has mean $theorMean2
	(estimated: $estMean2)")

# ╔═╡ 70182cd8-1ea1-11eb-094d-8d0d49cf15f3
md"## End of listing 3.11"

# ╔═╡ Cell order:
# ╟─ed174bc4-1ea0-11eb-1e2f-a32874cec549
# ╠═6fd616f6-1ea1-11eb-3814-8bfb4a096c49
# ╠═6ffe2628-1ea1-11eb-24ea-57f985146a72
# ╠═70179b62-1ea1-11eb-13ce-7fb4d54bad4b
# ╠═75c334f0-2134-11eb-132d-dfcb59e14474
# ╠═75c37638-2134-11eb-150e-a5788c7f72a7
# ╠═75c3fa78-2134-11eb-0259-699e1c89a023
# ╠═de66cfee-2134-11eb-2c7a-add4bee67341
# ╠═a315bbee-2134-11eb-0249-17f3c6af3543
# ╠═75cedfbc-2134-11eb-3b3c-5bdc69e4735a
# ╠═75cf627a-2134-11eb-0dc9-e5e3d869f4a8
# ╟─70182cd8-1ea1-11eb-094d-8d0d49cf15f3


function collatz_steps(n::Integer)
    n < 1 && throw(DomainError("n must be positive"))
    count = 0
    while n > 1
        count += 1
        n = isodd(n) ? 3n + 1 : n ÷ 2
    end
    count
end

# Recursion: hard to reason about and slow in Julia as there's no TCO.
function collatz_steps_slow(n::Integer)
    # Avoid checking n < 1 in every loop
    n < 1 && throw(DomainError("n must be positive"))
    _collatz_steps(n)
end

function _collatz_steps(n::Integer)
    n == 1 && return 0
    isodd(n) && return 2 + _collatz_steps((3n + 1) ÷ 2)
    1 + _collatz_steps(n÷2)
end


# # ImageInspector

# ImageInspector is a small package for educational purposes. Its main goal is not presenting functionality, but presenting package structure. This is its short documentation created in the package [Literate](https://fredrikekre.github.io/Literate.jl/v2) which uses the [Markdown](https://www.markdownguide.org/cheat-sheet) syntax.

# To use the package, we need to load first the required packages.

using ImageInspector
using Plots

# ## Grayscale images

# As a test example, we create the real matrix `img1` representing a circle. We first discretize the domain $[-1,1]$ in `xs`. We assign black colour whenever $x^2 + y^2 \le 1$. Since the white colour is represented by `[1; 1; 1]` and the black colour by `[0; 0; 0]`, we can do it by the following code:

xs = -1:0.001:1
img1 = [x^2+y^2>1 for x in xs, y in xs];

# This is a two-dimensional matrix, which represents a grayscale image. We convert it to an image by calling `image` and then we plot it.

plot(image(img1); axis = nothing, border = :none)

# We can convert it to a three-dimensinal representation by the `reshape` function. The resulting plot is the same even though a different method for the `image` function is called.

img2 = reshape(img1, (size(img1)..., 1))

plot(image(img2); axis = nothing, border = :none)

# ## Coloured images

# We can repeat a similar procedure for coloured images. We create again a three-dimensional matrix, convert it to an image and then plot it.

img2 = ones(size(img1)..., 3)
img2[:,:,2:3] .= [x^2+y^2>1 for x in xs, y in xs]

plot(image(img2); axis = nothing, border = :none)