using StructuralCausalModels

ProjDir = @__DIR__
cd(ProjDir) #do

df = DataFrame!(CSV.File(scm_path("..", "data", "WaffleDivorce.csv"), delim=';'));
df = DataFrame(
  :s => df[:, :South],
  :a => df[:, :MedianAgeMarriage],
  :m => df[:, :Marriage],
  :w => df[:, :WaffleHouses],
  :d => df[:, :Divorce]
);

fname = scm_path("..", "examples", "SR", "SR6.4.3", "sr6.4.3.dot")
Sys.isapple() && run(`open -a GraphViz.app $(fname)`)

d = OrderedDict(
  :s => [:a, :m, :w],
  :a => [:m, :d],
  :m => [:d],
  :w => [:d]
);

dag = DAG("sr6.4.3", d; df=df);
show(dag)

adjustmentsets = adjustment_sets(dag, :w, :d)
println("Adjustment sets:\n")
adjustmentsets |> display
println()

function show_differences(res)
   if length(res) > 0
    println()
    res |> display
    println()
  end
end

debug = true

a = copy(dag.a)
ar = copy(a);
vars = names(a, 1)
zna = NamedArray(zeros(Int, size(a)), (vars, vars), ("Rows", "Cols"));

c = [:s]
m = Symbol[]

println()
s = StructuralCausalModels.update_s(a, c, debug)
#@assert s == [:s, :a, :m, :w, :d]

count = 0
while true
  local res
  global count += 1
  println("\ncount = $count\n")
  println()

  global ar
  at = copy(ar)

  println("\nupdate_21\n")
  ar21 = StructuralCausalModels.update_21(ar, m, debug);
  res = findall(ar21 .!== ar)
  show_differences(res)

  ar = copy(ar21);
  println("\nupdate_22\n")
  ar22 = StructuralCausalModels.update_22(ar, s, debug)
  res = findall(ar22 .!== zna)
  show_differences(res)

  ar .+= ar22

  println("\nupdate_23\n")
  ar23 = StructuralCausalModels.update_23(ar, m, debug)
  res = findall(ar23 .!== zna)
  show_differences(res)

  ar .+= ar23

  println("\nupdate_24\n")
  ar24 = StructuralCausalModels.update_24(ar, m, debug)
  res = findall(ar24 .!== zna)
  show_differences(res)

  ar .+= ar24

  println("\nupdate_25\n")
  ar25 = StructuralCausalModels.update_25(ar, m, debug)
  res = findall(ar25 .!== zna)
  show_differences(res)

  ar .+= ar25

  println("\nupdate_26\n")
  ar26 = StructuralCausalModels.update_26(ar, m, debug)
  res = findall(ar26 .!== zna)
  show_differences(res)

  ar .+= ar26

  println("\nupdate_27\n")
  ar27 = StructuralCausalModels.update_27(ar, s, debug)
  res = findall(ar27 .!== zna)
  show_differences(res)

  ar .+= ar27

  println("\nupdate_28\n")
  ar28 = StructuralCausalModels.update_28(ar, s, debug)
  res = findall(ar28 .!== zna)
  show_differences(res)

  ar .+= ar28

  println("\nupdate_29\n")
  ar29 = StructuralCausalModels.update_29(ar, m, debug)
  res = findall(ar29 .!== zna)
  show_differences(res)

  ar .+= ar29

  println("\nupdate_30\n")
  ar30 = StructuralCausalModels.update_30(ar, m, debug)
  res = findall(ar30 .!== zna)
  show_differences(res)

  ar .+= ar30

  ar == at && break

end

println("\nEnd of 1st while loop ($count iterations).\n")

ar_old = copy(ar)
println("\nupdate_a\n")
ar = StructuralCausalModels.update_a(ar, true)
res = findall(ar .!== ar_old)
show_differences(res)

ar_old = copy(ar)
println("\nupdate_b\n")
ar = StructuralCausalModels.update_b(ar, s, true)
res = findall(ar .!== ar_old)
show_differences(res)

an = copy(ar)
count = 0
while true
  global count += 1
  println("\ncount = $count\n")
  println()

  global ar
  global an
  global res
  at = copy(ar)

  println("\nupdate_27n\n")
  a27n = StructuralCausalModels.update_27n(ar, an)
  res = findall(a27n .!== zna)
  show_differences(res)

  an .+= a27n
  ar .+= a27n

  println("\nupdate_22n\n")
  a22n = StructuralCausalModels.update_22n(ar, an)
  res = findall(a22n .!== zna)
  show_differences(res)

  an .+= a22n
  ar .+= a22n

  at == ar && break

end

println("\nEnd of 2nd while loop ($count iterations).\n")

println("\nupdate_i\n")
ai = StructuralCausalModels.update_i(ar)
res = findall(ar .!== ai)
show_differences(res)

ar = copy(ai)

select = setdiff(vars, vcat(c, m))
println(select)
fr = ar[select, select]
println()

display(fr)
println()


export  vdm_jac, vdm_iter!, vdm_newton

#----------------------------------------------------------------------
function vdm_jac(w, Xi, L)
    n = size(Xi,1)
    r = size(Xi,2)
    J = fill(typeof(Xi[1])(0), length(L), r + r*n)

    for k in 1:r
        for i in 1:length(L)
            J[i,k] = 1.;
            for v in 1:n
                J[i,k]*= Xi[v,k]^((L[i].z)[v])
            end
            for l in 1:n
                J[i,r+(k-1)*n+l] = w[k];
                for v in 1:n
                    if v==l
                        if (L[i].z)[v] > 0
                            J[i,r+(k-1)*n+l] *= (L[i].z)[v]*Xi[v,k]^((L[i].z)[v]-1)
                        else
                            J[i,r+(k-1)*n+l] = 0.0
                        end
                    else
                        J[i,r+(k-1)*n+l] *= Xi[v,k]^((L[i].z)[v])
                    end
                end
            end
        end
    end
    J
end

#----------------------------------------------------------------------
function vdm_iter!(w, Xi, s0, L)
    f = fill(typeof(Xi[1])(0), length(L))
    mnt = moment(w,Xi)
    for i in 1:length(L)
        f[i] = mnt(L[i].z)-s0[L[i]]
    end
    print("|ε|=", norm(f))
    J = vdm_jac(w,Xi,L)
    dwXi = J\f
    delta = norm(dwXi)
    println("  δ=", delta)

    r = length(w)
    n = size(Xi,1)
    for i in 1:r
        w[i] -= dwXi[i]
    end
    for i in 1:r
        for j in 1:n
            Xi[j,i] -= dwXi[r+(i-1)*n+j]
        end
    end

    delta
end

#----------------------------------------------------------------------
function vdm_newton(w0, Xi0, s0, L;  args...)
    eps::Float64 = 1.e-5
    cmax::Int64  = 10
    for arg in args
        if arg[1]==:maxit
            cmax=arg[2]
        end
        if arg[1]==:eps
            eps=arg[2]
        end
    end

    w = copy(w0)
    Xi = copy(Xi0)
    c = 0; delta = 1.0
    while delta > eps && c< cmax
        delta = vdm_iter!(w,Xi,s0,L)
        c += 1;
    end
    w, Xi
end

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


# general formula for elementwise activation: output = act(input)
# some examples of act: logistic, tanh, relu, etc
immutable Activation{F,T} <: Transformation
    n::Int
    # α::T  # scaling parameter for some activations
    input::SumNode{T,1}
    output::OutputNode{T,1}

    # construct a new Activation, and link the nodes if it's the identity
    function Activation(n::Int) #, α::T = zero(T))
        input = InputNode(T,n)
        output = OutputNode(T,n)
        # if F == :identity
        #     link_nodes!(output, input)
        # end
        new(n, input, output)
    end
end
Activation(s::Symbol, n::Int) = Activation{s,Float64}(n)

Base.show{F,T}(io::IO, act::Activation{F,T}) = print(io, "$F{$(act.n)}")

input_length(act::Activation) = act.n
output_length(act::Activation) = act.n

# params_length(act::Activation) = 0
# params{F,T}(act::Activation{F,T}) = zeros(T,0)
# grad{F,T}(act::Activation{F,T}) = zeros(T,0)

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

const _exp_threshold = 20

# # identity: nothing to do, since we linked the input to output
# transform!(act::Activation{:identity}) = act.output.val
# grad!(act::Activation{:identity}) = act.input.∇
identity′{T<:Number}(x::T, y::T) = one(T)

# for the following, compute the derivative f′(x), where y = act(x) is assumed precomputed
# ref: https://en.wikipedia.org/wiki/Activation_function

logit(x::Real) = log(x / (one(x) - x))
logistic(x::Real) = one(x) / (one(x) + exp(-x))
logistic′{T<:Number}(x::T, y::T) = y * (one(T) - y)

# tanh: act(x) = (eˣ .- e⁻ˣ) ./ (eˣ .+ e⁻ˣ)
tanh′{T<:Number}(x::T, y::T) = one(T) - y^2

softsign{T<:Number}(x::T) = x / (one(T) + abs(x))
softsign′{T<:Number}(x::T, y::T) = (one(T) / (one(T) + abs(x)))^2

relu{T<:Number}(x::T) = max(zero(T), x)
relu′{T<:Number}(x::T, y::T) = x >= zero(T) ? one(T) : zero(T)

# softplus{T<:Number}(x::T) = log(one(T) + exp(x))
# softplus′{T<:Number}(x::T, y::T) = logistic(x)

# set to f(x)=x, f'(x)=1 when x is too big due to floating point math
softplus{T<:Number}(x::T) = x > _exp_threshold ? x : log(one(T) + exp(x))
softplus′{T<:Number}(x::T, y::T) = x > _exp_threshold ? one(T) : logistic(x)

sinusoid{T<:Number}(x::T) = sin(x)
sinusoid′{T<:Number}(x::T, y::T) = cos(x)

gaussian{T<:Number}(x::T) = exp(-(x^2))
gaussian′{T<:Number}(x::T, y::T) = -2x*y

threshold{T<:Number}(x::T) = x > zero(T) ? one(T) : zero(T)
threshold′{T<:Number}(x::T, y::T) = zero(T)

sign′{T<:Number}(x::T, y::T) = zero(T)

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

# generic implementations... ensure there's a derivative method of the correct name

const activations = [
    :logistic,
    :tanh,
    :softsign,
    :relu,
    :softplus,
    :sinusoid,
    :gaussian,
    :threshold,
    :sign,
]

for act in activations
    s = string(act)
    f′ = Symbol(s*"′")

    @eval begin
        # elementwise map from input to output
        transform!(act::Activation{Symbol($s)}) = map!($act, act.output.val, act.input.val)

        # backprop gradient calc using specialized derivative
        function grad!(act::Activation{Symbol($s)})
            ∇x = input_grad(act)
            ∇y = output_grad(act)
            x = input_value(act)
            y = output_value(act)
            for i=1:act.n
                ∇x[i] = $f′(x[i], y[i]) * ∇y[i]
            end
        end

        # x-only version
        function $f′(x::Number)
            y = $act(x)
            $f′(convert(typeof(y), x), y)
        end

        value_func(act::Activation{Symbol($s)}) = $act
        deriv_func(act::Activation{Symbol($s)}) = $f′

        # export both functions
        export $act, $f′
    end
end

# ----------------------------------------------------------------------------
# softmax

function transform!{T}(act::Activation{:softmax,T})
    val = act.input.val
    out = act.output.val
    maxinput = maximum(val)
    for i=1:act.n
        # subtract the maxinput to prevent overflow... doesn't change final result
        out[i] = exp(val[i] - maxinput)
    end
    s = one(T) / sum(out)
    for i=1:act.n
        out[i] *= s
    end
    out
end

# the calc is done in the CrossEntropyLoss... just pass that gradient back
function grad!{T}(act::Activation{:softmax,T})
    copy!(act.input.∇, act.output.∇)
end

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

default_range(act::Activation) = linspace(-5,5)

# user recipe adds a default x range
@recipe act{F}(act::Activation{F}) = act, default_range(act)

# type recipe converts to a function of xi
@recipe act{A<:Activation}(::Type{A}, act::A) = Transformations.value_func(act)

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


# ------------------------------------------------------------------
# Licensed under the MIT License. See LICENSE in the project root.
# ------------------------------------------------------------------

"""
   PoissonProcess(λ)

A Poisson process with intensity `λ`.
"""
struct PoissonProcess{L<:Union{Real,Function}} <: PointProcess
  λ::L
end

Base.union(p₁::PoissonProcess{<:Real}, p₂::PoissonProcess{<:Real}) =
  PoissonProcess(p₁.λ + p₂.λ)

Base.union(p₁::PoissonProcess{<:Function}, p₂::PoissonProcess{<:Function}) =
  PoissonProcess(x -> p₁.λ(x) + p₂.λ(x))

Base.union(p₁::PoissonProcess{<:Real}, p₂::PoissonProcess{<:Function}) =
  PoissonProcess(x -> p₁.λ + p₂.λ(x))

Base.union(p₁::PoissonProcess{<:Function}, p₂::PoissonProcess{<:Real}) =
  PoissonProcess(x -> p₁.λ(x) + p₂.λ)

ishomogeneous(p::PoissonProcess{<:Real}) = true
ishomogeneous(p::PoissonProcess{<:Function}) = false

default_sampling_algorithm(::PoissonProcess, ::Geometry) = DiscretizedSampling()
default_sampling_algorithm(::PoissonProcess{<:Real}, ::Box) = ProductSampling()

#------------------
# HOMOGENEOUS CASE
#------------------

function rand_single(rng::Random.AbstractRNG,
                     p::PoissonProcess{<:Real}, g::Geometry,
                     ::DiscretizedSampling)
  # simulate number of points
  λ = p.λ; V = measure(g)
  n = rand(rng, Poisson(λ*V))

  # simulate homogeneous process
  pts = sample(g, HomogeneousSampling(n))

  # return point pattern
  PointSet(collect(pts))
end

function rand_single(rng::Random.AbstractRNG,
                     p::PoissonProcess{<:Real}, b::Box,
                     ::ProductSampling)
  # region configuration
  lo, up = coordinates.(extrema(b))

  # simulate number of points
  λ = p.λ; V = measure(b)
  n = rand(rng, Poisson(λ*V))

  # product of uniform distributions
  U = product_distribution([Uniform(lo[i], up[i]) for i in 1:embeddim(b)])

  # return point pattern
  PointSet(rand(rng, U, n))
end

#--------------------
# INHOMOGENEOUS CASE
#--------------------

function rand_single(rng::Random.AbstractRNG,
                     p::PoissonProcess{<:Function}, b::Box,
                     algo::DiscretizedSampling)
  # discretize region
  # TODO

  # discretize retention
  # TODO

  # sample each element
  # TODO
end

function rand_single(rng::Random.AbstractRNG,
                     p::PoissonProcess{<:Function}, b::Box,
                     algo::ThinnedSampling)
  # Lewis-Shedler algorithm
  # TODO
end


ndiff(A::Array, n::Integer) = n < 1 ? A : diff(ndiff(A, n-1))

s = [90, 47, 58, 29, 22, 32, 55, 5, 55, 73]
println.(collect(ndiff(s, i) for i in 0:9))


# module Lupin

function hreshape(V::Vector{T}; dims) where T
    A = Array{T}(undef, dims)
    (m, n) = dims
    for i=axes(A, 1), j=axes(A, 2)
        A[i,j] = V[(i-1)*n+j]
    end
    A
end

# similar Base.Iterators.partition but returns Matrix
function partition(v::Vector{T}; cols::Int, fill)::Matrix{T} where T
    len = length(v)
    n = div(len, cols)
    if iszero(len % cols)
        hreshape(v, dims=(n, cols))
    else
        A = hreshape(v[1:n*cols], dims=(n, cols))
        vcat(A, [v[n*cols+1:end]... Base.fill(fill, cols - len % cols)...])
    end
end

function partition(r::AbstractRange; kwargs...)
    partition(collect(r); kwargs...)
end

# partition by condition
function partition(f::Function, array::Array{T})::Tuple{Array{T}, Array{T}} where {T}
    (getindex(array, findall(f, array)),
     getindex(array, findall(!f, array)))
end

function partition(f::Function, r::AbstractRange{T})::Tuple{Vector{T}, Vector{T}} where {T}
    (getindex(r, findall(f, r)),
     getindex(r, findall(!f, r)))
end

# module Lupin


abstract type AbstractPolynomial{N, T} end

Base.broadcastable(p::AbstractPolynomial) = Ref(p)

"""
```math
\\int_{0}^{t} p\\left(\\tau\\right) e^{c \\tau} d\\tau
```

Can be found using integration by parts.
"""
@inline function exponential_integral(p::AbstractPolynomial{N}, c, t; inv_c = inv(c), exp_c_t=exp(c * t)) where N
    if N === 1
        return inv_c * constant(p) * (exp_c_t - 1)
    else
        return inv_c * (p(t) * exp_c_t - constant(p) - exponential_integral(derivative(p), c, t; inv_c=inv_c, exp_c_t=exp_c_t))
    end
end


"""
    calculate_residuals(ũ, u₀, u₁, α, ρ, internalnorm, t)

Calculate element-wise residuals
```math
\\frac{ũ}{α+\\max{|u₀|,|u₁|}*ρ}
```
"""
@inline @muladd function calculate_residuals(ũ::Number, u₀::Number, u₁::Number,
                                    α, ρ, internalnorm,t)
    ũ / (α + max(internalnorm(u₀,t), internalnorm(u₁,t)) * ρ)
end

@inline function calculate_residuals(ũ::Array{T}, u₀::Array{T}, u₁::Array{T}, α::T2,
                                             ρ::Real, internalnorm,t) where
                                             {T<:Number,T2<:Number}
    out = similar(ũ)
    calculate_residuals!(out, ũ, u₀, u₁, α, ρ, internalnorm,t)
    out
end

@inline function calculate_residuals(ũ, u₀, u₁, α, ρ, internalnorm,t)
  @.. calculate_residuals(ũ, u₀, u₁, α, ρ, internalnorm,t)
end

"""
    calculate_residuals(u₀, u₁, α, ρ, internalnorm, t)

Calculate element-wise residuals
```math
\\frac{u₁ - u₀}{α+\\max{|u₀|,|u₁|}*ρ}
```
"""

@inline @muladd function calculate_residuals(u₀::Number, u₁::Number,
                                    α, ρ, internalnorm,t)
    (u₁ - u₀) / (α + max(internalnorm(u₀,t), internalnorm(u₁,t)) * ρ)
end

@inline function calculate_residuals(u₀::Array{T}, u₁::Array{T}, α::T2,
                                             ρ::Real, internalnorm,t) where
                                             {T<:Number,T2<:Number}
    out = similar(u₀)
    calculate_residuals!(out, u₀, u₁, α, ρ, internalnorm,t)
    out
end

@inline function calculate_residuals(u₀, u₁, α, ρ, internalnorm,t)
  @.. calculate_residuals(u₀, u₁, α, ρ, internalnorm,t)
end

"""
    calculate_residuals(E₁, E₂, u₀, u₁, α, ρ, δ, scalarnorm, t)

Return element-wise residuals
```math
\\frac{δ E₁ + E₂}{α+\\max{scalarnorm(u₀),scalarnorm(u₁)}*ρ}.
```
"""
@inline @muladd function calculate_residuals(E₁::Number, E₂::Number, u₀::Number, u₁::Number,
                                             α::Real, ρ::Real, δ::Number, scalarnorm, t)
    (δ * E₁ + E₂) / (α + max(scalarnorm(u₀,t), scalarnorm(u₁,t)) * ρ)
end

@inline function calculate_residuals(E₁::Array{<:Number}, E₂::Array{<:Number},
                                     u₀::Array{<:Number}, u₁::Array{<:Number}, α::Real,
                                     ρ::Real, δ::Number, scalarnorm, t)
    out = similar(u₀)
    calculate_residuals!(out, E₁, E₂, u₀, u₁, α, ρ, δ, scalarnorm, t)
    out
end

@inline function calculate_residuals(E₁, E₂, u₀, u₁, α, ρ, δ, scalarnorm, t)
  @.. calculate_residuals(E₁, E₂, u₀, u₁, α, ρ, δ, scalarnorm, t)
end

# Inplace Versions

"""
    DiffEqBase.calculate_residuals!(out, ũ, u₀, u₁, α, ρ)

Save element-wise residuals
```math
\\frac{ũ}{α+\\max{|u₀|,|u₁|}*ρ}
```
in `out`.
"""
@inline function calculate_residuals!(out, ũ, u₀, u₁, α, ρ, internalnorm,t)
  @.. out = calculate_residuals(ũ, u₀, u₁, α, ρ, internalnorm,t)
  nothing
end

"""
    calculate_residuals!(out, u₀, u₁, α, ρ)

Save element-wise residuals
```math
\\frac{u₁ - u₀}{α+\\max{|u₀|,|u₁|}*ρ}
```
in `out`.
"""
@inline function calculate_residuals!(out, u₀, u₁, α, ρ, internalnorm,t)
  @.. out = calculate_residuals(u₀, u₁, α, ρ, internalnorm,t)
end

"""
    calculate_residuals!(out, E₁, E₂, u₀, u₁, α, ρ, δ, scalarnorm)

Calculate element-wise residuals
```math
\\frac{δ E₁ + E₂}{α+\\max{scalarnorm(u₀),scalarnorm(u₁)}*ρ}.
```
"""
@inline function calculate_residuals!(out, E₁, E₂, u₀, u₁, α, ρ, δ, scalarnorm, t)
  @.. out = calculate_residuals(E₁, E₂, u₀, u₁, α, ρ, δ, scalarnorm, t)
  out
end


using CairoMakie, SpecialFunctions
p(s, β) = (β + 1)*a(β) * s^β * exp(-a(β)*s^(β+1))
a(β) = gamma(((β + 2)/(β + 1))^(β + 1))
s = LinRange(0,3,30)
colors = ["#FF410D", "#6EE2FF", "#F7C530", "#95CC5E", "#D0DFE6", "#F79D1E"]

with_theme(theme_dark()) do
    fig = Figure(resolution = (600,400))
    ax = Axis(fig[1,1]; palette = (; color = colors))
    for β in [0,1], step in [:pre, :center, :post]
        stairs!(s, p.(s, β), step = step, label = "$(β), :$(step)")
    end
    lines!(s, p.(s, 1), color = :grey90)
    lines!(s, p.(s, 0), color = :grey70)
    text!(L"p(s)=(\beta+1)a_{\beta}\,s^{\beta}\exp(-a_{\beta}s^{\beta+1})",
        position = (0.7, 0.95), color = :white)
    text!(L"a_{\beta} = \Gamma[(\beta+2)/(\beta+1)]^{\beta + 1}",
        position = (1.7, 0.25), color = :white)
    axislegend("β, step", position = :rt)
    save("stairs.png", fig, px_per_unit = 2)
    display(fig)
end


using FEASTSolver
using LinearAlgebra
using Printf
using Random
using MatrixMarket
using NonlinearEigenproblems: nep_gallery, compute_Mder, polyeig, PEP
using DelimitedFiles

function bf_arrays()
    N = diagm(-1 => [1,1,1,1,1,1,1])
    Mh0 = 1/6*(4*I + N + N')
    Mh1 = N - N'
    Mh2 = -1*(2*I - N - N')
    Mh3 = Mh1
    Mh4 = -Mh2
    c = [0.6 1.3; 1.3 0.1; 0.1 1.2; 1.0 1.0; 1.2 1.0]
    I8 = Matrix(I, 8, 8)
    M0 = c[1,1] * kron(I8, Mh0) + c[1,2] * kron(Mh0, I8)
    M1 = c[2,1] * kron(I8, Mh1) + c[2,2] * kron(Mh1, I8)
    M2 = c[3,1] * kron(I8, Mh2) + c[3,2] * kron(Mh2, I8)
    M3 = c[4,1] * kron(I8, Mh3) + c[4,2] * kron(Mh3, I8)
    M4 = c[5,1] * kron(I8, Mh4) + c[5,2] * kron(Mh4, I8)
    return [M0, M1, M2, M3, M4]
end

function deficient_arrays()
	# Random.seed!(1234)
	# A0 = complex.(rand(15,15))
	# A1 = complex.(rand(15,15))
	# A0[:,1] .= 0
	A0 = mmread("data/quadraticM0.mtx")
	A1 = mmread("data/quadraticM1.mtx")

	return [(A0 .- 0.02.*A1), (0.1.*A1), A1]
end

function quadratic()
	A0 = mmread("data/system5A0.mtx")
	A1 = mmread("data/system5A1.mtx")
	A2 = mmread("data/system5A2.mtx")

	function T(z::ComplexF64)
		return (A2 .* (z*z)) .+ (A1 .* z) .+ A0
	end

	return T
end

function gun()
	nep_gun = nep_gallery("nlevp_native_gun")
	T(z) = compute_Mder(nep_gun, z)
	return T
end

function hadeler()
	nep_hadeler = nep_gallery("nlevp_native_hadeler", 100, 200)
	T(z) = compute_Mder(nep_hadeler, z)
	return T
end

# for i=2:8
# 	println("\nBeyn N=$(2^i)")
# 	e, v, res = nlfeast!(bf(), rand(ComplexF64,64,30), 2^i, 0, c=complex(1.0,1.0), r=0.5)
# 	info(e, v, res, complex(1.0,1.0), 0.5)
# end

# Random.seed!(1729)

println("\nButterfly Problem")
nep = PEP(bf_arrays())
e,v = polyeig(nep)
e_in = e[abs.(e .- (1.0+1.0im)) .<= 0.5]
bf_eig = hcat(real.(e), imag.(e))
bf_eig_in = hcat(real.(e_in), imag.(e_in))
writedlm("test/data/bf_eig.dat", bf_eig)
writedlm("test/data/bf_eig_in.dat", bf_eig_in)

println("\nDeficient Quadratic Problem")
nep = PEP(deficient_arrays())
e,v = polyeig(nep)
e_in = e[abs.(e) .<= 0.25]
def_eig = hcat(real.(e), imag.(e))
def_eig_in = hcat(real.(e_in), imag.(e_in))
writedlm("test/data/deficient_eig.dat", def_eig)
writedlm("test/data/deficient_eig_in.dat", def_eig_in)

# println("\nLarger Quadratic Problem")
# quad_feast_call(X, N, iter, err_tol) = nlfeast_moments!(quadratic(), X, N, iter, c=complex(-1.55,0.0), r=0.05, ϵ=err_tol, moments=2, store=true)
# quad_res = res_table(quad_feast_call, (1000,70), max_iter_all, 6, 1e-6, complex(-1.55,0.0), 0.05, err_tol_all)
# writedlm("test/data/quad.dat", zip(1:max_iter_all, eachcol(quad_res)...))

println("\nGun Cavity Problem")
# gun_feast_call(X, N, iter, err_tol) = nlfeast!(gun(), X, N, iter, c=complex(140000.0, 0.0), r=30000, ϵ=err_tol, store=false)
# gun_res = res_table(gun_feast_call, (9956,32), max_iter_all, 6, 1e-6, complex(140000.0, 0.0), 30000, err_tol_all)
e, v, res = nlfeast!(gun(), rand(ComplexF64,9956,84), 2^4, 10, c=complex(140000.0, 0.0), r=2*30000, debug=true, ϵ=10e-16, store=false)
e_conv = e[res .<= 1e-10]
e_in = e_conv[abs.(e_conv .- 140000) .<= 30000]
gun_eig = hcat(real.(e_conv), imag.(e_conv))
gun_eig_in = hcat(real.(e_in), imag.(e_in))
writedlm("test/data/gun_eig.dat", gun_eig)
writedlm("test/data/gun_eig_in.dat", gun_eig_in)

println("\nHadeler Problem")
# gun_feast_call(X, N, iter, err_tol) = nlfeast!(gun(), X, N, iter, c=complex(140000.0, 0.0), r=30000, ϵ=err_tol, store=false)
# gun_res = res_table(gun_feast_call, (9956,32), max_iter_all, 6, 1e-6, complex(140000.0, 0.0), 30000, err_tol_all)
e, v, res = nlfeast!(hadeler(), rand(ComplexF64,200,100), 2^5, 10, c=complex(-30.0, 0.0), r=20, debug=true, ϵ=10e-16, store=true)
e_conv = e[res .<= 1e-10]
e_in = e_conv[abs.(e_conv .+ 30) .<= 10]
hadeler_eig = hcat(real.(e_conv), imag.(e_conv))
hadeler_eig_in = hcat(real.(e_in), imag.(e_in))
writedlm("test/data/hadeler_eig.dat", hadeler_eig)
writedlm("test/data/hadeler_eig_in.dat", hadeler_eig_in)


include("VecUtils.jl")

struct NURBS
    number::Array{Int64,1}
    coefs::Array{Float64}
    knots::Array{Array{Float64,1}}
    order::Array{Int64,1}
end

function nrbmak(coefs, knots)
    np = [i for i in size(coefs)]
    dim = np[1]

    if length(knots)==3
        if length(np)==3
            np = push!(np,1)
        elseif length(np)==2
            np = append!(np,[1,1])
        end
        #constructing a volume
        number = np[2:4]
        if dim<4
            temp_coefs = coefs
            coefs = repeat([0., 0., 0., 1.], 1, np[2], np[3], np[4])
            coefs[1:dim,:,:,:] = temp_coefs
        end
        uorder = length(knots[1])-np[2]
        vorder = length(knots[2])-np[3]
        worder = length(knots[3])-np[4]
        uknots = sort(knots[1])
        vknots = sort(knots[2])
        wknots = sort(knots[3])
        knots = [uknots, vknots, wknots]
        order = [uorder, vorder, worder]
    elseif length(knots)==2
        if length(np)==2
            np = push!(np,1)
        end
        #constructing a surface
        number = np[2:3]
        if dim<4
            temp_coefs = coefs
            coefs = repeat([0., 0., 0., 1.], 1, np[2], np[3])
            coefs[1:dim,:,:,:] = temp_coefs
        end
        uorder = length(knots[1])-np[2]
        vorder = length(knots[2])-np[3]
        uknots = sort(knots[1])
        vknots = sort(knots[2])
        knots = [uknots, vknots]
        order = [uorder, vorder]
    else
        #constructing a curve
        if length(np)==1
            number = [np[1]]
        else
            number = [np[2]]
        end
        if dim<4
            temp_coefs = coefs
            coefs = repeat([0., 0., 0., 1.], 1, number[1])
            coefs[1:dim,:,:,:] = temp_coefs
        end
        order = [length(knots[1])-number[1]]
        uknots = sort(knots[1])
        knots = [uknots]
    end

    nrb = NURBS(number, coefs, knots, order)
    return nrb
end

function findspan(n::Int64, u, knot::Array{Float64,1})
    if (minimum(u)<knot[1]) || (maximum(u)>knot[end])
        error("Some value is outside the knot span")
    end
    if isa(u, Number)
        s = [0]
    else
        s = similar(u, Int64)
    end
    for j=1:length(u)
        if u[j]==knot[n+2]
            s[j]=n
            continue
        end
        s[j]=findlast(knot.<=u[j])-1
    end
    return s
end

function basisfun(iv::Array{Int64}, uv, p::Integer, U)
    B = zeros(length(uv), p+1)
    N = zeros(p+1)
    for jj = 1:length(uv)
        i = iv[jj]+1 #findspan uses 0-based numbering
        u = uv[jj]
        left = zeros(p+1)
        right = zeros(p+1)
        N[1] = 1

        for j=1:p
            left[j+1] = u - U[i+1-j]
            right[j+1] = U[i+j] - u;
            saved = 0
            for r = 0:j-1
                temp = N[r+1]/(right[r+2]+left[j-r+1])
                N[r+1] = saved + right[r+2]*temp
                saved = left[j-r+1]*temp
            end
            N[j+1] = saved
        end
        B[jj,:] = N
    end
    return B
end

function bspeval(d, c, k, u)
    nu = length(u)
    mc, nc = size(c)
    #readline(stdin)
    s = findspan(nc-1, u, k)
    N = basisfun(s, u, d, k)
    tmp1 = s .- d .+ 1
    p = zeros(mc, nu)
    for i=0:d
        p = p + repeat(N[:,i+1]',mc,1).*c[:,tmp1.+i]
    end
    return p
end

function nrbeval(nurbs::NURBS, tt::AbstractArray)
    foption = true
    degree = nurbs.order .- 1
    if length(nurbs.knots)==3
        #NURBS structure represents a volume
        num1 = nurbs.number[1]
        num2 = nurbs.number[2]
        num3 = nurbs.number[3]

        if isa(tt[1], Array)
            nt1 = length(tt[1])
            nt2 = length(tt[2])
            nt3 = length(tt[3])

            #Evaluate along the w direction
            val = reshape(nurbs.coefs, 4*num1*num2, num3)
            val = bspeval(degree[3], val, nurbs.knots[3], tt[3])
            val = reshape(val, (4, num1, num2, nt3))

            #Evaluate along the v direction
            val = permutedims(val, [1, 2, 4, 3])
            val = reshape(val, 4*num1*nt3, num2)
            val = bspeval(degree[2], val, nurbs.knots[2], tt[2])
            val = reshape(val, 4, num1, nt3, nt2)
            val = permutedims(val, [1, 2, 4, 3])

            #Evaluate along the u direction
            val = permutedims(val, [1, 3, 4, 2])
            val = reshape(val, 4*nt2*nt3, num1)
            val = bspeval(degree[1], val, nurbs.knots[1], tt[1])
            val = reshape(val, 4, nt2, nt3, nt1)
            val = permutedims(val, [1, 4, 2, 3])
            pnts = val
            p = pnts[1:3,:,:,:]
            w = pnts[4,:,:,:,:]'
            if foption
                p = p./repeat(w, 3, 1, 1, 1)
            end
        else
            #Evaluate at scattered points
            #tt[1,:] represents the u direction
            #tt[2,:] represents the v direction
            #tt[3,:] represents the w direction
            st = size(tt)
            if (st[1]!=3 && st[2]==3 && length(st)==2)
                tt = tt'
                st = size(tt)
            end
            nt = prod(st[2:end])
            tt = reshape(tt, (3, nt))
            # Evaluate along the w direction
            val = reshape(nurbs.coefs, 4*num1*num2, num3)
            val = bspeval(degree[3], val, nurbs.knots[3], tt[3,:])
            val = reshape(val, (4, num1, num2, nt))
            # Evaluate along the v direction
            val2 = zeros(4*num1, nt)
            for v=1:nt
                coefs = reshape(val[:,:,:,v], (4*num1, num2))
                val2[:,v] = bspeval(degree[2], coefs, nurbs.knots[2], tt[2,v])
            end
            val2 = reshape(val2, (4, num1, nt))
            # Evaluate along the u direction
            pnts = zeros(4, nt)
            for v = 1:nt
                coefs = reshape(val2[:,:,v], (4, num1))
                pnts[:,v] = bspeval(degree[1], coefs, nurbs.knots[1], tt[1,v])
            end
            w = pnts[4,:]'
            p = pnts[1:3,:]
            if (foption)
                p = p./repeat(w,3,1)
            end

            if length(st)!=2
                w = reshape(w, st[2:end])
                p = reshape(p, tuple((3,)...,st[2:end]...))
            end
        end
    elseif length(nurbs.knots)==2
        # NURBS structure represents a surface
        num1 = nurbs.number[1]
        num2 = nurbs.number[2]
        if isa(tt[1],Array)
            #Evaluate over a [u,v] grid
            # tt[1] represents the u direction
            # tt[2] represents the v direction
            nt1 = length(tt[1])
            nt2 = length(tt[2])

            #Evaluate along the v direction
            val = reshape(nurbs.coefs, 4*num1, num2)
            val = bspeval(degree[2], val, nurbs.knots[2], tt[2])
            val = reshape(val, 4, num1, nt2)

            #Evaluate along the u direction
            val = permutedims(val, [1, 3, 2])
            val = reshape(val, 4*nt2, num1)
            val = bspeval(degree[1], val, nurbs.knots[1], tt[1])
            val = reshape(val, 4, nt2, nt1)
            val = permutedims(val, [1, 3, 2])

            w = val[[4],:,:]
            p = val[1:3, :, :]
            if (foption)
                p = p./repeat(w, 3, 1, 1)
            end
        else
            # Evaluate at scattered points
            # tt[1,:] represents the u direction
            # tt[2,:] represents the v direction
            st = size(tt)
            if (st[1]!=2 && st[2]==2 && length(st)==2)
                tt = tt'
                st = size(tt)
            end
            nt = prod(st[2:end])
            tt = reshape(tt, (3, nt))
            # Evaluate along the v direction
            val = reshape(nurbs.coefs, 4*num1, num2)
            val = bspeval(degree[2], val, nurbs.knots[2], tt[2,:])
            val = reshape(val, (4, num1, nt))
            #Evaluate along the u direction
            pnts = zeros(4, nt)
            for v = 1:nt
                coefs = reshape(val[:,:,v], (4, num1))
                pnts[:,v] = bspeval(degree[1], coefs, nurbs.knots[1], tt[1,v])
            end
            w = pnts[4,:]'
            p = pnts[1:3,:]
            if (foption)
                p = p./repeat(w,3,1)
            end
            if (length(st)!=2)
                w = reshape(w, st[2:end])
                p = reshape(p, tuple((3,)...,st[2:end]...))
            end
        end
    else
        if isa(tt[1], Array) && length(tt)==1
            tt =tt[1]
        end
        st = size(tt)
        val = bspeval(nurbs.order[1]-1, nurbs.coefs, nurbs.knots[1], [tt...])
        w = val[4,:]'
        p = val[1:3,:]
        if foption
            p = p./repeat(w,3,1)
        end
        if (st[1]!=1 || length(st)!=2)
            w = reshape(w, st)
            p = reshape(p, tuple((3,)...,st...))
        end
    end
    return p
end

function nrbextract(srf::NURBS)
    for idim = 1:length(srf.knots)
        ord = srf.order[idim]
        if (srf.knots[idim][1]!=srf.knots[idim][ord] ||
            srf.knots[idim][end]!=srf.knots[idim][end-ord+1])
            error("nrbextract: only working with open knot vectors")
        end
    end

    if (length(srf.knots)==2)
        crvs = Array{NURBS}(undef, 4)
        for ind=1:2
            ind2 = mod(ind,2)+1
            bnd1 = (ind-1)*2+1
            bnd2 = (ind-1)*2+2
            if ind==1
                coefs1 = srf.coefs[:,1,:]
                coefs2 = srf.coefs[:,end,:]
            elseif ind==2
                coefs1 = srf.coefs[:,:,1]
                coefs2 = srf.coefs[:,:,end]
            end
            crvs[bnd1] = nrbmak(coefs1, srf.knots[ind2])
            crvs[bnd2] = nrbmak(coefs2, srf.knots[ind2])
        end
    elseif (length(srf.knots)==3)
        crvs = Array{NURBS}(undef, 6)
        for ind=1:3
            inds = setdiff(1:3, ind)
            bnd1 = (ind - 1)*2 + 1
            bnd2 = (ind - 1)*2 + 2
            if ind==1
                coefs1 = srf.coefs[:,1,:,:]
                coefs2 = srf.coefs[:,end,:,:]
            elseif ind==2
                coefs1 = srf.coefs[:,:,1,:]
                coefs2 = srf.coefs[:,:,end,:]
            elseif ind==3
                coefs1 = srf.coefs[:,:,:,1]
                coefs2 = srf.coefs[:,:,:,end]
            end
            crvs[bnd1] = nrbmak(coefs1, [srf.knots[inds[1]], srf.knots[inds[2]]])
            crvs[bnd2] = nrbmak(coefs2, [srf.knots[inds[1]], srf.knots[inds[2]]])
        end
    else
        error("The entity is not a surface nor a volume")
    end
    return crvs
end

function nrbline(points...)
    numPts = length(points)
    if numPts==1
        error("More than one point should be specfified")
    end
    coefs = [zeros(3,numPts); ones(1,numPts)]
    for iPt = 1:numPts
        coefs[1:length(points[iPt]),iPt] .= points[iPt]
    end
    knots = vcat(0, LinRange(0,1,numPts), 1)
    line = nrbmak(coefs, [knots])
    return line
end

function nrbline()
    p1 = [0,0]
    p2 = [1,0]
    line = nrbline(p1, p2)
    return line
end

function nrbrect(width, height)
    coefs =[0. width width width width 0. 0. 0.;
          0. 0. 0. height height height height 0.;
          0. 0. 0. 0. 0. 0. 0. 0.;
          1. 1. 1. 1. 1. 1. 1. 1.]
    knots = [0, 0, 0.25, 0.25, 0.5, 0.5, 0.75, 0.75, 1, 1]
    curve = nrbmak(coefs, [knots])
    return curve
end

function nrbrect(size)
    return nrbrect(size, size)
end

function nrbrect()
    return nrbrect(1.)
end

function nrbcirc(radius::Real, center::Vector, sang::Real, eang::Real)
    sweep = eang - sang
    if sweep < 0
        sweep = 2*pi + sweep
    end

    if abs(sweep) <= pi/2
        narcs = 1   # number of arc segments
        knots = [0, 0, 0, 1, 1, 1]
    elseif abs(sweep) <= pi
        narcs = 2
        knots = [0, 0, 0, 0.5, 0.5, 1, 1, 1]
    elseif abs(sweep) <= 3*pi/2
        narcs = 3
        knots = [0, 0, 0, 1/3, 1/3, 2/3, 2/3, 1, 1, 1]
    else
        narcs = 4
        knots = [0, 0, 0, 0.25, 0.25, 0.5, 0.5, 0.75, 0.75, 1, 1, 1]
    end

    dsweep = sweep/(2*narcs)  #arc segment sweep angle/2

    # determine middle control point and weight
    wm = cos(dsweep)
    x  = radius*wm
    y  = radius*sin(dsweep)
    xm = x+y*tan(dsweep)

    # arc segment control points
    ctrlpt = [ x wm*xm x;    # w*x - coordinate
              -y 0     y;    # w*y - coordinate
               0 0     0;    # w*z - coordinate
               1 wm    1]    # w   - coordinate

    # build up complete arc from rotated segments
    coefs = zeros(4,2*narcs+1)   # nurbs control points of arc
    xx = vecrotz(sang + dsweep)
    coefs[:,1:3] = xx*ctrlpt;     # rotate to start angle
    xx = vecrotz(2*dsweep);
    for n = 2:narcs
       m = 2*n.+[0, 1]
       coefs[:,m] = xx*coefs[:,m.-2]
    end

    # vectrans arc if necessary
    if !isempty(center)
      xx = vectrans(center)
      coefs = xx*coefs
    end

    curve = nrbmak(coefs,[knots])
    return curve
end

function nrbcirc(radius::Real, center::Vector)
    sang = 0.
    eang = 2*pi
    curve = nrbcirc(radius, center, sang, eang)
    return curve
end

function nrbcirc(radius::Real)
    center = []
    curve = nrbcirc(radius, center)
    return curve
end

function nrbcirc()
    radius = 1
    curve = nrbcirc(radius)
    return curve
end

function bspkntins(d, c, k, u)
    tolEq = 1e-10
    mc, nc = size(c)
    sort!(u)
    nu = length(u)
    nk = length(k)

    ic = zeros(mc, nc+nu)
    ik = zeros(nk+nu)

    n = nc - 1
    r = nu - 1

    m = n + d + 1
    a = findspan(n, u[1], k)[1]
    b = findspan(n, u[r+1], k)[1]
    b += 1

    ic[:, 1:a-d+1] = c[:, 1:a-d+1]
    ic[:, b+nu:nc+nu] = c[:, b:nc]

    ik[1:a+1] = k[1:a+1]
    ik[b+d+nu+1:m+nu+1] = k[b+d+1:m+1]

    ii = b + d - 1
    ss = ii + nu

    for jj = r:-1:0
        ind = (a+1):ii
        ind = ind[u[jj+1].<=k[ind.+1]]
        ic[:, ind.+ss.-ii.-d] = c[:,ind.-d]
        ik[ind.+ss.-ii.+1] = k[ind.+1]
        ii = ii - length(ind)
        ss = ss - length(ind)

        ic[:,ss-d] = ic[:, ss-d+1]
        for l=1:d
            ind = ss - d + l
            alfa = ik[ss+l+1] - u[jj+1]
            if abs(alfa) < tolEq
                ic[:,ind] = ic[:, ind+1]
            else
                alfa = alfa/(ik[ss+l+1]-k[ii-d+l+1])
                tmp = (1-alfa) * ic[:, ind+1]
                ic[:,ind] = alfa*ic[:,ind] + tmp
            end
        end
        ik[ss+1] = u[jj+1]
        ss = ss-1
    end
    return ic, ik
end

function nrbkntins(nurbs::NURBS, iknots::AbstractArray)
    degree = nurbs.order .- 1
    fmax(x,y) = any(y.>maximum(x))
    fmin(x,y) = any(y.<minimum(x))
    for i=1:length(nurbs.knots)
        if any(fmax(nurbs.knots[i], iknots[i])) || any(fmin(nurbs.knots[i], iknots[i]))
            error("Trying to insert a knot outside the interval of definition")
        end
    end
    knots = Array{Array}(undef, length(nurbs.knots))
    if length(nurbs.knots)==3
        # NURBS represents a volume
        num1 = nurbs.number[1]
        num2 = nurbs.number[2]
        num3 = nurbs.number[3]

        # Insert knots along the w direction
        if isempty(iknots[3])
            coefs = nurbs.coefs
            knots[3] = nurbs.knots[3]
        else
            coefs = reshape(nurbs.coefs, 4*num1*num2, num3)
            coefs, knots[3] = bspkntins(degree[3], coefs, nurbs.knots[3], iknots[3])
            num3 = size(coefs, 2)
            coefs = reshape(coefs, 4, num1, num2, num3)
        end

        # Insert knots along the v direction
        if isempty(iknots[2])
            knots[2] = nurbs.knots[2]
        else
            coefs = permutedims(coefs, [1, 2, 4, 3])
            coefs = reshape(coefs, 4*num1*num3, num2)
            coefs, knots[2] = bspkntins(degree[2], coefs, nurbs.knots[2], iknots[2])
            num2 = size(coefs, 2)
            coefs = reshape(coefs, 4, num1, num3, num2)
            coefs = permutedims(coefs, [1, 2, 4, 3])
        end

        #Insert knots along the u direction
        if isempty(iknots[1])
            knots[1] = nurbs.knots[1]
        else
            coefs = permutedims(coefs, [1, 3, 4, 2])
            coefs = reshape(coefs, 4*num2*num3, num1)
            coefs, knots[1] = bspkntins(degree[1], coefs, nurbs.knots[1], iknots[1])
            coefs = reshape(coefs, 4, num2, num3, size(coefs, 2))
            coefs = permutedims(coefs, [1, 4, 2, 3])
        end
    elseif length(nurbs.knots)==2
        # NURBS rrepresents a surface
        num1 = nurbs.number[1]
        num2 = nurbs.number[2]

        # Insert knots along the v direction
        if isempty(iknots[2])
            coefs = nurbs.coefs
            knots[2] = nurbs.knots[2]
        else
            coefs = reshape(nurbs.coefs, 4*num1, num2)
            coefs, knots[2] = bspkntins(degree[2], coefs, nurbs.knots[2], iknots[2])
            num2 = size(coefs, 2)
            coefs = reshape(coefs, 4, num1, num2)
        end

        # Insert knots along the u directions
        if isempty(iknots[1])
            knots[1] = nurbs.knots[1]
        else
            coefs = permutedims(coefs, [1, 3, 2])
            coefs = reshape(coefs, 4*num2, num1)
            coefs, knots[1] = bspkntins(degree[1], coefs, nurbs.knots[1], iknots[1])
            coefs = reshape(coefs, 4, num2, size(coefs,2))
            coefs = permutedims(coefs, [1, 3, 2])
        end
    else
        # NURBS represents a curve
        if isempty(iknots[1])
            coefs = nurbs.coefs
            knots = nurbs.knots
        else
            coefs, knots[1] = bspkntins(degree[1], nurbs.coefs, nurbs.knots[1], iknots[1])
        end
    end

    #construct the new NURBS
    inurbs = nrbmak(coefs, knots)
    return inurbs
end

"""
Joins two 1D NURBS objects (NURBS curves), nurbsA and nurbsB, with matching end control points. The
output is a single NURBS curve with scaled and normalized knot vector
"""
function nrbjoin(nurbsA::NURBS, nurbsB::NURBS)
    coefsA = nurbsA.coefs
    coefsB = nurbsB.coefs
    knotsA = nurbsA.knots[1]
    knotsB = nurbsB.knots[1]
    tol_eq = 1e-10
    if norm(coefsA[:,end]-coefsB[:,1])>tol_eq
        error("The two NURBS objects do not appear to have a common endpoint")
    end
    coefs_new = hcat(coefsA, coefsB[:,2:end])
    knotsB .+= nurbsA.knots[1][end]
    knots_new = vcat(knotsA, knotsB[2:end])
    #linearly map knots_new to the interval [0,1]
    knots_new = (knots_new .- knots_new[1])./(knots_new[end]-knots_new[1])
    nrb_new = nrbmak(coefs_new, [knots_new])
    return nrb_new
end

function bspdegelev(d, c, k, t)
    mc, nc = size(c)
    ic = zeros(mc, nc*(t+1))
    n = nc - 1
    bezalfs = zeros(d+1, d+t+1)
    bpts = zeros(mc, d+1)
    ebpts = zeros(mc, d+t+1)
    Nextbpts = zeros(mc, d+1)
    alfs = zeros(d)

    m = n + d + 1
    ph = d + t
    ph2 = floor(Int, ph/2)
    # Compute Bezier degree elevation coefficients
    bezalfs[1,1] = 1
    bezalfs[d+1, ph+1] = 1

    for i=1:ph2
        inv = 1/binomial(ph, i)
        mpi = min(d,i)

        for j=max(0, i-t):mpi
            bezalfs[j+1, i+1] = inv*binomial(d,j)*binomial(t, i-j)
        end
    end

    for i=ph2+1:ph-1
        mpi = min(d,i)
        for j=max(0,i-t):mpi
            bezalfs[j+1, i+1] = bezalfs[d-j+1, ph-i+1]
        end
    end

    mh = ph
    kind = ph+1
    r = -1
    a = d
    b = d + 1
    cind = 1
    ua = k[1]

    ic[1:mc,1] = c[1:mc,1]
    ik = ua.*ones(ph+1)

    # Initialize the first Bezier segment
    #bpts[1:mc, 1:d+1] = c[1:mc, 1:d+1]
    bpts = copy(c)

    # Big loop through knot vector
    while b<m
        i=b
        while b<m && k[b+1] == k[b+2]
            b += 1
        end
        mul = b - i + 1
        mh = mh + mul + t
        ub = k[b+1]
        oldr = r
        r = d - mul

        # Insert knot u[b] r times
        if oldr > 0
            lbz = floor(Int, (oldr+2)/2)
        else
            lbz = 1
        end

        if r>0
            rbz = ph - floor(Int, (r+1)/2)
        else
            rbz = ph
        end

        if r>0
            # Insert knot to get Bezier segment
            numer = ub - ua
            for q = d:-1:mul+1
                alfs[q-mul] = numer / (k[a+q+1]-ua)
            end

            for j = 1:r
                save = r-j
                s = mul + j

                for q = d:-1:s
                    for ii = 0:mc-1
                        tmp1 = alfs[q-s+1]*bpts[ii+1, q+1]
                        tmp2 = (1-alfs[q-s+1])*bpts[ii+1, q]
                        bpts[ii+1, q+1] = tmp1 + tmp2
                    end
                end

                Nextbpts[:, save+1] = bpts[:, d+1]
            end
        end
        # End of insert knot

        #Degree elevate Bezier
        for i=lbz:ph
            ebpts[:,i+1] = zeros(mc)
            mpi = min(d,i)
            for j=max(0,i-t):mpi
                for ii=0:mc-1
                    tmp1 = ebpts[ii+1, i+1]
                    tmp2 = bezalfs[j+1, i+1]*bpts[ii+1, j+1]
                    ebpts[ii+1, i+1] = tmp1 + tmp2
                end
            end
        end
        # End of degree elevating Bezier

        if oldr > 1
            # Must remove knot u=k[a] oldr times
            first = kind - 2
            last = kind
            den = ub - ua
            bet = floor(Int, (ub-ik[kind])/den)

            # Knot removal loop
            for tr = 1:oldr-1
                i = first
                j = last
                kj = j - kind + 1
                while j-i > tr
                    # Loop and compute the new control points for one removal step
                    if i < cind
                        alf = (ub - ik[i+1])/(ua - ik[i+1])
                        tmp1 = alf.*ic[:,i+1]
                        tmp2 = (1-alf).*ic[:, i]
                        ic[:,i+1] = tmp1 + tmp2
                    end
                    if j >= lbz
                        if j-tr <= kind - ph + oldr
                            gam = (ub-ik[j-tr+1])/den
                            tmp1 = gam.*ebpts[:, kj+1]
                            tmp2 = (1-gam).*ebpts[:, kj+2]
                            ebpts[:, kj+1] = tmp1 + tmp2
                        else
                            tmp1 = bet.*ebpts[:, kj+1]
                            tmp2 = (1-bet).*ebpts[:, kj+2]
                            ebpts[:, kj+1] = tmp1 + tmp2
                        end
                    end
                    i += 1
                    j -= 1
                    kj -= 1
                end
                first -= 1
                last += 1
            end
        end
        # End of removing knot n=k[a]

        # Load the knot ua
        if a!=d
            for i=0:ph-oldr-1
                push!(ik, ua)
                #ik[kind+1] = ua
                kind += 1
            end
        end

        for j = lbz:rbz
            for ii = 0:mc-1
                ic[ii+1, cind+1] = ebpts[ii+1, j+1]
            end
            cind += 1
        end

        if b < m
            # Setup for next pass through loop
            bpts[:,1:r] = Nextbpts[:, 1:r]
            bpts[:,r+1:d+1] = c[:, b-d+r+1:b+1]
            a = b
            b += 1
            ua = ub
        else
            for i=0:ph
                #ik[kind+i+1] = ub
                push!(ik, ub)
            end
        end
    end
    #End big while loop
    ic = ic[:, 1:cind]
    return ic, ik
end

function nrbdegelev(nurbs::NURBS, ntimes::Array{Int,1})
    degree = nurbs.order .- 1
    knots = Array{Array}(undef, length(nurbs.knots))
    if length(nurbs.knots)==3
        # NURBS represents a volume
        _, num1, num2, num3 = size(nurbs.coefs)

        # Degree elevate along the w direction
        if ntimes[3] == 0
            coefs = nurbs.coefs
            knots[3] = nurbs.knots[3]
        else
            coefs = reshape(nurbs.coefs, 4*num1*num2,num3)
            coefs, knots[3] = bspdegelev(degree[3], coefs, nurbs.knots[3], ntimes[3])
            num3 = size(coefs, 2)
            coefs = reshape(coefs, 4, num1, num2, num3)
        end

        # Degree elevate along the v direction
        if ntimes[2] == 0
            knots[2] = nurbs.knots[2]
        else
            coefs = permutedims(coefs, [1, 2, 4, 3])
            coefs = reshape(coefs, 4*num1*num3, num2)
            coefs, knots[2] = bspdegelev(degree[2], coefs, nurbs.knots[1], ntimes[1])
            num2 = size(coefs, 2)
            coefs = reshape(coefs, 4, num1, num3, num2)
            coefs = permutedims(coefs, [1, 2, 4, 3])
        end

        # Degree elevate along the u direction
        if ntimes[1] == 0
            knots[1] = nurbs.knots[1]
        else
            coefs = permutedims(coefs, [1, 3, 4, 2])
            coefs = reshape(coefs, 4*num2*num3, num1)
            coefs, knots[1] = bspdegelev(degree[1], coefs, nurbs.knots[1], ntimes[1])
            coefs = reshape(coefs, 4, num2, num3, size(coefs, 2))
            coefs = permutedims(coefs, [1, 4, 2, 3])
        end
    elseif length(nurbs.knots)==2
        _, num1, num2 = size(nurbs.coefs)

        # Degree elevate along the v direction
        if ntimes[2] == 0
            coefs = nurbs.coefs
            knots[2] = nurbs.knots[2]
        else
            coefs = reshape(nurbs.coefs, 4*num1, num2)
            coefs, knots[2] = bspdegelev(degree[2], coefs, nurbs.knots[2], ntimes[2])
            num2 = size(coefs, 2)
            coefs = reshape(coefs, 4, num1, num2)
        end

        # Degree elevate along the u direction
        if ntimes[1] == 0
            knots[1] = nurbs.knots[1]
        else
            coefs = permutedims(coefs, [1, 3, 2])
            coefs = reshape(coefs, 4*num2, num1)
            coefs, knots[1] = bspdegelev(degree[1], coefs, nurbs.knots[1], ntimes[1])
            coefs = reshape(coefs, 4, num2, size(coefs, 2))
            coefs = permutedims(coefs, [1, 3, 2])
        end
    else
        # NURBS represents a curve
        if isempty(ntimes)
            coefs = nurbs.coefs
            knots = nurbs.knots
        else
            coefs, knots[1] = bspdegelev(degree[1], nurbs.coefs, nurbs.knots[1], ntimes[1])
        end
    end

    #construct new NURBS
    inurbs = nrbmak(coefs, knots)
    return inurbs
end

function nrbruled(crv1::NURBS, crv2::NURBS)
    # Ensure both curves have a common degree
    d = max(crv1.order, crv2.order)[1]
    crv1 = nrbdegelev(crv1, [d - crv1.order[1]])
    crv2 = nrbdegelev(crv2, [d - crv2.order[1]])

    # Merge the knot vectors to obtain a common knot vector
    k1 = crv1.knots[1]
    k2 = crv2.knots[1]

    ku = unique(vcat(k1, k2))
    n = length(ku)

    ka = Array{Float64,1}(undef,0)
    kb = Array{Float64,1}(undef,0)

    for i = 1:n
        i1 = length(findall(k1.==ku[i]))
        i2 = length(findall(k2.==ku[i]))
        m = max(i1, i2)
        ka = vcat(ka, ku[i].*ones(m-i1))
        kb = vcat(kb, ku[i].*ones(m-i2))
    end
    crv1 = nrbkntins(crv1, [ka])
    crv2 = nrbkntins(crv2, [kb])

    coefs = cat(crv1.coefs, crv2.coefs, dims=3)
    srf = nrbmak(coefs, [crv1.knots[1], [0,0,1,1]])
    return srf
end

function nrbtform(nurbs::NURBS, tmat)
    if length(nurbs.knots)==2
        # NURBS is a surface
        dim, nu, nv = size(nurbs.coefs)
        coefs = reshape(tmat*reshape(nurbs.coefs, dim, nu*nv), dim, nu, nv)
    elseif length(nurbs.knots)==3
        # NURBS is a volume
        dim, nu, nv, nw = size(nurbs.coefs)
        coefs = reshape(tmat*reshape(nurbs.coefs, dim, nu*nv*nw), dim, nu, nv, nw)
    else
        # NURBS is a curve
        coefs = tmat*nurbs.coefs
    end
    tnurbs = nrbmak(coefs, nurbs.knots)
    return tnurbs
end

function nrbtransp(srf)
    if length(srf.knots)==1
        error("A NURBS curve cannot be transposed")
    elseif length(srf.knots)==3
        error("The transposition of NURBS volumes has not been implemented")
    end
    tsrf = nrbmak(permutedims(srf.coefs, [1,3,2]), reverse(srf.knots))
    return tsrf
end

function nrb4surf(p11, p12, p21, p22)
    coefs = cat(zeros(3,2,2), ones(1,2,2), dims=1)
    coefs[1:length(p11),1,1] = p11[:]
    coefs[1:length(p12),2,1] = p12[:]
    coefs[1:length(p21),1,2] = p21[:]
    coefs[1:length(p22),2,2] = p22[:]

    knots = [[0,0,1,1], [0,0,1,1]]
    srf = nrbmak(coefs, knots)
    return srf
end

function nrbcoons(u1::NURBS, u2::NURBS, v1::NURBS, v2::NURBS)
    tolEq = 1e-10
    if maximum(abs.(nrbeval(u1,u1.knots[1][[1]]) - nrbeval(v1,v1.knots[1][[1]])))>tolEq ||
            maximum(abs.(nrbeval(u1,u1.knots[1][[end]]) - nrbeval(v2,v2.knots[1][[1]])))>tolEq ||
            maximum(abs.(nrbeval(u2,u2.knots[1][[1]]) - nrbeval(v1,v1.knots[1][[end]])))>tolEq ||
            maximum(abs.(nrbeval(u2,u2.knots[1][[end]]) - nrbeval(v2,v2.knots[1][[end]])))>tolEq
        error("The four curves do not define a closed boundary")
    end

    r1 = nrbruled(u1, u2)
    r2 = nrbtransp(nrbruled(v1, v2))
    t = nrb4surf(u1.coefs[:,1], u1.coefs[:,end], u2.coefs[:,1], u2.coefs[:,end])

    # Raise all surfaces to a common degree
    du = max(r1.order[1], r2.order[1], t.order[1])
    dv = max(r1.order[2], r2.order[2], t.order[2])
    r1 = nrbdegelev(r1, [du - r1.order[1], dv - r1.order[2]])
    r2 = nrbdegelev(r2, [du - r2.order[1], dv - r2.order[2]])
    t = nrbdegelev(t, [du - t.order[1], dv - t.order[2]])

    # Merge the knots vectors, to obtain a commmon knot vector

    #Uknots
    k1 = r1.knots[1]
    k2 = r2.knots[1]
    k3 = t.knots[1]
    k = unique(vcat(k1, k2, k3))
    n = length(k)
    kua = Array{Float64,1}(undef,0)
    kub = Array{Float64,1}(undef,0)
    kuc = Array{Float64,1}(undef,0)

    for i=1:n
        i1 = length(findall(k1.==k[i]))
        i2 = length(findall(k2.==k[i]))
        i3 = length(findall(k3.==k[i]))
        m = max(i1, i2, i3)
        kua = vcat(kua, k[i].*ones(m-i1))
        kub = vcat(kub, k[i].*ones(m-i2))
        kuc = vcat(kuc, k[i].*ones(m-i3))
    end

    # Vknots
    k1 = r1.knots[2]
    k2 = r2.knots[2]
    k3 = t.knots[2]

    k = unique(vcat(k1, k2, k3))
    n = length(k)

    kva = Array{Float64,1}(undef,0)
    kvb = Array{Float64,1}(undef,0)
    kvc = Array{Float64,1}(undef,0)

    for i=1:n
        i1 = length(findall(k1.==k[i]))
        i2 = length(findall(k2.==k[i]))
        i3 = length(findall(k3.==k[i]))
        m = max(i1, i2, i3)
        kva = vcat(kva, k[i].*ones(m-i1))
        kvb = vcat(kvb, k[i].*ones(m-i2))
        kvc = vcat(kvc, k[i].*ones(m-i3))
    end

    r1 = nrbkntins(r1, [kua, kva])
    r2 = nrbkntins(r2, [kub, kvb])
    t = nrbkntins(t, [kuc, kvc])

    # combine coefficients to construct the Coons surface
    coefs = zero(r1.coefs)
    coefs[1,:,:] = r1.coefs[1,:,:] + r2.coefs[1,:,:] - t.coefs[1,:,:]
    coefs[2,:,:] = r1.coefs[2,:,:] + r2.coefs[2,:,:] - t.coefs[2,:,:]
    coefs[3,:,:] = r1.coefs[3,:,:] + r2.coefs[3,:,:] - t.coefs[3,:,:]
    coefs[4,:,:] = r1.coefs[4,:,:] + r2.coefs[4,:,:] - t.coefs[4,:,:]
    srf = nrbmak(coefs, r1.knots)
end

function nrbrevolve(curve::NURBS, pnt, vec, theta)
    if length(curve.knots)==3
        error("The function nrbrevolve is not yet ready to create volumes")
    end

    # Translate the center point to the origin
    if isempty(pnt)
        pnt = zeros(3)
    end

    if length(pnt)!=3
        error("All point and vecto coordinates must be 3D")
    end

    # Translate and rotate the original curve or surface into alignment with the z-axis
    T = vectrans(-pnt)
    angx = vecangle(vec[1], vec[3])
    RY = vecroty(-angx[1])
    vectmp = RY*vcat(vecnorm(vec), 1.)
    angy = vecangle(vectmp[2], vectmp[3])
    RX = vecrotx(angy[1])
    curve = nrbtform(curve, RX*RY*T)

    # Construct an arc
    arc = nrbcirc(1.0, [], 0.0, theta)
    if length(curve.knots)==2
        # Construct the revolved volume
        coefs = zeros(4, arc.number[1], curve.number[1], curve.number[2])
        angle = vecangle(curve.coefs[2,:,:], curve.coefs[1,:,:])
        radius = dropdims(vecmag(curve.coefs[1:2, :, :]), dims=1)
        for i=1:curve.number[1]
            for j=1:curve.number[2]
                coefs[:,:,i,j] = vecrotz(angle[i,j])*vectrans([0., 0., curve.coefs[3,i,j]])*
                            vecscale([radius[i,j], radius[i,j]])*arc.coefs
                coefs[4,:,i,j] = coefs[4, :, i, j]*curve.coefs[4,i,j]
            end
        end
        surf = nrbmak(coefs, vcat(arc.knots, curve.knots))
    else
        # Construct the revolved surface
        coefs = zeros(4, arc.number[1], curve.number[1])
        angle = vecangle(curve.coefs[2,:], curve.coefs[1,:])
        radius = vecmag(curve.coefs[1:2, :])
        for i = 1:curve.number[1]
            coefs[:, :, i] = vecrotz(angle[i])*vectrans([0., 0., curve.coefs[3,i]])*
                        vecscale([radius[i], radius[i]])*arc.coefs
            coefs[4, :, i] = coefs[4, :, i]*curve.coefs[4, i]
        end
        surf = nrbmak(coefs, vcat(arc.knots, curve.knots))
    end

    # Rotate and vectrans the surface back into position
    T = vectrans(pnt)
    RX = vecrotx(-angy[1])
    RY = vecroty(angx[1])
    surf = nrbtform(surf, T*RY*RX)
    return surf
end

function nrbrevolve(curve::NURBS, pnt, vec)
    surf = nrbrevolve(curve, pnt, vec, 2*pi)
    return surf
end

"""
Refine a given knot vector by dividing each interval uniformly,
             maintaining the continuity in previously existing knots.
"""
function kntrefine(knots, n_sub, degree, regularity)
    if length(n_sub)!=length(degree) || length(n_sub)!=length(regularity) ||
        length(n_sub)!=length(knots)
        error("kntrefine: n_sub, degree and regularity must have the same length
            as the number of knot vectors")
    end
    rknots = Array{Array}(undef, length(knots))
    new_knots = Array{Array}(undef, length(knots))
    zeta = Array{Array}(undef, length(knots))
    for idim = 1:length(n_sub)
        min_mult = degree[idim] - regularity[idim]
        z = unique(knots[idim])
        nz = length(z)
        deg = sum(knots[idim].==z[1])-1
        rknots[idim] = z[ones(Int, degree[idim]+1)]
        new_knots[idim] = [];
        for ik = 2:nz
            insk = LinRange(z[ik-1], z[ik], n_sub[idim]+2)
            insk = vec(repeat(insk[2:end-1], min_mult,1))
            old_mult = sum(knots[idim].==z[ik])
            mult = max(min_mult, degree[idim] - deg + old_mult)

            rknots[idim] = vcat(rknots[idim], insk, z[ik*ones(Int, mult)])
            if mult>=old_mult
                new_knots[idim] = vcat(new_knots[idim], insk, z[ik*ones(Int, mult-old_mult)])
            else
                new_knots[idim] = vcat(new_knots[idim], insk)
            end
        end
        zeta[idim] = unique(rknots[idim])
    end
    return rknots, zeta, new_knots
end

function nrbsquare(corner, lengthx, lengthy, degree, nsubdiv)
    if isempty(corner)
        corner = [0,0]
    end

    if length(degree)==1
        deg = [degree, degree]
    elseif length(degree)==2
        deg = degree
    else
        error("The degree vector should provide the degree in each direction (one or two values)")
    end

    if length(nsubdiv)==1
        nsub = [nsubdiv, nsubdiv]
    elseif length(nsubdiv)==2
        nsub = nsubdiv
    else
        error("The nsubdiv vector should provide the number of intervals in each direction (one or two values)")
    end

    srf = nrb4surf(corner, corner+[lengthx, 0], corner+[0, lengthy], corner+[lengthx, lengthy])
    srf = nrbdegelev(srf, deg-[1, 1])
    _, _, new_knots = kntrefine(srf.knots, nsub.-1, deg, deg - [1, 1] )
    srf = nrbkntins(srf, new_knots)
    return srf
end

"""
Reverse the evaluation direction of a NURBS geometry
Only implemented for curves for now
"""
function nrbreverse(nrb)
    if length(nrb.knots)==1
        #reverse a NURBS curve
        new_knots = sort(nrb.knots[1][end].-nrb.knots[1])
        new_coefs = reverse(nrb.coefs, dims=2)
        new_nrb = nrbmak(new_coefs, [new_knots])
    end
    return new_nrb
end


using LinearAlgebra, Random, JuMP, Ipopt, DataFrames, CSV, Plots, Optim, Statistics
Random.seed!(1);
# FK - 122/156 11.05.22

# ============================================================
# Ler dados
# ============================================================
# Série teste
# n = 100;
# y = rand(n);
# Série real
y = CSV.read("data.csv", DataFrame)[!, "Valor"];
# y = y/1000
n = length(y);
# Formata série
y = [[x] for x in y];

# ============================================================
# MEB com tendência linear local e sazonalidade estocástica
# ============================================================
# Z (1, 13)
Z = [1 0 1 0 1 0 1 0 1 0 1 0 1];
# T (13, 13)
T = zeros(13, 13);
T[1,1] = 1;
T[1,2] = 1;
T[2,2] = 1;
for j in 1:5
    c = [[cos(pi*j/6), -sin(pi*j/6)] [sin(pi*j/6), cos(pi*j/6)]];
    T[2*j+1:2*j+2, 2*j+1:2*j+2] = c
end
T[13, 13] = -1;
# R (13, 13)
R = I;

# ============================================================
# Inicialização
# ============================================================
# Big Kappa
a = Vector{Vector{Real}}(undef, n+1);
a[1] = zeros(13);
P = Vector{Array{Real, 2}}(undef, n+1);
P[1] = diagm([1e6 for _ in 1:13]);

# Resultados armazenados para o smoother
F_aux = Vector{Any}(undef, n);
K_aux = Vector{Any}(undef, n);
V_aux = Vector{Any}(undef, n);

# ============================================================
# Estimação do Filtro de Kalman e cálculo da verossimilhança
# ============================================================
function get_likelihood(var_epsilon::X, var_eta::X, var_csi::X, var_omega::X) where {X}

    # H
    H = [var_epsilon];
    # Q
    Q = zeros(X, 13,13);
    Q[1,1] = var_eta;
    Q[2,2] = var_csi;
    for j in 3:13
        Q[j,j]=var_omega;
    end

    # FK
    likelihood = 0.0;
    for t in 1:n

        F = Z*P[t]*Z' + H;                        # (1,13)*(13,13)*(13,1) + (1,1) = (1,1)
        K = T*P[t]*Z' * ((F)^(-1));               # (13,13)*(13,13)*(13,1)*(1,1) = (13,1)
        V = y[t] - Z*a[t];                        # (1,1) - (1,13)*(13,1) = (1,1)

        a[t+1] = T*a[t] + K*V;                    # (13,13)*(13,1) + (13,1)*(1,1) = (13,1)
        P[t+1] = T*P[t] * (T - K*Z)' + R*Q*R';    # (13,13)*(13,13) * [(13,13) - (13,1)*(1,13)]' + (13,13)*(13,13)*(13,13) = (13,13)

        if t > 13
            likelihood += (-1/2)*log(abs(F[1])) + (-1/2)*(V[1]^2)/F[1];
        end

        # Resultados armazenados para o smoother
        F_aux[t] = F;
        K_aux[t] = K;
        V_aux[t] = V;
    end
    return likelihood
end

function get_likelihood2(des)

    des_epsilon, des_eta, des_csi, des_omega = des
    var_epsilon = des_epsilon^2
    var_eta = des_eta^2
    var_csi = des_csi^2
    var_omega = des_omega^2
    # H
    H = [var_epsilon];
    # Q
    Q = zeros(13,13);
    Q[1,1] = var_eta;
    Q[2,2] = var_csi;
    for j in 3:13
        Q[j,j]=var_omega;
    end

    # FK
    skip = 13
    likelihood = -(n-skip)/2 * log(2*pi);
    for t in 1:n

        F = Z*P[t]*Z' + H;                        # (1,13)*(13,13)*(13,1) + (1,1) = (1,1)
        K = T*P[t]*Z' * ((F)^(-1));               # (13,13)*(13,13)*(13,1)*(1,1) = (13,1)
        V = y[t] - Z*a[t];                        # (1,1) - (1,13)*(13,1) = (1,1)

        a[t+1] = T*a[t] + K*V;                    # (13,13)*(13,1) + (13,1)*(1,1) = (13,1)
        P[t+1] = T*P[t] * (T - K*Z)' + R*Q*R';    # (13,13)*(13,13) * [(13,13) - (13,1)*(1,13)]' + (13,13)*(13,13)*(13,13) = (13,13)

        if t > skip
            likelihood += (-1/2)*log(F[1]) + (-1/2)*(V[1]^2)/F[1];
        end
        # Resultados armazenados para o smoother
        F_aux[t] = F;
        K_aux[t] = K;
        V_aux[t] = V;
    end
    return -likelihood/(n-skip)
end

r = optimize(get_likelihood2, [sqrt(var(y)[1]), sqrt(var(y)[1]), 1.0, 1.0], LBFGS(),Optim.Options(show_trace=true)) #NelderMead

model = Model(Ipopt.Optimizer);

register(model, :get_likelihood, 4, get_likelihood; autodiff = true);

psi = Vector{VariableRef}(undef, 4);
psi[1] = @variable(model, var_epsilon >= 0.0, start = 1.0);
psi[2] = @variable(model, var_eta >= 0.0, start = 1.0);
psi[3] = @variable(model, var_csi >= 0.0, start = 1.0);
psi[4] = @variable(model, var_omega >= 0.0, start = 1.0);
# psi[1] = @variable(model, var_epsilon >= 0.0, start = 2.4371e5);
# psi[2] = @variable(model, var_eta >= 0.0, start = 2.4371e7);
# psi[3] = @variable(model, var_csi >= 0.0, start = 24371.5);
# psi[4] = @variable(model, var_omega >= 0.0, start = 24371.5);
@NLobjective(model, Max, get_likelihood(psi...));

set_optimizer_attribute(model, "max_iter", 200);
set_time_limit_sec(model, 120.0);

JuMP.optimize!(model);

# ============================================================
# FK ótimo
# ============================================================

a_otimo = Vector{Vector{Real}}(undef, n);
a_otimo[1] = zeros(13);
for index in 2:n
    a_otimo[index] = [];
    for i in 1:13
        push!(a_otimo[index], a[index][i]);
    end
end

# ============================================================
# Suavizador
# ============================================================
# Pega dados do FK
F_otimo = [x[1] for x in F_aux];

K_otimo = [[y for y in x] for x in K_aux];

V_otimo = zeros(n);
V_otimo[1] = V_aux[1][1];
V_otimo[2:end] = [x[1] for x in V_aux[2:end]];

P_otimo = P;
P_otimo[2:end] = [[y for y in x] for x in P[2:end]];

# Suavizador
r = Vector{Vector{Real}}(undef, n+1);
r[end] = zeros(13);
N = Vector{Array{Real, 2}}(undef, n+1);
N[end] = zeros(13,13);

for t in n:-1:1

    L = T - K_otimo[t]*Z;                                               # (13,13) - (13,1)*(1,13) = (13,13)
    r[t] = vec(Z' * ((F_otimo[t])^(-1)) * V_otimo[t] + L'*r[t+1]);      # (13,1)*(1,1)*(1,1) + (13,13)*(13,1) = (13,1)

    N[t] = Z' * ((F_otimo[t])^(-1)) * Z + L' * N[t+1] * L;              # (13,1)*(1,1)*(1,13) + (13,13)*(13,13)*(13,13) = (13,13)

end
for t in 1:n
    alpha_hat[t] = a_otimo[t] + P_otimo[t+1]*r[t];                      # (13,1) + (13,13)*(13,1) = (13,1)
    V_true = P_otimo[t+1] - P_otimo[t+1]*N[t]*P_otimo[t+1];  # (13,13) - (13,13)*(13,13)*(13,13) = (13,13)
end
# alpha_hat = a_otimo + P_otimo[2:end]*r[1:end-1];                      # (13,1) + (13,13)*(13,1) = (13,1)
# V_true = P_otimo[2:end] - P_otimo[2:end]*N[1:end-1]*P_otimo[2:end];  # (13,13) - (13,13)*(13,13)*(13,13) = (13,13)

# ============================================================
# Plot
# ============================================================

# Y
plot([x[1] for x in y], label = "Real")
plot!([(Z*x)[1] for x in a_otimo], label = "FK")
plot!([(Z*x)[1] for x in alpha_hat], label = "Smoother")

psi_val = value.(psi);
println("Level: $(psi_val[2])")
println("Slope: $(psi_val[3])")
println("Seasonal: $(psi_val[4])")
println("Irregular: $(psi_val[1])")

if false

    # Seasonality
    plot([(Z[:, 3:end]*x[3:end])[1] for x in alpha_hat], label = "Seasonality")

    # Level
    plot([x[1] for x in y], label = "Real")
    plot!([(Z[:, 1]*x[1])[1] for x in alpha_hat], label = "Level")

    # Slope
    plot([x[2] for x in alpha_hat], label = "Slope")
end


# Box-Muller test
Nsim = 500000000

function f(N)
    for i = 1:N
        r = rand(2)
        theta = 2*pi*r[1]
        r2 = -2*log(r[2])

        x = sqrt(r2)*cos(theta)
        y = sqrt(r2)*sin(theta)
    end
end

@time f(Nsim)


using NumCME
using Test

p1 = propensity((x,p) -> x[2])
@test !istimevarying(p1)

p2 = propensity((t, x,p) -> (1.0+sin(t))*x[1]*x[2])
@test istimevarying(p2)
@test !istimeseparable(p2)

p3 = propensity((x,p)->x[1]*x[2], (t,p) -> (1.0+sin(t)))
@test istimevarying(p3)
@test istimeseparable(p3)


function exponential!(H, g, x)
    fx = exp((2.0 - x[1])^2) + exp((3.0 - x[2])^2)

    if is_something(g)
        g[1] = -2.0 * (2.0 - x[1]) * exp((2.0 - x[1])^2)
        g[2] = -2.0 * (3.0 - x[2]) * exp((3.0 - x[2])^2)
    end
    if is_somthing(H)
        H[1, 1] = 2.0 * exp((2.0 - x[1])^2) * (2.0 * x[1]^2 - 8.0 * x[1] + 9)
        H[1, 2] = 0.0
        H[2, 1] = 0.0
        H[2, 2] = 2.0 * exp((3.0 - x[2])^2) * (2.0 * x[2]^2 - 12.0 * x[2] + 19)
    end
    objective_return(fx, g, H)
end
exponential_prb = Dict()
exponential_prb["twicediffed!"] = TwiceDiffed(exponential!)
exponential_prb["oncediffed!"] = OnceDiffed(exponential!)
exponential_prb["initial_x"] = [0.0, 0.0]
exponential_prb["minimizer"] = [2.0, 3.0]
exponential_prb["minimum"] = exponential!(nothing, nothing, [2.0, 3.0])
problems["unconstrained"]["exponential"] = exponential_prb

function fletcher_powell!(∇f, x)
    function theta(x)
        if x[1] > 0
            return atan(x[2] / x[1]) / (2.0 * pi)
        else
            return (pi + atan(x[2] / x[1])) / (2.0 * pi)
        end
    end

    if ( x[1]^2 + x[2]^2 == zero(eltype(x)) )
        dtdx1 = zero(eltype(x))
        dtdx2 = zero(eltype(x))
    else
        dtdx1 = - x[2] / ( 2.0 * pi * ( x[1]^2 + x[2]^2 ) )
        dtdx2 =   x[1] / ( 2.0 * pi * ( x[1]^2 + x[2]^2 ) )
    end

    fx = 100.0 * ((x[3] - 10.0 * theta(x))^2 +
        (sqrt(x[1]^2 + x[2]^2) - 1.0)^2) + x[3]^2

    if is_something(∇f)
        ∇f[1] = -2000.0*(x[3]-10.0*theta(x))*dtdx1 +
            200.0*(sqrt(x[1]^2+x[2]^2)-1.0)*x[1]/sqrt( x[1]^2+x[2]^2 )
        ∇f[2] = -2000.0*(x[3]-10.0*theta(x))*dtdx2 +
            200.0*(sqrt(x[1]^2+x[2]^2)-1.0)*x[2]/sqrt( x[1]^2+x[2]^2 )
        ∇f[3] =  200.0*(x[3]-10.0*theta(x)) + 2.0*x[3]
        return fx, ∇f
    end
    return fx
end

fletcher_powell_prb = Dict()
fletcher_powell_prb["twicediffed!"] = TwiceDiffed(fletcher_powell!)
fletcher_powell_prb["oncediffed!"] = OnceDiffed(fletcher_powell!)
fletcher_powell_prb["initial_x"] = [-1.0,0.0,0.0]
fletcher_powell_prb["minimizer"] = [1.0,0.0,0.0]
fletcher_powell_prb["minimum"] = fletcher_powell!(nothing, nothing, [1.0,0.0,0.0])
problems["unconstrained"]["fletcher_powell"] = fletcher_powell_prb

function himmelblau!(x, g, H)
  fx = (x[1]^2 + x[2] - 11)^2 + (x[1] + x[2]^2 - 7)^2

  if !isnothing(g)
    g[1] = 4.0 * x[1]^3 + 4.0 * x[1] * x[2] -
    44.0 * x[1] + 2.0 * x[1] + 2.0 * x[2]^2 - 14.0
    g[2] = 2.0 * x[1]^2 + 2.0 * x[2] - 22.0 +
    4.0 * x[1] * x[2] + 4.0 * x[2]^3 - 28.0 * x[2]
  end
  if !isnothing(H)
    H[1, 1] = 12.0 * x[1]^2 + 4.0 * x[2] - 42.0
    H[1, 2] = 4.0 * x[1] + 4.0 * x[2]
    H[2, 1] = 4.0 * x[1] + 4.0 * x[2]
    H[2, 2] = 12.0 * x[2]^2 + 4.0 * x[1] - 26.0
  end
  objective_return(fx, g, H)
end

himmelblau_prb = Dict()
himmelblau_prb["twicediffed!"] = TwiceDiffed(himmelblau!)
himmelblau_prb["oncediffed!"] = OnceDiffed(himmelblau!)
himmelblau_prb["initial_x"] = [2.0, 2.0]
himmelblau_prb["minimizer"] = [3.0, 2.0]
himmelblau_prb["minimum"] = himmelblau_!(nothing, nothing, [3.0, 2.0])
problems["unconstrained"]["himmelblau"] = himmelblau_prb

function hosaki!(H, ∇f, x)
    a = (1.0 - 8.0 * x[1] + 7.0 * x[1]^2 - (7.0 / 3.0) * x[1]^3 + (1.0 / 4.0) * x[1]^4)
    fx = a * x[2]^2 * exp(-x[2])

    if !(∇f isa Nothing)
    ∇f[1] = (x[1]^3 - 7.0 * x[1]^2 + 14.0 * x[1] - 8)* x[2]^2 * exp(-x[2])
    ∇f[2] = 2.0 * (1.0 - 8.0 * x[1] + 7.0 * x[1]^2 - (7.0 / 3.0) * x[1]^3 + (1.0 / 4.0) * x[1]^4) * x[2] * exp(-x[2]) - (1.0 - 8.0 * x[1] + 7.0 * x[1]^2 - (7.0 / 3.0) * x[1]^3 + (1.0 / 4.0) * x[1]^4) * x[2]^2 * exp(-x[2])
    end
    if !(H isa Nothing)
    H[1, 1] = (3.0 * x[1]^2 - 14.0 * x[1] + 14.0) * x[2]^2 * exp(-x[2])
    H[1, 2] = 2.0 * (x[1]^3 - 7.0 * x[1]^2 + 14.0 * x[1] - 8.0) * x[2] * exp(-x[2])  - (x[1]^3 - 7.0 * x[1]^2 + 14.0 * x[1] - 8.0) * x[2]^2 * exp(-x[2])
    H[2, 1] =  2.0 * (x[1]^3 - 7.0 * x[1]^2 + 14.0 * x[1] - 8.0) * x[2] * exp(-x[2])  - (x[1]^3 - 7.0 * x[1]^2 + 14.0 * x[1] - 8.0) * x[2]^2 * exp(-x[2])
    H[2, 2] = 2.0 * (1.0 - 8.0 * x[1] + 7.0 * x[1]^2 - (7.0 / 3.0) * x[1]^3 + (1.0 / 4.0) * x[1]^4) * exp(-x[2]) - 4.0 * ( 1.0 - 8.0 * x[1] + 7.0 *  x[1]^2 - (7.0 / 3.0) * x[1]^3 + (1.0 / 4.0) * x[1]^4) * x[2] * exp(-x[2]) + (1.0 - 8.0 * x[1] + 7.0 * x[1]^2 - (7.0 / 3.0) * x[1]^3 + (1.0 / 4.0) * x[1]^4) * x[2]^2 * exp(-x[2])
    end
    objective_return(fx, ∇f, H)
end
td_obj = TwiceDiffed(hosaki!)

hosaki_prb = Dict()
hosaki_prb["twicediffed!"] = TwiceDiffed(hosaki!)
hosaki_prb["oncediffed!"] = OnceDiffed(hosaki!)
hosaki_prb["initial_x"] = [3.6, 1.9]
hosaki_prb["minimizer"] = [4.0, 2.0]
hosaki_prb["minimum"] = hosaki!(nothing, nothing, [4.0, 2.0])
problems["unconstrained"]["hosaki"] = hosaki_prb

function large_polynomial!(H, ∇f, x)
    fx = zero(x[1])
    for i in 1:250
        fx += (i - x[i])^2
    end
    if !(∇f isa Nothing)
        for i in 1:250
            ∇f[i] = -2.0 * (i - x[i])
        end
    end
    if !(H isa Nothing)
        for i in 1:250
            for j in i:250
                if i == j
                    H[i, j] = 2.0
                else
                    H[i, j] = 0.0
                    H[j, i] = 0.0
                end
            end
        end
    end
    objective_return(fx, ∇f, H)
end

td_obj = TwiceDiffed(large_polynomial!)
large_polynomial = Dict()
large_polynomial["twicediffed!"] = TwiceDiffed(large_polynomial!)
large_polynomial["oncediffed!"] = OnceDiffed(large_polynomial!)
large_polynomial["initial_x"] = zeros(250)
large_polynomial["minimizer"] = collect(float(1:250))
large_polynomial["minimum"] = large_polynomial!(nothing, nothing, collect(float(1:250)))
problems["unconstrained"]["large_polynomial"] = large_polynomial

function parabola!(H, ∇f, x)
    fx =  (1.0 - x[1])^2 + (2.0 - x[2])^2 + (3.0 - x[3])^2 +
        (5.0 - x[4])^2 + (8.0 - x[5])^2

    if !(∇f isa Nothing)
        ∇f[1] = -2.0 * (1.0 - x[1])
        ∇f[2] = -2.0 * (2.0 - x[2])
        ∇f[3] = -2.0 * (3.0 - x[3])
        ∇f[4] = -2.0 * (5.0 - x[4])
        ∇f[5] = -2.0 * (8.0 - x[5])
    end
    if !(H isa Nothing)
        for i in 1:5
            for j in 1:5
                if i == j
                    H[i, j] = 2.0
                else
                    H[i, j] = 0.0
                end
            end
        end
    end
    objective_return(fx, ∇f, H)
end

parabola = Dict()
parabola["twicediffed!"] = TwiceDiffed(parabola!)
parabola["oncediffed!"] = OnceDiffed(parabola!)
parabola["initial_x"] = zeros(5)
parabola["minimizer"] = [1.0, 2.0, 3.0, 5.0, 8.0]
parabola["minimum"] = parabola!(nothing, nothing, [1.0, 2.0, 3.0, 5.0, 8.0])
problems["unconstrained"]["parabola"] = parabola

function polynomial!(H, ∇f, x)
    fx  = (10.0 - x[1])^2 + (7.0 - x[2])^4 + (108.0 - x[3])^4
    if !(∇f isa Nothing)
        ∇f[1] = -2.0 * (10.0 - x[1])
        ∇f[2] = -4.0 * (7.0 - x[2])^3
        ∇f[3] = -4.0 * (108.0 - x[3])^3
    end
    if !(H isa Nothing)
        H[1, 1] = 2.0
        H[1, 2] = 0.0
        H[1, 3] = 0.0
        H[2, 1] = 0.0
        H[2, 2] = 12.0 * (7.0 - x[2])^2
        H[2, 3] = 0.0
        H[3, 1] = 0.0
        H[3, 2] = 0.0
        H[3, 3] = 12.0 * (108.0 - x[3])^2
    end
    objective_return(fx, ∇f, H)
end

polynomial = Dict()
polynomial["twicediffed!"] = TwiceDiffed(polynomial!)
polynomial["oncediffed!"] = OnceDiffed(polynomial!)
polynomial["initial_x"] = zeros(3)
polynomial["minimizer"] = [10.0, 7.0, 108.0]
polynomial["minimum"] = polynomial!(nothing, nothing, [10.0, 7.0, 108.0])
problems["unconstrained"]["polynomial"] = polynomial

function powell!(H, ∇f, x)
    fx = (x[1] + 10.0 * x[2])^2 + 5.0 * (x[3] - x[4])^2 +
    (x[2] - 2.0 * x[3])^4 + 10.0 * (x[1] - x[4])^4
    if !(∇f isa Nothing)
        ∇f[1] = 2.0 * (x[1] + 10.0 * x[2]) + 40.0 * (x[1] - x[4])^3
        ∇f[2] = 20.0 * (x[1] + 10.0 * x[2]) + 4.0 * (x[2] - 2.0 * x[3])^3
        ∇f[3] = 10.0 * (x[3] - x[4]) - 8.0 * (x[2] - 2.0 * x[3])^3
        ∇f[4] = -10.0 * (x[3] - x[4]) - 40.0 * (x[1] - x[4])^3
    end
    if !(H isa Nothing)
        H[1, 1] = 2.0 + 120.0 * (x[1] - x[4])^2
        H[1, 2] = 20.0
        H[1, 3] = 0.0
        H[1, 4] = -120.0 * (x[1] - x[4])^2
        H[2, 1] = 20.0
        H[2, 2] = 200.0 + 12.0 * (x[2] - 2.0 * x[3])^2
        H[2, 3] = -24.0 * (x[2] - 2.0 * x[3])^2
        H[2, 4] = 0.0
        H[3, 1] = 0.0
        H[3, 2] = -24.0 * (x[2] - 2.0 * x[3])^2
        H[3, 3] = 10.0 + 48.0 * (x[2] - 2.0 * x[3])^2
        H[3, 4] = -10.0
        H[4, 1] = -120.0 * (x[1] - x[4])^2
        H[4, 2] = 0.0
        H[4, 3] = -10.0
        H[4, 4] = 10.0 + 120.0 * (x[1] - x[4])^2
    end
    objective_return(fx, ∇f, H)
end

powell = Dict()
powell["twicediffed!"] = TwiceDiffed(powell!)
powell["oncediffed!"] = OnceDiffed(powell!)
powell["initial_x"] = [-3.0, -1.0, 0.0, 1.0]
powell["minimizer"] = zeros(4)
powell["minimum"] = powell!(nothing, nothing, zeros(4))
problems["unconstrained"]["powell"] = powell

function rosenbrock!(H, ∇f, x::Vector)
    fx = (1.0 - x[1])^2 + 100.0 * (x[2] - x[1]^2)^2
    if is_something(∇f)
    ∇f[1] = -2.0 * (1.0 - x[1]) - 400.0 * (x[2] - x[1]^2) * x[1]
    ∇f[2] = 200.0 * (x[2] - x[1]^2)
    end
    if is_something(H)
    H[1, 1] = 2.0 - 400.0 * x[2] + 1200.0 * x[1]^2
    H[1, 2] = -400.0 * x[1]
    H[2, 1] = -400.0 * x[1]
    H[2, 2] = 200.0
    end
    objective_return(fx, ∇f, H)
end

rosenbrock = Dict()
rosenbrock["twicediffed!"] = TwiceDiffed(rosenbrock!)
rosenbrock["oncediffed!"] = OnceDiffed(rosenbrock!)
rosenbrock["initial_x"] = [-1.2, 1.0]
rosenbrock["minimizer"] = ones(2)
rosenbrock["minimum"] = rosenbrock!(nothing, nothing, ones(4))
problems["unconstrained"]["rosenbrock"] = rosenbrock

function run_all(obj, x0, approx, globals)
    res = []
    _res = minimize!(obj, copy(x0), approx)
    push!(res, _res)
    for glob in globals
      _res = minimize!(obj, copy(x0), (approx, glob))
      printstyled(_res; color=maximum(abs, _res[3]) ≤ sqrt(eps(Float64)) ? :green : :red);println()
      push!(res, _res)
    end
    res
end

direct_tr = (NWI(), Dogleg())
direct_globals = (HZAW(), Backtracking(), Backtracking(;interp=FFQuadInterp()), NWI(), Dogleg())
inverse_ls = (HZAW(), Backtracking(), Backtracking(; interp=FFQuadInterp()))
inverse_globals = (HZAW(), Backtracking(), Backtracking(;interp=FFQuadInterp()))
run_all(od_obj, x0, BFGS(Inverse()), inverse_globals)
run_all(od_obj, x0, DBFGS(Inverse()), inverse_globals)
#run_all(od_obj, x0, SR1(Inverse()), inverse_ls)
run_all(od_obj, x0, DFP(Inverse()), inverse_globals)
run_all(od_obj, x0, BFGS(Direct()), direct_globals)
run_all(od_obj, x0, DBFGS(Direct()), direct_globals)
run_all(od_obj, x0, SR1(Direct()), direct_globals)
run_all(od_obj, x0, DFP(Direct()), direct_globals)

run_all(td_obj, x0, Newton(), direct_globals)
run_all(td_obj, x0, Newton(), direct_globals)

#==
  Flether-Powell
==#

run_all(od_obj, x0, BFGS(Inverse()), inverse_globals)
run_all(od_obj, x0, DBFGS(Inverse()), inverse_globals)
run_all(od_obj, x0, SR1(Inverse()), inverse_ls)
run_all(od_obj, x0, DFP(Inverse()), inverse_globals)
run_all(od_obj, x0, BFGS(Direct()), direct_globals)
run_all(od_obj, x0, DBFGS(Direct()), direct_globals)
run_all(od_obj, x0, SR1(Direct()), direct_globals)
run_all(od_obj, x0, DFP(Direct()), direct_globals)

function himmelblau!(H, g, x::Vector)
    fx = (x[1]^2 + x[2] - 11)^2 + (x[1] + x[2]^2 - 7)^2

    if !(g isa Nothing)
    g[1] = 4.0 * x[1]^3 + 4.0 * x[1] * x[2] -
    44.0 * x[1] + 2.0 * x[1] + 2.0 * x[2]^2 - 14.0
    g[2] = 2.0 * x[1]^2 + 2.0 * x[2] - 22.0 +
    4.0 * x[1] * x[2] + 4.0 * x[2]^3 - 28.0 * x[2]
    end
    if !(H isa Nothing)
    H[1, 1] = 12.0 * x[1]^2 + 4.0 * x[2] - 42.0
    H[1, 2] = 4.0 * x[1] + 4.0 * x[2]
    H[2, 1] = 4.0 * x[1] + 4.0 * x[2]
    H[2, 2] = 12.0 * x[2]^2 + 4.0 * x[1] - 26.0
    end
    objective_return(fx, g, H)
end
x0 = [2.0, 2.0]
xopt = [3.0, 2.0]
od_obj = OnceDiffed(himmelblau!)
td_obj = TwiceDiffed(himmelblau!)

run_all(td_obj, x0, BFGS(Inverse()), inverse_globals);
run_all(td_obj, x0, DBFGS(Inverse()), inverse_globals);
run_all(td_obj, x0, SR1(Inverse()), inverse_ls);
run_all(td_obj, x0, DFP(Inverse()), inverse_globals);
run_all(td_obj, x0, BFGS(Direct()), direct_globals);
run_all(td_obj, x0, DBFGS(Direct()), direct_globals);
run_all(td_obj, x0, SR1(Direct()), direct_globals);
run_all(td_obj, x0, DFP(Direct()), direct_globals);
run_all(td_obj, x0, Newton(), direct_globals);

function hosaki!(H, ∇f, x)
    a = (1.0 - 8.0 * x[1] + 7.0 * x[1]^2 - (7.0 / 3.0) * x[1]^3 + (1.0 / 4.0) * x[1]^4)
    fx = a * x[2]^2 * exp(-x[2])

    if !(∇f isa Nothing)
    ∇f[1] = (x[1]^3 - 7.0 * x[1]^2 + 14.0 * x[1] - 8)* x[2]^2 * exp(-x[2])
    ∇f[2] = 2.0 * (1.0 - 8.0 * x[1] + 7.0 * x[1]^2 - (7.0 / 3.0) * x[1]^3 + (1.0 / 4.0) * x[1]^4) * x[2] * exp(-x[2]) - (1.0 - 8.0 * x[1] + 7.0 * x[1]^2 - (7.0 / 3.0) * x[1]^3 + (1.0 / 4.0) * x[1]^4) * x[2]^2 * exp(-x[2])
    end
    if !(H isa Nothing)
    H[1, 1] = (3.0 * x[1]^2 - 14.0 * x[1] + 14.0) * x[2]^2 * exp(-x[2])
    H[1, 2] = 2.0 * (x[1]^3 - 7.0 * x[1]^2 + 14.0 * x[1] - 8.0) * x[2] * exp(-x[2])  - (x[1]^3 - 7.0 * x[1]^2 + 14.0 * x[1] - 8.0) * x[2]^2 * exp(-x[2])
    H[2, 1] =  2.0 * (x[1]^3 - 7.0 * x[1]^2 + 14.0 * x[1] - 8.0) * x[2] * exp(-x[2])  - (x[1]^3 - 7.0 * x[1]^2 + 14.0 * x[1] - 8.0) * x[2]^2 * exp(-x[2])
    H[2, 2] = 2.0 * (1.0 - 8.0 * x[1] + 7.0 * x[1]^2 - (7.0 / 3.0) * x[1]^3 + (1.0 / 4.0) * x[1]^4) * exp(-x[2]) - 4.0 * ( 1.0 - 8.0 * x[1] + 7.0 *  x[1]^2 - (7.0 / 3.0) * x[1]^3 + (1.0 / 4.0) * x[1]^4) * x[2] * exp(-x[2]) + (1.0 - 8.0 * x[1] + 7.0 * x[1]^2 - (7.0 / 3.0) * x[1]^3 + (1.0 / 4.0) * x[1]^4) * x[2]^2 * exp(-x[2])
    end
    objective_return(fx, ∇f, H)
end
td_obj = TwiceDiffed(hosaki!)

x0 = [3.6, 1.9]
xopt = [4.0, 2.0]
run_all(td_obj, x0, BFGS(Inverse()), inverse_globals);
run_all(td_obj, x0, DBFGS(Inverse()), inverse_globals);
run_all(td_obj, x0, SR1(Inverse()), inverse_ls);
run_all(td_obj, x0, DFP(Inverse()), inverse_globals);
run_all(td_obj, x0, BFGS(Direct()), direct_globals);
run_all(td_obj, x0, DBFGS(Direct()), direct_globals);
run_all(td_obj, x0, SR1(Direct()), direct_globals);
run_all(td_obj, x0, DFP(Direct()), direct_globals);
run_all(td_obj, x0, Newton(), direct_globals);

examples["Hosaki"] = OptimizationProblem("Hosaki",
                                         hosaki,
                                         hosaki_gradient!,
                                         nothing,
                                         hosaki_hessian!,
                                         nothing, # Constraints
                                         hosaki([4.0, 2.0]),
                                         true,
                                         true)


function motion(v,α,dt,m₀,μ,ξ,Ω,graph)

    n = length(ξ)
    rnoise = diagm(0 => [8e-4,8e-4,1e-6])
    jcb = jacobian_motion(μ[3],v,α,dt)

    ψ = inv(jcb)-I
    ind = [[2*i+2;2*i+3] for i ∈ sort(m₀)]
    ind = vcat(ind...)
    ind = [1:3;ind]
    Ω₁ = Ω[ind,1:3]
    ψ₁ = Ω₁*ψ

    λ = zeros(n,n)
    λ[ind,1:3] = ψ₁
    λ[1:3,ind] .+= ψ₁'
    λ[1:3,1:3] .+= ψ'*Ω[1:3,1:3]*ψ
    Φ = Ω + λ

    Φₓ = Φ[1:3,ind]
    κ = zeros(n,n)
    κ[ind,ind] = Φₓ'*inv(inv(rnoise)+Φ[1:3,1:3])*Φₓ
    Ω .+= λ - κ

    δ = equation_motion(μ[3],v,α,dt)
    ξ[ind] .+= Ω[ind,1:3]*δ + (λ-κ)[ind,:]*μ
    μ[1:3] += δ

    if length(m₀)>0
        edges = [[i+1 j+1] for i ∈ m₀,j ∈ m₀ if i ≠ j]
        edges = vcat(edges...)
        if size(edges,1)>0
            sg = size(graph,1)
            adjacency = sparse(edges[:,1],edges[:,2],true,sg,sg)
            graph .|= adjacency
        end
    end
end


abstract MultivariateFun{T,N}
typealias BivariateFun{T} MultivariateFun{T,2}

export grad, lap, curl

#implements coefficients/values/evaluate
space{T,N}(f::MultivariateFun{T,N})=mapreduce(k->space(f,k),⊗,1:N)
domain{T,N}(f::MultivariateFun{T,N})=mapreduce(k->domain(f,k),*,1:N)

domain(f::MultivariateFun,k::Integer)=domain(space(f,k))

differentiate(u::BivariateFun,i::Integer,j::Integer)=j==0?u:differentiate(differentiate(u,i),i,j-1)
grad(u::BivariateFun)=[differentiate(u,1),differentiate(u,2)]
lap(u::BivariateFun)=differentiate(u,1,2)+differentiate(u,2,2)
Base.div{B<:BivariateFun}(u::Vector{B})=differentiate(u[1],1)+differentiate(u[2],2)
curl{B<:BivariateFun}(u::Vector{B})=differentiate(u[2],1)-differentiate(u[1],2)

Base.chop(f::MultivariateFun)=chop(f,10eps())
Base.eltype{T}(::MultivariateFun{T})=T
Base.eltype{T,N}(::Type{MultivariateFun{T,N}})=T
Base.eltype{MF<:MultivariateFun}(::Type{MF})=eltype(supertype(MF))

include("VectorFun.jl")
include("ProductDomain.jl")
include("TensorSpace.jl")
include("LowRankFun.jl")
include("ProductFun.jl")

arglength(f)=length(Base.uncompressed_ast(f.code.def).args[1])

## Convert between Fun and MultivariateFun
# need to chop extra zeros
Fun(f::ProductFun) =
    Fun(chop!(fromtensor(space(f),coefficients(f)),0),space(f))
Fun(f::ProductFun,sp::TensorSpace) = Fun(ProductFun(f,sp))
Fun(f::LowRankFun) = Fun(ProductFun(f))

Fun(f::MultivariateFun,sp::Space)=Fun(Fun(f),sp)

Fun(f,d1::Domain,d2::Domain)=Fun(f,d1*d2)
Fun{T<:Number,V<:Number}(f,d1::Vector{T},d2::Vector{V})=Fun(f,convert(Domain,d1),convert(Domain,d2))

coefficients(f::BivariateFun,sp::TensorSpace)=coefficients(f,sp[1],sp[2])

points(f::BivariateFun,k...)=points(space(f),size(f,1),size(f,2),k...)

function *(vx::LowRankFun,u0::ProductFun)
    ret=zeros(space(u0))
    for k=1:length(vx.A)
        a,b=vx.A[k],vx.B[k]
        ret+=((b*((a*u0).')).')
    end
    ret
end

*(a::ProductFun,b::LowRankFun)=b*a
*(a::MultivariateFun,b::MultivariateFun)=LowRankFun(a)*ProductFun(b)

for OP in (:+,:-,:*,:/)
    @eval begin
        $OP(f::Fun,g::MultivariateFun)=$OP(ProductFun(f),g)
        $OP(f::MultivariateFun,g::Fun)=$OP(f,ProductFun(g))
    end
end

Base.sum{TS<:TensorSpace}(f::Fun{TS},k::Integer)=sum(ProductFun(f),k)
Base.sum{TS<:TensorSpace}(f::Fun{TS})=sum(ProductFun(f))

## kron

function Base.kron(f::Fun,g::Fun)
    sp=space(f)⊗space(g)
    it=tensorizer(sp)
    N=ncoefficients(f);M=ncoefficients(g)
    cfs=Vector{promote_type(eltype(f),eltype(g))}()
    for (k,j) in it
        # Tensor product is N x M, so if we are outside
        # the (N+M)th diagonal we have no more entries
        if k+j > N+M
            break
        elseif k ≤ N && j ≤ M
            push!(cfs,f.coefficients[k]*g.coefficients[j])
        else
            push!(cfs,0)
        end
    end
    Fun(cfs,sp)
end
Base.kron(f::Fun,g::Number) = kron(f,Fun(g))
Base.kron(f::Number,g::Fun) = kron(Fun(f),g)


module _cXMSEn
export cXMSEn
using Statistics: std, mean, median, var
using Plots
    """
        MSx, CI = cXMSEn(Sig, Mobj)

    Returns a vector of composite multiscale cross-entropy values (`MSx`)
    between two univariate data sequences contained in `Sig` using the
    parameters specified by the multiscale object (`Mobj`) using the composite
    multiscale method (cMSE) over 3 temporal scales.

        MSx, CI = cXMSEn(Sig::AbstractArray{T,2} where T<:Real, Mobj::NamedTuple;
                              Scales::Int=3, RadNew::Int=0, Refined::Bool=false, Plotx::Bool=false)

    Returns a vector of composite multiscale cross-entropy values (`MSx`)
    between the data sequences contained in `Sig` using the parameters
    specified by the multiscale object (`Mobj`) and the following keyword arguments:

    # Arguments:
    `Scales`   - Number of temporal scales, an integer > 1   (default: 3)\n
    `RadNew`   - Radius rescaling method, an integer in the range [1 4].
                 When the entropy specified by Mobj is `XSampEn` or `XApEn`,
                 RadNew rescales the radius threshold of the sub-sequences
                 at each time scale (Ykj). If a radius value is specified by
                 `Mobj` (`r`), this becomes the rescaling coefficient, otherwise
                 it is set to 0.2 (default). The value of RadNew specifies
                 one of the following methods:\n
                 [1]    Standard Deviation          - r*std(Ykj)\n
                 [2]    Variance                    - r*var(Ykj)\n
                 [3]    Mean Absolute Deviation     - r*mean_ad(Ykj)\n
                 [4]    Median Absolute Deviation   - r*med_ad(Ykj,1)\n
    `Refined`  - Refined-composite XMSEn method. When `Refined` == true and the
                 entropy function specified by `Mobj` is 'XSampEn', cXMSEn
                 returns the refined-composite multiscale entropy (rcXMSEn).
                 (default: false)
    `Plotx`    - When `Plotx` == true, returns a plot of the entropy value at each
                 time scale (i.e. the multiscale entropy curve) [default: false]

    # See also `MSobject`, `XMSEn`, `rXMSEn`, `hXMSEn`, `XSampEn`, `XApEn`, `cMSEn`

    # References:
        [1] Rui Yan, Zhuo Yang, and Tao Zhang,
            "Multiscale cross entropy: a novel algorithm for analyzing two
            time series."
            5th International Conference on Natural Computation.
            Vol. 1, pp: 411-413 IEEE, 2009.

        [2] Yi Yin, Pengjian Shang, and Guochen Feng,
            "Modified multiscale cross-sample entropy for complex time
            series."
            Applied Mathematics and Computation
            289 (2016): 98-110.

        [3] Madalena Costa, Ary Goldberger, and C-K. Peng,
            "Multiscale entropy analysis of complex physiologic time series."
            Physical review letters
            89.6 (2002): 068102.

        [4] Antoine Jamin, et al,
            "A novel multiscale cross-entropy method applied to navigation
            data acquired with a bike simulator."
            41st annual international conference of the IEEE EMBC
            IEEE, 2019.

        [5] Antoine Jamin and Anne Humeau-Heurtier.
            "(Multiscale) Cross-Entropy Methods: A Review."
            Entropy
            22.1 (2020): 45.

        [6] Shuen-De Wu, et al.,
            "Time series analysis using composite multiscale entropy."
            Entropy
            15.3 (2013): 1069-1084.

    """
    function cXMSEn(Sig::AbstractArray{T,2} where T<:Real, Mobj::NamedTuple;
        Scales::Int=3, RadNew::Int=0, Refined::Bool=false, Plotx::Bool=false)

    size(Sig,1) == 2 ? Sig = Sig' : nothing

    (size(Sig,1)>10) ? nothing : error("Sig:   must be a numeric vector" )
    (length(Mobj) >= 1) ? nothing :  error("Mobj:    must be a multiscale entropy
            object created with the function EntropyHub.MSobject")
    (Scales>1) ? nothing : error("Scales:     must be an integer > 1")
    (RadNew==0 || (RadNew in 1:4 && String(Symbol(Mobj.Func)) in ("XSampEn","XApEn"))) ?
            nothing : error("RadNew:     must be 0, or an integer in range [1 4] with
                entropy function 'XSampEn' or 'XApEn'")

    MSx = zeros(Scales)
    Args = NamedTuple{keys(Mobj)[2:end]}(Mobj)
    if RadNew > 0
        if RadNew == 1
            Rnew = x -> std(x, corrected=false)
        elseif RadNew == 2
            Rnew = x -> var(x, corrected=false)
        elseif RadNew == 3
            Rnew = x -> mean(abs.(x .- mean(x)))
        elseif RadNew == 4
            Rnew = x -> median(abs.(x .- median(x)))
        end

        if haskey(Mobj,:r)
            Cx = Mobj.r
        else
            Cy = ("Standard Deviation","Variance","Mean Abs Deviation",
                    "Median Abs Deviation")
            @warn("No radius value provided in Mobj.
                Default set to 0.2*$(Cy[RadNew]) of each new time-series.")
            Cx = .2
        end
    end

    for T = 1:Scales
        Temp = modified(Sig,T)
        N = Int(T*floor(size(Temp,1)/T))
        Temp3 = zeros(T)
        Temp2 = zeros(T)

        for k = 1:T
            print(" .")
            RadNew > 0 ? Args = (Args..., r=Cx*Rnew(Temp[k:T:N,:])) : nothing

            if Refined == 1
                _, Ma, Mb = Mobj.Func(Temp[k:T:N,:]; Args...)
                Temp2[k] = Ma[end]
                Temp3[k] = Mb[end]
            else
                Temp2 = Mobj.Func(Temp[k:T:N,:]; Args...)
                typeof(Temp2)<:Tuple ? Temp3[k] = Temp2[1][end] : Temp3[k] = Temp2[end]
                #Temp3[k] = Temp2[1][end]
            end
        end

        Refined == 1 ? MSx[T] = -log(sum(Temp2)/sum(Temp3)) : MSx[T] = mean(Temp3)
    end
    CI = sum(MSx)
    print("\n")
    if any(isnan.(MSx))
        println("Some entropy values may be undefined.")
    end

    if Plotx
        Refined ? strx = "Refined-Composite" : strx = "Composite"

        p1 = plot(1:Scales, MSx, c=RGB(8/255, 63/255, 77/255), lw=3)
        scatter!(1:Scales, MSx, markersize=6, c=RGB(1, 0, 1),
        xlabel = "Scale Factor", ylabel = "Entropy Value",
        guidefont = font(12, "arial", RGB(7/255, 54/255, 66/255)),
        tickfontsize = 10, tickfontfamily="arial", legend=false,
        title = strx*" Multiscale $(Mobj.Func)",
        plot_titlefontsize=16, plot_titlefontcolor=RGB(7/255, 54/255, 66/255))
        display(p1)
    end

    return MSx, CI
    end

    function modified(Z,sx)
        Ns = size(Z,1) - sx + 1
        Y = zeros(Ns,2)
        for k = 1:Ns
            Y[k,1] = mean(Z[k:k+sx-1,1])
            Y[k,2] = mean(Z[k:k+sx-1,2])
        end
        return Y
    end

end

"""
Copyright 2021 Matthew W. Flood, EntropyHub

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.

For Terms of Use see https://github.com/MattWillFlood/EntropyHub
"""


@testset "Utils" begin
    De, Dx = 3, 2
    g = Flux.batch([GNNGraph(erdos_renyi(10, 30),
                             ndata=rand(Dx, 10),
                             edata=rand(De, 30),
                             graph_type=GRAPH_T) for i=1:5])
    x = g.ndata.x
    e = g.edata.e

    @testset "reduce_nodes" begin
        r = reduce_nodes(mean, g, x)
        @test size(r) == (Dx, g.num_graphs)
        @test r[:,2] ≈ mean(getgraph(g, 2).ndata.x, dims=2)
    end

    @testset "reduce_edges" begin
        r = reduce_edges(mean, g, e)
        @test size(r) == (De, g.num_graphs)
        @test r[:,2] ≈ mean(getgraph(g, 2).edata.e, dims=2)
    end

    @testset "softmax_nodes" begin
        r = softmax_nodes(g, x)
        @test size(r) == size(x)
        @test r[:,1:10] ≈ softmax(getgraph(g, 1).ndata.x, dims=2)
    end

    @testset "softmax_edges" begin
        r = softmax_edges(g, e)
        @test size(r) == size(e)
        @test r[:,1:60] ≈ softmax(getgraph(g, 1).edata.e, dims=2)
    end

    @testset "broadcast_nodes" begin
        z = rand(4, g.num_graphs)
        r = broadcast_nodes(g, z)
        @test size(r) == (4, g.num_nodes)
        @test r[:,1] ≈ z[:,1]
        @test r[:,10] ≈ z[:,1]
        @test r[:,11] ≈ z[:,2]
    end

    @testset "broadcast_edges" begin
        z = rand(4, g.num_graphs)
        r = broadcast_edges(g, z)
        @test size(r) == (4, g.num_edges)
        @test r[:,1] ≈ z[:,1]
        @test r[:,60] ≈ z[:,1]
        @test r[:,61] ≈ z[:,2]
    end

    @testset "softmax_edge_neighbors" begin
        s = [1,2,3,4]
        t = [5,5,6,6]
        g2 = GNNGraph(s, t)
        e2 = randn(Float32, 3, g2.num_edges)
        z = softmax_edge_neighbors(g2, e2)
        @test size(z) == size(e2)
        @test z[:,1:2] ≈ NNlib.softmax(e2[:,1:2], dims=2)
        @test z[:,3:4] ≈ NNlib.softmax(e2[:,3:4], dims=2)
    end
end


#Convert number in [0,1)^2 to [0,1)
function xy2d(x::Float64, y:: Float64)
    N = 2^30
    xint = floor(Int64, x*N)
    yint = floor(Int64, y*N)
    # d will be in [0,N^2)
    d = xy2d(N, xint, yint)/N^2
end

function d2xy(d:: Float64)
    N::Int64 = 2^30
    dint = floor(Int64, d*N^2)
    x::Float64, y::Float64 = d2xy(N^2, dint)
    x/N, y/N
end

#Converted from wikipedia
#convert (x,y) to d
function xy2d(n::Integer, x::Integer, y::Integer)
    rx, ry, s, d = 0, 0, 0, 0
    s = div(n,2)
    while s>0
        rx = (x & s) > 0
        ry = (y & s) > 0
        d += s * s * ((3*rx) $ ry)
        x, y = rot(s, x, y, rx, ry)
        s = div(s,2)
    end
    return d::Integer
end

#convert d to (x,y)
function d2xy(n::Integer, d::Integer)
    rx, ry, s, t = 0, 0, 0, d
    x, y = 0, 0
    s = 1
    while s<n
        rx = 1 & (div(t,2))
        ry = 1 & (t $ rx)
        x, y = rot(s, x, y, rx, ry)
        x += s*rx
        y += s*ry
        t = div(t,4)
        s *= 2
    end
    return x::Integer, y::Integer
end

#rotate/flip a quadrant appropriately
function rot(n::Integer, x::Integer, y::Integer, rx::Integer, ry::Integer)
    if ry == 0
        if rx == 1
            x = n-1 - x
            y = n-1 - y
        end
        #Swap x and y
        t = x
        x = y
        y = t
    end
    return x::Integer, y::Integer
end


x = 2_000_000
ba = BitArray(repeat([1], x))
ba[1] = 0

for i = 4:2:x
  ba[i] = 0
end

for i = 3:2:x
  for j = 2:Int(floor(x/i))
    ba[i*j] = 0
  end
end

t = 0
for i = 2:x
  global t
  if ba[i]
    t += i
  end
end

println(t)


Qx, x = PolynomialRing(FlintQQ, "x")
QasNumberField, _ = NumberField(x - 1)
Kt, t = PolynomialRing(QasNumberField, "t")
K1, a1 = NumberField(x^3 - 2)
K2, (a2, ) = NumberField([x^3 - 2])
K3, a3 = NumberField(t^3 - 2)
K4, (a4, ) = NumberField([t^3 - 2])

@testset for (K, a) in [(K1, a1), (K2, a2), (K3, a3), (K4, a4)]
  b = one(K)
  fl = @inferred isintegral(b)
  @test fl
  fl = @inferred isintegral(a)

  B = @inferred basis(K)
  c = @inferred basis_matrix([one(K), a^4])
  @assert nrows(c) == 2
  @assert ncols(c) == 3
  @assert sum(B[i] * c[1, i] for i in 1:3) == one(K)
  @assert sum(B[i] * c[2, i] for i in 1:3) == a^4

  f = @inferred charpoly(one(K()))
  @test f == (gen(parent(f)) - 1)^3
  f = @inferred minpoly(one(K))
  @test f == (gen(parent(f)) - 1)

  f = @inferred charpoly(a)
  @test f == gen(parent(f))^3 - 2
  f = @inferred minpoly(a)
  @test f == gen(parent(f))^3 - 2

end