@testset "test_discrete" begin

C_111 = ss([-5], [2], [3], [0])
C_212 = ss([-5 -3; 2 -9], [1; 2], [1 0; 0 1], [0; 0])
C_221 = ss([-5 -3; 2 -9], [1 0; 0 2], [1 0], [0 0])
C_222_d = ss([-5 -3; 2 -9], [1 0; 0 2], [1 0; 0 1], [1 0; 0 1])

@test c2d_x0map(ss(4*[1 0; 0 1]), 0.5, :zoh) == (ss(4*[1 0; 0 1], 0.5), zeros(0, 2))
@test c2d_x0map(ss(4*[1 0; 0 1]), 0.5, :foh) == (ss(4*[1 0; 0 1], 0.5), zeros(0, 2))
@test_tupleapprox(c2d_x0map(C_111, 0.01, :zoh),
    ss([0.951229424500714], [0.019508230199714396], [3], [0], 0.01), [1 0])
@test_tupleapprox(c2d_x0map(C_111, 0.01, :foh),
    ss([0.951229424500714], [0.01902855227625244], [3], [0.029506188017136226], 0.01),
    [1 -0.009835396005712075])
@test_tupleapprox(c2d_x0map(C_212, 0.01, :zoh),
    ss([0.9509478368863918 -0.027970882212682433; 0.018647254808454958 0.9136533272694819],
    [0.009466805409666932; 0.019219966830212765], [1 0; 0 1], [0; 0], 0.01),
    [1 0 0; 0 1 0])
@test_tupleapprox(c2d_x0map(C_212, 0.01, :foh),
    ss([0.9509478368863921 -0.027970882212682433; 0.018647254808454954 0.913653327269482],
    [0.008957940478201584; 0.018468989584974498], [1 0; 0 1], [0.004820885889482196;
    0.009738343195298675], 0.01), [1 0 -0.004820885889482196; 0 1 -0.009738343195298675])
@test_tupleapprox(c2d_x0map(C_221, 0.01, :zoh),
    ss([0.9509478368863918 -0.027970882212682433; 0.018647254808454958 0.9136533272694819],
    [0.009753161420545834 -0.0002863560108789034; 9.54520036263011e-5 0.019124514826586465],
    [1.0 0.0], [0.0 0.0], 0.01), [1 0 0 0; 0 1 0 0])
@test_tupleapprox(c2d_x0map(C_221, 0.01, :foh),
    ss([0.9509478368863921 -0.027970882212682433; 0.018647254808454954 0.913653327269482],
    [0.009511049106772921 -0.0005531086285713394; 0.00018436954285711309 0.018284620042117387],
    [1 0], [0.004917457305816479 -9.657141633428213e-5], 0.01),
    [1 0 -0.004917457305816479 9.657141633428213e-5;
    0 1 -3.219047211142736e-5 -0.009706152723187249])
@test_tupleapprox(c2d_x0map(C_222_d, 0.01, :zoh),
    ss([0.9509478368863918 -0.027970882212682433; 0.018647254808454958 0.9136533272694819],
    [0.009753161420545834 -0.0002863560108789034; 9.54520036263011e-5 0.019124514826586465],
    [1 0; 0 1], [1 0; 0 1], 0.01), [1 0 0 0; 0 1 0 0])
@test_tupleapprox(c2d_x0map(C_222_d, 0.01, :foh),
    ss([0.9509478368863921 -0.027970882212682433; 0.018647254808454954 0.913653327269482],
    [0.009511049106772921 -0.0005531086285713394; 0.00018436954285711309 0.018284620042117387],
    [1 0; 0 1], [1.0049174573058164 -9.657141633428213e-5; 3.219047211142736e-5 1.0097061527231872], 0.01),
    [1 0 -0.004917457305816479 9.657141633428213e-5; 0 1 -3.219047211142736e-5 -0.009706152723187249])

# Test c2d for transfer functions
G = tf([1, 1], [1, 3, 1])
Gd = c2d(G, 0.2)
@test typeof(Gd) <: TransferFunction{<:Discrete, <:ControlSystems.SisoRational}
@test Gd ≈ tf([0, 0.165883310712090, -0.135903621603238], [1.0, -1.518831946985175, 0.548811636094027], 0.2) rtol=1e-14

# Issue #391
@test c2d(tf(C_111), 0.01, :zoh) ≈ tf(c2d(C_111, 0.01, :zoh))
@test c2d(tf(C_111), 0.01, :foh) ≈ tf(c2d(C_111, 0.01, :foh))

# Issue #471 discussion, test c2d for zero row C
C_210 = ss(C_212.A, C_212.B, zeros(0, 2), zeros(0, 1))
@test c2d(C_210, 0.01).A ≈ c2d(C_212, 0.01).A

# c2d on a zpk model should arguably return a zpk model
Gdzpk = c2d(zpk(G), 0.2)
@test typeof(Gdzpk) <: TransferFunction{<:Discrete, <:ControlSystems.SisoZpk}
@test Gdzpk ≈ zpk([0.8192724211968178], [0.9264518519788194, 0.5923800950063556], 0.16588331071209, 0.2) rtol=1e-14

# ERRORS
@test_throws MethodError c2d(ss([1], [2], [3], [4], 0.01), 0.01)   # Already discrete
@test_throws MethodError c2d(ss([1], [2], [3], [4], -1), 0.01)     # Already discrete

# d2c
@test d2c(c2d(C_111, 0.01)) ≈ C_111
@test d2c(c2d(C_212, 0.01)) ≈ C_212
@test d2c(c2d(C_221, 0.01)) ≈ C_221
@test d2c(c2d(C_222_d, 0.01)) ≈ C_222_d
@test d2c(Gd) ≈ G

sys = ss([0 1; 0 0], [0;1], [1 0], 0)
sysd = c2d(sys, 1)
@test d2c(sysd) ≈ sys

# forward euler / tustin
@test c2d(C_111, 1, :fwdeuler).A == I + C_111.A
for method in (:fwdeuler, :tustin)
    @test d2c(c2d(C_111, 0.01, method), method) ≈ C_111 atol = sqrt(eps())
    @test d2c(c2d(C_212, 0.01, method), method) ≈ C_212 atol = sqrt(eps())
    @test d2c(c2d(C_221, 0.01, method), method) ≈ C_221 atol = sqrt(eps())
    @test d2c(c2d(C_222_d, 0.01, method), method) ≈ C_222_d atol = sqrt(eps())
    @test d2c(c2d(G, 0.01, method), method) ≈ G atol = sqrt(eps())
end

matlab_tustin = let
    A = [0.95094630230333 -0.02800401390866; 0.018669342605773 0.913607617091783]
    B = [0.009754731511517 -0.000280040139087; 9.3346713029e-5 0.019136076170918]
    C = [0.975473151151665 -0.01400200695433; 0.009334671302887 0.956803808545892]
    D = [1.004877365755758 -0.000140020069543; 4.6673356514e-5 1.009568038085459]
    ss(A,B,C,D,0.01)
end

sys_tustin = c2d(C_222_d, 0.01, :tustin)
@test sys_tustin ≈ matlab_tustin atol = 1e-12

matlab_prewarp = let
    A = [0.950906169509825 -0.028025790680969; 0.018683860453979 0.913538448601866]
    B = [0.009762667760265 -0.00028049168885; 9.3497229617e-5 0.019151346602063]
    C = [0.975453084754912 -0.014012895340485; 0.00934193022699 0.956769224300933]
    D = [1.004881333880133 -0.000140245844425; 4.6748614808e-5 1.009575673301031]
    sys = ss(A,B,C,D,0.01)
end

sys_prewarp = c2d(C_222_d, 0.01, :tustin, w_prewarp=10)
@test sys_prewarp ≈ matlab_prewarp atol = 1e-12

Cd = c2d(C_111, 0.001, :fwdeuler)
t = 0:0.001:2
y,_ = step(C_111, t)
yd,_ = step(Cd, t)
@test norm(yd-y) / norm(y) < 1e-3

# dab
import DSP: conv
ζ = 0.2
ω = 1
B = [1]
A   = [1, 2ζ*ω, ω^2]
P  = tf(B,A)
# Control design
ζ0 = 0.7
ω0 = 2
Am = [1, 2ζ0*ω0, ω0^2]
Ao = conv(2Am, [1/2, 1]) # Observer polynomial, add extra pole due to the integrator
AR = [1,0] # Force the controller to contain an integrator ( 1/(s+0) )

B⁺  = [1] # The process numerator polynomial can be facored as B = B⁺B⁻ where B⁻ contains the zeros we do not want to cancel (non-minimum phase and poorly damped zeros)
B⁻  = [1]
Bm  = conv(B⁺, B⁻) # In this case, keep the entire numerator polynomial of the process

R,S,T = rstc(B⁺,B⁻,A,Bm,Am,Ao,AR) # Calculate the 2-DOF controller polynomials

Gcl = tf(conv(B,T),zpconv(A,R,B,S)) # Form the closed loop polynomial from reference to output, the closed-loop characteristic polynomial is AR + BS, the function zpconv takes care of the polynomial multiplication and makes sure the coefficient vectores are of equal length

@test ControlSystems.isstable(Gcl)

p = poles(Gcl)
# Test that all desired poles are in the closed-loop system
@test norm(minimum(abs.((poles(tf(Bm,Am)) .- sort(p, by=imag)')), dims=2)) < 1e-6
# Test that the observer poles are in the closed-loop system
@test norm(minimum(abs.((poles(tf(1,Ao)) .- sort(p, by=imag)')), dims=2)) < 1e-6

end


module rbc
using LinearAlgebra, SymbolicUtils, LaTeXStrings
const max_order = 2
const n_y = 3
const n_x = 2
const n_p = 6
const n_ϵ = 1
const n_z = 2
const η = reshape([0; -1], 2, 1)
const Q = [1.0 0.0 0.0 0.0 0.0; 0.0 0.0 1.0 0.0 0.0]
const has_Ω = true
# Display definitions
const x_symbols = [:k, :z]
const y_symbols = [:c, :q, :i]
const u_symbols = [:c, :q, :i, :k, :z]
const p_symbols = [:α, :β, :ρ, :δ, :σ, :Ω_1]
const H_latex = L"\begin{equation}
\left[
\begin{array}{c}
\frac{ - \beta \left( 1 - \delta + \left( k\left( 1 + t \right) \right)^{-1 + \alpha} \alpha e^{z\left( 1 + t \right)} \right)}{c\left( 1 + t \right)} + \frac{1}{c\left( t \right)} \\
 - q\left( t \right) - \left( 1 - \delta \right) k\left( t \right) + c\left( t \right) + k\left( 1 + t \right) \\
 - \left( k\left( t \right) \right)^{\alpha} e^{z\left( t \right)} + q\left( t \right) \\
 - \rho z\left( t \right) + z\left( 1 + t \right) \\
 - k\left( 1 + t \right) + \left( 1 - \delta \right) k\left( t \right) + i\left( t \right) \\
\end{array}
\right]
\end{equation}
"
const steady_states_latex = L"\begin{align}
k\left( \infty \right) =& \left( \frac{-1 + \delta + \frac{1}{\beta}}{\alpha} \right)^{\frac{1}{-1 + \alpha}} \\
z\left( \infty \right) =& 0 \\
c\left( \infty \right) =& \left( \frac{-1 + \delta + \frac{1}{\beta}}{\alpha} \right)^{\frac{\alpha}{-1 + \alpha}} - \left( \frac{-1 + \delta + \frac{1}{\beta}}{\alpha} \right)^{\frac{1}{-1 + \alpha}} \delta \\
q\left( \infty \right) =& \left( \frac{-1 + \delta + \frac{1}{\beta}}{\alpha} \right)^{\frac{\alpha}{-1 + \alpha}} \\
i\left( \infty \right) =& \left( \frac{-1 + \delta + \frac{1}{\beta}}{\alpha} \right)^{\frac{1}{-1 + \alpha}} \delta
\end{align}
"
const steady_states_iv_latex = L"\begin{align}
k\left( \infty \right) =& \left( \frac{-1 + \delta + \frac{1}{\beta}}{\alpha} \right)^{\frac{1}{-1 + \alpha}} \\
z\left( \infty \right) =& 0 \\
c\left( \infty \right) =& \left( \frac{-1 + \delta + \frac{1}{\beta}}{\alpha} \right)^{\frac{\alpha}{-1 + \alpha}} - \left( \frac{-1 + \delta + \frac{1}{\beta}}{\alpha} \right)^{\frac{1}{-1 + \alpha}} \delta \\
q\left( \infty \right) =& \left( \frac{-1 + \delta + \frac{1}{\beta}}{\alpha} \right)^{\frac{\alpha}{-1 + \alpha}} \\
i\left( \infty \right) =& \left( \frac{-1 + \delta + \frac{1}{\beta}}{\alpha} \right)^{\frac{1}{-1 + \alpha}} \delta
\end{align}
"
# Function definitions
include("rbc/zero_order_ip.jl")
include("rbc/first_order_ip.jl")
include("rbc/second_order_ip.jl")
end


using sparsePolyChaos
using PolyChaos

maxDeg = 10
interactions = 1

## Define the model
# μ = 0
# σ = 1
# model(x) = exp(μ + σ * x)
# model(x) = x^7 - 21 * x^5 + 105 * x^3 - 105 * x + x^4 - 6 * x^2 + 3 # He7 + He4
# model(x) = x^4 - 3* x^2
model(x) = x^10 - 45 * x^8 + 630 * x^6 - 3150 * x^4 + 4725 * x^2 - 945 # He10. Bad conditioning: Restart needed until sample size = 1800
# model(x) = 0

## Build the full model
op = GaussOrthoPoly(maxDeg)

## Compute sparse PCE solution
pce = sparsePCE(op, model; pMax = maxDeg, jMax = interactions)

println("Basis polynomials: ", pce[:Ap])
println("PCE coefficients: ", pce[:coeffs])
# println("Max degree:: ", p)
println("R² error: ", pce[:R²])
println("Q² error: ", pce[:Q²])

# ## Sparse PCE with custom sampling method

# println("\n\n== Sparse PCE with custom sampling method ==")
# function sampleFromGaussianMixture(n::Int,μ::Vector{},σ::Vector{},w::Vector{})
#     X = Float64[]
#     for i in 1:n
#         k = findfirst(x -> x > rand(), cumsum(w))
#         push!(X, μ[k] + σ[k]*randn())
#     end
#     return X
# end

# function ρ_gauss(x,μ,σ)
#     1 / sqrt(2*π*σ^2) * exp(-(x - μ)^2 / (2σ^2))
# end

# μ, σ, w = [2.1, 3.2], [0.3, 0.4], [0.3, 0.7]
# ρ(x) = sum( w[i]*ρ_gauss(x,μ[i],σ[i]) for i in 1:length(w) )
# meas = Measure("my_GaussMixture", ρ, (-Inf,Inf), false, Dict(:μ=>μ,:σ=>σ,:w=>w)) # build measure
# op_gm = OrthoPoly("my_op",maxDeg,meas;Nquad=150,Nrec = 5*maxDeg, discretization=stieltjes) # construct orthogonal polynomial

# sampleFun(sampleSize) = sampleFromGaussianMixture(sampleSize,μ,σ,w)
# pce = sparsePCE(op_gm, model, sampleFun; pMax = maxDeg)

# println("Basis polynomials: ", pce[:Ap])
# println("PCE coefficients: ", pce[:coeffs])
# # println("Max degree:: ", pce[:p])
# println("R² error: ", pce[:R²])
# println("Q² error: ", pce[:Q²])

#TODO: Comparison to monte carlo
# # ------- Monte-Carlo -------
# # Evaluate model function on same set of previously drawn samples X
# y_mc = model.(X)


function spre(O::AbstractArray)
    N = size(O, 1)
    A = spdiagm( ones(ComplexF64, N) )
    B = O
    return kron(A, B)
end

function spost(O::AbstractArray)
    N = size(O, 1)
    A = sparse( transpose(O) )
    B = spdiagm( ones(ComplexF64, N) )
    return kron(A, B)
end

function sprepost(A, B::AbstractArray)
    return sparse( spre(A) * spost(B) )
end

function lindblad_dissipator(O::AbstractArray)
    Od_O = adjoint(O) * O
    return sprepost(O, adjoint(O)) - 0.5 * spre(Od_O) - 0.5 * spost(Od_O)
end

function destroy(N::Number)
    return spdiagm(1 => Array{ComplexF64}(sqrt.(1:N - 1)))
end

function eye(N::Number)
    return spdiagm(ones(ComplexF64, N))
end

function fock(N::Number, pos::Number)
    array = zeros(N)
    array[pos + 1] = 1
    return Array{ComplexF64}( array )
end

function projection(N::Number, i::Number, j::Number, shift = false)
    if shift
        return sparse( fock(N, i - 1) * adjoint(fock(N, j - 1)) )
    else
        return sparse( fock(N, i) * adjoint(fock(N, j)) )
    end
end

# function exp_gpu(M)
#     M_tmp = copy(M)
#     E_gpu, U_gpu = CUDA.CUSOLVER.heevd!('V','U', M_tmp)
#     return U_gpu * Diagonal(exp.(E_gpu)) * adjoint(U_gpu)
# end

function sinm(O::AbstractArray)
    M = collect(O)
    return sparse( 0.5im * (exp(1im * M) - exp(-1im * M)) )
end

function cosm(O::AbstractArray)
    M = collect(O)
    return sparse( 0.5 * (exp(1im * M) + exp(-1im * M)) )
end

function expect(op::AbstractArray, state::AbstractArray)
    if ishermitian(op)
        return real(adjoint(state) * op * state)
    else
        return adjoint(state) * op * state
    end
end


const affine_index = [1]  # which affine vector index to use

reset_affine_index() = affine_index[1] = 1

@doc """
An affine quantity for affine arithmetic.
The usual way to create an affine quantity is from an interval `X`, via `FullAffine(X)`.
"""
struct FullAffine{T<:AbstractFloat}
    c::T   # mid-point
    γ::Vector{T}  # error terms
end

function show(io::IO, C::FullAffine)
    print(io, "⟨", C.c, "; ", C.γ, "⟩")
end

==(C::FullAffine, D::FullAffine) = C.c == D.c && C.γ == D.γ

@doc """
    FullAffine(X::Interval)

Construct a new `FullAffine` quantity from an `Interval`.
"""
function FullAffine(X::Interval)
    c = mid(X)
    r = radius(X)

    index = affine_index[1]
    affine_index[1] += 1

    γ = zeros(index)
    γ[end] = r

    return FullAffine(c, γ)
end

# conversion of numerical constant to affine:
FullAffine(c::Real) = FullAffine(c, Float64[])

range(C::FullAffine) = C.c + sum(abs.(C.γ))*(-1..1)
range(X::Interval) = X

# morally:
# +(C::FullAffine, D::Afine) = FullAffine(C.c + D.c, C.γ + D.γ)

for op in (:+, :-)
    @eval function $op(C::FullAffine, D::FullAffine)
        k = length(C.γ)
        l = length(D.γ)

        # account for unequal lengths:
        m = min(k, l)
        common_γ = $op(C.γ[1:m], D.γ[1:m])

        if l >= k
            γ = [ common_γ; D.γ[m+1:l] ]
        else
            γ = [ common_γ; C.γ[m+1:k] ]
        end

        FullAffine($op(C.c, D.c), γ)
    end
end

+(C::FullAffine, α::Real) = FullAffine(C.c + α, C.γ)
+(α::Real, C::FullAffine) = C + α

-(C::FullAffine, α::Real) = FullAffine(C.c - α, C.γ)
-(α::Real, C::FullAffine) = FullAffine(α - C.c, [-x for x in C.γ])

function *(C::FullAffine, D::FullAffine)

    c = C.c
    d = D.c

    k = length(C.γ)
    l = length(D.γ)

    # account for unequal lengths:
    m = min(k, l)
    common_γ = d * C.γ[1:m] + c * D.γ[1:m]

    error_bound = sum(abs.(C.γ)) * sum(abs.(D.γ))

    if l >= k
        γ = [ common_γ; c*D.γ[m+1:l]; error_bound ]
    else
        γ = [ common_γ; d*C.γ[m+1:k]; error_bound ]
    end

    # |γ|₁ = sum(abs.(γ))

    FullAffine(C.c * D.c, γ)
end

*(α::Real, C::FullAffine) = FullAffine(α*C.c, α*C.γ)
*(C::FullAffine, α::Real) = α * C

^(x::FullAffine, n::Integer) = Base.power_by_squaring(x, n)

Base.literal_pow(::typeof(^), x::FullAffine{T}, ::Val{p}) where {T,p} = x^p

eltype(C::FullAffine{T}) where T = T
zero(C::FullAffine{T}) where T = FullAffine(zero(T))
zero(::Type{FullAffine{T}}) where T = FullAffine(zero(T))

one(C::FullAffine{T}) where T = FullAffine(one(T))
one(::Type{FullAffine{T}}) where T = FullAffine(one(T))

#one(C::FullAffine)


using ImageReconstruction
using Test

@testset "radon - centered impulse response" begin
    pixels = 129
    views = 200
    I = zeros(pixels, pixels)
    I[pixels÷2, pixels÷2] = 1
    θ = range(0, step = 2π / views, length = views)
    t = -100:100

    P = radon(I, θ, t)

    @test all(P[101, :] .== 1)
end

using Images: shepp_logan, sad
@testset "iradon - shepp logan" begin
    Igt = shepp_logan(128)
    views = 200
    θ = range(0, 2π, length=views)
    t = -150:150
    P = radon(Igt, θ, t)
    I = iradon(P, θ, t)
	@test sad(I, Igt) < 500
end

using Images: shepp_logan
@testset "sinogram - shepp logan" begin
    Igt = shepp_logan(128)
    sinogram(Igt,10)
end


using Test
using FlightMechanics.Models
using FlightMechanics

llh = [deg2rad(49.996908), 0.000000, 9907.31]
xyz_ecef = [4114496.258, 0.0, 4870157.031]
xyz_earth = [0., 0., -9907.31]

pllh = PositionLLH(llh...)

@test isapprox(get_llh(pllh), llh)
@test isapprox(get_xyz_earth(pllh), xyz_earth)
@test isapprox(get_xyz_ecef(pllh), xyz_ecef)
@test isapprox(get_height(pllh), llh[3])

xyz_earth = [0., 0., -9907.31]
pearth = PositionEarth(xyz_earth..., llh[1:2]...)

@test isapprox(get_llh(pearth), llh)
@test isapprox(get_xyz_earth(pearth), xyz_earth)
@test isapprox(get_xyz_ecef(pearth), xyz_ecef)
@test isapprox(get_height(pearth), -xyz_earth[3])

pecef = PositionECEF(xyz_ecef...)

@test isapprox(get_llh(pecef), llh, rtol=1e-5)
@test isapprox(get_xyz_earth(pecef), xyz_earth, rtol=1e-5)
@test isapprox(get_xyz_ecef(pecef), xyz_ecef)
@test isapprox(get_height(pecef), llh[3], atol=0.17)

pzero = Position()

@test isapprox(get_llh(pzero), [0, 0, 0])
@test isapprox(get_xyz_earth(pzero), [0, 0, 0])
@test isapprox(get_xyz_ecef(pzero), llh2ecef(0, 0, 0))
@test isapprox(get_height(pzero), 0)


@testset "probabilityweightedmoment.jl" begin
    @testset "pwm(x, p, r, s)" begin

        # p < 0 throws error
        @test_throws AssertionError Extremes.pwm([1.0], -1, 1, 1)

        # r < 0 throws error
        @test_throws AssertionError Extremes.pwm([1.0], 1, -1, 1)

        #  s < 0 throws error
        @test_throws AssertionError Extremes.pwm([1.0], 1, 1, -1)

        # Probability weighted moment with p=1, r=0 and s=k
        μ = .5
        σ = 1.0
        pd = Logistic(μ,σ)
        y = rand(pd, 10000)
        # theoritical form of the pwm for the Logistic distribution (Greenwood and al., 1979)
        f(k::Int) = μ/(1+k) - σ/(1+k)*sum(1/i for i=1:k)

        for k=1:3
            @test f(k) ≈ Extremes.pwm(y,1,0,k) rtol=.1
        end

        # Probability weighted moment with p=1, r=j and s=0
        μ = .5
        σ = 1.0
        ξ = 0
        pd = GeneralizedExtremeValue(μ,σ,0)
        y = rand(pd, 10000)

        # theoritical form of the pwm for the Gumbel distribution (Greenwood and al., 1979)
        f2(j::Int) = μ/(1+j) + σ/(1+j) * (log(1+j) + MathConstants.eulergamma)

        for j=1:3
            @test f2(j) ≈ Extremes.pwm(y,1,j,0) rtol=.1
        end

    end

    include(joinpath("probabilityweightedmoment", "probabilityweightedmoment_gev_test.jl"))
    include(joinpath("probabilityweightedmoment", "probabilityweightedmoment_gp_test.jl"))
    include(joinpath("probabilityweightedmoment", "probabilityweightedmoment_gumbel_test.jl"))

end


"""
This function calculates the field components given two wire center position coordinates
    It assumes you want to fild the fields AT C₂ FROM C₁ as well as the (fully ANTIsymmetric) fields from the wire at C₂ on itself given some wire diameter WireD and current I₂ (assume 1 Amp)

    This will be some constant term, which will be ⟂ to the vector between the two(C₂-C₁), and have some gradient that decays in the direction of C₂-C₁.

This is a 2D function and returns the field
"""
function WireDist2Field(C₁::Vector,C₂::Vector,WireD=0.001,I₁=1,I₂=1)
    μ₀ = 4π*1e-7
    r = WireD/2
    DistVec = C₂ .- C₁
    Dist = √(sum(DistVec.^2))
    # D̂ = Dist ./ sum(Dist.^2) #the unit vector between the two coordinates
    # D′ = D̂ * [0 -1;1 0] #the unit vector rotated 90°

    MagH₂₁_Near = WireNearField_B(Dist-r,r;I=I₁)/μ₀ # The field from 1 on 2 is ⟂ to the vector between them
    MagH₂₁_Far = WireNearField_B(Dist+r,r;I=I₁)/μ₀ # The field from 1 on 2 is ⟂ to the vector between them
    GradH₂₁ = DerivField(Dist,r;I=I₁)/μ₀

    MagH₂₂ = WireNearField_B(r,r,I=I₂)/μ₀

    ## There are assumed to be four boundary conditions on wire 2, K₁,₂,₃,₄ where 1 is the point farthest from wire 1, 4 is the closest, and 2 is the point between 1,4 in the direction of D′ and 4 is the opposite of 2

    HK₁ =      MagH₂₂ + (MagH₂₁_Far)
    HK₃ = -1 * MagH₂₂ + (MagH₂₁_Near)

    HK₂ = -1 * MagH₂₂
    HK₄ =      MagH₂₂

    return HK₁,HK₂,HK₃,HK₄
end

"""
This function calculates the H field distribution in a Conductor (1D) where the width of the wire is W(meters),The skin depth is δ, the external field is ANTIsymmetric (skin effect) and directed in the Z direction (current is in Y) with a magnitude of K. X is the dimension that is ⟂ to I,H, and extends from -W/2, to W/2

    Boundary conditions are ±K A/m at x=±W/2
"""
function HAntiSym(x,δ,W = 1., K=1.)
    if (x)>(W/2)
        return -K
    elseif (x)<(-1*W/2)
        return K
    else
        return  K*(exp.(-1*(1+im)*x/δ) - exp.((1+im)*x/δ)) / (exp.((1+im)*W/(2*δ)) - exp.(-1*(1+im)*W/(2*δ)))
    end

end

"""
This resultant current distribution that corresponds to HAntiSym(...). J = -dH/dx

With anti-symmetric fields, the current on opposite sides of the conductor is in phase(non-zero net current)
"""
function JAntiSym(x,δ,W = 1., K=1.)
    if abs(x)>(W/2)
        return 0
    else
        return  K*((1+im)/δ)*(exp.(-1*(1+im)*x/δ) + exp.((1+im)*x/δ)) /
                      (exp.((1+im)*W/(2*δ)) - exp.(-1*(1+im)*W/(2*δ)))
    end

 end

 function HSym(x,δ,W = 1, K=1)
    if (x)>(W/2)
        return K
    elseif (x)<(-1*W/2)
        return K
    else
        return  K*(exp.(-1*(1+im)*x/δ) + exp.((1+im)*x/δ)) / (exp.((1+im)*W/(2*δ)) + exp.(-1*(1+im)*W/(2*δ)))
    end

end

"""
This resultant current distribution that corresponds to HSym(...). J = -dH/dx
With symmetric fields (even, as in proximity effect), the current on opposite sides of the conductor is in phase(zero net current)
"""
function JSym(x,δ,W = 1, K=1)
    if abs(x)>(W/2)
        return 0
    else
        return  K*((1+im)/δ)*(exp.(-1*(1+im)*x/δ) - exp.((1+im)*x/δ)) /
                      (exp.((1+im)*W/(2*δ)) - exp.(-1*(1+im)*W/(2*δ)))
    end

 end

 function SkinDepth(f,ρ = 1.67e-8)
    μ₀ = 4π*1e-7
    SkinDepth = sqrt.(ρ ./ (π * f * μ₀))
 end

 function plot1DMagDiffusion(K₋,K₊,W,δ,n=1000)

    HTot, JTot = MagDiffusion1D(K₋,K₊,W,δ,1000)
    δX = W/n
    x = (-1*W/2):δX:(W/2)
    figure()
    pygui(true)

    subplot(221)
    plot(x,abs.(abs.(HTot)))
    title("|H|")
    subplot(223)
    plot(x,rad2deg.(angle.(HTot)))
    title("∠H")
    subplot(222)
    plot(x,abs.(JTot))
    plot(x,real.(JTot))

    subplot(224)
    plot(x,rad2deg.(angle.(JTot)))

    println(sum(JTot)/length(JTot)*pi/4*.001^2)
    return HTot, JTot
 end

 """
Calculates the proximity effect losses in one wire at C₁ due to many others in an Nx2 array, Cₐ
 """
 function MultiWireProximity(C₁::Vector,Cₐ::Array,f,WireD=0.001,I₁=1,Iₐ=1)

    Nₐ = length(Cₐ[:,1])
    Losses = 0.
    Losses += OneWireLoss(f,WireD,I₁)
    for i in 1:Nₐ
        Losses += sum(TwoWireLoss(Cₐ[i,:],C₁,f,WireD,1,0))*2
    end
    println(Losses)

    return Losses
 end

 """
Calculates the losses IN WIRE 2 FROM WIRE 1. By default only proximity losses are considered so the default is to have zero current in wire 2

 """
 function TwoWireLoss(C₁::Vector,C₂::Vector,f,WireD=0.001,I₁=1,I₂=0)

    δ = SkinDepth(f)
    HK₁,HK₂,HK₃,HK₄ = WireDist2Field(C₁,C₂,WireD,I₁,I₂)
    println("$([HK₁,HK₂,HK₃,HK₄])")
    HTot_LR, JTot_LR = MagDiffusion1D(HK₁,HK₃,WireD,δ,100000)
    HTot_UD, JTot_UD = MagDiffusion1D(HK₂,HK₄,WireD,δ,100000)
    J_LR² =  abs.(JTot_LR).^2
    J_UD² =  abs.(JTot_UD).^2
    # JTot_LR = WeightRadial!(JTot_LR)
    # JTot_UD = WeightRadial!(JTot_UD)
    σ=58e6
    LossLR = 1/2*(sum(J_LR²)/length(JTot_LR)*pi/4*WireD^2)/σ
    LossUD = 1/2*(sum(J_UD²)/length(JTot_UD)*pi/4*WireD^2)/σ
    # println(LossLR)
    # println(LossUD)
    # θ = LinRange(-π,π,  100)
    # TotLoss =√(sum( (sin.(θ)*LossUD).^2 .+ (cos.(θ)*LossLR).^2))/length(θ)
    return LossLR, LossUD

 end

 function OneWireLoss(f,WireD=0.001,I=1)

    Area = pi/4*WireD^2
    μ₀ = 4π*1e-7
    r = WireD/2
    MagH = WireNearField_B(r,r,I=I)/μ₀
    δ = SkinDepth(f)
    HTot, JTot = MagDiffusion1D(-MagH,MagH,WireD,δ,100000)
    JTot = 2 .* JTot ##account for X and Y
    σ=58e6
    # JTot = WeightRadial!(JTot)
    # J² =  WeightRadial!(abs.(JTot).^2)
    J²RMS =  WeightRadial!(abs.(JTot ./ (√2)).^2)
    print("Net current:")
    println(round( (sum(JTot)/length(JTot)*Area);sigdigits=3))
    Losses = (sum(J²RMS)/length(JTot)*Area)/σ

    #println(Losses)

    return Losses
 end

function MagDiffusion1D(HK₋,HK₊,W,δ,n=1000)
    δX = W/n
    x = (-1*W/2):δX:(W/2)

    meanK = (HK₊+HK₋)
    ΔK = (HK₋-HK₊)/2

    HTot = HAntiSym.(x,δ,W, ΔK) .+ HSym.(x,δ,W, meanK)
    JTot = JAntiSym.(x,δ,W, ΔK) .+ JSym.(x,δ,W, meanK)

    return HTot, JTot
end

function SeriesWireBundleLosses(Cᵦ::Array, f,I,WireD)

    Nₐ = length(Cᵦ[:,1])
    Losses = 0.

    for i in 1:Nₐ
        Cₐ = Cᵦ[setdiff(1:end, i), :]
        C₁ = Cᵦ[i,:]

        Losses += MultiWireProximity(C₁,Cₐ,f,WireD,I,I)
    end
    return Losses
end

"""
This function takes in a vector and weights it such that the values are weighted by distance from the middle (for example when integrating a radial system)
"""
function WeightRadial!(V::Vector)
    Weights = abs.(collect(LinRange(-2,2,length(V))))
    Weights = Weights ./(sum(Weights))*length(V)
    V = V.*Weights
    return V
end

"""
This function takes in a vector and weights it such that the values are weighted by distance from the middle except with more weight towards the center.
"""
function WeightCenter(V::Vector)
    Weights = abs.(abs.(collect(LinRange(-1,1,length(V)))) .- 2)
    # Weights = Weights ./(sum(Weights))*length(V)
    V = V.*Weights
    return V
end


using ConvDiffMIPDECO
using jInv.InverseSolve
using Test
using jInv.Mesh
using jInv.Utils
using LinearAlgebra

# build regular mesh and Iact
domain = [0;5;0;5;]
n     = [12;13]
M     = getRegularMesh(domain,n)
mc    = randn(M.nc)

regFuns = [(m,mref,M)->mipReg(m,mref,M,q=1), (m,mref,M)->mipReg(m,mref,M,q=3)]
for k=1:length(regFuns)
	# checkDerivative of $(regFuns[k])
	function testFun(x,v=[])
		Sc,dS,d2S = regFuns[k](x,0.0.*x,M)
		if isempty(v)
			return Sc
		else
			return Sc,dot(dS,v)
		end
	end
	chkDer, = checkDerivative(testFun,mc,out=true)
	@test chkDer
end


"""
A 'Zipfian traveller population': interactors are located on a two-dimensional
lattice. Each interactor has a 'home' cell on the lattice, but also has the
option of travelling to another cell. The probability to travel to a cell
with coordinates ``x'`` and ``y'`` scales as the inverse of the distance
between that cell and the interactor's home (coordinates ``x`` and ``y``),
measured as the Manhattan distance ``d``:

```math
P \\propto \\frac{1}{(1 + d)^\\lambda}
```

where ``\\lambda`` is a scaling parameter. Interactors only rendezvous with
other interactors currently residing in the same cell.
"""
mutable struct ZipfTravellerPopulation{T<:AbstractInteractor} <: AbstractLatticePopulation
  # Census
  census::Array{T}

  # Spatial census
  spatial_census::Dict{Point{Int}, Array{Int}}

  # Interactors' homes
  coordinates::Array{Point{Int}}

  # Interactors' temporary (within-epoch) locations
  locations::Array{Point{Int}}

  # Lattice size
  size::Int

  # Decay exponent
  de::Float64

  # Probabilities
  probabilities::Array{Float64}

  # Distance function
  distance::Function

  # Range of possible distances
  possible_distances::Dict{Point{Int}, Array{Int}}

  # How many actions have occurred
  nactions::Int

  # Cells at a given distance from given cell
  lookup::Dict{Tuple{Point{Int}, Int}, Array{Point{Int}}}
end

"""
    ZipfTravellerPopulation(K::Int, lambda::Float64)

Construct a Zipf traveller population of size `K` by `K` with scaling
parameter `lambda`.
"""
function ZipfTravellerPopulation{T}(K::Int, lambda::Float64) where {T<:AbstractInteractor}
  # Possible distances on lattice
  possdist = collect(0:2*(K-1))

  # Possible distances for each cell
  possdistcell = Dict{Point{Int}, Array{Int}}()

  # Spatial census
  spatcens = Dict{Point{Int}, Array{Int}}()

  # Make lookup table
  lup = Dict{Tuple{Point{Int}, Int}, Array{Point{Int}}}()
  for x in 1:K
    for y in 1:K
      point = Point{Int}(x, y)
      for dist in possdist
        for i in 1:K
          for j in 1:K
            point2 = Point{Int}(i, j)
            if manhattan(point, point2) == dist
              if !haskey(possdistcell, point)
                possdistcell[point] = Array{Int}(undef, 0)
              end
              push!(possdistcell[point], dist)
              if !haskey(lup, (point, dist))
                lup[(point, dist)] = Array{Point{Int}}(undef, 0)
              end
              push!(lup[(point, dist)], point2)
            end
          end
        end
      end
      possdistcell[point] = unique(possdistcell[point])
      spatcens[point] = []
    end
  end

  # Construct
  ZipfTravellerPopulation(Array{T}(undef, 0), spatcens, Array{Point{Int}}(undef, 0), Array{Point{Int}}(undef, 0), K, lambda, zipf(2*(K-1) + 1, lambda), manhattan, possdistcell, 0, lup)
end

"""
    inject!(x::AbstractInteractor, y::ZipfTravellerPopulation)

Insert an interactor into a Zipf traveller population.
"""
function inject!(x::AbstractInteractor, y::ZipfTravellerPopulation)
  push!(y.census, x)
  home = Point{Int}(rand(1:y.size), rand(1:y.size))
  push!(y.coordinates, home)
  push!(y.locations, home)
  push!(y.spatial_census[home], length(y.census))
end

"""
    eject!(x::Int, y::ZipfTravellerPopulation)

Remove an interactor from a Zipf traveller population.
"""
function eject!(x::Int, y::ZipfTravellerPopulation)
  home = y.coordinates[x]
  loca = y.locations[x]
  splice!(y.census, x)
  splice!(y.coordinates, x)
  splice!(y.locations, x)
  y.spatial_census[loca] = setdiff(y.spatial_census[loca], x)
  for k in keys(y.spatial_census)
    for i in 1:length(y.spatial_census[k])
      if y.spatial_census[k][i] > x
        y.spatial_census[k][i] -= 1
      end
    end
  end
end

"""
    travel!(x::Int, y::ZipfTravellerPopulation)

Make interactor with index `x` travel in a Zipf traveller population.
"""
function travel!(x::Int, y::ZipfTravellerPopulation)
  home = y.coordinates[x]
  loca = y.locations[x]
  possdist = y.possible_distances[home]
  dist = Distributions.wsample(possdist, zipf(maximum(possdist) + 1, y.de))
  y.locations[x] = rand(y.lookup[(home, dist)])

  if y.locations[x] != loca
    y.spatial_census[loca] = setdiff(y.spatial_census[loca], x)
    push!(y.spatial_census[y.locations[x]], x)
  end
end

"""
    travel_all!(x::ZipfTravellerPopulation)

Make every interactor in a Zipf traveller population travel.
"""
function travel_all!(x::ZipfTravellerPopulation)
  for s in 1:length(x.census)
    travel!(s, x)
  end
end

"""
    travel!(x::ZipfTravellerPopulation)

Make a random interactor in a Zipf traveller population travel.
"""
function travel!(x::ZipfTravellerPopulation)
  travel!(rand(1:length(x.census)), x)
end

"""
    rendezvous!(x::ZipfTravellerPopulation)

Rendezvous in a Zipf traveller population (between two random interactors
currently in the same cell).
"""
function rendezvous!(x::ZipfTravellerPopulation)
  length(x.census) > 1 || return false
  int1 = rand(1:length(x.census))
  location = x.locations[int1]
  length(x.spatial_census[location]) > 1 || return false
  friends = setdiff(x.spatial_census[location], int1)
  int2 = rand(friends)
  rendezvous!(x, int1, int2)
end


sine1_3D_lc = environment_3D(x -> sin(x[1]) + sin(x[2]))
sine2_3D_lc = environment_3D(x -> 0.075 * sin(2π * x[1]))
sine3_3D_lc = environment_3D(x -> 0.075 * sin(2π * x[1]) * sin(2π * x[2]))

sine1_2D_lc = environment_2D(x -> 0.05 * (cos(pi * x[1]) - 1.0))
sine2_2D_lc = environment_2D(x -> 0.10 * sin(2π * x[1]))
sine3_2D_lc = environment_2D(x -> 0.03 * (cos(pi * x[1]) - 1.0))


mutable struct BlackCalculator{S <: StrikedTypePayoff}
  payoff::S
  strike::Float64
  forward::Float64
  stdDev::Float64
  discount::Float64
  variance::Float64
  d1::Float64
  d2::Float64
  alpha::Float64
  beta::Float64
  DalphaDd1::Float64
  DbetaDd2::Float64
  n_d1::Float64
  cum_d1::Float64
  n_d2::Float64
  cum_d2::Float64
  x::Float64
  DxDs::Float64
  DxDstrike::Float64
end

function BlackCalculator(p::S, fwd::Float64, stdDev::Float64, disc::Float64) where {S <: StrikedTypePayoff}
  calc = BlackCalculator{S}(p, p.strike, fwd, stdDev, disc, stdDev * stdDev, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
  initialize!(calc, p)

  return calc
end

function gen_addl_black_vars!(calc::BlackCalculator, optType::Call)
  calc.alpha = calc.cum_d1
  calc.DalphaDd1 = calc.n_d1
  calc.beta = -calc.cum_d2
  calc.DbetaDd2 = -calc.n_d2

  return calc
end

function gen_addl_black_vars!(calc::BlackCalculator, optType::Put)
  calc.alpha = -1.0 + calc.cum_d1
  calc.DalphaDd1 = calc.n_d1
  calc.beta = 1.0 - calc.cum_d2
  calc.DbetaDd2 = -calc.n_d2

  return calc
end

function initialize!(calc::BlackCalculator, p::StrikedTypePayoff)
  calc.strike >= 0.0 || error("strike $strike must be non-negative")
  calc.forward > 0.0 || error("forward $forward must be positive")
  calc.stdDev >= 0.0 || error("stdDev $stdDev must be non-negative")
  calc.discount > 0.0 || error("discount $discount must be positive")

  if calc.stdDev >= eps()
    if is_close(calc.strike, 0.0)
      calc.d1 = JQ_MAX
      calc.d2 = JQ_MAX
      calc.cum_d1 = 1.0
      calc.cum_d2 = 1.0
      calc.n_d1 = 0.0
      calc.n_d2 = 0.0
    else
      calc.d1 = log(calc.forward / calc.strike) / calc.stdDev + 0.5 * calc.stdDev
      calc.d2 = calc.d1 - calc.stdDev
      w = Normal() # Distributions
      calc.cum_d1 = cdf(w, calc.d1)
      calc.cum_d2 = cdf(w, calc.d2)
      calc.n_d1 = distribution_derivative(w, calc.d1)
      calc.n_d2 = distribution_derivative(w, calc.d2)
    end
  else
    if is_close(calc.forward, calc.strike)
      calc.d1 = 0.0
      calc.d2 = 0.0
      calc.cum_d1 = 0.5
      calc.cum_d2 = 0.5
      calc.n_d1 = M_SQRT_2 * M_1_SQRTPI
      calc.n_d2 = M_SQRT_2 * M_1_SQRTPI
    elseif calc.forward > calc.strike
      calc.d1 = JQ_MAX
      calc.d2 = JQ_MAX
      calc.cum_d1 = 1.0
      calc.cum_d2 = 1.0
      calc.n_d1 = 0.0
      calc.n_d2 = 0.0
    else
      calc.d1 = JQ_MIN
      calc.d2 = JQ_MIN
      calc.cum_d1 = 0.0
      calc.cum_d2 = 0.0
      calc.n_d1 = 0.0
      calc.n_d2 = 0.0
    end
  end

  calc.x = calc.strike
  calc.DxDstrike = 1.0

  calc.DxDs = 0.0

  gen_addl_black_vars!(calc, p.optionType)

  return calc
end

value(calc::BlackCalculator) = calc.discount * (calc.forward * calc.alpha + calc.x * calc.beta)

itm_cash_probability(calc::BlackCalculator) = calc.cum_d2
itm_asset_probability(calc::BlackCalculator) = calc.cum_d1

function delta(calc::BlackCalculator, spot::Float64)
  spot > 0.0 || error("positive spot value required.  $spot not allowed")

  DfowardDs = calc.forward / spot

  temp = calc.stdDev * spot
  DalphaDs = calc.DalphaDd1 / temp
  DbetaDs = calc.DbetaDd2 / temp
  temp2 = DalphaDs * calc.forward + calc.alpha * DfowardDs + DbetaDs * calc.x + calc.beta * calc.DxDs

  return calc.discount * temp2
end

function delta_forward(calc::BlackCalculator)
  temp = calc.stdDev * calc.forward
  DalphaDforward = calc.DalphaDd1 / temp
  DbetaDforward = calc.DbetaDd2 / temp
  temp2 = DalphaDforward * calc.forward + calc.alpha + DbetaDforward * calc.x

  return calc.discount * temp2
end

function elasticity(calc::BlackCalculator, spot::Float64)
  val = value(calc)
  del = delta(calc, spot)

  if val > eps()
    return del / val * spot
  elseif abs(del) < eps()
    return 0.0
  elseif del > 0.0
    return JQ_MAX
  else
    return JQ_MIN
  end
end

function gamma(calc::BlackCalculator, spot::Float64)
  spot > 0.0 || error("positive spot value required.  $spot not allowed")

  DforwardDs = calc.forward / spot

  temp = calc.stdDev * spot
  DalphaDs = calc.DalphaDd1 / temp
  DbetaDs = calc.DbetaDd2 / temp

  D2alphaDs2 = -DalphaDs / spot * (1 + calc.d1 / calc.stdDev)
  D2betaDs2 = -DbetaDs / spot * (1 + calc.d2 / calc.stdDev)

  temp2 = D2alphaDs2 * calc.forward + 2.0 * DalphaDs * DforwardDs + D2betaDs2 * calc.x + 2.0 * DbetaDs * calc.DxDs

  return calc.discount * temp2
end

function rho(calc::BlackCalculator, mat::Float64)
  mat >= 0.0 || error("negative maturity not allowed")

  DalphaDr = calc.DalphaDd1 / calc.stdDev
  DbetaDr = calc.DbetaDd2 / calc.stdDev

  temp = DalphaDr * calc.forward + calc.alpha * calc.forward + DbetaDr * calc.x

  return mat * (calc.discount * temp - value(calc))
end

function dividend_rho(calc::BlackCalculator, mat::Float64)
  mat >= 0.0 || error("negative maturity not allowed")

  DalphaDq = -calc.DalphaDd1 / calc.stdDev
  DbetaDq = -calc.DbetaDd2 / calc.stdDev

  temp = DalphaDq * calc.forward - calc.alpha * calc.forward + DbetaDq * calc.x

  return mat * calc.discount * temp
end

function vega(calc::BlackCalculator, mat::Float64)
  temp = log(calc.strike / calc.forward) / calc.variance
  DalphaDsigma = calc.DalphaDd1 * (temp + 0.5)
  DbetaDsigma = calc.DbetaDd2 * (temp - 0.5)

  temp2 = DalphaDsigma * calc.forward + DbetaDsigma * calc.x

  return calc.discount * sqrt(mat) * temp2
end

theta_per_day(calc::BlackCalculator, spot::Float64, mat::Float64) = theta(calc, spot, mat) / 365.0

function theta(calc::BlackCalculator, spot::Float64, mat::Float64)
  if mat < 0.0
    println("WARN: Maturity is negative")
    return -1.0
  end

  is_close(mat, 0.0) && return 0.0

  return -( log(calc.discount) * value(calc) + log(calc.forward / spot) * spot * delta(calc, spot)
          + 0.5 * calc.variance * spot * spot * gamma(calc, spot)) / mat
end

function strike_sensitivity(calc::BlackCalculator)
  temp = calc.stdDev * calc.strike
  DalphaDstrike = -calc.DalphaDd1 / temp
  DbetaDstrike = -calc.DbetaDd2 / temp

  temp2 = DalphaDstrike * calc.forward + DbetaDstrike * calc.x + calc.beta * calc.DxDstrike

  return calc.discount * temp2
end


import Base.+, Base.*

# Basic idea: Data structure to store a differential 1-form e.g. a contact form
struct OneForm
	# mapping from R^3 to R^3 that represents a contact from. Input are three arguments and a single 3-component vector should be returned
	rep
end

# the names should tell you everything
dx = OneForm((x,y,z) -> [1 0 0])
dy = OneForm((x,y,z) -> [0 1 0])
dz = OneForm((x,y,z) -> [0 0 1])

# addition of two contact forms
function (+)(a::OneForm, b::OneForm)
	return OneForm((x, y, z) -> a.rep(x,y,z) + b.rep(x,y,z))
end

# addition of a real number and a contact form
function (+)(a::Real, b::OneForm)
	return OneForm((x, y, z) -> a + b.rep(x,y,z))
end

# addition of a real number and a contact form
function (+)(a::OneForm, b::Real)
	return OneForm((x, y, z) -> a.rep(x,y,z) + b)
end

# addition of a real matrix and a contact form
function (+)(a::Matrix, b::OneForm)
	return OneForm((x, y, z) -> a + b.rep(x,y,z))
end

# addition of a real matrix and a contact form
function (+)(a::OneForm, b::Matrix)
	return OneForm((x, y, z) -> a.rep(x,y,z) + b)
end

# multiplication of a R^3 -> R function and a contact form
function (*)(f::Function, a::OneForm)
	return OneForm((x,y,z) -> f(x,y,z) * a.rep(x,y,z))
end

# multiplication of a real number and a contact form
function (*)(a::Real, b::OneForm)
	return OneForm((x,y,z) -> a * b.rep(x,y,z))
end


using MultivariateSeries

X = @ring x1 x2
L = monoms(X,5)

Xi0 = rand(2,3)
w0 = rand(3)

sigma = series(w0, Xi0, L)

w, Xi = decompose(sigma, cst_rkf(3))


"compute the kl divergence with mask where p is already the log(p)"
logkldivergence(q::Abstract3DTensor{T},
                logp::Abstract3DTensor{T},
                mask) where T =
                    sum(reshape(sum(sum(q .* (log.(q .+ eps(q[1])) .- logp); dims=1) .* mask; dims=2), :) ./ reshape(sum(mask; dims=2), :))

function logkldivergence(q, logp, mask)
    kld = (q .* (log.(q .+ eps(q[1])) .- logp)) #handle gpu broadcast error
    sum(kld .* mask) / sum(mask)
end

"compute the cross entropy with mask where p is already the log(p)"
logcrossentropy(q::Abstract3DTensor{T},
                logp::Abstract3DTensor{T},
                mask) where T =
                    -sum(reshape(sum(sum(q .* logp; dims=1) .* mask; dims=2), :) ./ reshape(sum(mask; dims=2), :))

function logcrossentropy(q, logp, mask)
    ce = q .* logp
    -sum(ce .* mask) / sum(mask)
end

"compute the kl divergence where p is already the log(p)"
logkldivergence(q::Abstract3DTensor{T},
                logp::Abstract3DTensor{T}) where T =
                    sum(reshape(sum(sum(q .* (log.(q .+ eps(q[1])) .- logp); dims=1); dims=2), :) ./ size(q, 2))

function logkldivergence(q, logp)
    kld = (q .* (log.(q .+ eps(q[1])) .- logp)) #handle gpu broadcast error
    sum(kld) / size(kld, 2)
end

"compute the cross entropy where p is already the log(p)"
logcrossentropy(q::Abstract3DTensor{T},
                logp::Abstract3DTensor{T}) where T =
                    -sum(reshape(sum(sum(q .* logp; dims=1); dims=2), :) ./ size(q, 2))

function logcrossentropy(q, logp)
    ce = q .* logp
    -sum(ce) / size(ce, 2)
end


using DataFrames, CSV;
using Serialization;

function calcEnt(datasets)
    num_samples = size(datasets, 1)
    label_counts = Dict()
    for label in datasets[end]
        if label in keys(label_counts)
            label_counts[label] += 1
        else
            label_counts[label] = 1
        end
    end
    # $ Ent(D) = - ∑_{p ∈ P} p log_2(p) $
    return - sum([p * log2(p) for p in values(label_counts) ./ num_samples ])
end

function calcGain(datasets, class, ent=calcEnt(datasets))
    num_samples = size(datasets, 1)
    groups = groupby(datasets, class)
    entries = [(size(subsets, 1), calcEnt(subsets)) for subsets in groups]
    # $Gain(D,a) = Ent(D) - ∑_{v ∈ V} |D^v| / |D| Ent(D^v)$
    return ent - sum([group_size / num_samples * grout_ent for (group_size, grout_ent) in entries])
end

function calcGainRatio(datasets, class, ent=calcEnt(datasets))
    num_samples = size(datasets, 1)
    groups = groupby(datasets, class)
    entries = [(size(subsets, 1), calcEnt(subsets)) for subsets in groups]
    gain =  ent - sum([group_size / num_samples * grout_ent for (group_size, grout_ent) in entries])
    # $IV = - ∑^V_{v=1} |D^v| / |D| log2(|D^v| / |D|)$
    IV = - sum([group_size / num_samples * log2(group_size / num_samples) for (group_size, grout_ent) in entries])
    # $Gain(D,a) = Gain(D,a) / IV$
    return gain / IV
end

function maxLabel(datasets)
    return by(datasets, :好瓜, df -> DataFrame(N=size(df, 1)))
end

function createTree(datasets, classes=names(datasets))
    groups = groupby(datasets, classes[end])
    if length(groups) == 1          # 取出分类标签（好瓜/坏瓜），如果只有一种，不需要继续分割
        return groups[end][end][end]
    end
    if length(classes) == 1         # 没有别的类别可以用来分割，取概率最大的分类标签
        max_group = groups[1]
        for group in groups[2:end - 1]
            if length(group) > length(max_group)
                max_group = group
            end
        end
        return max_group[end][end]
    end

    best_class = classes[1]         # 取出最好的分类类别，即信息增益最大的类别
    best_gain = calcGain(datasets, best_class)
    for class in classes[1:end - 1]
        curr_gain = calcGain(datasets, class)
        if curr_gain > best_gain
            best_class = class
            best_gain = curr_gain
        end
    end

    new_classes = copy(classes)     # 递归调用，下一个分支需要用到的分类标签
    filter!(x -> x != best_class, new_classes)   # 由于julia传引用，需要拷贝一份
    best_class_label = Set(datasets[best_class])
    tree = Dict(best_class => Dict())

    # 遍历该类别标准下所有子分类，递归求解
    for group in groupby(datasets, best_class)
        tree[best_class][group[best_class][1]] = createTree(group, new_classes)
    end

    return tree
end

datasets = CSV.read("data/2.0.csv", DataFrame);
tree = createTree(datasets)

print(tree)

describe(datasets)
by(datasets, :好瓜, df -> DataFrame(N=size(df, 1)))

stream = open("data/tree.data", "w")
serialize(stream,tree)
close(stream)

stream = open("data/tree.data", "r")
read_tree = deserialize(stream)
close(stream)


# Isomap
# ------
# A Global Geometric Framework for Nonlinear Dimensionality Reduction,
# J. B. Tenenbaum, V. de Silva and J. C. Langford, Science 290 (5500): 2319-2323, 22 December 2000

"""

    Isomap{NN <: AbstractNearestNeighbors} <: AbstractDimensionalityReduction

The `Isomap` type represents an isometric mapping model constructed with a help of the `NN` nearest neighbor algorithm.
"""
struct Isomap{NN <: AbstractNearestNeighbors} <: AbstractDimensionalityReduction
    nearestneighbors::NN
    component::AbstractVector{Int}
    model::KernelPCA
end
Isomap(nn::NN, model::KernelPCA) where {NN <: AbstractNearestNeighbors} = Isomap(nn, model, Int[])

## properties
outdim(R::Isomap) = outdim(R.model)
eigvals(R::Isomap) = principalvars(R.model)
neighbors(R::Isomap) = R.nearestneighbors.k
vertices(R::Isomap) = R.component

## show
summary(io::IO, R::Isomap{T}) where T =
    print(io, "Isomap{$T}(outdim = $(outdim(R)), neighbors = $(neighbors(R)))")

## interface functions
"""
    fit(Isomap, data; k=12, maxoutdim=2, nntype=BruteForce)

Fit an isometric mapping model to `data`.

# Arguments
* `data`: a matrix of observations. Each column of `data` is an observation.

# Keyword arguments
* `k`: a number of nearest neighbors for construction of local subspace representation
* `maxoutdim`: a dimension of the reduced space.
* `nntype`: a nearest neighbor construction class (derived from `AbstractNearestNeighbors`)

# Examples
```julia
M = fit(Isomap, rand(3,100)) # construct Isomap model
R = transform(M)             # perform dimensionality reduction
```
"""
function fit(::Type{Isomap}, X::AbstractMatrix{T};
             k::Int=12, maxoutdim::Int=2, nntype=BruteForce) where {T<:Real}
    # Construct NN graph
    NN = fit(nntype, X, k)
    D, E = knn(NN, X)
    G, C = largest_component(SimpleWeightedGraph(adjmat(D,E)))

    # Compute shortest path for every point
    n = length(C)
    DD = zeros(T, n, n)
    for i in 1:n
        dj = dijkstra_shortest_paths(G, i)
        DD[i,:] = dj.dists
    end

    M = fit(KernelPCA, dmat2gram(DD), kernel=nothing, maxoutdim=maxoutdim)

    return Isomap{nntype}(NN, C, M)
end

"""
    transform(R::Isomap)

Transforms the data fitted to the Isomap model `R` into a reduced space representation.
"""
transform(R::Isomap) = transform(R.model)

"""
    transform(R::Isomap, X::AbstractVecOrMat)

Returns a transformed out-of-sample data `X` given the Isomap model `R` into a reduced space representation.
"""
function transform(R::Isomap, X::AbstractVecOrMat{T}) where {T<:Real}
    n = size(X,2)
    D, E = knn(R.nearestneighbors, X, self = true)
    DD = gram2dmat(R.model.X)
    G = zeros(size(R.model.X,2), n)
    for i in 1:n
        G[:,i] = minimum(DD[:,E[:,i]] .+ D[:,i]', dims=2)
    end

    broadcast!(x->-x*x/2, G, G)
    transform!(R.model.center, G)
    return projection(R.model)'*G'
end


"""
    compute_ke(t_mesh_sp, t_mesh_v, f)

Compute kinetic Energy from phase space distribution `f`.

∫∫ v^2 f(x,v,t) dv dx

# Arguments
- `t_mesh_sp::NTuple{Nsp, UniformMesh{T}}`: space mesh.
- `t_mesh_v::NTuple{Nv, UniformMesh{T}}`: velocity mesh.
- `f::Array{T,Nsum}`: distribution function array.
"""
function compute_ke(
    t_mesh_sp::NTuple{Nsp,UniformMesh{T}},
    t_mesh_v::NTuple{Nv,UniformMesh{T}},
    f::Array{T,Nsum},
) where {T,Nsp,Nv,Nsum}
    Nsum == Nsp + Nv || "Nsp=$Nsp, Nv=$Nv, Nsum=$Nsum, we must have Nsum==Nsp+Nv"
    szv = length.(t_mesh_v)
    dsp = prod(step, t_mesh_sp)
    dv = prod(step, t_mesh_v)
    sum_sp = Array{T,Nv}(undef, szv)
    sum_sp .= reshape(sum(f, dims = ntuple(x -> x, Nsp)), szv)
    return (dsp * dv) * sum(dotprod(points.(t_mesh_v)) .^ 2 .* sum_sp)
end

"""
    compute_ke(self::AdvectionData)

Compute kinetic Energy.

∫∫ v^2 f(x,v,t) dv dx

# Arguments
- `self::AdvectionData`: mutable structure of variables data.
"""
function compute_ke(self::AdvectionData{T,N}) where {T,N}
    pvar::PoissonVar = getext(self)
    Nsp, Nv = getNspNv(pvar)
#    @show Nsp, Nv
    adv = self.adv
    szv = length.(adv.t_mesh[Nsp+1:N])
    dsp = prod(step, adv.t_mesh[1:Nsp])
    dv = prod(step, adv.t_mesh[Nsp+1:N])
#   @show szv
    res = sum(getdata(self), dims = ntuple(x -> x, Nsp))
#    @show size(res)
    sum_sp = reshape(res, szv)
    return (dsp * dv) * sum(pvar.pc.v_square .* sum_sp)
end

"""
    compute_charge!(rho, mesh_v, f)

Compute charge density from phase space distribution `f`.

ρ(x,t) = ∫ f(x,v,t) dv

# Arguments
- `rho::Array{T,Nsp}`: output result density array.
- `t_mesh_v::NTuple{Nv,UniformMesh{T}}`: velocity mesh.
- `f::Array{T,Nsum}`: distribution function array.
"""
function compute_charge!(
    rho::Array{T,Nsp},
    t_mesh_v::NTuple{Nv,UniformMesh{T}},
    f::Array{T,Nsum},
) where {T,Nsp,Nv,Nsum}
    Nsp + Nv == Nsum ||
        thrown(ArgumentError("Nsp=$Nsp Nv=$Nv Nsum=$Nsum we must have Nsp+Nv==Nsum"))
    dv = prod(step, t_mesh_v)
    rho .= dv * reshape(sum(f, dims = ntuple(x -> Nsp + x, Nv)), size(rho))
    rho .-= sum(rho) / prod(size(rho))
    nothing
end

function compute_elfield(
    t_mesh_x::NTuple{N,UniformMesh{T}},
    rho::Array{T,N},
    pfft,
) where {T<:AbstractFloat,N}

    fct_k(v) = im / sum(v .^ 2)

    v_k = vec_k_fft.(t_mesh_x)
    sz = length.(t_mesh_x)

    buf = fftgenall(pfft, rho) .* fct_k.(collect(Iterators.product(v_k...)))
    buf[1] = 0im

    return ntuple(
        x -> real(ifftgenall(pfft, reshape(v_k[x], tupleshape(x, N, sz[x])) .* buf)),
        N,
    )
end
"""
    compute_elfield!(elf::Array{T,1}, mesh::UniformMesh{T}, rho::Array{T,1}) where{T}

Computation of electric field of one dimension.
    ∇.e = - ρ

# Argument
 - `elf::Array{T,1}`: output Vector.
 - `mesh::UniformMesh{T}` : mesh of the vector
 - `rho::Array{T,1}` : rho computed before
"""
function compute_elfield!(elf::Array{T,1}, mesh::UniformMesh{T}, rho::Array{T,1}) where {T}
    elf .= compute_elfield((mesh,), rho, PrepareFftBig(size(rho), zero(T); numdims = 1))[1]
end

"""
    compute_ee(t_mesh_sp, t_elf)

Compute electric energy
|| E(t,.) ||_L2

# Arguments
- `t_mesh_sp::NTuple{N,UniformMesh{T}}`: space mesh.
- `t_elf::NTuple{N,Array{T,N}}`: electric field.
"""
function compute_ee(
    t_mesh_sp::NTuple{N,UniformMesh{T}},
    t_elf::NTuple{N,Array{T,N}},
) where {T,N}
    res = zero(T)
    dx = prod(step, t_mesh_sp)
    for i = 1:N
        res += sum(t_elf[i] .^ 2)
    end
    dx * res
end

"""
    compute_ee(self::AdvectionData)

Compute electric energy
|| E(t,.) ||_L2

# Argument
- `self::AdvectionData`: advection data structure.
"""
function compute_ee(self::AdvectionData)
    adv = self.adv
    pvar = getext(self)
    Nsp, Nv = getNspNv(pvar)
    dx = prod(step, adv.t_mesh[1:Nsp])
    return dx * sum(map(x -> sum(x .^ 2), pvar.t_elfield))
end
function getenergy(advd::AdvectionData)
    pv::PoissonVar = getext(advd)
    compute_charge!(pv, advd)
    compute_elfield!(pv)
    elenergy = compute_ee(advd)
    kinenergy = compute_ke(advd)
    energyall = elenergy + kinenergy
    return elenergy, kinenergy, energyall
end
function getenergyall(advd::AdvectionData)
    _, _, energyall = getenergy(advd)
    return energyall
end


using Econometrics, Statistics, Flux, Random, LinearAlgebra
using Base.Iterators
using BSON: @load
using BSON: @save

# unsmoothed QR objective
function QRobj(yhat, y, τ)
    ξ = y .- yhat
    sum(max.(τ.*ξ,(τ .-1.0).*ξ))
end
# smoothed QR objective
function SmoothQRobj(yhat, y, τ, α=0.02)
    ξ = y .- yhat
    sum(τ.*ξ .+ α.*log.(1.0 .+ exp.(-1.0 .* ξ./α)))
end

function Train()
    @load "cooked_data.bson" params statistics
    params = Float32.(params)
    statistics = Float32.(statistics)
    S = TrainingTestingSize # number of draws from prior
    trainsize = Int(TrainingProportion*S)
    yin = params[1:trainsize, :]'
    yout = params[trainsize+1:end, :]'
    xin = statistics[1:trainsize, :]'
    xout = statistics[trainsize+1:end, :]'
    # model
    nStats = size(xin,1)
    model = Chain(
        Dense(nStats,5*nParams, tanh),
        Dense(5*nParams, nParams)
    )
    θ = Flux.params(model)
    opt = AdaMax()
    # weight by inverse std. dev. of params, to put equal weight
    s = Float32.(std(params,dims=1)')
    # Select the objective here: ordinary regression or (smoothed) quantile regression
    #loss(x,y) = sqrt.(Flux.mse(model(x)./s,y./s))
    τ = 0.5  # choose the quantile you want
    #loss(x,y) = QRobj(model(x),y, τ)
    loss(x,y) = SmoothQRobj(model(x),y, τ)
    function monitor(e)
        println("epoch $(lpad(e, 4)): (training) loss = $(round(loss(xin,yin); digits=4)) (testing) loss = $(round(loss(xout,yout); digits=4))| ")
    end
    bestsofar = 1.0e10
    pred = 0.0 # define it here to have it outside the for loop
    batches = [(xin[:,ind],yin[:,ind])  for ind in partition(1:size(yin,2), 32)];
    for i = 1:Epochs
        Flux.train!(loss, θ, batches, opt)
        current = loss(xout,yout)
        if current < bestsofar
            bestsofar = current
            @save "best.bson" model
            xx = xout
            yy = yout
            println("________________________________________________________________________________________________")
            monitor(i)
            pred = model(xx) # map pred to param space
            error = yy .- pred
            results = [pred;error]
            rmse = sqrt.(mean(error.^Float32(2.0),dims=2))
            println(" ")
            println("RMSE for model parameters ")
            prettyprint(reshape(round.(rmse,digits=3),1,3))
            println(" ")
            println("dstats prediction of parameters:")
            dstats(pred');
            println(" ")
            println("dstats prediction error:")
            dstats(error');
        end
    end
    return nothing
end


######################################################## model
F(z::BorderedArray,θ::AbstractVector) = F(z.u,(θ=θ,p=z.p))
function F(u::AbstractVector,parameters::NamedTuple)

	@unpack θ,p = parameters
	μ₁,μ₂, a₁,a₂, k = θ

	f = first(u)*first(p)*first(θ)
	F = similar(u,typeof(f))

	F[1] = ( 10^a₁ + (p*u[2])^2 ) / ( 1 + (p*u[2])^2 ) - u[1]*10^μ₁
	F[2] = ( 10^a₂ + (k*u[1])^2 ) / ( 1 + (k*u[1])^2 ) - u[2]*10^μ₂

	return F
end

######################################################### targets and initial guess
X = StateSpace( 2, 3:0.001:7, [4,5] )
θ = SizedVector{5}(0.0,0.0,-1.0,-1.0,2.0)


"""
    Metropolis(y, x, β₀, β, M)

A Metropolis algorithm for probit regression.

# Arguments
- `y::Vector{Bool}`: boolean vector of outcomes
- `x::Matrix{<:Real}`: real valued vector or matrix of inputs
- `β₀::MvNormal`: a univariate or multivariate normal distribution for the prior
- `β::MvNormal`: a univariate or multivariate normal distribution for the candidate
- `M::Integer`: the number of draws

# Output
- `chain::Matrix{<:Float}`: matrix of draws from the target distribution
- `accept::Vector{Bool}`: boolean vector of acceptance indicators

The function can be called without 'x' in which case a constant-only model is estimated.
"""

function estimate(Time::Vector{T} where T<:Real,
                  Indicator::BitVector,
                  X::Matrix{T} where T<:Real,
                  β::MvNormal,
                  α::Exponential,
                  draws = 10_000::Integer)

    N, J = size(X)
    @assert length(Time) == N "Time and X must have the same number of rows."
    @assert length(Indicator) == N "Indicator and X must have the same number of rows."
    @assert J == length(β.μ) "The number of columns in x must match the dimension of β."

    # Initial values
    b = zeros(draws, J)
    a = zeros(draws)

    b[1, :] = β.μ
    a[1] = α.θ

    Ts = copy(Time)
    Ss = findall(Indicator .== 1)
    for i in 2:draws
        # Propose new values
        b[i, :], a[i] = proposal(b[i-1, :], a[i-1], β, α, Ts, X)

        # Time - update missing values (i.e. censored values)
        for n in Ss
            λ = exp(-dot(X[n, :], b[i, :]) * a[i])
            Ts[n] = rand(Truncated(Weibull(a[i], λ), Time[n], Inf))
        end
    end
    return (b, a)
end

function lambda(b::Vector, a::AbstractFloat, X::Matrix)
    m = X * b
    λ = exp.(-m * a)
    return λ
end

function loglik(b::Vector, a::AbstractFloat, Time::Vector, X::Matrix)
    λ = lambda(b, a, X)
    ll = sum(logpdf.(Weibull.(a, λ), Time))
    return ll
end

function logprior(prior, x)
    return sum(logpdf(prior, x))
end

function proposal(b, a, β, α, Time, X)
    # Existing
    pr = logprior(α, a) + logprior(β, b) # Log-prior
    ll = loglik(b, a, Time, X)           # Log-likelihood
    po = pr + ll                         # Log-posterior

    # Proposal
    a1 = exp(rand(Normal(log(a), 0.05))) # Proposal for alpha
    b1 = rand.(Normal.(b, 0.05))         # Proposal for beta

    pr1 = logprior(α, a1) + logprior(β, b1) # Log-prior
    ll1 = loglik(b1, a1, Time, X)           # Log-likelihood
    po1 = pr1 + ll1                         # Log-posterior

    if log(rand()) < (po1 - po)
        return (b1, a1)
    else
        return (b, a)
    end
end


# Everything about intersecting
export meet_check, meet, ∧, stab_count, is_simple

"""
`meet_check(L::HLine,LL::HLine)` determines if two lines intersect.
Also for any combination of lines, segments, or rays.
"""
function meet_check(L::HLine, LL::HLine)::Bool
    if L == LL
        return false
    end
    s = L.s
    t = L.t
    ss = LL.s
    tt = LL.t

    if s < ss < t < tt
        return true
    end
    if ss < s < tt < t
        return true
    end
    return false
end

function meet_check(L::HLine, S::HSegment)::Bool
    LL = HLine(S)  # extend S
    if !meet_check(L,LL)
        return false
    end

    P = meet(L,LL)
    return in(P,S)
end

meet_check(S::HSegment,L::HLine)::Bool = meet_check(L,S)

function meet_check(S::HSegment, SS::HSegment)::Bool
    L = HLine(S)
    LL = HLine(SS)

    if L==LL  # see if they share exactly one endpoint
        A,B = endpoints(S)
        AA,BB = endpoints(SS)
        if A==AA && between(B,A,BB)
            return true
        end
        if A==BB && between(A,B,BB)
            return true
        end
        if B==AA && between(A,B,BB)
            return true
        end
        if B==BB && between(A,B,AA)
            return true
        end
        return false
    end

    if !meet_check(L,LL)
        return false
    end

    P = meet(L,LL)
    if in(P,S) && in(P,SS)
        return true
    end
    return false
end

function meet_check(L::HLine, R::HRay)::Bool
    LL = HLine(R)
    if !meet_check(L,LL)
        return false
    end
    P = meet(L,LL)
    return in(P,R)
end
meet_check(R::HRay, L::HLine)::Bool = meet_check(L,R)

function meet_check(R::HRay, S::HSegment)::Bool
    LR = HLine(R)
    LS = HLine(S)

    if LR == LS   # check if endpoints match up
        A,B = endpoints(S)
        X = get_vertex(R)
        if A==X
            return !in(B,R)
        end
        if B==X
            return !in(A,R)
        end
        return false
    end
    # R and S noncollinear
    if !meet_check(LR,LS)
        return false
    end
    P = meet(LR,LS)
    return in(P,R) && in(P,S)
end
meet_check(S::HSegment, R::HRay)::Bool = meet_check(R,S)

function meet_check(R::HRay, RR::HRay)::Bool
    L = HLine(R)
    LL = HLine(RR)

    if L==LL   # make sure same vertex, opposite directions
        if get_vertex(R) != get_vertex(RR)
            return false
        end
        if abs(R.t - RR.t) <= THRESHOLD * eps(1.0)
            return false
        end
        return true
    end

    if !meet_check(L,LL)
        return false
    end
    P = meet(L,LL)
    return in(P,R) && in(P,RR)
end

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

"""
`meet(L,LL)` finds a point on lines `L` and `LL` or throws an
error if they don't intersect.  Also for a line and a segment, or
two segments.

See `meet_check`.
"""
function meet(L::HLine, LL::HLine)::HPoint
    @assert meet_check(L,LL) "The lines do not intersect"

    s = L.s   # artificially rotate to 0
    t = L.t
    ss = LL.s
    tt = LL.t

    a = exp(im*(t-s))
    b = exp(im*(ss-s))
    c = exp(im*(tt-s))

    A = real(in_up(a))
    B = real(in_up(b))
    C = real(in_up(c))

    R = abs(B-C)/2
    Z = (B+C)/2

    y = sqrt(R^2 - (A-Z)^2)

    p = up_in(A + im*y)*exp(im*s)

    return HPoint(p)
end

function meet(L::HLine, S::HSegment)::HPoint
    @assert meet_check(L,S) "The line and segment do not intersect at a unique point"
    p = meet(L, HLine(S))
    return p
end

meet(S::HSegment,L::HLine)::HPoint = meet(L,S)

function meet(S::HSegment,SS::HSegment)
    @assert meet_check(S,SS) "The segments do not intersect at a unique point"

    L = HLine(S)
    LL = HLine(SS)

    if L != LL
        p = meet(HLine(S),HLine(SS))
        return p
    end

    # special case: segments overlap in an end point
    A,B = endpoints(S)
    AA,BB = endpoints(SS)

    if A==AA || A==B
        return A
    end
    if B==AA || B==BB
        return B
    end
    # Should never get here
    @error "Programming error in meet(HSegment, HSegment)"
end

function meet(L::HLine, R::HRay)::HPoint
    @assert meet_check(L,R) "The line and ray do not intersect at a unique point"
    return meet(L, HLine(R))
end
meet(R::HRay, L::HLine)::HPoint = meet(L,R)

function meet(R::HRay, S::HSegment)::HPoint
    @assert meet_check(R,S) "The ray and segment do not intersect at a unique point"
    LR = HLine(R)
    LS = HLine(S)
    if LR != LS
        return meet(HLine(R, HLine(S)))
    end
    # must have an end of S as the common point
    A,B = endpoints(S)
    if in(A,R)
        return A
    end
    return B
end
meet(S::HSegment, R::HRay)::HPoint = meet(R,S)

function meet(R::HRay, RR::HRay)::HPoint
    @assert meet_check(R,RR) "The two rays do not instersect at a unique point"
    if HLine(R) == HLine(RR)
        return get_vertex(R)
    end
    return meet(HLine(R,HLine(RR)))
end

(∧)(L::HLinear, LL::HLinear)::HPoint = meet(L,LL)

"""
`stab_count(R,targets)` is used to count how many items in `targets` are
intersected by `R` where `targets` is a list of `HLinear` objects and `R`
is an `HLinear`.
"""
function stab_count(R::S, targets::Array{T,1}) where {S<:HLinear, T <: HLinear}
    count = 0
    nt = length(targets)
    for j = 1:nt
        if meet_check(R,targets[j])
            count += 1
        end
    end
    return count
end

"""
`in(p::HPoint, X::HPolygon)` determines if the point lies on the boundary or,
or is interior to, the polygon. Be sure that `polygon_check(X)` and
`is_simple(X)` both return `true`.
"""
function in(p::HPoint, X::HPolygon)::Bool
    for a in X.plist
        if p==a
            return true
        end
    end

    ss = sides(X)
    for s in ss
        if in(p,s)
            return true
        end
    end

    # Get a ray that doesn't go through a vertex (and therefore not through a side)
    R = RandomHRay(p)
    while true
        for a in X.plist
            if in(a,R)
                R = RandomHRay()
                continue
            end
        end
        break
    end

    return stab_count(R,ss)%2 == 1
end


function fourth(∇f, x)
    fourth(nothing, ∇f, x)
end
function fourth(∇²f, ∇f, x)
    if !(∇²f == nothing)
        ∇²f = 12x^2
    end
    if !(∇f == nothing)
        ∇f = 4x^3
    end

    fx = x^4
    if ∇f == nothing && ∇²f == nothing
        return fx
    elseif ∇²f == nothing
        return fx, ∇f
    else
        return fx, ∇f, ∇²f
    end
end

minimize(fourth, 4.0, BFGS(DirectApprox()))


## eer.jl  Equal Error Rate and friends
## (c) 2014--2015 David A. van Leeuwen
##
## This code is licenced under the MIT software license

## eerch: computes the eer using the convex hull method
"""
`eerch()` computes the Equal Error Rate using the Convex Hull interpretation.  This computes the error
rate at which the convex hull of the ROC curve crosses the `y=x` diagonal in the ROC.

This value is less sensitive to trial sampling at low number of trials, and has the interpretation of
`the maximum while vary prior of the minimum decision costs'---a useful quantity in decision cost
function analysis of a two class classification system.

Arguments:

 - `r::Roc`: an object of type `Roc`, the result of `roc()`

 - `tar, non`: Vectors of target and non-target scores

 - `pfa, pmiss, ch`: Vectors of the false positive and false negative rate, and an array indicating
  the membership of the `(pfa, pmiss)` point on the convex hull.  These points are found by `roc()`.
"""
eerch(r::Roc) = eerch(r.pfa, r.pmiss, r.ch)
eerch(tar::Vector, non::Vector) = eerch(roc(tar, non))

## compute the EER given corresponding pfa and pmiss array, but use the
## convex hull points only.
function eerch(pfa::Vector{T}, pmiss::Vector{T}, ch::BitVector) where T<:AbstractFloat
    @assert length(pfa) == length(pmiss) == length(ch)
    ## find the index on the ch where the crossing of pfa=pmiss occurs
    chi = findall(ch)
    i = chi[1]
    direction = sign(pfa[i] - pmiss[i]) # >0 if first pfa > pmiss
    ## does it pay off to do a binary search?  It should, really.
    li = i
    for loop_variable in 1:length(chi)
        i = chi[loop_variable]
        if sign(pfa[i] - pmiss[i]) != direction
            break
        end
        li = i
    end
    ## compute the crossing
    (ax,bx) = pfa[[li,i]]
    (ay,by) = pmiss[[li,i]]
    return crossing(ax, ay, bx, by)
end

## compute crossing with y=x points (ax,ay) and (bx,by)
function crossing(ax, ay, bx, by)
    den = ax-ay-bx+by
    if den == 0
        return (ax + bx + ay + by) / 4
    else
        return ax + (ax-ay)*(bx-ax) / den
    end
end

## just a simple EER approximation, optimized for memory and speed
"""
`eer(tar, non)` computes a simple approximation to the Equal Error
Rate, the intersection of the ROC with the line `y=x`, i.e., the error
rate at which the false positive rate and the false negative rate are
approximately equal.

For a more consistent interpretation and implementation of the EER, please consider `eerch()`,
the Equal Error Rate - Convex Hull computation.
"""
eer(tar::Vector{T}, non::Vector{T}) where {T<:Real} = eer_sorted(sort(tar), sort(non))
eer(r::Roc) = eerch(r.pfa, r.pmiss, trues(length(r.pfa)))

## if tar and non are sorted, we may be doing even faster...
function eer_sorted(tar::Vector{T}, non::Vector{T}) where T<:Real
    ntar = length(tar)
    nnon = length(non)
    ntar > 0 && nnon > 0 || error("target and non-target arrays must not be empty")
    Δfa = 1. / nnon
    Δmiss = 1. / ntar
    pmiss = 0.
    ni = binsearch(tar[1], non, lower=true) # index in non-targets of lowest target score
    pfa = 1. - ni * Δfa
    ti = 1
    ni += 1
    lpfa, lpmiss = pfa, pmiss
    while ni ≤ nnon && ti ≤ ntar
        if tar[ti] < non[ni]
            pmiss += Δmiss
            ti += 1
        elseif tar[ti] > non[ni]
            pfa -= Δfa
            ni += 1
        else
            pmiss += Δmiss
            ti += 1
            pfa -= Δfa
            ni += 1
        end
        if pfa < pmiss
            break
        end
        lpfa, lpmiss = pfa, pmiss
    end
    return crossing(pfa, pmiss, lpfa, lpmiss)
end

## Returns the index to the largest value in the sorted array `a` ≤ `x` if lower==false
## If lower==true, the value must be strictly < `x`
function binsearch(x::Real, a::Vector{T}; lower=false, check=false) where T<:Real
    check && (issorted(a) || error("Array needs to be sorted"))
    mi = 1
    ma = length(a)
    if x < a[mi] || lower && x == a[mi]
        return 0
    elseif x > a[ma] || !lower && x == a[ma]
        return ma
    end
    while ma - mi > 1
        h = mi + div(ma-mi,2)
        if x > a[h] || !lower && x == a[h]
            mi = h
        else
            ma = h
        end
    end
    return mi
end

## EER, given the sort order of `[tar, non]`
## The idea is that you don't need to sort multiple times if you have
## multiple selections.  This cannot be accurate for cases where
## `tar` and `non` have the same value
function eer_so(so::Vector{T}, ntar::T, selection::BitVector) where T<:Integer
    length(so) == length(selection) || error("selection not the same length as sort order")
    eer(so[findin(so, find(selection))], ntar)
end

## This computes the actual approximate eer from sortorder and number of targets and nontargets
## The sort order "so" may be a subset of an original sort order, "thres" should be the
## original number of targets.
function eer_so(so::Vector{T}, thres::T, ntar::T=sum(so .<= thres)) where T<:Integer
    nnon = length(so) - ntar
    Δfa = 1. / nnon
    Δmiss = 1. / ntar
    pfa = 1.
    pmiss = 0.
    lpfa, lpmiss = pfa, pmiss
    for s in so
        if s <= thres            # target score
            pmiss += Δmiss
        else
            pfa -= Δfa
        end
        if pfa < pmiss
            break
        end
        lpfa, lpmiss = pfa, pmiss
    end
    return crossing(pfa, pmiss, lpfa, lpmiss)
end

## experimental routine for timing and optimization...
function eer_experimental(tar::Vector{T}, non::Vector{T}; method=:naive, fast=true) where T
    ntar = length(tar)
    nnon = length(non)
    @time scores = vcat(non,tar)      # 1
    @time so = sortperm(scores)       # 1
    @time truth = so .> nnon          # 1/64
    @time if !fast
        pfa = 1.0 - cumsum(!truth)/nnon
        pmiss = cumsum(truth)/ntar
    else                        # this takes less memory, but that's all I can say for it.
        Δ = 1/nnon
        pfa = similar(scores)   # 1
        s = one(T)
        for i=1:length(scores)
            s -= Δ*!truth[i]
            pfa[i] = s
        end
        Δ = 1/ntar
        pmiss = similar(scores) # 1
        s = zero(T)
        for i=1:length(scores)
            s += Δ*truth[i]
            pmiss[i] = s
        end
    end
    @time if method==:naive
        i = indmin(abs(pfa-pmiss))
        res = (pfa[i] + pmiss[i]) / 2
    end
    return res, (pfa, pmiss, scores[so]) # 1
end


mutable struct LinearRegression{T} <: AbstractMLModel
    β0::T
    β::Vector{T}
    A::Matrix{T}
end
LinearRegression(T = Float64) = LinearRegression(zero(T), zeros(T, 0), zeros(T, 0, 0))

algorithm(::LinearRegression{T}) where {T} = Sweep(T)

mutable struct Sweep{T} <: ClosedFormSolver
    A::Matrix{T}  # [x 1 y]' * [x 1 y] =
end
Sweep(T=Float64) = Sweep(zeros(T, 0, 0))

# function update!(model::LinearRegression{T}, strat::Sweep{T}, data::MLData{X,Y,Nothing,:rows}) where {T,X,Y}
#     x, y = xy(data)
#     n = nobs(data)
#     p = npredictors(data)
#     strat.A = zeros(T, p + 2, p + 2)
#     model.β = zeros(T, p)
#     strat.A[1:end-2, 1:end-2] = x'x / n
#     strat.A[1:end-2, end-1] = vec(mean(x, 1))
#     strat.A[end-1, ]
#     strat.A[end] = one(T)
# end

# function learn!(model, strat::LearningStrategy, data)
#     setup!(strat, model[, data])
#     for (i, item) in enumerate(data)
#         update!(model, strat[, i], item)
#         hook(strat, model[, data], i)
#         finished(strat, model[, data], i) && break
#     end
#     cleanup!(strat, model)
#     model
# end


module ReSample
using ..Cubes
using ..DAT
export spatialinterp
import Interpolations: BSpline, scale,extrapolate, interpolate, Constant, Linear, Quadratic,
  Cubic, OnGrid, Flat, Line,Free, Periodic, Reflect
function fremap(xout,xin,oldlons,oldlats,newlons,newlats;order=Linear(),bc = Flat())
  #interp = LinearInterpolation((oldlons,oldlats),xin);
  interp = extrapolate(scale(interpolate(xin, BSpline(order)), oldlons,oldlats), bc)
  for (ilat,lat) in enumerate(newlats)
    for (ilon,lon) in enumerate(newlons)
      xout[ilon,ilat]=interp(lon,lat)
    end
  end
end

"""
  spatialinterp(c::AbstractCubeData,newlons::AbstractRange,newlats::AbstractRange;order=Linear(),bc = Flat())
"""
function spatialinterp(c::AbstractCubeData,newlons::AbstractRange,newlats::AbstractRange;order=Linear(),bc = Flat())
  oldlons = getAxis("Lon",c).values
  oldlats = getAxis("Lat",c).values
  indims=InDims("Lon","Lat")
  outdims=OutDims(LonAxis(newlons),LatAxis(newlats))
  if step(oldlats)<0
    oldlats = reverse(oldlats)
    newlats = reverse(newlats)
  end

  mapCube(fremap,c,oldlons,oldlats,newlons,newlats,indims=indims,outdims=outdims,order=order,bc=bc)
end
spatialinterp(c::AbstractCubeData,newlons::CubeAxis,newlats::CubeAxis;kwargs...)=
  spatialinterp(c,newlons.values,newlats.values)
# spatialinterp(c::AbstractCubeData,target_cube::AbstractCubeData)=
#   spatialinterp(c,getAxis("Lon",))
end #module


using Test, JuMP

using Distributed
addprocs(2)
@everywhere using CharibdeOptim
@everywhere using IntervalArithmetic

@testset "Using Charibde for Constrained Optimsation" begin
      @everywhere using ModelingToolkit

      @everywhere vars = ModelingToolkit.@variables x y
      @everywhere C1 = constraint(vars, x+y, -Inf..4)
      @everywhere C2 = constraint(vars, x+3y, -Inf..9)
      @everywhere constraints = [C1, C2]
      (maxima, maximisers, info) = charibde_max(X->((x,y)=X;-(x-4)^2-(y-4)^2), IntervalBox(-4..4, -4..4), constraints)
      @test maxima ⊆ -8.01 .. -7.99
      @test maximisers[1] ⊆ (1.99 .. 2.01) × (1.99 .. 2.01)
end

@testset "Using JuMP syntax for Constrained Optimisation" begin
      model = Model(with_optimizer(CharibdeOptim.Optimizer))
      @variable(model, -4 <= x <= 4)
      @variable(model, -4 <= y <= 4)
      @NLconstraint(model, x+y<=4)
      @NLconstraint(model, 5<=x+3y<=9)
      @NLobjective(model, Max, -(x-4)^2-(y-4)^2)
      optimize!(model)

      @test JuMP.termination_status(model) == MOI.OPTIMAL
      @test JuMP.primal_status(model) == MOI.FEASIBLE_POINT
      @test JuMP.objective_value(model) ⊆ -8.01 .. -7.99
      @test JuMP.value(x) ⊆ (1.99 .. 2.01)
      @test JuMP.value(y) ⊆ (1.99 .. 2.01)
end

@testset "Using Interval bound and contract algorithm for Constrained Optimisation" begin
      vars = ModelingToolkit.@variables x y
      C1 = constraint(vars, x+y, -Inf..4)
      C2 = constraint(vars, x+3y, -Inf..9)

      (maxima, maximisers, info) = ibc_maximise(X->((x,y)=X;-(x-4)^2-(y-4)^2), IntervalBox(-4..4, -4..4),[C1, C2])
      @test maxima ⊆ -8.01 .. -7.99
      @test maximisers[1] ⊆ (1.99 .. 2.01) × (1.99 .. 2.01)
end

@testset "Optimising by Interval Branch and Contract Algorithm" begin
      (global_min, minimisers)= ibc_minimise(X->((x,y)= X;x^2 + y^2), IntervalBox(2..3, 3..4))
      @test global_min ⊆ 13 .. 13.01
      @test minimisers[1] ⊆ (2.0 .. 2.001) × (3.0 .. 3.001)
end

@testset "Optimising by Differencial Evolution" begin
      (global_min, minimiser)= diffevol_minimise(X->((x,y)= X;x^2 + y^2), IntervalBox(2..3, 3..4), iterations = 40)
      @test global_min ⊆ 13 .. 13.01
      @test minimiser ⊆ (2.0 .. 2.001) × (3.0 .. 3.001)
end

@testset "Optimising by Charibde (A hybrid approach) using only one worker" begin

      (global_min, minimisers, info) = charibde_min(X->((x,y)=X;x^2+y+1), IntervalBox(1..2, 2..3), workers = 1)
      @test global_min ⊆ 4.0 .. 4.01
      @test minimisers[1] ⊆ (1..1.001) × (2..2.001)

      (global_min, minimisers, info)= charibde_min(X->((x,y)=X;x^2 + y^2), IntervalBox(2..3, 3..4), workers = 1)
      @test global_min ⊆ 13 .. 13.01
      @test minimisers[1] ⊆ (2.0 .. 2.001) × (3..3.001)
end

@testset "Using JuMP syntax by using only one worker " begin           #for using two workers just dont pass 'workers' arguments as its value is set to 2
      model = Model(with_optimizer(CharibdeOptim.Optimizer, workers = 1))
      @variable(model, 1<=x<=2)
      @variable(model, 1<=y<=2)
      @NLobjective(model, Min, x^2+y^2)
      optimize!(model)

      @test JuMP.termination_status(model) == MOI.OPTIMAL
      @test JuMP.primal_status(model) == MOI.FEASIBLE_POINT
      @test JuMP.objective_value(model) ≈ 2.0
      @test JuMP.value(x) ≈ 1.0
      @test JuMP.value(y) ≈ 1.0
end

 # No need to add worker because a worker is already added while running testset "Using Charibde for Constrained Optimsation".
 # Otherwise we have to add a worker by using 'Distributed.addprocs(1)' and load the package on each worker
 # by '@everywhere using CharibdeOptim'.

@testset "Optimising by Charibde (A hybrid approach) using 2 workers" begin
      (global_min, minimisers, info) = charibde_min(X->((x,y)=X;x^3 + 2y + 5), IntervalBox(2..4, 2..3))
      @test global_min ⊆ 17.0 .. 17.01
      @test minimisers[1] ⊆ (2..2.001) × (2..2.001)

      (global_min, minimisers, info)= charibde_min(X->((x,y)=X;x^2 + y^2), IntervalBox(2..3, 3..4))
      @test global_min ⊆ 13 .. 13.01
      @test minimisers[1] ⊆ (2.0 .. 2.001) × (3..3.001)

end

@testset "Optimising difficult problem using JuMP" begin
      model = Model(with_optimizer(CharibdeOptim.Optimizer))

      @variable(model, 2 <= x1 <= 3)
      @variable(model, 3 <= x2 <= 4)
      @variable(model, 9 <= x3 <= 14)
      @variable(model, 2 <= x4 <= 3)
      @variable(model, 3 <= x5 <= 4)
      @variable(model, 9 <= x6 <= 14)
      @variable(model, 2 <= x7 <= 3)
      @variable(model, 3 <= x8 <= 4)
      @variable(model, 9 <= x9 <= 14)

      @NLobjective(model, Min, x1^2 + x2^2 + x3^4 - x4^7 - 200x5 - x6^5 - x7^9 + x8^5 - 8x9^3)

      optimize!(model)

      @test JuMP.termination_status(model) == MOI.OPTIMAL
      @test JuMP.primal_status(model) == MOI.FEASIBLE_POINT
      @test JuMP.objective_value(model) ≈ -575629.0
      @test JuMP.value(x1) ⊆ 1.99 .. 2.01
      @test JuMP.value(x2) ⊆ 2.99 .. 3.01
      @test JuMP.value(x3) ⊆ 8.99 .. 9.01
      @test JuMP.value(x4) ⊆ 2.99 .. 3.01
      @test JuMP.value(x5) ⊆ 3.99 .. 4.01
      @test JuMP.value(x6) ⊆ 13.99 .. 14.01
      @test JuMP.value(x7) ⊆ 2.99 .. 3.01
      @test JuMP.value(x8) ⊆ 2.99 .. 3.01
      @test JuMP.value(x9) ⊆ 13.99 .. 14.01
end

@testset "Optimising difficult problems using Charibde" begin
      f = X->((x1,x2,x3,x4,x5,x6,x7,x8,x9)=X;x1^2 + x2^2 + x3^4 - x4^7 - 200x5 - x6^5 - x7^9 + x8^5 - 8x9^3)
      X = IntervalBox(2..3, 3..4, 9..14, 2..3, 3..4, 9..14, 2..3, 3..4, 9..14)
      (global_min, minimisers, info) = charibde_min(f, X)

      @test global_min ⊆ -575630 .. -575628
      @test minimisers[1] ⊆ (1.99 .. 2.01) × (2.99 .. 3.01) × (8.99 .. 9.01) × (2.99 .. 3.01) × (3.99 .. 4.01) × (13.99 .. 14.01) × (2.99 .. 3.01) × (2.99 .. 3.01) × (13.99 .. 14.01)

end


using Revise
using Basins
using Printf
using Plots
using DynamicalSystems

@inline @inbounds function duffing(u, p, t)
    d = p[1]; F = p[2]; omega = p[3]
    du1 = u[2]
    du2 = -d*u[2] + u[1] - u[1]^3 + F*sin(omega*t)
    return SVector{2}(du1, du2)
end

#F=0.3818791946308725; ω= 0.1966442953020134
F=0.2771812080536913; ω=0.1;  # smooth boundary
#ω=0.1617;F = 0.395
#ω=0.3;F = 0.1
ds = ContinuousDynamicalSystem(duffing, rand(2), [0.15, F, ω])
integ_df  = integrator(ds; reltol=1e-8, save_everystep=false)
xg = yg = range(-2.2,2.2,length=100)
@time bsn = Basins.basins_map2D(xg, yg, integ_df; T=2*pi/ω)

# Basin entropy
@show Sb,Sbb = basin_entropy(bsn; eps_x=20, eps_y=20)

# Wada merge Haussdorff distances
@time max_dist,min_dist = detect_wada_merge_method(xg, yg, bsn)
epsilon = xg[2]-xg[1]
@show dmax = max_dist/epsilon
@show dmin = min_dist/epsilon

# Wada grid
W = detect_wada_grid_method(integ_df, bsn; max_iter=8)
@show W[:,end]

# Uncertainty exponent for these parameter and grid
bd = box_counting_dim(xg, yg, bsn)
α = 2 - bd

D = uncertainty_exponent(bsn, integ_df)
@show 2-D

println("---------------")
println("---------------")
println("Basin Report: ")
println("---------------")
println("---------------")

@printf("Basin entropy %.2f \n", Sb)
@printf("Boundary Basin Entropy: %.2f\n", Sbb)
@printf("Uncertainty exponent: α= %.2f\n", α )
@printf("Box counting dim: bd= %.2f\n", bd)
@printf("Uncertainty dim estimator: d = %.2f\n", 2-D[1])
@printf("Number of basins: %d\n", bsn.Na)
@printf("Merge Method: Max fattening parameter: %.2f\n", dmax)
@printf("Wada Grid Method: W_Na = %.2f\n ", W[end,end] )

plot(xg, yg, bsn.basin', seriestype=:heatmap)


"""
```julia
	latp_algorithm(G:SimpleWeightedGraph,s::Int)
```
	Perform the *Least-Action trip planning* algorithm on G, starting from s with alpha parameter
"""
latp_algorithm(G::SimpleWeightedGraph,s::Int,alpha::Int)

function latp_algorithm(G::SimpleWeightedGraph,s::Int,alpha::Int,T::DateTime)
	V = Set(vertices(G))
	Random.seed!(time_ns())
	Visited = [s] #It's a vector because we need to maintain visiting order
	Visited_time = [T]
	c = s

	next = 0
	while length(setdiff(V,Visited)) != 0
		#Firstly, get all unvisited nodes next to current node and order them in closeness
		neighbors_ordered = sort([ (G.weights[c,i],i) for i in setdiff(neighbors(G,c),Visited)])
		#Then the probability vector is calculated
		prob_vector = map( d->(1/d[1])^alpha, neighbors_ordered)
		prob_sum = sum( prob_vector )
		prob_vector = map( d -> d / prob_sum, prob_vector)
		#Find node: the probability of selecting a node i is the sum of the probability of all preceiding probs
		p = rand()
		for i in 1:length(neighbors_ordered)
			if p <= sum(prob_vector[1:i])
				next = neighbors_ordered[i][2]
				break
			end
		end
		#TODO: Add pause time
		push!(Visited,next)
		push!(Visited_time,Visited_time[length(Visited_time)] + Dates.Millisecond(10)) #10 Millisecond, just a test
		c = next
	end
	return [ (Visited[i],Visited_time[i]) for i in 1:length(Visited) ] #Combine visited nodes with timestamp
end


"""
Package: DataWrangler

outlie\\[!\\]([x], y, σ = 2; q)

Replace or detect outlier values in vector `x` describing a time series

# Parameters
- `x`: Optional vector containing the support for `y` (no missing value allowed)
- `y`: Vector with outliers to be dealt with
- `σ`: Number of sigmas used to identify outliers, it defaults to four.
- `q`: Number of closest vector values to the imputation to be considered, it defaults to 3*length(y)÷4).

# Description

When using `outlie!` the function replaces outliers with a `missing` value and return the index of the values replaced, if `outlie` is used isntead only the outliers index poistion are returned.

Outliers are indentified by measureing its distance in number of sigmas estimated from a loess fitting with window `q`. When datasets with noise close to zero all outliers will still be detected but non-outliers might be indentified as such, a way to prevent non-outliers to be indentified in this situtaion is by introducing a small amount of noise ϵ in `y`, e.g. `outlie!(x,y+ϵ)`.

# Examples
```julia
n = 1000
x = sort(rand(n))*2*pi;
y = Array{Union{Missing,Float64}}(undef,n);
y[:] = sin.(x).+randn(n)/10
mid = vcat(100,300,600,950);
y[mid] .= x[mid] .+ 2*(randn(length(mid)).+1)
y[500] = x[500] - 2*(randn(1)[1]+1)

outlie(x,y)

julia> outlie(x,y)
5-element Vector{Int64}:
 100
 300
 500
 600
 950
```
"""
function outlie(y::AbstractVector{<:Union{Missing,T}}, nσ::Real=4.0;
                q = 3*length(y)÷4) where  {R<:Real,T<:Real}
    cy = copy(y)
    outlie!(1:length(y),cy,nσ;q)
end

function outlie(x::AbstractVector{R}, y::AbstractVector{<:Union{Missing,T}}, nσ::Real=4.0;
                 q = 3*length(y)÷4) where  {R<:Real,T<:Real}
    cy = copy(y)
    outlie!(x,cy,nσ;q)
end

function outlie!(y::AbstractVector{<:Union{Missing,T}}, nσ::Real=4.0;
                q = 3*length(y)÷4) where  {R<:Real,T<:Real}
    outlie!(1:length(y),y,nσ;q)
end

function outlie!(x::AbstractVector{R}, y::AbstractVector{<:Union{Missing,T}}, nσ::Real=4.0;
                 q = 3*length(y)÷4) where  {R<:Real,T<:Real}
    allout = Int64[]
    eo = Int64[0]
    while !isempty(eo)
        eo = outlie1!(x,y,nσ;q)
        push!(allout,eo...)
    end
    sort(allout)
end

function outlie1!(x::AbstractVector{R}, y::AbstractVector{<:Union{Missing,T}}, nσ::Real=4.0;
                  q = 3*length(y)÷4) where  {R<:Real,T<:Real}

    @assert nσ > 1 "`nσ` should be larger than one"
    @assert !any(ismissing, x) "No missing values allowed in `x`"
    @assert issorted(x) "`x` values must be sorted"
    @assert length(x) == length(y) "`x` and `y` must have the same length"

    loq = loess(x,y; q)
    loqy = loq(x)

    dy = y-loqy
    dy = dy .- median(skipmissing(dy))
    sdy = collect(skipmissing(sort(dy)))
    lsdy = length(sdy)
    sdy = sdy[lsdy÷10:end-lsdy÷10]  # robust sd
    dσ = std(skipmissing(sdy))*T(sqrt(2))

    outlist = findall(v-> abs(v) > nσ*dσ, skipmissing(dy))

    y[outlist] .= missing
    outlist

end


using MultipleViewGeometry, Base.Test
using BenchmarkTools
using StaticArrays

ℳ = [Point2DH(x,y,1)  for x=-1000:0.5:1000 for y=-1000:0.5:1000]
𝐓 = hartley_transformation(ℳ)
@time hartley_transformation(ℳ)
@time hartley_transformation(ℳ)

@time hartley_normalization(ℳ)
@time hartley_normalization(ℳ)

𝒪, 𝐓 = hartley_normalization(ℳ)

ℳ[1]
𝒪[1]

@time transform(HomogeneousCoordinates(),CanonicalToHartley(),ℳ)


using AssemblyCalculus
using AssemblyCalculus: overlap, freeze_synapses!, synapse_noise!
using Graphs
using Plots
using StatsBase
using LinearAlgebra: norm

K = 20
N = K^2
MEAN_DEGREE = K
P = MEAN_DEGREE / N
BETA = 0.0
TIMESTEPS = 1000

g = erdos_renyi(N, P, is_directed=true)
brain = Brain(g, [N], [K], [BETA])
synapse_noise!(brain)
freeze_synapses!(brain)

stim = rand_stim(brain[1])
zstim = zero_stim(brain[1])

_, zsp = simulate!([zstim], TIMESTEPS)
_, sp = simulate!([stim], TIMESTEPS)

zspikes = zsp[1]
spikes = sp[1]

function visit_prob(neurons::Array{Int, N}, max_idx::Int) where N
    neuron_counts = counts(reshape(neurons, :), max_idx)
    neruon_prob = neuron_counts ./ sum(neuron_counts)
    return neruon_prob
end

function prob_conv(final_prob::Array{T, 1}, probs::Array{T, 2}) where T
    return reshape(mapslices(x -> maximum(abs.(x - final_prob)), probs, dims=1), :)
end

function ktop_compare(x::Array{T, 1}, y::Array{T, 1}) where T
    xperm = sortperm(x)
    yperm = sortperm(y)
    compare = [length(intersect(xperm[1:i], yperm[1:i])) / i for i in 1:length(x)]
    return compare
end

final_z_prob = visit_prob(zspikes, N)
final_stim_prob = visit_prob(spikes, N)

z_prob = mapreduce(i -> visit_prob(zspikes[:, 1:i], N), hcat, 1:TIMESTEPS)
stim_prob = mapreduce(i -> visit_prob(spikes[:, 1:i], N), hcat, 1:TIMESTEPS)

rand_prob = rand(N)
rand_prob ./= sum(rand_prob)
baseline_conv = prob_conv(rand_prob, z_prob)

z_conv = prob_conv(final_z_prob, z_prob)
stim_conv = prob_conv(final_stim_prob, stim_prob)

adj = convert(Array{Float64, 2}, adjacency_matrix(g))
row_prob = reshape(sum(adj', dims=2) ./ sum(adj), :)

row_current_prob = reshape(sum(adj', dims=2), :) .+ stim.currents
row_current_prob ./= sum(row_current_prob)

row_conv = prob_conv(row_prob, z_prob)
row_current_conv = prob_conv(row_current_prob, stim_prob)
row_stim_conv = prob_conv(row_prob, stim_prob)

eigen_prob = eigenvector_centrality(DiGraph(adj'))
eigen_prob ./= sum(eigen_prob)
eigen_conv = prob_conv(eigen_prob, z_prob)

p1 = plot()
line_attr = Dict(:alpha=>0.7, :linewidth=>3)

plot!(p1, zeros(TIMESTEPS), linestyle=:dash, color=:gray, label=nothing)

plot!(p1, baseline_conv; color=:gray, label="Baseline", line_attr...)

plot!(p1, z_conv; label="Zero stim to true distr", line_attr...)
plot!(p1, row_conv; label="Zero stim to row sums", line_attr...)
plot!(p1, eigen_conv; label="Zero stim to eigen", line_attr...)

plot!(p1, stim_conv; label="Stim to true distr", line_attr...)
plot!(p1, row_current_conv; label="Stim to row sums + current", line_attr...)
plot!(p1, row_stim_conv; label="Stim to row sums", line_attr...)

p2 = bar(final_z_prob, alpha=0.5, label="True")
bar!(p2, eigen_prob, alpha=0.5, label="Row Sum")

p3 = plot(legend=:bottomright)
plot!(p3, ktop_compare(eigen_prob, final_z_prob); label="Eigencentrality", line_attr...)
plot!(p3, ktop_compare(row_prob, final_z_prob); label="In-Degree", line_attr...)
plot!(p3, ktop_compare(rand(N), final_z_prob); label="Random", color=:gray, line_attr...)
ylabel!(p3, "Accuracy")
xlabel!(p3, "K Largest")
title!(p3, "Accuracy of Centrality Methods at Predicting the K Most Visited Neurons")
plot(p2, p1, p3; size=(1200, 400), layout=@layout [a; b c])


export gali
using OrdinaryDiffEq, LinearAlgebra
#####################################################################################
#                               Continuous GALI                                     #
#####################################################################################
"""
    gali(ds::DynamicalSystem, tmax, k::Int | Q0; kwargs...) -> GALI_k, t
Compute ``\\text{GALI}_k`` [1] for a given `k` up to time `tmax`.
Return ``\\text{GALI}_k(t)`` and time vector ``t``.

The third argument, which sets the order of `gali`, can be an integer `k`, or
a matrix with its columns being the deviation vectors (then
`k = size(Q0)[2]`). In the first case random orthonormal vectors are chosen.

## Keyword Arguments
* `threshold = 1e-12` : If `GALI_k` falls below the `threshold`
  iteration is terminated.
* `dt = 1` : Time-step between deviation vector normalizations. For continuous
  systems this is approximate.
* `u0` : Initial state for the system. Defaults to `get_state(ds)`.
* `diffeq...` : Keyword arguments propagated into `init` of DifferentialEquations.jl.
  See [`trajectory`](@ref) for examples. Only valid for continuous systems.

## Description
The Generalized Alignment Index,
``\\text{GALI}_k``, is an efficient (and very fast) indicator of chaotic or regular
behavior type in ``D``-dimensional Hamiltonian systems
(``D`` is number of variables). The *asymptotic* behavior of
``\\text{GALI}_k(t)`` depends critically on
the type of orbit resulting
from the initial condition. If it is a chaotic orbit, then
```math
\\text{GALI}_k(t) \\sim
\\exp\\left[\\sum_{j=1}^k (\\lambda_1 - \\lambda_j)t \\right]
```
with ``\\lambda_j`` being the `j`-th Lyapunov exponent
(see [`lyapunov`](@ref), [`lyapunovs`](@ref)).
If on the other hand the orbit is regular, corresponding
to movement in ``d``-dimensional torus with `` 1 \\le d \\le D/2``
then it holds
```math
\\text{GALI}_k(t) \\sim
    \\begin{cases}
      \\text{const.}, & \\text{if} \\;\\; 2 \\le k \\le d  \\; \\; \\text{and}
      \\; \\;d > 1 \\\\
      t^{-(k - d)}, & \\text{if} \\;\\;  d < k \\le D - d \\\\
      t^{-(2k - D)}, & \\text{if} \\;\\;  D - d < k \\le D
    \\end{cases}
```

Traditionally, if ``\\text{GALI}_k(t)`` does not become less than
the `threshold` until `tmax`
the given orbit is said to be chaotic, otherwise it is regular.

Our implementation is not based on the original paper, but rather in
the method described in [2], which uses the product of the singular values of ``A``,
a matrix that has as *columns* the deviation vectors.

## Performance Notes
This function uses a [`tangent_integrator`](@ref).
For loops over initial conditions and/or
parameter values one should use the low level method that accepts
an integrator, and `reinit!` it to new initial conditions.
See the "advanced documentation" for info on the integrator object.
The low level method is
```
ChaosTools.gali(tinteg, tmax, dt, threshold)
```

## References

[1] : Skokos, C. H. *et al.*, Physica D **231**, pp 30–54 (2007)

[2] : Skokos, C. H. *et al.*, *Chaos Detection and Predictability* - Chapter 5
(section 5.3.1 and ref. [85] therein), Lecture Notes in Physics **915**,
Springer (2016)
"""
gali(ds::DS, tmax::Real, k::Int; kwargs...) =
    gali(ds, tmax, orthonormal(dimension(ds), k); kwargs...)

function gali(ds::DS{IIP, S, D}, tmax::Real, Q0::AbstractMatrix;
    threshold = 1e-12, dt = 1, u0 = get_state(ds),
    diffeq...) where {IIP, S, D}

    size(Q0)[1] != D && throw(ArgumentError(
    "Deviation vectors must have first dimension equal to the dimension of the "*
    "system, $D."))
    2 ≤ size(Q0)[2] ≤ D || throw(ArgumentError(
    "The order of GALI_k must be 2 ≤ k ≤ $D."))

    # Create tangent integrator:
    if typeof(ds) <: DDS
        tinteg = tangent_integrator(ds, Q0; u0 = u0)
    else
        tinteg = tangent_integrator(ds, Q0; u0 = u0, diffeq...)
    end

    ST = stateeltype(ds)
    TT = typeof(ds.t0)
    gal::Vector{ST}, tvec::Vector{TT} = gali(tinteg, tmax, dt, threshold)
    return gal, tvec
end

function gali(tinteg, tmax, dt, threshold)

    rett = [tinteg.t]
    gali_k = [one(eltype(tinteg.u))]
    t0 = tinteg.t

    while tinteg.t < tmax + t0
        step!(tinteg, dt)
        # Normalize deviation vectors
        normalize_deviations!(tinteg)
        # Calculate singular values:
        zs = LinearAlgebra.svd(get_deviations(tinteg)).S
        push!(gali_k, prod(zs))
        push!(rett, tinteg.t)

        if gali_k[end] < threshold
            break
        end
    end
    return gali_k, rett
end

#####################################################################################
#                            Normalize Deviation Vectors                            #
#####################################################################################
# Metaprogramming ontributed by @saschatimme
function normalize_impl(::Type{SMatrix{D, K, T, DK}}) where {D, K, T, DK}
    exprs = []
    for j = 1:K
        c_j = Symbol("c", j)
        push!(exprs, :($c_j = normalize(A[:, $j])))
    end

    ops = Expr[]
    for j=1:K, i=1:D
        c_j = Symbol("c", j)
        push!(ops, :($c_j[$i]))
    end

    Expr(:block,
        exprs...,
        Expr(:call, SMatrix{D, K, T, D*K}, ops...)
        )
end
@generated function normalize_devs(A::SMatrix)
    normalize_impl(A)
end

# I AM SURE THE FOLLOWING CAN BE SHORTENED USING UNIONS!!!

# OOP version (either cont or disc)
function normalize_deviations!(
    tinteg::ODEIntegrator{Alg, S}) where {Alg, S<:SMatrix}
    norms = normalize_devs(get_deviations(tinteg))
    set_deviations!(tinteg, norms)
    return
end
function normalize_deviations!(
    tinteg::TDI{false})
    norms = normalize_devs(get_deviations(tinteg))
    set_deviations!(tinteg, norms)
    return
end

# IIP
function normalize_inplace!(A)
    for i in 1:size(A)[2]
        LinearAlgebra.normalize!(view(A, :, i))
    end
end
function normalize_deviations!(tinteg::Union{
        ODEIntegrator{Alg, S},
        MDI{Alg, S}}) where {Alg, S<:Matrix}
    A = get_deviations(tinteg)
    normalize_inplace!(A)
    # no reason to call set_deviations, because
    # get_deviations always returns views for IIP
    u_modified!(tinteg, true)
    return
end
function normalize_deviations!(tinteg::TDI{true})
    A = get_deviations(tinteg)
    normalize_inplace!(A)
    return
end


immutable Baseline{T} <: Model
    β₀::T
end

Base.show(io::IO, ::Type{Baseline}) = print(io, "Heuristic Baseline")

function Base.show(io::IO, m::Baseline)
    @printf(
        io,
        "Heuristic Baseline (β₀ = %.3f)",
        m.β₀,
    )
end

initial_parameters(::Type{Baseline}) = Float64[0.0]

canonical_parameters(m::Baseline) = (m.β₀, )

function Baseline(parameters::Vector)
    β₀ = parameters[1]
    return Baseline(β₀)
end

@inline function predict(m::Baseline, x2::Real, t2::Real, x1::Real, t1::Real)
    β₀ = m.β₀
    z = β₀
    p = invlogit(z)
    return p
end

@inline function gradient_component(
    m::Baseline,
    x2::Real,
    t2::Real,
    x1::Real,
    t1::Real,
)
    gr = (1.0, )
    return gr
end


# developed with Julia 1.1.1
#
# Stochastic Dynamic Programming (SDP) controller
# for EMS simulation
#
# SDP is computed with a package available at
# https://github.com/adrien-le-franc/StoOpt.jl

using EMSx
using StoOpt

using JLD2

include("../arguments.jl")
include("function.jl")
include("calibrate.jl")

args = parse_commandline()

mutable struct Sdp <: EMSx.AbstractController
   model::StoOpt.SDP
   value_functions::Union{StoOpt.ArrayValueFunctions, Nothing}
   Sdp() = new()
end

## constant values and functions for both simulation and calibration

const controller = Sdp()
const dx = 0.1
const du = 0.1
const horizon = 672

function EMSx.initialize_site_controller(controller::Sdp, site::EMSx.Site, prices::EMSx.Prices)

    controller = Sdp()

    offline_law_data_frames = net_demand_offline_law(site.path_to_train_data_csv)
    noises = data_frames_to_noises(offline_law_data_frames)

    function offline_dynamics(t::Int64, state::Array{Float64,1}, control::Array{Float64,1},
    noise::Array{Float64,1})
        scale_factor = site.battery.power*0.25/site.battery.capacity
        soc = state + (site.battery.charge_efficiency*max.(0.,control) -
            max.(0.,-control)/site.battery.discharge_efficiency)*scale_factor
        return soc
    end

    function offline_cost(t::Int64, state::Array{Float64,1}, control::Array{Float64,1},
    noise::Array{Float64,1})
        control = control[1]*site.battery.power*0.25
        imported_energy = control + noise[1]
        return (prices.buy[t]*max(0.,imported_energy) - prices.sell[t]*max(0.,-imported_energy))
    end

    model = StoOpt.SDP(Grid(0:dx:1, enumerate=true),
        Grid(-1:du:1),
        noises,
        offline_cost,
        offline_dynamics,
        horizon)

    controller.model = model

   return controller

end

## calibration specific function

function compute_value_functions(controller::Sdp)
    return StoOpt.compute_value_functions(controller.model)
end

## simulation specific functions

function load_value_functions(site_id::String)
    return load(joinpath(args["save"],
                "sdp",
                "value_functions",
                site_id*".jld2"))["value_function"]
end

function EMSx.compute_control(controller::Sdp, information::EMSx.Information)

    if information.t == 1
        controller.value_functions = load_value_functions(information.site_id)
    end

    control = compute_control(controller.model, information.t, [information.soc],
        StoOpt.RandomVariable(controller.model.noises, information.t), controller.value_functions)

    return control[1]

end

if args["calibrate"]

    calibrate_sites(controller,
    joinpath(args["save"], "sdp"),
    args["price"],
    args["metadata"],
    args["train"])

end

if args["simulate"]

EMSx.simulate_sites(controller,
    joinpath(args["save"], "sdp"),
    args["price"],
    args["metadata"],
    args["test"],
    args["train"])

end


"""
    obstacles
"""

struct ObstacleConstraints <: Constraints
    n
    ineq
    n_stage
end

function constraints!(c, Z, con::ObstacleConstraints, model, idx, h, T)
    for t = 1:T
        x = view(Z, idx.x[t])
        obstacles!(view(c, (t - 1) * con.n_stage .+ (1:con.n_stage)), x)
    end
    nothing
end

function constraints_jacobian!(∇c, Z, con::ObstacleConstraints, model, idx, h, T)
    n = model.n

    c_tmp = zeros(con.n_stage)
    shift = 0

    for t = 1:T
        r_idx = (t - 1) * con.n_stage .+ (1:con.n_stage)
        c_idx = idx.x[t]

        len = length(r_idx) * length(c_idx)
        x = view(Z, idx.x[t])

        ForwardDiff.jacobian!(reshape(view(∇c, shift .+ (1:len)),
            con.n_stage, n),
            obstacles!, c_tmp, x)

        shift += len
    end

    return nothing
end

function constraints_sparsity(con::ObstacleConstraints, model, idx, T;
    shift_row = 0, shift_col = 0)
    n = model.n

    row = []
    col = []

    for t = 1:T
        r_idx = shift_row + (t - 1) * con.n_stage .+ (1:con.n_stage)
        c_idx = shift_col .+ idx.x[t]

        row_col!(row, col, r_idx, c_idx)
    end

    return collect(zip(row, col))
end

function circle_obs(x, y, xc, yc, r)
    (x - xc)^2.0 + (y - yc)^2.0 - r^2.0
end


"""
Holds data for hierarchical logistic model test example.
"""
struct BinomLLData{Ty, T}
    y::Ty
    X::T
    Z::T
end
"""
Outer constructor for subsetting by random effects group
"""
function BinomLLData(bld::BinomLLData, grp::Integer)
    idx = Z[:, grp] .== 1
    BinomLLData(bld.y[idx], bld.X[idx, :], bld.Z[idx, grp:grp])
end
"""
Functor that holds parameter values and data for logistic hierarchical
regression model.
"""
struct BinomLL{Tβ, Tu, Tσgrp, Tbld<:BinomLLData} <: Function
    β::Tβ
    u::Tu
    σgrp::Tσgrp
    bld::Tbld
end
"""
Convenient outer constructor to make swapping random effects vectors easier.
"""
function BinomLL(BLL::BinomLL, u)
    BinomLL(BLL.β, u, BLL.σgrp, BLL.bld)
end
"""
Hierarchical logistic regression negative log-likelihood.
"""
function (BLL::BinomLL)()
    lo = BLL.bld.X * BLL.β + BLL.bld.Z * BLL.u
    p = logistic.(lo)
    datalik = -sum(logpdf.(Bernoulli.(p), BLL.bld.y))
    randlik = -sum(logpdf.(Normal(0.0, BLL.σgrp), BLL.u))
    datalik + randlik
end
function (BLL::BinomLL)(u)
    BinomLL(BLL, u)()
end