## Aircraft analysis benchmarking
using BenchmarkTools
using TimerOutputs

# All subsequent analyses use no symmetry tricks for performance as AeroMDAO hasn't implemented them and apples must be compared to apples.

## AeroMDAO tests: https://github.com/GodotMisogi/AeroMDAO.jl
#=======================================================#
using AeroMDAO

println("AeroMDAO Aircraft -")
function aeromdao_steady_vlm()
    # Wing
    wing = Wing(foils     = fill(naca4((0,0,1,2)), 2),
                chords    = [1.0, 0.6],
                twists    = [2.0, 2.0],
                spans     = [10.0],
                dihedrals = [11.31],
                sweeps    = [2.29]);

    # Horizontal tail
    htail = Wing(foils     = fill(naca4((0,0,1,2)), 2),
                 chords    = [0.7, 0.42],
                 twists    = [0.0, 0.0],
                 spans     = [2.5],
                 dihedrals = [0.],
                 sweeps    = [6.39],
                 position  = [4., 0, 0],
                 angle     = 0.,
                 axis      = [0., 1., 0.])

    # Vertical tail
    vtail = HalfWing(foils     = fill(naca4((0,0,0,9)), 2),
                     chords    = [0.7, 0.42],
                     twists    = [0.0, 0.0],
                     spans     = [1.0],
                     dihedrals = [0.],
                     sweeps    = [7.97],
                     position  = [4., 0, 0],
                     angle     = 90.,
                     axis      = [1., 0., 0.])

    wing_panels, wing_normals   = panel_wing(wing, 20, 10,
                                             spacing = Cosine()
                                            )

    htail_panels, htail_normals = panel_wing(htail, 12, 6;
                                             spacing  = Cosine()
                                            )

    vtail_panels, vtail_normals = panel_wing(vtail, 10, 5;
                                             spacing  = Cosine()
                                            )

    # Aircraft assembly
    aircraft = [
                Horseshoe.(wing_panels,  wing_normals)[:];
                Horseshoe.(htail_panels, htail_normals)[:];
                Horseshoe.(vtail_panels, vtail_normals)[:];
               ]

    α, β = 5.0, 0.0
    Ω    = [0.0, 0.0, 0.0]
    fs   = AeroMDAO.Freestream(α, β, Ω)
    refs = References(
                         speed     = 10.0,
                         density   = 1.225,
                         viscosity = 1.5e-5,
                         area      = 9.0,
                         span      = 10.0,
                         chord     = 0.9,
                         location  = [0.5, 0., 0.]
                     )

    # reset_timer!()
    # @timeit "Solving Case"
    data = solve_case(aircraft, fs, refs;)

    # @timeit "Computing Forces" Fs = surface_forces(data)
    CFs, CMs = surface_coefficients(data)
    # FFs      = farfield_coefficients(data)

    sum(CFs), sum(CMs)
    # nearfield(data), farfield(data)
    # print_timer()
end

t1 = @benchmark aeromdao_steady_vlm()

##
println("AeroMDAO Aircraft ComponentArrays.jl -")
function aeromdao_steady_vlm_components()
    # Wing
    wing = Wing(foils     = fill(naca4((0,0,1,2)), 2),
                chords    = [1.0, 0.6],
                twists    = [2.0, 2.0],
                spans     = [10.0],
                dihedrals = [11.31],
                sweeps    = [2.29]);

    # Horizontal tail
    htail = Wing(foils     = fill(naca4((0,0,1,2)), 2),
                 chords    = [0.7, 0.42],
                 twists    = [0.0, 0.0],
                 spans     = [2.5],
                 dihedrals = [0.],
                 sweeps    = [6.39],
                 position  = [4., 0, 0],
                 angle     = 0.,
                 axis      = [0., 1., 0.])

    # Vertical tail
    vtail = HalfWing(foils     = fill(naca4((0,0,0,9)), 2),
                     chords    = [0.7, 0.42],
                     twists    = [0.0, 0.0],
                     spans     = [1.0],
                     dihedrals = [0.],
                     sweeps    = [7.97],
                     position  = [4., 0, 0],
                     angle     = 90.,
                     axis      = [1., 0., 0.])

    wing_mesh = WingMesh(wing, [20], 10,
                         span_spacing = Cosine()
                        )

    htail_mesh = WingMesh(htail, [12], 6;
                          span_spacing  = Cosine()
                         )

    vtail_mesh = WingMesh(vtail, [10], 5;
                          span_spacing  = Cosine()
                         )

    # Aircraft assembly
    aircraft = ComponentArray(
                    wing  = make_horseshoes(wing_mesh),
                    htail = make_horseshoes(htail_mesh),
                    vtail = make_horseshoes(vtail_mesh),
                )

    α, β = 5.0, 0.0
    Ω    = [0.0, 0.0, 0.0]
    fs   = AeroMDAO.Freestream(α, β, Ω)
    refs = References(
                         speed     = 10.0,
                         density   = 1.225,
                         viscosity = 1.5e-5,
                         area      = 9.0,
                         span      = 10.0,
                         chord     = 0.9,
                         location  = [0.5, 0., 0.]
                     )

    # reset_timer!()
    # @timeit "Solving Case"
    data = solve_case(aircraft, fs, refs;)

    # @timeit "Computing Forces" Fs = surface_forces(data)
    CFs, CMs = surface_coefficients(data)
    # FFs      = farfield_coefficients(data)

    sum(CFs), sum(CMs)
    # nearfield(data), farfield(data)
    # print_timer()
end

t2 = @benchmark aeromdao_steady_vlm_components()

## BYU FLOW Lab tests: https://github.com/byuflowlab/VortexLattice.jl
#=======================================================#

# Example taken from: https://flow.byu.edu/VortexLattice.jl/dev/examples/

# Pulled these out to avoid compilation for proper benchmarking
fc   = fill((xc) -> 0, 2) # camberline function for each section
fc_h = fill((xc) -> 0, 2) # camberline function for each section
fc_v = fill((xc) -> 0, 2) # camberline function for each section

##
using VortexLattice

println("BYU FLOW Lab VortexLattice.jl - ")
function byu_steady_vlm()
    # wing
    xle = [0.0, 0.2]
    yle = [0.0, 5.0]
    zle = [0.0, 1.0]
    chord = [1.0, 0.6]
    theta = [2.0*pi/180, 2.0*pi/180]
    phi = [0.0, 0.0]
    ns = 20
    nc = 10
    spacing_s = VortexLattice.Cosine()
    spacing_c = VortexLattice.Cosine()
    mirror = true

    # horizontal stabilizer
    xle_h = [0.0, 0.14]
    yle_h = [0.0, 1.25]
    zle_h = [0.0, 0.0]
    chord_h = [0.7, 0.42]
    theta_h = [0.0, 0.0]
    phi_h = [0.0, 0.0]
    ns_h = 12
    nc_h = 6
    spacing_s_h = VortexLattice.Cosine()
    spacing_c_h = VortexLattice.Cosine()
    mirror_h = true

    # vertical stabilizer
    xle_v = [0.0, 0.14]
    yle_v = [0.0, 0.0]
    zle_v = [0.0, 1.0]
    chord_v = [0.7, 0.42]
    theta_v = [0.0, 0.0]
    phi_v = [0.0, 0.0]
    ns_v = 10
    nc_v = 5
    spacing_s_v = VortexLattice.Cosine()
    spacing_c_v = VortexLattice.Cosine()
    mirror_v = false

    Sref = 9.0
    cref = 0.9
    bref = 10.0
    rref = [0.5, 0.0, 0.0]
    Vinf = 1.0
    ref = Reference(Sref, cref, bref, rref, Vinf)

    alph = 5.0*pi/180
    beta = 0.0
    Omega = [0.0; 0.0; 0.0]
    fs = VortexLattice.Freestream(Vinf, alph, beta, Omega)

    symmetric = [false, false, false]

    # generate surface panels for wing
    wgrid, wing = wing_to_surface_panels(xle, yle, zle, chord, theta, phi, ns, nc;
        mirror=mirror, fc = fc, spacing_s=spacing_s, spacing_c=spacing_c)

    # generate surface panels for horizontal tail
    hgrid, htail = wing_to_surface_panels(xle_h, yle_h, zle_h, chord_h, theta_h, phi_h, ns_h, nc_h;
        mirror=mirror_h, fc=fc_h, spacing_s=spacing_s_h, spacing_c=spacing_c_h)
    VortexLattice.translate!(hgrid, [4.0, 0.0, 0.0]) # Plots also exports translate! -_-
    VortexLattice.translate!(htail, [4.0, 0.0, 0.0]) # Plots also exports translate! -_-

    # generate surface panels for vertical tail
    vgrid, vtail = wing_to_surface_panels(xle_v, yle_v, zle_v, chord_v, theta_v, phi_v, ns_v, nc_v;
        mirror=mirror_v, fc=fc_v, spacing_s=spacing_s_v, spacing_c=spacing_c_v)
    VortexLattice.translate!(vgrid, [4.0, 0.0, 0.0]) # Plots also exports translate! -_-
    VortexLattice.translate!(vtail, [4.0, 0.0, 0.0]) # Plots also exports translate! -_-

    # display(size.([ wing, htail, vtail ])) # Checking sizes

    grids = [wgrid, hgrid, vgrid]
    surfs = [wing, htail, vtail]
    surface_id = [1, 2, 3]

    system = steady_analysis(surfs, ref, fs; symmetric=symmetric, surface_id=surface_id)

    CF, CM = body_forces(system; frame=VortexLattice.Wind())

    # CDiff = far_field_drag(system)

    # dCF_b, dCM_b = body_derivatives(system)
    # dCF_s, dCM_s = stability_derivatives(system)

    # CF, CM, CDiff #, dCF_b, dCM_b, dCF_s, dCM_s
end

t2 = @benchmark byu_steady_vlm()


function HszDk5dwPs(y,x,z,yhatQ=false,m=0,ScaleByNtQ=0,vvzx=[],wM=[])
#HszDk5dwPs LS and Driscoll-Kray standard errors for unbalanced panel, assuming
#           x are the same across individuals, while z are time-varying individual
#           characteristics. The effective regressors are kron(z,x).
#
#
#
#
#  Usage: fnOutput = HszDk5dwPs(y,x,z[,yhatQ[,m[,ScaleByNtQ[,vvzx[,wM]]]]])
#
#  Input:    y             TxN matrix with the dependent variable, y(t,i) is for period t, individual i
#            x             TxK matrix with K factors that are common for all investors, should
#                          at least contain Tx1 matrix ones(T)
#            z             TxNxL matrix with L (time-varying) individual characteristics, should
#                          at least contain TxNx1 array of ones, ones(T,N,1)
#            yhatQ         (optional) scalar, 1: generate and report fitted values
#            m             (optional), scalar, number of lags in covariance estimation [0]
#            ScaleByNtQ    (optional), scalar, 1: scales all moment conditions by N(t), not by N;
#                          2: scale all moment conditions by wM; [0]
#                          can be used to replicate a portfolio (Calendar time) approach
#            vvzx          (optional), vector of indices of colums in HDirProdPs(z_t,x0_t) to keep
#            wM            (optional) TxN matrix, weights on observation (t,i)
#
#  Output:   fnOutput      named tuple with:
#              theta         (K*L)x1 vector, LS estimates of regression coeefficients on kron(z,x)
#              stdDK         (K*L)x1 vector, Driscoll-Kraay standard errors
#              stdW          (K*L)x1 vector, White's standard errors
#              CovDK         (K*L)x(K*L) matrix, Driscoll-Kraay covariance matrix
#              yhat          TxN matrix with fitted values
#              R2a           scalar, (pseudo-) R2
#              CovW          covariance matrix, White's
#              CovDKj        covariance matrix, DK with lags
#              stdDKj        standard errors, DK with lags
#
#
#  Notice:   (a) the effective regressors are kron(z,x). For instance, with z = [1,z1] and
#                x = [1,x1,x2,x3], we have [1,x1,x2,x3,z1,z1*x1,z1*x2,z1*x3].
#
#
#
#  Uses:     FindNoNaNPs and HDirProdPs
#
#
#
#
#  Paul.Soderlind@unisg.ch   May 2010, to Julia Oct 2015
#------------------------------------------------------------------------------

  if isempty(vvzx)
    vvzx = 1:(size(x,2)*size(z,3))
  end

  (T,N) = (size(y,1),size(y,2))
  K     = size(x,2)
  L     = size(z,3)
  KL    = sum(vvzx .>0)

  msg = " "
  xx  = zeros(KL,KL)                   #Sum[x(t)*x(t)',t=1:T]
  xy  = zeros(KL)                      #Sum[x(t)*y(t),t=1:T]

  Nb  = zeros(Int,T)                   #effective number of obs, after pruning NaNs
  for t = 1:T                             #loop over time
    x_t  = HDirProdPs(reshape(z[t,:,:],N,L),x[t:t,:])  #effective regressors, z_t is NxL, x_t is 1xK
    x_t  = x_t[:,vvzx]
    vv   = FindNoNaNPs(1,y[t,:],x_t)
    N_t  = sum(vv)
    if ScaleByNtQ == 1
      Nb[t] = N_t
      w_t   = ones(N_t)
    elseif ScaleByNtQ == 2
      Nb[t] = N
      w_t   = wM[t,vv]
    else
      Nb[t] = N
      w_t   = ones(N_t)
    end
    if N_t > 0                           #don't accumulate [] to xx and xy (generates [])
      (y_t,x_t) = (y[t,vv],x_t[vv,:])    #pruning NaNs
      y_t = y_t .* w_t                   #put weights on observation i (in t), y_t .* w_t
      lmul!(Diagonal(w_t),x_t)           #x_t = w_t .* x_t
      xx .= xx + x_t'x_t/Nb[t]
      xy .= xy + x_t'y_t/Nb[t]
    end
    msg = IterationPrintPs(t,T,msg,100)
  end

  Tb    = sum(Nb .> 0)                    #number of effective time periods
  xx    = xx/Tb
  xy    = xy/Tb
  theta = xx\xy                           #ols estimates, solves xx*theta-xy=0 in MSE sense

  if yhatQ
    yhat = fill(NaN,(T,N))
  else
    yhat = Float64[]
  end
  omega0DK = zeros(KL,KL)               #DK, lag 0
  omega0W  = zeros(KL,KL)               #White's
  omegajDK = zeros(KL,KL,m)             #DK, lags 1 to m
  h_tLag   = zeros(m,KL)                #lag1;lag2;...,lagm
  for t = 1:T                           #loop over time
    x_t    = HDirProdPs(reshape(z[t,:,:],N,L),x[t:t,:])
    x_t    = x_t[:,vvzx]
    yhat_t = x_t*theta
    r_t    = y[t,:] - yhat_t
    vv     = FindNoNaNPs(1,r_t,x_t)
    N_t    = sum(vv)
    if ScaleByNtQ == 1
      w_t = ones(N_t)
    elseif ScaleByNtQ == 2
      w_t = wM[t,vv]
    else
      w_t = ones(N_t)
    end
    if N_t > 0                       #don't accumulate [] to omega0DK
      (r_t,x_t) = (r_t[vv],x_t[vv,:])
      lmul!(Diagonal(r_t .* w_t.^2),x_t)  #x_t is henceforth the moment condition
      h_t       = sum(x_t,dims=1)/Nb[t]     #for (i,t), x_t .* (r_t .* w_t.^2)
      omega0DK .= omega0DK + h_t'h_t
      omega0W  .= omega0W  + x_t'x_t/Nb[t]^2
      for j = 1:m
        omegajDK[:,:,j] .= omegajDK[:,:,j] + h_t'h_tLag[j:j,:]    #h(t)*h(t-j)'
      end
      h_tLag = [h_t;h_tLag[1:end-1,:]]  #update only if !isempty(rx_t), effectively disregarding t if no data
    end
    if yhatQ
      yhat[t,:] = yhat_t
    end
    msg = IterationPrintPs(t,T,msg,100)
  end

  Shatj = copy(omega0DK)
  for j = 1:m
    Shatj .= Shatj + (1-j/(m+1))*(omegajDK[:,:,j]+omegajDK[:,:,j]')
  end

  zx_1   = inv(xx)
  CovDK  = zx_1 * (omega0DK/Tb^2) * zx_1'           #covariance matrix, DK
  CovW   = zx_1 * (omega0W/Tb^2)  * zx_1'           #covariance matrix, White's
  CovDKj = zx_1 * (Shatj/Tb^2)    * zx_1'           #covariance matrix, DK with lags
  stdDK  = sqrt.(diag(CovDK))                       #standard errors, DK
  stdW   = sqrt.(diag(CovW))                        #standard errors, White's
  stdDKj = sqrt.(diag(CovDKj))                      #standard errors, DK with lags

  if yhatQ
    vv  = FindNoNaNPs(1,vec(yhat),vec(y))
    R2a = cor(vec(yhat)[vv],vec(y)[vv])^2
  else
    R2a  = Float64[]
  end

  #fnOutput = [theta,stdDK,stdW,CovDK,yhat,R2a,CovW,CovDKj,stdDKj]
  fnOutput = (theta=theta,stdDK=stdDK,stdW=stdW,CovDK=CovDK,yhat=yhat,R2a=R2a,
              CovW=CovW,CovDKj=CovDKj,stdDKj=stdDKj)

  return fnOutput

end
#------------------------------------------------------------------------------


## All of these methods are defined in
## Huber, M., 2017. Optimal linear Bernoulli factories for small mean problems.
## Methodology and Computing in Applied Probability, 19(2), pp.631-645.

function _logistic(f::F, C::Float64, rng::RNG) where {F<:Function,
  RNG<:AbstractRNG}
  A::Float64 = randexp(rng)
  T::Float64 = randexp(rng)/C
  flips::Int64 = 0
  while T < A
    B::Bool = f()
    flips += 1
    if B
      return true, flips
    else
      T += randexp(rng)/C
    end
  end
  return false, flips
end

function _algorithmA(f::F, m::Int64, C::Float64, rng::RNG) where {F<:Function,
  RNG<:AbstractRNG}
  s::Int64 = 1
  flips::Int64 = 0
  while 0 < s < m
    B, newFlips = _logistic(f, C, rng)
    flips += newFlips
    s += 1-2*B
  end
  return s == 0, flips
end

function _highPowerLogistic(f::F, m::Int64, β::Float64, C::Float64,
  rng::RNG) where {F<:Function, RNG<:AbstractRNG}
  s::Int64 = 1
  flips::Int64 = 0
  α::Float64 = β*C
  while 0 < s <= m
    B, newFlips = _logistic(f, α, rng)
    flips += newFlips
    s += 2*B-1
  end
  return s == m+1, flips
end

function _algorithmB(f::F, ϵ::Float64, m::Int64, β::Float64, C::Float64,
  rng::RNG) where {F<:Function, RNG<:AbstractRNG}

  γ::Float64 = 1.0-(1.0-ϵ)*β
  α::Float64 = β*C
  flips::Int64 = 0
  while true
    B1, newFlips = _huber2017(f, α, γ, rng)
    flips += newFlips
    if !B1
      return false, flips
    end
    B2, newFlips = _highPowerLogistic(f, m-2, β, C, rng)
    flips += newFlips
    if B2
      return true, flips
    end
    m -= 1
  end
end

function _huber2017(f::F, C::Float64, ϵ::Float64, rng::RNG) where {F<:Function,
  RNG<:AbstractRNG}
  m::Int64 = ceil(Int64, 4.5/ϵ) + 1
  β::Float64 = 1.0 + 1.0/(m-1.0)
  flips::Int64 = 0
  B1, newFlips = _algorithmA(f, m, β*C, rng)
  flips += newFlips
  if !B1 return false, flips end
  if rand(rng) < 1.0/β
    return true, flips
  else
    B, newFlips = _algorithmB(f, ϵ, m, β, C, rng)
    flips += newFlips
    return B, flips
  end
end

function _huber2017Small(f::F, C::Float64, ϵ::Float64, rng::RNG) where {F<:Function,
  RNG<:AbstractRNG}
  @assert ϵ > 0.5
  M::Float64 = 1.0 - ϵ
  β::Float64 = 1.0/(1.0-2*M)
  Y, flips = _logistic(f, β*C, rng)
  if !Y return false, flips end
  B::Bool = rand(rng) < 1/β
  if B return true, flips end
  ϵ = 1.0 - β/(β-1.0)*M
  X, newFlips = _huber2017(f, C*β/(β-1.0), ϵ, rng)
  return X, flips + newFlips
end


x = Variable(4, 3)
y = Variable()

x + y == 3
3 == y
rand(4, 3) == x - 4*y
sum(y, 1) == rand(1, 3)

# lists of constraints
list = []
list += x + y == ones(4, 3)

list2 = EqConstraint[]
list2 += y == 3
list2 += list

info("All equality constraints tests passed")


using Surrogates

#1D
lb = 0.0
ub = 5.0
obj_1D = x -> log(x)*exp(x)
x = sample(5,lb,ub,SobolSample())
y = obj_1D.(x)
my_second_order_poly = SecondOrderPolynomialSurrogate(x,y,lb,ub)
val = my_second_order_poly(5.0)
add_point!(my_second_order_poly,5.0,238.86)
add_point!(my_second_order_poly,[6.0,7.0],[722.84,2133.94])

#ND
lb = [0.0,0.0]
ub = [10.0,10.0]
obj_ND = x -> log(x[1])*exp(x[2])
x = sample(5,lb,ub,UniformSample())
y = obj_ND.(x)
my_second_order_poly = SecondOrderPolynomialSurrogate(x,y,lb,ub)
val = my_second_order_poly((5.0,7.0))
add_point!(my_second_order_poly,(5.0,7.0),1764.96)
add_point!(my_second_order_poly,[(1.5,1.5),(3.4,5.4)],[1.817,270.95])


#=
Example continuous state Markov chains within the stochastic growth
model

@author : Spencer Lyon <spencer.lyon@nyu.edu>

@date: 2014-08-05

References
----------

Simple port of the file quantecon/examples/stochasticgrowth.py

http://quant-econ.net/stationary_densities.html
=#
using QuantEcon: LAE, lae_est
using Distributions
using PyPlot

s = 0.2
δ = 0.1
a_σ = 0.4  # A = exp(B) where B ~ N(0, a_sigma)
α = 0.4  # We set f(k) = k**alpha
ψ_0 = Beta(5.0, 5.0)  # Initial distribution
ϕ = LogNormal(0.0, a_σ)

function p(x, y)
    #=
    Stochastic kernel for the growth model with Cobb-Douglas production.
    Both x and y must be strictly positive.
    =#
    d = s * x.^α

    # scipy silently evaluates the pdf of the lognormal dist at a negative
    # value as zero. It should be undefined and Julia recognizes this.
    pdf_arg = clamp((y .- (1-δ) .* x) ./ d, eps(), Inf)
    return pdf(ϕ, pdf_arg) ./ d
end

n = 10000  # Number of observations at each date t
T = 30  # Compute density of k_t at 1,...,T+1

# Generate matrix s.t. t-th column is n observations of k_t
k = Array(Float64, n, T)
A = rand!(ϕ, Array(Float64, n, T))

# Draw first column from initial distribution
k[:, 1] = rand(ψ_0, n) ./ 2  # divide by 2 to match scale=0.5 in py version
for t=1:T-1
    k[:, t+1] = s*A[:, t] .* k[:, t].^α + (1-δ) .* k[:, t]
end

# Generate T instances of LAE using this data, one for each date t
laes = [LAE(p, k[:, t]) for t=T:-1:1]

# Plot
fig, ax = subplots()
ygrid = linspace(0.01, 4.0, 200)
greys = [string(g) for g in linspace(0.0, 0.8, T)]
for (psi, g) in zip(laes, greys)
    ax[:plot](ygrid, lae_est(psi, ygrid), color=g, lw=2, alpha=0.6)
end
ax[:set_xlabel]("capital")
t=LaTeXString("Density of \$k_1\$ (lighter) to \$k_T\$ (darker) for \$T=$T\$")
ax[:set_title](t)
show()


# # Adaptive Cruise Controller (ACC)
#
#md # [![](https://img.shields.io/badge/show-nbviewer-579ACA.svg)](@__NBVIEWER_ROOT_URL__/models/ACC.ipynb)
#
# The Adaptive Cruise Control (ACC) benchmark is a system that tracks a set
# velocity and maintains a safe distance from a lead vehicle by adjusting
# the longitudinal acceleration of an ego vehicle.
# The neural network computes optimal control actions while satisfying safe
# distance, velocity, and acceleration constraints.

module ACC  #jl

using NeuralNetworkAnalysis, MAT
using NeuralNetworkAnalysis: FunctionPreprocessing

# ## Model
#
# For this case study, the ego car is set to travel at a set speed $v_{set} = 30$
# and maintains a safe distance $D_{safe}$ from the lead car.  The car’s dynamics
# are described as follows:
#
# ```math
# \left\{ \begin{array}{lcl}
# \dot{x}_{lead}(t) &=& v_{lead}(t) \\
# \dot{v}_{lead}(t) &=& \gamma_{lead}(t) \\
# \dot{\gamma}_{lead}(t) &=& -2\gamma_{lead}(t) + 2a_{lead}(t) - uv_{lead}^2(t)  \\
# \dot{x}_{ego}(t) &=& v_{ego}(t) \\
# \dot{v}_{ego}(t) &=& \gamma_{ego}(t) \\
# \dot{\gamma}_{ego}(t) &=& -2\gamma_{ego}(t) + 2a_{ego}(t) - uv_{ego}^2(t)
# \end{array} \right.
# ```
# where $x_i$ is the position, $v_i$ is the velocity, $γ_i$ is the
# acceleration of the car, $a_i$ is the acceleration control input applied
# to the car, and $u = 0.0001$ is the friction parameter, where
# $i ∈ \{ego, lead\}$. For this benchmark we are given four neural network
# controllers with 3, 5, 7, and 10 hidden layers of 20 neurons each, but only evaluate
# the controller with 5 hidden layers. All of them have the same number of inputs
# $(v_{set}, T_{gap}, v_{ego}, D_{rel}, v_{rel})$ and one output ($a_{ego}$).

const u = 0.0001  # friction parameter
const a_lead = -2.0  # acceleration control input applied to the lead vehicle

@taylorize function ACC!(dx, x, p, t)
    v_lead = x[2]  # lead car velocity
    γ_lead = x[3]  # lead car acceleration
    v_ego = x[5]  # ego car velocity
    γ_ego = x[6]  # ego car acceleration
    a_ego = x[7]  # ego car acceleration control input

    ## lead car dynamics
    dx[1] = v_lead
    dx[2] = γ_lead
    dx[3] = 2 * (a_lead - γ_lead) - u * v_lead^2

    ## ego car dynamics
    dx[4] = v_ego
    dx[5] = γ_ego
    dx[6] = 2 * (a_ego - γ_ego) - u * v_ego^2
    dx[7] = zero(a_ego)
    return dx
end

# ## Specification

# The uncertain initial condition is:
#
# $x_{lead}(0) ∈ [90, 110], v_{lead}(0) ∈ [32, 32.2]$,
# $γ_{lead}(0) = γ_{ego}(0) = 0, v_{ego}(0) ∈ [30, 30.2], x_{ego} ∈ [10, 11]$
#
# The controller input is
# $(v_{set}, T_{gap}, v_{ego}, x_{lead} - x_{ego}, v_{lead} - v_{ego})$.
# To extract from the current state the last three inputs to the network
# we define a projection matrix $M$.
#
# The verification objective of this system is that given a scenario where
# both cars are driving safely, the lead car suddenly slows down with
# $a_{lead} = -2$.
# We want to check whether there is a collision in the following 5 seconds.
# A control period of 0.1 seconds is used.
#
# Formally, the safety specification can be expressed as:
# ```math
#     D_{rel} = x_{lead} - x_{ego} ≥ D_{safe},
# ```
# where $D_{safe} = D_{default} + T_{gap} * v_{ego}$,
# $T_{gap} = 1.4$ sec, and $D_{default} = 10$.
# After substitution, the specification reduces to:
# $x_{lead} - x_{ego} - T_{gap} * v_{ego} ≥ D_{default}$.

## We choose the controller with 5 hidden layers.
controller = read_nnet_mat(@modelpath("ACC", "controller_5_20.mat");
                           act_key="act_fcns");

## The initial states according to the specification are:
X₀ = Hyperrectangle(low=[90, 32, 0, 10, 30, 0],
                    high=[110, 32.2, 0, 11, 30.2, 0])
U₀ = ZeroSet(1);

## The system has 6 state variables and 1 control variable:
vars_idx = Dict(:state_vars=>1:6, :control_vars=>7)
ivp = @ivp(x' = ACC!(x), dim: 7, x(0) ∈ X₀ × U₀)
period = 0.1;  # control period

## Preprocessing function for the network input:
v_set = 30.0  # ego car's set speed
T_gap = 1.4
M = zeros(3, 6)
M[1, 5] = 1.0
M[2, 1] = 1.0
M[2, 4] = -1.0
M[3, 2] = 1.0
M[3, 5] = -1.0
function preprocess(X::LazySet)  # version for set computations
    Y1 = Singleton([v_set, T_gap])
    Y2 = linear_map(M, X)
    return cartesian_product(Y1, Y2)
end
function preprocess(X::AbstractVector)  # version for simulations
    Y1 = [v_set, T_gap]
    Y2 = M * X
    return vcat(Y1, Y2)
end
control_preprocessing = FunctionPreprocessing(preprocess)

prob = ControlledPlant(ivp, controller, vars_idx, period;
                       preprocessing=control_preprocessing);

## Safety specification
T = 5.0  # time horizon

D_default = 10.0
d_rel = [1.0, 0, 0, -1, 0, 0, 0]
d_safe = [0, 0, 0, 0, T_gap, 0, 0]

d_prop = d_rel - d_safe
safe_states = HalfSpace(-d_prop, -D_default)
predicate = X -> X ⊆ safe_states;

# ## Results

# To integrate the ODE, we use the Taylor-model-based algorithm:
alg = TMJets(abstol=1e-6, orderT=6, orderQ=1);

# To propagate sets through the neural network, we use the `Ai2` algorithm:
alg_nn = Ai2()

function benchmark(; silent::Bool=false)
    ## We solve the controlled system:
    silent || println("flowpipe construction")
    res_sol = @timed solve(prob, T=T, alg_nn=alg_nn, alg=alg)
    sol = res_sol.value
    silent || print_timed(res_sol)

    ## Next we check the property for an overapproximated flowpipe:
    silent || println("property checking")
    solz = overapproximate(sol, Zonotope)
    res_pred = @timed predicate(solz)
    silent || print_timed(res_pred)
    if res_pred.value
        silent || println("The property is satisfied.")
    else
        silent || println("The property may be violated.")
    end
    return solz
end

benchmark(silent=true)  # warm-up
res = @timed benchmark()  # benchmark
sol = res.value
println("total analysis time")
print_timed(res);

# We also compute some simulations:

import DifferentialEquations

println("simulation")
res = @timed simulate(prob, T=T, trajectories=10, include_vertices=true)
sim = res.value
print_timed(res);

# Finally we plot the results:

using Plots
import DisplayAs

fig = plot(leg=(0.4, 0.3))
xlabel!(fig, "time")
F = flowpipe(sol)

fp_rel = linear_map(Matrix(d_rel'), F)
output_map_rel = d_rel

fp_safe = affine_map(Matrix(d_safe'), [D_default], F)
output_map_safe = vcat([D_default], d_safe)

plot!(fig, fp_rel, vars=(0, 1), c=:red, alpha=.4)
plot!(fig, fp_safe, vars=(0, 1), c=:blue, alpha=.4)

plot_simulation!(fig, sim; output_map=output_map_rel, color=:red, lab="Drel")
plot_simulation!(fig, sim; output_map=output_map_safe, color=:blue, lab="Dsafe")

fig = DisplayAs.Text(DisplayAs.PNG(fig))
## savefig("ACC.png")
fig

#-

end  #jl
nothing  #jl


include("../Resistances/heat.jl")
include("../../Utilities/summary.jl")

using Measurements, Modia
#ENV["MODIA_PLOT"] = "PyPlot"
Modia.usePlotPackage("PyPlot")
@usingModiaPlot

# Boundary model(s)
T_a = 293.15u"K"
T_b = 313.15u"K"

r_inner = measurement(1.0,0.1)u"m"
r_outer = 2.0u"m"
lambda = 5.0u"W/(m*K)"

# Base Classes
PartialTest = Model(
    Q_total=Var(init=0.0u"J"),
    partialEquations =:[
        der(Q_total) = Qb_flow
    ]
)

# Sphere Model
sphere_model = PartialTest | Model(
    boundaryT_a = Temperature | Map(T=T_a),
    boundaryT_b = Temperature | Map(T=T_b),
    resistor = Sphere | Map(r_inner=r_inner,r_outer=r_outer,lambda=lambda),
    equations =:[
        Qb_flow = resistor.port_a.Q_flow
    ],
    connect =:[
        (boundaryT_a.port,resistor.port_a)
        (resistor.port_b,boundaryT_b.port)
    ]
)

result = @instantiateModel(sphere_model, log=false,FloatType=Measurement{Float64})
@time simulate!(result, stopTime=1.0, tolerance=1e-4, log=false)
y_min, t_min, y_max, t_max = reportMinMax("Q_total","Sphere Model",result)
errorCheck(-2513.2742u"J"±0.0u"J", y_min)
plot(result, ["Q_total"], figure=1)

# Convection model
surfaceArea = 2u"m^2"
alpha = 15u"W/(m^2*K)"

convection_model = PartialTest | Model(
    boundaryT_a = Temperature | Map(T=T_a),
    boundaryT_b = Temperature | Map(T=T_b),
    resistor = Convection | Map(surfaceArea = surfaceArea, alpha=alpha),
    equations =:[
        Qb_flow = resistor.port_a.Q_flow
    ],
    connect =:[
        (boundaryT_a.port,resistor.port_a)
        (resistor.port_b,boundaryT_b.port)
    ]
)

result = @instantiateModel(convection_model, log=false,FloatType=Measurement{Float64})
@time simulate!(result, stopTime=1.0, tolerance=1e-4, log=false)
y_min, t_min, y_max, t_max = reportMinMax("Q_total","Convection Model",result)
errorCheck(-600u"J"±0.0u"J", y_min)
plot(result, ["Q_total"], figure=1)

# Contact model
surfaceArea = 2u"m^2" # units hid until constant unit issue solved
Rc_pp = 5u"m^2*K/W"

contact_model = PartialTest | Model(
    boundaryT_a = Temperature | Map(T=T_a),
    boundaryT_b = Temperature | Map(T=T_b),
    resistor = Contact | Map(surfaceArea = surfaceArea, Rc_pp=Rc_pp),
    equations =:[
        Qb_flow = resistor.port_a.Q_flow
    ],
    connect =:[
        (boundaryT_a.port,resistor.port_a)
        (resistor.port_b,boundaryT_b.port)
    ]
)

result = @instantiateModel(contact_model, log=false,FloatType=Measurement{Float64})
@time simulate!(result, stopTime=1.0, tolerance=1e-4, log=false)
y_min, t_min, y_max, t_max = reportMinMax("Q_total","Contact Model",result)
errorCheck(-8u"J"±0.0u"J", y_min)
plot(result, ["Q_total"], figure=1)

# Radiation exact model
surfaceArea = 2u"m^2"
epsilon = 0.5u"m/m"

radiationExact_model = PartialTest | Model(
    boundaryT_a = Temperature | Map(T=T_a),
    boundaryT_b = Temperature | Map(T=T_b),
    resistor = Radiation | Map(surfaceArea = surfaceArea, epsilon=epsilon),
    equations =:[
        Qb_flow = resistor.port_a.Q_flow
    ],
    connect =:[
        (boundaryT_a.port,resistor.port_a)
        (resistor.port_b,boundaryT_b.port)
    ]
)

result = @instantiateModel(radiationExact_model, log=false,FloatType=Measurement{Float64})
@time simulate!(result, stopTime=1.0, tolerance=1e-4, log=false)
y_min, t_min, y_max, t_max = reportMinMax("Q_total","Radiation Exact Model",result)
errorCheck(-126.51638u"J"±0.0u"J", y_min)
plot(result, ["Q_total"], figure=1)

# Radiation approximate model
radiationApproximate_model = PartialTest | Model(
    boundaryT_a = Temperature | Map(T=T_a),
    boundaryT_b = Temperature | Map(T=T_b),
    resistor = Radiation | Map(surfaceArea = surfaceArea, epsilon=epsilon, useExact=false),
    equations =:[
        Qb_flow = resistor.port_a.Q_flow
    ],
    connect =:[
        (boundaryT_a.port,resistor.port_a)
        (resistor.port_b,boundaryT_b.port)
    ]
)

result = @instantiateModel(radiationApproximate_model, log=false,FloatType=Measurement{Float64})
@time simulate!(result, stopTime=1.0, tolerance=1e-4, log=false)
y_min, t_min, y_max, t_max = reportMinMax("Q_total","Radiation Approximate Model",result)
errorCheck(-126.51638u"J"±0.0u"J", y_min)
plot(result, ["Q_total"], figure=1)


using DataFrames
using GLM
using RegressionDiscontinuity
using Test

Zs = randn(1000)

ZsR = RunningVariable(Zs, cutoff=0.0)
Zsgeg0 = RunningVariable(Zs, cutoff=1.0)
Zsless0 = RunningVariable(Zs, cutoff=1.0, treated=:<)
@test all(xor.(Zsgeg0.Ws + Zsless0.Ws) .== 1)

tmp_df = DataFrame(Ws=ZsR.Ws, Zs=ZsR.Zs, ZsC=ZsR.ZsC)
@test Tables.schema(tmp_df) == Tables.schema(ZsR)

Ys = rand(1000)
rdd_data = RegressionDiscontinuity.RDData(Ys, ZsR)
myfit = fit(LinearModel, @formula(Ys ~ Zs + Ws), rdd_data)


mutable struct Observable
    v1::Float64
    v2::Float64
    t::Int64

    # kahan reminders
    cv1::Float64
    cv2::Float64
end

Observable(v1, v2, t) = Observable(v1, v2, t, 0., 0.)
Observable() = Observable(0,0,0)

function obs_from_mean_err_samp(m, err, t)
    v1 = m*t
    var = err^2 *(t-1)
    v2 = (var + m^2)*t
    return Observable(v1, v2, t)
end

function (&)(a::Observable,b::Observable)
    Observable(a.v1 + b.v1, a.v2 + b.v2, a.t + b.t)
end

# Kahan summation algorithm
function (&)(a::Observable,val::Real)
    if !isfinite(val)
        @warn("$val observation not considered")
        return copy(a)
    end
    y = val - a.cv1
    z = a.v1 + y
    cv1 = (z - a.v1) - y
    v1 = z

    y = val*val - a.cv2
    z = a.v2 + y
    cv2 = (z - a.v2) - y
    v2 = z

    t = a.t + 1

    Observable(v1, v2, t, cv1, cv2)
end

function (&)(a::Observable,vals::AbstractArray{T}) where T<:Real
     b = deepcopy(a)
    for x in vals
        b &= x
    end
    return b
end

Statistics.mean(a::Observable) = a.t > 0 ? a.v1/a.t : 0
Statistics.var(a::Observable) = a.v2 / a.t - mean(a)^2
err(a::Observable) = a.t <= 1 ? 0 :
                       var(a) < 0 ? 0. : sqrt(var(a) /(a.t -1)) # sometimes numerical errors for small var

function shortshow(io::IO, a::Observable)
    max_round = 10
    if err(a) > 0.
        r = round(Int, log(10, err(a))) - 2
        r = abs(min(0,r))
        r = min(r, max_round)
        # const fmt = "%.$(r)f %.2e"
        # @eval @printf($io, $fmt, $(mean(a)), $(error(a)))#mean(a), " ", error(a))
        print(io, "$(round(mean(a), digits=r)) ")
        @printf(io, "%.2e", err(a))
    else
        print(io, "$(round(mean(a), digits=max_round)) $(err(a))")
    end
end

Base.show(io::IO, a::Observable) = shortshow(io, a)

*(a::Observable, val::Real) = Observable(val*a.v1, val^2*a.v2, a.t)
*(val::Real, a::Observable) = *(a, val)

+(a::Observable, val::Real) = Observable(a.v1 + val*a.t, a.v2 + val*a.v1 + val^2*a.t, a.t)
+(val::Real, a::Observable) = +(a, val)
==(a::Observable, b::Observable) = all(getfield(a,f) == getfield(b,f) for f in fieldnames(typeof(a)))
copy(a::Observable) = deepcopy(a)

"""
Desidered properties for `t` of obs resulting from composition of Observables `a` and `b`:

1. symmetric in `a` and `b`
2. error(a) -> 0  =>  t ~ b.t
2. error(a) >> error(b)  =>  t ~ a.t
3. a.t -> ∞  =>  t = b.t
4. a == b  => t = a.t = b.t

One possible choice is `t ≃ (error(a) + error(b)) / (error(a)/a.t + error(b)/b.t)`
"""
function t_composition(a::Observable, b::Observable)
    ea, eb = error(a), error(b)
    return ceil(Int, (ea + eb)/(ea/a.t + eb/b.t))
end

function +(a::Observable, b::Observable)
    t = t_composition(a, b)
    ea, eb = err(a), err(b)
    ma, mb = mean(a), mean(b)
    e = sqrt(ea^2 + eb^2)
    v1 = (ma + mb) * t
    v2 = e^2*t + v1^2/t
    Observable(v1, v2, t)
end

function *(a::Observable, b::Observable)
    t = t_composition(a, b)
    ma, mb = mean(a), mean(b)
    ea, eb = err(a), err(b)
    e = sqrt(mb^2*ea^2 + ma^2*eb^2)
    v1 = ma*mb*t
    v2 = e^2*t + v1^2/t
    Observable(v1, v2, t)
end


include("Tanh.jl")
include("jacobian.jl")
using Base.Test

X = map(Float32, randn(5, 3))
l = Tanh{Float32}()

# Test grad. wrt. input X
Dfwd = jacobian_fwd(l, X)
Dbwd = jacobian_bwd(l, X)

@test_approx_eq Dfwd Dbwd


using SuperFastGraphs
using Base.Test
using LightGraphs

# g = loadgraph("./testdata/barabasi_example.lg", "graph")
g = loadgraph("./testdata/karate.lg", "graph")
g = loadgraph("./testdata/internet.lg", "graph")
# g = barabasi_albert(1000, 100)

# g = Graph(5)

# add_edge!(g, 1, 2)
# add_edge!(g, 1, 3)
# add_edge!(g, 1, 4)
# add_edge!(g, 1, 5)
# add_edge!(g, 2, 4)
# add_edge!(g, 2, 3)

tic()
t = triangles(g)
toc()

tic()
triangleNumberD = triangleCountingDegree(g)
toc()

tic()
nt = triangleForward(g)
toc()

println(nt == sum(t)/3)


RMSEA(sem_fit::SemFit{Mi, So, St, Mo, O} where {Mi, So, St, Mo <: AbstractSemSingle, O}) = RMSEA(df(sem_fit), χ²(sem_fit), n_obs(sem_fit))

RMSEA(sem_fit::SemFit{Mi, So, St, Mo, O} where {Mi, So, St, Mo <: SemEnsemble, O}) = sqrt(length(sem_fit.model.sems))*RMSEA(df(sem_fit), χ²(sem_fit), n_obs(sem_fit))

function RMSEA(df, chi2, n_obs)
    rmsea = (chi2 - df) / (n_obs*df)
    rmsea > 0 ? nothing : rmsea = 0
    return sqrt(rmsea)
end

#= # SemFit splices loss functions ---------------------------------------------------------------------
RMSEA(sem_fit::SemFit{Mi, So, St, Mo, O} where {Mi, So, St, Mo <: AbstractSemSingle, O}) =
    RMSEA(
        sem_fit,
        sem_fit.model.observed,
        sem_fit.model.imply,
        sem_fit.model.diff,
        sem_fit.model.loss.functions...
        ) =#

#=
# RAM + SemML
function RMSEA(sem_fit::SemFit, obs, imp::Union{RAM, RAMSymbolic}, diff, loss_ml::Union{SemML, SemFIML, SemWLS})
    df_ = df(sem_fit)
    rmsea = (χ²(sem_fit) - df_) / ((n_obs(sem_fit))*df_)
    rmsea > 0 ? nothing : rmsea = 0
    return sqrt(rmsea)
end =#


function getBirkhoffStepSize(x_star,x,y)
    return minimum((x_star-x).*y - (y.-1));
end


# Original Java version Copyright 2005 Google Inc.
# Julia translation Copyright 2022 Ted Dunning
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

# This class represents a point on the unit sphere as a pair of latitude-longitude coordinates.

struct S2LatLng
    lat::Float64
    lng::Float64
end

S2LatLng(lat::S1Angle, lon::S1Angle) = S2LatLng(radians(lat), radians(lon))
S2LatLng(p::Vector) = verifyPoint(p) && S2LatLng(latitude(p), longitude(p))

radians2LL(lat::Number, lon::Number) = S2LatLng(lat, lon)
deg2LL(lat::Number, lon::Number) = S2LatLng(deg2rad(lat), deg2rad(lon))

latitude(p::Vector) = verifyPoint(p) && S1Angle(atan(p[3], hypot(p[1], p[2])))
latitude(p::S2LatLng) = S1Angle(p.lat)
longitude(p::Vector) = verifyPoint(p) && S1Angle(atan(p[2], p[1]))
longitude(p::S2LatLng) = S1Angle(p.lngRadians)

md"""
Returns a new S2LatLng based on this instance for which {@link #isValid()} will be {@code
true}.

* Latitude is clipped to the range {@code [-90, 90]}
* Longitude is normalized to be in the range {@code [-180, 180]}

f the current point is valid then the returned point will have the same coordinates.
"""
function LinearAlgebra.normalize(p::S2LatLng)::S2LatLng
    S2LatLng(max(-π, min(π, p.lat)),
             normalizeRange(p.lng))
end

function S2Point(ll::S2LatLng)
    ϕ = ll.lat
    θ = ll.lng
    cosphi = cos(ϕ)
    return S2Point(cos(θ) * cosphi, sin(θ) * cosphi, sin(ϕ))
end

md"""
Returns the distance (measured along the surface of the sphere) to the given point.

This code uses the Haversine formula which is very good for short distances but is
not as good for nearly antipodal points (but that only means errors of 10cm). An
alternative is to convert to S2Points and then to S1Angle to get 15 digits of accuracy
for all distances.
"""
function distance(a::S2LatLng, b::S2LatLng)
    dlat = Math.sin(0.5 * (a.lat - b.latRadians))
    dlng = Math.sin(0.5 * (a.lngRadians - b.lngRadians))
    x = dlat * dlat + dlng * dlng * cos(a.latRadians) * cos(b.latRadians)
    return S1Angle.radians(2 * asin(sqrt(min(1.0, x))))
end

md"Returns the surface distance to the given point assuming a constant radius."
distance(a::S2LatLng, b::S2LatLng, radius) = distance(distance(a, b), radius)

md"""
Adds the given point to this point. Note that there is no guarantee that the new point will be
valid.
"""
(+)(a::S2LatLng, b::S2LatLng) = S2LatLng(a.latRadians + b.latRadians, a.lngRadians + o.lngRadians)

md"""
Subtracts the given point from this point. Note that there is no guarantee that the new point
will be valid.
"""
(-)(a::S2LatLng, b::S2LatLng) = S2LatLng(a.latRadians - b.latRadians, a.lngRadians - b.lngRadians)

(*)(a::S2LatLng, m) = S2LatLng(a.latRadians * m, a.lngRadians * m)

equals(a::S2LatLng, b::S2LatLng) = (a.latRadians == b.latRadians) && (a.lngRadians == b.lngRadians)


using ChaosTools
using DynamicalSystemsBase
using Test
using LinearAlgebra
using OrdinaryDiffEq

println("\nTesting basins of attraction...")

@testset "Basins tests" begin

@testset "Discrete map" begin
    ds = Systems.henon(zeros(2); a = 1.4, b = 0.3)
    xg = yg = range(-2.,2.,length=100)
    basin, attractors = basins_of_attraction((xg,yg), ds; show_progress = false)
    # pcolormesh(xg,yg, basin')
    @test 4260 ≤ count(basin .== 1) ≤ 4280
    @test 5700 ≤ count(basin .== -1) ≤ 5800
end

@testset "Test basin stroboscopic map" begin
    ds = Systems.duffing([0.1, 0.25]; ω = 1.0, f = 0.2, d = 0.15, β = -1)
    xg = yg = range(-2.2,2.2,length=100)
    T = 2π/1.0
    basin,attractors = basins_of_attraction((xg,yg), ds;
    T, diffeq = (;alg=Tsit5()), show_progress = false)
    # pcolormesh(xg, yg, basin')
    @test length(unique(basin)) == 2
    @test 4900 ≤ count(basin .== 1) ≤ 5100
    @test  4900 ≤  count(basin .== 2) ≤ 5100
end

@testset "Test basin poincare map" begin
    ds = Systems.thomas_cyclical(b = 0.1665)
    xg = yg = range(-6.0, 6.0; length = 100)
    pmap = poincaremap(ds, (3, 0.0), 1e6;
        rootkw = (xrtol = 1e-8, atol = 1e-8), diffeq=(reltol=1e-9,)
    )
    basin,attractors = basins_of_attraction((xg,yg), pmap; show_progress = false)
    # pcolormesh(xg,yg, basin')
    @test length(attractors) == 3
    @test  4610 ≤ count(basin .== 1) ≤ 4641
    @test  2660 ≤ count(basin .== 2) ≤ 2691
    @test  2640 ≤ count(basin .== 3) ≤ 2691
end

@testset "basins_of_attraction" begin
    ds = Systems.magnetic_pendulum(γ=1, d=0.2, α=0.2, ω=0.8, N=3)
    xg = range(-2,2,length=100)
    yg = range(-2,2,length=100)
    complete_state(y) = SVector(0.0, 0.0)
    basin, attractors = basins_of_attraction((xg,yg), ds;
    idxs=1:2, Δt = 1., complete_state, show_progress = false)
    # pcolormesh(xg,yg, basin')
    @test count(basin .== 1) == 3332
    @test count(basin .== 2) == 3332

    # Now test the zoom capability
    xg = yg = range(-2,-1.9,length=50)
    basins, att = basins_of_attraction((xg,yg), ds;
    idxs = 1:2, Δt = 1., complete_state, show_progress = false,
    attractors = attractors, mx_chk_lost = 1000, ε = 1e-3)
    @test count(basins .== 2) == 407
    @test count(basins .== 3) == 737
end

@testset "3D basins" begin
    ds = Systems.lorenz84()
    xg=yg=range(-1.,2.,length=100)
    zg=range(-1.5,1.5,length=30)
    bsn,att = basins_of_attraction((xg, yg, zg), ds; Δt = 1.0, show_progress = false)
    @test length(size(bsn)) == 3
    for i in 1:size(bsn)[3]
        # While there are 4 attractors, because system is chaotic we might
        # miss one of the 4 due to coarse state space partition
        @test sort!(unique(bsn[:, :, i])) ∈ ([1,2,3], [1,2,3,4])
    end
end

@testset "Basins for in-place system" begin
    ds = Systems.henon_iip(zeros(2); a = 1.4, b = 0.3)
    xg = yg = range(-2.,2.;length=100)
    basin, attractors = basins_of_attraction((xg,yg), ds; show_progress = false)
    # pcolormesh(xg,yg, basin')
    @test 4260 ≤ count(basin .== 1) ≤ 4280
    @test 5600 ≤ count(basin .== -1) ≤ 5800

    ds = Systems.lorenz_iip()
    xg = yg = range(-30.0, 30.0; length=10)
    zg = range(0, 50; length=10)
    basin, attractors = basins_of_attraction((xg,yg,zg), ds; show_progress=false, mx_chk_fnd_att = 1000)
    @test length(attractors) == 1
    @test unique(basin) == [1]
end

@testset "matching attractors" begin
    d, α, ω = 0.3, 0.2, 0.5
    ds = Systems.magnetic_pendulum(; d, α, ω)
    xg = yg = range(-3, 3, length = 100)
    b₋, a₋ = basins_of_attraction((xg,yg), ds; Δt=1., idxs=1:2, show_progress = false)
    @testset "method $method" for method ∈ (:overlap, :distance)
        @testset "γ3 $γ3" for γ3 ∈ [0.2, 0.1] # still 3 at 0.2, but only 2 at 0.1
            ds = Systems.magnetic_pendulum(; d, α, ω,  γs = [1, 1, γ3])
            b₊, a₊ = basins_of_attraction((xg,yg), ds; Δt=1., idxs=1:2, show_progress = false)
            match_attractors!(b₋, a₋, b₊, a₊, method)
            for k in keys(a₊)
                dist = minimum(norm(x .- y) for x ∈ a₊[k] for y ∈ a₋[k])
                @test dist < 0.2
            end
        end
    end
end

end


"""
0  Solve the Nominal Problem
1a Determine the uncertain Parameters.
1b Determine the uncertainty set.
2  Check robustness of the nominal solution
   IF the nominal Solution is robust enougth THEN Stop.
3a Determine the adjustable variables
3b Determine the type of decision rules
4  Formulate the robust counterpart
5  Solve the (adjustable) robust counterpart

größer gleich
klein gleich
rhs
Lhs
ist es ein vektor
coeffizienzten

verschiedne Uncertentset
uncertentyset link zu uncertain constraint

"""


module QuadTrigInterpMod

import FEM: AbstractInterpolator, Point2, FENode2, Vertex6, inv2x2t!, det2x2,
       dNmatrix, Jmatrix, dNdxmatrix, Nvec, get_area

export QuadTrigInterp

immutable QuadTrigInterp <: AbstractInterpolator
    N::Vector{Float64}
    dN::Matrix{Float64}
    dNdx::Matrix{Float64}
    J::Matrix{Float64}
end

function QuadTrigInterp()
    N = zeros(6)
    dN = zeros(6,2)
    dNdx = Array(Float64, 6, 2)
    J = Array(Float64, 2, 2)
    QuadTrigInterp(N, dN, dNdx, J)
end
get_allocated_N(i::QuadTrigInterp) = i.N
get_allocated_dN(i::QuadTrigInterp) = i.dN
get_allocated_dNdx(i::QuadTrigInterp) = i.dNdx
get_allocated_J(i::QuadTrigInterp) = i.J

# Shape functions in local coords
# http://www.colorado.edu/engineering/CAS/courses.d/IFEM.d/IFEM.Ch16.d/IFEM.Ch16.pdf
function Nvec(interp::QuadTrigInterp, loc_coords::Point2)

    ξ = loc_coords[1]
    η = loc_coords[2]
    γ = 1 - ξ - η

    N = get_allocated_N(interp)

    N[1] = ξ * (2ξ - 1)
    N[2] = η * (2η - 1)
    N[3] = γ * (2γ - 1)
    N[4] = 4ξ * η
    N[5] = 4η * γ
    N[6] = 4ξ * γ

    return N
end

function dNmatrix(interp::QuadTrigInterp, loc_coords::Point2)
    ξ = loc_coords.(1)
    η = loc_coords.(2)
    γ = 1 - ξ - η

    dN = get_allocated_dN(interp)

    dN[1, 1] = 4ξ - 1
    dN[1, 2] = 0

    dN[2, 1] = 0
    dN[2, 2] = 4η - 1

    dN[3, 1] = -4γ + 1
    dN[3, 2] = -4γ + 1

    dN[4, 1] = 4η
    dN[4, 2] = 4ξ

    dN[5, 1] = -4η
    dN[5, 2] = 4(γ - η)

    dN[6, 1] = 4(γ - ξ)
    dN[6, 2] = -4ξ

    return dN
end

#const nfields = 6
function Jmatrix(interp::QuadTrigInterp,
                 vertices::Vertex6, nodes::Vector{FENode2},
                 dN::Matrix{Float64})

    J = get_allocated_J(interp)
    fill!(J, 0.0)
    for row in 1:size(dN, 1)
        x = nodes[vertices.(row)].coords.x
        y = nodes[vertices.(row)].coords.y

        J[1, 1] += dN[row, 1] * x
        J[1, 2] += dN[row, 1] * y
        J[2, 1] += dN[row, 2] * x
        J[2, 2] += dN[row, 2] * y
    end
    return J
end

function dNdxmatrix(interp::QuadTrigInterp, local_coords::Point2,
                    vertices::Vertex6, nodes::Vector{FENode2})
        dN = dNmatrix(interp, local_coords)
        J = Jmatrix(interp, vertices, nodes, dN)
        A_mul_B!(interp.dNdx, dN, inv2x2t!(J))
        return interp.dNdx
end

end # module


export CalculateSampling,
Pad5D,
IRLS,
FFTOp,
WeightingOp,
ConjugateGradients,
LinearOperator,
MatrixMultiplyOp,
InnerProduct,
DotTest,
FISTA,
power_method
include("CalculateSampling.jl")
include("Pad5D.jl")
include("IRLS.jl")
include("FFTOp.jl")
include("WeightingOp.jl")
include("ConjugateGradients.jl")
include("LinearOperator.jl")
include("MatrixMultiplyOp.jl")
include("InnerProduct.jl")
include("DotTest.jl")
include("FISTA.jl")
include("power_method.jl")


"""
    SpinBasis(n)

Basis for spin-n particles.

The basis can be created for arbitrary spinnumbers by using a rational number,
e.g. `SpinBasis(3//2)`. The Pauli operators are defined for all possible
spin numbers.
"""
struct SpinBasis{S,T} <: Basis
    shape::Vector{T}
    spinnumber::Rational{T}
    function SpinBasis{S}(spinnumber::Rational{T}) where {S,T<:Integer}
        n = numerator(spinnumber)
        d = denominator(spinnumber)
        @assert d==2 || d==1
        @assert n >= 0
        N = numerator(spinnumber*2 + 1)
        new{spinnumber,T}([N], spinnumber)
    end
end
SpinBasis(spinnumber::Rational) = SpinBasis{spinnumber}(spinnumber)
SpinBasis(spinnumber) = SpinBasis(convert(Rational{Int}, spinnumber))

==(b1::SpinBasis, b2::SpinBasis) = b1.spinnumber==b2.spinnumber

"""
    sigmax([T=ComplexF64,] b::SpinBasis)

Pauli ``σ_x`` operator for the given Spin basis.
"""
function sigmax(::Type{T}, b::SpinBasis) where T
    N = length(b)
    diag = T[complex(sqrt(real((b.spinnumber + 1)*2*a - a*(a+1)))) for a=1:N-1]
    data = spdiagm(1 => diag, -1 => diag)
    SparseOperator(b, data)
end
sigmax(b::SpinBasis) = sigmax(ComplexF64,b)

"""
    sigmay([T=ComplexF64,] b::SpinBasis)

Pauli ``σ_y`` operator for the given Spin basis.
"""
function sigmay(::Type{T}, b::SpinBasis) where T
    N = length(b)
    diag = T[1im*complex(sqrt(real((b.spinnumber + 1)*2*a - a*(a+1)))) for a=1:N-1]
    data = spdiagm(-1 => diag, 1 => -diag)
    SparseOperator(b, data)
end
sigmay(b::SpinBasis) = sigmay(ComplexF64,b)

"""
    sigmaz([T=ComplexF64,] b::SpinBasis)

Pauli ``σ_z`` operator for the given Spin basis.
"""
function sigmaz(::Type{T}, b::SpinBasis) where T
    N = length(b)
    diag = T[complex(2*m) for m=b.spinnumber:-1:-b.spinnumber]
    data = spdiagm(0 => diag)
    SparseOperator(b, data)
end
sigmaz(b::SpinBasis) = sigmaz(ComplexF64,b)

"""
    sigmap([T=ComplexF64,] b::SpinBasis)

Raising operator ``σ_+`` for the given Spin basis.
"""
function sigmap(::Type{T}, b::SpinBasis) where T
    N = length(b)
    S = (b.spinnumber + 1)*b.spinnumber
    diag = T[complex(sqrt(float(S - m*(m+1)))) for m=b.spinnumber-1:-1:-b.spinnumber]
    data = spdiagm(1 => diag)
    SparseOperator(b, data)
end
sigmap(b::SpinBasis) = sigmap(ComplexF64,b)

"""
    sigmam([T=ComplexF64,] b::SpinBasis)

Lowering operator ``σ_-`` for the given Spin basis.
"""
function sigmam(::Type{T}, b::SpinBasis) where T
    N = length(b)
    S = (b.spinnumber + 1)*b.spinnumber
    diag = T[complex(sqrt(float(S - m*(m-1)))) for m=b.spinnumber:-1:-b.spinnumber+1]
    data = spdiagm(-1 => diag)
    SparseOperator(b, data)
end
sigmam(b::SpinBasis) = sigmam(ComplexF64,b)

"""
    spinup([T=ComplexF64,] b::SpinBasis)

Spin up state for the given Spin basis.
"""
spinup(::Type{T}, b::SpinBasis) where T = basisstate(T, b, 1)
spinup(b::SpinBasis) = spinup(ComplexF64, b)

"""
    spindown([T=ComplexF64], b::SpinBasis)

Spin down state for the given Spin basis.
"""
spindown(::Type{T}, b::SpinBasis) where T = basisstate(T, b, b.shape[1])
spindown(b::SpinBasis) = spindown(ComplexF64, b)


const INTERIOR_COEFFICIENTS = Dict{Int, Vector{Float64}}(
2 => [ 0.482962913145 ; 0.836516303738 ; 0.224143868042 ; -0.129409522551 ]
,
3 => [ .332670552950 ; .806891509311 ; .459877502118 ; -.135011020010 ; -.085441273882 ; .035226291882 ]
,
4 => [ 0.045570345896 ; -0.0178247014417 ; -0.140317624179 ; 0.421234534204 ; 1.13665824341 ; 0.703739068656 ; -0.0419109651251 ; -0.107148901418 ] / sqrt2
,
5 => [ 0.0276321529578 ; -0.0298424998687 ; -0.247951362613 ; 0.0234789231361 ; 0.89658164838 ; 1.02305296689 ; 0.281990696854 ; -0.0553441861166 ; 0.0417468644215 ; 0.0386547959548 ] / sqrt2
,
6 => [ -0.0110318675094 ; 0.00249992209279 ; 0.06325056266 ; -0.0297837512985  ; -0.102724969862 ; 0.477904371333 ; 1.11389278393 ; 0.694457972958 ; -0.0683231215866  ; -0.166863215412 ; 0.00493661237185 ; 0.0217847003266 ] / sqrt2
,
7 => [ 0.014521394762 ; 0.00567134268574 ; -0.152463871896 ; -0.198056706807 ; 0.408183939725 ; 1.08578270981 ; 0.758162601964  ; 0.0246656594886  ; -0.070078291222 ; 0.0960147679355 ; 0.043155452582 ; -0.0178704316511 ; -0.0014812259146 ; 0.0037926585342 ] / sqrt2
,
8 => [ 0.00267279339281 ; -0.000428394300246 ; -0.0211456865284 ; 0.00538638875377 ; 0.0694904659113 ; -0.0384935212634 ; -0.0734625087609 ; 0.515398670374 ; 1.09910663054 ; 0.68074534719 ; -0.0866536154058 ; -0.202648655286 ; 0.0107586117505 ; 0.0448236230437 ; -0.000766690896228 ; -0.0047834585115 ] / sqrt2
)


# Tests for SPMF-code

using NonlinearEigenproblemsTest
using NonlinearEigenproblems
using Test
using LinearAlgebra
using Random
using SparseArrays

@testset "SPMF" begin
    # Create an SPMF
    n=5;
    Random.seed!(1)
    A0=sparse(randn(n,n));
    A1=sparse(randn(n,n));
    t::Float64=3.0

    minusop = S -> -S
    oneop = S -> one(S)
    expmop = S -> exp(-t*S)
    fi=[minusop, oneop, expmop];

    J = SparseMatrixCSC(1.0I, n, n)
    nep1 = SPMF_NEP([J, A0, A1], fi)
    nep2 = SPMF_NEP([J, A0, A1], fi; Schur_fact=true)

    @bench @testset "compute_Mlincomb" begin
        # test that the function compute_Mlincomb does not overwrite the input
        λ =randn(); V=randn(n,3); W=copy(V); a=[1; 0; 2];
        z=compute_Mlincomb(nep1,λ,V,a);
        @test norm(V-W)<sqrt(eps())*100
        # test that the function compute_Mlincomb! overwrites the input
        z=compute_Mlincomb!(nep1,λ,V,a);
        @test norm(V-W)>sqrt(eps())*100
    end

    @bench @testset "compute_MM" begin

        S=randn(3,3);
        V=randn(n,3);

        # Check if prefactorize with Schur gives the same result
        @test opnorm(compute_MM(nep1,S,V)-compute_MM(nep2,S,V))<sqrt(eps())
        # Check compute_MM
        @test opnorm(compute_MM(nep1,S,V)-(-V*S+A0*V+A1*V*exp(-t*S)))<sqrt(eps())

        # Check if compute_MM is correct (by comparing against diagonalization of S).
        # This is done with the identity MM(V,S)=MM(V,W*D*inv(W))=MM(V*W,D)*inv(W)
        # and MM(V1,D) for diagonal D can be computed with compute_Mlincomb

        N1=compute_MM(nep1,S,V);
        # Diagonalize S
        d,W = eigen(S)
        D = diagm(0 => d)
        V1 = V*W
        #
        N2=hcat(compute_Mlincomb(nep1,d[1],V1[:,1]),
                compute_Mlincomb(nep1,d[2],V1[:,2]),
                compute_Mlincomb(nep1,d[3],V1[:,3]))*inv(W)
        @test opnorm(N1-N2)<sqrt(eps())

    end
#    @bench @testset "SPMF sparse" begin
#
#
#        Random.seed!(0)
#        B0=sprandn(5,5,0.2)
#        B1=sprandn(5,5,0.2)
#
#        fv=[S->one(S),S->sin(S)];
#        spmf1=SPMF_NEP([B0,B1],fv,align_sparsity_patterns=false)
#        spmf2=SPMF_NEP([copy(B0),copy(B1)],fv,align_sparsity_patterns=true)
#
#        spmf3=SPMF_NEP([Matrix(B0),Matrix(B1)],fv)
#
#        λ=1.0;
#        Ma1=compute_Mder(spmf1,λ);
#        Ma2=compute_Mder(spmf2,λ);
#        Ma3=compute_Mder(spmf3,λ);
#        @test Ma1 ≈ Ma2
#        @test Matrix(Ma1) ≈ Matrix(Ma3)
#
#
#        λ=1.0+3im;
#        Mb1=compute_Mder(spmf1,λ);
#        Mb2=compute_Mder(spmf2,λ);
#        Mb3=compute_Mder(spmf3,λ);
#        @test Matrix(Mb1) ≈ Matrix(Mb2)
#        @test Matrix(Mb1) ≈ Matrix(Mb3)
#
#
#    end
    @bench @testset "compute_Mder_from_MM" begin

        S=randn(3,3);
        V=randn(n,3);

        # Check compute_Mder_from_MM()
        λ=2
        T1=compute_Mder_from_MM(nep1,λ,1)
        T2 = -I - t*A1*exp(-t*λ)
        @test opnorm(T1-T2)<sqrt(eps())

        λ=2
        T3=compute_Mder_from_MM(nep1,λ,2)
        T4=t^2*A1*exp(-t*λ)
        @test opnorm(T3-T4)<sqrt(eps())

        # Check consistency of MM and Mder

        # Exact Mlincomb:
        Zexact = (-λ*I + A0 + A1*exp(-t*λ)) * V[:,1] +
            (-I - t*A1*exp(-t*λ)) * V[:,2] +
            (t^2*A1*exp(-t*λ)) * V[:,3]

        Z1=compute_Mlincomb_from_MM(nep1,λ,V,[1.0,1.0,1.0])
        @test norm(Z1-Zexact)<sqrt(eps())

        Z2=compute_Mlincomb_from_Mder(nep1,λ,V,[1.0,1.0,1.0])
        @test norm(Z2-Zexact)<sqrt(eps())

    end

    @bench @testset "Compute_Mlincomb" begin

        Random.seed!(10)
        S=randn(3,3)+120^2*I;
        V=randn(n,3);

        # Same nonlinearities as the GUN NLEVP problem
        minusop= S-> -S
        oneop = S -> one(S)
        sqrt1op = S -> 1im * sqrt(S)
        sqrt2op = S -> 1im * sqrt(S - 108.8774^2*one(S))

        A0=sparse(randn(n,n))+1im*sparse(randn(n,n));
        A1=sparse(randn(n,n))+1im*sparse(randn(n,n));
        A2=sparse(randn(n,n))+1im*sparse(randn(n,n));
        A3=sparse(randn(n,n))+1im*sparse(randn(n,n));
        nep2=SPMF_NEP([A0,A1,A2,A3],[oneop,minusop,sqrt1op,sqrt2op]);

        N1=compute_MM(nep2,S,V);
        # Diagonalize S
        d,W = eigen(S)
        D = diagm(0 => d)
        V1 = V*W
        #
        N2=hcat(compute_Mlincomb(nep2,d[1],V1[:,1]),
                compute_Mlincomb(nep2,d[2],V1[:,2]),
                compute_Mlincomb(nep2,d[3],V1[:,3]))*inv(W)
        @test norm(N1-N2)<sqrt(eps())*100

    end

    @bench @testset "PEP" begin
        Random.seed!(99)
        A0=randn(5,5)
        A1=randn(5,5)
        A2=randn(5,5)
        A3=randn(5,5)
        # Check that PEP generates the
        # same values as an SPMF generated from the PEP
        nep0=PEP([A0,A1,A2,A3])
        nep1=SPMF_NEP(get_Av(nep0),get_fv(nep0))
        for  λ in [3,10,-3,-100]
            M0=compute_Mder(nep0,λ)
            M1=compute_Mder(nep1,λ)
            @test M0 ≈ M1

            M0=compute_Mder(nep0,λ,2)
            M1 = 2 .* A2 + (λ*6) .* A3
            @test M0 ≈ M1
        end

    end

    @onlybench @testset "SPMF benchmark" begin
        # To check performance of SPMF-compute_MM function
        large_benchmark = false
        if large_benchmark
            n_values = (5, 10, 50)
            p_mm_values = (5, 50)
            p_mder_values = (1, 3, 10, 20)
            m_terms = 500
        else
            n_values = (5, 10)
            p_mm_values = (5, 10, 15, 100)
            p_mder_values = (1, 3, 10)
            m_terms = 20
        end

        for Schur_fact in (true, false)
            for n in n_values
                # small number of terms.
                fv=[S->S, S->cos(S)]
                A0=randn(n,n);
                A1=randn(n,n);
                spmf1=SPMF_NEP([A0,A1],fv,Schur_fact = Schur_fact,Ftype=Float64);
                @testset "two-term SPMF (n=$n,schur=$Schur_fact): MM S-matrix p x p: p=$p" for p in p_mm_values
                    V=randn(size(spmf1,1),p);
                    S=randn(p,p);
                    Z=compute_MM(spmf1,S,V);
                    @test eltype(Z) == promote_type(eltype(S),eltype(V),eltype(A0))
                end
                @testset "two-term SPMF (n=$n,schur=$Schur_fact): Mder($p)" for p in p_mder_values
                    λ=3.0+1im;
                    MM=compute_Mder(spmf1,λ,p);
                    @test eltype(MM) == promote_type(typeof(λ),eltype(A0))
                end

                # large number of terms
                m=m_terms;
                fv=Vector{Function}(undef,m);
                Av=Vector{Matrix{Float64}}(undef,m);
                for k=1:m
                    fv[k] = S-> inv(sqrt(k)*I-S); # 1/(I√k-S)
                    Av[k] = randn(n,n)
                end

                spmf2=SPMF_NEP(Av,fv,Schur_fact = Schur_fact, Ftype=Float64);
                @testset "$m-term SPMF (n=$n,schur=$Schur_fact): MM S-matrix p x p: p=$p" for p in p_mm_values
                    p=5;
                    V=randn(size(spmf2,1),p);
                    S=randn(p,p);
                    Z=compute_MM(spmf2,S,V);
                    @test eltype(Z) == promote_type(eltype(S),eltype(V),eltype(A0))
                end
                # Disabled until it is optimized.
                #@testset "$m-term SPMF (n=$n,schur=$Schur_fact): Mder($p)" for p in p_mder_values
                #    λ=3.0+1im;
                #    MM=compute_Mder(spmf2,λ,p);
                #    @test eltype(MM) == promote_type(typeof(λ),eltype(A0))
                #end
            end
        end
    end

    @bench @testset "REP" begin
        Random.seed!(10)
        A0=randn(5,5)
        A1=randn(5,5)
        A2=randn(5,5)
        # Check that REP generates the
        # same values as an SPMF generated from the REP
        d1=1;
        d2=2;
        d3=3.3;
        c1=1.5;
        c2=2.4;
        c3=9.0;
        c4=-9.9
        nep0=REP([A0,A1],[c1; c2; c3; c4], [d1; d2; d3])
        nep1=SPMF_NEP(get_Av(nep0),get_fv(nep0))
        for  λ in [3,10,-3,-100]
            M0=compute_Mder(nep0,λ)
            M1=compute_Mder(nep1,λ)
            @test M0 ≈ M1
        end

    end

    @bench @testset "DEP" begin
        Random.seed!(88)
        A1=randn(5,5)
        A2=randn(5,5)
        τ1::Float64 = 1.5
        τ2::Float64 = 3.75
        # Check that DEP generates the
        # same values as an SPMF generated from the DEP
        nep0=DEP([A1,A2],[τ1, τ2])
        nep1=SPMF_NEP(get_Av(nep0),get_fv(nep0))
        for  λ in [3,10,-3,-100]
            M0=compute_Mder(nep0,λ)
            M1=compute_Mder(nep1,λ)
            @test M0 ≈ M1
            M2 = -λ*I + A1 .* exp(-τ1*λ) + A2 .* exp(-τ2*λ)
            @test M0 ≈ M2
        end
    end
end


"""
    AshPriors

Empirical Bayes priors that are used in the simulations of:
> Stephens, M., 2017. False discovery rates: a new deal. Biostatistics, 18(2), pp.275-294.
"""
const AshPriors = Dict(
    :Spiky => MixtureModel(
        [
            Normal(0, 0.25)
            Normal(0, 0.5)
            Normal(0, 1)
            Normal(0, 2)
        ],
        [0.4; 0.2; 0.2; 0.2],
    ),
    :NearNormal => MixtureModel([Normal(0, 1); Normal(0, 2)], [1 / 3; 2 / 3]),
    :FlatTop => MixtureModel([Normal(x, 0.5) for x = -1.5:0.5:1.5]),
    :Skew => MixtureModel(
        [Normal(-2, 2), Normal(-1, 1.5), Normal(0, 1), Normal(1, 1)],
        [1 / 4; 1 / 4; 1 / 3; 1 / 6],
    ),
    :BigNormal => Normal(0, 4),
    :Bimodal => MixtureModel([Normal(-2, 1); Normal(2, 1)], [0.5; 0.5]),
)

"""
    MarronWandGaussianMixtures

Flexible Gaussian Mixture distributions described in

> Marron, J. Steve, and Matt P. Wand. Exact mean integrated squared error.
The Annals of Statistics (1992): 712-736.
"""
const MarronWandGaussianMixtures = Dict(
    :KurtoticUnimodal => MixtureModel([Normal(0,1), Normal(0, 0.1)],
                                          [2/3, 1/3]),
    :SkewedUnimodal => MixtureModel([Normal(0,1), Normal(0.5, 2/3), Normal(13/12, 5/9)],
                                        [1/5, 1/5, 3/5]),
    :Bimodal => MixtureModel([Normal(-1.0, 2/3), Normal(1.0, 2/3)]),
    :SeparatedBimodal => MixtureModel([Normal(-1.5, 0.5), Normal(1.5, 0.5)]),
    :SkewedBimodal =>  MixtureModel([Normal(0,1), Normal(3/2, 1/3)],
                                    [0.75, 0.25]),
    :Outlier => MixtureModel([Normal(0,1), Normal(0, 1/10)], [1/10, 9/10])
)

"""
    IWPriors

Empirical Bayes priors that are used in the simulations of:
> Ignatiadis, N. and Wager, S., 2019. Bias-aware confidence intervals for empirical Bayes analysis. arXiv preprint arXiv:1902.02774.
"""
const IWPriors = Dict(
    :Unimodal => MixtureModel([ Normal(-0.2,.2), Normal(0,0.9)],[0.7, 0.3]),
    :Bimodal => MixtureModel([Normal(-1.5,.2), Normal(1.5, .2)]),
    :NegSpiky => MixtureModel([Normal(-0.25, 0.25), Normal(0, 1.0)], [0.8, 0.2]))

"""
    EfronPriors

Empirical Bayes priors that are used in the simulations of:
> Efron, B., 2016. Empirical Bayes deconvolution estimates. Biometrika, 103(1), pp.1-20.
"""
const EfronPriors = Dict(
    :UniformNormal => MixtureModel([Uniform(-3.0,3.0), Normal(0.0, 0.5)], [1/8, 7/8]))


"""
    dn = datenum_gregjulian(year,month,day,gregorian::Bool)

Days since 1858-11-17 according to the Gregorian (`gregorian` is `true`) or
Julian Calendar (`gregorian` is `false`) based on the Algorithm of
Jean Meeus [1] modified to handle dates prior to 100 BC.

The year -1, correspond to 1 BC. The year 0 does not exist in the
Gregorian or Julian Calendar.

[1] Meeus, Jean (1998) Astronomical Algorithms (2nd Edition).
Willmann-Bell,  Virginia. p. 63
"""
function datenum_gregjulian(year,month,day,gregorian::Bool,has_year_zero = false)
    # turn year equal to -1 (1 BC) into year = 0
    if (year < 0) && !has_year_zero
        year = year+1
    end

    if gregorian
        # bring year in range of 1601 to 2000
        ncycles = (2000 - year) ÷ 400
        year = year + 400 * ncycles
        # 146_097 = days in 400 years = 365.2425 * 400
        return datenum_ac(year,month,day,gregorian) - ncycles*146_097
    else
        return datenum_ac(year,month,day,gregorian)
    end

end

# Meeus, Jean (1998) Astronomical Algorithms (2nd Edition). Willmann-Bell,  Virginia. p. 63
# However, the algorithm does not work for -100:03:01 and before in
# the proleptic Gregorian Calendar

function datenum_ac(year,month,day,gregorian::Bool)

    if month <= 2
        # if the date is January or February, it is considered
        # the 13rth or 14th month of the preceeding year
        year = year - 1
        month = month + 12
    end

    B =
        if gregorian
            A = year ÷ 100
            2 - A + A ÷ 4
        else
            0
        end

    # benchmark shows that it is 40% faster replacing
    # trunc(Int64,365.25 * (year + 4716))
    # by
    # (1461 * (year + 4716)) ÷ 4
    #
    # and other floating point divisions

    # Z is the Julian Day plus 0.5
    # 1461/4 is 365.25
    # 153/5 is 30.6

    # why 153/5 (or 30.6001 ?)
    # month+1 varies between 4 (March), 5 (April), .. 14 (December),
    # 15 (January), 16 (February)

    # cm = 153 * (4:16) ÷ 5; cm[2:end]-cm[1:end-1]
    #
    # length of each month
    # --------------------
    # 31  March
    # 30  April
    # 31  May
    # 30  June
    # 31  July
    # 31  August
    # 30  September
    # 31  October
    # 30  November
    # 31  December
    # 31  January
    # 30  February (wrong, but not used, since it is the last month)

    Z = (1461 * (year + 4716)) ÷ 4 + (153 * (month+1)) ÷ 5 + day + B - 2401525
    # Modified Julan Day
    return Z + DATENUM_OFFSET
end

"""
    year, month, day = datetuple_gregjulian(Z::Integer,gregorian::Bool)

Compute year, month and day from Z which is the Modified Julian Day
for the Gregorian (true) or Julian (false) calendar.
using the algorithm of Meeus (1998) modified to handle dates prior to 300 AC.

For example:
Z = 0 for the 1858 November 17 00:00:00

Algorithm:

Meeus, Jean (1998) Astronomical Algorithms (2nd Edition). Willmann-Bell,
Virginia. p. 63
"""
function datetuple_gregjulian(Z0::T,gregorian::Bool,has_year_zero = false) where T

    # promote to at least Int64
    Z = promote_type(T, Int64)(Z0)

    # Z is Julian Day plus 0.5
    Z = Z + (2_400_001 - DATENUM_OFFSET)

    A = Z

    if gregorian
        # 1867216.25 - 0.5 corresponds to the date 400-02-29T18:00:00
        # lets magic happen
        #α = trunc(Int64, (Z - 1867_216.25)/36524.25)
        # 400 years = 146097 days = (400 * 365 + 100 - 4 + 1) days
        # α number of centuries since 400-02-29
        α = (4*Z - 7468865) ÷ 146097

        β = Z + (-7468865 + 146097) ÷ 4
        if (β <= 0) && (β % 146097 == 0)
            # correction for 300-03-01 AC, 101-03-01 BC, 501-03-01 BC, ...
            A += 1
        end

        # +α: add leap days for 1700, 1800, 1900, 2000, 2100,
        # -(α ÷ 4): remove leap days for 2000, 2400, ... (already included)
        # so that Julian and Gregorian calendar coincide
        A += 1 + α - (α ÷ 4)
    end

    # even more magic...
    B = A + 1524
    #C = trunc(Int64, (B - 122.1) / 365.25)
    C = (20*B - 2442) ÷ 7305
    #D = trunc(Int64, 365.25 * C)
    # 1461 = 3*365 + 366
    D = 1461 * C ÷ 4
    #E = trunc(Int64, (B-D)/30.6001)
    E = (10000 * (B-D)) ÷ 306001

    #day = B - D - trunc(Int64,30.6001 * E)
    day = B - D - (306001 * E) ÷ 10000

    month = (E < 14 ? E-1 : E-13)
    y = (month > 2 ? C - 4716 : C - 4715)

    # turn year 0 into year -1 (1 BC)
    if (y <= 0) && !has_year_zero
        y = y-1
    end
    return y,month,day
end

datetuple_ymd(::Type{DateTimeProlepticGregorian},Z::Number) =
    datetuple_gregjulian(Z,true,_hasyear0(DateTimeProlepticGregorian))

datetuple_ymd(::Type{DateTimeJulian},Z::Number) =
    datetuple_gregjulian(Z,false,_hasyear0(DateTimeJulian))

datetuple_ymd(::Type{DateTimeStandard},Z::Number) =
    datetuple_gregjulian(Z,Z >= DN_GREGORIAN_CALENDAR,_hasyear0(DateTimeStandard))

datenum(::Type{DateTimeProlepticGregorian},y,m,d) =
    datenum_gregjulian(y,m,d,true,_hasyear0(DateTimeProlepticGregorian))

datenum(::Type{DateTimeJulian},y,m,d) =
    datenum_gregjulian(y,m,d,false,_hasyear0(DateTimeJulian))

datenum(::Type{DateTimeStandard},y,m,d) =
    datenum_gregjulian(y,m,d,(y,m,d) >= GREGORIAN_CALENDAR,_hasyear0(DateTimeStandard))


include("euler/euler.jl")
using .Numbers: is_s_number
using BenchmarkTools
BenchmarkTools.DEFAULT_PARAMETERS.samples = 100

function compute(n::Int)::Int
    result = 0
    for i ∈ 2:n if is_s_number(i, i * i) result += i * i end end
    return result
end

compute(100)

compute(1_000_000)

@benchmark compute(1_000_000)


# ---
# title: 90. Subsets II
# id: problem90
# author: Tian Jun
# date: 2020-10-31
# difficulty: Medium
# categories: Array, Backtracking
# link: <https://leetcode.com/problems/subsets-ii/description/>
# hidden: true
# ---
#
# Given a collection of integers that might contain duplicates, **_nums_** ,
# return all possible subsets (the power set).
#
# **Note:** The solution set must not contain duplicate subsets.
#
# **Example:**
#
#
#
#     Input: [1,2,2]
#     Output:
#     [
#       [2],
#       [1],
#       [1,2,2],
#       [2,2],
#       [1,2],
#       []
#     ]
#
#
#
## @lc code=start
using LeetCode

function subsets_with_dup(nums::Vector{Int})::Vector{Vector{Int}}
    sort!(nums)
    res = Vector{Int}[Int[]]

    n = 0
    for (i, x) in enumerate(nums)
        if i == 1
            n = 1
        elseif nums[i] != nums[i - 1]
            n = length(res)
            ## n is used to record how many sets to skip in case nums[i+1] == nums[i]
        end

        if n == length(res)
            for j in 1:n
                push!(res, [res[j]..., nums[i]])
            end
        else
            for j in (length(res) - n + 1):length(res)
                push!(res, [res[j]..., nums[i]])
            end
        end
    end
    return res
end
## @lc code=end


#=
1. Construct N parallel cuboids of size 10000 × 1 × 100 spaced one unit apart in y -direction as object W .
2. Construct N parallel cuboids of size 1 × 10000 × 100 spaced one unit apart in x -direction as object W ′ .
3. Align W and W ′ at their lower front left corner.
4. Move W ′ along the z-axis for fifty units.
5. Measure time for W ∪ W′.
=#

using ViewerGL; GL = ViewerGL
using LinearAlgebraicRepresentation
Lar = LinearAlgebraicRepresentation;

n = 30
V1,(_,EV1,FV1,_) = Lar.cuboid([1,2n,10],true);
cuboid1 = (V1,FV1,EV1);
V2,(_,EV2,FV2,_) = Lar.cuboid([2n,1,10],true);
cuboid2 = (V2,FV2,EV2);

# @time let
	group1 = Lar.struct2lar(Lar.Struct(repeat([cuboid1, Lar.t(2,0,0)],outer=n)));
	group2 = Lar.struct2lar(Lar.Struct(repeat([cuboid2, Lar.t(0,2,0)],outer=n)));
	assembly = Lar.Struct([group1, Lar.t(0,0,5), group2]);

	V,FV,EV = Lar.struct2lar(assembly);
	cop_EV = convert(Lar.ChainOp, Lar.coboundary_0(EV::Lar.Cells));
	cop_FE = Lar.coboundary_1(V, FV::Lar.Cells, EV::Lar.Cells); ## TODO: debug
	W = convert(Lar.Points, V');
	V, copEV, copFE, copCF = Lar.space_arrangement( W, cop_EV, cop_FE);
	W = convert(Lar.Points, V');
	V,CVs,FVs,EVs = Lar.pols2tria(W, copEV, copFE, copCF);
# end;

GL.VIEW( GL.GLExplode(V,CVs[2:end],5,5,5,99,0.5) );
GL.VIEW( GL.GLExplode(V,CVs[1:1],5,5,5,99,0.5) );


function PolynomialFunctionLagrangeForm(arg0::Vector{jdouble}, arg1::Vector{jdouble})
    return PolynomialFunctionLagrangeForm((Vector{jdouble}, Vector{jdouble}), arg0, arg1)
end

function degree(obj::PolynomialFunctionLagrangeForm)
    return jcall(obj, "degree", jint, ())
end

function evaluate(::Type{PolynomialFunctionLagrangeForm}, arg0::Vector{jdouble}, arg1::Vector{jdouble}, arg2::jdouble)
    return jcall(PolynomialFunctionLagrangeForm, "evaluate", jdouble, (Vector{jdouble}, Vector{jdouble}, jdouble), arg0, arg1, arg2)
end

function get_coefficients(obj::PolynomialFunctionLagrangeForm)
    return jcall(obj, "getCoefficients", Vector{jdouble}, ())
end

function get_interpolating_points(obj::PolynomialFunctionLagrangeForm)
    return jcall(obj, "getInterpolatingPoints", Vector{jdouble}, ())
end

function get_interpolating_values(obj::PolynomialFunctionLagrangeForm)
    return jcall(obj, "getInterpolatingValues", Vector{jdouble}, ())
end

function value(obj::PolynomialFunctionLagrangeForm, arg0::jdouble)
    return jcall(obj, "value", jdouble, (jdouble,), arg0)
end

function verify_interpolation_array(::Type{PolynomialFunctionLagrangeForm}, arg0::Vector{jdouble}, arg1::Vector{jdouble}, arg2::jboolean)
    return jcall(PolynomialFunctionLagrangeForm, "verifyInterpolationArray", jboolean, (Vector{jdouble}, Vector{jdouble}, jboolean), arg0, arg1, arg2)
end


using MPI
using ScaLapack
using ScaLapack: BLACS, ScaLapackLite

# for check correction
using LinearAlgebra

# for DEBUG
using Printf
const DEBUG = true
function prtf(x)
    if x < 0; s = @sprintf("%.7f", x);
    else s = @sprintf(" %.7f", x); end;
end

# problem size
const nrows = 20
const ncols = 20
const nrows_block = 6
const ncols_block = 6
const nprocrows = 2
const nproccols = 2

# test type
const testtype = Float64

# finalizer
function mpi_finalizer(comm)
    MPI.Finalize()
end

function mutiple_test(root, comm)

    rank = MPI.Comm_rank(comm)
    nproc = MPI.Comm_size(comm)
    MPI.Barrier(comm);

    # generate matrix
    if rank == 0
        A = Matrix{testtype}(undef, nrows, ncols)
        B = Matrix{testtype}(undef, nrows, ncols)
        for ia::Integer = 1 : nrows
            for ja::Integer = 1 : ncols
                A[ia, ja] = convert(Float64, ia+ja)
                B[ia, ja] = convert(Float64, ia*ja)
            end
        end
    else
        A = Matrix{testtype}(undef, 0, 0)
        B = Matrix{testtype}(undef, 0, 0)
    end
    MPI.Barrier(comm)

    if DEBUG
        if rank == root
            print("\ninput matrix:\n")
        end
        MPI.Barrier(comm)
        print("\nrank[$rank];\nA = \n$A\nB = \n$B\n")
    end
    MPI.Barrier(comm)

    # prepare ScaLapackLite params
    params = ScaLapackLite.ScaLapackLiteParams(nrows_block, ncols_block, nprocrows, nproccols, root)
    slm_A = ScaLapackLite.ScaLapackLiteMatrix(params, A)
    slm_B = ScaLapackLite.ScaLapackLiteMatrix(params, B)

    # perform C = A*B
    slm_C = slm_A * slm_B'
    if DEBUG
        if rank == root
            print("\noutput matrix:\n")
        end
        MPI.Barrier(comm)
        print("\nrank[$rank];\nC = \n$(slm_C.X)\n")
    end

    MPI.Barrier(comm)
    # clean up
    BLACS.exit()

end

function hessenberg_test(root, comm)
    rank = MPI.Comm_rank(comm)
    nproc = MPI.Comm_size(comm)
    MPI.Barrier(comm);

    A = Matrix{testtype}(undef, nrows, ncols)
    for ia::Integer = 1 : nrows
        for ja::Integer = 1 : ncols
            A[ia, ja] = convert(testtype, rand())
        end
    end
    params = ScaLapackLite.ScaLapackLiteParams(nrows_block, ncols_block, nprocrows, nproccols, root)
    slm_A = ScaLapackLite.ScaLapackLiteMatrix(params, A)

    slm_hA = ScaLapackLite.hessenberg(slm_A, true)
    print("[$rank] A: $(slm_hA.X)\n")
end

function schur_test(root, comm)
    rank = MPI.Comm_rank(comm)
    nproc = MPI.Comm_size(comm)
    MPI.Barrier(comm);

    A = Matrix{testtype}(undef, nrows, ncols)
    for ia::Integer = 1 : nrows
        for ja::Integer = 1 : ncols
            A[ia, ja] = convert(testtype, rand())
        end
    end
    params = ScaLapackLite.ScaLapackLiteParams(nrows_block, ncols_block, nprocrows, nproccols, root)
    slm_A = ScaLapackLite.ScaLapackLiteMatrix(params, A)

    if rank == root
        # reference: original matrix A
        A_ref = deepcopy(A)
        # reference: LAPACK routine
        (eig_ref,eigvec_ref)=LinearAlgebra.eigen(A)
    end

    # perform Schur decomposition and en-return T
    (eig, slm_Z, slm_T) = ScaLapackLite.schur(slm_A, true)

    # CHECK: recompose A from Schur vectors Z; A = Z * T * Z^H
    slm_A_ = slm_Z * slm_T * slm_Z'
    if rank == root
        frobenius_norm = norm(slm_A_.X-A_ref, 2)
    end

    if rank == root
        eidx=sortperm(real(eig))
        eidx_ref=sortperm(real(eig_ref))
        diff = [ abs(eig[eidx[j]]-eig_ref[eidx_ref[j]]) for j=1:nrows ]

        print("\n--- eigenvalues ---\n")
        wlines = "\nindex / eig(ScaLapackLite) / eig(LAPACK) / | eig(ScaLapackLite)-eig(LAPACK) |\n"
        for idx = 1:nrows
            wlines *= @sprintf("  %s / %s + i %s / %s + i %s / %s\n",
                          lpad(idx, 3, "0"),
                          prtf(real(eig[eidx[idx]])), prtf(imag(eig[eidx[idx]])),
                          prtf(real(eig_ref[eidx_ref[idx]])), prtf(imag(eig_ref[eidx_ref[idx]])),
                          prtf(diff[idx]))
        end
        wlines *= "\n"; print(wlines);

        print("\n--- Schur vectors / eigen vectors ---\n")

        wline = @sprintf("\nFrobenius norm: || Z * T * Z^H - A ||_{2} = %s\n", prtf(frobenius_norm))
        print(wline)

        for jdx = 1:ncols
            print("\n[$jdx-th vec]:\n")
            wlines = "\nindex / Schur vector (ScaLapackLite) / eig vector (LAPACK)\n"
            for idx = 1:nrows
                wlines *= @sprintf("  %s / %s + i %s / %s + i %s\n",
                            lpad(idx, 3, "0"),
                            prtf(real(slm_Z.X[idx, eidx[jdx]])),
                            prtf(imag(slm_Z.X[idx, eidx[jdx]])),
                            prtf(real(eigvec_ref[idx,eidx_ref[jdx]])),
                            prtf(imag(eigvec_ref[idx,eidx_ref[jdx]])))
            end
            wlines *= "\n"; print(wlines);
        end

    end

end

function eig_test(root, comm)
    rank = MPI.Comm_rank(comm)
    nproc = MPI.Comm_size(comm)
    MPI.Barrier(comm);

    A = Matrix{testtype}(undef, nrows, ncols)
    for ia::Integer = 1 : nrows
        for ja::Integer = 1 : ncols
            A[ia, ja] = convert(testtype, rand())
            # A[ia, ja] = convert(testtype, rand()+im*rand())
        end
    end
    A_copied = deepcopy(A)
    params = ScaLapackLite.ScaLapackLiteParams(nrows_block, ncols_block, nprocrows, nproccols, root)
    slm_A = ScaLapackLite.ScaLapackLiteMatrix(params, A)

    if rank == root
        # reference: LAPACK routine
        (eigvals_ref,eigvecs_ref)=LinearAlgebra.eigen(A)
    end

    # find eigenvalues and eigenvectors
    eigidx = Vector{Int64}([])
    (eigvals, eigvecs) = ScaLapackLite.eigen(slm_A, eigidx, false)

    testtype_e = testtype
    if testtype == Float32; testtype_e = ComplexF32;
    elseif testtype == Float64; testtype_e = ComplexF64; end

    if rank == root
        eidx = sortperm(real(eigvals))
        eidx_ref = sortperm(real(eigvals_ref))

        print("\n--- eigenvalues ---\n")
        wlines = "\nindex / eig(ScaLapackLite) / eig(LAPACK)\n"
        for idx = 1:nrows
            wlines *= @sprintf("  %s / %s + i %s / %s + i %s\n",
                          lpad(idx, 3, "0"),
                          prtf(real(eigvals[eidx[idx]])), prtf(imag(eigvals[eidx[idx]])),
                          prtf(real(eigvals_ref[eidx_ref[idx]])), prtf(imag(eigvals_ref[eidx_ref[idx]])))
        end
        wlines *= "\n"; print(wlines);

        print("\n--- eigv(ScaLapackLite) / eigv(LAPACK) ---\n")
        for jdx = 1:ncols

            # calc error
            x = Vector{testtype_e}(eigvecs[:,eidx[jdx]])
            x_ref = Vector{testtype_e}(eigvecs_ref[:,eidx_ref[jdx]])
            ej = eigvals[eidx[jdx]]
            ej_ref = eigvals_ref[eidx_ref[jdx]]
            error = norm(A_copied*x - ej*x, 2)
            error_ref = norm(A_copied*x_ref - ej_ref*x_ref, 2)
            wlines = @sprintf("\nlog10(|| Ax - λx ||)[ScaLapackLite] = %s\nlog10(|| Ax - λx ||)[LAPACK]        = %s\n",
                               prtf(log10(error)), prtf(log10(error_ref)))

            print("\n[$jdx-th vec]:\n")
            print(wlines)
            wlines = "\nindex / eigv(ScaLapackLite) / eigv(LAPACK)\n"
            for idx = 1:nrows
                wlines *= @sprintf("  %s / %s + i %s / %s + i %s\n",
                            lpad(idx, 3, "0"),
                            prtf(real(eigvecs[idx, eidx[jdx]])), prtf(imag(eigvecs[idx, eidx[jdx]])),
                            prtf(real(eigvecs_ref[idx, eidx_ref[jdx]])), prtf(imag(eigvecs_ref[idx, eidx_ref[jdx]])))
            end
            wlines *= "\n"; print(wlines);
        end

    end

end

function main()
    # MPI initilaize
    MPI.Init()
    comm = MPI.COMM_WORLD
    finalizer(mpi_finalizer, comm)
    # test
    # mutiple_test(0, comm)
    # hessenberg_test(0, comm)
    # schur_test(0, comm)
    eig_test(0, comm)
end

main()


"""
	isapprox(a::AbstractPoint2D, b::AbstractPoint2D) -> Bool

Test if the two points `a` and `b` are approximately equal in the `l1`-norm.
"""
function Base.isapprox(a::VoronoiDelaunay.AbstractPoint2D, b::VoronoiDelaunay.AbstractPoint2D)
	isapprox(getx(a), getx(b)) && isapprox(gety(a), gety(b))
end

"""
	contains(pts::AbstractPoints2D, p::AbstractPoint2D) -> Bool

Test if the point `p` is in the vector of points `pts`.
"""
function contains(pts::Vector{<:VoronoiDelaunay.AbstractPoint2D}, p::VoronoiDelaunay.AbstractPoint2D)
	for element in pts
		if isapprox(p, element)
			return true
		end
	end
	return false
end

"""
	dist_squared(p, q) -> Real

The `l2` distance squared between the points `p` and `q`.
"""
function dist_squared(p::VoronoiDelaunay.AbstractPoint2D, q::VoronoiDelaunay.AbstractPoint2D)
	(getx(p)-getx(q))^2 + (gety(p)-gety(q))^2
end

"""
	quadrant(p::AbstractPoint2D)

Get the quadrant number of `p`.

The quadrants are numbered in the usual way, starting with 1 in the upper
right and consecutively with positive orientation:

* 1 is the upper right
* 2 is the upper left
* 3 is the lower left
* 4 is the lower right
"""
function quadrant(p::VoronoiDelaunay.AbstractPoint2D)
	isleft = getx(p) < MIDDLEx
	islow = gety(p) < MIDDLEy

	if !islow && !isleft
		Q = 1
	elseif !islow && isleft
		Q = 2
	elseif islow && isleft
		Q = 3
	elseif islow && !isleft
		Q = 4
	end

	return Q
end

"""
	fit2boundingbox(x::Vector, y::Vector; ...) -> Points2D, Float, Float

Scale `x` and `y` to be in the GeometricalPredicates window [1,2]x[1,2].
Return a vector of `Point2D`'s and the scaling factors applied in each
dimension.

As an optional argument the bounding box of `x` and `y` can be changed
from the unit square.
"""
function fit2boundingbox(x::AbstractVector{Float64}, y::AbstractVector{Float64}, rw::Vector{Float64}=[0.0;1.0;0.0;1.0])
	(N = length(x)) == length(y) || throw(DimensionMismatch())

	RW_LEFT = rw[1]
	RW_LOWER = rw[3]
	minimum(x) >= RW_LEFT && maximum(x) <= rw[2] && minimum(y) >= RW_LOWER && maximum(y) <= rw[4] || throw(DomainError(x))

	SCALEX = rw[2] - RW_LEFT
	SCALEY = rw[4] - RW_LOWER

	pts = [IndexablePoint2D( LEFT + (x[n]-RW_LEFT)/SCALEX, LOWER + (y[n]-RW_LOWER)/SCALEY, n) for n = 1:N]

	return pts, SCALEX, SCALEY
end