using Test

# 2-category
############

A, B, C, D = Ob(FreeCategory2, :A, :B, :C, :D)
f, g, F, G = [ Hom(sym, A, B) for sym in [:f,:g,:F,:G] ]
h, k, H, K = [ Hom(sym, B, C) for sym in [:h,:k,:H,:K] ]

# Domains and codomains
α, β = Hom2(:α, f, g), Hom2(:β, g, h)
@test dom(α) == f
@test codom(α) == g
@test dom(dom(α)) == A
@test codom(dom(α)) == B
@test dom(compose(α,β)) == f
@test codom(compose(α,β)) == h
@test_throws SyntaxDomainError composeH(α,β)

α, β = Hom2(:α, f, g), Hom2(:β, h, k)
@test dom(composeH(α,β)) == compose(f,h)
@test codom(composeH(α,β)) == compose(g,k)

# Infix notation (Unicode)
α, β = Hom2(:α, f, g), Hom2(:β, g, h)
@test unicode(compose(f,h)) == "f⋅h"
@test unicode(compose(α,β)) == "α⋅β"

α, β = Hom2(:α, f, g), Hom2(:β, h, k)
@test unicode(composeH(α,β)) == "α*β"

# Double category
#################

A, B, C, D, X, Y = Ob(FreeDoubleCategory, :A, :B, :C, :D, :X, :Y)
f, g, h, k = HomH(:f, A, X), HomH(:g, B, Y), HomH(:h, X, C), HomH(:k, Y, D)
l, r, rr = HomV(:ϕ, A, B), HomV(:r, X, Y), HomV(:rr, C, D)
α, β = Hom2(:α, f, g, l, r), Hom2(:β, h, k, r, rr)
@test composeH(α, β) == α⋆β
αβ = composeH(α, β)
@test top(αβ) == composeH(f, h)
@test bottom(αβ) == composeH(g, k)

# Mmonoidal double category
###########################

A, B, C, D = Ob(FreeSymmetricMonoidalDoubleCategory, :A, :B, :C, :D)
E, F, G, H = Ob(FreeSymmetricMonoidalDoubleCategory, :E, :F, :G, :H)
t, b, l, r = HomH(:t, A, B), HomH(:b, C, D), HomV(:l, A, C), HomV(:r, B, D)
t′, b′, l′, r′ = HomH(:t′, E, F), HomH(:b′, G, H), HomV(:l′, E, G), HomV(:r′, F, H)
s = Hom2(:s, t, b, l, r)
s′ = Hom2(:s′, t′, b′, l′, r′)

# Domains and codomains
@test dom(otimes(t,b)) == otimes(dom(t),dom(b))
@test codom(otimes(t,b)) == otimes(codom(t),codom(b))
@test dom(otimes(l,r)) == otimes(dom(l),dom(r))
@test codom(otimes(l,r)) == otimes(codom(l),codom(r))
@test top(otimes(s,s′)) == otimes(top(s),top(s′))
@test bottom(otimes(s,s′)) == otimes(bottom(s),bottom(s′))

@test dom(braidV(A,B)) == otimes(A,B)
@test codom(braidV(A,B)) == otimes(B,A)
@test top(braidH(t,b)) == otimes(t,b)
@test bottom(braidH(t,b)) == otimes(b,t)
@test left(braidH(t,b)) == braidV(A,C)
@test right(braidH(t,b)) == braidV(B,D)
@test σV(A, B) == braidV(A,B)
@test σH(t, b) == braidH(t,b)

# Associativity and unit
I = munit(FreeSymmetricMonoidalDoubleCategory.Ob)
@test otimes(A,I) == A
@test otimes(I,A) == A
@test otimes(otimes(A,B),A) == otimes(A,otimes(B,A))
@test otimes(otimes(t,b),t) == otimes(t,otimes(b,t))


import Base.*
import Base.convert
import Base.promote_rule

# Hierarchia typow

function type_hierarchy(t)
    if(t != Any)
        type_hierarchy(supertype(t))
        print(" -> ")
    end

    print(t)
end

type_hierarchy(Float32)

# Struktura

struct G{N} <: Integer
    x::Int

    function G(m)
        if(m >= N)
            m = m % N
        end

        if(m < 0 || gcd(m, N) != 1)
            throw(DomainError())
        end

        new(m)
    end
end

# Mnozenie

function *{N}(a::G{N}, b::G{N})
    return G{N}((a.x * b.x) % N)
end

function *{N}(a::G{N}, b::Integer)
    return a * G{N}(b)
end

function *{N}(a::Integer, b::G{N})
    return G{N}(a) * b
end

# Konwersja

function convert{N}(::Type{G{N}}, v::Int64)
    return G{N}(v)
end

function convert{N}(::Type{Int64}, v::G{N})
    return v.x
end

# Promocja

promote_rule(::Type{G{N}}, ::Type{T}) where {T <: Integer, N} = T

# Potegowanie

function ^{N}(a::G{N}, b::Integer)
    ret::G{N} = 1
    for i = 1:b
       ret = ret * a
    end
    return ret
end

# Okres

function period{N}(a::G{N})
    for i = 1:N
        if a ^ i == 1
            return i
        end
    end

    return -1
end

# Element odwrotny

function inverse{N}(a::G{N})
    for i = 1:N
        try
            if a * i == 1
                return G{N}(i)
            end
        end
    end

    throw(DomainError())
end

# Moc zbioru elementów

function card{N}(::Type{G{N}})
    count = 0

    for i = 0:N
        try
            G{N}(i)
            count += 1
        end
    end

    return count
end

# Testowanie

println(G{13}(7) * G{13}(3))

println(promote_type(G{5}, Int8))

println(G{7}(5)^5)

println(period(G{1023}(1022)))

println(inverse(G{1023}(1019)))

println(card(G{1029}))

# RSA

N = 55
c = G{N}(17)
b = G{N}(4)

r = period(b)
println(r)

d = inverse(c)
println(d)

a = b ^ d
println(a)

println(b, " ", (a ^ c))


## Mass segregation computation in stellar clusters.
## References : Cartwright et al. 2004, 2009 ; Allison et al. 2009 ; Olczak et al. 2011 ; Yu et al. 2011
##

## Create a graph with the selected stars
## ind: subset to select the x,y
## dtyp= "2d" or "3d"
##

##!! WARNING !! 2d is for the projection YZ since X is the distance with GAIA
##
function mst_graph(ind,x,y,z=[] , dtyp="2d")
    nxy= length(ind)
    if dtyp == "2d"
        A= Array{Float64}(undef,2,nxy)
    elseif dtyp == "3d"
       A= Array{Float64}(undef,3,nxy)
    end

    for i in 1:nxy
        if dtyp == "2d"
            A[:,i]= [y[ind[i]] z[ind[i]]]
        elseif dtyp == "3d"
            A[:,i]= [x[ind[i]] y[ind[i]] z[ind[i]]]
        end
    end

    d= Euclidean()
    p= pairwise(d, A, A, dims=2)

    src= Array{Int}(undef,0)
    dst= Array{Int}(undef,0)
    wgt= Array{Float64}(undef,0)

    for i in 1:nxy
        for j in 1:nxy
            if i != j
                push!(src,i)
                push!(dst,j)
                push!(wgt,p[i,j])
            end
        end
    end

    G= SimpleWeightedGraph(src, dst, wgt)
    kr= kruskal_mst(G, G.weights)

    return(kr)
end

## Computation of the λMST using either arithmetic or geometric average (Olczak et al. 2011)
##
## dtype= "ari" or "geo"
function lambda_mst(edges, dtyp="ari")
    nedg= length(edges)
    ltot= 0

    for w in edges
        if dtyp== "ari"
            ltot+= w.weight
        elseif dtyp== "geo"
            ltot+= log(w.weight)
        end
    end

    if dtyp== "ari"
        λmst= ltot/nedg
    elseif dtyp== "geo"
        λmst= exp(ltot/nedg)
    end
    return(λmst)
end

## estimate the sample size for the stats given n and N for the massive and total sample
## p= 99%
## Olczak et al. 2011)
function sample_size(n, N, p=0.99)
    k= log(1-p)/log(1-n/N)
    return(convert(Int,ceil(k)))
end

## Compute the mass segregation metric comparing two population set (2nd is random)
##
## nrandom: number of random set (default=1)
##
function kappa_ms(indMass, x, y , z, nrandom=1, dtyp="2d", daver= "geo")
    npop= length(indMass)
    ntot= length(x)

    ## Massive stars
    krMass= mst_graph(indMass, x,y,z , dtyp)
    λMass= lambda_mst(krMass,daver)
    ## Random set
    λArr= []
    κArr= []
    for i in 1:nrandom
        perm= randperm(ntot)
        indRef= perm[1:npop]
        krRef= mst_graph(indRef, x,y,z, dtyp)
        λRef= lambda_mst(krRef,daver)
        push!(λArr,λRef)
        push!(κArr, log(λRef/λMass))
    end
    λMean= mean(λArr)
    σκ= std(κArr)

    ## ratio (log) of the reference over massive ones
    κms= log(λMean/λMass)

    return(κms , σκ)
end

## Select the N percentile most massive stars (brighter)
##
## reverse:true -> light stars
function select_massivestars(mag, percentile=15 , reverse=false)
    nstar= length(mag)
    isort= sortperm(mag, rev=reverse)
    nselect= convert(Int, floor(nstar*percentile/100))

    return(isort[1:nselect])
end

## return the MS kappa and kappa_err
## IN
## mag: magnitude array
## X,Y,Z: positions array
## perc: percentile of massive stars
## dtyp: "3d" or "2d"
## daver: "geo" or "ari"
## reverse: false:massive, true:light - stars
##
function get_kappaMS(mag, X, Y, Z, perc, dtyp="3d", daver="geo", reverse=false)
    imass= select_massivestars(mag, perc, reverse)
    ns= sample_size(length(imass), length(X))
    κms, κms_err= kappa_ms(imass, X, Y, Z, ns, dtyp, daver)
    return(κms, κms_err)
end

#####################
## Compute the Q ratio structure to measure the clustering of the OC.
## See Cartwright et al. (2004)
## directly adapted to 3D
##

### !!!!!!! Normalization to be checked on λ , Cartwright & 2004
function get_Q(X, Y , Z)
    nxyz= length(X)
    A= Array{Float64}(undef,3,nxyz)
    for i in 1:nxyz
            A[:,i]= [X[i] Y[i] Z[i]]
    end

    d= Euclidean()
    p= pairwise(d, A, A, dims=2)

    src= Array{Int}(undef,0)
    dst= Array{Int}(undef,0)
    wgt= Array{Float64}(undef,0)

    for i in 1:nxyz
        for j in 1:nxyz
            if i != j
                push!(src,i)
                push!(dst,j)
                push!(wgt,p[i,j])
            end
        end
    end

    G= SimpleWeightedGraph(src, dst, wgt)
    kr= kruskal_mst(G, G.weights)

    ## compute the mean (arithmtic) edge of MST and of star separation. We do not apply other normalization
    λmst= 0
    for w in kr
        λmst+= w.weight
    end
    ## not the original normalization of Cartwright!
    k= (4π / 3 / nxyz )^(1/3)
    λmst /= k*length(kr)

    npair= nxyz*(nxyz-1)/2
    sbar= sum(p) / npair

    Q= λmst / sbar
    return(Q)
end


@testset "Jacobi identity" begin
  @test A × (B × C) ≈ -(B × (C × A) + C × (A × B))
end


using ParameterizedFunctions, OrdinaryDiffEq, Test

f = @ode_def begin
    dGut    = -Ka*Gut
    dCent   =  Ka*Gut - CL*(Cent/Vc) -Q*(Cent/Vc) + Q*(Periph/Vp)
    dPeriph =  Q*(Cent/Vc)  - Q*(Periph/Vp)
    dResp   =  Kin*(1-(IMAX*(Cent/Vc)^γ/(IC50^γ+(Cent/Vc)^γ)))  - Kout*Resp
end Ka CL Vc Q Vp Kin Kout IC50 IMAX γ

p = (Ka = 2.769674683505292, CL = 0.8315548845614537, Vc = 45.87339297906569,
      Q = 1.4544520084387496, Vp = 9.460961128900388, Kin = 6.360418843010778,
      Kout = 3.524234094210465, IC50 = 0.4483840673123028, IMAX = 1.0, γ = 2.0,)

u0 = [0.35793622925128044,
      0.7123008259214687,
      0.10685666303615937,
      0.00]
prob = ODEProblem(f, u0, (0,26.), p)
sol = solve(prob, AutoTsit5(Rodas5(), dtfac=2))

@test sol([21.2])[1][4] ≈ 1.803 atol = 1e-2

#=
using Plots
plot(sol, vars=4, lab="dtfac=1")
sol = solve(prob, AutoTsit5(Rodas5(), dtfac=2))
plt1 = plot!(sol, vars=4, lab="dtfac=2")
plt2 = plot(sol.t, sol.alg_choice, lab="alg_choice")
plot(plt1, plt2, layout=(2,1))
plot!(;title="AutoTsit5(Rodas5())",dpi=200)
=#


module FlowDiagnostics

export RichardsonNumber, RossbyNumber
export ErtelPotentialVorticity, ThermalWindPotentialVorticity
export IsotropicBuoyancyMixingRate, AnisotropicBuoyancyMixingRate
export IsotropicTracerVarianceDissipationRate, AnisotropicTracerVarianceDissipationRate

using Oceananigans
using Oceananigans.Operators
using Oceananigans.AbstractOperations
using Oceananigans.AbstractOperations: KernelFunctionOperation
using Oceananigans.Grids: Center, Face

# Some useful operators
@inline fψ²(i, j, k, grid, f, ψ) = @inbounds f(i, j, k, grid, ψ)^2

function RichardsonNumber(model; N²_bg=0, dUdz_bg=0, dVdz_bg=0)
    u, v, w = model.velocities
    b = model.tracers.b

    dBdz_tot = ∂z(b) + N²_bg
    dUdz_tot = ∂z(u) + dUdz_bg
    dVdz_tot = ∂z(v) + dVdz_bg

    return dBdz_tot / (dUdz_tot^2 + dVdz_tot^2)
end

function RossbyNumber(model; dUdy_bg=0, dVdx_bg=0, f=nothing)
    u, v, w = model.velocities
    if f==nothing
        f = model.coriolis.f
    end

    dUdy_tot = ∂y(u) + dUdy_bg
    dVdx_tot = ∂x(v) + dVdx_bg

    return (dVdx_tot - dUdy_tot) / f
end

#++++ Potential vorticity
function potential_vorticity_in_thermal_wind_fff(i, j, k, grid, u, v, b, f)

    dVdx =  ℑzᵃᵃᶠ(i, j, k, grid, ∂xᶠᶠᵃ, v) # F, F, C → F, F, F
    dUdy =  ℑzᵃᵃᶠ(i, j, k, grid, ∂yᶠᶠᵃ, u) # F, F, C → F, F, F
    dbdz = ℑxyᶠᶠᵃ(i, j, k, grid, ∂zᵃᵃᶠ, b) # C, C, F → F, F, F

    pv_barot = (f + dVdx - dUdy) * dbdz

    dUdz = ℑyᵃᶠᵃ(i, j, k, grid, ∂zᵃᵃᶠ, u) # F, C, F → F, F, F
    dVdz = ℑxᶠᵃᵃ(i, j, k, grid, ∂zᵃᵃᶠ, v) # C, F, F → F, F, F

    pv_baroc = -f*(dUdz^2 + dVdz^2)

    return pv_barot + pv_baroc
end

function ThermalWindPotentialVorticity(model; f=nothing)
    u, v, w = model.velocities
    b = model.tracers.b
    if f==nothing
        f = model.coriolis.f
    end
    return KernelFunctionOperation{Face, Face, Face}(potential_vorticity_in_thermal_wind_fff, model.grid;
                                                     computed_dependencies=(u, v, w, b), parameters=f)
end

function ertel_potential_vorticity_fff(i, j, k, grid, u, v, w, b, params)

    dWdy =  ℑxᶠᵃᵃ(i, j, k, grid, ∂yᶜᶠᵃ, w) # C, C, F  → C, F, F  → F, F, F
    dVdz =  ℑxᶠᵃᵃ(i, j, k, grid, ∂zᵃᵃᶠ, v) # C, F, C  → C, F, F  → F, F, F
    dbdx = ℑyzᵃᶠᶠ(i, j, k, grid, ∂xᶠᶜᵃ, b) # C, C, C  → F, C, C  → F, F, F
    pv_x = (params.fx + dWdy - dVdz) * dbdx # F, F, F

    dUdz =  ℑyᵃᶠᵃ(i, j, k, grid, ∂zᵃᵃᶠ, u) # F, C, C  → F, C, F → F, F, F
    dWdx =  ℑyᵃᶠᵃ(i, j, k, grid, ∂xᶠᶜᵃ, w) # C, C, F  → F, C, F → F, F, F
    dbdy = ℑxzᶠᵃᶠ(i, j, k, grid, ∂yᶜᶠᵃ, b) # C, C, C  → C, F, C → F, F, F
    pv_y = (params.fy + dUdz - dWdx) * dbdy # F, F, F

    dVdx =  ℑzᵃᵃᶠ(i, j, k, grid, ∂xᶠᶠᵃ, v) # C, F, C  → F, F, C → F, F, F
    dUdy =  ℑzᵃᵃᶠ(i, j, k, grid, ∂yᶠᶠᵃ, u) # F, C, C  → F, F, C → F, F, F
    dbdz = ℑxyᶠᶠᵃ(i, j, k, grid, ∂zᵃᵃᶠ, b) # C, C, C  → C, C, F → F, F, F
    pv_z = (params.fz + dVdx - dUdy) * dbdz

    return pv_x + pv_y + pv_z
end

function ErtelPotentialVorticity(model; location = (Face, Face, Face))
    if location != (Face, Face, Face)
        throw(ArgumentError("ErtelPotentialVorticity only implemented at location (Face, Face, Face) for now."))
    end

    u, v, w = model.velocities
    if ~(model.background_fields.velocities.u isa Oceananigans.Fields.ZeroField)
        u += model.background_fields.velocities.u
    end
    if ~(model.background_fields.velocities.v isa Oceananigans.Fields.ZeroField)
        v += model.background_fields.velocities.v
    end
    if ~(model.background_fields.velocities.w isa Oceananigans.Fields.ZeroField)
        w += model.background_fields.velocities.w
    end

    b = model.tracers.b
    if ~(model.background_fields.tracers.b isa Oceananigans.Fields.ZeroField)
        b += model.background_fields.tracers.b
    end

    coriolis = model.coriolis
    if coriolis isa FPlane
        fx = fy = fz = model.coriolis.f
    elseif coriolis isa ConstantCartesianCoriolis
        fx = coriolis.fx
        fy = coriolis.fy
        fz = coriolis.fz
    else
        throw(ArgumentError("ErtelPotentialVorticity only implemented for FPlane and ConstantCartesianCoriolis"))
    end

    return KernelFunctionOperation{Face, Face, Face}(ertel_potential_vorticity_fff, model.grid;
                                                     computed_dependencies=(u, v, w, b), parameters=(; fx, fy, fz))
end
#----

#+++++ Tracer variance dissipation
function isotropic_tracer_variance_dissipation_rate_ccc(i, j, k, grid, b, κᵇ)
    dbdx² = ℑxᶜᵃᵃ(i, j, k, grid, fψ², ∂xᶠᵃᵃ, b) # C, C, C  → F, C, C  → C, C, C
    dbdy² = ℑyᵃᶜᵃ(i, j, k, grid, fψ², ∂yᵃᶠᵃ, b) # C, C, C  → C, F, C  → C, C, C
    dbdz² = ℑzᵃᵃᶜ(i, j, k, grid, fψ², ∂zᵃᵃᶠ, b) # C, C, C  → C, C, F  → C, C, C

    return 2 * κᵇ[i,j,k] * (dbdx² + dbdy² + dbdz²)
end
function IsotropicTracerVarianceDissipationRate(model, b, κᵇ; location = (Center, Center, Center))
    if location == (Center, Center, Center)
        return KernelFunctionOperation{Center, Center, Center}(isotropic_tracer_variance_dissipation_rate_ccc, model.grid;
                                       computed_dependencies=(b, κᵇ))
    else
        throw(Exception)
    end
end

function anisotropic_tracer_variance_dissipation_rate_ccc(i, j, k, grid, b, params)
    dbdx² = ℑxᶜᵃᵃ(i, j, k, grid, fψ², ∂xᶠᵃᵃ, b) # C, C, C  → F, C, C  → C, C, C
    dbdy² = ℑyᵃᶜᵃ(i, j, k, grid, fψ², ∂yᵃᶠᵃ, b) # C, C, C  → C, F, C  → C, C, C
    dbdz² = ℑzᵃᵃᶜ(i, j, k, grid, fψ², ∂zᵃᵃᶠ, b) # C, C, C  → C, C, F  → C, C, C

    return 2 * (params.κx*dbdx² + params.κy*dbdy² + params.κz*dbdz²)
end
function AnisotropicTracerVarianceDissipationRate(model, b, κx, κy, κz; location = (Center, Center, Center))
    if location == (Center, Center, Center)
        return KernelFunctionOperation{Center, Center, Center}(anisotropic_tracer_variance_dissipation_rate_ccc, model.grid;
                                       computed_dependencies=(b,),
                                       parameters=(κx=κx, κy=κy, κz=κz),)
    else
        throw(Exception)
    end
end
#-----

end # module


import LightGraphs as LG
import MetaGraphs

# Distribution on directed forests, where the probability mass assigned to trees with the final node
# as the root is TREE_PROBABILITY_MASS (uniform over all directed forests that satisfy this property)
# and then the remainder of probability mass (1.0 - TREE_PROBABILITY_MASS) is spread uniformly over all
# directed forests that do not satisfy the property.

TREE_PROBABILITY_MASS = (1.0 - 1e-10)

struct TreePreferenceDiForest <: Gen.Distribution{LG.SimpleDiGraph} end
const tree_preference_diforest = TreePreferenceDiForest()
(::TreePreferenceDiForest)(n) = Gen.random(tree_preference_diforest, n)

function Gen.random(::TreePreferenceDiForest, n::Int)
    graph_with_edges(n,[])
end

# The number of directed forests on n nodes is (n+1) ^ (n-1) as given by Cayley's formula.
function Gen.logpdf(::TreePreferenceDiForest, diforest::LG.SimpleDiGraph, n::Int)
    n != nv(diforest) && return -Inf

    num_directed_forests = (n+1) ^ (n-1)
    num_trees_with_root_last_node = (n ^ (n-2)) / n

    if isTree(diforest) && (n ∈ S.rootsOfForest(diforest))
        return log(TREE_PROBABILITY_MASS / num_trees_with_root_last_node)
    else
        return log((1.0 - TREE_PROBABILITY_MASS) / (num_directed_forests - num_trees_with_root_last_node))
    end
end

export tree_preference_diforest


using NeuralPDE, Flux, ModelingToolkit, GalacticOptim, Optim, DiffEqFlux
import ModelingToolkit: Interval, infimum, supremum

@parameters x y t
@variables u(..)
Dx=Differential(x)^1
Dy=Differential(y)^1
Dxx = Differential(x)^2
Dyy = Differential(y)^2

# 2D PDE
eq  = Dxx(u(x,y)) + Dyy(u(x,y))+ u(x,y) ~ 0

# Boundary conditions

bcs = [Dxx(u(-1,y)) ~ -u(-1,y), Dxx(u(1,y)) ~ -u(1,y),
        Dyy(u(x,-1)) ~ -u(x,-1)+2,Dy(u(range(0, step=0.05, stop=1),0)) ~ 0,Dyy(u(x,1))~ -u(x,1)]
    #
# Space and time domains
domains = [x ∈ Interval(-1.0,1.0),
           y ∈ Interval(-1.0,1.0)]

# Neural network
dim = 2 # number of dimensions
chain = FastChain(FastDense(dim,8,Flux.σ),FastDense(8,8,Flux.σ),FastDense(8,1))
# Initial parameters of Neural network
initθ = Float64.(DiffEqFlux.initial_params(chain))

# Discretization
dx = 0.05
discretization = PhysicsInformedNN(chain,GridTraining(dx),init_params =initθ)

@named pde_system = PDESystem(eq,bcs,domains,[x,y],[u(x, y)])
prob = discretize(pde_system,discretization)

#Optimizer
opt = Optim.BFGS()

#Callback function
cb = function (p,l)
    println("Current loss is: $l")
    return false
end

res = GalacticOptim.solve(prob, opt, cb = cb, maxiters=1500)
phi = discretization.phi

using Plots

xs,ys = [infimum(d.domain):dx/10:supremum(d.domain) for d in domains]
analytic_sol_func(x,y) = y^2*sin(pi*x)

u_predict = reshape([first(phi([x,y],res.minimizer)) for x in xs for y in ys],(length(xs),length(ys)))
u_real = reshape([analytic_sol_func(x,y) for x in xs for y in ys], (length(xs),length(ys)))
diff_u = abs.(u_predict .- u_real)

p1 = plot(xs, ys, u_real, linetype=:surface,title = "analytic");
p2 = plot(xs, ys, u_predict, linetype=:surface,title = "predict");
p3 = plot(xs, ys, diff_u,linetype=:contourf,title = "error");
plot(p1,p2,p3)


using SequenceTransformations, LinearAlgebra, SpecialFunctions, Test

a = Sequence(i->(-1)^(i-1)/(2i-1))

@test isa(a, Sequence)

s = cumsum(a)

@test isa(s, AbstractSequence)

@test norm(ϵ(s)[19:20].-π/4) < 20eps()

a = Sequence(i->(-1)^(i-1)/(2i-1)^2)
s = cumsum(a)

D,L,W = Drummond(s),Levin(s),Weniger(s)

@test isa(D, Transformation)

@test norm(D[40]-MathConstants.catalan) < 10eps()
@test norm(L[20]-MathConstants.catalan) < 10eps()
@test norm(W[20]-MathConstants.catalan) < 10eps()

const Gompertz = -.596347362323194074341078499369279376074
a = Sequence(i->(-1)^i*gamma(i))
s = cumsum(a)

@test norm(Levin(s)[18]-Gompertz) < 5e-12
@test Weniger(s)[19] ≈ Gompertz

@test Δ(s)[1] == a[2]

E = z-> logabsgamma(z)[1] - z*log(z)+z-log(2(π/z))/2
z = range(10, stop=100, length=10)
@test norm(stirlingseries(z,Val{15}())-E.(z),Inf) < sqrt(eps())


using StaticArrays: SVector
using LinearAlgebra: I
using Combinatorics: combinations

function Point(coords::AbstractVector{<:Real})
    N = length(coords)
    return Point{N}(SVector{N, Float64}(coords))
end

Point(coords::Vararg{<:Real}) = Point(collect(coords))

Point(b::Barycentric) = Point(barycentric_matrix(b.simplex) * b.coords)

Point(b::SimpleBarycentric) = Point(Barycentric(b))

function Simplex(points::AbstractVector{Point{N}}) where N
    K = length(points)
    return Simplex{N, K}(SVector{K, Point{N}}(points))
end

Simplex(points::Vararg{Point{N}}) where N = Simplex(collect(points))

function Simplex(c::Cell)
    @assert length(c.points) == c.K
    return Simplex(c.points)
end

Simplex(s::SimpleSimplex) = Simplex(s.points)

SimpleSimplex(points::AbstractVector{Point{N}}) where N = SimpleSimplex{N}(collect(points))

SimpleSimplex(s::Simplex) = SimpleSimplex(s.points)

SimpleSimplex(s::Vector{SimpleBarycentric{N}}) where N = SimpleSimplex(map(Point, s))

function Barycentric(s::Simplex{N, K}, coords::AbstractVector{<:Real}) where {N, K}
    @assert length(coords) == K
    return Barycentric{N, K}(s, SVector{K, Float64}(coords))
end

Barycentric(s::Simplex, coords::Vararg{<:Real}) = Barycentric(s, collect(coords))

"""
    Barycentric(m::Metric{N}, s::Simplex{N}, p::Point{N}) where N

Find the Barycentric for the projection of a point onto the affine subspace spanned by a
simplex.
"""
function Barycentric(m::Metric{N}, s::Simplex{N}, p::Point{N}) where N
    A, B = barycentric_projection_matrices(m, s)
    coords = A * p.coords + B
    return Barycentric(s, coords)
end

Barycentric(b::SimpleBarycentric) = Barycentric(Simplex(b.simplex), b.coords)

SimpleBarycentric(s::SimpleSimplex{N}, coords::AbstractVector{<:Real}) where N =
    SimpleBarycentric{N}(s, collect(coords))

SimpleBarycentric(b::Barycentric) = SimpleBarycentric(SimpleSimplex(b.simplex), b.coords)

export subsimplices
"""
    subsimplices(s::Simplex{N, K}, k::Int) where {N, K}

Find all subsimplices of `s` with dimension `k-1`. If `k == 0` or `k > K`, return an
empty vector.
"""
function subsimplices(s::Simplex{N, K}, k::Int) where {N, K}
    @assert k >= 0
    if (k == 0) || (k > K)
        return Simplex{N, k}[]
    else
        return [Simplex(ps...) for ps in combinations(s.points, k)]
    end
end

export barycentric_matrix
"""
    barycentric_matrix(s::Simplex)

Find the matrix `M` so that `Mx` is the point corresponding to the barycentric
coordinates `x`.
"""
barycentric_matrix(s::Simplex) = hcat([p.coords for p in s.points]...)

export barycentric_projection_matrices
"""
    barycentric_projection_matrices(m::Metric{N}, s::Simplex{N, K}) where {N, K}

For a vector `p`, extremize `norm_square(m, p - barycentric_matrix(s) * x)` over `x`
with the constraint `sum(x) == 1`. The result is the barycentric coordinates of the
projection of `p` onto the affine subspace spanned by `s` and it can be written in the form
`x = A * p + B`. Return `A`, `B`.
"""
function barycentric_projection_matrices(m::Metric{N}, s::Simplex{N, K}) where {N, K}
    b = barycentric_matrix(s)
    # solve sum(x) == 1 by saying x = c * y + d
    c = transpose(hcat(Matrix{Float64}(I, K-1, K-1), -ones(K-1)))
    d = zeros(K); d[end] = 1
    bc = b * c; bcT = transpose(bc)
    A = c * ((bcT * m.mat * bc) \ (bcT * m.mat))
    B = d - A * b * d
    return A, B
end

export distance_quadratic
"""
    distance_quadratic(m::Metric{N}, s::Simplex{N})

The squared distance from a point with barycentric coordinates `x` to a point `p`
can be written in the form `xᵀAx + Bᵀx` where `B` depends on `p` but `A` does not. Return A.
"""
distance_quadratic(m::Metric{N}, s::Simplex{N}) where N = -.5 * circumsphere_matrix(m, s)

export distance_linear
"""
    distance_linear(m::Metric{N}, s::Simplex{N}, p::Point{N})

The squared distance from a point with barycentric coordinates `x` to a point `p`
can be written in the form `xᵀAx + Bᵀx` where `B` depends on `p` but `A` does not. Return B.
"""
distance_linear(m::Metric{N}, s::Simplex{N}, p::Point{N}) where N = [norm_square(
    m, p.coords - q.coords) for q in s.points]

export distance_square
"""
    distance_square(m::Metric{N}, b::Barycentric{N}, p::Point{N}) where N

Find the squared distance between the points `Point(b)` and `p`. Note that this
value can be negative if the metric is not positive semi-definite.
"""
function distance_square(m::Metric{N}, b::Barycentric{N}, p::Point{N}) where N
    A = distance_quadratic(m, b.simplex)
    B = distance_linear(m, b.simplex, p)
    return transpose(b.coords) * A * b.coords + transpose(B) * b.coords
end

export circumsphere_matrix
"""
    circumsphere_matrix(m::Metric{N}, s::Simplex{N}) where N

If `x` are the barycentric coordinates of the circumcenter of `s` then there is a matrix M
such that `M * x = 2 * R² * ones(K)` where `R²` is the square circumradius. Return M.
"""
function circumsphere_matrix(m::Metric{N}, s::Simplex{N}) where N
    norm_squares = [[norm_square(m, p.coords - q.coords) for p in s.points] for q in s.points]
    return transpose(hcat(norm_squares...))
end

export circumsphere_barycentric
"""
    circumsphere_barycentric(m::Metric{N}, s::Simplex{N, K}) where {N, K}

Find the Barycentric of the circumcenter and the squared circumradius.
"""
function circumsphere_barycentric(m::Metric{N}, s::Simplex{N, K}) where {N, K}
    if K == 1
        return Barycentric(s, [1.0]), 0
    else
        coords = circumsphere_matrix(m, s) \ ones(K)
        sum_coords = sum(coords)
        return Barycentric(s, coords / sum_coords), .5/sum_coords
    end
end

export circumsphere
"""
    circumsphere(m::Metric{N}, s::Simplex{N}) where N

Find the circumcenter and the squared circumradius.
"""
function circumsphere(m::Metric{N}, s::Simplex{N}) where N
    b, R² = circumsphere_barycentric(m, s)
    return Point(b), R²
end

export circumcenter
"""
    circumcenter(m::Metric{N}) where N

Return a function taking a `Simplex{N}` to the Barycentric of its circumcenter.
"""
circumcenter(m::Metric{N}) where N = s::Simplex{N} -> circumsphere_barycentric(m, s)[1]

export centroid
"""
    centroid(s::Simplex{N, K}) where {N, K}

Compute the Barycentric of the centroid of a simplex.
"""
centroid(s::Simplex{N, K}) where {N, K} = Barycentric(s, SVector{K, Float64}(ones(K)/K))

"""
    barycentric_subspace(m::Metric{N}, s::Simplex{N, K}, f::Simplex{N, J},
        p::Point{N}) where {N, K, J}

Let `s` and `f` be simplices where `f` lies in the affine subspace spanned by `s`. For a
point `p`, the subset of points with the same projection onto `f` as that of `p` is a line.
This line can be represented in the form `Mx = v` where `x` are barycentric coordinates
with respect to `s`. Return M and v.
"""
function barycentric_subspace(m::Metric{N}, s::Simplex{N, K}, f::Simplex{N, J},
    p::Point{N}) where {N, K, J}
    @assert J <= K
    # require that the projection of x onto f is the same as for p
    A, _ = barycentric_projection_matrices(m, f)
    M = A * barycentric_matrix(s)
    v = A * p.coords
    # since the sum of barycentric coordinates is 1, one equation is redundant
    # and one variable is redundant. Replace the last equation with sum(x) == 1
    M[end, :] = ones(K)
    v[end] = 1
    return M, v
end

"""
    rotate_about_face(m::Metric{N}, s::Simplex{N, K}, p::Point{N}, i::Int) where {N, K}

Let `s` be a simplex, `q = s.points[i]`, and `f` the face of `s` opposite `q`. Rotate the
point `p` about `f` into the affine subspace spanned by `s` and on the halfspace bounded
by `f` that does not contain `q`.
"""
function rotate_about_face(m::Metric{N}, s::Simplex{N, K}, p::Point{N}, i::Int) where {N, K}
    f = Simplex(s.points[filter(j -> j != i, 1:K)])
    a, b = parametrize_subspace(barycentric_subspace(m, s, f, p)...)
    q = f.points[1]
    A, B = distance_quadratic(m, s), distance_linear(m, s, q)
    C = -norm_square(m, p.coords - q.coords)
    x1, x2 = intersect_linear_quadratic(A, B, C, a, b[:]) # b[:] reshapes it to a vector
    x = x1[i] > 0 ? x2 : x1
    return Barycentric(s, x)
end

"""
    pairwise_delaunay(m::Metric{N}, s1::Simplex{N, K}, s2::Simplex{N, K}) where {N, K}

Given two simplices `s1` and `s2` that share a face, rotate `s2` into the affine subspace
spanned by `s1`. Let `p` be the image of the the point of `s2` that is not in `s1` under
this rotation and let `c` and `R²` be the circumcenter and squared circumradius of `s1`.
Return the squared distance from `p` to `c` minus `R²`.
"""
function pairwise_delaunay(m::Metric{N}, s1::Simplex{N, K}, s2::Simplex{N, K}) where {N, K}
    face_points = Point{N}[]
    i = 0
    for j in 1:K
        if s1.points[j] in s2.points
            push!(face_points, s1.points[j])
        else
            i = j
        end
    end
    @assert length(face_points) == K-1
    @assert i > 0
    p = first_setdiff(s2.points, face_points)
    b = rotate_about_face(m, s1, p, i)
    return transpose(b.coords) * distance_quadratic(m, s1) * b.coords
end


### A Pluto.jl notebook ###
# v0.12.17

using Markdown
using InteractiveUtils

# ╔═╡ 864b72c0-3e11-11eb-2cd6-9951cdcb78c3
begin#imports
	using Flux:gradient,params
	using Flux.Optimise:Descent,train!,@epochs
	using Statistics:mean
end

# ╔═╡ 588bf94e-3e17-11eb-216c-090eb107222e
using Flux:Chain,Dense,softmax

# ╔═╡ c30bbc58-3e11-11eb-1e98-cda1cf1337e3
begin #one-variable gradient
	f(x)=x^2
	df(x)=gradient(f,x)
end

# ╔═╡ a055c570-3e12-11eb-07d2-955dda5ae765
f(3),df(3)

# ╔═╡ e3427b4e-3e12-11eb-3779-7fa565a2bc17
begin #multivariable
	g(x,y,z)=x.*42 .+ y^2 .+ sin(z)
	dg(x,y,z)=gradient(g,x,y,z)
end

# ╔═╡ f2632f96-3e13-11eb-373c-d5bc583b99e9
dg(4,0,0)

# ╔═╡ bcf3b9d8-3e14-11eb-0f98-7f9dde938646
begin
	x=rand(42,1)
	W=rand(1,42)
	U=rand(1)
	grads = gradient(()->(W*x*U)[1], params([W,U]))
end

# ╔═╡ 40d88bd4-3e15-11eb-2628-71a41b821c59
grads[W],grads[U]

# ╔═╡ 354afdd6-3e17-11eb-2b2a-8fc10a36d820
model=Chain(Dense(42,9),
	Dense(9,1))

# ╔═╡ 6932f126-3e17-11eb-3b87-e5bdbd2a3f54
model(x)

# ╔═╡ ae7c1ad2-3e17-11eb-074a-03df157b9566
params(model)

# ╔═╡ 33b26a94-3e18-11eb-1125-af41dc7537e7
begin
X,y=rand(42,1000),rand(42,1000)
loss(x, y) = mean((model(x).-y))
@epochs 5 train!(loss, params(model), [(X,y)], Descent(1e-3))
end

# ╔═╡ 47eeeb8a-3e19-11eb-2158-5bf7ceb2ed90
loss(X,y)

# ╔═╡ Cell order:
# ╠═864b72c0-3e11-11eb-2cd6-9951cdcb78c3
# ╠═c30bbc58-3e11-11eb-1e98-cda1cf1337e3
# ╠═a055c570-3e12-11eb-07d2-955dda5ae765
# ╠═e3427b4e-3e12-11eb-3779-7fa565a2bc17
# ╠═f2632f96-3e13-11eb-373c-d5bc583b99e9
# ╠═bcf3b9d8-3e14-11eb-0f98-7f9dde938646
# ╠═40d88bd4-3e15-11eb-2628-71a41b821c59
# ╠═588bf94e-3e17-11eb-216c-090eb107222e
# ╠═354afdd6-3e17-11eb-2b2a-8fc10a36d820
# ╠═6932f126-3e17-11eb-3b87-e5bdbd2a3f54
# ╠═ae7c1ad2-3e17-11eb-074a-03df157b9566
# ╠═33b26a94-3e18-11eb-1125-af41dc7537e7
# ╠═47eeeb8a-3e19-11eb-2158-5bf7ceb2ed90


function compute_crowding_distance(pop)
    crowding = zeros(length(pop))

    if length(pop[1].f) == 2
        sort!(pop, by = x -> x.f[1])
        pop[1].f[1] == pop[end].f[1] && return #Don't waste time if all indivs are the same
        crowding[1] = crowding[end] = Inf

        width_y1 = (pop[end].f[1] - pop[1].f[1])
        width_y2 = (pop[1].f[2] - pop[end].f[2])
        for i = 2:length(pop)-1
            crowding[i] = (pop[i+1].f[1] - pop[i-1].f[1]) / width_y1 + (pop[i-1].f[2] - pop[i+1].f[2]) / width_y2
        end

        return crowding
    end

    for j = 1:length(first(pop).f) # Foreach objective
        let j = j
            sort!(pop, by = x -> x.f[j]) #sort by the objective value
        end
        crowding[1] = crowding[end] = Inf #Assign infinite value to extremas
        if pop[1].f[j] != pop[end].f[j]
            for i = 2:length(pop)-1
                crowding[i] += (pop[i+1].f[j] - pop[i-1].f[j]) / (pop[end].f[j] - pop[1].f[j])
            end
        end
    end

    crowding
end

function update_crowding_distance!(pop)
    crowding = compute_crowding_distance(pop)
    for (i, sol) in enumerate(pop)
        sol.crowding = crowding[i]
    end

    pop
end

function truncate_population!(population, N)
    fast_non_dominated_sort!(population)

    let f::Int = 1
        ind = 0
        indnext = findlast(x -> x.rank == f, population)
        while 0 < indnext <= N
            ind = indnext
            f += 1
            indnext = findlast(x -> x.rank == f, population)
        end
        indnext == 0 && (indnext = length(population))
        update_crowding_distance!(view(population, ind+1:indnext))
        sort!(view(population, ind+1:indnext), by = x -> x.crowding, rev = true, alg = PartialQuickSort(N-ind))
    end
    deleteat!(population, N+1:length(population))
end


#= Numerical recipes implementation of Error function
Press, et al (2007) Numerical Recipes: The Art of Scientific Computing. 3rd Ed.
Section 6.2.2 =#

const erfcof = (-1.3026537197817094, 6.4196979235649026e-1,
    1.9476473204185836e-2,-9.561514786808631e-3,-9.46595344482036e-4,
    3.66839497852761e-4,4.2523324806907e-5,-2.0278578112534e-5,
    -1.624290004647e-6,1.303655835580e-6,1.5626441722e-8,-8.5238095915e-8,
    6.529054439e-9,5.059343495e-9,-9.91364156e-10,-2.27365122e-10,
    9.6467911e-11, 2.394038e-12,-6.886027e-12,8.94487e-13, 3.13092e-13,
    -1.12708e-13,3.81e-16,7.106e-15,-1.523e-15,-9.4e-17,1.21e-16,-2.8e-17)

function erf(x::Float64)
    if x ≥ 0
        return 1 - erfccheb(x)
    else
        return erfccheb(-x) - 1
    end
end

erf(x::Float64, y::Float64) = erfc(y, x)

function erfc(x::Float64)
    if x ≥ 0
        return erfccheb(x)
    else
        return 2 - erfccheb(-x)
    end
end

"""
    erfc(y) - erfc(x)
"""
function erfc(x::Float64, y::Float64)
    if x > y
        return -erfc(y, x)
    elseif abs(x) > abs(y)
        return -erfc(-x, -y)
    elseif x < 0 ≤ y
        return erf(x) - erf(y)
    elseif 0 ≤ x ≤ y
        return erfccheb(x, y)
    else
        return NaN
    end
end

function erfcx(x::Float64)
    if x ≥ 0
        return erfcxcheb(x)
    else
        return 2exp(x^2) - erfcxcheb(-x)
    end
end

inverf(p::Float64) = inverfc(1 - p)

# erfc
function erfccheb(z::Float64)
    z ≥ 0 || throw(ArgumentError("erfccheb requires nonnegative argument"))
    d = dd = 0.0
    t = 2 / (2 + z)
    ty = 4t - 2
    for j = length(erfcof) : -1 : 2
        tmp = d
        d = ty * d - dd + erfcof[j]
        dd = tmp
    end
    return t * exp(-z^2 + 0.5 * (erfcof[1] + ty * d) - dd)
end

# erfcx
function erfcxcheb(z::Float64)
    if !(z ≥ 0)
        return NaN
    end
    d = dd = 0.0
    t = 2 / (2 + z)
    ty = 4t - 2
    for j = length(erfcof) : -1 : 2
        tmp = d
        d = ty * d - dd + erfcof[j]
        dd = tmp
    end
    return t * exp(0.5 * (erfcof[1] + ty * d) - dd)
end

# erfc(y) - erfc(x)
function erfccheb(x::Float64, y::Float64)
    if !(0 ≤ x ≤ y)
        return NaN
    end
    dx = ddx = 0.0
    dy = ddy = 0.0
    tx = 2 / (2 + x)
    ty = 2 / (2 + y)
    qx = 4tx - 2
    qy = 4ty - 2
    for j = length(erfcof) : -1 : 2
        tmpx = dx
        tmpy = dy
        dx = qx * dx - ddx + erfcof[j]
        dy = qy * dy - ddy + erfcof[j]
        ddx = tmpx
        ddy = tmpy
    end
    fx = tx * exp(-x^2 + (erfcof[1] + qx * dx) / 2 - ddx)
    fy = ty * exp(-y^2 + (erfcof[1] + qy * dy) / 2 - ddy)
    return fy - fx
end

function inverfc(p::Float64)
    if p ≥ 2.0
        return -100.0
    elseif p <= 0.0
        return 100.0
    end

    pp = p < 1.0 ? p : 2.0 - p
    t = sqrt(-2 * log(pp / 2))
    x = -0.70711 * ((2.30753 + 0.27061t) / (1 + (0.99229 + 0.04481t)t) - t)
    for j = 1:2
        err = erfc(x) - pp
        x += err/(1.12837916709551257exp(-x^2) - x * err)
    end
    return p < 1.0 ? x : -x
end


# This file is a part of BAT.jl, licensed under the MIT License (MIT).

@doc doc"""
    BAT.FunnelDistribution([a=1, b=0.5, n=3])

Funnel distribution (Caldwell et al.)[https://arxiv.org/abs/1808.08051].

# Arguments
- `a::Real`: Variance of the dominant normal distribution.
- `b::Real`: Variance of the supporting normal distributions.
- `n::Int`: Number of dimensions.

Constructors:

```julia
BAT.FunnelDistribution(a::Real, b::Real, n::Int)
```
"""
struct FunnelDistribution <: ContinuousMultivariateDistribution
    a::Real
    b::Real
    n::Int
end

function FunnelDistribution(;a::Real=1, b::Real=0.5, n::Integer=3)
    a, b = promote(a, b)
    FunnelDistribution(a, b, n)
end

Base.length(d::FunnelDistribution) = d.n
Base.eltype(d::FunnelDistribution) = Base.eltype(d.a)

Statistics.mean(d::FunnelDistribution) = zeros(d.n)

function Statistics.cov(dist::BAT.FunnelDistribution)
    cov(nestedview(rand(bat_determ_rng(), sampler(dist), 10^5)))
end

StatsBase.params(d::FunnelDistribution) = (d.a, d.b, d.n)

function Distributions._logpdf(d::FunnelDistribution, x::AbstractArray)
    idxs = eachindex(x)
    s = logpdf(Normal(0, d.a^2), x[idxs[1]])
    @inbounds for i in idxs[2:end]
        s += logpdf(Normal(0, exp(2 * d.b * x[1])), x[i])
    end
    s
end

function Distributions._rand!(rng::AbstractRNG, d::FunnelDistribution, x::AbstractVector)
    idxs = eachindex(x)
    x[idxs[1]] = rand(rng, Normal(0, d.a^2))
    @inbounds for i in idxs[2:end]
        x[i] = rand(rng, Normal(0, exp(2 * d.b * x[1])))
    end
    return x
end


module DGMultiFEOperatorsTests

# Testing Stokes problem with Dirichlet BCs for the velocity
# and pressure prescribed in a single point

using Test
using Gridap
import Gridap: ∇
import LinearAlgebra: tr

const T = VectorValue{2,Float64}

u(x) = VectorValue(x[1]*x[1], x[2])

∇u(x) = TensorValue(2*x[1],0.0,0.0,1.0)

∇(::typeof(u)) = ∇u

Δu(x) = VectorValue(2.0,0.0)

p(x) = x[1] - x[2]

∇p(x) = VectorValue(1.0,-1.0)

f(x) = - Δu(x) + ∇p(x)

g(x) = tr(∇u(x))

ud(x) = u(x)

# Discrete model
L = 1.0
limits = (0.0, L, 0.0, L)
ncellx = 4
model = CartesianDiscreteModel(domain=limits, partition=(ncellx,ncellx))
model = simplexify(model)

h = L / ncellx

order = 2
γ = order*(order+1)
γ0 = 1.0/10.0

# Construct the FEspace 1
fespace1 = DLagrangianFESpace(T,model,order)

# Construct the FEspace 2
fespace2 = DLagrangianFESpace(Float64,model,order)
fespace2 = ZeroMeanFESpace(fespace2,order)

# Define test and trial
Vh = TestFESpace(fespace1)
Qh = TestFESpace(fespace2)
Yh = [Vh, Qh]

Uh = TrialFESpace(fespace1)
Ph = TrialFESpace(fespace2)
Xh = [Uh, Ph]

# Define integration mesh and quadrature for volume
trian = Triangulation(model)
quad = CellQuadrature(trian,degree=2*order)

btrian = BoundaryTriangulation(model)
bquad = CellQuadrature(btrian,degree=2*order)
nb = NormalVector(btrian)

strian = SkeletonTriangulation(model)
squad = CellQuadrature(strian,degree=2*order)
ns = NormalVector(strian)

# Weak form

function A_Ω(y,x)
  u, p = x
  v, q = y
  inner(∇(v), ∇(u)) - inner(∇(q), u) + inner(v, ∇(p))
end

function B_Ω(y)
  v, q = y
  inner(v,f) + inner(q, g)
end

function A_∂Ω(y,x)
  u, p = x
  v, q = y
  (γ/h) * inner(v,u) - inner(v, ∇(u)*nb )  - inner(∇(v)*nb, u) + 2*inner(q*nb,u) # - inner(v, p*nb) + inner(v,p*nb)
end

function B_∂Ω(y)
  v, q = y
  (γ/h) * inner(v,ud) - inner(∇(v)*nb, ud) + inner(q*nb, ud)
end

function A_Γ(y,x)
  u, p = x
  v, q = y
  (γ/h) * inner( jump(outer(v,ns)), jump(outer(u,ns))) -
    inner( jump(outer(v,ns)), mean(∇(u)) ) -
    inner( mean(∇(v)), jump(outer(u,ns)) ) +
    (γ0*h) * inner( jump(q*ns), jump(p*ns)  ) +
    inner( jump(q*ns), mean(u) ) -
    inner( mean(v), jump(p*ns) )
end

t_Ω = AffineFETerm(A_Ω,B_Ω,trian,quad)

t_∂Ω = AffineFETerm(A_∂Ω,B_∂Ω,btrian,bquad)

t_Γ = LinearFETerm(A_Γ,strian,squad)

# Define the FEOperator
op = LinearFEOperator(Yh,Xh,t_Ω,t_∂Ω,t_Γ)

# Solve!
xh = solve(op)
uh, ph = xh

# Define exact solution and error
eu = u - uh

ep = p - ph

#writevtk(trian,"trian",cellfields=["uh"=>uh,"ph"=>ph, "eu"=>eu, "ep"=>ep])

# Define norms to measure the error
l2(u) = inner(u,u)
h1(u) = inner(∇(u),∇(u)) + l2(u)

# Compute errors
eul2 = sqrt(sum( integrate(l2(eu),trian,quad) ))
euh1 = sqrt(sum( integrate(h1(eu),trian,quad) ))

epl2 = sqrt(sum( integrate(l2(ep),trian,quad) ))

@test eul2 < 1.e-8
@test euh1 < 1.e-8

@test epl2 < 1.e-8

end # module


using ChebyshevQuantum
using LinearAlgebra

function go(N ; Rmax = 20, L=0)

    @time begin
        pts = ChebyshevQuantum.Interp.getCpts(N);
        D = ChebyshevQuantum.Interp.getD(pts);
    end

    @time begin
        r = (1 .+ pts) * Rmax / 2
        H1 = -0.5*(D*D )[2:N,2:N] * 4/Rmax^2
        H2 = -diagm(1 ./ r[2:N])
        H3 = L*(L+1)*diagm(1 ./ r[2:N].^2) / 2.0
        H = H1+H2+H3
    end

    @time v = eigvals( H )

    m = min(10, length(v))
    return v[1:m] * 2 .* ( (1:m) .+ L).^2
end

function go_exp(N ; Rmax = 20, L=0, a = 1.0)

    @time begin
        pts = ChebyshevQuantum.Interp.getCpts(N);
        D = ChebyshevQuantum.Interp.getD(pts);
    end

    @time begin
        r = (1 .+ pts) * Rmax / 2
        H1 = -0.5*(D*D )[2:N,2:N] * 4/Rmax^2

        #H1a = zeros(N-1,N-1)
        H1a = (D )[2:N,2:N] * 2/Rmax * a

        #        H1b = -0.5*I(N-1) * a^2
        #H1b = zeros(N-1, N-1)

        H2 = -diagm(1 ./ r[2:N])
        H3 = L*(L+1)*diagm(1 ./ r[2:N].^2) / 2.0
        H = H1+H1a+H2+H3
    end

    @time v = eigvals( H )
    v = v .- 0.5 * a^2
    m = min(10, length(v))
    return v[1:m] * 2 .* ( (1:m) .+ L).^2
end

#=function go_exp2(N ; Rmax = 20, L=0, a = 1.0)

    @time begin
        pts = ChebyshevQuantum.Interp.getCpts(N);
        D = ChebyshevQuantum.Interp.getD(pts);
    end

    @time begin
        r = (1 .+ pts) * Rmax / 2
        H1 = -0.5*(D*D )[2:N,2:N] * 4/Rmax^2

        #H1a = zeros(N-1,N-1)
        H1a = (D )[2:N,2:N] * 2/Rmax * a

        H1b = -0.5*I(N-1) * a^2
        #H1b = zeros(N-1, N-1)

        H2 = -diagm(1 ./ r[2:N])
        H3 = L*(L+1)*diagm(1 ./ r[2:N].^2) / 2.0
        H = H1+H1a+H1b+H2+H3
    end

    @time v = eigvals( H )
#    v = v .- 0.5 * a^2
    m = min(10, length(v))
    return v[1:m] * 2 .* ( (1:m) .+ L).^2
end
=#


@testset "Test the pivoted QR" begin

    ψ = randn(100, 20)
    u = randn(100)
    m, n = size(ψ)

    ctrue = ψ\u

    # Force a greedy approach over the entire set of indices
    # With pivoted QR
    idxpivot, cpivot, ϵpivot = pivotedqr(ψ, u; invert = true, verbose = true, ϵrel = eps())

    @test norm(ctrue[idxpivot]-cpivot)<1e-14

    # Residual error must decrease
    @test all(ϵpivot[2:end]-ϵpivot[1:end-1] .< 0) == true

    for k=1:n
        cverif = view(ψ,:, idxpivot[1:k])\u
        @test abs(ϵpivot[k+1] - norm(u - view(ψ,:, idxpivot[1:k])*cverif))<1e-14
    end
end


@testset "Autodiff propagation" begin
  f = ToCartesian(r->[r[1] * cos(r[2]), r[1]*sin(r[2])], 2)
  f⁻¹ = FromCartesian(x->[sqrt(x[1]^2 + x[2]^2), atan(x[2], x[1])], 2)

  X = rand(2) .- 0.5
  R = f⁻¹(X)

  @test ForwardDiff.jacobian(f, R) ≈ inv(ForwardDiff.jacobian(f⁻¹, X))
  @test inv(ForwardDiff.jacobian(f, R)) ≈ ForwardDiff.jacobian(f⁻¹, X)
  @test X ≈ f⁻¹ \ R
  @test inv(ForwardDiff.jacobian(f, R)) ≈ ForwardDiff.jacobian(r-> f⁻¹(f⁻¹ \ r), R)
end


"""
	PerformanceIndicators

This module includes performance indicators to assess evolutionary multi-objective
optimization algorithms.

- `gd` Generational Distance.
- `igd` Inverted Generational Distance.
- `gd_plus` Generational Distance plus.
- `igd_plus` Inverted Generational Distance plus.
- `covering` Covering indicator (C-metric).
- `hypervolume` Hypervolume indicator.

### Example

```jldoctest
julia> import Metaheuristics: PerformanceIndicators, TestProblems

julia> A = [ collect(1:3) collect(1:3) ]
3×2 Array{Int64,2}:
 1  1
 2  2
 3  3

julia> B = A .- 1
3×2 Array{Int64,2}:
 0  0
 1  1
 2  2

julia> PerformanceIndicators.gd(A, B)
0.47140452079103173

julia> f, bounds, front = TestProblems.get_problem(:ZDT1);

julia> front
                          F space
         ┌────────────────────────────────────────┐
       1 │⠁⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀│
         │⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀│
         │⠈⠄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀│
         │⠀⠈⢆⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀│
         │⠀⠀⠀⠢⡀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀│
         │⠀⠀⠀⠀⠈⠢⡀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀│
         │⠀⠀⠀⠀⠀⠀⠉⠢⡄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀│
   f_2   │⠀⠀⠀⠀⠀⠀⠀⠀⠈⠑⢤⡀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀│
         │⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠈⠲⢄⡀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀│
         │⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠈⠒⢤⡀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀│
         │⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠈⠙⠢⢄⡀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀│
         │⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠈⠑⠢⢄⡀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀│
         │⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠈⠉⠢⠤⣀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀│
         │⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠉⠑⠢⢤⣀⠀⠀⠀⠀⠀│
       0 │⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠉⠒⠢⢄⣀│
         └────────────────────────────────────────┘
         0                                        1
                            f_1

julia> PerformanceIndicators.igd_plus(front, front)
0.0
```

"""
module PerformanceIndicators

import ..State, ..fvals, ..norm, ..xFgh_indiv, ..fval, ..compare, ..mean
import ..get_non_dominated_solutions

include("hypervolume.jl")

"""
	generational_distance(front, true_pareto_front; p = 1, inverted = false, plus=false)

Returns the Generational Distance.

### Parameters

- `front` and `true_pareto_front` can be:
	- `N×m` matrix where `N` is the number of points and `m` is the number of objectives.
	- `State`
	- `Array{xFgh_indiv}` (usually `State.population`)
- `true_pareto_front` is a `M×m` matrix.
- `p` is the power in for the ‖⋅‖_p
- `plus` if true then computes the GD+
- `inverted` if true then computes IGD
"""
function generational_distance(front, true_pareto_front; p = 1, inverted = false, plus=false)

	if inverted
		tmp = true_pareto_front
		true_pareto_front = front
		front = tmp
	end

	distances = zeros(size(true_pareto_front, 1))

	s = 0.0
	for i in 1:size(front,1)
		x = view(front, i, :)
		for j = 1:size(true_pareto_front, 1)
			y = view(true_pareto_front, j, :)
			if plus	&& inverted
				distances[j] = norm( max.(y - x, 0) )
			elseif plus && !inverted
				distances[j] = norm( max.(x - y, 0) )
			else
				distances[j] = norm( y - x)
			end

		end

		s += minimum(distances) ^ p
	end

	return (s^(1/p)) /  size(front, 1)

end

function generational_distance(front::Union{Array{xFgh_indiv}, State},
						       true_pareto_front::Union{Array{xFgh_indiv}, State};
								p = 1,
								inverted = false,
								plus = false
		)
	generational_distance(fvals(front), fvals(true_pareto_front); p=p,inverted=inverted,plus=plus)
end

function generational_distance(front::Union{Array{xFgh_indiv}, State},
						       true_pareto_front;
								p = 1,
								inverted = false,
								plus = false
		)
	generational_distance(fvals(front), (true_pareto_front); p=p,inverted=inverted,plus=plus)
end

"""
	gd(front, true_pareto_front; p = 1)

Returns the Generational Distance.

### Parameters

`front` and `true_pareto_front` can be:

- `N×m` matrix where `N` is the number of points and `m` is the number of objectives.
- `State`
- `Array{xFgh_indiv}` (usually `State.population`)
"""
gd(front, true_pareto_front; p = 1) = generational_distance(front, true_pareto_front, p=p)

"""
	igd(front, true_pareto_front; p = 1)

Returns the Inverted Generational Distance.

### Parameters

`front` and `true_pareto_front` can be:

- `N×m` matrix where `N` is the number of points and `m` is the number of objectives.
- `State`
- `Array{xFgh_indiv}` (usually `State.population`)

"""
igd(front, true_pareto_front; p = 1) = generational_distance(front, true_pareto_front, inverted=true, p=p)

"""
	gd_plus(front, true_pareto_front; p = 1)

Returns the Generational Distance Plus.

### Parameters

`front` and `true_pareto_front` can be:

- `N×m` matrix where `N` is the number of points and `m` is the number of objectives.
- `State`
- `Array{xFgh_indiv}` (usually `State.population`)
"""
gd_plus(front, true_pareto_front; p = 1) = generational_distance(front, true_pareto_front, p=p, plus=true)

"""
	igd_plus(front, true_pareto_front; p = 1)

Returns the Inverted Generational Distance Plus.

### Parameters

`front` and `true_pareto_front` can be:

- `N×m` matrix where `N` is the number of points and `m` is the number of objectives.
- `State`
- `Array{xFgh_indiv}` (usually `State.population`)

"""
igd_plus(front, true_pareto_front; p = 1) = generational_distance(front, true_pareto_front, inverted=true, p=p, plus=true)

"""
	spacing(A)
Computes the Schott spacing indicator. `spacing(A) == 0` means that vectors in `A` are
uniformly distributed.
"""
function spacing(A::Array{Array{Float64,1},1})
    n = length(A)

    d = zeros(n)
    for i = 1:n
    	@inbounds d[i] = minimum( [ sum( abs.(A[i] - A[j]) ) for j in deleteat!(collect(1:n), i) ] )
    end

    d_hat = mean(d)
    s = (1.0 / ( n - 1.0 )) * sum( (d_hat .- d) .^ 2 )
    return sqrt(s)

end

spacing(A::Array{xFgh_indiv}) = spacing( fval.(A) )
spacing(A::State{xFgh_indiv}) = spacing(A.population)
spacing(A::Matrix) = spacing([A[i,:] for i in 1:size(A,1)])

"""
	covering(A, B)
Computes the covering indicator (percentage of vectors in B that are dominated by vectors in A)
from two sets with non-dominated solutions.

A and B with size (n, m) where n is number of samples and m is the vector dimension.

Note that `covering(A, B) == 1` means that all solutions in B are dominated by those
in A. Moreover, `covering(A, B) != covering(B, A)` in general.

If `A::State` and `B::State`, then computes `covering(A.population, B.population)` after
ignoring dominated solutions in each set.
"""
function covering(A::Array{Vector{T}}, B::Array{Vector{T}}) where T <: Real
    n_a = length(A)
    n_b = length(B)
    s = 0.0
    for i = 1:n_b
        for j = 1:n_a
			# `compare` returns 1 if argument 1 dominates argument 2
            if compare(A[j], B[i]) == 1
                s += 1.0
                break
            end
        end

    end

    return s / n_b

end

covering(A::Vector{xFgh_indiv}, B::Vector{xFgh_indiv}) = covering(fval.(A), fval.(B))

function covering(A::State{xFgh_indiv}, B::State{xFgh_indiv})
	A_non_dominated = get_non_dominated_solutions(A.population)
	B_non_dominated = get_non_dominated_solutions(B.population)

	length(A_non_dominated) != length(A.population) &&
	@warn "Some solutions in B dominate each other (solutions ignored)."

	length(B_non_dominated) != length(B.population) &&
	@warn "Some solutions in B dominate each other (solutions ignored)."

	covering(A_non_dominated, B_non_dominated)
end

function covering(A::Matrix, B::Matrix)
	A_arr = [ A[i,:] for i in 1:size(A,1) ]
	B_arr = [ B[i,:] for i in 1:size(B,1) ]
	covering(A_arr, B_arr)
end

"""
	hypervolume(front, reference_point)
Computes the hypervolume indicator, i.e., volume between points in `front` and `reference_point`.

Note that each point in `front` must (weakly) dominates to `reference_point`. Also, `front`
is a non-dominated set.

If `front::State` and `reference_point::Vector`, then computes `hypervolume(front.population, reference_point)` after
ignoring solutions in `front` that do not dominate `reference_point`.
"""
function hypervolume(front::Array{Vector{T}}, reference_point::Vector) where T <: Real

	weaklyDominates(point, other) = begin
		for i in 1:length(point)
			if point[i] > other[i]
				return false
			end
		end
		return true
	end

	relevantPoints = Vector[]
	for point in front
		# only consider points that dominate the reference point
		if weaklyDominates(point, reference_point)
			push!(relevantPoints, point)
		end
	end

	if length(relevantPoints) != length(front)
		ign = length(front) - length(relevantPoints)
		rel = length(relevantPoints)
		@warn "Ignoring $ign points dominated by the reference point ($rel points are used)."
	end

	return HyperVolume.hv(relevantPoints, reference_point)
end

hypervolume(front::Vector{xFgh_indiv}, reference_point::Vector{xFgh_indiv}) = hypervolume(fval.(front), fval(reference_point))
hypervolume(front::Vector{xFgh_indiv}, reference_point::Vector) = hypervolume(fval.(front), reference_point)

function hypervolume(front::Matrix, reference_point::Vector)
	front_ = [ front[i,:] for i in 1:size(front,1) ]
	hypervolume(front_, reference_point)
end

"""
	deltap(front, true_pareto_front; p = 1)
	Δₚ(front, true_pareto_front; p = 1)

Returns the averaged Hausdorff distance indicator aka Δₚ (Delta p).

"`Δₚ`" can be typed as `\\Delta<tab>\\_p<tab>`.

### Parameters

`front` and `true_pareto_front` can be:

- `N×m` matrix where `N` is the number of points and `m` is the number of objectives.
- `Array{xFgh_indiv}` (usually `State.population`)

"""
function deltap(front::Array{Vector{T}}, true_pareto_front::Array{Vector{T}}; p = 1) where T <: Real
	distances = zeros(length(true_pareto_front), length(front))

	for i in eachindex(true_pareto_front)
		for j in eachindex(front)
			distances[i,j] = norm(true_pareto_front[i] - front[j])
		end
	end

	gd  = mean(minimum(distances, dims = 1) .^ p)
	igd = mean(minimum(distances, dims = 2) .^ p)
	return max(gd^(1/p), igd^(1/p))
end

deltap(front::Vector{xFgh_indiv}, true_pareto_front::Vector{xFgh_indiv}; kargs...) = deltap(fval.(front), fval.(true_pareto_front); kargs...)

function deltap(front::AbstractMatrix, true_pareto_front::AbstractMatrix; kargs...)
	front_ = [ front[i,:] for i in 1:size(front,1) ]
	true_pareto_front_ = [ true_pareto_front[i,:] for i in 1:size(true_pareto_front,1) ]
	deltap(front_, true_pareto_front_; kargs...)
end

const Δₚ = deltap
#Δₚ(args...;kargs...) = deltap(args...;kargs...)

end


using SumBassSolver
using Plots

p=fill(0.001,2)
q=[0.1,0.11]
m=fill(100,2)
c0=fill(0,2)
alpha = fill(1,2)
F = fill(0,2)
T = fill(100,2)
Pi=90

sumBassSolver(p,q,m,c0,alpha,F,T,Pi)
sumBassSolver(p,q,m,c0,alpha,F,T,Pi,localopt=true)
sumBassSolver(p,q,m,c0,alpha,F,T,Pi,alg="SB")
sumBassSolver(p,q,m,c0,alpha,F,T,Pi,alg="SB",localopt=true)

Pis = 50:100
v = Vector{Float64}(undef,length(Pis))
x = Array{Float64}(undef,2,length(Pis))

@time for i in 1:length(Pis)
    x[:,i],v[i] = sumBassSolver(p,q,m,c0,alpha,F,T,Pis[i],localopt=true)
end

pl1 = plot(Pis,v);
pl2 = plot(Pis,x[1,:],label = "plot1");
plot!(Pis,x[2,:],label = "plot2");
plot!(Pis,fill(Bass_inflection(p[1],q[1],m[1],c0[1]),length(Pis)),label="inflection1");
plot!(Pis,fill(Bass_inflection(p[2],q[2],m[2],c0[2]),length(Pis)),label="inflection2");
pl2


struct AliasTable <: Sampleable{Univariate,Discrete}
    accept::Vector{Float64}
    alias::Vector{Int}
end
ncategories(s::AliasTable) = length(s.alias)

function AliasTable(probs::AbstractVector)
    n = length(probs)
    n > 0 || throw(ArgumentError("The input probability vector is empty."))
    accp = Vector{Float64}(undef, n)
    alias = Vector{Int}(undef, n)
    StatsBase.make_alias_table!(probs, 1.0, accp, alias)
    AliasTable(accp, alias)
end

function rand(rng::AbstractRNG, s::AliasTable)
    i = rand(rng, 1:length(s.alias)) % Int
    u = rand(rng)
    @inbounds r = u < s.accept[i] ? i : s.alias[i]
    r
end

show(io::IO, s::AliasTable) = @printf(io, "AliasTable with %d entries", ncategories(s))


using OrdinaryDiffEq, DiffEqParamEstim, Distributions, Test,RecursiveArrayTools
      # ,BlackBoxOptim,

pf_func = function (du,u,p,t)
  du[1] = p[1] * u[1] - p[2] * u[1]*u[2]
  du[2] = -3.0 * u[2] + u[1]*u[2]
end
u0 = [1.0;1.0]
tspan = (0.0,10.0)
p = [1.5,1.0]
prob1 = ODEProblem(pf_func,u0,tspan,p)
sol = solve(prob1,Tsit5())
t = collect(range(0, stop=10, length=200))
function generate_data(sol,t)
  randomized = VectorOfArray([(sol(t[i]) + .01randn(2)) for i in 1:length(t)])
  data = convert(Array,randomized)
end
aggregate_data = convert(Array,VectorOfArray([generate_data(sol,t) for i in 1:100]))

distributions = [fit_mle(Normal,aggregate_data[i,j,:]) for i in 1:2, j in 1:200]
obj = build_loss_objective(prob1,Tsit5(),LogLikeLoss(t,distributions),
                                     maxiters=10000,verbose=false)
bound1 = Tuple{Float64, Float64}[(0.5, 5),(0.5, 5)]
result = bboptimize(obj;SearchRange = bound1, MaxSteps = 11e3)
@test result.archive_output.best_candidate ≈ [1.5,1.0] atol = 1e-1

data_distributions = [fit_mle(Normal,aggregate_data[i,j,:]) for i in 1:2, j in 1:200]
diff_distributions = [fit_mle(Normal,aggregate_data[i,j,:]-aggregate_data[i,j-1,:]) for j in 2:200, i in 1:2 ]
obj = build_loss_objective(prob1,Tsit5(),LogLikeLoss(t,data_distributions,diff_distributions),
                                     maxiters=10000,verbose=false)
bound1 = Tuple{Float64, Float64}[(0.5, 5),(0.5, 5)]
 result = bboptimize(obj;SearchRange = bound1, MaxSteps = 11e3)
@test result.archive_output.best_candidate ≈ [1.5,1.0] atol = 1e-1

data_distributions = [fit_mle(Normal,aggregate_data[i,j,:]) for i in 1:2, j in 1:200]
diff_distributions = [fit_mle(Normal,aggregate_data[i,j,:]-aggregate_data[i,j-1,:]) for j in 2:200, i in 1:2 ]
obj = build_loss_objective(prob1,Tsit5(),LogLikeLoss(t,data_distributions,diff_distributions,0.3),
                                     maxiters=10000,verbose=false)
bound1 = Tuple{Float64, Float64}[(0.5, 5),(0.5, 5)]
result = bboptimize(obj;SearchRange = bound1, MaxSteps = 11e3)
@test result.archive_output.best_candidate ≈ [1.5,1.0] atol = 1e-1

distributions = [fit_mle(MvNormal,aggregate_data[:,j,:]) for j in 1:200]
diff_distributions = [fit_mle(MvNormal,aggregate_data[:,j,:]-aggregate_data[:,j-1,:]) for j in 2:200]
priors = [Truncated(Normal(1.5,0.1),0,2),Truncated(Normal(1.0,0.1),0,1.5)]
obj = build_loss_objective(prob1,Tsit5(),LogLikeLoss(t,distributions,diff_distributions),
                                     maxiters=10000,verbose=false,priors=priors)
bound1 = Tuple{Float64, Float64}[(0.5, 5),(0.5, 5)]
result = bboptimize(obj;SearchRange = bound1, MaxSteps = 11e3)
@test result.archive_output.best_candidate ≈ [1.5,1.0] atol = 1e-1


import Base: <, iszero, ==, <=, >=, sign, signbit, isinteger, isfinite
export <, iszero, ==, <=, >=, sign, signbit, isinteger, isfinite

function isinteger(a::NumberInterval)
    if floor(a) ≺ a ≺ ceil(a)
        return false
    elseif issingleton(a)
        #NOTE given the previous result, singletons must contain single integers
        return true
    end
    return missing_or_exception(a)
end

function isfinite(a::NumberInterval)
    # per IEEE standard, intervals cannot contain infinities
    if !isempty(a) && !isnan(a)
        return true
    end
    return missing_or_exception(a)
end

for (numberf, setf) in ((:<, :strictprecedes), (:<=, :precedes))
    @eval function $numberf(a::NumberInterval, b::NumberInterval)
        if $setf(a, b)
            return true
        elseif $setf(b, a)
            return false
        end
        return missing_or_exception((a, b))
    end
end

>=(a::NumberInterval, b::NumberInterval) = b <= a

function iszero(a::NumberInterval)
    if !contains_zero(a)
        return false
    end
    if a ⊆ zero(typeof(a))
        return true
    end
    return missing_or_exception(a)
end

function ==(a::NumberInterval, b::NumberInterval)
    if isnan(a) || isnan(b)
        return false
    elseif isdisjoint(a, b)
        return false
    elseif issingleton(a) && issingleton(b) && a ⊆ b
        return true
    end
    return missing_or_exception((a, b))
end

function sign(a::NumberInterval)
    z = zero(typeof(a))
    if a ≺ z
        return -1
    elseif z ≺ a
        return +1
    elseif a ⊆ z
        return  0
    end
    return missing_or_exception(a)
end

function signbit(a::NumberInterval)
    z = zero(typeof(a))
    if a ≺ z
        return true
    elseif precedes(z, a)
        return false
    end
    return missing_or_exception(a)
end


# This file is part of RLinearAlgebra.jl

# using LinearAlgebra

"""
    LSLogOracle <: LinSysSolverLog

A mutable structure that stores information about a randomized linear solver's behavior.
    The log assumes that the true solution of the system is known and will be supplied.
    The goal of this log is usually for research, development, or testing.

# Fields
- `solution::AbstractVector`, a solution to the given linear system.
- `collection_rate::Int64`, the frequency with which to record information to append to the
    remaining fields, starting with the initialization (i.e., iteration 0).
- `error_hist::Vector{Float64}`, retains a vector of numbers corresponding to the error
    between the iterates of the solver and the `solution`
- `error_norm::Function`, a function that accepts a single vector argument and returns a
    scalar. Used to compute the error size.
- `resid_hist::Vector{Float64}`, retains a vector of numbers corresponding to the residual
    (uses the whole system to compute the residual)
- `resid_norm::Function`, a function that accepts a single vector argument and returns a
    scalar. Used to compute the residual size.
- `iterations::Int64`, the number of iterations of the solver.
- `converged::Bool`, a flag to indicate whether the system has converged by some measure

# Constructors
- Calling `LSLogOracle(x_star::Vector{Float64})` sets `solution = x_star`,
    `collection_rate = 1`, `error_hist = Float64[]`, `error_norm = norm` (Euclidean norm),
    `resid_hist = Float64[]`, `resid_norm = norm` (Euclidean norm), `iterations=-1`,
    and `converged = false`.
- Calling `LSLogOracle(x_star::Vector{Float64}, cr::Int64)` sets the structure with the
    same parameters as for `LSLogOracle(x_star)` except `collection_rate = cr`.
"""
mutable struct LSLogOracle <: LinSysSolverLog
    solution::AbstractVector
    collection_rate::Int64
    error_hist::Vector{Float64}
    error_norm::Function
    resid_hist::Vector{Float64}
    resid_norm::Function
    iterations::Int64
    converged::Bool
end
LSLogOracle(x_star::Vector{Float64}) = LSLogOracle(x_star, 1, Float64[], norm, Float64[],
    norm, -1, false)
LSLogOracle(x_star::Vector{Float64}, cr::Int64) = LSLogOracle(x_star, cr, Float64[], norm,
    Float64[],norm, -1, false)

# Common interface for update
function log_update!(
    log::LSLogOracle,
    sampler::LinSysSampler,
    x::AbstractVector,
    samp::Tuple,
    iter::Int64,
    A::AbstractArray,
    b::AbstractVector
)
    # Update iteration counter
    log.iterations = iter

    # Check whether to record information
    if mod(iter, 1:log.collection_rate) == log.collection_rate
        # Push absolute error between iterate and solution
        push!(log.error_hist, log.error_norm(x - log.solution))

        # Push residual norm of iterate
        push!(log.resid_hist, log.resid_norm(A * x - b))
    end

    return nothing
end


# vim: set ft=julia sw=4 ts=4:
#
# Fibonacci.jl
#
# Calculate Fibonacci numbers in Julia

module Fibonacci

export fib_naive, fib

"Naive implementation of Fibonacci number calculation"
function fib_naive(n)
	if n < 2
		n
	else
		fib_naive(n-2) + fib_naive(n-1)
	end
end

"Tail-call optimized implementation"
function fib(n)
	function aux(n, a, b)
		if n < 1
			a
		else
			aux(n-1, b, a+b)
		end
	end
	aux(n, BigInt(0), BigInt(1))
end

end # module


using DataStructure101
const DS = DataStructure101

export bfs

function bfs(g::DS.AbstractGraph, v::Int64)
    traversed = Int[]
    queue = Int[]
    push!(queue, v)

    while length(queue) > 0
        current_v = queue[1]
        push!(traversed, popfirst!(queue))

        neighbors = DS.neighbor(g, current_v)
        for n in neighbors
            if !(n in traversed || n in queue) push!(queue, n) end
        end
    end

    return traversed
end


##### ----- Power Law ----- #####
# Calculate probability density for a residence time τ given α of power law,
# minimum and maximum residence times τ₋ and τ₊
function E_powerLaw(τ, α, τ₋, τ₊)
    ((τ^(-α) - τ₊^(-α))*(-α+1.0)/(τ₊^(-α+1.0)-τ₋^(-α+1.0)-τ₊^(-α)*(τ₊ - τ₋)*(1.0-α)))
end

# Calculate rate of hyporheic exchange given a hyporheic zone volume (S)
# and parameters describing the pdf of the residence time distribution
function qCalc_powerLaw(S, τ₋, τ₊, α)
    S/quadgk(τ -> (τ₊^(-α+1.00) - τ^(-α+1.00)- (τ₊^(-α)*(τ₊-τ)*(-α+1.00)))/(τ₊^(-α+1.00) - τ₋^(-α+1.00)- (τ₊^(-α)*(τ₊-τ₋)*(-α+1.00))), τ₋, τ₊)[1]
end

##### ----- Exponential ----- #####


function ellpic_bulirsch(n::T,k2::T) where {T <: Real}
# Computes the complete elliptical integral of the third kind using
# the algorithm of Bulirsch (1965):
kc = sqrt(1.0-k2)
ca = sqrt(eps(n))
if (kc*(n+1.0)) == 0
  ellpic = NaN
else
  ee = kc
  m0 = 1.0
  if n > -1.0
    c = 1.0
    p = sqrt(n+1.0)
    d = 1.0/p
  else
    g = -n
    f = -k2-n
    p = sqrt(f/g)
    d = -k2/(g*p)
    c = 0.0
  end

  f = copy(c)
  c += d/p
  g = ee/p
  d = (f*g+d)*2.0
  p += g
  g = m0
  m0 += kc
  while abs(g-kc) > (ca*g)
    kc = 2.0*sqrt(ee)
    ee = kc*m0
    f = copy(c)
    c += d/p
    g = ee/p
    d = (f*g+d)*2.0
    p += g
    g = m0
    m0 += kc
  end
  ellpic= 0.5*pi*(c*m0+d)/(m0*(m0+p))
end
return ellpic
end


# TODO: Test
function prob_miss(y::AbstractFloat, beta::Vector{Float64})
  n = length(beta)
  x = sum((y .^ (0:n-1)) .* beta)
  return MCMC.sigmoid(x)
end

function assertValidBounds(y::Vector{Float64}, p::Vector{Float64})
  n = length(y)
  @assert length(y) == length(p) == n

  @assert 2 <= n <= 3

  if n == 3
    @assert p[1] < p[2] > p[3] # Quadratic missing mechanism
    @assert y[1] < y[2] < y[3]
  elseif n ==2
    @assert p[1] > p[2] # Linear missing mechanism
    @assert y[1] < y[2]
  else
    println("In assertValidBounds: This should never be printed!")
    @assert false
  end
end

function solveBeta(y::Vector{Float64}, p::Vector{Float64})
  assertValidBounds(y, p)

  n = length(y)
  Y = [fill(1.0, n) (y) (y .^ 2)]
  beta = Y \ MCMC.logit.(p)

  @assert all(abs.(MCMC.sigmoid.(Y * beta) - p) .< 1E-10)

  return beta
end


include("src/Modeling.jl")
include("src/Controls.jl")
using Plots
ENV["GKSwstype"]="nul" # Call this if using WSL

mutable struct Params
    mc::Float64
    mp::Float64
    g::Float64
    l::Float64
    function Params()
        return new(10., 1., 10., 2.)
    end
end

# Differential equation used in model of inverted pendulum
function xdot(x::Vector{Float64}, params::Params, force::Float64)
    return [
        x[2],
        (-params.mp * params.g * sin(x[3]) * cos(x[3]) + params.mp * params.l * sin(x[3]) * x[4] ^ 2 + force) /
        (params.mc + params.mp - params.mp * sin(x[3])^2),
        x[4],
        (-params.g * (params.mc + params.mp) * sin(x[3]) + params.mp * params.l * sin(x[3]) * cos(x[3]) * x[4] ^ 2 + force * cos(x[3])) /
        (-params.l * (params.mc + params.mp) + params.mp * params.l * cos(x[3]) ^ 2)
    ]
end

params = Params()

# Linearized State-Space model for making the controller
A = [
    0 1 0 0;
    0 0 (params.mp * params.g / params.mc) 0;
    0 0 0 1;
    0 0 (params.g * (params.mc + params.mp) / params.mc / params.l) 0
]

B = [0; 1 / params.mc; 0; -1 / params.mc / params.l]

C = [1 0 0 0]

ss = StateSpace(A, B, C)
controller = Controller(ss, [-1, -1.1, -1.2, -1.3])

# Setting up time constraints
dt = 0.01
t = 0:dt:20
x = [[0., 0., 0., 0.]] #Initial conditions of pendulum
u = [0.]

# Iterating through all the time steps
for i in t[2:end]
    push!(u, get_u(controller, last(x), 0.0))
    push!(x, rk4(xdot, last(x), dt, params, last(u)))
    if i == 1.0
        last(x)[4] = -2.0 # Instantaneous change in angular velocity, simulating flicking the ball
    end
end

# Making GIF

anim = @animate for i in 1:length(t)
    p1 = plot([x[i][1], x[i][1] + params.l * sin(x[i][3])],
        [0, params.l * cos(x[i][3])],
        leg = false,
        xlims = (-10, 1),
        ylims = (-1, 2.5),
        aspect_ratio = 1)
    scatter!(p1, [x[i][1]], [0], m = (:rect, 12))
    scatter!(p1, [x[i][1] + params.l * sin(x[i][3])], [params.l * cos(x[i][3])], m = (:circle, 8))
    p2 = plot(t, [j[1] for j in x], label = "Position", legend_position = :bottomright)
    scatter!(p2, [t[i]], [x[i][1]], label = "")
    p3 = plot(t, [j[3] for j in x], label = "Angle")
    scatter!(p3, [t[i]], [x[i][3]], label = "")
    plot(p1,p2, p3, layout = (3,1), size = (1200, 800))
end every 10;
gif(anim, "gifs/Inverted_pendulum.gif");


using ADCME
using ADSeismic
using PyPlot
using DelimitedFiles
using JLD2

scale = 201

param = AcousticPropagatorParams(NX=scale, NY=scale,
    NSTEP=1000, DELTAT=1e-4,  DELTAX=1.0, DELTAY=1.0,
    PropagatorKernel = 0, Rcoef = 1e-8)

rc = Ricker(param, 30.0, 200.0, 1e6)
srci = [div(param.NX,2)]
srcj = [div(param.NY,5)]
srcv = reshape(rc, :, 1)
src = AcousticSource(srci, srcj, srcv)

layers = ones(param.NX+2, param.NY+2)
n_piece = div(param.NX + 1, 3) + 1
for k = 1:3
    i_interval = (k-1)*n_piece+1:min(k*n_piece, param.NX+2)
    layers[:, i_interval] .= 0.5 + (k-1)*0.25
end

C = placeholder(3300*layers)
model = AcousticPropagatorSolver(param, src, C)

sess = Session(); init(sess)
u = run(sess, model.u)

p = visualize_wavefield(u, param)
saveanim(p, "wavefield.gif")

visualize_model(3300*layers, param)
savefig("model.png")

close("all")
plot(LinRange(0, 1, 1000), run(sess, rc))
xlabel("Time")
savefig("ricker.png")

@save "data.jld2" u


using ApproxFunRational, AKNS, SpecialFunctions, ApproxFun, AbstractIterativeSolvers
using Test

##
@testset "Fourier transform" begin
     f = x -> exp(-x^2)
     fhat = k -> sqrt(pi)*exp(-k^2/4)
     n = 201
     Fhat = ODEFT(f,n,n)
     @test abs(Fhat(-1.0)[1] - fhat(-1.0)) < 1e-14
end

@testset "Forward scattering transform" begin
    A = 1.65; γ = 0.1;
    w = z -> -1im*z - A*γ*1im/2 + 0.5
    T = sqrt(γ^2/4 -1 |> Complex)
    wp = -1im*A*(T + γ/2)
    wm = 1im*A*(T-γ/2)

    q = x -> -1im*A*sech(x)*exp(-1im*γ*A*log(cosh(x)))
    r = x -> -conj(q(x))

    logasech = z -> loggamma(w(z)) + loggamma(w(z)-wm-wp) - loggamma(w(z)-wp) - loggamma(w(z)-wm)
    logbsech = z -> loggamma(w(z)) + loggamma(1-w(z)+wp+wm) - loggamma(wp) - loggamma(wm)
    ρsechexplog = z -> 1im*1/A*2^(-1im*γ*A)*exp(logbsech(z) - logasech(z))
    bsechexplog = z -> 1im*1/A*2^(-1im*γ*A)*exp(logbsech(z))
    asechexplog = z -> exp(logasech(z))

    S = AKNS.AKNSscattering(q,r,300,400,12.0)
    @test abs(S(1.0)[1,1] - asechexplog(1.0)) < 1e-11
    @test abs(S(1.0)[2,1] - bsechexplog(1.0)) < 1e-11
end

@testset "Inverse scattering transform" begin
    A = 1.65; γ = 2.0;
    w = z -> -1im*z - A*γ*1im/2 + 0.5
    T = sqrt(γ^2/4 -1 |> Complex)
    wp = -1im*A*(T + γ/2)
    wm = 1im*A*(T-γ/2)

    q = x -> -1im*A*sech(x)*exp(-1im*γ*A*log(cosh(x)))
    r = x -> -conj(q(x))

    logasech = z -> loggamma(w(z)) + loggamma(w(z)-wm-wp) - loggamma(w(z)-wp) - loggamma(w(z)-wm)
    logbsech = z -> loggamma(w(z)) + loggamma(1-w(z)+wp+wm) - loggamma(wp) - loggamma(wm)
    ρsechexplog = z -> 1im*1/A*2^(-1im*γ*A)*exp(logbsech(z) - logasech(z))
    bsechexplog = z -> 1im*1/A*2^(-1im*γ*A)*exp(logbsech(z))
    asechexplog = z -> exp(logasech(z))

    𝓒⁺ = Cauchy(+1)
    𝓒⁻ = Cauchy(-1)
    x = 1.0
    ρ1 = Fun(ρsechexplog,OscLaurent(2x,1.0),240)
    ρ2 = Fun(x -> -conj(ρsechexplog(x)) ,OscLaurent(-2x,1.0),240)
    Z = Fun(OscLaurent(0.0im,1.0),zeros(240)*0im)
    J2 = map(SumFun,[ρ1, 0*ρ2])
    J1 = map(SumFun,[0*ρ1, -ρ2])

    function Sop(x)
        out = copy(x)
        out[1] -= (𝓒⁺*x[2])*ρ1
        out[2] += (𝓒⁻*x[1])*ρ2
        return out
    end

    tol = 2e-12
    simp(f) = chop(combine!(chop(f,tol/1000000)),tol/1000000)
    out = GMRES(Sop,J1,⋅,tol,14,x -> simp(x))
    sol = +([out[2][i]*out[1][i] for i=1:length(out[2])]...)
    u = -(sum(sol)/pi)[2]
    println(abs(u - q(x)))
    @test abs(u - q(x)) < 1e-9  # Weakened because linux is not passing with 1e-12 on 1.5, need to investigate?
end


"""
    slope(spl::Spline,dλ)

Returns the pair of `λ, s'` (1st derivative of the spline function).
"""
function slope(spl::Spline,dλ)
    xs=collect(dλ)#,(x[end]-x[1])/dλ)  # spline x vector
    s=zeros(length(xs))         # spline spectral vector
    j=1
    @inbounds for i in 1 : length(xs)
        if j < length(spl.x)
            while xs[i] > spl.x[j+1]
                j+=1
            end
            delta = xs[i]-spl.x[j]
            if typeof(spl) == CubicSpline
                s[i]= 3.0 * spl.d[j] * delta * delta + 2.0 * spl.c[j] * delta + spl.b[j]
            elseif typeof(spl) == HermiteSpline
                xb = (xs[i] - spl.x[j]) / (spl.x[j+1] - spl.x[j])
                xb2 = xb * xb
                #Φ0_ = 6 * xb^2 - 6 * xb
                #Φ1_ = 3 * xb^2 - 4 * xb + 1
                #Ψ0_ = -6 * xb^2 + 6 * xb
                #Ψ1_ = 3 * xb^2 - 2 * xb
                h = spl.x[j+1] - spl.x[j]
                #s[i] = (spl.a[j] * Φ0_ + h * spl.b[j] * Φ1_ + spl.a[j+1] * Ψ0_ + h * spl.b[j+1] * Ψ1_) / h
                s[i] = (spl.a[j] * (6 * xb2 - 6 * xb) + h * spl.b[j] * (3 * xb2 - 4 * xb + 1) + spl.a[j+1] * (-6 * xb2 + 6 * xb) + h * spl.b[j+1] * (3 * xb2 - 2 * xb)) / h
            elseif typeof(spl) == QuadraticSpline
                s[i]= 2.0 * spl.c[j] * delta + spl.b[j]
            elseif typeof(spl) == LinearSpline
                s[i] = spl.b[j]
            elseif typeof(spl) == NearestNeighborSpline
                s[i] = 0.0
            end
        end
    end
    return xs,s
end

# alias
const deriv1 = slope

"""
    curvature(spl::Spline,dλ)

Returns the 2nd derivative of the spline function (s'')
"""
function curvature(spl::Spline,dλ)
    xs=collect(dλ)#,(x[end]-x[1])/dλ)  # spline x vector
    s=zeros(length(xs))         # spline spectral vector
    j=1
    @inbounds for i in 1 : length(xs)
        if j < length(spl.x)
            while xs[i] > spl.x[j+1]
                j+=1
            end
            delta = xs[i]-spl.x[j]
            if typeof(spl) == CubicSpline
                s[i] = 6.0 * spl.d[j] * delta + 2.0 * spl.c[j]
            elseif typeof(spl) == HermiteSpline
                xb = (xs[i] - spl.x[j]) / (spl.x[j+1] - spl.x[j])
                #Φ0__ = 12 * xb - 6
                #Φ1__ = 6 * xb - 4
                #Ψ0__ = -12 * xb + 6
                #Ψ1__ = 6 * xb - 2
                h = spl.x[j+1] - spl.x[j]
                #s[i] = (spl.a[j] * Φ0__ + h * spl.b[j] * Φ1__ + spl.a[j+1] * Ψ0__ + h * spl.b[j+1] * Ψ1__) / h
                s[i] = (spl.a[j] * (12 * xb - 6) + h * spl.b[j] * (6 * xb - 4) + spl.a[j+1] * (-12 * xb + 6) + h * spl.b[j+1] * (6 * xb - 2)) / h
                #s[i] = 2.0 * spl.b[j]
            elseif typeof(spl) == QuadraticSpline
                s[i] = 2.0 * spl.c[j]
            elseif typeof(spl) == LinearSpline
                s[i] = 0.0
            elseif typeof(spl) == NearestNeighborSpline
                s[i] = 0.0
            else
                nothing
            end
        end
    end
    return xs,s
end

# alias
const deriv2 = curvature

"""
    curvrate(spl::Spline,dλ)

Returns the 3rd derivative of the spline function (s''').
"""
function curvrate(spl::Spline,dλ)
    xs=collect(dλ)#,(x[end]-x[1])/dλ)  # spline x vector
    s=zeros(length(xs))         # spline spectral vector
    j=1
    @inbounds for i in 1 : length(xs)
        if j < length(spl.x)
            while xs[i] > spl.x[j+1]
                j+=1
            end
            typeof(spl) == CubicSpline ? s[i]= 6.0 * spl.d[j] :
            typeof(spl) == QuadraticSpline ? s[i] = 0.0 :
            typeof(spl) == LinearSpline ? s[i] = 0.0 :
            typeof(spl) == NearestNeighborSpline ? s[i] = 0.0 : nothing
        end
    end
    return xs,s
end

# alias
const deriv3 = curvrate


export
    Quat,
    quat_for_a2b,
    get_axis,
    get_rotation_angle,

    RPY

struct Quat <: FieldVector{4, Float64}
    x::Float64
    y::Float64
    z::Float64
    w::Float64
end
Quat(v::VecE3, w::Float64) = Quat(v.x, v.y, v.z, w)
Quat(w::Float64, v::VecE3) = Quat(v.x, v.y, v.z, w)

Base.show(io::IO, q::Quat) = @printf(io, "Quat({%6.3f, %6.3f, %6.3f}, %6.3f)", q.x, q.y, q.z, q.w)

"""
Conjugate of a quaternion
"""
Base.conj(q::Quat) = Quat(-q.x,-q.y,-q.z, q.w)

"""
A VecE3 of the imaginary part (x,y,z)
"""
Base.imag(q::Quat) = VecE3(q.x, q.y, q.z)

"""
Return the multiplicative inverse of q.
Note that in most cases, i.e., if you simply want the opposite rotation,
and/or the quaternion is normalized, then it is enough to use the conjugate.
Based on Eigen's inverse.

Returns the zero Quat if q is zero.
"""
function Base.inv(q::Quat)
  n2 = q.x^2 + q.y^2 + q.z^2 + q.w^2 # squared norm
  if n2 > 0
    return Quat(conj(q) / n2)
  else
    # return an invalid result to flag the error
    return Quat(0.0,0.0,0.0,0.0)
  end
end

"""
Rotate a vector by a quaternion.
If you are going to rotate a bunch of vectors, covert q to a rotation matrix first.

Based on the Eigen implementation for _transformVector
"""
function rot(q::Quat, v::VecE3)
    uv = imag(q)×v
    uv += uv
    return v + q.w*uv + imag(q)×uv;
end

"""
Quaternion multiplication
"""
function Base.:*(a::Quat, b::Quat)
    r1, v1 = a.w, imag(a)
    r2, v2 = b.w, imag(b)
    return Quat(r1*v2 + r2*v1 + v1×v2, r1*r2 - v1⋅v2)
end
Base.:*(v::VecE3, b::Quat) = Quat(0.0, v) * b
Base.:*(a::Quat, v::VecE3) = a * Quat(0.0, v)

"""
The angle (in radians) between two rotations
"""
function angledist(a::Quat, b::Quat)
    d = a * conj(b)
    return 2*atan2(norm(imag(d)), abs(d.w))
end

"""
The quaternion that transforms a into b through a rotation
Based on Eigen's implementation for setFromTwoVectors
"""
function quat_for_a2b(a::VecE3, b::VecE3)
    v0 = normalize(a)
    v1 = normalize(b)
    c = v1⋅v0

    # if dot == -1, vectors are nearly opposites
    # => accurately compute the rotation axis by computing the
    #    intersection of the two planes. This is done by solving:
    #       x^T v0 = 0
    #       x^T v1 = 0
    #    under the constraint:
    #       ||x|| = 1
    #    which yields a singular value problem
    if c < -1.0 + DUMMY_PRECISION
        c = max(c, -1.0)
        M = hcat(convert(Vector{Float64}, v0),
                 convert(Vector{Float64}, v1))'
        s = svdfact(M, thin=false)
        axis = convert(VecE3, s[:V][:,2])
        w2 = (1.0 + c)*0.5
        return Quat(axis * sqrt(1.0 - w2), sqrt(w2))
    else
        axis = v0×v1
        s = sqrt((1.0+c)*2.0)
        invs = 1.0/s
        return Quat(axis * invs, s * 0.5)
    end
end

"""
Returns the spherical linear interpolation between the two quaternions
a and b for t ∈ [0,1].

Based on the Eigen implementation for slerp.
"""
function lerp(a::Quat, b::Quat, t::Float64)

    d = a⋅b
    absD = abs(d)

    if(absD ≥ 1.0 - eps())
        scale0, scale1 = 1.0 - t, t
    else
        # θ is the angle between the 2 quaternions
        θ = acos(absD)
        sinθ = sin(θ)

        scale0, scale1 = sin( ( 1.0 - t ) * θ) / sinθ,
                         sin( ( t * θ) ) / sinθ
    end

    if d < 0
        scale1 = -scale1
    end

    return Quat(scale0*a + scale1*b)
end

"""
The rotation axis for a quaternion.
"""
function get_axis(q::Quat)

    θ = 2*atan2(sqrt(q.x*q.x + q.y*q.y + q.z*q.z), q.w)
    h = sin(θ/2)

    x = q.x / h
    y = q.y / h
    z = q.z / h

    VecE3(x, y, z)
end

"""
The rotation angle for a quaternion.
"""
get_rotation_angle(q::Quat) = 2.0*acos(q.w)

"""
Draw a random unit quaternion following a uniform distribution law on SO(3).

Based on the Eigen implementation for UnitRandom.
"""
function Base.rand(::Type{Quat})
    u1 = rand()
    u2 = 2π*rand()
    u3 = 2π*rand()
    a = sqrt(1 - u1)
    b = sqrt(u1)
    return Quat(a*sin(u2), a*cos(u2), b*sin(u3), b*cos(u3))
end

"""
Roll Pitch Yaw
"""
struct RPY <: FieldVector{3, Float64}
    r::Float64
    p::Float64
    y::Float64
end
function Base.convert(::Type{RPY}, q::Quat)

    q2 = normalize(q)
    x = q2[1]
    y = q2[2]
    z = q2[3]
    w = q2[4]

    # roll (x-axis rotation)
    sinr = 2(w * x + y * z)
    cosr = 1.0 - 2(x * x + y * y)
    roll = atan2(sinr, cosr)

    # pitch (y-axis rotation)
    sinp = 2(w * y - z * x)
    if (abs(sinp) >= 1)
        pitch = π/2 * sign(sinp) # use 90 degrees if out of range
    else
        pitch = asin(sinp)
    end

    # yaw (z-axis rotation)
    siny = 2(w * z + x * y)
    cosy = 1.0 - 2(y * y + z * z)
    yaw = atan2(siny, cosy)

    RPY(roll, pitch, yaw)
end
function Base.convert(::Type{Matrix{Float64}}, quat::Quat)

    # convert it to a rotation matrix

    w = quat.w
    x = quat.x
    y = quat.y
    z = quat.z

    n = w * w + x * x + y * y + z * z
    s = (n == 0.0) ? 0.0 : 2.0 / n
    wx = s * w * x; wy = s * w * y; wz = s * w * z
    xx = s * x * x; xy = s * x * y; xz = s * x * z
    yy = s * y * y; yz = s * y * z; zz = s * z * z

    [ 1 - (yy + zz)         xy - wz          xz + wy;
           xy + wz     1 - (xx + zz)         yz - wx;
           xz - wy          yz + wx     1 - (xx + yy) ]
end
function Base.convert(::Type{Quat}, rpy::RPY)
    t0 = cos(rpy.y/2)
    t1 = sin(rpy.y/2)
    t2 = cos(rpy.r/2)
    t3 = sin(rpy.r/2)
    t4 = cos(rpy.p/2)
    t5 = sin(rpy.p/2)

    qw = t0 * t2 * t4 + t1 * t3 * t5
    qx = t0 * t3 * t4 - t1 * t2 * t5
    qy = t0 * t2 * t5 + t1 * t3 * t4
    qz = t1 * t2 * t4 - t0 * t3 * t5

    return Quat(qx, qy, qz, qw)
end


# DifferentialEquations
using DifferentialEquations
f(u,p,t) = 1.01*u
u0 = 1/2
tspan = (0.0,1.0)
prob = ODEProblem(f,u0,tspan)
sol = solve(prob, Tsit5(), reltol=1e-8, abstol=1e-8)

# DiffEqSensitivity
using DiffEqSensitivity, OrdinaryDiffEq, Zygote
function fiip(du,u,p,t)
  du[1] = dx = p[1]*u[1] - p[2]*u[1]*u[2]
  du[2] = dy = -p[3]*u[2] + p[4]*u[1]*u[2]
end
p = [1.5,1.0,3.0,1.0]; u0 = [1.0;1.0]
prob = ODEProblem(fiip,u0,(0.0,10.0),p)
sol = concrete_solve(prob,Tsit5())


using Flux
println("Integro Differential equation tests")
using DiffEqFlux
println("Starting Soon!")
using ModelingToolkit
using DiffEqBase
using Test, NeuralPDE
println("Starting Soon!")
using GalacticOptim
using Optim
using Quadrature,Cubature, Cuba
using QuasiMonteCarlo
using SciMLBase
using OrdinaryDiffEq
import ModelingToolkit: Interval, infimum, supremum
using DomainSets

using Random
Random.seed!(100)

cb = function (p,l)
    println("Current loss is: $l")
    return false
end

#Integration Tests
println("Integral Tests")
@parameters t
@variables i(..)
Di = Differential(t)
Ii = Integral(t in DomainSets.ClosedInterval(0, t))
eq = Di(i(t)) + 2*i(t) + 5*Ii(i(t)) ~ 1
bcs = [i(0.) ~ 0.0]
domains = [t ∈ Interval(0.0,2.0)]
chain = Chain(Dense(1,15,Flux.σ),Dense(15,1))
initθ = Float64.(DiffEqFlux.initial_params(chain))
strategy_ = NeuralPDE.GridTraining(0.1)
discretization = NeuralPDE.PhysicsInformedNN(chain,
                                             strategy_;
                                             init_params = nothing,
                                             phi = nothing,
                                             derivative = nothing,
                                             )
@named pde_system = PDESystem(eq,bcs,domains,[t],[i(t)])
sym_prob = NeuralPDE.symbolic_discretize(pde_system, discretization)
prob = NeuralPDE.discretize(pde_system,discretization)
res = GalacticOptim.solve(prob, BFGS(); cb = cb, maxiters=100)
ts = [infimum(d.domain):0.01:supremum(d.domain) for d in domains][1]
phi = discretization.phi

analytic_sol_func(t) = 1/2*(exp(-t))*(sin(2*t))
u_real  = [analytic_sol_func(t) for t in ts]
u_predict  = [first(phi([t],res.minimizer)) for t in ts]
@test Flux.mse(u_real, u_predict) < 0.001
# plot(ts,u_real)
# plot!(ts,u_predict)

## Simple Integral Test
println("Simple Integral Test")

@parameters x
@variables u(..)
Ix = Integral(x in DomainSets.ClosedInterval(0, x))
eq = Ix(u(x)) ~ (x^3)/3
bcs = [u(0.) ~ 0.0]
domains = [x ∈ Interval(0.0,1.00)]
chain = Chain(Dense(1,15,Flux.σ),Dense(15,1))
initθ = Float64.(DiffEqFlux.initial_params(chain))
strategy_ = NeuralPDE.GridTraining(0.1)
discretization = NeuralPDE.PhysicsInformedNN(chain,
                                             strategy_;
                                             init_params = nothing,
                                             phi = nothing,
                                             derivative = nothing,
                                             )
@named pde_system = PDESystem(eq,bcs,domains,[x],[u(x)])
prob = NeuralPDE.discretize(pde_system,discretization)
res = GalacticOptim.solve(prob, BFGS(); cb = cb, maxiters=100)
xs = [infimum(d.domain):0.01:supremum(d.domain) for d in domains][1]
phi = discretization.phi
u_predict  = [first(phi([x],res.minimizer)) for x in xs]
u_real  = [x^2 for x in xs]
@test Flux.mse(u_real, u_predict) < 0.001

# plot(xs,u_real)
# plot!(xs,u_predict)

#simple multidimensitonal integral test
println("simple multidimensitonal integral test")

@parameters x,y
@variables u(..)
Dx = Differential(x)
Dy = Differential(y)
Ix = Integral((x,y) in DomainSets.UnitSquare())
eq = Ix(u(x,y)) ~ 1/3
bcs = [u(0., 0.) ~ 1, Dx(u(x,y)) ~ -2*x , Dy(u(x ,y)) ~ -2*y ]
domains = [x ∈ Interval(0.0,1.00), y ∈ Interval(0.0,1.00)]
chain = Chain(Dense(2,15,Flux.σ),Dense(15,1))
initθ = Float64.(DiffEqFlux.initial_params(chain))
strategy_ = NeuralPDE.GridTraining(0.1)
discretization = NeuralPDE.PhysicsInformedNN(chain,
                                             strategy_;
                                             init_params = nothing,
                                             phi = nothing,
                                             derivative = nothing,
                                             )
@named pde_system = PDESystem(eq,bcs,domains,[x,y],[u(x, y)])
prob = NeuralPDE.discretize(pde_system,discretization)
res = GalacticOptim.solve(prob, BFGS(); cb = cb, maxiters=100)
xs = 0.00:0.01:1.00
ys = 0.00:0.01:1.00
phi = discretization.phi

u_real = collect(1 - x^2 - y^2 for y in ys, x in xs);
u_predict = collect(Array(phi([x,y], res.minimizer))[1] for y in ys, x in xs);
@test Flux.mse(u_real, u_predict) < 0.001

# error_ = u_predict .- u_real
# p1 = plot(xs,ys,u_real,linetype=:contourf,label = "analytic")
# p2 = plot(xs,ys,u_predict,linetype=:contourf,label = "predict")
# p3 = plot(xs,ys,error_,linetype=:contourf,label = "error")
# plot(p1,p2,p3)

@parameters x,y
@variables u(..)
Dx = Differential(x)
Dy = Differential(y)
Ix = Integral((x,y) in DomainSets.ProductDomain(UnitInterval(),ClosedInterval(0 ,x)))
eq = Ix(u(x,y)) ~ 5/12
bcs = [u(0., 0.) ~ 0, Dy(u(x,y)) ~ 2*y , u(x, 0) ~ x ]
domains = [x ∈ Interval(0.0,1.00), y ∈ Interval(0.0,1.00)]
chain = Chain(Dense(2,15,Flux.σ),Dense(15,1))
initθ = Float64.(DiffEqFlux.initial_params(chain))
strategy_ = NeuralPDE.GridTraining(0.1)
discretization = NeuralPDE.PhysicsInformedNN(chain,
                                             strategy_;
                                             init_params = nothing,
                                             phi = nothing,
                                             derivative = nothing,
                                             )
@named pde_system = PDESystem(eq,bcs,domains,[x,y],[u(x, y)])
prob = NeuralPDE.discretize(pde_system,discretization)
res = GalacticOptim.solve(prob, BFGS(); cb = cb, maxiters=100)
xs = 0.00:0.01:1.00
ys = 0.00:0.01:1.00
phi = discretization.phi

u_real = collect( x + y^2 for y in ys, x in xs);
u_predict = collect(Array(phi([x,y], res.minimizer))[1] for y in ys, x in xs);
@test Flux.mse(u_real, u_predict) < 0.01

# error_ = u_predict .- u_real
# p1 = plot(xs,ys,u_real,linetype=:contourf,label = "analytic")
# p2 = plot(xs,ys,u_predict,linetype=:contourf,label = "predict")
# p3 = plot(xs,ys,error_,linetype=:contourf,label = "error")
# plot(p1,p2,p3)


using PyPlot

mutable struct point
    x::Float64
    y::Float64
end

function inCone(pOrg, pDeb, pFin, pCur)
    # pOrg : point origine du cone (la ou il est pointe)
    # pDeb : point depart du cone (point du rayon [pOrg,pDeb])
    # pFin : point final du cone (point du rayon [pOrg,pFin])
    # pCur : point courant a tester
    # retourne VRAI si pCur est dans le cone pDeb-pFin-pOrg, FAUX sinon

    cp_pDeb_pFin = (pDeb.x - pOrg.x) * (pFin.y - pOrg.y) - (pDeb.y - pOrg.y) * (pFin.x - pOrg.x)
    cp_pDeb_pCur = (pDeb.x - pOrg.x) * (pCur.y - pOrg.y) - (pDeb.y - pOrg.y) * (pCur.x - pOrg.x)
    cp_pFin_pCur = (pFin.x - pOrg.x) * (pCur.y - pOrg.y) - (pFin.y - pOrg.y) * (pCur.x - pOrg.x)

    if (cp_pDeb_pFin > 0)
        if ((cp_pDeb_pCur >= 0) && (cp_pFin_pCur <= 0))
            return true
        else
            return false
        end
    else
        if (!((cp_pDeb_pCur < 0) && (cp_pFin_pCur > 0)))
            return true
        else
            return false
        end
    end
end

function inCone1VersZ(pOrg, pDeb, pFin, pCur)
    return inCone(pOrg, pDeb, pFin, pCur)
end

function inCone2Vers0(pOrg, pDeb, pFin, pCur)
    return !inCone(pOrg, pDeb, pFin, pCur)
end

xlim(0,10)
ylim(0,10)
B=point(2.0,5.0); scatter(2.0,5.0,marker=".",color="black")
O=point(2.5,2.5); scatter(2.5,2.5,marker="x",color="red")
A=point(5.0,2.0); scatter(5.0,2.0,marker=".",color="black")
#N=point(5.0,5.0); scatter(5.0,5.0,marker="+")

x=round(rand()*8+2;digits=2); y=round(rand()*8+2;digits=2)
N=point(x,y); scatter(x,y,marker="x",color="blue")

x=round(rand()*10;digits=2); y=round(rand()*10;digits=2)
M=point(x,y); scatter(x,y,marker="o",color="magenta")

lx1=[O.x,B.x]; ly1=[O.y,B.y]; plot(lx1,ly1,color="red")
lx1=[O.x,0]; ly1=[O.y,(B.y-O.y)/(B.x-O.x)*(0-O.x)+O.y]; plot(lx1,ly1,color="red",linestyle="dotted")
lx2=[O.x,A.x]; ly2=[O.y,A.y]; plot(lx2,ly2,color="red")
lx2=[O.x,10]; ly2=[O.y,(A.y-O.y)/(A.x-O.x)*(10-O.x)+O.y]; plot(lx2,ly2,color="red",linestyle="dotted")

lx3=[N.x,B.x]; ly3=[N.y,B.y]; plot(lx3,ly3,color="blue",linestyle="dashed")
lx4=[N.x,A.x]; ly4=[N.y,A.y]; plot(lx4,ly4,color="blue",linestyle="dashed")

@show inCone1VersZ(O, A, B, M)
@show inCone2Vers0(N, A, B, M)
nothing


using Revise
using AdFem
using PyCall
using LinearAlgebra
using PyPlot
using SparseArrays
np = pyimport("numpy")

β = 1/4; γ = 1/2
a = b = 0.1
m = 40
n = 20
h = 0.01
NT = 200
Δt = 5/NT
bdedge = []
for j = 1:n
  push!(bdedge, [(j-1)*(m+1)+m+1 j*(m+1)+m+1])
end
bdedge = vcat(bdedge...)

bdnode = Int64[]
for j = 1:n+1
  push!(bdnode, (j-1)*(m+1)+1)
end

M = compute_fem_mass_matrix1(m, n, h)
S = spzeros((m+1)*(n+1), (m+1)*(n+1))
M = [M S;S M]

H = [1.0 0.0 0.0
    0.0 1.0 0.0
    0.0 0.0 0.5]
K = compute_fem_stiffness_matrix(H, m, n, h)
C = a*M + b*K # damping matrix

L = M + γ*Δt*C + β*Δt^2*K
L, Lbd = fem_impose_Dirichlet_boundary_condition(L, bdnode, m, n, h)

a = zeros(2(m+1)*(n+1))
v = zeros(2(m+1)*(n+1))
d = zeros(2(m+1)*(n+1))
U = zeros(2(m+1)*(n+1),NT+1)

Sigma = zeros(NT+1, 4m*n, 3)
Varepsilon = zeros(NT+1, 4m*n, 3)
for i = 1:NT
    global a, v, d
    T = eval_f_on_boundary_edge((x,y)->0.1, bdedge, m, n, h)
    T = [T zeros(length(T))]
    rhs = compute_fem_traction_term(T, bdedge, m, n, h)
    if i*Δt>3.0
      rhs = zero(rhs)
    end

    td = d + Δt*v + Δt^2/2*(1-2β)*a
    tv = v + (1-γ)*Δt*a
    rhs = rhs - C*tv - K*td
    rhs[[bdnode; bdnode.+(m+1)*(n+1)]] .= 0.0

    a = L\rhs
    d = td + β*Δt^2*a
    v = tv + γ*Δt*a
    U[:,i+1] = d

    Varepsilon[i+1,:,:] = eval_strain_on_gauss_pts(U[:,i+1], m, n, h)
    Sigma[i+1,:,:] = Varepsilon[i+1,:,:] * H
end

visualize_displacement(U, m, n, h; name = "_linear", xlim_=[-0.01,0.5], ylim_=[-0.05,0.22])
visualize_displacement(U, m, n, h;  name = "_linear")
visualize_stress(H, U, m, n, h;  name = "_linear")

close("all")
figure(figsize=(15,5))
subplot(1,3,1)
idx = div(n,2)*(m+1) + m+1
plot((0:NT)*Δt, U[idx,:])
xlabel("time")
ylabel("x displacement")

subplot(1,3,2)
idx = 4*(div(n,2)*m + m)
plot((0:NT)*Δt, Sigma[:,idx,1])
xlabel("time")
ylabel("x stress")

subplot(1,3,3)
idx = 4*(div(n,2)*m + m)
plot((0:NT)*Δt, Varepsilon[:,idx,1])
xlabel("time")
ylabel("x strain")
savefig("linear.png")


module LinearAlgebraX
using LinearAlgebra, SimplePolynomials, Mods, Permutations

# IntegerX is any sort of real or Gaussian integer
IntegerX = Union{S,Complex{S}} where {S<:Integer}

# RationalX is a Rational or Complex Rational based on integers
RationalX = Union{Rational{S},Complex{Rational{S}}} where {S<:Integer}

TypeX = Union{IntegerX,RationalX,AbstractMod}

function _recip(x::T) where {T<:IntegerX}
    return 1 // x
end
_recip(x) = inv(x)

include("row_ops.jl")
include("detx.jl")
include("cofactor_det.jl")
include("eye.jl")
include("rrefx.jl")
include("invx.jl")
include("rankx.jl")
include("nullspacex.jl")
include("char_poly.jl")
include("homogeneous.jl")
include("perm.jl")

end # module


using FiniteVolumes, OrdinaryDiffEq, Plots

mesh = PeriodicCartesianMesh(100)
u0 = [sin(2π*x) for x in cell_centers(mesh)]
F(u, n) = u^2 / 2 * n

dudt(u, t, p) = -FiniteVolumes.div(F, mesh, u, Upwind())
problem = ODEProblem(dudt, u0, (0.0, 0.5))
sol = solve(problem, Euler(), dt=1e-3, saveat=0.05)

plot(mesh, [u0 sol(0.25) sol(0.5)],
     label=["initial condition" "middle state" "final state"])


using Parameters
using LinearAlgebra, StatsBase

sigmoid(x::Matrix, β::Vector) = @. 1 / (1 + exp(-$*(x,β)))

"""
    QDA(μ = μ, Σ = Σ)
Quadratic discrimination analysis

"""
@with_kw struct QDA
    μ::Array # 平均
    Σ::Matrix # 分散共分散行列
    invΣ::Matrix = inv(Σ) # 分散共分散行列の逆行列
    detΣ = det(Σ) # 分散共分散行列の行列式
end

"""
    function-like object of QDA

returns log likelihood of multivariate normal distribution

Example
```julia

```
"""
function (qda::QDA)(x...;prior=1.0)
    data = collect(x)
    @unpack μ,invΣ,detΣ = qda
    a = -0.5*(data-μ)' * invΣ * (data-μ)
    a[1] - log(detΣ) + log(prior)
end

function table_count(test,pred)
    @assert length(test) == length(pred)
    m = unique(test) |> length
    count = zeros(Int,m,m)
    for i in 1:length(test)
        count[test[i],pred[i]] += 1
    end
    return count
end

function knn(X_train::Matrix,y_train::Vector,X_test::Vector, k)
    n = size(X_train)[1]
    distance = [norm(X_train[i,:]-X_test) for i in 1:n]
    S = sortperm(distance)[1:k]
    u = counts(y_train[S], 1:k)
    u_max = maximum(u)
    m = findall(c->c==u_max,u)
    while length(m)!==1
        k -=1
        S = S[1:k]
        u = counts(S, 1:k)
        u_max = maximum(u)
        m = findall(c->c==u_max,u)
    end
    return m[1]
end

function knn(X_train::Matrix,y_train::Vector,X_test::Matrix, k)
    l = size(X_test)[1]
    w = Array{Int}(undef,l)
    for i in 1:l
        w[i] = knn(X_train,y_train,X_test[i,:], k)
    end
    return w
end


using JuMP
using CPLEX
include("parser.jl")

function dualisation(MyFileName::String)
  n, s, t, S, d1, d2, p, ph, d, D = read_instance(MyFileName)

  # Create the model
  m = Model(CPLEX.Optimizer)

  ## Variables
  @variable(m, x[1:n,1:n], Bin) # certains arcs n'existent pas, ils ne sont pas utilises dans les contraintes
  @variable(m, y[1:n], Bin)
  @variable(m, t1 >= 0)
  @variable(m, t2 >= 0)
  @variable(m, z[1:n,1:n] >= 0, Int) # meme remarque
  @variable(m, zprim[1:n] >= 0, Int)

  ## Constraints
  @constraint(m,sum(x[s,j] for j in 1:n if d[s,j] != 0) == 1)
  @constraint(m,sum(x[i,t] for i in 1:n if d[i,t] != 0) == 1)
  @constraint(m,[v in 1:n; v != s && v != t], sum(x[i,v] for i in 1:n if d[i,v] != 0) == sum(x[v,j] for j in 1:n if d[v,j] != 0))
  @constraint(m,[v in 1:n; v != t], y[v] == sum(x[v,j] for j in 1:n if d[v,j] != 0))
  @constraint(m,[i in 1:n; i != t], y[t] == sum(x[i,t] for i in 1:n if d[i,t] != 0))
  @constraint(m, t2*d2 + sum(p[i]*y[i] + 2*zprim[i] for i in 1:n) <= S)
  @constraint(m, [i in 1:n, j in 1:n ; d[i,j] != 0], t1 + z[i,j] >= d[i,j]*x[i,j])
  @constraint(m, [i in 1:n], t2 + zprim[i] >= ph[i]*y[i])

  ## Objective
  @objective(m, Min, d1*t1 + sum(d[i,j]*x[i,j] + D[i,j]*z[i,j] for i in 1:n, j in 1:n if d[i,j] != 0))

  #resolution
  optimize!(m)

  object = JuMP.objective_value.(m)
  x_star = JuMP.getvalue.( m[:x] )
  y_star = JuMP.getvalue.( m[:y] )
  t1_star = JuMP.getvalue.( m[:t1] )
  t2_star = JuMP.getvalue.( m[:t2] )
  z_star = JuMP.getvalue.( m[:z] )
  zprim_star = JuMP.getvalue.( m[:zprim] )
  println("objectif ", object)
  println("x_star ", x_star)
  println("y_star ", y_star)
  println("z_star ", z_star)
  println("t1_star ", t1_star)
  println("t2_star ", t2_star)
  println("zprim_star ", zprim_star)

end


"""
    second-order cone
        assumes: z ∈ R³
"""
function κ_so(z)
    z1 = view(z, 2:3)
    z0 = z[1]

    z_proj = zero(z)
    status = false

    if norm(z1) <= z0
        z_proj = copy(z)
        if norm(z1) < z0
            status = true
        end
        # status = true
    elseif norm(z1) <= -z0
        z_proj = zero(z)
    else
        a = 0.5 * (1.0 + z0 / norm(z1))
        z_proj[1:end-1] = a * z1
        z_proj[end] = a * norm(z1)
    end

    return z_proj, status
end

function cone_product(z, s)
    [z' * s; z[1] * view(s, 2:3) + s[1] * view(z, 2:3)]
end