# Part One: using direct collection method
using InfiniteOpt, Ipopt, Distributions, Plots

# Set the SEIR parameters
b = 0.525
d = 0.5
c = 0.0001
e = 0.5
g = 0.1
a = 0.2
A = 0.1

# Set the domain information
t0 = 0
tf = 20

# Set the intial condition values
s0 = 1000
e0 = 100
i0 = 50
n0 = 1165

model = InfiniteModel(Ipopt.Optimizer)

@infinite_parameter(model, t ∈ [t0, tf], num_supports = 101,
    derivative_method = OrthogonalCollocation(2))

@variable(model, S ≥ 0, Infinite(t))
@variable(model, E ≥ 0, Infinite(t))
@variable(model, I ≥ 0, Infinite(t))
@variable(model, N ≥ 0, Infinite(t))
@variable(model, 0 ≤ u ≤ 0.9, Infinite(t), start = 0.2)

@objective(model, Min, ∫(A * I + u^2, t))

# Define the initial conditions
@constraint(model, S(0) == s0)
@constraint(model, E(0) == e0)
@constraint(model, I(0) == i0)
@constraint(model, N(0) == n0)

# Define the SEIR equations
@constraint(model, s_constr, ∂(S, t) == b * N - d * S - c * S * I - u * S)
@constraint(model, e_constr, ∂(E, t) == c * S * I - (e + d) * E)
@constraint(model, i_constr, ∂(I, t) == e * E - (g + a + d) * I)
@constraint(model, n_constr, ∂(N, t) == (b - d) * N - a * I)

# Define the infection rate limit

print(model)

optimize!(model)

I_value = value(I)
u_value = value(u)
tspan = value(t)
plot(tspan, I_value)
plot(tspan, u_value)

# Part Two: using Deep Learning Method
using DiffEqFlux, DifferentialEquations
const solveeq = DifferentialEquations.solve

tspan = (0.0f0, 20.0f0)
ann = FastChain(FastDense(1, 32, tanh), FastDense(32, 1), (x, p) -> 0.9 * sigmoid.(x))
θ = initial_params(ann)
function sir_nn(du, u, p, t)
    S, E, I, N = u
    du[1] = b * N - d * S - c * S * I - ann([t], p)[1] * S
    du[2] = c * S * I - (e + d) * E
    du[3] = e * E - (g + a + d) * I
    du[4] = (b - d) * N - a * I
end
u0 = [s0, e0, i0, n0]
ts = Float32.(collect(0.0:0.2:tspan[2]))
prob = ODEProblem(sir_nn, u0, tspan, θ)
sol_init = solveeq(prob, Vern9(), abstol = 1e-10, reltol = 1e-10)
plot(sol_init)
function predict_adjoint(θ)
    Array(solveeq(prob, Vern9(), p = θ, saveat = ts, sensealg = InterpolatingAdjoint(autojacvec = ReverseDiffVJP(true))))
end
function loss_adjoint(θ)
    x = predict_adjoint(θ)
    mean(abs, A .* x[3, :]) + mean(abs2, [first(ann([t], θ)) for t in ts])
end
l = loss_adjoint(θ)
cb = function (θ, l)
    println(l)
    p = plot(solveeq(remake(prob, p = θ), Tsit5(), saveat = 0.2), lw = 3)
    #plot!(p, ts, [first(ann([t], θ)) for t in ts], label = "u(t)", lw = 3)
    display(p)
    return false
end
# Display the ODE with the current parameter values.
cb(θ, l)
loss1 = loss_adjoint(θ)
res1 = DiffEqFlux.sciml_train(loss_adjoint, θ, ADAM(0.05), cb = cb, maxiters = 100)
res2 = DiffEqFlux.sciml_train(loss_adjoint, res1.u,
    BFGS(initial_stepnorm = 0.01), cb = cb, maxiters = 100,
    allow_f_increases = false)

# Part Three: Saving the results
using DataFrames
using CSV
results_saving_opt = DataFrame()
results_saving_opt.t = tspan
results_saving_opt.I = I_value
results_saving_opt.u = u_value
results_saving_opt.I_nn = Array(solveeq(remake(prob, p = res2.u), Tsit5(), saveat = 0.2))[3, :]
results_saving_opt.u_nn = [first(ann([t], re2.u)) for t in ts]
CSV.write("results_saving_SEINmodel.csv", results_saving_opt)

plot(results_saving_opt.t, results_saving_opt.I, lw = 2, label = "I(t) by direct collection")
plot!(results_saving_opt.t, results_saving_opt.I_nn, lw = 2, label = "I(t) by deep learning")
xlabel!("t(day)")
ylabel!("I(t)")
savefig("sein_I.png")

plot(results_saving_opt.t[2:end-1], results_saving_opt.u[2:end-1], lw = 2, label = "u(t) by direct collection")
plot!(results_saving_opt.t[2:end-1], results_saving_opt.u_nn[2:end-1], lw = 2, label = "u(t) by deep learning")
xlabel!("t(day)")
ylabel!("u(t)")
savefig("sein_control.png")

# Loss saving

opt_method_loss = 13.90369069230782
nn_method_loss = 13.711496454176164


"""
Linear MPC Controller

    # Parameters

    -A discrete state-space system with states and inputs constraints (states are considered available)
    -Weithing matrices with appropriate dimension Q and R and P
    _Horizon length N
    _Quadratic programming solver

    # Examples

    method = linear_mpc(sys, Q, R, P, N, opt)

"""

mutable struct Regulator
    formulation
    x_t::AbstractVector
    x_ref::AbstractMatrix
    u_ref::AbstractMatrix
    method::String
end

function linear_mpc(A::AbstractMatrix, B::AbstractMatrix, x_cons::AbstractMatrix, u_cons::AbstractMatrix, Q::AbstractMatrix, R::AbstractMatrix, P::AbstractMatrix, N::Integer)
    C = JuMP.Model(OSQP.Optimizer)
    JuMP.set_optimizer_attribute(C, "verbose", false)

    JuMP.@variables(C, begin
        x[1:size(A,1), 1:N + 1]
        x_tilde[1:size(A,1), 1:N + 1]
        x_ref[1:size(A,1), 1:N + 1]
        u[1:size(B,2), 1:N]
        u_tilde[1:size(B,2), 1:N]
        u_ref[1:size(B,2), 1:N]
    end)

    for k in 1 : 1 : N
        JuMP.@constraint(C, x_tilde[:,k+1] .== A * x_tilde[:,k] + B * u_tilde[:,k])
    end

    for k in 1 : 1 : N
        JuMP.@constraint(C, u_cons[:,1] .<= u[:,k] .<= u_cons[:,2])
    end

    for k in 1 : 1 : N
        JuMP.@constraint(C, x_cons[:,1] .<= x[:,k] .<= x_cons[:,2])
    end

    #JuMP.@constraint(C, -0.1 .<= xtilde[:,end] .<= 0.1) #terminal constraint to mimic Xf = 0, should be improved with maximum invariant set computation

    for k in 1 : 1 : N + 1
        JuMP.@constraint(C, x_tilde[:,k] .== x[:,k] .- x_ref[:,k])
    end

    for k in 1 : 1 : N
        JuMP.@constraint(C, u_tilde[:,k] .== u[:, k] .- u_ref[:,k])
    end

    JuMP.@objective(C, Min, x_tilde[:,end]' * P * x_tilde[:,end] + sum(x_tilde[:,k]' * Q * x_tilde[:,k] + u_tilde[:,k]' * R * u_tilde[:,k] for k in 1 : N))

    return Regulator(C, zeros(size(A,1)), zeros(size(A,1),N), zeros(size(B,2),N), "l")
end


export lanczos

# this finds the OPs and recurrence for a
function lanczos(w,N)
    x = Fun(identity,space(w))

    f1=Fun(1/sqrt(sum(w)),space(x))

    P = Array{Fun}(undef, N + 1)
    β = Array{eltype(w)}(undef, N)
    γ = Array{eltype(w)}(undef, N)

    P[1] = f1

    v = x*P[1]
    β[1] = sum(w*v*P[1])

    v = v - β[1]*P[1]
    γ[1] = sqrt(sum(w*v^2))

    P[2] = v/γ[1]

    for k = 2:N
        v = x*P[k] - γ[k-1]*P[k-1]
        β[k] = sum(w*v*P[k])
        v = v - β[k]*P[k]
        γ[k] = sqrt(sum(w*v^2))
        P[k+1] = v/γ[k]
    end

    P,β,γ
end


__precompile__()

module Utils

export gaussian_pdf, get_nn_pairs

function gaussian_pdf(n, μ, σ)
    x = collect(1:1<<n)
    pl = @. 1 / sqrt(2pi * σ^2) * exp(-(x - μ)^2 / (2 * σ^2))
    pl / sum(pl)
end

function get_nn_pairs(n)
    pairs = []
    for inth in 1:2
        for i in inth:2:n
            push!(pairs, i=>i % n + 1)
        end
    end
    pairs
end

end


## Model Generators for 2D Gaussian PSFs

using SpecialFunctions

"""
    PSF_airy2D <: PSF

Contains the parameter used to calculate an Airy Pattern PSF

The Airy PSF is

I(r)=ν²/(4π)(2*J₁(ν*r)/(ν*r))²

where

ν=πD/(λf)=2*π*nₐ/λ

!!! note
    The Gaussian approximation is σ = 0.42*π/ν
"""
struct PSF_airy2D <: PSF
    ν::Float32
end

# midpoint rule approximation to besselj1 integral
function mybesselj1(x::Float32, m::Int32=Int32(10))
    j1 = 0f0;
    for k = 1:(m - 1)
        j1 += sin(x * sin(pi / (2.0f0 * m) * (k + 0.5f0))) * sin(pi / (2.0f0 * m) * (k + .5f0))
    end
    return j1 / m
end

function airy_amplitude(r::Float32, ν::Float32)
    w = r * ν
    w = max(w, 1f-5)
    return (4π)^-(1 / 2) * ν * (2 * besselj1(w) / w)
end

function genmodel!(s::StateFlatBg, sz::Int32, psf::PSF_airy2D, model::Array{Float32,2})

    for jj = 1:sz
        for ii = 1:sz
            model[ii,jj] = s.bg
        end
    end

    for nn = 1:s.n
        y=s.y[nn]
        x=s.x[nn]
        photons=s.photons[nn];
         for jj = 1:sz
            for ii = 1:sz
                r = sqrt((ii - y)^2 + (jj - x)^2)
                model[ii,jj] += photons * airy_amplitude(r, psf.ν)^2
            end
        end
    end

end

function max2int(psf::PSF_airy2D)
    return 4π / psf.ν^2
end


using LinearAlgebra
using LinearMaps
using ArnoldiMethod
using SphericalOperators
import SphericalOperators: ord

struct InverseMap{M} <: LinearMap{M}
    factorization
end
InverseMap(A::AbstractMatrix) = InverseMap{eltype(A)}(factorize(A))

Base.size(IM::InverseMap) = size(IM.factorization)

function LinearMaps.A_mul_B!(B, IM::InverseMap, A)
    B[:] = IM.factorization\A
end

function eigenstates(H, σ=-10.0; kwargs...)
    Hs = H - sparse(I,size(H)...)*σ
    Hf = InverseMap(Hs)

    ee, = partialschur(Hf; kwargs...)

    λ = 1 ./ diag(ee.R) .+ σ

    λ,ee.Q
end

function ground_state(H, args...; kwargs...)
    λ,ϕ = eigenstates(H, args...; kwargs...)
    real(λ[1]),real.(ϕ[:,1])
end

function ground_state(basis::FEDVR.Basis, L::AbstractSphericalBasis,
                      ℓ₀::Integer, m₀::Integer=0,
                      ::Type{O}=SphericalOperators.LexicalOrdering,
                      args...; kwargs...) where {O<:SphericalOperators.Ordering}
    ℓ₀ ∉ eachℓ(L) && error("Requested initial partial wave $(ℓ₀) not in range $(eachℓ(L))")
    m₀ ∉ eachm(L,ℓ₀) && error("Requested initial projection quantum number $(m₀) not in range $(eachm(L,ℓ₀))")
    Hℓ₀ = hamiltonian(basis, ℓ₀)

    nᵣ = basecount(basis.grid)
    M = prod(size(L))
    ψ₀ = zeros(M)
    ψ₀[ord(L,O,ℓ₀,m₀,1:nᵣ)] = ground_state(Hℓ₀, args...; kwargs...)[2]
    ψ₀
end

export eigenstates, ground_state


std_rnorm = Normal()

abstract type ConditionalDistribution end

struct GaussianConditional <: ConditionalDistribution
    mu_intercept::Real
    mu_slope::Real
    sigma::Real
end

struct DiscreteConditional <: ConditionalDistribution
    p_0::Real
end

function sample(conditional::GaussianConditional, condition)
    x = rand(std_rnorm, 1)
    return x[1] * conditional.sigma + conditional.mu_intercept +
        conditional.mu_slope * condition
    return x
end

function sample(conditional::DiscreteConditional, condition)

end

function gibbs_sampler(px_y, py_x; y0=0, num_samples=10000, burn_in=100, thin=10)
    x_vec = zeros(0)
    y_vec = zeros(0)
    y = y0
    for i in 1:num_samples
        x = sample(px_y, y)
        y = sample(py_x, x)
        if((i > burn_in) && (i % thin == 0))
            push!(x_vec, x)
            push!(y_vec, y)
        end
    end
    return x_vec, y_vec
end

function check_discrete_compatibility(px_y, py_x)

end

function check_gaussian_compatibility(px_y, py_x)

end

function inconsistent_gaussian_gibbs()
    px_y = GaussianConditional(0, 1.5, 2)
    py_x = GaussianConditional(0, 0.5, 1)
    x,y = gibbs_sampler(px_y, py_x)
    p = plot(x, y, seriestype=:scatter, xlabel="X", ylabel="Y", legend=false)
    display(p)
    return x,y
end


using BayesianSumProductNetworks
using Test
using SpecialFunctions

@testset "normal inverse gamma - mllh" begin

    x = [0.1, -0.1, 0.0]

    H = NormalInverseGamma(0, 1, 2, 3)
    sstat = NormalInvGammaSufficientStats(length(x), sum(x), sum( (x .- mean(x)).^2 ))

    n = length(x)
    (μ0, ν0, a0, b0) = params(H)
    (μn, νn, an, bn) = postparams(H, sstat)

    # Compute marginal likelihood according to Murphy.
    pD = (gamma(an) / gamma(a0)) * sqrt( (ν0/νn)) * ((2*b0)^a0 / (2*bn)^an) * (1/(π^(n/2)))
    logpD = log(pD)
    logpD2 = BayesianSumProductNetworks.invgmllh(μ0, ν0, a0, b0, μn, νn, an, bn, n)

    @test logpD ≈ logpD2
end

@testset "categorical - mllh" begin

    x = [0, 1, 0]

    γ = [0.5, 0.5]
    sstat = map(v -> sum(x .== v), 0:1)
    n = length(x)

    # Compute marginal likelihood according to Stephen Tu.
    pD = (gamma(sum(γ)) / prod(gamma.(γ))) * (prod(gamma.(map(j -> sum(γ[j] + sstat[j]), 1:2))) / gamma(n + sum(γ)))
    @test log(pD) ≈ BayesianSumProductNetworks.categoricalMLL(γ, sstat)
end


"""
    HighOrderCBF

High Order Control barrier function for a control affine system.
# Fields
- `CBF`: CBF used to derive HOCBF
- `ψ`: highest order derivative of CBF
- `∇ψ`: gradient of ψ
"""
struct HighOrderCBF <: BarrierFunction
    ψ
    ∇ψ
    α
end

"""
	HighOrderCBF(CBF::ControlBarrierFunction, Σ::ControlAffineSystem, degree::Int)

Construct a High Order Control Barrier Function of a specified degree.
"""
function HighOrderCBF(CBF::ControlBarrierFunction, Σ::ControlAffineSystem, degree::Int)
    if degree == 2
        return second_order_hocbf(CBF, Σ)
    end
end

"""
	(HOCBF::HighOrderCBF)(x)

Evaluate HOCBF at state x.
"""
function (HOCBF::HighOrderCBF)(x)
    return HOCBF.ψ(x)
end

"""
	second_order_hocbf(CBF::ControlBarrierFunction, Σ::ControlAffineSystem)

Construct a HOCBF of relative degree 2.
"""
function second_order_hocbf(CBF::ControlBarrierFunction, Σ::ControlAffineSystem)
    # Need to do these autodiffs separately for now...maybe we can automate this later.
    dfdx(x) = jacobian(Σ.f, x)[1]
    dh2dx2(x) = hessian(CBF.h, x)
    dα(s) = gradient(CBF.α, s)[1]
    ψ1(x) = CBF.∇h(x) * Σ.f(x) + CBF.α(CBF(x))
    ∇ψ1(x) = CBF.∇h(x) * dfdx(x) + (dh2dx2(x) * Σ.f(x))' + dα(CBF(x)) * CBF.∇h(x)

    return HighOrderCBF(ψ1, ∇ψ1, CBF.α)
end


using DataFrames
using CSV
using Gurobi
using JuMP

distance_data = CSV.read("distance_data.csv",header=[:i, :j, :dist, :route], missing=:none)

N = length(unique(distance_data.i))
const distance_mx = zeros(Float64,(N,N))
for r in 1:nrow(distance_data)
   distance_mx[distance_data.i[r],distance_data.j[r]] = distance_data.dist[r]
end

#http://opensourc.es/blog/mip-tsp
N = 50 # this time we use 50 points

m = Model(solver=GurobiSolver());
@variable(m, x[f=1:N,t=1:N], Bin)
@objective(m, Min, sum( x[i,j]*distance_mx[i,j] for i=1:N,j=1:N))
@constraint(m, notself[i=1:N], x[i,i] == 0)
@constraint(m, oneout[i=1:N], sum(x[i,1:N]) == 1)
@constraint(m, onein[j=1:N], sum(x[1:N,j]) == 1)
for f=1:N, t=1:N
    @constraint(m, x[f,t]+x[t,f] <= 1)
end

function getcycle(m, N)
    x_val = getvalue(x)
    cycle_idx = Vector{Int}()
    push!(cycle_idx, 1)
    while true
        v, idx = findmax(x_val[cycle_idx[end], 1:N])
        if idx == cycle_idx[1]
            break
        else
            push!(cycle_idx, idx)
        end
    end
    cycle_idx
end

function callbackhandle(cb)
    cycle_idx = getcycle(cb, N)
    println("Callback! N= $N cycle_idx: ", cycle_idx)
    println("Length: ", length(cycle_idx))
    if length(cycle_idx) < N
        @lazyconstraint(cb, sum(x[cycle_idx,cycle_idx]) <= length(cycle_idx)-1)
        println("added a lazy constraint")
    end
end

addlazycallback(m, callbackhandle)
solve(m)

sbws_la = CSV.read("Subway_LV.csv", missing=:none)

using PyPlot
cycle_idx = getcycle(m, N)
ids = vcat(cycle_idx, cycle_idx[1])
PyPlot.cla()
PyPlot.ioff()
p = PyPlot.plot(sbws_la.long[ids], sbws_la.latt[ids])
PyPlot.display_figs()
PyPlot.show()


export getPaddedMesh, jInvObj

"""
function getPaddedMesh(M::RegularMesh)

return regular mesh padded by h/2 on all sided and with
one cell added, so that the cell-centered points on M
are nodal points on the padded mesh.
"""
function getPaddedMesh(M::RegularMesh)
	domainPad = copy(M.domain);
	domainPad[1:2:end] -= M.h/2
	domainPad[2:2:end] += M.h/2
	return getRegularMesh(domainPad,M.n.+1)
end

"""
function jInvObj(mc,pMis,pInv)

evaluates the jInv objective function defined by pMis and pInv
"""
function jInvObj(mc::BitArray{1},pMis,pInv)
   return jInvObj(Array{Float64}(mc),pMis,pInv);
end
function jInvObj(mc::Array{Float64},pMis,pInv)

    sig,dsig              = pInv.modelfun(mc)
	to = @elapsed begin
		Dc,F,dF,d2F,pMis,tMis = computeMisfit(sig,pMis,true)
	end
	timeF = to
	dF                    = dsig'*dF

	# Compute regularizer
	to = @elapsed begin
	R,dR,d2R = computeRegularizer(pInv.regularizer,mc,pInv.mref,pInv.MInv,pInv.alpha)
	end
	timeR = to

	# Objective function
	Jc = F  + R
	gc = dF + dR

    return Jc,gc,Dc,F,R,timeF,timeR
end


using LinearAlgebra

"""
    regularized_Picard(B, K, dpp_samples_ids, Fredholm_sample_ids, lambda, it_max, tol, use_inverse)

Runs regularized Picard iteration starting from an initial positive definite matrix B.

# Arguments
- `B::Array{Float64,2}`: an initial positive definite matrix B.
- `K::Array{Float64,2}`: full kernel matrix.
- `dpp_samples_ids:Array{Array{Int64,1},1}`: array of arrays containing indices of s DPP samples.
- `Fredholm_sample_ids:Array{Int64,1}`: indices of samples for approximating Fredholm determinant.
- `lambda:Float`: regularization parameter.
- `it_max::Integer`: largest number of iterations.
- `tol:Float`: relative objective variation desired for regularized Picard iteration.
- `use_inverse:Bool`: if true, inverse matrices are computed, otherwise only linear systems.

# Output
- `B::Array{Float64,2}`: approximate solution.
- `R::Array{Float64,2}`: Cholesky s.t. K = R'R.
- `obj`: array containing the objective values at each iteration.
- `i_stop`: number of iterations runned before stopping.

"""
function regularized_Picard(B::Array{Float64,2},K::Array{Float64,2}, dpp_samples_ids::Array{Array{Int64,1},1}, Fredholm_sample_ids::Array{Int64,1}, lambda::Float64, it_max::Int64 ,tol::Float64,use_inverse::Bool)

# number of dpp_samples_ids
nb_dpp_samples_ids = length(dpp_samples_ids);

# total number of points
m = size(K,1);

# Chol decomposition
R = cholesky(K).U;

# sampling matrix for uniformSample
nb_unif = length(Fredholm_sample_ids)

# initialization
obj = zeros(it_max,1);
i_stop = it_max;

# regularization
epsilon = 1e-10;

# iterations
for i in 1:it_max
    # construct  Delta
    BRDeltaRtB = zeros(m,m);
    RtBR = R'*B*R
    BR = B*R
    for l = 1:nb_dpp_samples_ids
        id = dpp_samples_ids[l];
        M = (RtBR + UniformScaling(epsilon))[id,id]
        BRid = BR[:,id]
        if use_inverse
            BRDeltaRtB +=  BRid * inv(M) * BRid';
        else
            BRDeltaRtB +=  BRid * (M\(BRid'));
        end
    end

    N = (RtBR + UniformScaling(nb_unif))[Fredholm_sample_ids,Fredholm_sample_ids]
    BRunif = BR[:,Fredholm_sample_ids]
    if use_inverse
        BRDeltaRtB = BRDeltaRtB/nb_dpp_samples_ids - BRunif*inv(N)*BRunif';
    else
        BRDeltaRtB = BRDeltaRtB/nb_dpp_samples_ids - BRunif*(N\(BRunif'));
    end

    BRDeltaRtB = 0.5*(BRDeltaRtB + BRDeltaRtB');

    # Picard iteration
    pB = B + BRDeltaRtB;

    # final expression
    B = (0.5/lambda)*(real(sqrt(I+4*lambda*pB))-I);
    B = 0.5*(B+B');

    # track the objective values
    obj_det,ob_reg = Picard_objective(B, dpp_samples_ids, Fredholm_sample_ids, R,lambda);
    obj[i] = obj_det + ob_reg;
    # printing in repl
    if i%100 == 0
        rel_variation = abs(obj[i]-obj[i-1])/abs(obj[i])
        print("---------------------------------------------------------------\n")
        print("$(i) / $it_max\n")
        print("relative objective variation $(rel_variation)\n")
        print("objective = $(obj[i]) \n")

    end
    # stopping criterion
    if i>1 && abs(obj[i]-obj[i-1])/abs(obj[i])< tol
        i_stop = i;
        print("---------------------------------------------------------------\n")
        print("Relative tolerance $(tol) attained after $(i) iterations.\n")
        print("Final objective= $(obj[i])\n")
        print("---------------------------------------------------------------\n")
        break
    end
    if i==it_max
        print("iteration has not yet converged.\n")
    end
end

return B, R, obj, i_stop

end

"""
    Picard_objective(B, dpp_samples_ids, Fredholm_samples_ids, R,lambda)

Computes Picard objective function.

# Arguments
- `B::Array{Float64,2}`: positive definite matrix B.
- `dpp_samples_ids:Array{Array{Int64,1},1}`: array of arrays containing indices of s DPP samples.
- `Fredholm_sample_ids:Array{Int64,1}`: indices of samples for approximating Fredholm determinant.
- `R`: Cholesky decomposition such that K = R'R.
- `lambda:Float`: regularization parameter.

# Outputs
- `ob_det:Float`: term of the objective including the determinants.
- `ob_reg:Float`: term of the objective including the regularization.

For the full objective do ob_det + ob_reg
"""
function Picard_objective(B, dpp_samples_ids, Fredholm_samples_ids, R,lambda)

    # number of dpp samples
    nb_dpp_samples_ids = length(dpp_samples_ids);

    # samples Fredholm
    nb_unif = length(Fredholm_samples_ids)

    PhiBPhi = R'*B*R;
    ob_det = 0
    for l = 1:nb_dpp_samples_ids
        id = dpp_samples_ids[l];
        ob_det -= logdet(PhiBPhi[id,id]);
        if ob_det==Inf
            error("singular determinant in objective\n")
        end
    end
    ob_det = ob_det/nb_dpp_samples_ids;
    ob_det += logdet(I + (1/nb_unif)*PhiBPhi[Fredholm_samples_ids,Fredholm_samples_ids]);
    ob_reg = lambda*tr(B);

    # full objective = ob_det + ob_reg
    return ob_det, ob_reg;
end


include("./params.jl")

function pprocess_inhomopoisson(lambda)
    # Example usage:
    # L = 100000;
    # ts = linspace(0, 10, L);
    # f = 1.0;
    # lambda = 20*(1+sin(2.0 * pi * f * ts));
    # x, rs = pprocess_inhomopoisson(lambda)
    #
    # Input argument
    # lambda: A vector that specifies a time-dependent spike-rate.
    #
    # Output argument
    # x:    Spike time.
    # rs:   Intensity function.

    #Initialize parameters

    x = [0.0]; # push!(x, num)
    rs = zeros(L);

    R = 0;

    rs[1] = lambda[1];

    eta = - log(rand());
    for i = 2:L
        tau = i*dt - x[end]

        r = lambda[i]; # Intensity
        R = R + r*dt;

        if eta <= R
            push!(x,i*dt) # Spike time

            R = 0.0;
            eta = - log(rand());
        end
        rs[i] = r;
    end

    return x, rs
end


module Exp

using Base.Test
using Bitmanip, Defs, FloatProps, Rounding

export vexp, vexp10, vexp2

# Note: The Coefficients were determined by a Mathematica script,
# available at <https://bitbucket.org/eschnett/vecmathlib>. They
# minimize the error in a least-square sense over the interval dx ∈
# [-0.5; +0.5]. A better alternative would be to minimize the maximum
# of the error.

# One can use a smaller set of coefficients to increase speed at the
# extent of a slightly larger error.

# TODO: Use @evalpoly for this
@inline function vexp2_poly(dx::Float32)
    # Coefficients for Float32,
    # error=4.55549108005200277750378992345e-9
    r = mkFloat(Float32, 0.000154653240842602623787395880898)
    r = muladd(r, dx, mkFloat(Float32, 0.00133952915439234389712105060319))
    r = muladd(r, dx, mkFloat(Float32, 0.0096180399118156827664944870552))
    r = muladd(r, dx, mkFloat(Float32, 0.055503406540531310853149866446))
    r = muladd(r, dx, mkFloat(Float32, 0.240226511015459465468737123346))
    r = muladd(r, dx, mkFloat(Float32, 0.69314720007380208630542805293))
    r = muladd(r, dx, mkFloat(Float32, 0.99999999997182023878745628977))
    return r
end

@inline function vexp2_poly(dx::Float64)
    # Coefficients for Float64,
    # error=9.32016781355638010975628074746e-18
    r = mkFloat(Float64, 4.45623165388261696886670014471e-10)
    r = muladd(r, dx, mkFloat(Float64, 7.0733589360775271430968224806e-9))
    r = muladd(r, dx, mkFloat(Float64, 1.01780540270960163558119510246e-7))
    r = muladd(r, dx, mkFloat(Float64, 1.3215437348041505269462510712e-6))
    r = muladd(r, dx, mkFloat(Float64, 0.000015252733849766201174247690629))
    r = muladd(r, dx, mkFloat(Float64, 0.000154035304541242555115696403795))
    r = muladd(r, dx, mkFloat(Float64, 0.00133335581463968601407096905671))
    r = muladd(r, dx, mkFloat(Float64, 0.0096181291075949686712855561931))
    r = muladd(r, dx, mkFloat(Float64, 0.055504108664821672870565883052))
    r = muladd(r, dx, mkFloat(Float64, 0.240226506959101382690753994082))
    r = muladd(r, dx, mkFloat(Float64, 0.69314718055994530864272481773))
    r = muladd(r, dx, mkFloat(Float64, 0.9999999999999999978508676375))
    return r
end

@inline function vexp2{T}(x::T)
    # Here it does not matter how ties are broken
    xi = vround(x)
    dx = x - xi
    @vassert mkFloat(T,-0.5) <= dx <= mkFloat(T,0.5)
    r = vexp2_poly(dx)
    # r = vldexp(r, mkInt(T, xi))
    r = vldexp(r, xi)
    r = vifthen(x < mkFloat(T, min_exponent(T)), mkFloat(T,0.0), r)
    r = vifthen(x > mkFloat(T, max_exponent(T)), mkFloat(T,Inf), r)
    return r
end

const M_LOG2E = 1.442695040888963407359924681001892137426645954152985934135449406931109219181187e+00
@inline function vexp{T}(x::T)
    # TODO: Re-calculate the coefficients for this function to
    # increase accuracy and avoid the multiplication
    return vexp2(mkFloat(T,M_LOG2E)*x)
end

const M_LOG10E = 4.342944819032518276511289189166050822943970058036665661144537831658646492088688e-01
@inline function vexp10{T}(x::T)
    # TODO: Re-calculate the coefficients for this function to
    # increase accuracy and avoid the multiplication
    return vexp2(mkFloat(T,M_LOG10E)*x)
end

function selftest_type(T::Type)
    zero = convert(T,0)
    min = realmin(T)
    eps = Base.eps(T)
    max = realmax(T)
    vals = T[zero, nextfloat(zero),
             prevfloat(min), min, nextfloat(min),
             0.1*eps, eps, 10.0*eps,
             0.1*sqrt(eps), sqrt(eps), 10.0*sqrt(eps),
             1.0-eps, 1.0, 1.0+eps,
             2.0, 10.0, 100.0,
             0.1*sqrt(max), sqrt(max), 10.0*sqrt(max),
             0.1*max, prevfloat(max), max, Inf, NaN]
    for uval in vals
        for val in (uval, -uval)
            @test isequal(vexp2(val), exp2(val))
        end
    end
end

function selftest()
    selftest_type(Float32)
    selftest_type(Float64)
end

selftest()

end


findnmax(x) = round(Int, 2 + x + 4x^(1 / 3))

struct Mie end

function scattering(m, x, ::Mie)

    nmax = findnmax(x)
    an, bn = mie_ab(m, x, nmax)

    return 2 / x^2 * sum((2n + 1) * (abs(an[n])^2 + abs(bn[n])^2) for n = 1:nmax)
end

absorption(m, x, s::Mie) = extinction(m, x, s) - scattering(m, x, s)

function extinction(m, x, ::Mie)

    nmax = findnmax(x)
    an, bn = mie_ab(m, x, nmax)

    return 2 / x^2 * sum((2n + 1) * real(an[n] + bn[n]) for n = 1:nmax)
end

function backscattering(m, x, ::Mie)

    nmax = findnmax(x)
    an, bn = mie_ab(m, x, nmax)

    return 1 / x^2 * abs(sum((2n + 1) * (-1)^n * (an[n] - bn[n]) for n = 1:nmax))^2
end

struct MieProblem
    m
    x
    nmax
    an
    bn

    function MieProblem(m, x)
        nmax = findnmax(x)
        an, bn = mie_ab(m, x, nmax)
        return new(m, x, nmax, an, bn)
    end
end

scattering(s::MieProblem) = 2 / s.x^2 * sum((2n + 1) * (abs(s.an[n])^2 + abs(s.bn[n])^2) for n = 1:s.nmax)
absorption(s::MieProblem) = extinction(s) - scattering(s)
extinction(s::MieProblem) = 2 / s.x^2 * sum((2n + 1) * real(s.an[n] + s.bn[n]) for n = 1:s.nmax)
backscattering(s::MieProblem) = 1 / s.x^2 * abs(sum((2n + 1) * (-1)^n * (s.an[n] - s.bn[n]) for n = 1:s.nmax))^2

@doc raw"""
    mie_ab(m, x, nmax)

Computes external field coefficients ``a_n`` and ``b_n`` based on inputs of `m` and ``x = \pi \, d_p / \lambda``.

# Arguments
- m: The complex refractive index with the convention ``m = n+ik``.
- x: The size parameter ``x = \pi \, d_p / \lambda``.

# Return values
- an, bn: Arrays of size ``n_max = 2 + x + 4x^{1/3}``

"""
function mie_ab(m, x, nmax)

    mx = m * x
    nmx = max(nmax, round(Int, abs(mx))) + 16

    n = 1:nmax

    nu = n .+ 0.5
    sx = sqrt(0.5 * π * x)

    px = sx * besselj.(nu, x)
    p1x = append!([sin(x)], px[1:end-1])

    chx = -sx * bessely.(nu, x)
    ch1x = append!([cos(x)], chx[1:end-1])

    gsx = px - 1im * chx
    gs1x = p1x - 1im * ch1x

    # B&H Equation 4.89
    Dn = zeros(ComplexF64, nmx)
    for i = nmx-1:-1:1
        Dn[i] = (i / mx) - (1 / (Dn[i+1] + i / mx))
    end

    D = Dn[2:nmax+1] # Dn(mx), drop terms beyond nMax

    da = D / m + n / x
    db = m * D + n / x

    an = @. (da * px - p1x) / (da * gsx - gs1x)
    bn = @. (db * px - p1x) / (db * gsx - gs1x)

    return an, bn
end

# spherical bessel functions
sbesselj(nu, x) = √(π / 2x) * besselj(nu + 0.5, x)
sbessely(nu, x) = √(π / 2x) * bessely(nu + 0.5, x)
shankelh1(nu, x) = sbesselj(nu, x) + 1im * sbessely(nu, x)

@doc raw"""
    mie_abcd(m, x, n::Int)

Calculate n-th order Mie coefficients a,b,c,d

# Arguments
- `m`: relative refractive index (``n_\mathrm{material} / n_\mathrm{environment}``)
- `x`: size parameter (wavenumber * radius)
- `n::Int`
"""
function mie_ab_n(m, x, n::Int)

    # Bessel/Hankel functions
    jx = sbesselj(n, x)
    jmx = sbesselj(n, m * x)
    hx = shankelh1(n, x)

    # derivative
    d1xjx = x * sbesselj(n - 1, x) - n * sbesselj(n, x)
    d1mxjmx = m * x * sbesselj(n - 1, m * x) - n * sbesselj(n, m * x)
    d1xhx = x * shankelh1(n - 1, x) - n * shankelh1(n, x)

    an = (m^2 * jmx * d1xjx - jx * d1mxjmx) / (m^2 * jmx * d1xhx - hx * d1mxjmx)
    bn = (jmx * d1xjx - jx * d1mxjmx) / (jmx * d1xhx - hx * d1mxjmx)

    # cn = (jx * d1xhx - hx * d1xjx) / (jmx * d1xhx - hx * d1mxjmx)
    # dn = (m * jx * d1xhx - m * hx * d1xjx) / (m^2 * jmx * d1xhx - hx * d1mxjmx)

    # return an, bn, cn, dn
    return an, bn
end

@doc raw"""
    mie_pi_tau(mu, nmax)

Calculates ``\pi_n`` and ``\tau_n``.

This function uses recurrence relations to calculate ``\pi_n`` and ``\tau_n``, beginning with ``\pi_0 = 1``, ``\pi_1 = 3 \mu`` (where ``mu`` is the cosine of the scattering angle), ``\tau_0 = \mu``, and ``\tau_1 = 3 cos(2 cos^{-1}(\mu))``:
```math
\pi_n = \frac{2n-1}{n-1} \mu \pi_{n-1} - \frac{n}{n-1} \pi_{n-2}
```
```math
\tau_n = n \mu \pi_n - (n+1) \pi_{n-1}
```

# Arguments
- mu : The cosine of the scattering angle.
- nmax: The number of elements to compute. Typically, ``n_{max} = floor(2+x+4x^{1/3})``, but can be given any integer.

# Return values
- p, t: The ``\pi_n`` and ``\tau_n`` arrays, of length `nmax`.
"""
function mie_pi_tau(mu, nmax)

    p = zeros(nmax)
    t = zeros(nmax)

    p[1] = 1
    p[2] = 3mu

    t[1] = mu
    t[2] = 3 * cos(2 * acos(mu))

    for n = 3:nmax
        p[n] = (2n - 1) / (n - 1) * mu * p[n-1] - n / (n - 1) * p[n-2]
        t[n] = n * mu * p[n] - (n + 1) * p[n-1]
    end

    return p, t
end

@doc raw"""
    mie_pi_tau(mu, n::Int)

See Sec. 4.3.1 of B&H ref of pi,tau: [D. Deirmendjian, “Electromagnetic Scattering on Spherical Polydispersions"]
"""
function mie_pi_tau_n(mu, n::Int)
    # See Sec. 4.3.1 of B&H
    # ref of pi,tau: [D. Deirmendjian, “Electromagnetic Scattering on Spherical Polydispersions"]

    if mu < -1 || mu > 1
        error("mu is cos(theta), -1 <= mu <= 1")
    end

    if n == 0
        pin = 0.0  # pi(0, mu) = 0
        taun = 0.0  # not used but tau(0, mu) = 0
    elseif n == 1
        pin = 1.0  # pi(1, mu) = 1
        taun = mu  # note that pi(0, mu) = 0
    elseif n > 1
        # recurrence, eq.(4.47) of B&H
        pin = (2n - 1) / (n - 1) * mu * mie_pi_tau_n(mu, n - 1)[1] - (n) / (n - 1) * mie_pi_tau_n(mu, n - 2)[1]
        taun = n * mu * pin - (n + 1) * mie_pi_tau_n(mu, n - 1)[1]
    else
        error("n should be larger than 0")
    end
    return pin, taun
end

@doc raw"""
    mie_scattering(m, x, [nmax])

Computes Mie efficencies *Q* and asymmetry parameter *g* of a single, homogeneous particle. Uses `mie_ab` to calculate ``a_n`` and ``b_n``, and then calculates *Q* via:
```math
Q_{ext} = \frac{2}{x^2} \sum \limits_{n=1}^{n_{max}} (2n+1) \: \text{Re} \left\{ a_n + b_n \right\}
```
```math
Q_{sca} = \frac{2}{x^2} \sum_{n=1}^{n_{max}} (2n+1) (|a_n|^2 + |b_n|^2)
```
```math
Q_{abs} = Q_{ext} - Q_{sca}
```
```math
Q_{back} = \frac{1}{x^2} \left| \sum\limits_{n=1}^{n_{max}} (2n+1) (-1)^n (a_n - b_n) \right|^2
```
```math
Q_{ratio} = \frac{Q_{back}}{Q_{sca}}
```
```math
g = \frac{4}{Q_{sca}x^2} \left[ \sum\limits_{n=1}^{n_{max}} \frac{n(n+2)}{n+1} \text{Re}\left\{a_n a_{n+1}^* + b_n b_{n+1}^*\right\} + \sum\limits_{n=1}^{n_{max}} \frac{2n+1}{n(n+1)} \text{Re}\left\{a_n b_n^*\right\}\right]
```
```math
Q_{pr} = Q_{ext} - g Q_{sca}
```
where asterisks denote the complex conjugates.

# Arguments
- m: The complex refractive index, with the convention *m = n+ik*.
- wavelength: The wavelength of incident light, in nanometers.
- diameter: The diameter of the particle, in nanometers.
- n_medium: The refractive index of the surrounding medium. This must be positive, nonzero, and real. Any imaginary part will be discarded.

# Return values
- qext, qsca, qabs, g, qpr, qback, qratio: The Mie efficencies described above.

# Examples

For example, compute the Mie efficencies of a particle ``300 \, [\mathrm{nm}]`` in diameter with ``m = 1.77+0.63i``, illuminated by ``\lambda = 375 \, [\mathrm{nm}]``:
```julia-repl
julia> mie_scattering(1.77+0.63im, 375, 300)

(2.858497199156411, 1.3149276685170943, 1.543569530639317, 0.2014551048135256)
```
"""
function mie_scattering(m, x, nmax = findnmax(x))

    if x ≈ 0
        return 0.0, 0.0, 0.0, 0.0
    end

    an, bn = mie_ab(m, x, nmax)

    # Note:
    # Q_i = \sigma_i / (\pi a^2)
    # where
    # - Q_i is the efficiency coefficients
    # - \sigma_{i} is the cross section of the respective process

    qsca = 2 / x^2 * sum((2n + 1) * (abs(an[n])^2 + abs(bn[n])^2) for n = 1:nmax)
    qext = 2 / x^2 * sum((2n + 1) * real(an[n] + bn[n]) for n = 1:nmax)
    qabs = qext - qsca
    qback = 1 / x^2 * abs(sum((2n + 1) * (-1)^n * (an[n] - bn[n]) for n = 1:nmax))^2

    # g = 4 / (qsca * x^2) * (sum(n * (n + 2) / (n + 1) * real(an[n]*an[n+1]'+bn[n]*bn[n+1]') for n = 1:nmax-1) + sum((2n + 1) / (n * (n + 1)) * real(an[n]*bn[n]')  for n = 1:nmax-1))
    # qpr = qext - qsca * g
    # qratio = qback / qsca

    return qext, qsca, qabs, qback
end

@doc raw"""
    mie_scattering_n(m, x, nmax::Int=-1)

Calculate scattering efficiency of a sphere

# Arguments
- `m`: relative refractive index (``n_\mathrm{material} / n_\mathrm{environment}``)
- `x`: size parameter (wavenumber * radius)
- `namx`: maximun order of resonances being considered
      (nmax = 1: dipole, 2: dipole+quadrupole, ...,
       -1: sufficiently large value determined by x)

# Return values
- `Qsca`: Scattering efficiency (total up to n-th order resonances)
"""
function mie_scattering_n(m, x, nmax = findnmax(x))

    if x ≈ 0
        return 0.0
    end

    qsca, qext = 0.0, 0.0
    for n = 1:nmax
        an, bn = mie_ab_n(m, x, n)
        qsca += (2n + 1) * (abs(an)^2 + abs(bn)^2)
        qext += (2n + 1) * real(an + bn)
    end
    qsca *= 2 / x^2
    qext *= 2 / x^2

    qabs = qext - qsca

    return qext, qsca, qabs, nothing
end

@doc raw"""
    mie_S1_S2(m,x,mu)

Calculates ``S_1`` and ``S_2`` at ``\mu = cos(\theta)``, where ``\theta`` is the scattering angle.

Uses `mie_ab` to calculate `a_n` and `b_n`, and `mie_pi_tau` to calculate ``\pi_n`` and ``\tau_n``. ``S_1`` and ``S_2`` are calculated by:

```math
S_1 = \sum\limits_{n=1}^{n_{max}} \frac{2n+1}{n(n+1)} (a_n \pi_n + b_n \tau_n)
```
```math
S_2 = \sum\limits_{n=1}^{n_{max}} \frac{2n+1}{n(n+1)} (a_n \tau_n + b_n \pi_n)
```

# Parameters
- m: The complex refractive index with the convention *m = n+ik*.
- x: The size parameter ``x = \pi \, d_p / \lambda``.
- mu: The cosine of the scattering angle.

# Return values
- S1, S2: The ``S_1`` and ``S_2`` values.

"""
function mie_S1_S2(m, x, mu, nmax = findnmax(x))

    an, bn = mie_ab(m, x, nmax)
    pin, taun = mie_pi_tau(mu, nmax)

    S1 = sum(1:nmax) do n
        (2n + 1) / (n * (n + 1)) * (an[n] * pin[n] + bn[n] * taun[n])
    end

    S2 = sum(1:nmax) do n
        (2n + 1) / (n * (n + 1)) * (an[n] * taun[n] + bn[n] * pin[n])
    end

    return S1, S2
end

@doc raw"""
    mie_S1_S2(m, x, mu, n::Int)

calculate n-th order S1,S2 component of scattering matrix.
"""
function mie_S1_S2_n(m, x, mu, n)

    a, b = mie_ab_n(m, x, n)
    p, t = mie_pi_tau_n(mu, n)

    S1 = (2n + 1) / (n * (n + 1)) * (a * p + b * t)
    S2 = (2n + 1) / (n * (n + 1)) * (a * t + b * p)

    return S1, S2
end

@doc raw"""
    scattering_function(theta, m, x)

Creates arrays for plotting the angular scattering intensity functions in theta-space with parallel, perpendicular, and unpolarized light. Also includes an array of the angles for each step. This angle can be in either degrees, radians, or gradians for some reason. The angles can either be geometrical angle or the qR vector (see *Sorensen, M. Q-space analysis of scattering by particles: a review. J. Quant. Spectrosc. Radiat. Transfer 2013, 131, 3-12 <http://www.sciencedirect.com/science/article/pii/S0022407313000083>*). Uses `mie_S1_S2` to compute ``S_1`` and ``S_2``, then computes parallel, perpendicular, and unpolarized intensities by

```math
SL(\theta) = |S_1|^2
```
```math
SR(\theta) = |S_2|^2
```
```math
SU(\theta) = \frac{1}{2}(SR + SL)
```

# Arguments

- theta: An array of the angles used in calculations.
- m: The complex refractive index with the convention ``m = n+ik``.
- wavelength: The wavelength of incident light, in nanometers.
- diameter: The diameter of the particle, in nanometers.
- n_medium: The refractive index of the surrounding medium. This must be positive, nonzero, and real. Any imaginary part will be discarded.

# Return values

- SL: An array of the scattered intensity of left-polarized (perpendicular) light. Same size as the `theta` array.
- SR: An array of the scattered intensity of right-polarized (parallel) light. Same size as the `theta` array.
- SU: An array of the scattered intensity of unpolarized light, which is the average of SL and SR. Same size as the `theta` array.
"""
function scattering_function(theta, m, x)

    n = length(theta)

    SL = zeros(n)
    SR = zeros(n)
    SU = zeros(n)

    if x ≈ 0
        return SL, SR, SU
    end

    for i = 1:n
        mu = cos(theta[i])
        s1, s2 = mie_S1_S2(m, x, mu)
        SL[i] = abs(s1^2)
        SR[i] = abs(s2^2)
        SU[i] = (SR[i] + SL[i]) / 2
    end

    return SL, SR, SU
end

@doc raw"""
    scattering_function_n(theta, m, x)

"""
function scattering_function_n(theta, m, x)

    n = length(theta)

    SL = zeros(n)
    SR = zeros(n)
    SU = zeros(n)

    if x ≈ 0
        return SL, SR, SU
    end

    for i = 1:n
        mu = cos(theta[i])
        nmax = findnmax(x)
        s1, s2 = 0.0, 0.0
        for n = 1:nmax
            s1n, s2n = mie_S1_S2_n(m, x, mu, n)
            s1 += s1n
            s2 += s2n
        end

        SL[i] = abs(s1^2)
        SR[i] = abs(s2^2)
        SU[i] = (SR[i] + SL[i]) / 2
    end

    return SL, SR, SU
end


using LowLevelParticleFilters, LinearAlgebra, StaticArrays, Distributions, Random

#=M = 1000 # Number of smoothing trajectories, NOTE: if this is set higher, the result will be better at the expense of linear scaling of the computational cost.
N = 1000 # Number of particles in the particle filter
@inline pos(s) = s[SVector(1,2)]

dgσ = 1#0.1 # the deviation of the measurement noise distribution
dvσ = 1#1 # the deviation of the dynamics noise distribution
daσ = 0.7#1

const dg = MvNormal(zero(SVector{2, Float64}), dgσ)
const df = MvNormal(zero(SVector{4, Float64}), [0.01, 0.01, dvσ, daσ])

const noisevec = zeros(4)

@inline function dynamics(s,u,t,noise=false)
    # current states
    x, y, v, a = s
    # get noise
    if noise
        x_noise, y_noise, v_noise, a_noise = rand!(df, noisevec)
    else
        x_noise, y_noise, v_noise, a_noise = 0.,0.,0.,0.
    end
    # next states
    dy, dx = sincos(a)
    x += x_noise + dx*v
    y += y_noise + dy*v
    v⁺ = max(0.999v + v_noise, 0.0)
    a += a_noise
    SVector{4,Float64}(x, y, v⁺, a)
end
function measurement_likelihood(s,y,t)
    logpdf(dg, pos(s)-y) # A simple linear measurement model with normal additive noise
end
@inline measurement(s,t=1) = pos(s)
@inline measurement(s,t,noise) = measurement(s) + noise*rand(dg) # We observer the position coordinates with the measurement

function smoothed(xy)
    d0 = MvNormal(SVector(xy[1]..., norm(xy[2] - xy[1]), atan(reverse(xy[2] - xy[1])...)), ones(4))
    u = zeros(length(xy))
    pf = AuxiliaryParticleFilter(AdvancedParticleFilter(N, dynamics, measurement, measurement_likelihood, df, d0))
    x1,w,we,ll=forward_trajectory(pf,u,xy)
    xh = mean_trajectory(x1,we)
    sb, _ = smooth(pf, M, u, xy) # Sample smooting particles (b for backward-trajectory)
    s = smoothed_mean(sb)     # Calculate the mean of smoothing trajectories
    s[1:2, :], s[3, :], s[4, :]
end=#

M = 100 # Number of smoothing trajectories, NOTE: if this is set higher, the result will be better at the expense of linear scaling of the computational cost.
N = 1000 # Number of particles in the particle filter
@inline pos(s) = s[SVector(1,2)]

dgσ = 3#0.1 # the deviation of the measurement noise distribution
dvσ = 1#1 # the deviation of the dynamics noise distribution
daσ = 0.06 # 0.05 works really well

const dg = MvNormal(zero(SVector{2, Float64}), dgσ)
# const df = MvNormal(zero(SVector{4, Float64}), [0.01, 0.01, dvσ, daσ])
const switch_prob = 0.01
const df = LowLevelParticleFilters.TupleProduct((Normal.(0,[0.01, 0.01, dvσ, daσ])..., Binomial(1, switch_prob)))

const noisevec = zeros(5)

@inline function dynamics(s,u,t,noise=false)
    # current states
    x, y, v, a, m = s
    # get noise
    if noise
        x_noise, y_noise, v_noise, a_noise, _ = rand!(df, noisevec)
    else
        x_noise, y_noise, v_noise, a_noise = 0.,0.,0.,0.
    end
    # next states
    dy, dx = sincos(a)
    x += x_noise + dx*v
    y += y_noise + dy*v
    v⁺ = max(0.999v + v_noise, 0.0)
    m⁺ = m == 0 ? Int(rand() < switch_prob) : 1
    a += a_noise*(1 + 6m⁺)
    SVector{5,Float64}(x, y, v⁺, a, m⁺)
end
function measurement_likelihood(s,y,t)
    logpdf(dg, pos(s)-y) # A simple linear measurement model with normal additive noise
end
@inline measurement(s,t=1) = pos(s)
@inline measurement(s,t,noise) = measurement(s) + noise*rand(dg) # We observer the position coordinates with the measurement

function smoothed(xy)
    d0 = MvNormal(SVector(xy[1]..., norm(xy[2] - xy[1]), atan(reverse(xy[2] - xy[1])...), 0), Float64[1, 1, 1, 1, 0])
    u = zeros(length(xy))
    pf = AuxiliaryParticleFilter(AdvancedParticleFilter(N, dynamics, measurement, measurement_likelihood, df, d0))
    # x1,w,we,ll=forward_trajectory(pf,u,xy)
    # xh = mean_trajectory(x1,we)
    sb, _ = smooth(pf, M, u, xy) # Sample smooting particles (b for backward-trajectory)
    s = smoothed_mean(sb)     # Calculate the mean of smoothing trajectories
    s[1:2, :], s[3, :], s[4, :], s[5, :]
end

# 26 41
#=i = Common(x[5])
xy = i.track.xy
_xy, v, a, m = smoothed(xy)
xys = Point.(eachcol(_xy))
tp = findfirst(x -> x > 0.5, m)
sc = Scene(scale_plot = false, limits = FRect(-100, -100, 200, 200))
s1 = slider(2:length(xy), raw = true, camera = campixel!, start = 2)
lines!(sc, lift(i -> xy[1:i], s1[end][:value]), color = :black)
lines!(sc, lift(i -> xys[1:i], s1[end][:value]), color = :red)
scatter!(sc, lift(i -> i > tp ? [xys[tp]] : [Point(NaN, NaN)], s1[end][:value]), color = :blue, markersize = 5)
hbox(s1, sc)=#


for N in [Float64, Float32, Rational{Int}]
    # not implemented for dimension != 2
    p = BallInf(zeros(N, 3), N(1))
    @test_throws ArgumentError surface(p)
    @test_throws AssertionError area(p)

    # sets with zero area
    p = Singleton(N[0, 1])
    @test area(p) ≈ N(0)
    p = Interval(N(0), N(1)) × Interval(N(0), N(0))
    @test area(p) ≈ N(0)

    # triangle
    p = VPolygon([N[13, 14], N[16, 30], N[50, 10]])
    @test area(p) ≈ N(302)

    # quadrilateral
    vlist = [N[3, 4], N[5, 11], N[12, 8], N[9, 5], N[5, 6]]
    p = VPolygon(vlist) # convex hull => 4 points
    @test area(p) ≈ N(35)

    # more than four vertices
    p2 = VPolygon([N[1, 0], N[0, 0], N[0, 1], N[1/2, 2], [1, 1]])
    @test area(p2) == N(3/2)

    # non-convex polygon (via list of vertices)
    @test LazySets._area_polygon(vlist) ≈ N(30)

    # test surface function, which falls back to the area function for 2D sets
    @test surface(p) ≈ N(35)
end


function klein(m::AbstractModel)

    #################
    # Linearization:
    #################
    Jac1 = Matrix{Float64}(jacobian(m))
    ##################################################################################
    # Klein Solution Method---apply generalized Schur decomposition a la Klein (2000)
    ##################################################################################

    # NK is number of predetermined variables
    NK = get_setting(m, :n_predetermined_variables)
    # n is number of variables (predet + non-predet)
	n = size(Jac1, 1)

    # A and B matrices
	A::Matrix{Float64} = Jac1[:, 1:n]
	B = -Jac1[:, n+1:2*n]

    # Apply generlaized Schur decomposition
    # A ≈ QZ[:Q]*QZ[:S]*QZ[:Z]'
    # B ≈ QZ[:Q]*QZ[:T]*QZ[:Z]'
	QZ = schur(A,B)

    # Reorder so that stable comes first
    alpha::Vector{complex(promote_type(eltype(A), eltype(B)))} = QZ.α
    beta::Vector{complex(promote_type(eltype(A), eltype(B)))} = QZ.β
	eigs = QZ.β ./ QZ.α #real(QZ.β ./ QZ.α)
	eigselect::AbstractArray{Bool} = abs.(eigs) .< 1 # returns false for NaN gen. eigenvalue which is correct here bc they are > 1
	ordschur!(QZ, eigselect)

    # Check that number of stable eigenvalues equals the number of predetermined state variables
	nk = sum(eigselect)
	if nk>NK
	    @warn "Equilibrium is locally indeterminate"
	elseif nk<NK
	    @warn "No local equilibrium exists"
	end

	U::Matrix{Float64} = QZ.Z'
	T::Matrix{Float64} = QZ.T
	S::Matrix{Float64} = QZ.S

    U11 = Matrix{Float64}(undef, NK, NK)
    U12 = Matrix{Float64}(undef, NK, NK-2)
    U21 = Matrix{Float64}(undef, NK-2, NK)
    U22 = Matrix{Float64}(undef, NK-2, NK-2)
    S11 = Matrix{Float64}(undef, NK, NK)
    T11 = Matrix{Float64}(undef, NK, NK)

    U11 = U[1:NK,1:NK]
	U12 = U[1:NK,NK+1:end]
	U21 = U[NK+1:end,1:NK]
	U22 = U[NK+1:end,NK+1:end]

	S11 = S[1:NK,1:NK]
	T11 = T[1:NK,1:NK]

    # Find minimum norm solution to U₂₁ + U₂₂*g_x = 0 (more numerically stable than -U₂₂⁻¹*U₂₁)
    gx_coef = Matrix{Float64}(undef, n-NK, NK)
	gx_coef = try
        -U22'*pinv(U22*U22')*U21
    catch ex
        if isa(ex, LinearAlgebra.LAPACKException)
            #@info "LAPACK exception thrown while computing pseudo inverse of U22*U22'"
            return gx_coef, Array{Float64, 2}(undef, NK, NK), -1
        else
            rethrow(ex)
        end
    end

    # Solve for h_x (in a more numerically stable way)
	S11invT11 = S11\T11;
	Ustuff = (U11 + U12*gx_coef);
	invterm = try
        pinv(eye(NK) + gx_coef' * gx_coef)
    catch ex
        if isa(ex, LinearAlgebra.LAPACKException)
            #@info "LAPACK exception thrown while computing pseudo inverse of eye(NK) + gx_coef'*gx_+coef"
            return gx_coef, Array{Float64, 2}(undef, NK, NK), -1
        else
            rethrow(ex)
        end
    end
    # hx_coef = Array{Float64, 2}(NK, NK)
	hx_coef = invterm*Ustuff'*S11invT11*Ustuff;

	# Ensure that hx and S11invT11 should have same eigenvalues
	# (eigst,valst) = eig(S11invT11);
	# (eighx,valhx) = eig(hx_coef);
	eigst = eigvals(S11invT11)
    eighx = eigvals(hx_coef)
    if abs(norm(eighx, Inf) - norm(eigst, Inf)) > 1e-4
		@warn "max abs eigenvalue of S11invT11 and hx are different!"
	end

	# next, want to represent policy functions in terms of meaningful things
	# gx_fval = Qy'*gx_coef*Qx
	# hx_fval = Qx'*hx_coef*Qx
    return gx_coef, hx_coef, 0
end

# Need an additional transition_equation function to properly stack the
# individual state and jump transition matrices/shock mapping matrices to
# a single state space for all of the model_states
function klein_transition_matrices(m::AbstractModel,
                                   TTT_state::Matrix{Float64}, TTT_jump::Matrix{Float64})
    TTT = zeros(n_model_states(m), n_model_states(m))

    # Loading mapping time t states to time t+1 states
    TTT[1:n_backward_looking_states(m), 1:n_backward_looking_states(m)] = TTT_state

    # Loading mapping time t jumps to time t+1 states
    TTT[1:n_backward_looking_states(m), n_backward_looking_states(m)+1:end] .= 0.

    # Loading mapping time t states to time t+1 jumps
    TTT[n_backward_looking_states(m)+1:end, 1:n_backward_looking_states(m)] = TTT_jump*TTT_state

    # Loading mapping time t jumps to time t+1 jumps
    TTT[n_backward_looking_states(m)+1:end, n_backward_looking_states(m)+1:end] .= 0.

    RRR = shock_loading(m, TTT_jump)

    return TTT, RRR
end


@testset "fixedpoints" begin
    A = [0.5 0; 0 0.2]
    b = [1.0; 2.0]
    f(x) = A * x + b

    x0 = rand(2)

    Γx, x, sc = fixedpoint(f, x0)

    @test sc == :success
    @test norm(Γx-x) <= sqrt(eps(eltype(x)))

    As = SMatrix{2,2}(0.5,0,0,0.2)
    bs = SVector(1.0, 2.0)
    xs0 = SVector(0.0, 0.0)
    fs(xs) = As * xs + bs

    Γxs, xs, scs = fixedpoint(fs, xs0)
    @test scs == :success
end


# ---
# title: 483. Smallest Good Base
# id: problem483
# author: Indigo
# date: 2021-02-03
# difficulty: Hard
# categories: Math, Binary Search
# link: <https://leetcode.com/problems/smallest-good-base/description/>
# hidden: true
# ---
#
# For an integer n, we call k>=2 a _**good base**_ of n, if all digits of n base
# k are 1.
#
# Now given a string representing n, you should return the smallest good base of
# n in string format.
#
# **Example 1:**
#
#
#
#     Input: "13"
#     Output: "3"
#     Explanation: 13 base 3 is 111.
#
#
#
#
# **Example 2:**
#
#
#
#     Input: "4681"
#     Output: "8"
#     Explanation: 4681 base 8 is 11111.
#
#
#
#
# **Example 3:**
#
#
#
#     Input: "1000000000000000000"
#     Output: "999999999999999999"
#     Explanation: 1000000000000000000 base 999999999999999999 is 11.
#
#
#
#
# **Note:**
#
#   1. The range of n is [3, 10^18].
#   2. The string representing n is always valid and will not have leading zeros.
#
#
#
#
## @lc code=start
using LeetCode

function smallest_good_base(n::Int)
    for m in 62:-1:2
        k = floor(Int, n^(1 / m))
        k ≤ 1 && continue
        s = (Int128(k)^(m + 1) - 1) ÷ (k - 1)
        (s == n) && return k
    end
    n - 1
end
## @lc code=end


using Test
using PolynomialBasis
using ImplicitDomainQuadrature
# using Revise
using CutCellDG
include("../useful_routines.jl")

function bulk_modulus(l, m)
    return l + 2m / 3
end

function lame_lambda(k, m)
    return k - 2m / 3
end

function analytical_coefficient_matrix(inradius, outradius, ls, ms, lc, mc)
    a = zeros(3, 3)
    a[1, 1] = inradius
    a[1, 2] = -inradius
    a[1, 3] = -1.0 / inradius
    a[2, 1] = 2 * (lc + mc)
    a[2, 2] = -2 * (ls + ms)
    a[2, 3] = 2ms / inradius^2
    a[3, 2] = 2(ls + ms)
    a[3, 3] = -2ms / outradius^2
    return a
end

function analytical_coefficient_rhs(ls, ms, theta0)
    r = zeros(3)
    Ks = bulk_modulus(ls, ms)
    r[2] = -Ks * theta0
    r[3] = Ks * theta0
    return r
end

struct AnalyticalSolution
    inradius::Any
    outradius::Any
    center::Any
    A1c::Any
    A1s::Any
    A2s::Any
    ls::Any
    ms::Any
    lc::Any
    mc::Any
    theta0::Any
    function AnalyticalSolution(
        inradius,
        outradius,
        center,
        ls,
        ms,
        lc,
        mc,
        theta0,
    )
        a = analytical_coefficient_matrix(inradius, outradius, ls, ms, lc, mc)
        r = analytical_coefficient_rhs(ls, ms, theta0)
        coeffs = a \ r
        new(
            inradius,
            outradius,
            center,
            coeffs[1],
            coeffs[2],
            coeffs[3],
            ls,
            ms,
            lc,
            mc,
            theta0,
        )
    end
end

function radial_displacement(A::AnalyticalSolution, r)
    if r <= A.inradius
        return A.A1c * r
    else
        return A.A1s * r + A.A2s / r
    end
end

function (A::AnalyticalSolution)(x)
    relpos = x - A.center
    r = sqrt(relpos' * relpos)
    ur = radial_displacement(A, r)
    if ur ≈ 0.0
        [0.0, 0.0]
    else
        costheta = (x[1] - A.center[1]) / r
        sintheta = (x[2] - A.center[2]) / r
        u1 = ur * costheta
        u2 = ur * sintheta
        return [u1, u2]
    end
end

function onboundary(x, L, W)
    return x[2] ≈ 0.0 || x[1] ≈ L || x[2] ≈ W || x[1] ≈ 0.0
end

function displacement_error(
    width,
    center,
    inradius,
    outradius,
    stiffness,
    theta0,
    nelmts,
    polyorder,
    numqp,
    penaltyfactor;
    eta = +1,
)
    L = W = width

    lambda1, mu1 = CutCellDG.lame_coefficients(stiffness, +1)
    lambda2, mu2 = CutCellDG.lame_coefficients(stiffness, -1)
    transfstress =
        CutCellDG.plane_strain_transformation_stress(lambda1, mu1, theta0)

    dx = width / nelmts
    meanmoduli = 0.5 * (lambda1 + lambda2 + mu1 + mu2)
    penalty = penaltyfactor / dx * meanmoduli

    analyticalsolution = AnalyticalSolution(
        inradius,
        outradius,
        center,
        lambda1,
        mu1,
        lambda2,
        mu2,
        theta0,
    )

    elasticitybasis = LagrangeTensorProductBasis(2, polyorder)
    levelsetbasis = HermiteTensorProductBasis(2)

    dim, nf = size(interpolation_points(levelsetbasis))
    refpoints = interpolation_points(elasticitybasis)

    mesh = CutCellDG.DGMesh(
        [0.0, 0.0],
        [width, width],
        [nelmts, nelmts],
        refpoints,
    )
    cgmesh = CutCellDG.CGMesh([0.0, 0.0], [width, width], [nelmts, nelmts], nf)

    levelset = CutCellDG.LevelSet(
        x -> -circle_distance_function(x, center, inradius)[1],
        cgmesh,
        levelsetbasis,
    )

    cutmesh = CutCellDG.CutMesh(mesh, levelset)
    cellquads = CutCellDG.CellQuadratures(cutmesh, levelset, numqp)
    interfacequads = CutCellDG.InterfaceQuadratures(cutmesh, levelset, numqp)
    facequads = CutCellDG.FaceQuadratures(cutmesh, levelset, numqp)

    mergedmesh =
        CutCellDG.MergedMesh!(cutmesh, cellquads, facequads, interfacequads)

    sysmatrix = CutCellDG.SystemMatrix()
    sysrhs = CutCellDG.SystemRHS()

    CutCellDG.assemble_displacement_bilinear_forms!(
        sysmatrix,
        elasticitybasis,
        cellquads,
        stiffness,
        mergedmesh,
    )
    CutCellDG.assemble_bulk_transformation_linear_form!(
        sysrhs,
        transfstress,
        elasticitybasis,
        cellquads,
        mergedmesh,
    )
    CutCellDG.assemble_interelement_condition!(
        sysmatrix,
        elasticitybasis,
        facequads,
        stiffness,
        mergedmesh,
        penalty,
        eta,
    )
    CutCellDG.assemble_interelement_transformation_linear_form!(
        sysrhs,
        transfstress,
        elasticitybasis,
        facequads,
        mergedmesh,
    )

    CutCellDG.assemble_coherent_interface_condition!(
        sysmatrix,
        elasticitybasis,
        interfacequads,
        stiffness,
        mergedmesh,
        penalty,
        eta,
    )
    CutCellDG.assemble_coherent_interface_transformation_linear_form!(
        sysrhs,
        transfstress,
        elasticitybasis,
        interfacequads,
        mergedmesh,
    )

    CutCellDG.assemble_penalty_displacement_bc!(
        sysmatrix,
        sysrhs,
        analyticalsolution,
        elasticitybasis,
        facequads,
        stiffness,
        mergedmesh,
        x -> onboundary(x, L, W),
        penalty,
    )
    CutCellDG.assemble_penalty_displacement_transformation_linear_form!(
        sysrhs,
        transfstress,
        elasticitybasis,
        facequads,
        mergedmesh,
        x -> onboundary(x, L, W),
    )

    matrix = CutCellDG.sparse_displacement_operator(sysmatrix, mergedmesh)
    rhs = CutCellDG.displacement_rhs_vector(sysrhs, mergedmesh)

    solution = matrix \ rhs
    nodaldisplacement = reshape(solution, 2, :)

    err = mesh_L2_error(
        nodaldisplacement,
        analyticalsolution,
        elasticitybasis,
        cellquads,
        mergedmesh,
    )

    return err
end

function test_circular_interface()
    lambda1, mu1 = 100.0, 80.0
    lambda2, mu2 = 80.0, 60.0
    theta0 = -0.067
    stiffness = CutCellDG.HookeStiffness(lambda1, mu1, lambda2, mu2)

    width = 1.0
    penaltyfactor = 1e3

    polyorder = 2
    numqp = required_quadrature_order(polyorder) + 2

    center = [width / 2, width / 2]
    inradius = width / 4
    outradius = width

    powers = [3, 4, 5]
    nelmts = [2^p + 1 for p in powers]

    err = [
        displacement_error(
            width,
            center,
            inradius,
            outradius,
            stiffness,
            theta0,
            ne,
            polyorder,
            numqp,
            penaltyfactor,
        ) for ne in nelmts
    ]

    dx = 1.0 ./ nelmts
    u1err = [er[1] for er in err]
    u2err = [er[2] for er in err]

    u1rate = convergence_rate(dx, u1err)
    u2rate = convergence_rate(dx, u2err)

    @test all(u1rate .> 2.8)
    @test all(u2rate .> 2.8)
end

function test_corner_curved_interface()
    lambda1, mu1 = 100.0, 80.0
    lambda2, mu2 = 80.0, 60.0
    theta0 = -0.067
    stiffness = CutCellDG.HookeStiffness(lambda1, mu1, lambda2, mu2)

    width = 1.0
    penaltyfactor = 1e3

    polyorder = 2
    numqp = required_quadrature_order(polyorder) + 2

    center = [width, width]
    inradius = width / 2
    outradius = 2width
    powers = [3, 4, 5]
    nelmts = [2^p + 1 for p in powers]

    err = [
        displacement_error(
            width,
            center,
            inradius,
            outradius,
            stiffness,
            theta0,
            ne,
            polyorder,
            numqp,
            penaltyfactor,
        ) for ne in nelmts
    ]

    dx = 1.0 ./ nelmts
    u1err = [er[1] for er in err]
    u2err = [er[2] for er in err]

    u1rate = convergence_rate(dx, u1err)
    u2rate = convergence_rate(dx, u2err)

    @test all(u1rate .> 2.9)
    @test all(u2rate .> 2.9)
end

test_circular_interface()
test_corner_curved_interface()


using MatrixNetworks
using DelimitedFiles
A = MatrixNetworks.readSMAT("commutes-all.smat")
xy = readdlm("commutes-all.xy")
##
using SparseArrays
i = rand(1:size(A,1))
scatter(xy[:,1],xy[:,2],markersize=1, markerstrokewidth=0)
neighs,vals = findnz(A[:,i])
println(A[:,i].nzval)
scatter!([xy[i,1]], [xy[i,2]], markersize=9)
scatter!(xy[neighs,1],xy[neighs,2], markerstrokewidth=0, markersize=5, marker_z = vals)

##
##
using Plots
using GraphRecipes
using SparseArrays
src,dst = findnz(A)[1:2]
graphplot(src,dst,x = xy[:,1], y = xy[:,2],
  markersize=0, linewidth=0.5, linealpha=0.1)


using MyGraph

G1 = @buildgraph false 0,1,4 0,7,8 1,2,8 1,7,11 2,3,7 2,8,2 2,5,4 3,4,9 3,5,14 4,5,10 5,6,2 6,8,6 6,7,1 7,8,7
D1,T1 = mstprimjarnik(G1)

G2 = buildconnectedgraph(collect(0:11); f = (x,y) -> rand(10:200))
D2,T2 = mstprimjarnik(G2)

printpath(D2, 0,5)


# Copyright (c) 2021, Joseph M. Rutherford
# All rights reserved.
#
# Code provided under the license contained in the LICENSE file.

include("../src/sudoku.jl") # Defines Sudoku module

# Form a random solution and puzzle pair
using Random
solution, puzzle = Sudoku.random_puzzle(3,MersenneTwister(12345),10000,45)

# Solve puzzle and report findings
print("\nUnsolved puzzle\n")
print(Sudoku.as_text_grid(puzzle))
iterations, uncertainty = Sudoku.iterative_solve!(puzzle,2)
print("\n\nSolution after $iterations step(s)\n")
print(Sudoku.as_text_grid(puzzle))

if uncertainty != 0
    print("Failure: puzzle solution is incomplete\n")
else
    if Sudoku.as_values(puzzle) == Sudoku.as_values(solution)
        print("Success: solved puzzle matches expected solution\n")
    else
        print("Failure: solved puzzle does not match expected solution\n")
        print("Expected\n")
        print(Sudoku.as_text_grid(solution))
    end
end


using ManifoldLearning
using Test
import Random

Random.seed!(3483743871)

@testset "ManifoldLearning" begin

    # setup parameters
    k = 12
    d = 2
    X, L = ManifoldLearning.swiss_roll()

    # test NN graph
    D, E = ManifoldLearning.find_nn(X, k)
	@test size(X,2) == size(D,2) && size(D, 1) == k
	@test size(X,2) == size(E,2) && size(E, 1) == k

    # test algorithms
    @testset for algorithm in [Isomap, LEM, LLE, HLLE, LTSA, DiffMap]
        for (k, T) in zip([5, 12], [Float32, Float64])
            # construct KW parameters
            kwargs = [:maxoutdim=>d]
            if algorithm == DiffMap
                push!(kwargs, :t => k)
            else
                push!(kwargs, :k => k)
            end

            # call transformation
            Y = fit(algorithm, convert(Array{T}, X); kwargs...)

            # test results
            @test outdim(Y) == d
            @test size(transform(Y), 2) == size(X, 2)
            @test length(split(sprint(show, Y), '\n')) > 1
            if algorithm !== DiffMap
                @test neighbors(Y) == k
                @test length(eigvals(Y)) == d
            end
            if algorithm === Isomap || algorithm === LEM
                @test length(vertices(Y)) > 1
            end
        end
    end

end


#=

  Euler #45 in Julia.

  Problem 45:
  """
  Triangle, pentagonal, and hexagonal numbers are generated by the following formulae:

  Triangle 	  	Tn=n(n+1)/2 	  	1, 3, 6, 10, 15, ...
  Pentagonal 	  	Pn=n(3n−1)/2 	  	1, 5, 12, 22, 35, ...
  Hexagonal 	  	Hn=n(2n−1) 	  	1, 6, 15, 28, 45, ...

  It can be verified that T(285) = P(165) = H(143) = 40755.

  Find the next triangle number that is also pentagonal and hexagonal.
  """

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

=#

include("Euler.jl")

pent(n) = floor(Int, n*(3*n-1) / 2)
tri(n) = floor(Int, n*(n+1) / 2)
hex(n) = n*(2*n-1)

# 0.00013708s
function euler45a()
    t = 285+1
    tt = tri(t)
    p = 165
    pp = pent(p)
    h = 143
    hh = hex(h)
    while tt != pp || pp != hh
        t += 1
        tt = tri(t)
        if tt > pp p+=1; pp = pent(p) end
        if pp > hh h+=1; hh = hex(h) end
        if tt > hh h+=1; hh = hex(h) end
    end

    return tt
end

run_euler(euler45a)


using Yao
using Yao.Intrinsics

"""
    binary_basis(geometry...) -> BitArray

Return bases in reshaped binary form (e.g. binary images)
"""
binary_basis(geometry...) = reshape(bitarray(geometry|>prod|>basis|>collect, num_bit=prod(geometry)), (geometry..., :))

# a sample is a bar or a stripe.
is_bar(mat::AbstractMatrix) = sum(abs.(diff(mat, 1)), (1, 2))[] == 0
is_stripe(mat::AbstractMatrix) = sum(abs.(diff(mat, 2)), (1, 2))[] == 0
is_bs(mat::AbstractMatrix) = is_bar(mat) || is_stripe(mat)

"""
    barstripe_pdf(M::Int, N::Int) -> Vector{Float64}

get bar and stripes PDF
"""
function barstripe_pdf(M::Int, N::Int)
    bss = binary_basis(M, N)
    [bss[:,:,i] |> is_bs for i in indices(bss, 3)] |> normalize
end

"""
    bs_configs(M::Int, N::Int) -> BitArray{3}

the total configuration space for bar and stripes.
"""
function bs_configs(M::Int, N::Int)
    bss = binary_basis(M, N)
    slicedim(bss, 3, [bss[:,:,i] |> is_bs for i in indices(bss, 3)])
end


using FANN
using Base.Test

# test simple linear problem for which we know the solution
net = ANN([2, 1], [:linear], b=0.1)

X = [ 0.0 1.0 0.0 ;
	  0.0 0.0 1.0 ]
y = [ 0.0 1.0 1.0 ]

dset = DataSet(X, y)

# train
train!(net, dset,
	   max_epochs=1000,
	   epochs_between_reports=0,
	   desired_error=1e-8)

# there is somethign strange here, since the network stops training
# but the mean square error is not less than the desired error. Where
# is this bug from?
out = predict(net, X)
@test sumabs2(out - y) < 100*1e-8


using GeneticAlgorithms
include("equalityga.jl")

model = runga(equalityga; initial_pop_size = 16)

population(model)


@testset "Reproducing Coles Results" begin
    @testset "Port Pirie example from Coles (2001)" begin

        data = Extremes.dataset("portpirie")

        # Fit of the GEV distribution by maximum likelihood
        fm = gevfit(data, :SeaLevel)

        # Parameter estimates
        θ̂ = fm.θ̂

        # Approximate variance-covariance matrix of the parameter estimates
        V̂ = Extremes.parametervar(fm)

        # correction factor for using ϕ instead of σ (computation using the delta method)
        c = [
            1.0 exp(θ̂[2]) 1.0
            exp(θ̂[2]) exp(2 * θ̂[2]) exp(θ̂[2])
            1.0 exp(θ̂[2]) 1.0
        ]

        # 10-year return level
        R₁₀ = returnlevel(fm, 10)
        R₁₀_cint = cint(R₁₀, 0.95)
        # 100-year return level
        R₁₀₀ = returnlevel(fm, 100)
        R₁₀₀_cint = cint(R₁₀₀, 0.95)

        # Parameter estimates in Coles
        θ = [3.87; log(0.198); -0.050]

        # Approximate variance-covariance matrix of the parameter estimates in Coles
        V = [
            0.000780 0.000197 -.00107
            0.000197 0.000410 -.000778
            -.00107 -.000778 0.00965
        ]

        @test θ̂ ≈ θ rtol = 0.1
        @test c .* V̂ ≈ V rtol = 0.1
        @test Extremes.loglike(fm.model, θ̂) ≈ 4.34 rtol = 0.1

        confint = cint(fm)
        @test confint[1] ≈ [3.82; 3.93] atol = .02
        @test confint[2] ≈ log.([0.158,0.238]) atol = .05
        @test confint[3] ≈ [-0.242; 0.142] atol = .02

        @test R₁₀.value[] ≈ 4.30 rtol = 0.1
        @test R₁₀_cint[] ≈ [4.19; 4.45] rtol = 0.1
        @test R₁₀₀.value[] ≈ 4.69 rtol = 0.1
        @test R₁₀₀_cint[] ≈ [4.5; 5.27] rtol = 0.1

    end

    @testset "Fremantle example from Coles (2001)" begin

        df = Extremes.dataset("fremantle")

        n = length(df[:, :SeaLevel])

        t = collect(1:n)

        fm = gevfit(df[:, :SeaLevel], locationcov = [Variable("t", t)])

        # Parameter estimates
        θ̂ = fm.θ̂

        # Variance of the parameter estimates
        V̂ = diag(Extremes.parametervar(fm))

        # Computing the variance of σ instead of ϕ with the delta method
        V̂[3] = exp(2 * θ̂[3]) * V̂[3]

        # Computing the standard errors of the parameters estimates
        Ŝ = sqrt.(V̂)

        # Values given in Coles (2001, Chap 6) for the temporal linear trend in μ
        θ = [1.38; 0.00203; log(0.124); -.125]
        S = [0.03; 0.00052; 0.010; 0.070]

        @test θ̂ ≈ θ rtol = 0.1
        @test Ŝ ≈ S rtol = 0.1
        @test Extremes.loglike(fm.model, θ̂) ≈ 49.9 rtol = 0.1

        evt = Variable("t", t)
        evt² = Variable("t²", t .^ 2)
        evSOI = Variable("SOI", df[:, :SOI])

        # Quadratic trend in μ
        fm = gevfit(df[:, :SeaLevel], locationcov = [evt, evt²])
        @test Extremes.loglike(fm.model, fm.θ̂) ≈ 50.6 rtol = 0.1

        # Linear trend in both μ and ϕ
        fm = gevfit(df[:, :SeaLevel], locationcov = [evt], logscalecov = [evt])
        @test Extremes.loglike(fm.model, fm.θ̂) ≈ 50.7 rtol = 0.1

        # μ as a function of the SOI
        fm = gevfit(df, :SeaLevel, locationcovid = [:SOI])
        @test Extremes.loglike(fm.model, fm.θ̂) ≈ 47.2 rtol = 0.1

        # Linear trend in μ as function of the time and the SOI
        fm = gevfit(df[:, :SeaLevel], locationcov = [evt, evSOI])
        @test Extremes.loglike(fm.model, fm.θ̂) ≈ 53.9 rtol = 0.1
        @test fm.θ̂[3] ≈ 0.055 rtol = 0.1

    end

    @testset "Rainfall example from Coles (2001)" begin
        df = Extremes.dataset("rain")

        threshold = 30

        data = filter(row -> row.Rainfall > threshold, df)

        y = data[:, :Rainfall] .- threshold

        fm = gpfit(y)

        θ̂ = fm.θ̂

        # Approximate variance-covariance matrix of the parameter estimates
        V̂ = Extremes.parametervar(fm)

        # correction factor for using ϕ instead of σ (computation using the delta method)
        c = [
            exp(2*θ̂[1]) exp(θ̂[1])
            exp(θ̂[1]) 1.0
        ]

        V̂ = c.*V̂
        V = [.9188 -.0655 ;
            -.0655 .0102]

        @test fm.θ̂ ≈ [log(7.44); 0.184] rtol = 0.1
        @test Extremes.loglike(fm.model, fm.θ̂) ≈ -485.1 rtol = 0.1
        @test V̂ ≈ V rtol = 0.1

        r = returnlevel(fm, threshold, size(df,1), 365, 100)

        @test r.value[] ≈ 106.3 rtol = .1

    end

    @testset "Dow Jones example from Coles (2001), Section 4.4.2" begin
        df = Extremes.dataset("dowjones")

        X = df[:,:Index]

        X̃ = 100*[ log(X[i+1]) - log(X[i])  for i in 1:(length(X)-1)]

        n = length(X̃)

        threshold = 2.0

        z = X̃[X̃ .> threshold] .- threshold

        fm = gpfit(z)

        θ̂ = fm.θ̂

        # Approximate variance-covariance matrix of the parameter estimates
        V̂ = Extremes.parametervar(fm)

        # correction factor for using ϕ instead of σ (computation using the delta method)
        c = [
            exp(2*θ̂[1]) exp(θ̂[1])
            exp(θ̂[1]) 1.0
        ]

        V̂ = c.*V̂

        # Test the Generalized Pareto estimates
        @test exp(fm.θ̂[1]) ≈ .495 rtol = 0.1
        @test fm.θ̂[2] ≈ .288 rtol = 0.1

        # Test the standard errors of estimates
        @test sqrt(V̂[1,1]) ≈ .150 rtol = 0.1
        @test sqrt(V̂[2,2]) ≈ .258 rtol = 0.1

    end
end


module WeightedLS

using LinearAlgebra

greet() = print("Hello World!")

function normalLS(A::Array{Float64,2}, b::Array{Float64,1})
    return A\b
end

# On the modified Gram-Schmidt algorithm for weighted and constrained linear least squares problems
# https://github.com/borglab/gtsam/blob/develop/gtsam/linear/NoiseModel.cpp#L445
# solve min (b-Ax)^TW(b-Ax)
function weightedLS(A::Array{Float64,2}, b::Array{Float64,1}, W::Array{Float64,1})
    # TODO: input size check

    # get size, A is mxn, b is nx1, W is nx1
    m = size(A,1)
    n = size(A,2)
    max_rank = min(m,n)
    Ab = [A b]
    weights = W;
    outweights = zeros(n, 1)
    Rd = zeros(m, n+1)
    rd_row = 1
    rd = zeros(1, n+1)
    for j=1:n  # loop over columns
        a = Ab[1:m,j:j]  # get jth column of new system
        constraint_row = checkConstraint(dropdims(a,dims=2),W,m)
        if constraint_row > 0
            rd = Ab[constraint_row:constraint_row,:]      # get constraint row
            Rd[rd_row,:] = rd                             # row of R matrix
            outweights[rd_row] = weights[constraint_row]
            rd_row += 1
            if rd_row > max_rank
                break
            end

            m -= 1  # decrease number of rows?

            if constraint_row != m+1
                Ab[constraint_row,:] = Ab[m+1,:]
                weights[constraint_row,:] = weights[m+1,:]
            end
            a_reduced = Ab[1:m,j:j]

            a_reduced = a_reduced .* 1.0/rd[1,j]  # divide by the jth column
            Ab[1:m,j+1:n+1] -= a_reduced * rd[1:1,j+1:n+1]  # subtract off the rest of the row?
        else
            precision = 0.0
            pseudo = zeros(m, 1)
            rd = zeros(1, n+1)
            for i=1:m
                ai = a[i]
                if (abs(ai) > 1e-9)
                    pseudo[i,1] = weights[i]*ai
                    precision += pseudo[i]*ai
                else
                    pseudo[i,1] = 0.0
                end
            end
            if precision > 1e-8
                pseudo = pseudo ./ precision
                rd[1,j] = 1.0
                rd[1:1,j+1:n+1] = pseudo' * Ab[1:m,j+1:n+1]
                Rd[rd_row,:] = rd
                outweights[rd_row] = precision
                rd_row += 1
            else
                continue
            end
            if rd_row > max_rank
                break
            end
            Ab[1:m,j+1:n+1] -= a * rd[1:1,j+1:n+1]

        end
    end
    return Rd, outweights
end

function checkConstraint(a::Array{Float64,1}, W::Array{Float64,1}, m::Integer)
    max_element = 1e-9
    constraint_row = -1
    for i = 1:m
        if W[i] != Inf
            continue
        end
        abs_ai = abs(a[i])
        if abs_ai > max_element
            max_element = abs_ai
            constraint_row = i
        end
    end
    return constraint_row
end

end # module


using Thebes, Luxor, Colors

@png begin
    setlinejoin("bevel")
    helloworld()
    perspective(300)
    eyepoint(800, 700, 600)
    uppoint(0, 0, 10)
    setopacity(0.7)
    k = 50
    xmax = 6π
    ymax = 6π
    st = 0.15
    f(x, y) = 2(sin(x) * cos(y)) + (cos(x) * sin(y))
    setline(.5)
    fontsize(5)
    for x in -xmax:st:xmax
        for y in -ymax:st:ymax
            sethue(HSB(360rescale(f(x, y), -2, 2), .7, .7))
            p1 = Point3D(k * x,        k * y,       k * f(x, y))
            p2 = Point3D(k * (x ),     k * (y + st), k * f(x, y +st))
            p3 = Point3D(k * (x + st),  k * (y + st), k * f(x + st, y + st))
            p4 = Point3D(k * (x + st),  k * y,       k * f(x + st, y))
            pin([p1, p2, p3, p4], gfunction = (p3s, p2s) -> begin
                poly(p2s, close=true, :fill)
                sethue("red")
                poly(p2s, close=true, :stroke)
            end)
        end
    end
    axes3D()
end 500 500 "/tmp/t.png"


### fnt.jl
#
# Copyright (C) 2020 Andrey Oskin.
# Copyright (C) 2020 Jakub Wronowski.
#
# Maintainer: Jakub Wronowski <jakubwro@users.noreply.github.com>
# Keywords: number theoretic transform, fermat number transform
#
# This file is a part of NumberTheoreticTransforms.jl.
#
# License is MIT.
#
### Commentary:
#
# This file contains implementation of Fermat Number Transform.
#
### Code:

export fnt, fnt!, ifnt, ifnt!, modfermat, isfermat

"""
    isfermat(n)

Checks if a given number is a Fermat number \$ 2^{2^t}+1 \$.
"""
function isfermat(number::T) where {T<:Integer}
    if !ispow2(number - one(T))
        return false
    end

    return ispow2(trailing_zeros(number - one(T)))
end

"""
    modfermat(n, q)

Equivalent of mod(n, q) but uses faster algorithm.
Constraints:
-   `q` must be a Fermat number \$ 2^{2^t}+1 \$
-   `n` must be smaller or equal to \$ (q-1)^2 \$
-   `n` must be grater or equal to \$ 0 \$
If above constraints are not met, the result is undefined.
"""
function modfermat(n::T, q::T) where T <: Integer
    x = n & (q - T(2)) - n >>> trailing_zeros(q - T(1)) + q
    x = x >= q ? x - q : x
end

"""
Order input to perform radix-2 structured calculation.
It sorts array by bit-reversed 0-based sample index.
"""
function radix2sort!(data::Array{T, 1}) where {T<:Integer}
    N = length(data)
    @assert ispow2(N)

    l = 1
    for k in 1:N
        if l > k
            data[l], data[k] = data[k], data[l]
        end

        l = l - 1
        m = N
        while l & (m >>= 1) != 0
            l &= ~m
        end
        l = (l | m) + 1
    end

    return data
end

function fnt!(x::Array{T, 1}, g::T, q::T) where {T<:Integer}
    N = length(x)
    @assert ispow2(N)
    @assert isfermat(q)
    @assert all(v -> 0 <= v < q, x)

    radix2sort!(x)

    logN = trailing_zeros(N)

    for l in 1:logN
        M = 1 << (l-1) # [1,2,4,8,...,N/2]
        interval = 1 << l # [2,4,8,...,N]
        p = 1 << (logN - l) # [N/2,...,4,2,1]
        gp = powermod(g, p, q)
        W = 1
        for m in 1:M
            for i in m:interval:N
               j = i + M
               xi, xj = x[i], x[j]
               Wxj = modfermat(W * xj, q)
               xi, xj = xi + Wxj, xi - Wxj + q
               xi = xi >= q ? xi - q : xi # mod q
               xj = xj >= q ? xj - q : xj # mod q
               x[i], x[j] = xi, xj
            end
            W = modfermat(W * gp, q)
        end
    end

    return x
end

"""
    fnt!(x, g, q)

In-place version of `fnt`. That means it will store result in the `x` array.
"""
function fnt!(x::Array{T,2}, g::T, q::T) where {T<:Integer}
    N, M = size(x)
    @assert N == M #TODO: make it work for N != M (need different g for each dim)

    for n in 1:N
        x[n, :] = fnt!(x[n, :], g, q)
    end

    for m in 1:M
        x[:, m] = fnt!(x[:, m], g, q)
    end

    return x
end

"""
    fnt(x, g, q)

The Fermat Number Transform returns the same result as `ntt` function using
more performant algorithm. When `q` has \$ 2^{2^t}+1 \$ form the calculation
can be performed with O(N*log(N)) operation instead of O(N^2) for `ntt`.
"""
function fnt(x::Array{T}, g::T, q::T) where {T<:Integer}
    return fnt!(copy(x), g, q)
end

"""
    ifnt!(y, g, q)

In-place version of `ifnt`. That means it will store result in the `y` array.
"""
function ifnt!(y::Array{T,1}, g::T, q::T) where {T<:Integer}
    N = length(y)
    inv_N = invmod(N, q)
    inv_g = invmod(g, q)

    x = fnt!(y, inv_g, q)
    for i in eachindex(x)
        x[i] = mod(inv_N * x[i], q)
    end

    return x
end

function ifnt!(y::Array{T,2}, g::T, q::T) where {T<:Integer}
    N, M = size(y)

    for m in 1:M
        y[:, m] = ifnt!(y[:, m], g, q)
    end

    for n in 1:N
        y[n, :] = ifnt!(y[n, :], g, q)
    end

    return y
end

"""
    ifnt(y, g, q)

Calculates inverse of Fermat Number Transform for array `y` using
mod \$ 2^{2^t}+1 \$ arithmetic.

The input must be array of integers calculated by `fnt` function with the same
`g` and `q` params.
"""
function ifnt(y::Array{T}, g::T, q::T) where {T<:Integer}
    return ifnt!(copy(y), g, q)
end


using OptionsPricing
@static if VERSION < v"0.7.0-DEV.2005"
    using Base.Test
else
    using Test
end

@testset "vanilla" begin
    op = Option(100.0, 90.0, 0.05, 0.3, 180/365, "Put")

    @test isapprox(bs(op),  1, 3.228199973191849 = 0.1)
    @test isapprox(crr(op), 1, 3.307679908124034 = 0.1)
    @test isapprox(mc(op),  1, 3.248427229875847 = 0.1)

    oc = Option(100.0, 90.0, 0.05, 0.3, 180/365, "Call")

    @test isapprox(bs(oc),  1, 15.420241820690038 = 0.1)
    @test isapprox(crr(oc), 1, 15.419616011114611 = 0.1)
    @test isapprox(mc(oc),  1, 15.725438452320487 = 0.1)
end


@defcomp sealevelrise begin

    ThermSLR    = Variable(index=[time])    # Path of SLR from thermal expansion
    GSICSLR     = Variable(index=[time])    # Path of SLR from G&SIC
    GISSLR      = Variable(index=[time])    # Path of SLR from GIS
    AISSLR      = Variable(index=[time])    # Path of SLR from AIS
    TotSLR      = Variable(index=[time])    # Path of total SLR

    TATM        = Parameter(index=[time])   # Path of atmospheric temperature anomalies (from climate dynamics?)

    therm0      = Parameter()   # Initial SLR from thermal expansion (meters above 2000)
    gsic0       = Parameter()   # Initial SLR from G&SIC
    gis0        = Parameter()   # Initial SLR from GIS
    ais0        = Parameter()   # Initial SLR from AIS

    therm_asym  = Parameter()   # Asymptotic rise from thermal expansion
    gsic_asym   = Parameter()   # Asymptotic rise from G&SIC
    gis_asym    = Parameter()   # Asymptotic rise from GIS
    ais_asym    = Parameter()   # Asymptotic rise from AIS

    thermrate   = Parameter()   # Rate of thermal expansion
    gsicrate    = Parameter()   # Rate of G&SIC
    gisrate     = Parameter()   # Rate of GIS
    aisrate     = Parameter()   # Rate of AIS

    slrthreshold = Parameter()  # Temperature threshold for AIS

    function run_timestep(p, v, d, t)
        if is_first(t)
            v.ThermSLR[t] = p.therm0
            v.GSICSLR[t]  = p.gsic0
            v.GISSLR[t]   = p.gis0
            v.AISSLR[t]   = p.ais0
        else

            v.ThermSLR[t] = v.ThermSLR[t - 1] + p.thermrate * p.TATM[t]
            v.GSICSLR[t]  = v.GSICSLR[t - 1] + p.gsicrate * (p.gsic_asym - v.GSICSLR[t - 1]) * p.TATM[t]
            v.GISSLR[t]   = v.GISSLR[t - 1]  + p.gisrate  * (p.gis_asym  - v.GISSLR[t - 1])  * p.TATM[t]
            v.AISSLR[t]   = 0

            if p.TATM[t] > p.slrthreshold
                v.AISSLR[t] = v.AISSLR[t - 1] + p.aisrate * (p.ais_asym - v.AISSLR[t - 1]) * p.TATM[t]
            end
        end

        v.TotSLR[t] = v.ThermSLR[t] + v.GSICSLR[t] + v.GISSLR[t] + v.AISSLR[t]
    end
end


#=
Levis, C., Johnson, J. T., & Teixeira, F. L. (2010). Radiowave propagation: physics and applications. John Wiley & Sons.
=#
@doc """
 Flat Earth Model
 freq: signal frequency
 txH: base station's height
 rxH: mobile's height
 """
type FlatEarthModel<:Model
  freq::Float64
  txH::Float64
  rxH::Float64
 FlatEarthModel() = new(800,50,1.5)
end

"Calculate Path loss for distance dist (km) with Flat Earth Model"
function pathloss_op(model::FlatEarthModel, dist::AbstractFloat)
  hm::Float64 = model.rxH
  hb::Float64 = model.txH
  d::Float64 = dist
  f::Float64 = model.freq

   L1 = -20*log10(hb)
   L2 = -20*log10(hm)
   Lo = 120 +10*4*log10(d)
   L = Lo + L1 + L2
   return L
end # pathloss_op end


using GaussianProcesses
using Random

Random.seed!(20140430)
# Training data
n=183;                          #number of training points
x = 2π * rand(n);              #predictors
y = sin.(x) + 0.05*randn(n);   #regressors

#Select mean and covariance function
mZero = MeanZero()                   #Zero mean function
kern = SE(0.0,0.0)                   #Sqaured exponential kernel (note that hyperparameters are on the log scale)
                    # log standard deviation of observation noise (this is optional)
gp = GP(x,y,mZero,kern)       #Fit the GP


function compute_world_X_depth()
    # These values are rather undocumented and come from the source of the
    # glpclview example in libfreenect.
    fx = 594.21
    fy = 591.04
    a = -0.0030711
    b = 3.3309495
    cx = 339.5
    cy = 242.7
    W = [1 / fx 0       0  -cx / fx
         0      -1 / fy 0  cy / fy
         0      0       0  -1
         0      0       a  b]
    P = [ 0.  0. -1.  0.
         -1.  0.  0.  0.
          0.  1.  0.  0.
          0.  0.  0.  1.]

    return P * W
end
const world_X_depth = compute_world_X_depth()

"""
$(TYPEDSIGNATURES)

Helper for getting a pointcloud from the camera at `index`.

The resulting point cloud is relative to the camera, with X forward, Y left,
and Z up.

This function uses a fixed homography matrix - you may have better results
for your own hardware by calibrating your Kinect.
"""
function sync_get_pointcloud(index::Integer)
    depth, timestamp = sync_get_depth(index, depth_11bit)
    cloud = zeros((3, 480, 640))
    for i ∈ 1:480, j ∈ 1:640
        uvw = Float64[j - 1, i - 1, depth[i, j], 1.0]
        xyzw = world_X_depth * uvw
        cloud[:, i, j] .= xyzw[1:3] ./ xyzw[4]
    end

    return cloud, timestamp
end
export sync_get_pointcloud


using Images
using TestImages
using StaticArrays
using OffsetArrays
using Random
using CoordinateTransformations

function evaluate_error(dims, flow::Array{SVector{2, Float64}, 2},  Δ,  tol)
    error_count = 0
    maximum_error = 0
    for r = 1:first(dims)
        for c = 1:last(dims)
            δ = flow[r,c]
            if abs(δ[1] - Δ[1]) > tol || abs(δ[2] - Δ[2]) > tol
                    error_count += 1
            end
            error = sqrt( (δ[1] - Δ[1])^2 + (δ[2] - Δ[2])^2 )
            if error > maximum_error
                maximum_error = error
            end
        end
    end
    return error_count, maximum_error
end

@testset "Farneback" begin
    @testset "Polynomial Expansion" begin
        @info "Running Polynomial Expansion test."
        img = Gray{Float64}.(testimage("mandrill"))
        @info "Running Polynomial Expansion Convolution Implementation."
        @time A, B, C = polynomial_expansion(ConvolutionImplementation(), img, 6, 2)
        @info "Running Polynomial Expansion Matrix Implementation."
        @time P, Q, R = polynomial_expansion(MatrixImplementation(), img, 6, 2)
        @test sum(abs.(vec(A)-vec(P))) ≈  0 atol=1e-9
        @test sum(abs.(vec(B)-vec(Q))) ≈  0 atol=1e-9
        @test sum(abs.(vec(C)-vec(R))) ≈  0 atol=1e-9

    end

    @testset "Horizontal Motion" begin
        @info "Running Horizontal Motion test."
        maximum_percentage_error = 7.5
        img1 = Gray{Float64}.(testimage("mandrill"))

        tol = 0.3
        Δ = (0.0, 3.0)
        trans = Translation(-Δ[1], -Δ[2])
        img2 = warp(img1, trans, axes(img1))

        algorithm = Farneback(7, estimation_window = 19,
                                 σ_estimation_window = 6.0,
                                 expansion_window = 6,
                                 σ_expansion_window = 1.5)

        @time flow = optical_flow(img1, img2, algorithm)

        error_count, maximum_error = evaluate_error(size(img1), flow, Δ, tol)
        percentage_error = (error_count / prod(size(img1))) * 100

        @info "Case: Horizontal Motion", "Percentage Error: $(percentage_error)", "Maximum Error: $(maximum_error)"
        @test percentage_error  < maximum_percentage_error
        @test maximum_error < 4

        # Same as above, except that we pass in an initial displacement field.
        displacement = Array{SVector{2, Float64}, 2}(undef,size(img1))
        for i in eachindex(displacement)
                displacement[i] = SVector{2, Float64}(0.0, 0.0)
        end
        @time flow = optical_flow(img1, img2, displacement, algorithm)

        error_count, maximum_error = evaluate_error(size(img1), flow, Δ, tol)
        percentage_error = (error_count / prod(size(img1))) * 100

        @info "Case: Horizontal Motion", "Percentage Error: $(percentage_error)", "Maximum Error: $(maximum_error)"
        @test percentage_error  < maximum_percentage_error
        @test maximum_error < 4
    end

    @testset "Vertical Motion" begin
        @info "Running Vertical Motion test."
        maximum_percentage_error = 7.5
        img1 = Gray{Float64}.(testimage("mandrill"))

        tol = 0.3
        Δ = (3.0, 0.0)
        trans = Translation(-Δ[1], -Δ[2])
        img2 = warp(img1, trans, axes(img1))

        algorithm = Farneback(7, estimation_window = 19,
                                 σ_estimation_window = 6.0,
                                 expansion_window = 6,
                                 σ_expansion_window = 1.5)

        @time flow = optical_flow(img1, img2, algorithm)

        error_count, maximum_error = evaluate_error(size(img1), flow, Δ, tol)
        percentage_error = (error_count / prod(size(img1))) * 100

        @info "Case: Vertical Motion", "Percentage Error: $(percentage_error)", "Maximum Error: $(maximum_error)"
        @test percentage_error  < maximum_percentage_error
        @test maximum_error < 4

        # Same as above, except that we pass in an initial displacement field.
        displacement = Array{SVector{2, Float64}, 2}(undef,size(img1))
        for i in eachindex(displacement)
                displacement[i] = SVector{2, Float64}(0.0, 0.0)
        end
        @time flow = optical_flow(img1, img2, displacement, algorithm)

        error_count, maximum_error = evaluate_error(size(img1), flow, Δ, tol)
        percentage_error = (error_count / prod(size(img1))) * 100

        @info "Case: Vertical Motion", "Percentage Error: $(percentage_error)", "Maximum Error: $(maximum_error)"
        @test percentage_error  < maximum_percentage_error
        @test maximum_error < 4
    end

    @testset "Combined Motion" begin
        @info "Running Combined  Motion test."
        maximum_percentage_error = 13
        img1 = Gray{Float64}.(testimage("mandrill"))

        tol = 0.3
        Δ = (3.0, -1.0)
        trans = Translation(-Δ[1], -Δ[2])
        img2 = warp(img1, trans, axes(img1))

        algorithm = Farneback(7, estimation_window = 19,
                                 σ_estimation_window = 6.0,
                                 expansion_window = 6,
                                 σ_expansion_window = 1.5)

        @time flow = optical_flow(img1, img2, algorithm)

        error_count, maximum_error = evaluate_error(size(img1), flow, Δ, tol)
        percentage_error = (error_count / prod(size(img1))) * 100

        @info "Case: Combined Motion", "Percentage Error: $(percentage_error)", "Maximum Error: $(maximum_error)"
        @test percentage_error  < maximum_percentage_error
        @test maximum_error < 4

        # Same as above, except that we pass in an initial displacement field.
        displacement = Array{SVector{2, Float64}, 2}(undef,size(img1))
        for i in eachindex(displacement)
                displacement[i] = SVector{2, Float64}(0.0, 0.0)
        end
        @time flow = optical_flow(img1, img2, displacement, algorithm)

        error_count, maximum_error = evaluate_error(size(img1), flow, Δ, tol)
        percentage_error = (error_count / prod(size(img1))) * 100

        @info "Case: Combined Motion", "Percentage Error: $(percentage_error)", "Maximum Error: $(maximum_error)"
        @test percentage_error  < maximum_percentage_error
        @test maximum_error < 4

    end

end


#################################################################
### This test was lifted from the vingette for the R gmm package.
#################################################################
## library(gmm)

## g1 <- function(tet,x) {
##     m1 <- (tet[1]-x)
##     m2 <- (tet[2]^2 - (x - tet[1])^2)
##     m3 <- x^3-tet[1]*(tet[1]^2+3*tet[2]^2)
##     f <- cbind(m1,m2,m3)
##     return(f)
## }

## Dg <- function(tet,x)
## {
## G <- matrix(c(1, 2*(-tet[1]+mean(x)),
##               -3*tet[1]^2-3*tet[2]^2, 0,
##               2*tet[2], -6*tet[1]*tet[2]),
##             nrow=3,ncol=2)
## return(G)
## }

## x <- as.numeric(as.vector(read.csv2('rand_norm.csv',
##                           header=FALSE)[["V1"]]))
## res <-gmm(g1, x, c(mu = 1.0, sig = 1.0), grad = Dg)

using Distributions

srand(42)
x = rand(Normal(4, 2), 1000)

function g(θ, x)
    m1 = θ[1] - x
    m2 = θ[2]^2 - (x - θ[1]).^2
    m3 = x.^3 - θ[1].*(θ[1]^2 + 3*θ[2]^2)
    return [m1 m2 m3]
end

step_1     = gmm(g, [1.0, 1.0], eye(3), data=x)
step_2_hac = gmm(g, coef(step_1),
                 optimal_W(step_1, QuadraticSpectralKernel(0.91469)), data=x)
step_2_iid = gmm(g, coef(step_1), optimal_W(step_1, HC0()),  data=x)

step_qs_mgr = gmm(g, [1.,1.], eye(3), data = x, mgr = TwoStepGMM(QuadraticSpectralKernel(0.91469)))

step_iid_mgr = gmm(g, [1.,1.], eye(3), data = x, mgr = TwoStepGMM(HC0()))


#-----------------------------------------------------------------------
#       BEGIN   geosol.jl
#-----------------------------------------------------------------------

using OrdinaryDiffEq

#-----------------------------------------------------------------------
#       INITIAL DATA NULL ENFORCER FUNCTIONS
#-----------------------------------------------------------------------

#-----------------------------------------------------------------------
"""
    nullenforcerf( V0::RealVec , X::RealVec , gfunc::Function )

The `nullenforcerf` function takes a vector `V0` and modifies the time
or first component so that it is null at the point `X` with respect to
`gfunc` and future directed; the function returns the modified vector.

"""
function nullenforcerf( V0::RealVec , X::RealVec , gfunc::Function )
    tpfl=typeof(V0[1])
    nx = length(V0)

    Vn = copy(V0)

    g = gfunc(X)

    if g[1,1] < zero(tpfl)
        Vn[1] =  -((g[1,2]*Vn[2] + g[1,3]*Vn[3] + g[1,4]*Vn[4] +
        sqrt(
              4*(g[1,2]*Vn[2] + g[1,3]*Vn[3] + g[1,4]*Vn[4])^2 -
              4*g[1,1]*(g[2,2]*Vn[2]^2 + 2*g[2,3]*Vn[2]*Vn[3] +
              g[3,3]*Vn[3]^2 + 2*g[2,4]*Vn[2]*Vn[4] +
              2*g[3,4]*Vn[3]*Vn[4] + g[4,4]*Vn[4]^2)
            )/2)/g[1,1])

        return Vn

    elseif g[1,1] == zero(tpfl)
        Vn[1] =  (-(g[2,2]*Vn[2]^2) - 2*g[2,3]*Vn[2]*Vn[3] -
                    g[3,3]*Vn[3]^2 - 2*g[2,4]*Vn[2]*Vn[4] -
                    2*g[3,4]*Vn[3]*Vn[4] -
                    g[4,4]*Vn[4]^2)/(2*(g[1,2]*Vn[2] +
                    g[1,3]*Vn[3] + g[1,4]*Vn[4]))

        return Vn
    else
        print("Need g_{11} to be negative or zero.")
        return zeros(tpfl,2*nx)
    end
end     #---------------------------------------------------------------

#-----------------------------------------------------------------------
"""
    nullenforcerp( V0::RealVec , X::RealVec , gfunc::Function )

The `nullenforcerp` function takes a vector `V0` and modifies the time
or first component so that it is null at the point `X` with respect to
`gfunc` and past directed; the function returns the modified vector.

"""
function nullenforcerp( V0::RealVec , X::RealVec , gfunc::Function )
    tpfl=typeof(V0[1])
    nx = length(V0)

    Vn = copy(V0)

    g = gfunc(X)

    if g[1,1] < zero(tpfl)
        Vn[1] =  (-2*g[1,2]*Vn[2] - 2*g[1,3]*Vn[3] - 2*g[1,4]*Vn[4] +
        sqrt(4*(g[1,2]*Vn[2] + g[1,3]*Vn[3] + g[1,4]*Vn[4])^2 -
        4*g[1,1]*(g[2,2]*Vn[2]^2 + 2*g[2,3]*Vn[2]*Vn[3] +
        g[3,3]*Vn[3]^2 + 2*g[2,4]*Vn[2]*Vn[4] + 2*g[3,4]*Vn[3]*Vn[4] +
        g[4,4]*Vn[4]^2)))/(2*g[1,1])

        return Vn

    elseif g[1,1] == zero(tpfl)
        Vn[1] =  (-(g[2,2]*Vn[2]^2) - 2*g[2,3]*Vn[2]*Vn[3] -
                    g[3,3]*Vn[3]^2 - 2*g[2,4]*Vn[2]*Vn[4] -
                    2*g[3,4]*Vn[3]*Vn[4] -
                    g[4,4]*Vn[4]^2)/(2*(g[1,2]*Vn[2] +
                    g[1,3]*Vn[3] + g[1,4]*Vn[4]))

        return Vn
    else
        print("Need g_{11} to be negative or zero.")
        return zeros(tpfl,2*nx)
    end
end     #---------------------------------------------------------------

#-----------------------------------------------------------------------
"""
    HamGeo( Z::RealVec , gfunc::Function )

The `HamGeo` function computes the geodesic Hamiltonian
``H = \\frac{1}{2} g^{μν} p_μ p_ν``
from the metric ``g_{μν}`` provided in the function `gfunc` and the
spacetime position `x` and momentum `p` encoded in `Z=(x,p)`.

"""
function HamGeo( Z::RealVec , gfunc::Function )
    tpfl=typeof(Z[1])

    ( x , p ) = ( Z[1:4] , Z[5:8] )

    gin = inv(gfunc(x))

    return tpfl(1/2)*(transpose(p)*gin*p)
end     #---------------------------------------------------------------

#-----------------------------------------------------------------------
"""
    Jsympl( Zarg::RealVec )

The `Jsympl` function effectively applies the symplectic matrix
``J^{αβ}`` to the vector `Zarg` (the vector ``{∂H}/{∂z^β}``).

"""
function Jsympl( Zarg::RealVec )
    tpfl=typeof(Zarg[1])
    n2 = length(Zarg)
    Z = zeros(tpfl,n2)

    if iseven(n2)
        n = Int(n2 / 2)
        for i=1:n
            Z[i]    = Zarg[n+i]
            Z[n+i]  = - Zarg[i]
        end

        return Z
    else
        return Z
    end
end     #---------------------------------------------------------------

#-----------------------------------------------------------------------
"""
    ZdotGeo( Z::RealVec , gfunc::Function )

The `ZdotGeo` function computes the gradient of the Hamiltonian and
applies the symplectic operator to the gradient.

"""
function ZdotGeo( Z::RealVec , gfunc::Function )
    tpfl=typeof(Z[1])

    VH = ForwardDiff.gradient(z->HamGeo(z,gfunc),Z)
    return Jsympl( VH )
end     #---------------------------------------------------------------

#-----------------------------------------------------------------------
"""
    ZF!( dZ , Z , gfunc::Function )

The function `ZF` converts the function `ZdotGeo` into a function with
mutable arguments.

"""
function ZF!( dZ , Z , gfunc::Function )
    copyto!(dZ, ZdotGeo( Z, gfunc ))
end     #---------------------------------------------------------------

#-----------------------------------------------------------------------
"""
    solveZ( Z0::RealVec , gfunc::Function , δ1::Real , δ2::Real ,
            integrator=AutoVern7(Rodas5()) , δt=0 )

The function `solveZ` performs the integration of geodesics given
the initial data `Z0`, and the metric function `gfunc`. The integration
is performed using the integrator specified by the variable `integrator`
using the relative tolerance parameter `δ1` and the absolute tolerance
parameter `δ2`. The parameter `δt` determines the timestep parameter
`dt` in ODEProblem.

The function `solveZ` outputs only the endpoint of the solution for the
geodesic equation.

"""
function solveZ( Z0::RealVec , gfunc::Function , δ1::Real , δ2::Real
                 , integrator=AutoVern7(Rodas5()) , δt=0 )
    tpfl=typeof(Z0[1])

    tspan = (0.0,1.0)

    Zo = vcat(Z0[1:4],gfunc(Z0[1:4])*Z0[5:8]) # Lowering index of p

    F = (dZ,Z,p,t)->ZF!( dZ , Z , gfunc )

    if δt == 0
        slv = solve( ODEProblem(F,Zo,tspan),integrator,reltol=δ1
                 ,abstol=δ2,save_everystep=false )
    else
        slv = solve( ODEProblem(F,Zo,tspan),integrator,reltol=δ1
                 ,abstol=δ2,dt=δt,save_everystep=false )
    end

    return last(slv.u)
end     #---------------------------------------------------------------

#-----------------------------------------------------------------------
#       END     geosol.jl
#-----------------------------------------------------------------------


function zero_closure(sys::MomentEquations, binary_vars::Array{Int,1}=Int[])

    closure = OrderedDict()
    closure_exp = OrderedDict()

    if !isempty(binary_vars)
        sys = bernoulli_moment_eqs(sys, binary_vars)
    end

    if typeof(sys) == CentralMomentEquations
        for i in sys.iter_q
            closure[sys.M[i]] = 0
            closure_exp[sys.M[i]] = 0
        end
    else
        μ = copy(sys.μ)
        μ_symbolic = copy(sys.μ)
        raw_to_central = raw_to_central_moments(sys.N, sys.q_order)

        unique_iter_q = unique(sort(i) for i in sys.iter_q)
        sub = Dict()

        for i in unique_iter_q
            μ[i] = -(raw_to_central[i]-μ[i])
            closure[sys.μ[i]] = simplify(μ[i], expand=true)
            μ[i] = simplify(substitute(μ[i], closure_exp), expand=true)
            closure_exp[sys.μ[i]] = μ[i]

            perms = collect(multiset_permutations(i, length(i)))[2:end]

            for iter_perm in perms

                iter_perm_ind = sortperm(sortperm(iter_perm))
                for r in sys.iter_all
                    sub[sys.μ[r]] = sys.μ[r[iter_perm_ind]]
                end

                iter_perm = Tuple(iter_perm)
                closure[sys.μ[iter_perm]] = substitute(closure[sys.μ[i]], sub)
                closure_exp[sys.μ[iter_perm]] = substitute(closure_exp[sys.μ[i]], sub)
            end

        end
    end

    close_eqs(sys, closure_exp, closure, true)

end


#=

  Euler #47 in Julia.

  Problem 47:
  """
  The first two consecutive numbers to have two distinct prime factors are:

  14 = 2 x 7
  15 = 3 x 5

  The first three consecutive numbers to have three distinct
  prime factors are:

  644 = 2^2 x 7 x 23
  645 = 3 x 5 x 43
  646 = 2 x 17 x 19.

  Find the first four consecutive integers to have four distinct primes
  factors. What is the first of these numbers?
  """

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

=#

include("Euler.jl")

# 0.01653001s
function euler47a()
    maxn = 1_000_000
    f = zeros(maxn)
    for i in 2:maxn-1
        if f[i] == 0
            for j in 2*i:i:maxn-1
                f[j] += 1
            end
        end
    end

    goal = [4,4,4,4]
    found = 0
    for i in 1:maxn-3
        if [f[i],f[i+1],f[i+2],f[i+3]] == goal
            found = i
            break
        end
    end
    return found
end

run_euler(euler47a)