using Distributions
using Plots
using ProgressMeter
using DifferentialEquations
using Statistics
using DelimitedFiles

function ODEmodel(du,u,params,t)

    (xC,xD) = u
    (alphaC,alphaD,sigma,beta,kappa,n,dC,dD,eta,Nt) = params

    # Compute expected public good benefit
    y = xC/(xC+xD)

    pC = beta/n*(1.0 + (n-1)*y)
    pD = beta/n*(n-1)*y

    (rC,rD) = nonlinearPublicGoodForm(pC,pD,params)

    du[1] = rC*xC*(1.0 - (xC+xD)/eta) - dC*xC
    du[2] = rD*xD*(1.0 - (xC+xD)/eta) - dD*xD

end

function cumsum(x)
    y = Array{Float64}(undef,length(x))
    for n = 1 : length(x)
        y[n] = sum(x[1:n])
    end

    y = y./y[end]

    return y
end

function computePublicGoodBenefit(c,d,n,beta)

    if c-1 >= 0 && d >= 0 && n > 1
        ############ A cooperator sees n-1 other cells: ############
        dist = Hypergeometric(c-1,d,n-1)
        cdfdist = cdf.(Ref(dist),support(dist))

        # Find the number of cooperators in the group
        roll = rand()
        cInGroup = findall(x->(x>roll),cdfdist)[1]-1

        if d < n-1         # Less in population than group size
            cInGroup += n-d-1
        end

        # Public good benefit
        pC = beta/n*(1.0 + cInGroup)

        # println(cInGroup)

    else
        pC = beta/n
    end
    if c >=0 && d-1 >= 0 && n > 1
        ############ A defector sees n-1 other cells: ############
        # Probability of picking individuals without replacement
        dist = Hypergeometric(c,d-1,n-1)
        cdfdist = cdf.(Ref(dist),support(dist))

        # Find the number of cooperators in the group
        roll = rand()
        cInGroup = findall(x->(x>roll),cdfdist)[1]-1

        if d < n-1         # Less in population than group size
            cInGroup += n-d-1
        end

        # Public good benefit
        pD = beta/n*cInGroup
    else
        pD = 0.0
    end

    # println(cInGroup)

    return (pC,pD)

end

function nonlinearPublicGoodForm(pC,pD,params)

    (alphaC,alphaD,sigma,beta,kappa,n,dC,dD,eta) = params

    # Compute rC and rD
    rC = alphaC*(1+exp(sigma))/(1+exp(sigma-pC)) - kappa
    rD = alphaD*(1+exp(sigma))/(1+exp(sigma-pD))

    return (rC,rD)

end

function transitionRates(x,params)

    # Grab populations and parameters
    (c, d) = x
    (alphaC,alphaD,sigma,beta,kappa,n,dC,dD,eta,Nt) = params

    count = 0
    # pCvec = Vector{Float64}
    # pDvec = Vector{Float64}
    # t_10  = Vector{Float64}

    # Initialize the rates array
    rates = Array{Float64}(undef,4)

    cVec = Array{Float64}(undef,Nt); cVec[1] = c
    dVec = Array{Float64}(undef,Nt); dVec[1] = d
    pCvec = Array{Float64}(undef,Nt-1);
    pDvec = Array{Float64}(undef,Nt-1);
    t = Array{Float64}(undef,Nt); t[1] = 0.0

    for i = 2 : Nt

        # Compute public good benefit
        (pC,pD) = computePublicGoodBenefit(c,d,n,beta)
        # Compute rC and rD
        (rC,rD) = nonlinearPublicGoodForm(pC,pD,params)

        # print(rC," ",rD,"\n")

        # Rate function (includes all the possible transitions)
        rates[1] = max(0,rC*c*(1.0 - (c + d)/eta))      # Birth of a cooperator
        rates[2] = max(0,rD*d*(1.0 - (c + d)/eta))      # Birth of a defector
        rates[3] = dC*c                                 # Death of a cooperator
        rates[4] = dD*d                                 # Death of a defector

        # Calculate the ratetotal this will be used to compute time till next event
        # and which event occurs
        ratetotal = sum(rates)

        # println(rates)

        # Calculate time till next event
        roll = rand()
        t[i] = t[i-1] - log(roll)/ratetotal

        # Cumulative sum of the rates
        cumrates = cumsum(rates)

        # Now we roll to see which event will occur
        roll = rand()
        # print(roll,"\n")
        index = findall(x->(x>roll),cumrates)[1]

        decreaseC = [3;5;6]     # Events which lead to decrease in cooperators
        decreaseD = [4;7;8]     # Events which lead to decrease in defectors

        # Change the population size
        if any(decreaseC .== index)        # C -> C - 1
            c -= 1
            # println("c dies\n")
        elseif any(decreaseD .== index)    # D -> D - 1
            d -= 1
            # println("d dies\n")
        elseif index == 1                           # C -> C + 1
            c += 1
            # println("c born\n")
        else
            d += 1
            # println("d born\n")
        end

        cVec[i] = c
        dVec[i] = d

        pCvec[i-1] = pC
        pDvec[i-1] = pD

        if c*d == 0
            t    = t[1:i]
            cVec = cVec[1:i]
            dVec = dVec[1:i]
            pCvec = pCvec[1:i]
            pDvec = pDvec[1:i]
            break
        end

        if i % 10000 == 0
            # println(pC," ", pD,"\n")
        end

    end

return (t,cVec,dVec,pCvec,pDvec)

end

# (C0,D0)
C0 = 50; D0 = 50;
x = (C0,D0)

betaval = 5.0; n = 15; sigma = 2.0;
Nt = 10^5
plotsteps = Int(max(Nt/1000,100))

# # (alphaC,alphaD,sigma,beta,kappa,n,dC,dD,eta,Nt)
# params = (1.0,1.0,2.0,betaval,0.5,n,0.1,0.1,1000.0,Nt)

Nruns = 200

savematrix = Array{Any}(undef,Nruns)

params = (1.0,1.0,sigma,betaval,0.5,n,0.1,0.1,1000.0,Nt)

@showprogress 1 "Computing..." for i = 1 : Nruns

    (t,cooperators,defectors,pCvec,pDvec) = transitionRates(x,params)

    t = t[1:plotsteps:end];
    cooperators = cooperators[1:plotsteps:end]
    defectors = defectors[1:plotsteps:end]
    savematrix[i] = [t cooperators defectors]

end

# writedlm(outfile,savematrix)

u0 = [float(C0),float(D0)]
tspan = (0.0,1000.0)

prob = ODEProblem(ODEmodel,u0,tspan,params)
sol = solve(prob,Tsit5())
plotting = false

Nt_ODE = length(sol.t)
xC      = Array{Float64}(undef,Nt_ODE)
xD      = Array{Float64}(undef,Nt_ODE)
y       = Array{Float64}(undef,Nt_ODE)
pC_ODE  = Array{Float64}(undef,Nt_ODE)
pD_ODE  = Array{Float64}(undef,Nt_ODE)
for i = 1 : Nt_ODE
    xC[i] = sol.u[i][1]
    xD[i] = sol.u[i][2]
    y[i]  = xC[i]/(xC[i]+xD[i])
    pC_ODE[i] = betaval/n*(1.0+y[i]*(n-1))
    pD_ODE[i] = betaval/n*y[i]*(n-1)
end

# writedlm(outfile,[sol.t xC])


# This file is a part of AstroLib.jl. License is MIT "Expat".
# Copyright (C) 2016 Mosè Giordano.

function _mag2geo(lat::T, long::T, pole_lat::T, pole_long::T) where {T<:AbstractFloat}
    r    = 1 # Distance from planet center.  Value unimportant -- just need
             # a length for conversion to rectangular coordinates
    sin_lat, cos_lat = sincos(deg2rad(lat))
    sin_long, cos_long = sincos(deg2rad(long))

    # convert to rectangular coordinates
    #   x-axis: defined by the vector going from Earth's center towards
    #        the intersection of the equator and Greenwich's meridian.
    #   z-axis: axis of the geographic poles
    #   y-axis: defined by y=z^x
    x = r * cos_lat * cos_long
    y = r * cos_lat * sin_long
    z = r * sin_lat

    # First rotation: in the plane of the current meridian from magnetic pole to
    # geographic pole.
    s, c = sincos(T(pi) / 2 - pole_lat)
    togeolat = SMatrix{3,3}(c,       zero(T),      -s,
                            zero(T),  one(T), zero(T),
                            s,       zero(T),       c)

    # Second rotation matrix: rotation around plane of the equator, from the
    # meridian containing the magnetic poles to the Greenwich meridian.
    sin_pole_long, cos_pole_long = sincos(pole_long)
    maglong2geolong = SMatrix{3,3}( cos_pole_long, sin_pole_long, zero(T),
                                   -sin_pole_long, cos_pole_long, zero(T),
                                    zero(T),             zero(T),  one(T))
    out = maglong2geolong * togeolat * SVector(x, y, z)

    geolat  = rad2deg(atan(out[3], hypot(out[1], out[2])))
    geolong = rad2deg(atan(out[2], out[1]))
    # I don't care about that one...just put it there for completeness' sake
    # magalt  = norm(out) - r
    return geolat, geolong
end

"""
    mag2geo(latitude, longitude[, year]) -> geographic_latitude, geographic_longitude

### Purpose ###

Convert from geomagnetic to geographic coordinates.

### Explanation ###

Converts from geomagnetic (latitude, longitude) to geographic (latitude,
longitude).  Altitude is not involved in this function.

### Arguments ###

* `latitude`: geomagnetic latitude (North), in degrees.
* `longitude`: geomagnetic longitude (East), in degrees.
* `year` (optional numerical argument): the year in which to perform conversion.
  If omitted, defaults to current year.

The coordinates can be passed as arrays of the same length.

### Output ###

The 2-tuple of geographic (latitude, longitude) coordinates, in degrees.

If geomagnetic coordinates are given as arrays, a 2-tuple of arrays of the same
length is returned.

### Example ###

Find position of North Magnetic Pole in 2016

```jldoctest
julia> using AstroLib

julia> mag2geo(90, 0, 2016)
(86.395, -166.29000000000002)
```

### Notes ###

This function uses list of North Magnetic Pole positions provided by World
Magnetic Model (https://www.ngdc.noaa.gov/geomag/data/poles/NP.xy).

`geo2mag` converts geographic coordinates to geomagnetic coordinates.

Code of this function is based on IDL Astronomy User's Library.
"""
mag2geo(lat::Real, long::Real, year::Real=Dates.year(Dates.now())) =
    _mag2geo(promote(float(lat), float(long),
                     deg2rad(POLELATLONG[year][1]::Float64),
                     deg2rad(POLELATLONG[year][2]::Float64))...)

function mag2geo(lat::AbstractArray{LA}, long::AbstractArray{LO},
                 year::Real=Dates.year(Dates.now())) where {LA<:Real, LO<:Real}
    @assert length(lat) == length(long)
    typela   = float(LA)
    geolat   = similar(lat, typela)
    geolong  = similar(lat, typela)
    polelat  = deg2rad(POLELATLONG[year][1])
    polelong = deg2rad(POLELATLONG[year][2])
    for i in eachindex(lat)
        geolat[i], geolong[i] =
            _mag2geo(promote(float(lat[i]), float(long[i]),
                             polelat, polelong)...)
    end
    return geolat, geolong
end


# # Example 02: Indentation experiment

# This script provides an example of fitting of a force-indentation curve.
# First import the packages.
using RHEOSafm
using RHEOS

using Plots
using Plots.PlotMeasures
gr();

# next import the .txt file obtained from the JPK software
input_file = joinpath(@__DIR__, "assets", "AFM_contact_test.txt")
R = 150e-9; # Radious of the indenter
interface = AFM(R);
data = importJPK(input_file, interface, sections = ["extend"]);

#md # !!! note "Beware!"
#md #     Currently RHEOSafm makes use of the Hertz contact model to convert force-displacement to stress-strain. Additional tip geometries will be added in next updates.
plot(data.ϵ, data.σ, legend = false, xlabel = "Strain", ylabel = "Stress", guidefont= 10, size = (300,300), label = "Original data", lw=3, margin=16mm)

# To detect the point at which approximately contact occurs using RHEOSafm it is possible to:
# 1) define a force threshold
# 2) apply Hertz spherical contact model
# In this example the "hertz" method is used. Two parameters are provided: the radious of the indenter and the segment of the curve used to estimate the contact point (number between 0 and 1). An application of the thrshold method is availabel in example 01.
data_contact = contact_point(data, interface, "hertz", (R = R, s = 0.5));
plot(data_contact.ϵ, data_contact.σ, legend = false, xlabel = "Strain", ylabel = "Stress", guidefont= 10, size = (300,300), label = "Shifted data", lw=3, margin=16mm)

#md # !!! compat "Note"
#md #     From this point, RHEOS functionalities are used. For more information refer to [RHEOS documentation](https://juliarheology.github.io/RHEOS.jl/stable/).

# The indentation curve is then fitted using a Standard Linear Solid model
SLS_model = modelfit(data_contact,SLS_Zener, strain_imposed);
# Now we can extract the strain pattern
SLS_predict = extract(data_contact, strain_only);
# and calculate the stress based on the fitted model
SLS_predict = modelpredict(SLS_predict, SLS_model);
# Now we can plot data and model together for comparison
plot(data_contact.ϵ, data_contact.σ, legend = true, xlabel = "Strain", ylabel = "Stress", label = "Experimental data", guidefont= 10, size = (500,500), lw=3, margin=16mm)
plot!(SLS_predict.ϵ, SLS_predict.σ, label = "Fitted",guidefont= 10, size = (400,400), lw=2)


"""
    centrality_katz(N::Unipartite; a::Float64=0.1, k::Int64=5)

This measure can work on different path length (`k`), and give a different
weight to every subsequent connection (`a`). `k` must be at least 1 (only
immediate neighbors are considered). `a` (being a weight), must be positive.

> Katz, L., 1953. A new status index derived from sociometric analysis.
> Psychometrika 18, 39–43. doi:10.1007/bf02289026

"""
function centrality_katz(N::Union{UnipartiteNetwork, UnipartiteProbabilisticNetwork}; a::Float64=0.1, k::Int64=5)
	@assert a <= 1.0
	@assert a >= 0.0
	@assert k >= 1
	centr = sum(hcat(map((x) -> vec(sum((a^x).*(N.A^x); dims=1)), [1:k;])...); dims=2)
	return Dict(zip(species(N), centr ./ sum(centr)))
end

"""
    centrality_closeness(N::UnipartiteNetwork; nmax::Int64=100)

The function calls `shortest_path` internally -- the `nmax` argument is the
maximal path length that wil be tried.

> Bavelas, A., 1950. Communication Patterns in Task‐Oriented Groups. The Journal
> of the Acoustical Society of America 22, 725–730. doi:10.1121/1.1906679
"""
function centrality_closeness(N::UnipartiteNetwork; nmax::Int64=100)
  d = shortest_path(N, nmax=nmax)
  n = richness(N)-1
  d[diagind(d)] .= 0
  interm = sum(d; dims=2)
  interm = vec(n ./ interm)
  for i in eachindex(interm)
    interm[i] = interm[i] == Inf ? 0.0 : interm[i]
  end
  return Dict(zip(species(N), interm))
end

"""
    centrality_degree(N::UnipartiteNetwork)

Degree centrality, corrected by the maximum degree (the most central species has
a degree of 1).
"""
function centrality_degree(N::UnipartiteNetwork)
  d = degree(N)
  dm = maximum(values(d))
  return Dict([p.first=>p.second/dm for p in d])
end


# --------------
# Initialization
# --------------
using LLLplus
using Test
using LinearAlgebra
using Documenter

# --------------
# tests with small matrices
# --------------
@testset "tests with small matrices..." begin

    # Matrix from http://home.ie.cuhk.edu.hk/~wkshum/wordpress/?p=442
    H =[1   9   1   2;
        1   8   8   3;
        7   4   5   1;
        2   6   7   1];
    B1=[2   3  -2  -4
        3  -1   2   1
        1   1   6  -4
        1   3  -1   3];
    T1=[0   0   1  -1
        0   1  -1   0
        0   0   0   1
        1  -3   3  -2];
    (B,T) = lll(H);
    @test sum(abs.(B-B1))==0
    @test sum(abs.(T-T1))==0
end

# --------------
# Run tests with random matrices
# --------------

@testset "random matrices" begin
    println("\nIn all the following tests, the first time includes the "*
            "JIT compilation; \nfor the second execution the "*
            "compilation is already done and the time\n"*
            "should be faster.\n")

    # Time LLL decomposition of a 1000x1000 real matrix with randn entries
    N = 1000;
    H = randn(N,N);
    println("Testing LLL on $(N)x$(N) real matrix...")
    @time (B,T,Q,R) = lll(H);
    @time (B,T,Q,R) = lll(H);

    # Time LLL, Seysen, VBLAST decompositions of a 10x10 complex matrix with
    # randn entries
    N = 10;
    println("\nTesting LLL on $(N)x$(N) complex matrix...")
    H = randn(N,N) + im*randn(N,N);
    @time (B,T,Q,R) = lll(H);
    @time (B,T,Q,R) = lll(H);
    println("Testing Seysen on same $(N)x$(N) complex matrix...")
    @time (B,T) = seysen(H);
    @time (B,T) = seysen(H);
    println("Testing VBLAST on same $(N)x$(N) complex matrix...")
    @time (W,P,B) = vblast(H);
    @time (W,P,B) = vblast(H);
    println("Testing Brun on real part of same $(N)x$(N) matrix...")
    @time (B,T) = brun(real.(H));
    @time (B,T) = brun(real.(H));
end

# Test CVP
@testset "CVP" begin
    N=500;
    println("\nTesting cvp on $(N)x$(N) BPSK system...")
    H=randn(N,N);
    Q,Rtmp=qr(H); R=UpperTriangular(Rtmp);
    u=Int.(rand([-1,1],N));
    y=H*u+rand(N)/100;
    uhat=cvp(Q'*y,R,Val(false),-1,1);
    errRate = sum(abs.(u-uhat))
    println("Error Rate is $(errRate). It should be zero or very small.\n")
end

# --------------
# doctests
# --------------
if VERSION >= v"1.1.0"
    println("\nRunning doctests...")
    DocMeta.setdocmeta!(LLLplus, :DocTestSetup,
                        :(using LLLplus, LinearAlgebra,Random); recursive=true)
    doctest(LLLplus)
end


VERSION >= v"0.4.0-dev+6521" && __precompile__()

module RandomQuantum

using Compat, QuantumInfo

import Base.rand

export  # types
        GinibreEnsemble,
        FubiniStudyPureState,
        FubiniStudyMixedState,
        HilbertSchmidtMixedState,
        BuresMixedState,
        ClosedHaarEnsemble,
        OpenHaarEnsemble,
        RandomClosedEvolution,
        RandomOpenEvolution,
        # functions
        rand

"""
RandomQuantum.GinibreEnsemble(rows,cols)

Type corresponding to the Ginibre distribution
of complex matrices.
"""
type GinibreEnsemble
    rows::Int64
    cols::Int64
end

GinibreEnsemble(dim::Number) = GinibreEnsemble(dim,dim)

function rand(dist::GinibreEnsemble)
    return (randn(dist.rows,dist.cols)+im*randn(dist.rows,dist.cols))/sqrt(2)
end

"""
RandomQuantum.FubiniStudyPureState(dim)

Type corresponding to unitarily-invariant distribution
of pure states for some Hilbert space dimension.
"""
type FubiniStudyPureState
    dim::Int64
end

function rand(dist::FubiniStudyPureState)
    return normalize( vec(rand(GinibreEnsemble(dist.dim,1))) )
end

"""
RandomQuantum.FubiniStudyMixedState(dim, bath_dim)

Type corresponding to distribution of mixed states obtained by
tracing out part of a pure state obtained from a FubiniStudy
distribution. When `dim == bath_dim`, this is identical to the
Hilbert-Schmidt distribution of mixed states.
See, e.g., Zyczkowski et al., [J. Math. Phys. 52, 062201
(2011)](http://dx.doi.org/10.1063/1.3595693)
[arXiv:1010.3570](http://arxiv.org/abs/1010.3570).
"""
type FubiniStudyMixedState
    dim::Int64
    bath_dim::Int64
end

function rand(dist::FubiniStudyMixedState)
    ψ = rand(FubiniStudyPureState(dist.dim*dist.bath_dim))
    return partialtrace(projector(ψ),[dist.dim,dist.bath_dim],2)
end

"""
RandomQuantum.HilbertSchmidtState(dim)

Type corresponding to distribution of mixed states according to the
Hilbert-Schmidt measure (induced by the Frobenius distance). This is
identical to the mixed state distribution induced by tracing out half
of a pure state distributed according to the Fubini-Study metric.
See, e.g., Zyczkowski et al., [J. Math. Phys. 52, 062201
(2011)](http://dx.doi.org/10.1063/1.3595693)
[arXiv:1010.3570](http://arxiv.org/abs/1010.3570).
"""
type HilbertSchmidtMixedState
    dim::Int64
end

function rand(dist::HilbertSchmidtMixedState)
    X = rand(GinibreEnsemble(dist.dim))
    M = X*X'
    return M/trace(M)
end

"""
RandomQuantum.BuresMixedState(dim)

Type corresponding to distribution of mixed states according to the
Bures metric.
See, e.g., Zyczkowski et al., [J. Math. Phys. 52, 062201
(2011)](http://dx.doi.org/10.1063/1.3595693)
[arXiv:1010.3570](http://arxiv.org/abs/1010.3570).
"""
type BuresMixedState
    dim::Int64
end

function rand(dist::BuresMixedState)
    G = rand(GinibreEnsemble(dist.dim))
    U = rand(ClosedHaarEnsemble(dist.dim))
    H = (eye(dist.dim)+U)*G
    return H*H'/trace(H*H')
end

"""
RandomQuantum.ClosedHaarEnsemble(dim)

Type corresponding to the unitarily invariant distribution of unitary
transformations for some Hilbert space dimension.  See, e.g.,
Mezzadri, [Notices Amer Math Soc 54 4 592
(2007)](http://www.ams.org/notices/200705/fea-mezzadri-web.pdf).
"""
type ClosedHaarEnsemble
    dim::Int64
end

function rand(dist::ClosedHaarEnsemble)
    X = rand(GinibreEnsemble(dist.dim))
    Q,_ = qr(X,thin=false)
    d = diag(Q)
    d = d./abs(d)
    Q = Q./d
    return Q
end

"""
RandomQuantum.OpenHaarEnsemble(dim, bath_dim)

Type corresponding to the unitarily invariant distribution of
unitary transformations for system and bath, such that the bath
(initially in the ground state) is traced out.
See, e.g., Bruzda et al., [Phys. Lett. A 373, 320-324
(2009)](http://dx.doi.org/10.1016/j.physleta.2008.11.043),
[arXiv:0804.2361](http://arxiv.org/abs/0804.2361).
"""
type OpenHaarEnsemble
    dim::Int64
    bath_dim::Int64
end

function rand(dist::OpenHaarEnsemble)
    X = rand(GinibreEnsemble(dist.dim^2,dist.bath_dim))
    W = X*X'
    W = W/trace(W)
    #Y = sqrtm(trace(W,[dist.dim,dist.dim],1))
    #IY = kron(eye(dist.dim),Y)
    Y = sqrtm(partialtrace(W,[dist.dim,dist.dim],2))
    IY = kron(Y,eye(dist.dim))
    R = IY\W/IY
    return choi2liou(R)/dist.dim
end

"""
RandomQuantum.GUE(dim)

Type corresponding to the Gaussian Unitary Ensemble of complex
matrices.
"""
type GUE
    size::Int64
end

function rand(dist::GUE)
    X = rand(GinibreEnsemble(dist.size))
    return (X+X')/2
end

"""
RandomQuantum.RandomClosedEvolution(dim, α)

Type corresponding to the integrated evolution of random Hamiltonians
(with unitarily invariant distribution) on some Hilbert space.
"""
type RandomClosedEvolution
    dim::Int64
    α::Float64
end

# TODO: Instead of matrix exponentiation, could this be done by
#       sampling eigenvalues, exponetiating them, and then multiplying
#       by a Haar random unitary? If so, that should be cheaper
function rand(dist::RandomClosedEvolution)
    evals,U = eig(rand(GUE(dist.dim)))
    return U*diagm(exp(-1im*dist.α*evals))*U'
end

"""
RandomQuantum.RandomOpenEvolution(dim, bath_dim, α)

Type corresponding to the distribution of completely positive trace
preserving maps obtained from the integrated evolution of random
Hamiltonians (with unitarily invariant distribution) on a
dimensional Hilbert space, such that bath (initially in the ground
state) is traced out after the evolution.
"""
type RandomOpenEvolution
    dim::Int64
    bath_dim::Int64
    α::Float64
end

function rand(dist::RandomOpenEvolution)
    d = dist.dim
    de = dist.bath_dim
    ru = rand(RandomClosedEvolution(d*de,dist.α)) * kron(eye(d),ket(0,de))
    k = Matrix{Complex128}[]
    for ii=1:de
        push!(k, kron(eye(d),bra(ii-1,de)) * ru )
    end
    return kraus2liou(k)
end

end


module JuliaSteps

export AbstractStep, GaussStep, PoissonStep, AppendSteps!, plot
using DataStructures

# # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # #
#    AbstractStep
# # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # #
# Step must have functions:
# - `ErrorSum(this, ξ₀, ξ₁)` that calculate error in data[ξ₀:ξ₁].
# - `AppendSteps!(this)` that conduct step detection.
#

abstract type AbstractStep end

function InitStep!(this::AbstractStep, data, p)
    this.data = Vector{Float64}(data)
    this.len = length(data)
    if 0 < p < 1
        this.penalty = log(p/(1-p))
    else
        this.penalty = -0.5log(this.len)
    end
    this.steplist = Vector{Int}([1, this.len + 1])
    this.fit = zeros(Float64, this.len)
    return this
end

function Finalize(this::AbstractStep)
    this.μlist = zeros(Float64, length(this.steplist)-1)

    for i in 1:length(this.steplist)-1
        ξ₀ = this.steplist[i]
        ξ₁ = this.steplist[i+1]
        μ = sum(@views(this.data[ξ₀:ξ₁-1]))/(ξ₁-ξ₀)
        this.fit[ξ₀:ξ₁-1] .= μ
        this.μlist[i] = μ
    end
    this.Δμlist = abs.(diff(this.μlist))
    return
end

# # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # #
#    GaussStep
# # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # #

struct GaussMoment
    fw1::Vector{Float64}
    fw2::Vector{Float64}
    bw1::Vector{Float64}
    bw2::Vector{Float64}
    total1::Float64
    total2::Float64
    # empty struct
    GaussMoment() = new([], [], [], [], -1., -1.)
    # only forward
    GaussMoment(fw1::Vector{Float64}, fw2::Vector{Float64}, nothing, t1::Float64, t2::Float64) = new(fw1, fw2, Vector{Float64}(), Vector{Float64}(), t1, t2)
    # only backward
    GaussMoment(nothing, bw1::Vector{Float64}, bw2::Vector{Float64}, t1::Float64, t2::Float64) = new(Vector{Float64}(), Vector{Float64}(), bw1, bw2, t1, t2)
    function GaussMoment(data::Vector{Float64})
        fw1 = cumsum(data[1:end-1])
        fw2 = cumsum(data[1:end-1].^2)
        t1 = fw1[end] + data[end]
        t2 = fw2[end] + data[end]^2
        bw1 = t1 .- fw1
        bw2 = t2 .- fw2
        new(fw1, fw2, bw1, bw2, t1, t2)
    end
end

function GetOptimalSplitter(m::GaussMoment)
    N = length(m.fw1)
    χ²all = Getχ²(m)
    χ² = m.fw2 - m.fw1.^2 ./ vec(1:N) + m.bw2 - m.bw1.^2 ./ vec(N:-1:1)
    ξ = argmin(χ²)
    return χ²[ξ] - χ²all, ξ+1
end

Getχ²(m::GaussMoment) = m.total2 - m.total1^2/(length(m.fw1)+1)

function complement!(m::GaussMoment)
    if isempty(m.fw1)
        append!(m.fw1, m.total1 .- m.bw1)
        append!(m.fw2, m.total2 .- m.bw2)
    elseif isempty(m.bw1)
        append!(m.bw1, m.total1 .- m.fw1)
        append!(m.bw2, m.total2 .- m.fw2)
    end
end

function split(m::GaussMoment, i::Int)
    g₁ = GaussMoment(m.fw1[1:i-2], m.fw2[1:i-2], nothing, m.fw1[i-1], m.fw2[i-1])
    g₂ = GaussMoment(nothing, m.bw1[i:end], m.bw2[i:end], m.bw1[i-1], m.bw2[i-1])
    complement!(g₁)
    complement!(g₂)
    return g₁, g₂
end

mutable struct GaussStep <: AbstractStep
    data::Vector{Float64}
    len::Int
    penalty::Float64
    steplist::Vector{Int}
    fit::Vector{Float64}
    μlist::Vector{Float64}
    Δμlist::Vector{Float64}

    function GaussStep(data, p=-1.)
        this = new()
        InitStep!(this, data, p)
        return this
    end
end

function AppendSteps!(this::GaussStep)
    g = GaussMoment(this.data)
    χ² = Getχ²(g)
    heap = BinaryMinHeap{Tuple{Float64, Int, Int, GaussMoment}}()
    push!(heap, (GetOptimalSplitter(g)..., 1, g))

    while true
        Δχ², Δξ, ξ₀, g = pop!(heap)
        ΔlogL = this.penalty - this.len*log(1 + Δχ²/χ²) / 2
        if ΔlogL > 0
            ξ = ξ₀ + Δξ - 1
            g₁, g₂ = split(g, Δξ)
            length(g₁.fw1) > 1 && push!(heap, (GetOptimalSplitter(g₁)..., ξ₀, g₁))
            length(g₂.fw1) > 1 && push!(heap, (GetOptimalSplitter(g₂)..., ξ, g₂))
            push!(this.steplist, ξ)
            χ² += Δχ²
        else
            break
        end
    end
    sort!(this.steplist)
    Finalize(this)
    return
end

# # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # #
#    PoissonStep
# # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # #

struct PoissonMoment
    fw1::Vector{Float64}
    bw1::Vector{Float64}
    total1::Float64
    # empty struct
    PoissonMoment() = new([], [], -1.)
    # only forward
    PoissonMoment(fw1::Vector{Float64}, nothing, t1::Float64) = new(fw1, Vector{Float64}(), t1)
    # only backward
    PoissonMoment(nothing, bw1::Vector{Float64}, t1::Float64) = new(Vector{Float64}(), bw1, t1)
    function PoissonMoment(data::Vector{Float64})
        fw1 = cumsum(data[1:end-1])
        t1 = fw1[end] + data[end]
        bw1 = t1 .- fw1
        new(fw1, bw1, t1)
    end
end

GetSlogμ(m::PoissonMoment) = m.total1 * log(m.total1 / (length(m.fw1)+1))

function GetOptimalSplitter(m::PoissonMoment)
    N = length(m.fw1)
    Slogμall = GetSlogμ(m)
    Slogμ = m.fw1 .* log.((m.fw1 .+ 1e-12) ./ vec(1:N)) + m.bw1 .* log.((m.bw1 .+ 1e-12) ./ vec(N:-1:1))
    ξ = argmax(Slogμ)
    return Slogμ[ξ] - Slogμall, ξ+1
end

function complement!(m::PoissonMoment)
    if isempty(m.fw1)
        append!(m.fw1, m.total1 .- m.bw1)
    elseif isempty(m.bw1)
        append!(m.bw1, m.total1 .- m.fw1)
    end
end

function split(m::PoissonMoment, i::Int)
    p₁ = PoissonMoment(m.fw1[1:i-2], nothing, m.fw1[i-1])
    p₂ = PoissonMoment(nothing, m.bw1[i:end], m.bw1[i-1])
    complement!(p₁)
    complement!(p₂)
    return p₁, p₂
end

mutable struct PoissonStep <: AbstractStep
    data::Vector{Float64}
    len::Int
    penalty::Float64
    steplist::Vector{Int}
    fit::Vector{Float64}
    μlist::Vector{Float64}
    Δμlist::Vector{Float64}

    function PoissonStep(data, p=-1)
        this = new()
        InitStep!(this, data, p)
        return this
    end

end

function AppendOneStep!(this::PoissonStep, m::PoissonMoment, start=1)
    if length(m.fw1) < 2
        return
    end
    Δslogμ, Δξ = GetOptimalSplitter(m)
    ΔlogL = this.penalty + Δslogμ
    if ΔlogL > 0
        ξ = start + Δξ - 1
        m1, m2 = split(m, Δξ)
        append!(this.steplist, ξ)
        AppendOneStep!(this, m1, start)
        AppendOneStep!(this, m2, ξ)
    end
    return
end

function AppendSteps!(this::PoissonStep)
    m = PoissonMoment(this.data)
    AppendOneStep!(this, m)
    sort!(this.steplist)
    Finalize(this)
    return
end

end


# Copyright (c) 2017: Hendrik Ranocha.

"""
An abstract type representing a balance law in `Dim` space dimensions.
"""
abstract type AbstractBalanceLaw{Dim} end

"""
    Burgers{T,Dim}
Burgers' equation
``
  \\partial_t u + \\partial_x \\frac{u^2}{2} = 0
``
in `Dim` space dimensions using `T` as scalar type.
"""
struct Burgers{T,Dim} <: AbstractBalanceLaw{Dim} end

function Burgers(T=Float64, Dim=1)
  Burgers{T,Dim}()
end

"""
    RiemannProblem{Model<:AbstractBalanceLaw{1}, U, T<:Real}
A Riemann problem in one space dimension for `model` with left and right values
`uₗ`,`uᵣ` at `x₀`,`t₀`.
"""
struct RiemannProblem{Model<:AbstractBalanceLaw{1}, U, T<:Real}
  model::Model
  uₗ::U
  uᵣ::U
  x₀::T
  t₀::T
end

"""
    RiemannProblem(model::AbstractBalanceLaw{1}, uₗ, uᵣ, x₀::Real=0, t₀::Real=0)
Create the `RiemannProblem` for `model` with left and right values  `uₗ`,`uᵣ` at
`x₀`,`t₀`.
"""
function RiemannProblem(model::AbstractBalanceLaw{1}, uₗ, uᵣ, x₀::Real=0, t₀::Real=0)
  @assert (typeof(uₗ) == typeof(uᵣ))
  x₀, t₀ == promote(x₀, t₀)
  RiemannProblem{typeof(model),typeof(uₗ),typeof(x₀)}(model, uₗ, uᵣ, x₀, t₀)
end

"""
    BurgersRiemannSolution{T,T1}
The solution of a Riemann problem `prob` for Burgers' equation.
"""
struct BurgersRiemannSolution{T,T1} <: Function
  prob::RiemannProblem{Burgers{T,1},T,T1}
  σ⁻::T
  σ⁺::T
end

"""
    minmax_speeds(sol::BurgersRiemannSolution)
Return the minimal and maximal speeds `σ⁻, σ⁺` that appear in the solution `sol`.
"""
function minmax_speeds(sol::BurgersRiemannSolution)
  sol.σ⁻, sol.σ⁺
end

"""
    (sol::BurgersRiemannSolution)(ξ::Real)
Evaluate the solution `sol` at the value `ξ` of the self-similarity variable
`ξ = (x - x₀) / (t - t₀)`.
"""
function (sol::BurgersRiemannSolution)(ξ::Real)
  σ⁻ = sol.σ⁻; σ⁺ = sol.σ⁺
  uₗ = sol.prob.uₗ; uᵣ = sol.prob.uᵣ

  if ξ < σ⁻
    uₗ
  elseif ξ < σ⁺
    uₗ + (ξ-σ⁻)/(σ⁺-σ⁻) * (uᵣ-uₗ)
  else
    uᵣ
  end
end

"""
    (sol::BurgersRiemannSolution)(t::Real, x::Real)
Evaluate the solution `sol` at the time and space coordinates `t` and `x`.
"""
function (sol::BurgersRiemannSolution)(x::Real, t::Real)
  x₀ = sol.prob.x₀; t₀ = sol.prob.t₀

  sol((x-x₀)/(t-t₀))
end

"""
    solve{T,T1}(prob::RiemannProblem{Burgers{T,1},T,T1})
Compute the solution of the Riemann prolem `prob`.
"""
function get_solution(prob::RiemannProblem{Burgers{T,1},T,T1}) where {T,T1}
  uₗ = prob.uₗ; uᵣ = prob.uᵣ
  if uₗ > uᵣ
    σ⁻ = σ⁺ = (uₗ + uᵣ) / 2
  else
    σ⁻ = uₗ
    σ⁺ = uᵣ
  end
  BurgersRiemannSolution(prob, σ⁻, σ⁺)
end


#=
CDLP:
- Julia version: 1.6.2
- Author: samuel
- Date: 2021-09-10
=#

using SuiteSparseGraphBLAS
using SparseArrays
using LinearAlgebra
using DataStructures

#minMode function: given a collection [1, 5, 5, 3, 3, 7] will return 3 because the highest repetition is 2
#and the lowest most repeated number is 3
function minMode(c)

    minVal = -1
    i = 0

    for row in eachrow(c)
    i = i+1
    row = collect(row)
    temp = filter(x -> x!=0, row)
    count = counter(temp)
    sortedCollection = sort(collect(count), by=x->x[2], rev=true)
    minVal = sortedCollection[1][1]
    repetitions = sortedCollection[1][2]
    for (key, value) in sortedCollection
        if(value == repetitions)
            if(key < minVal)
                minVal = key
            end
        end
    end

    end

    return minVal
end

#can be useful to tweak the algorithm
function diag_conversion(lab, n)
for i = 1:n
        for j = 1:n
            if(i==j)
                lab[i,j]=i
            else
                lab[i,j]=0
            end
        end
    end
    return lab
end

#A is the input matrix, n is the number of nodes, t the number of iterations Output: label propagation matrix
function cdlp(A, n, t)

    lab = GBMatrix{Int64}(n,n)

    for i = 1:n
        for j = 1:n
            if(i==j)
                lab[i,j]=i
            else
                lab[i,j]=0
            end
        end
    end

    print("STARTING\n")
    for k = 1:t
            F = mul(A, lab, Semirings.PLUS_TIMES)
            print("\n\n")
            for i = 1:n
            r = F[i,:]
            r = sort(r, dims=2)
            lab[i, i] = minMode(r)
        end

        print("\n\nITERATION COMPLETED\n\n")
        print(sparse(lab))

    end

    print("\n\n\nFINISHED")
    return lab

end


module TestProblems

import ..generateChild, ..gen_ref_dirs, ..norm, ..get_non_dominated_solutions

include("box-constrained.jl")
include("constrained.jl")
include("multiobjective.jl")

"""
    get_problem(problem)

Returns a 3-tuple with the objective function, the bounds and 100 Pareto solutions for
multi-objective optimization problems or the optimal solutions for (box)constrained optimization
problems.

Here, `problem` can be one of the following symbols:

Single-objective:
- `:sphere`
- `:discus`
- `:rastrigin`

Multi-objective:
- `:ZDT1`
- `:ZDT2`
- `:ZDT3`
- `:ZDT4`
- `:ZDT6`

Many-objective:
- `:DTLZ1`
- `:DTLZ2`
- `:DTLZ3`
- `:DTLZ4`
- `:DTLZ5`
- `:DTLZ6`

## Example

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

julia> f, bounds, pareto_solutions = TestProblems.get_problem(:ZDT3);

julia> bounds
2×30 Array{Float64,2}:
 0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  …  0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0  0.0
 1.0  1.0  1.0  1.0  1.0  1.0  1.0  1.0  1.0  1.0  1.0     1.0  1.0  1.0  1.0  1.0  1.0  1.0  1.0  1.0  1.0  1.0

julia> pareto_solutions
                           F space
          ┌────────────────────────────────────────┐
        1 │⢅⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀│
          │⠈⢢⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀│
          │⠀⠀⠙⠒⠀⠀⠀⠀⡆⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀│
          │⠀⠀⠀⠀⠀⠀⠀⠀⠘⡄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀│
          │⠀⠀⠀⠀⠀⠀⠀⠀⠀⠸⡀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀│
          │⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠱⠄⠀⠀⠀⠀⠀⠀⡄⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀│
          │⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢱⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀│
   f_2    │⠤⠤⠤⠤⠤⠤⠤⠤⠤⠤⠤⠤⠤⠤⠤⠤⠤⠤⠬⡦⠤⠤⠤⠤⠤⠤⠤⠤⠤⠤⠤⠤⠤⠤⠤⠤⠤⠤⠤⠤│
          │⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠘⠂⠀⠀⠀⠀⠀⠀⢢⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀│
          │⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠸⡀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀│
          │⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢧⡀⠀⠀⠀⠀⠀⠀⢀⠀⠀⠀│
          │⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⢸⠀⠀⠀│
          │⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠈⡆⠀⠀│
          │⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠙⠀⠀│
       -1 │⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀⠀│
          └────────────────────────────────────────┘
          0                                      0.9
                             f_1

```
"""
function get_problem(problem)
    expr = Meta.parse( string(problem) * "()")
    return eval(expr)
end

end


using IncrementalInference

##

@testset "test fourdoor early example" begin

## example parameters
# Number of kernels representing each marginal belief
N=100

# prior knowledge of four possible door locations
cv = 3.0
doorPrior = Mixture(Prior,
                    [Normal(-100,cv);Normal(0,cv);Normal(100,cv);Normal(300,cv)],
                    [1/4;1/4;1/4;1/4] )

## Build the factor graph object
fg = initfg()
getSolverParams(fg).useMsgLikelihoods = true

# first pose location
v1 = addVariable!(fg,:x1,ContinuousScalar,N=N)
# see a door for the first time
addFactor!(fg,[:x1], doorPrior)

# first solution with only one variable and factor (may take a few moments on first JIT compiling)
solveTree!(fg)

## drive to second pose location

addVariable!(fg,:x2, ContinuousScalar, N=N)
addFactor!(fg,[:x1;:x2],LinearRelative(Normal(50.0,2.0)))

# drive to third pose location
v3=addVariable!(fg,:x3,ContinuousScalar, N=N)
addFactor!(fg,[:x2;:x3], LinearRelative( Normal(50.0,4.0)))

# see a door for the second time
addFactor!(fg,[:x3], doorPrior)

# second solution should be much quicker
solveTree!(fg)

# drive to forth and final pose location
addVariable!(fg,:x4,ContinuousScalar, N=N)
addFactor!(fg,[:x3;:x4], LinearRelative( Normal(200.0,4.0)))

## make a third door sighting
addFactor!(fg,[:x4], doorPrior)

# solve over all data
tree = solveTree!(fg)

##

end

# # HMM computed ground truth, extended for 7 poses with landmark
# global gt = Dict{Symbol, Array{Float64,2}}()
# gt[:x0]=reshape(Float64[0.0;1.97304 ],2,1) # -0.0342366
# gt[:x2]=reshape(Float64[50.0; 2.83153 ],2,1) # 49.8797
# gt[:x3]=reshape(Float64[100.0; 1.65557 ],2,1) # 99.8351
# gt[:x4]=reshape(Float64[150.0; 1.64945 ],2,1) # 148.637
# gt[:x5]=reshape(Float64[200.0; 1.77992 ],2,1) # 198.62
# gt[:x6]=reshape(Float64[240.0; 2.20466 ],2,1) # 238.492
# gt[:x7]=reshape(Float64[300.0; 2.14353 ],2,1) # 298.467
# gt[:l1]=reshape(Float64[165.0; 1.17284 ],2,1) # 164.102
#


# ---
# title: 523. Continuous Subarray Sum
# id: problem523
# author: Indigo
# date: 2021-06-02
# difficulty: Medium
# categories: Math, Dynamic Programming
# link: <https://leetcode.com/problems/continuous-subarray-sum/description/>
# hidden: true
# ---
#
# Given a list of **non-negative** numbers and a target **integer** k, write a
# function to check if the array has a continuous subarray of size at least 2
# that sums up to a multiple of **k** , that is, sums up to n*k where n is also
# an **integer**.
#
#
#
# **Example 1:**
#
#
#
#     Input: [23, 2, 4, 6, 7],  k=6
#     Output: True
#     Explanation: Because [2, 4] is a continuous subarray of size 2 and sums up to 6.
#
#
# **Example 2:**
#
#
#
#     Input: [23, 2, 6, 4, 7],  k=6
#     Output: True
#     Explanation: Because [23, 2, 6, 4, 7] is an continuous subarray of size 5 and sums up to 42.
#
#
#
#
# **Constraints:**
#
#   * The length of the array won't exceed 10,000.
#   * You may assume the sum of all the numbers is in the range of a signed 32-bit integer.
#
#
## @lc code=start
using LeetCode

function check_subarray_sum(nums::Vector{Int}, k::Int)
    pushfirst!(nums, 0)
    cumsum!(nums, nums)
    nums .%= k
    st = Set{Int}()
    len = length(nums)
    for i in 3:len
        nums[i] ∈ st && return true
        push!(st, nums[i - 1])
    end
    return false
end
## @lc code=end


"
  Name     : c4_04_PyPlot_julia.jl
  Book     : Hands-on Data Science with Anaconda )
  Publisher: Packt Publishing Ltd.
  Author   : Yuxing Yan and James Yan
  Date     : 1/25/2018
  email    : yany@canisius.edu
             paulyxy@hotmail.com
"

# Pkg.add("Plots")

using PyPlot
x = linspace(0,2*pi,1000)
y = sin(2*x + 3*cos.(1.5*x));
plot(x, y, color="green", linewidth=2.0, linestyle="--")


mutable struct PrincetonDriver <: LaneFollowingDriver
    a::Float64 # predicted acceleration
    σ::Float64 # optional stdev on top of the model, set to zero or NaN for deterministic behavior
    k::Float64 # proportional constant for speed tracking [s⁻¹]
    v_des::Float64 # desired speed [m/s]

    function PrincetonDriver(;
        σ::Float64 = NaN,
        k::Float64 = 1.0,
        v_des::Float64 = 29.0,
        )

        retval = new()
        retval.a = NaN
        retval.σ = σ
        retval.k = k
        retval.v_des = v_des
        retval
    end
end
get_name(::PrincetonDriver) = "PrincetonDriver"
function set_desired_speed!(model::PrincetonDriver, v_des::Float64)
    model.v_des = v_des
    model
end
function track_longitudinal!(model::PrincetonDriver, v_ego::Float64, v_oth::Float64, headway::Float64)

    v_des = model.v_des
    k = model.k

    if !isnan(v_oth)
        v_des = min(v_oth*(1-exp(-k*headway/v_oth - 1)), v_des)
    end

    Δv = v_des - v_ego
    model.a = Δv*k # predicted accel to match target speed

    model
end
function Base.rand(model::PrincetonDriver)
    if isnan(model.σ) || model.σ ≤ 0.0
        LaneFollowingAccel(model.a)
    else
        LaneFollowingAccel(rand(Normal(model.a, model.σ)))
    end
end
function Distributions.pdf(model::PrincetonDriver, a::LaneFollowingAccel)
    if isnan(model.σ) || model.σ ≤ 0.0
        Inf
    else
        pdf(Normal(model.a, model.σ), a.a)
    end
end
function Distributions.logpdf(model::PrincetonDriver, a::LaneFollowingAccel)
    if isnan(model.σ) || model.σ ≤ 0.0
        Inf
    else
        logpdf(Normal(model.a, model.σ), a.a)
    end
end


### A Pluto.jl notebook ###
# v0.12.7

using Markdown
using InteractiveUtils

# ╔═╡ 6fd616f6-1ea1-11eb-3814-8bfb4a096c49
using Pkg, DrWatson

# ╔═╡ 6ffe2628-1ea1-11eb-24ea-57f985146a72
begin
	@quickactivate "StatisticsWithJuliaPlutoNotebooks"
	using Random, StatsBase, DataFrames, Plots
	using Distributions, LinearAlgebra, LaTeXStrings
	Random.seed!(1)
end;

# ╔═╡ ed174bc4-1ea0-11eb-1e2f-a32874cec549
md"## Listing 3.32"

# ╔═╡ 70179b62-1ea1-11eb-13ce-7fb4d54bad4b
begin
	N = 10^4

	SigY = [ 6 4 ;
			 4 9]
	muY = [15 ;
		   20]
	A = cholesky(SigY).L

	rngGens = [()->rand(Normal()),
			   ()->rand(Uniform(-sqrt(3),sqrt(3))),
			   ()->rand(Exponential())-1]

	rv(rg) = A*[rg(),rg()] + muY

	data = [[rv(r) for _ in 1:N] for r in rngGens]
end

# ╔═╡ e752604c-2211-11eb-36e4-9d0ca870afa5
stats(data) = begin
    data1, data2 = first.(data),last.(data)
    (
		round(mean(data1),digits=2),
		round(mean(data2),digits=2),
    	round(var(data1),digits=2),
		round(var(data2),digits=2),
        round(cov(data1,data2),digits=2)
	)
end

# ╔═╡ e753223c-2211-11eb-1b21-77985295723c
begin
	df = DataFrame()
	for d in data
		r = stats(d)
		append!(df, DataFrame(:Mean1 => r[1], :Mean2 => r[2], :Var1 => r[3],
				:Var2 => r[4], :Cov => r[5]))
	end
	df
end

# ╔═╡ e75f86ee-2211-11eb-24b6-3921f08d4bfd
begin
	scatter(first.(data[1]), last.(data[1]), c=:blue, ms=1, msw=0, label="Normal")
	scatter!(first.(data[2]), last.(data[2]), c=:red, ms=1, msw=0, label="Uniform")
	scatter!(first.(data[3]),last.(data[3]),c=:green, ms=1,msw=0,label="Exponential",
		xlims=(0,40), ylims=(0,40), legend=:bottomright, ratio=:equal,
		xlabel=L"X_1", ylabel=L"X_2")
end

# ╔═╡ 70182cd8-1ea1-11eb-094d-8d0d49cf15f3
md"## End of listing 3.32"

# ╔═╡ Cell order:
# ╟─ed174bc4-1ea0-11eb-1e2f-a32874cec549
# ╠═6fd616f6-1ea1-11eb-3814-8bfb4a096c49
# ╠═6ffe2628-1ea1-11eb-24ea-57f985146a72
# ╠═70179b62-1ea1-11eb-13ce-7fb4d54bad4b
# ╠═e752604c-2211-11eb-36e4-9d0ca870afa5
# ╠═e753223c-2211-11eb-1b21-77985295723c
# ╠═e75f86ee-2211-11eb-24b6-3921f08d4bfd
# ╟─70182cd8-1ea1-11eb-094d-8d0d49cf15f3


module CanonicalSymplecticForms

import Base
import LinearAlgebra

export canonical_one_form, CanonicalSymplecticMatrix, canonical_two_form

function checkn(T, n::Integer)
    n > 0 || throw(ArgumentError("$T must have positive size"))
    iseven(n) || throw(ArgumentError("$T must have even size"))
end

struct CanonicalSymplecticVector{T, VT <: AbstractVector{T}} <: AbstractVector{T}
    p::VT

    function CanonicalSymplecticVector{T, VT}(p) where {T, VT}
        Base.require_one_based_indexing(p)
        new{T, VT}(p)
    end
end

CanonicalSymplecticVector{T}(p::VT) where {T, VT} = CanonicalSymplecticVector{T, VT}(p)
CanonicalSymplecticVector(p::VT) where VT = CanonicalSymplecticVector{eltype(p), VT}(p)

Base.size(C::CanonicalSymplecticVector) = (length(C.p) * 2,)

function Base.getindex(C::CanonicalSymplecticVector{T}, i::Int) where T
    mid = length(C.p)
    @boundscheck let n = mid * 2
        if !(1 ≤ i ≤ n)
            msg = "attempt to access $n-element CanonicalSymplecticVector at index [$i]"
            throw(BoundsError(msg))
        end
    end

    if i ≤ mid
        return @inbounds C.p[i]
    else
        return zero(T)
    end
end

function LinearAlgebra.dot(C::CanonicalSymplecticVector, x::AbstractVector)
    m = length(C.p)
    axes(x, 1) == 1:2m || throw(DimensionMismatch())

    s = zero(promote_type(eltype(x), eltype(C)))
    @inbounds for i in 1:m
        s += C.p[i] * x[i]
    end

    s
end

# LinearAlgebra.dot(x::AbstractVector, C::CanonicalSymplecticVector) = LinearAlgebra.dot(C, x)

function canonical_one_form(z, t, p)
    n = length(z)
    iseven(n) || throw(ArgumentError("z must have even length"))
    mid = n ÷ 2
    CanonicalSymplecticVector(view(z, mid+1:n))
end

"""
    CanonicalSymplecticMatrix{T}(n::Integer)

constructs a canonical symplectic matrix of size `(n, n)` with eltype `T`.
`n` must be even and positive. See the examples to see the form of the canonical symplectic
matrix as defined here.

# Examples

```jldoctest
julia> CanonicalSymplecticMatrix(4)
4×4 CanonicalSymplecticMatrix{Int64}:
 0  0  -1   0
 0  0   0  -1
 1  0   0   0
 0  1   0   0

julia> CanonicalSymplecticMatrix{Int32}(6)
6×6 CanonicalSymplecticMatrix{Int32}:
 0  0  0  -1   0   0
 0  0  0   0  -1   0
 0  0  0   0   0  -1
 1  0  0   0   0   0
 0  1  0   0   0   0
 0  0  1   0   0   0
```
"""
struct CanonicalSymplecticMatrix{T} <: AbstractMatrix{T}
    mid::Int
    function CanonicalSymplecticMatrix{T}(n::Integer) where T
        checkn(CanonicalSymplecticMatrix, n)
        new{T}(n ÷ 2)
    end
end

CanonicalSymplecticMatrix(n::Integer) = CanonicalSymplecticMatrix{Int}(n)

Base.size(C::CanonicalSymplecticMatrix) = (sz = C.mid * 2; (sz, sz))

function Base.getindex(C::CanonicalSymplecticMatrix{T}, i1::Int, i2::Int) where T
    @boundscheck let n = C.mid * 2
        if !(1 ≤ i1 ≤ n && 1 ≤ i2 ≤ n)
            msg = "attempt to access $n-element CanonicalSymplecticMatrix{$T} at index [$i1, $i2]"
            throw(BoundsError(msg))
        end
    end

    if i2 - C.mid == i1
        return T(-1)
    elseif i1 - C.mid == i2
        return T(1)
    else
        return T(0)
    end
end

function LinearAlgebra.dot(x::AbstractVector, C::CanonicalSymplecticMatrix, y::AbstractVector)
    m = C.mid
    axes(x, 1) == axes(y, 1) == 1:2m || throw(DimensionMismatch())
    s = zero(promote_type(eltype(x), eltype(y)))
    @inbounds for i in 1:m
        s += y[i] * x[m + i]
        s -= y[m + i] * x[i]
    end

    s
end

canonical_two_form(z, t, p) = CanonicalSymplecticMatrix{eltype(z)}(length(z))

end  # module


using XDiff
using ReverseDiff: GradientTape, GradientConfig, gradient, gradient!, compile
using Base.Test
# using BenchmarkTools

function test_compare(f; inputs...)
    vals = ([val for (name, val) in inputs]...)
    df = xdiff(f; inputs...)
    # compare_test runs in an older world age than the generated function
    # need to invoke the latest one in the test
    dvals = Base.invokelatest(df, vals...)
    dvals_a = [dvals...]

    f_tape = GradientTape(f, vals)
    compiled_f_tape = compile(f_tape)
    cfg = GradientConfig(vals)
    results = map(similar, vals)
    gradient!(results, compiled_f_tape, vals)
    results_a = [results...]
    @test isapprox(results_a, dvals_a[2:end]; atol=0.1)
end

logistic(x) = 1 ./ (1 + exp.(-x))
@scalardiff logistic(x::Number) 1 (logistic(x) .* (1 .- logistic(x)))

include("vae.jl")
include("aggr.jl")
include("linreg.jl")
include("ann.jl")
include("autoencoder.jl")
include("others.jl")
include("destruct.jl")
include("special.jl")


using Base
using Plots

include("initialconditions.jl")
include("kernels.jl")
include("output.jl")
include("simulationstep.jl")

function runsimulation()
    # Initial quantities
    (x, ρ, u, v, p) = generateQuantities()
    writecsvstep(0, x, ρ, u, v, p)
    println("Step $(0)")
    gr()
    plot(x, ρ)

    # SPH operands
    L = zeros(PARTICLES_COUNT)
    H = zeros(PARTICLES_COUNT)

    # Simulation steps
    for step in 1:TIME_STEPS
        (x, ρ, u, v, p, L, H) = computeSimulationStep(step, x, ρ, u, v, p, L, H)
        writecsvstep(step, x, ρ, u, v, p)
        println("Step $(step)")
    end

    (x, ρ, u, v, p)
end


function [X,J,dXdx]=JacobiAnnulus(ksi,eta,F,Grid)
n1=size(ksi,1);
n2=size(eta,2);
X=zeros(n1,n2,2);
Phi1=atan2(F.P(2,1),F.P(1,1));
Rad1=sqrt(F.P(1,1)^2+F.P(2,1)^2);
Phi2=atan2(F.P(2,2),F.P(1,2));
Rad2=sqrt(F.P(1,2)^2+F.P(2,2)^2);
Phi3=atan2(F.P(2,3),F.P(1,3));
Rad3=sqrt(F.P(1,3)^2+F.P(2,3)^2);
Phi4=atan2(F.P(2,4),F.P(1,4));
Rad4=sqrt(F.P(1,4)^2+F.P(2,4)^2);
if Phi1<0
  Phi1=Phi1+2*pi;
end
if Phi2<0
  Phi2=Phi2+2*pi;
end
if Phi3<0
  Phi3=Phi3+2*pi;
end
if Phi4<0
  Phi4=Phi4+2*pi;
end
if Phi1>Phi3
  Phi3=Phi3+2*pi;
end
if Phi2>Phi4
  Phi4=Phi4+2*pi;
end
Phi=0.25*(Phi1*(1-ksi)*(1-eta)+...
  Phi2*(1+ksi)*(1-eta)+...
  Phi3*(1+ksi)*(1+eta)+...
  Phi4*(1-ksi)*(1+eta));
Rad=0.25*(Rad1*(1-ksi)*(1-eta)+...
  Rad2*(1+ksi)*(1-eta)+...
  Rad3*(1+ksi)*(1+eta)+...
  Rad4*(1-ksi)*(1+eta));
X(:,:,1)=cos(Phi).*Rad ;
X(:,:,2)=sin(Phi).*Rad;

if nargout > 2
  dX1dPhi=-sin(Phi).*Rad;
  dX1dRad=cos(Phi);
  dX2dPhi=cos(Phi).*Rad;
  dX2dRad=sin(Phi);

  dPhidksi=0.25*((-Phi1+Phi2+...
    Phi3-Phi4)*ones(n1,1)*ones(1,n2)+...
    ( Phi1-Phi2+...
    Phi3-Phi4)*ones(n1,1)*eta);
  dPhideta=0.25*((-Phi1-Phi2+...
    Phi3+Phi4)*ones(n1,1)*ones(1,n2)+...
    ( Phi1-Phi2+...
    Phi3-Phi4)*ksi*ones(1,n2));
  dRaddksi=0.25*((-Rad1+Rad2+...
    Rad3-Rad4)*ones(n1,1)*ones(1,n2)+...
    ( Rad1-Rad2+...
    Rad3-Rad4)*ones(n1,1)*eta);
  dRaddeta=0.25*((-Rad1-Rad2+...
    Rad3+Rad4)*ones(n1,1)*ones(1,n2)+...
    ( Rad1-Rad2+...
    Rad3-Rad4)*ksi*ones(1,n2));

  dXdx=zeros(n1,n2,2,2);
  dXdx(:,:,1,1)=dX1dPhi.*dPhidksi+dX1dRad.*dRaddksi;
  dXdx(:,:,2,1)=dX2dPhi.*dPhidksi+dX2dRad.*dRaddksi;
  dXdx(:,:,1,2)=dX1dPhi.*dPhideta+dX1dRad.*dRaddeta;
  dXdx(:,:,2,2)=dX2dPhi.*dPhideta+dX2dRad.*dRaddeta;
  J=dXdx(:,:,1,1).*dXdx(:,:,2,2)-dXdx(:,:,1,2).*dXdx(:,:,2,1);
end
end


using Test
using LinearAlgebra
using ProximalCore
using ProximalCore: prox, gradient, convex_conjugate
using ProximalCore: Zero, IndZero
import ProximalCore: prox!, is_convex, is_generalized_quadratic

@testset "Package sanity checks" begin
    @test isempty(detect_unbound_args(ProximalCore))
end

struct UnitInfNormBall end

is_convex(::Type{UnitInfNormBall}) = true

function (::UnitInfNormBall)(x)
    R = eltype(y)
    return norm(x, Inf) <= 1 ? R(0) : R(Inf)
end

function prox!(y, ::UnitInfNormBall, x, gamma)
    R = eltype(y)
    y .= max.(R(-1), min.(R(1), x))
    return R(0)
end

@testset "Basic types" begin

    @testset "Zero" begin

        @inferred (f -> Val(is_convex(f)))(Zero())
        @inferred (f -> Val(is_generalized_quadratic(f)))(Zero())

        @test is_convex(Zero())
        @test is_generalized_quadratic(Zero())

        for T in [Float32, Float64]
            @test let x = T[1.0, 2.0, 3.0]
                prox(Zero(), x, T(42)) == (x, T(0))
                gradient(Zero(), x) == (T[0, 0, 0], T(0))
            end
        end

    end

    @testset "IndZero" begin

        @inferred (f -> Val(is_convex(f)))(IndZero())
        @inferred (f -> Val(is_generalized_quadratic(f)))(IndZero())

        @test is_convex(IndZero())
        @test is_generalized_quadratic(IndZero())

        for T in [Float32, Float64]
            @test let x = T[1.0, 2.0, 3.0]
                prox(IndZero(), x, T(42)) == (T[0, 0, 0], T(0))
            end
        end

    end

    @testset "Conjugation" begin

        @test convex_conjugate(Zero()) isa IndZero
        @test convex_conjugate(IndZero()) isa Zero

    end

end

@testset "Custom types" begin

    @testset "UnitInfNormBall" begin
        f = UnitInfNormBall()
        x = Float64[-2, -1, 0, 1, 2]
        y, v = @inferred prox(f, x)

        @test is_convex(f)
        @test !is_generalized_quadratic(f)

        @test typeof(v) == real(eltype(x))
        @test y == Float64[-1, -1, 0, 1, 1]
        @test v == 0

        f_conjugate = convex_conjugate(f)
        y, v = @inferred prox(f_conjugate, x)

        @test is_convex(f_conjugate)
        @test !is_generalized_quadratic(f_conjugate)

        @test typeof(v) == real(eltype(x))
        @test y == Float64[-1, 0, 0, 0, 1]
        @test v == norm(y, 1)

        y, v = @inferred prox(f_conjugate, x, 0.5)

        @test typeof(v) == real(eltype(x))
        @test y == Float64[-1.5, -0.5, 0, 0.5, 1.5]
        @test v == norm(y, 1)

        f_biconjugate = convex_conjugate(f_conjugate)
        y, v = @inferred prox(f_biconjugate, x, 0.5)

        @test is_convex(f_biconjugate)
        @test !is_generalized_quadratic(f_biconjugate)

        @test typeof(v) == real(eltype(x))
        @test y == Float64[-1, -1, 0, 1, 1]
        @test v == 0
    end

end


module Rods

function get_fn(params::Array)
    function parseParam(p::Dict)
        return Dict(
            :radius => float(p["radius"]),
            :k => float(p["k"]),
            :center => [float(x) for x in p["center"]],
            :power  => p["exponent"],
            :direction => normalize([float(x) for x in p["direction"]])
        )
    end

    function nomalize{T<:AbstractFloat}(xs::Array{T,1})
        if norm(xs) < 1e-7
            return xs
        else
            return xs / norm(xs)
        end
    end

    function distance{T<:AbstractFloat}(point::Array{T,1}, p::Dict)
        v = point - p[:center]
        return norm(v - dot(v, p[:direction])*p[:direction])
    end

    function value{T<:AbstractFloat}(coordinates::Array{T,1}, p::Dict)
        d = distance(coordinates, p)
        if d >= p[:radius]
            return 0.0
        else
            # the center should have the highest energy
            return 0.5 * p[:k] * (d - p[:radius])^p[:power]
        end
    end

    function compute_aux{T<:AbstractFloat}(coordinates::Array{T,1}, param_list::Array, accum::AbstractFloat)
        if length(param_list) == 0
            return accum
        else
            return compute_aux(coordinates, param_list[2:end], accum + value(coordinates, param_list[1]))
        end
    end

    function fn_compute(coordinates)
        return compute_aux(coordinates, [parseParam(p) for p in params], 0.0)
    end

    return fn_compute
end

export get_fn
end


#CircuitAnalysis: Circuit math
#-------------------------------------------------------------------------------

#==NOTE:
Generic functions are not specialized. They will therefore not be efficient
for DataMD structures.==#

#==Generic functions
===============================================================================#
dB20(x) = 10*log10(abs2(x))
dB10(x) = 10*log10(x) #Power should neither be negative, nor complex

#By definition, P = VRMS²/R & P = IRMS²×R
#For V(t) = Vpk*sin(ωt+ϕ) => P = ∫(V²(t)/R/T)dt = Vpk²/R/2 => VRMS = √(P×R) = Vpk/√2
#Similarly, P = Ipk²×R/2 & IRMS = Ipk/√2
Vpk(V, ::DS{:VRMS}) = V*sqrt(2)
Vpk(P, ::DS{:W}; R=1) = sqrt(P*(2*R))
Vpk(v, ::DS{T}, args...; kwargs...) where T = throw(nosupport(Vpk, [T], v, T, args...; kwargs...))
Vpk(v, u::Symbol; kwargs...) = Vpk(v, DS(u); kwargs...)

Ipk(I, ::DS{:IRMS}) = I*sqrt(2)
Ipk(P, ::DS{:W}; R=1) = sqrt(P*(2/R))
Ipk(v, ::DS{T}, args...; kwargs...) where T = throw(nosupport(Ipk, [T], v, T, args...; kwargs...))
Ipk(v, u::Symbol; kwargs...) = Ipk(v, DS(u); kwargs...)

VRMS(V, ::DS{:Vpk}) = V/sqrt(2)
VRMS(P, ::DS{:W}; R=1) = sqrt(P*R)
VRMS(v, ::DS{T}, args...; kwargs...) where T = throw(nosupport(VRMS, [T], v, T, args...; kwargs...))
VRMS(v, u::Symbol; kwargs...) = VRMS(v, DS(u); kwargs...)

IRMS(I, ::DS{:Ipk}) = I/sqrt(2)
IRMS(P, ::DS{:W}; R=1) = sqrt(P/R)
IRMS(v, ::DS{T}, args...; kwargs...) where T = throw(nosupport(IRMS, [T], v, T, args...; kwargs...))
IRMS(v, u::Symbol; kwargs...) = IRMS(v, DS(u); kwargs...)

dB(P, ::DS{:Wratio}) = dB10(P)
dB(V, ::DS{:Vratio}) = dB20(V)
dB(I, ::DS{:Iratio}) = dB20(I)
dB(v, ::DS{T}, args...; kwargs...) where T = throw(nosupport(dB, [T], v, T, args...; kwargs...))
dB(v, u::Symbol; kwargs...) = dB(v, DS(u); kwargs...)

dBm(P, ::DS{:W}) = dB10(P) - dB10(1e-3)
dBm(V, ::DS{:VRMS}; R=1) = dB10(abs2(V)/R) - dB10(1e-3)
dBm(V, ::DS{:Vpk}; R=1) = dB10(abs2(VRMS(V, :Vpk))/R) - dB10(1e-3)
dBm(v, ::DS{T}, args...; kwargs...) where T = throw(nosupport(dBm, [T], v, T, args...; kwargs...))
dBm(v, u::Symbol; kwargs...) = dBm(v, DS(u); kwargs...)

dBW(P, ::DS{:W}) = dB10(P) - dB10(1)
dBW(V, ::DS{:VRMS}; R=1) = dB10(abs2(V)/R) - dB10(1)
dBW(V, ::DS{:Vpk}; R=1) = dB10(abs2(VRMS(V, :Vpk))/R) - dB10(1)
dBW(v, ::DS{T}, args...; kwargs...) where T = throw(nosupport(dBW, [T], v, T, args...; kwargs...))
dBW(v, u::Symbol; kwargs...) = dBW(v, DS(u); kwargs...)

#Last line


# SmallStrainContinuum

prop = Dict("name"=> "PlaneStrain", "rho"=> 1.0, "E"=> 2.0, "nu"=> 0.35)
ngp = 2
elnodes = [1;2;3;4]
nodes = [
    0.0 0.0
    1.0 0.0
    1.0 1.0
    0.0 1.0
]
element =  SmallStrainContinuum(nodes, elnodes, prop, ngp)

ν = 0.35
E = 2.0

H = zeros(3,3)
H[1,1] = E*(1. -ν)/((1+ν)*(1. -2. *ν));
H[1,2] = H[1,1]*ν/(1-ν);
H[2,1] = H[1,2];
H[2,2] = H[1,1];
H[3,3] = H[1,1]*0.5*(1. -2. *ν)/(1. -ν);

@testset "small strain gauss" begin
    ggp = getGaussPoints(e)
    c = e.coords
    scatter(c[:,1], c[:,2])
    scatter(ggp[:,1], ggp[:,2])
end

@testset "body force" begin
    ggp = getGaussPoints(element)
    f = @. ggp[:,1]^2 + ggp[:,2]^2
    fbody = getBodyForce(element, [f f])
    idx = [1;2;4;3]
    idx = [idx; idx .+ 4]
    @test fbody[idx] ≈ compute_fem_source_term(f, f, 1, 1, 1.)
end

m, n =  2, 2
h = 1/m

# Create a very simple mesh
elements = SmallStrainContinuum[]
prop = Dict("name"=> "PlaneStrain", "rho"=> 1.0, "E"=> 2.0, "nu"=> 0.35)
coords = zeros((m+1)*(n+1), 2)
for j = 1:n
    for i = 1:m
        idx = (m+1)*(j-1)+i
        elnodes = [idx; idx+1; idx+1+m+1; idx+m+1]
        ngp = 3
        nodes = [
            (i-1)*h (j-1)*h
            i*h (j-1)*h
            i*h j*h
            (i-1)*h j*h
        ]
        coords[elnodes, :] = nodes
        push!(elements, SmallStrainContinuum(nodes, elnodes, prop, ngp))
    end
end

# fixed on the bottom, push on the right
EBC = zeros(Int64, (m+1)*(n+1), 2)
FBC = zeros(Int64, (m+1)*(n+1), 2)
g = zeros((m+1)*(n+1), 2)
f = zeros((m+1)*(n+1), 2)

for i = 1:m+1
    for j = 1:n+1
        if i==1 || i==m+1 || j == 1|| j==n+1
            idx = (j-1)*(m+1) + i
            FBC[idx,:] .= -1
        end
    end
end

ndims = 2
domain = Domain(coords, elements, ndims, EBC, g, FBC, f)

function bt(x, y, t)
    f1 = @. x^2+y^2
    f2 = @. x^2+y^2
    [f1 f2]
end

Dstate = zeros(domain.neqs)
state = rand(domain.neqs)
velo =  zeros(domain.neqs)
acce =  zeros(domain.neqs)
globdat = GlobalData(state, Dstate, velo, acce, domain.neqs, gt, ft, bt)

assembleMassMatrix!(globdat, domain)
updateTimeDependentEssentialBoundaryCondition!(domain,globdat)
updateStates!(domain, globdat)

@testset "small strain body force full" begin
    fbody = getBodyForce(domain, globdat)
    f = eval_f_on_gauss_pts((x,y)->x^2+y^2, m, n, h)
    @test norm(fbody - compute_fem_source_term(f, f, m, n, h)) < 1e-10
end

@testset "small strain assembleMassMatrix!"
    @test maximum(abs.(compute_fem_mass_matrix(m, n, h) - globdat.M))<1e-10
end

@testset "small strain assembleStiffAndForce" begin
    fint, stiff = assembleStiffAndForce( globdat, domain)
    @test maximum(abs.(compute_fem_stiffness_matrix(H, m, n, h)-stiff))<1e-10
    @test norm(fint - stiff * state)<1e-10
end


using RandomMatrices
using Base.Test

if isdefined(:_HAVE_GSL)
N=5
A=randn(N,N)
B=randn(N,N)
Q=UniformHaar(2, N)

#Test case where there are no symbolic Haar matrices
@test_approx_eq eval(expectation(N, :Q, :(A*B))) A*B
#Test case where there is one pair of symbolic Haar matrices
@test_approx_eq eval(expectedtrace(N, :Q, :(A*Q*B*Q'))) trace(A)*trace(B)/N

println("Case 3")
println("E(A*Q*B*Q'*A*Q*B*Q') = ", eval(expectation(N, :Q, :(A*Q*B*Q'*A*Q*B*Q'))))

for elty in (Float64, Complex128)
	A = Stewart(elty, N)
	@test_approx_eq A'A eye(N)
end

end #_HAVE_GSL


using Statistics

"""
    peak_snr, peak_amp = peakSNR(snr, amp)

Estimate the snr peak by threading a line of slope 1 through the (`amp`,`snr`)
data. Both `amp` and `snr` are expressed in dB.
"""
function peakSNR(snr, amp)
    # Delete garbage data
    i = abs.(snr) .!= Inf
    snr = snr[i]
    amp = amp[i]
    i = .!isnan.(snr)
    snr = snr[i]
    amp = amp[i]
    i = snr .>= 3
    snr = snr[i]
    amp = amp[i]

    n = length(amp)

    # Sort by amplitude
    i = sortperm(amp)
    amp = amp[i]
    snr = snr[i]

    i = 1
    m = 0
    while any(i .!= 0) && n > 3
        # Draw a 45-degree median line through the data
        tmp = sort(snr-amp)
        if rem(n,2) == 0
            m = mean(tmp[n÷2 .+ [1,0]])
        else
            m = tmp[(n+1)÷2]
        end
        # Discard data that is more than 6dB off
        i = abs.(amp - snr .+ m) .> 6
        snr = snr[.!i]
        amp = amp[.!i]
        n = length(amp)
    end

    peak_amp = maximum(amp)
    peak_snr = peak_amp + m
    return peak_snr, peak_amp
end


function [X,J,dXdx]=JacobiAnnulusCyl(ksi,eta,F,Grid)
n1=size(ksi,1);
n2=size(eta,2);
X=zeros(n1,n2,2);
Phi1=atan2(Grid.Nodes(F.N(1)).P(2),Grid.Nodes(F.N(1)).P(1));
Rad1=sqrt(Grid.Nodes(F.N(1)).P(1)^2+Grid.Nodes(F.N(1)).P(2)^2);
Phi2=atan2(Grid.Nodes(F.N(2)).P(2),Grid.Nodes(F.N(2)).P(1));
Rad2=sqrt(Grid.Nodes(F.N(2)).P(1)^2+Grid.Nodes(F.N(2)).P(2)^2);
Phi3=atan2(Grid.Nodes(F.N(3)).P(2),Grid.Nodes(F.N(3)).P(1));
Rad3=sqrt(Grid.Nodes(F.N(3)).P(1)^2+Grid.Nodes(F.N(3)).P(2)^2);
Phi4=atan2(Grid.Nodes(F.N(4)).P(2),Grid.Nodes(F.N(4)).P(1));
Rad4=sqrt(Grid.Nodes(F.N(4)).P(1)^2+Grid.Nodes(F.N(4)).P(2)^2);
if Phi1<0
  Phi1=Phi1+2*pi;
end
if Phi2<0
  Phi2=Phi2+2*pi;
end
if Phi3<0
  Phi3=Phi3+2*pi;
end
if Phi4<0
  Phi4=Phi4+2*pi;
end
if Phi1>Phi3
  Phi3=Phi3+2*pi;
end
if Phi2>Phi4
  Phi4=Phi4+2*pi;
end
Phi=0.25*(Phi1*(1-ksi)*(1-eta)+...
  Phi2*(1+ksi)*(1-eta)+...
  Phi3*(1+ksi)*(1+eta)+...
  Phi4*(1-ksi)*(1+eta));
Rad=0.25*(Rad1*(1-ksi)*(1-eta)+...
  Rad2*(1+ksi)*(1-eta)+...
  Rad3*(1+ksi)*(1+eta)+...
  Rad4*(1-ksi)*(1+eta));
X(:,:,1)=Phi ;
X(:,:,2)=Rad;

if nargout > 2
  dPhidksi=0.25*((-Phi1+Phi2+...
    Phi3-Phi4)*ones(n1,1)*ones(1,n2)+...
    ( Phi1-Phi2+...
    Phi3-Phi4)*ones(n1,1)*eta);
  dPhideta=0.25*((-Phi1-Phi2+...
    Phi3+Phi4)*ones(n1,1)*ones(1,n2)+...
    ( Phi1-Phi2+...
    Phi3-Phi4)*ksi*ones(1,n2));
  dRaddksi=0.25*((-Rad1+Rad2+...
    Rad3-Rad4)*ones(n1,1)*ones(1,n2)+...
    ( Rad1-Rad2+...
    Rad3-Rad4)*ones(n1,1)*eta);
  dRaddeta=0.25*((-Rad1-Rad2+...
    Rad3+Rad4)*ones(n1,1)*ones(1,n2)+...
    ( Rad1-Rad2+...
    Rad3-Rad4)*ksi*ones(1,n2));

  dXdx=zeros(n1,n2,2,2);
  dXdx(:,:,1,1)=dPhidksi;
  dXdx(:,:,2,1)=dRaddksi;
  dXdx(:,:,1,2)=dPhideta;
  dXdx(:,:,2,2)=dRaddeta;
  J=dXdx(:,:,1,1).*dXdx(:,:,2,2)-dXdx(:,:,1,2).*dXdx(:,:,2,1);
end
end


using jInv.Mesh;
using ForwardHelmholtz
#import EikonalInv.expandModelNearest
#import EikonalInv.addAbsorbingLayer
plotting = false;
if plotting
	using PyPlot;
	close("all")
end

include("My_sub2ind.jl");

# m = readdlm("SEGmodel2Dsalt.dat"); m = m';
m = ones(256,128);
m = m*1e-3;
Minv = getRegularMesh([0.0,13.5,0.0,4.2],collect(size(m))-1);

pad = 25;

m = ones(size(m));
m = 1./m.^2

if plotting
	figure();
	imshow(1./sqrt(m'))
end
f = 1.5;
w = 2*pi*f

maxOmega = getMaximalFrequency(m,Minv);
ABLamp = maxOmega;

println("omega*h:");
println(w*Minv.h*sqrt(maximum(m)));
pad = pad*ones(Int64,Minv.dim);
H = GetHelmholtzOperator(Minv,m,w,ones(size(m))*0.01,true,pad,ABLamp,true)[1];
SH = H + GetHelmholtzShiftOP(m, w,0.1);
n = Minv.n+1; n_tup = tuple(n...);
src = div.(n,2);
src[end] = 1;
q = zeros(Complex,n_tup)
q[My_sub2ind(n,src)] = 1/(Minv.h[1]^2);

s = H\q[:];
s = real(reshape(s,n_tup));
s[My_sub2ind(n,src)] = 0.0;

if plotting
	figure();
	imshow(s');title("Helmholtz Solution");
end

sh = SH\q[:];
sh = real(reshape(sh,n_tup));
sh[My_sub2ind(n,src)]= 0.0;

if plotting
	figure();
	imshow(sh');title("Shifted Helmholtz Solution");
end


@testset "glaser_test.jl" begin

    epl375 = LaserLab.PulsedLaser(375.0nm, 140.0mW, 1.0MHz, 65.0ps)
    lmu_40x_nuv = LaserLab.Objective("LMU-40X-NUV", 5.3mm, 5.1mm, 40.0)
    w0 = lmu_40x_nuv.d /2.0
    gepl375 = LaserLab.GaussianLaser(epl375, w0)
    g2epl375 = LaserLab.propagate_paralell_beam(epl375, lmu_40x_nuv)

    @test gepl375.w0 ≈ w0
    @test gepl375.laser.P ≈ epl375.P
    @test g2epl375.w0 ≈ epl375.λ/(π * lmu_40x_nuv.NA)
    @test LaserLab.spot_size(g2epl375) ≈ 2 * g2epl375.w0
    @test LaserLab.angular_divergence(g2epl375) ≈ 2 * g2epl375.θ0
    @test LaserLab.depth_of_focus(g2epl375)  ≈ 2 * g2epl375.z0

    fI = LaserLab.I(g2epl375)
    I0 = fI(0.0*μm, 0.0*μm )
    @test g2epl375.I0 ≈ I0

    aspot = π * (LaserLab.spot_size(g2epl375) / 2.0)^2
    @test aspot ≈ π * g2epl375.w0^2

    P0 = I0 * aspot
    @test LaserLab.n_photons(g2epl375.laser.λ, P0) ≈ LaserLab.n_photons(g2epl375.laser.λ, g2epl375.I0 * aspot)

    nγ = uconvert(Hz, epl375.P / LaserLab.photon_energy(epl375.λ))
    @test 2 * nγ / (π * gepl375.w0^2) == gepl375.γ0
end


using Soss
using AbstractGPs
using KernelFunctions

x = randn(100)
y = sinpi.(x) + 0.3*randn(100)

import TransformVariables
const TV = TransformVariables
Soss.as(gp::AbstractGPs.FiniteGP, _data=NamedTuple()) = as(Array, asℝ,size(gp)...)

sqexpkernel(alpha::Real, rho::Real) =
alpha^2 * transform(SEKernel(), 1/(rho*sqrt(2)))
using LinearAlgebra
using PositiveFactorizations

# Define model.
m = @model x begin
    # Priors.
    alpha ~ LogNormal(0.0, 0.1)
    rho ~ LogNormal(0.0, 1.0)
    sigma ~ LogNormal(0.0, 1.0)

    # Covariance function.
    kernel = sqexpkernel(alpha, rho)

    # GP (implicit zero-mean).
    gp = GP(kernel)

    y ~ gp(x, 0.6+sigma^2)
    # Sampling Distribution (MvNormal likelihood).
    # y ~ gp(x, sigma^2 + 1e-6)  # add 1e-6 for numerical stability.
end;

PositiveFactorizations.floattype(::Type{T}) where {T} = T

post = dynamicHMC(m(x=x), (;y=y); ad_backend=Val(:Zygote));
particles(post)

gp1 = predict(m(x=x), post[1]).gp(x, post[1].sigma^2 + 1e-6)

gp1post = posterior(gp1, y)

using Plots
plt = scatter(x, y; label = "Data")
plot!(plt, gp1post, range(extrema(x)...; length=100); label="Posterior")

plot(gp1post)


using JuMP, BilevelJuMP
using Random

function bench_svr(dim, sample, optimizer, mode, seed = 1234)

    rng = Random.MersenneTwister(seed)

    # Toll Setting
    # SVR
    # Forecasting
    # Random

    # Generate SVR sample
    Dimension = dim#2
    SampleSize = sample#20
    InSampleFrac = 0.5

    x = 1 * 2 * (rand(rng, SampleSize, Dimension) .- 0.5)
    w_real = ones(Dimension)
    y = x * w_real .+ 0.1 * 2 * (rand(rng, SampleSize) .- 0.5)

    split_point = ceil(Int, SampleSize * InSampleFrac)
    InSample = 1:split_point
    OutSample = (split_point+1):SampleSize

    ctrs = []
    vars = []

    # MOI.empty!(optimizer)
    model = BilevelModel(optimizer, mode = mode)
    try
        JuMP.set_time_limit_sec(model, MAX_TIME)
    catch e
        @show e
        @show "failed to set limit time"
    end
    # JuMP.set_time_limit_sec(model, 1.)

    # hyper parameters
    @variable(Upper(model), C >= 0)
    @variable(Upper(model), ε >= 0)

    # classifier (support vector)
    @variable(Lower(model), w[1:Dimension])

    #=
        Upper level
    =#

    # absolute value extra variables
    @variable(Upper(model), a_up_pos[i=OutSample] >= 0)
    @variable(Upper(model), a_up_neg[i=OutSample] >= 0)

    for i in OutSample
        c = @constraint(Upper(model),
            a_up_pos[i] >= + sum(w[j]*x[i,j] for j in 1:Dimension) - y[i])
        push!(ctrs, c)
        c = @constraint(Upper(model),
            a_up_neg[i] >= - sum(w[j]*x[i,j] for j in 1:Dimension) + y[i])
        push!(ctrs, c)
    end

    @objective(Upper(model),
        Min, sum(a_up_pos[i] + a_up_neg[i] for i in OutSample))

    #=
        Lower level
    =#

    @variable(Lower(model), a_lo_pos[i=InSample] >= 0)
    @variable(Lower(model), a_lo_neg[i=InSample] >= 0)

    @variable(Lower(model), a_lo_max[i=InSample] >= 0)

    for i in InSample
        c = @constraint(Lower(model),
            a_lo_pos[i] >= + sum(w[j]*x[i,j] for j in 1:Dimension) - y[i])
        push!(ctrs, c)
        c = @constraint(Lower(model),
            a_lo_neg[i] >= - sum(w[j]*x[i,j] for j in 1:Dimension) + y[i])
        push!(ctrs, c)
        c = @constraint(Lower(model),
            a_lo_max[i] >= a_lo_pos[i] + a_lo_neg[i] - ε)
        push!(ctrs, c)
    end

    @objective(Lower(model),
        Min, C * sum(a_lo_max[i] for i in InSample) +
            1/2*sum(w[j]^2 for j in 1:Dimension))

    #=
        Optimize
    =#

    optimize!(model)

    # stack bounded vars
    push!(vars, C)
    push!(vars, ε)
    for i in InSample
        push!(vars, a_lo_pos[i])
        push!(vars, a_lo_neg[i])
        push!(vars, a_lo_max[i])
    end
    for i in OutSample
        push!(vars, a_up_pos[i])
        push!(vars, a_up_neg[i])
    end

    #=
    for v in vars
        val = value(v) # >= 0
    end

    for c in ctrs
        val = value(c) - normalized_rhs(c) # >= 0
    end
    =#

    @show primal_st = primal_status(model)
    @show term_st = termination_status(model) #in [MOI.OPTIMAL, MOI.LOCALLY_SOLVED, MOI.ALMOST_LOCALLY_SOLVED]

    solve_t = JuMP.solve_time(model)
    build_t = BilevelJuMP.build_time(model)

    obj_l = try
        objective_value(Lower(model))
    catch
        NaN
    end
    obj_u = try
        objective_value(Upper(model))
    catch
         NaN
    end
    gap = try
        bound = objective_bound(Upper(model))
        abs(obj_u - bound)/max(abs(bound), 1e-8)
    catch
        NaN
    end

    return primal_st, term_st, solve_t, build_t, obj_l, obj_u, gap
end


using SumBassSolver
using Random

f = [x-> 1/(1 + exp(-x)), x->2/(1 + exp(-x/2))]
df = [x->exp(-x)/(1 + exp(-x))^2, x-> exp(-x/2)/(1 + exp(-x/2))^2]
z = [0,0]
F = [-1,-1]
T = [2,2]
Pi = 2

sumSigmoidSolverSB(f,df,z,F,T,Pi)

Random.seed!(121)
# Generate random data. For implementation input proper data
n=50
p = 0.003*rand(n)
q = 0.3*rand(n)
m = 1000 .+ 500*rand(n)
c0 = m .* rand(n) ./ 2
alpha = rand(n)
F = zeros(n)
T = fill(100.0,n)
Pi = 2500

f = Vector{Function}(undef,n)
df = Vector{Function}(undef,n)
z = Vector{Float64}(undef,n)
df_inv = Vector{Function}(undef,n)
for i in 1:n
    f[i] = t -> alpha[i] * Bass(t,p[i],q[i],m[i],c0[i])
    df[i] = t -> alpha[i] * Bass_derivative(t,p[i],q[i],m[i],c0[i])
    z[i] = Bass_inflection(p[i],q[i],m[i],c0[i])
    df_inv[i] = t -> Bass_derivative_inverse(t/alpha[i],p[i],q[i],m[i],c0[i])[1]
end

x1,v1 = sumSigmoidSolverSB(f,df,z,F,T,Pi)
x2,v2 = sumSigmoidSolverSB(f,df,z,F,T,Pi,df_inv)
x3,v3 = @time sumBassSolver(p,q,m,c0,alpha,F,T,Pi,alg="SB",localopt=true)


using Test
using LinearAlgebra

using CompScienceMeshes
using BEAST

x = point(1.0, 0.0, 0.0)
y = point(0.0, 1.0, 0.0)
z = point(0.0, 0.0, 1.0)

κ = 1.0
S = SingleLayerTrace(im*κ)

h = 0.25
Γ = meshrectangle(1.0,1.0,h)
γ = meshsegment(1.0,1.0,3)

in_interior = CompScienceMeshes.interior_tpredicate(Γ)
on_junction = CompScienceMeshes.overlap_gpredicate(γ)
edges = skeleton(Γ,1) do edge
    in_interior(edge) ||on_junction(chart(Γ,edge))
end
X = raviartthomas(Γ, edges)

x = divergence(X)
y = ntrace(X,γ)
Z = assemble(S,y,x)

# test for the correct sparsity pattern
# I, J, V = findall(!iszero, Z)
Q = findall(!iszero, Z)
I = getindex.(Q,1)
J = getindex.(Q,2)
@test length(unique(I)) == 4
@test length(unique(J)) == 44

## test the acutal value of the penalty terms
using CompScienceMeshes
using BEAST
using Test

p1 = point(0,0,0)
p2 = point(1,0,0)
p3 = point(0,1,0)

q1 = point(0,0,0)
q2 = point(1,0,0)

m = Mesh([p1,p2,p3],[index(1,2,3)])
n = Mesh([q1,q2], [index(1,2)])
translate!(n, point(0,0,20))

X = lagrangecxd0(m)
Y = lagrangecxd0(n)

x = refspace(X)
y = refspace(Y)

N = BEAST.SingleLayerTrace(0.0)
Nyx = assemble(N,Y,X)

@test size(Nyx) == (1,1)

sx = chart(m, first(cells(m)))
sy = chart(n, first(cells(n)))

cx = neighborhood(sx, [1,1]/3)
cy = neighborhood(sy, [1]/2)

vx = x(cx)
vy = y(cy)

R = norm(cartesian(cx) - cartesian(cy))
estimate = volume(sx) * volume(sy) * vx[1][1] * vy[1][1] / (4π*R)
actual = Nyx[1,1]
@test norm(estimate - actual) / norm(actual) < 5e-4

# test that the trace and divergence work as advetised
X = raviartthomas(m,boundary(m))

D = divergence(X)
Y = ntrace(X,boundary(m))

@test numfunctions(D) == 3
@test isa(refspace(X), BEAST.RTRefSpace)
for _f in D.fns
    @test length(_f) == 1
    @test _f[1].cellid == 1
    @test _f[1].refid == 1
    @test _f[1].coeff ≈ (1 / volume(sx))
end

Σ = geometry(Y)
@test numfunctions(Y) == 3
@test isa(refspace(Y), BEAST.LagrangeRefSpace)
for _f in Y.fns
    @test length(_f) == 1
    @test 0 < _f[1].cellid < 4
    seg = chart(Σ, cells(Σ)[_f[1].cellid])
    @test _f[1].refid == 1
    @test _f[1].coeff ≈ (1 / volume(seg))
end


### A Pluto.jl notebook ###
# v0.12.10

using Markdown
using InteractiveUtils

# ╔═╡ 21346b42-29f2-11eb-37be-93e9e286032b
using Automata

# ╔═╡ 287d8030-29f2-11eb-11f1-57b2b9f1038f
using Plots

# ╔═╡ 81ce2f92-29f2-11eb-1255-f74ff4d43aa1
using Images

# ╔═╡ 447fe1d0-29f5-11eb-2a6e-a5c55163cd23
md"
# 1D Cellular Automata Function
"

# ╔═╡ 36ac5460-29f2-11eb-1fc8-1d02c14dfc7c
plot(Gray.(Automata.to_array(continuous(x -> (x / 2) % 1, 100))))

# ╔═╡ 2d272abe-29f5-11eb-14a2-33e070036822
a(x, θ) = continuous(x -> sum(fill(x / 3, length(θ)) .^ collect(0:(length(θ)-1)) .* θ) % 1, 3, Float32[x])[end][1][3]

# ╔═╡ 862fd4a0-29f5-11eb-262e-7d7b5bcb39ff
r = collect(0:0.01:1)

# ╔═╡ c27a8db0-29f5-11eb-2495-6f8b73b8acbd
anim = @animate for i = r
	plot(r, (x -> a(x, [i*3, i*3])).(r), ylim=(0, 1))
end

# ╔═╡ e38993b2-29f6-11eb-1c2b-2f961ace6f85
gif(anim, "tmp.gif")

# ╔═╡ Cell order:
# ╟─447fe1d0-29f5-11eb-2a6e-a5c55163cd23
# ╠═21346b42-29f2-11eb-37be-93e9e286032b
# ╠═287d8030-29f2-11eb-11f1-57b2b9f1038f
# ╠═81ce2f92-29f2-11eb-1255-f74ff4d43aa1
# ╠═36ac5460-29f2-11eb-1fc8-1d02c14dfc7c
# ╠═2d272abe-29f5-11eb-14a2-33e070036822
# ╠═862fd4a0-29f5-11eb-262e-7d7b5bcb39ff
# ╠═c27a8db0-29f5-11eb-2495-6f8b73b8acbd
# ╠═e38993b2-29f6-11eb-1c2b-2f961ace6f85


using Test
using ClimaCore.Geometry, ClimaCore.DataLayouts
using LinearAlgebra, StaticArrays

@testset "AxisTensors" begin
    x = Geometry.Covariant12Vector(1.0, 2.0)
    y = Geometry.Contravariant12Vector(1.0, 4.0)

    M = Geometry.Axis2Tensor(
        (Geometry.Cartesian12Axis(), Geometry.Covariant12Axis()),
        [1.0 0.0; 0.5 2.0],
    )

    @test dot(x, y) == x' * y == 9.0
    @test dot(y, x) == y' * x == 9.0

    @test x == x
    @test x != Geometry.components(x)
    @test x != Geometry.Contravariant12Vector(Geometry.components(x)...)

    @test x[1] == 1.0
    @test y[2] == 4.0
    @test M[2] == 0.5
    @test M[2, 1] == 0.5
    @test M[:, 1] == Geometry.Cartesian12Vector(1.0, 0.5)
    @test M[1, :] == Geometry.Covariant12Vector(1.0, 0.0)

    @test x * y' ==
          x ⊗ y ==
          Geometry.AxisTensor(
              (axes(x, 1), axes(y, 1)),
              Geometry.components(x) * Geometry.components(y)',
          )

    @test Geometry.components(M * inv(M)) == @SMatrix [1.0 0.0; 0.0 1.0]
    @test Geometry.components(inv(M) * M) == @SMatrix [1.0 0.0; 0.0 1.0]

    @test x ⊗ 3 == Geometry.Covariant12Vector(3.0, 6.0)
    @test x ⊗ (1, (a = 2, b = 3)) == (
        Geometry.Covariant12Vector(1.0, 2.0),
        (
            a = Geometry.Covariant12Vector(2.0, 4.0),
            b = Geometry.Covariant12Vector(3.0, 6.0),
        ),
    )

    @test Geometry.components(M * inv(M)) == @SMatrix [1.0 0.0; 0.0 1.0]
    @test Geometry.components(inv(M) * M) == @SMatrix [1.0 0.0; 0.0 1.0]

    @test M * y == Geometry.Cartesian12Vector(1.0, 8.5)
    @test M \ Geometry.Cartesian12Vector(1.0, 8.5) == y

    @test_throws DimensionMismatch dot(x, x)
    @test_throws DimensionMismatch M * x
    @test_throws DimensionMismatch M \ x

    @test DataLayouts.typesize(Float64, typeof(x)) == 2
end

@testset "transform" begin
    @test Geometry.transform(
        Geometry.Covariant12Axis(),
        Geometry.Covariant1Vector(2.0),
    ) == Geometry.Covariant12Vector(2.0, 0.0)
    @test Geometry.transform(
        Geometry.Covariant12Axis(),
        Geometry.Covariant13Vector(2.0, 0.0),
    ) == Geometry.Covariant12Vector(2.0, 0.0)
    @test_throws InexactError Geometry.transform(
        Geometry.Covariant12Axis(),
        Geometry.Covariant13Vector(2.0, 2.0),
    )

    @test Geometry.transform(
        Geometry.Covariant12Axis(),
        Geometry.Covariant1Vector(2.0) * Geometry.Cartesian1Vector(1.0)',
    ) == Geometry.Covariant12Vector(2.0, 0.0) * Geometry.Cartesian1Vector(1.0)'
    @test Geometry.transform(
        Geometry.Covariant12Axis(),
        Geometry.Covariant13Vector(2.0, 0.0) * Geometry.Cartesian1Vector(1.0)',
    ) == Geometry.Covariant12Vector(2.0, 0.0) * Geometry.Cartesian1Vector(1.0)'
    @test_throws InexactError Geometry.transform(
        Geometry.Covariant12Axis(),
        Geometry.Covariant13Vector(2.0, 2.0) * Geometry.Cartesian1Vector(1.0)',
    )
end

@testset "project" begin
    @test Geometry.project(
        Geometry.Covariant12Axis(),
        Geometry.Covariant1Vector(2.0),
    ) == Geometry.Covariant12Vector(2.0, 0.0)

    @test Geometry.project(
        Geometry.Covariant12Axis(),
        Geometry.Covariant12Vector(2.0, 2.0),
    ) == Geometry.Covariant12Vector(2.0, 2.0)

    @test Geometry.project(
        Geometry.Covariant12Axis(),
        Geometry.Covariant123Vector(2.0, 2.0, 0.0),
    ) == Geometry.Covariant12Vector(2.0, 2.0)

    @test Geometry.project(
        Geometry.Covariant12Axis(),
        Geometry.Covariant123Vector(2.0, 2.0, 1.0),
    ) == Geometry.Covariant12Vector(2.0, 2.0)

    @test Geometry.project(
        Geometry.Covariant12Axis(),
        Geometry.Covariant13Vector(2.0, 0.0),
    ) == Geometry.Covariant12Vector(2.0, 0.0)
    @test Geometry.project(
        Geometry.Covariant12Axis(),
        Geometry.Covariant13Vector(2.0, 2.0),
    ) == Geometry.Covariant12Vector(2.0, 0.0)

    @test Geometry.project(
        Geometry.Covariant12Axis(),
        Geometry.Covariant1Vector(2.0) * Geometry.Cartesian1Vector(1.0)',
    ) == Geometry.Covariant12Vector(2.0, 0.0) * Geometry.Cartesian1Vector(1.0)'
    @test Geometry.project(
        Geometry.Covariant12Axis(),
        Geometry.Covariant13Vector(2.0, 0.0) * Geometry.Cartesian1Vector(1.0)',
    ) == Geometry.Covariant12Vector(2.0, 0.0) * Geometry.Cartesian1Vector(1.0)'
    @test Geometry.project(
        Geometry.Covariant12Axis(),
        Geometry.Covariant13Vector(2.0, 2.0) * Geometry.Cartesian1Vector(1.0)',
    ) == Geometry.Covariant12Vector(2.0, 0.0) * Geometry.Cartesian1Vector(1.0)'
end

@testset "cross product" begin
    M = @SMatrix [
        4.0 1.0
        0.5 2.0
    ]
    J = det(M)
    local_geom = Geometry.LocalGeometry(
        Geometry.XYPoint(0.0, 0.0),
        J,
        J,
        Geometry.Axis2Tensor(
            (Geometry.UVAxis(), Geometry.Covariant12Axis()),
            M,
        ),
    )

    u = Geometry.UVVector(1.0, 2.0)
    v = Geometry.WVector(3.0)
    @test u × v == -v × u == Geometry.UVVector(6.0, -3.0)
    uⁱ = Geometry.ContravariantVector(u, local_geom)
    vⁱ = Geometry.ContravariantVector(v, local_geom)
    @test Geometry.UVVector(Geometry._cross(uⁱ, vⁱ, local_geom), local_geom) ==
          Geometry.UVVector(6.0, -3.0)
end


using DataFrames, CSV, Statistics

data = CSV.read("../data/temperatures.csv", copycols=true)
brisT = data.Brisbane
gcT = data.GoldCoast

sigB = std(brisT)
sigG = std(gcT)
covBG = cov(brisT, gcT)

meanVect = [mean(brisT) , mean(gcT)]
covMat = [sigB^2  covBG
          covBG   sigG^2]

outfile = open("../data/mvParams.jl","w")
write(outfile,"meanVect = $meanVect \ncovMat = $covMat")
close(outfile)
print(read("../data/mvParams.jl", String))


using ApproxFunOrthogonalPolynomials, ApproxFunBase, BandedMatrices, SpecialFunctions, Test
    import ApproxFunBase: bandwidth

#
# Fix for BigFloat stackoverflow
# https://github.com/JuliaLang/julia/pull/30952
#
Base._range(a::T, step::T, ::Nothing, len::Integer) where {T <: AbstractFloat} =
	    Base._rangestyle(Base.OrderStyle(T), Base.ArithmeticStyle(T), a, step, len)

@testset "Eigenvalue problems" begin
    @testset "Negative Laplacian with Neumann boundary conditions" begin
        #
        # -𝒟² u = λu,  u'(±1) = 0.
        #
        d = Segment(-1..1)
        S = Ultraspherical(0.5, d)
        NS = NormalizedPolynomialSpace(S)
        L = -Derivative(S, 2)
        C = Conversion(domainspace(L), rangespace(L))
        B = Neumann(S)
        QS = QuotientSpace(B)
        Q = Conversion(QS, S)
        D1 = Conversion(S, NS)
        D2 = Conversion(NS, S)
        R = D1*Q
        P = cache(PartialInverseOperator(C, (0, bandwidth(L, 1) + bandwidth(R, 1) + bandwidth(C, 2))))
        A = R'D1*P*L*D2*R
        B = R'R

        # Currently a hack to avoid a LAPACK calling bug with a pencil (A, B)
        # with smaller bandwidth in A.
        n = 50
        SA = Symmetric(Matrix(A[1:n,1:n]), :L)
        SB = Symmetric(Matrix(B[1:n,1:n]), :L)
        λ = eigvals(SA, SB)

        @test λ[1:round(Int, 2n/5)] ≈ (π^2/4).*(0:round(Int, 2n/5)-1).^2
    end

    @testset "Schrödinger with piecewise-linear potential with Dirichlet boundary conditions" begin
        #
        # [-𝒟² + V] u = λu,  u(±1) = 0,
        #
        # where V = 100|x|.
        #
        d = Segment(-1..0)∪Segment(0..1)
        S = PiecewiseSpace(Ultraspherical.(0.5, d.domains))
        NS = PiecewiseSpace(NormalizedUltraspherical.(0.5, d.domains))
        V = 100Fun(abs, S)
        L = -Derivative(S, 2) + V
        C = Conversion(domainspace(L), rangespace(L))
        B = [Dirichlet(S); continuity(S, 0:1)]
        QS = QuotientSpace(B)
        Q = Conversion(QS, S)
        D1 = Conversion(S, NS)
        D2 = Conversion(NS, S)
        R = D1*Q
        P = cache(PartialInverseOperator(C, (0, bandwidth(L, 1) + bandwidth(R, 1) + bandwidth(C, 2))))
        A = R'D1*P*L*D2*R
        B = R'R

        n = 100
        SA = Symmetric(A[1:n,1:n], :L)
        SB = Symmetric(B[1:n,1:n], :L)
        λ = eigvals(SA, SB)

        @test λ[1] ≈ parse(BigFloat, "2.19503852085715200848808942880214615154684642693583513254593767079468401198338e+01")
    end

    @testset "Schrödinger with piecewise-constant potential with Dirichlet boundary conditions" begin
        #
        # [-𝒟² + V] u = λu,  u(±1) = 0,
        #
        # where V = 1000[χ_[-1,-1/2](x) + χ_[1/2,1](x)].
        #
        d = Segment(-1..(-0.5))∪Segment(-0.5..0.5)∪Segment(0.5..1)
        S = PiecewiseSpace(Ultraspherical.(0.5, d.domains))
        NS = PiecewiseSpace(NormalizedUltraspherical.(0.5, d.domains))
        V = Fun(x->abs(x) ≥ 1/2 ? 1000 : 0, S)
        L = -Derivative(S, 2) + V
        C = Conversion(domainspace(L), rangespace(L))
        B = [Dirichlet(S); continuity(S, 0:1)]
        QS = QuotientSpace(B)
        Q = Conversion(QS, S)
        D1 = Conversion(S, NS)
        D2 = Conversion(NS, S)
        R = D1*Q
        P = cache(PartialInverseOperator(C, (0, bandwidth(L, 1) + bandwidth(R, 1) + bandwidth(C, 2))))
        A = R'D1*P*L*D2*R
        B = R'R

        n = 150
        SA = Symmetric(A[1:n,1:n], :L)
        SB = Symmetric(B[1:n,1:n], :L)
        λ = eigvals(SA, SB)
        # From Lee--Greengard (1997).
        λtrue = [2.95446;5.90736;8.85702;11.80147].^2
        @test norm((λ[1:4] - λtrue)./λ[1:4]) < 1e-5
    end

    @testset "Schrödinger with linear + Dirac potential with Robin boundary conditions" begin
        #
        # [-𝒟² + V] u = λu,  u(-1) = u(1) + u'(1) = 0,
        #
        # where V = x + 100δ(x-0.25).
        #
        d = Segment(-1..0.25)∪Segment(0.25..1)
        S = PiecewiseSpace(Ultraspherical.(0.5, d.domains))
        NS = PiecewiseSpace(NormalizedUltraspherical.(0.5, d.domains))
        V = Fun(identity, S)
        L = -Derivative(S, 2) + V
        C = Conversion(domainspace(L), rangespace(L))
        B4 = zeros(Operator{ApproxFunBase.prectype(S)}, 1, 2)
        B4[1, 1] = -Evaluation(component(S, 1), rightendpoint, 1) - 100*0.5*Evaluation(component(S, 1), rightendpoint)
        B4[1, 2] = Evaluation(component(S, 2), leftendpoint, 1)  - 100*0.5*Evaluation(component(S, 2), leftendpoint)
        B4 = ApproxFunBase.InterlaceOperator(B4, PiecewiseSpace, ApproxFunBase.ArraySpace)
        B = [Evaluation(S, -1); Evaluation(S, 1) + Evaluation(S, 1, 1); continuity(S, 0); B4]
        QS = QuotientSpace(B)
        Q = Conversion(QS, S)
        D1 = Conversion(S, NS)
        D2 = Conversion(NS, S)
        R = D1*Q
        P = cache(PartialInverseOperator(C, (0, bandwidth(L, 1) + bandwidth(R, 1) + bandwidth(C, 2))))
        A = R'D1*P*L*D2*R
        B = R'R

        n = 100
        SA = Symmetric(A[1:n,1:n], :L)
        SB = Symmetric(B[1:n,1:n], :L)

        k = 3

        λ, Q = eigen(SA, SB);
        u_QS = Fun(QS, Q[:, k])
        u_S = Fun(u_QS, S)
        u = Fun(u_S, PiecewiseSpace(Chebyshev.(d.domains)))
        u /= sign(u'(-1))
        u1, u2 = components(u)

        @test norm(u(-1)) < 100eps()
        @test u(1) ≈ -u'(1)
        @test u1(0.25) ≈ u2(0.25)
        @test u2'(0.25) - u1'(0.25) ≈ 100*u(0.25)
        @test -u1'' + component(V, 1)*u1 ≈ λ[k]*u1
        @test -u2'' + component(V, 2)*u2 ≈ λ[k]*u2
    end

    @testset "BigFloat negative Laplacian with Dirichlet boundary conditions" begin
        #
        # -𝒟² u = λu,  u(±1) = 0.
        #
        d = Segment(big(-1.0)..big(1.0))
        S = Ultraspherical(big(0.5), d)
        NS = NormalizedPolynomialSpace(S)
        L = -Derivative(S, 2)
        C = Conversion(domainspace(L), rangespace(L))
        B = Dirichlet(S)
        QS = QuotientSpace(B)
        Q = Conversion(QS, S)
        D1 = Conversion(S, NS)
        D2 = Conversion(NS, S)
        R = D1*Q
        P = cache(PartialInverseOperator(C, (0, bandwidth(L, 1) + bandwidth(R, 1) + bandwidth(C, 2))))
        A = R'D1*P*L*D2*R
        B = R'R

        n = 300
        SA = Symmetric(A[1:n,1:n], :L)
        SB = Symmetric(B[1:n,1:n], :L)
        BSA = BandedMatrix(SA)
        BSB = BandedMatrix(SB)
        begin
            v = zeros(BigFloat, n)
            v[1] = 1
            v[3] = 1/256
            λ = big(0.0)
            for _ = 1:7
                λ = dot(v, BSA*v)/dot(v, BSB*v)
                OP = Symmetric(SA.data-λ*SB.data, :L)
                F = ldlt!(OP)
                ldiv!(F, v)
                normalize!(v)
            end
            @test λ ≈ big(π)^2/4
        end
    end

    @testset "Skew differentiation matrices" begin
        #
        # 𝒟 u = λ u,  u(1) + u(-1) = 0.
        #
        d = Segment(-1..1)
        S = Ultraspherical(0.5, d)
        NS = NormalizedPolynomialSpace(S)
        Lsk = Derivative(S)
        Csk = Conversion(domainspace(Lsk), rangespace(Lsk))
        B = Evaluation(S, -1) + Evaluation(S, 1)
        QS = PathologicalQuotientSpace(B)
        Q = Conversion(QS, S)
        D1 = Conversion(S, NS)
        D2 = Conversion(NS, S)
        R = D1*Q
        Psk = cache(PartialInverseOperator(Csk, (0, 4 + bandwidth(Lsk, 1) + bandwidth(R, 1) + bandwidth(Csk, 2))))
        Ask = R'D1*Psk*Lsk*D2*R
        Bsk = R'R

        n = 100
        λim = imag(sort!(eigvals(Matrix(tril(Ask[1:n,1:n], 1)), Matrix(tril(Bsk[1:n,1:n], 2))), by = abs))

        @test abs.(λim[1:2:round(Int, 2n/5)]) ≈ π.*(0.5:round(Int, 2n/5)/2)
        @test abs.(λim[2:2:round(Int, 2n/5)]) ≈ π.*(0.5:round(Int, 2n/5)/2)
    end

    @testset "Complex spectra of principal finite sections of an ultraspherical discretization of a self-adjoint problem" begin
        #
        # -𝒟² u = λ u,  u'(±1) = 0.
        #
        S = Chebyshev()
        L = -Derivative(S, 2)
        B = Neumann(S)
        C = Conversion(domainspace(L), rangespace(L))
        n = 10 # ≥ 10 appears to do the trick
        λ = eigvals(Matrix([B;L][1:n,1:n]), Matrix([B-B;C][1:n,1:n]))
        @test eltype(λ) == Complex{Float64}
    end
end


using FeynmanKacParticleFilters, ExactWrightFisher

function multinomial_logpotential(obs_vector::AbstractArray{T, 1}) where T <: Real
    function pot(state::AbstractArray{T, 1})  where T <: Real
        return logpdf(Multinomial(sum(obs_vector), state), obs_vector)
    end
    return pot
end

function multinomial_logpotential(obs_vector::AbstractArray{T, 2}) where T <: Real
    if (size(obs_vector, 1) == 1) || size(obs_vector, 2) == 1
        return multinomial_logpotential(vec(obs_vector))
    else
        error("must write the multinomial potential function for multiple observations")
    end
end

function create_transition_kernels_WF(data, α_vec::AbstractArray{U, 1}) where {T <: Real, U <: Real}
    sα = sum(α_vec)
    K = length(α_vec)
    function create_Mt(Δt)
        return state -> Wright_Fisher_K_dim_transition_with_t005_approx(state, Δt, α_vec, sα)
    end

    prior = Dirichlet(repeat([1.], K))
    return FeynmanKacParticleFilters.create_transition_kernels(data, create_Mt, prior)
end


# Function for Synthetic Control
function normalz(xₒ, x₁, idx_control, idx_treat)
    x = hcat(xₒ, x₁);
    k, n = size(x);
    kₒ = size(xₒ, 1);
    k₁ = size(x₁, 1);

    xscale = 1 ./ std(x, dims = 2);

    xmat = ((x') * (eye(k) .* xscale))';
    xₒscale = xmat[1:kₒ, idx_control];
    x₁scale = xmat[1:k₁, idx_treat];

    return xₒscale, x₁scale;
end

# Create diagonal identiy matrix
eye(n) = Matrix{Float64}(I, n, n);

# Standardize matrix
function func_std(mat)
    # Function to standardize data
    T, k = size(mat);
    μ = mean(mat; dims = 1);
    σ = std(mat; dims = 1);
    return (mat .- μ) ./ σ;
end

# Loss function
function loss_ssr(ν)
    l = size(yₒpre, 2);

    ν = [1; ν];
    D = (eye(length(ν)) .* ν);

    H = xₒ' * D * xₒ;
    f = -1 .* (x₁' * D * xₒ);

    ω = abs.(solveQadProg(H, f));

    e = y₁pre - yₒpre*ω;
    ln_ssr = log(sum(e.^2));
    return ln_ssr
end
# More on the solver type:
# https://jump.dev/JuMP.jl/v0.21.1/installation/;
# Useful website on vectorization
# https://www.softcover.io/read/7b8eb7d0/juliabook/optimization
function solveQadProg(H, f)#, ceq, beq)
    #model = Model(with_optimizer(GLPK.Optimizer))
    optimizer = Juniper.Optimizer
    nl_solver = optimizer_with_attributes(Ipopt.Optimizer, "print_level"=>0)

    model = Model(optimizer_with_attributes(optimizer, "nl_solver"=>nl_solver))

    k = size(H, 1);

    @variable(model, x[1:k])
    @constraint(model, [i = 1:k], 0 <= x[i] <= 1)

    @objective(model, Min,
                    (
                        sum(0.5 * H[i,j] * x[i] * x[j] for i = 1:length(x), j = 1:length(x)) +
                        sum( f[i] * x[i] for i = 1:length(x))
                    )
                );
    @constraint(model, sum( x[i] for i = 1:length(x) ) == 1)

    optimize!(model)

    res = JuMP.value.(x);
    return res
end


module Bounds

using Statistics
using LinearAlgebra

export SimpleBound

### Silverman Rule of Thumb for bandwidth selection ##
function silverman(x::Vector{T}) where T<:Real
    # Silverman rule of thumb
    return 1.06*length(x)^(-0.2)*std(x)
end

### Epanechnikov Kernel function ###
function epa(x::T,x0::T=0.0,h::T=1.0) where T<:Real
    temp = min(abs((x-x0)/h),1.0)
    return 0.75*(1.0-temp^2)
end

function epa(x::Vector{T},x0::T=0.0,h::T=1.0) where T<:Real
    temp = min.(norm.((x.-x0)./h),1.0)
    return 0.75*(1.0.-temp.^2)
end

### Kernel density estimation ###
function kdens(X::Vector{T},x0::T,h::T) where T<:Real
    f = sum(epa.((X .- x0) ./ h))
    return f/(length(X)*h)
end

### Kernel regression estimation ###
function kreg(Y::Vector{T},X::Vector{T},x0::T,h::T) where T<:Real
    f = 0.0
    m = 0.0
	nx=length(X)
	ny=length(Y)
	if nx != ny
		error("length of Y and X do not match")
	else
		df = epa.((X .- x0) ./ h)
		f = sum(df)
		m = sum(Y.*df)
	end
	if f >0
        return m/f
    else
        return 0.0
    end
end

function kreg(Y::Vector{T},X::Vector{T},x0::T,h::T,w::Vector{T}) where T<:AbstractFloat
    # This is a version of kreg where we can introduce weights.
    # It is done mostly so we can conditon on binary weights, i.e. exclude some observations

    f = 0.0
    m = 0.0
	nx=length(X)
	ny=length(Y)
    nw=length(w)

    if nx != ny || nx !=nw
		error("length of Y and X do not match")
	else
		df = w.*epa.((X .- x0) ./ h)
		f = sum(df)
		m = sum(w.*Y.*df)
	end
	if f >0
        return m/f
    else
        return 0.0
    end
end

### Worst Case Scenario Bounds
mutable struct Treatment{T}
    prob0 :: T
    prob1 :: T
    yhat0 :: T
    yhat1 :: T
    bound0L :: T
    bound0U :: T
    bound1L :: T
    bount1U :: T
    treatL :: T
    treatU :: T
end

function treatment(y::Vector{T},z::Vector{T},x::Vector{T},x0::T,cont::Bool,h::T=1.0) where T<:Real
    ny=length(y)
    nx=length(x)
    nz=length(z)
    if (ny !=nx) || (ny != nz)
        error("vector length not matching")
    end
    zer0 ::T = 0
    res::Vector{T} = Treatment(zer0,0,0,0,0,0,0,0,0,0)
    if cont
        res.prob1 = kreg(z,x,x0,h)
        res.prob0 = 1- res.prob1
        res.yhat1 = kreg(y,x,x0,h,z)
        res.yhat0 = kreg(y,x,x0,h,1 .-z)
    else
        nx0 = count(x .== x0)
        temp1 = z .* (x .== x0)
        nz1 = count(temp1)

        res.prob1 = nz1/nx0
        res.prob0 = 1 - res.prob1
        res.yhat1 = count(y .* z .* (x .== x0))/nz1
        res.yhat0 = count(y .* (1 .-z) .* (x .== x0))/nz1
    end

end

struct SimpleBound
	LB :: Float64
	UB :: Float64
	method :: String
end

end # module


# we write a function that applies Euler's method to solve y' = f(x,y)
# in the interval [x0, x1] with y(x0) = y0 with n steps
function eulerSolve(f, x0, y0, x1, n)

    # we compute the step size h
    h = (x1 - x0)/n

    # we initialize the outpur vectors
    x = zeros(n+1)
    y = zeros(n+1)
    x[1] = x0
    y[1] = y0

    # we interate over the interval in n steps of length h
    for i = 1:n
    	# and calculate x and y accordingly
        y[i+1] = y[i] + h * f(x[i], y[i])
        x[i+1] = x[i] + h
    end

    # we return the data points of the calculated function y(x)
    return (x, y)
end


# Based on
# U. Fjordholm, S. Mishra, E. Tadmor, Arbitrarly high-order accurate entropy
# stable essentially nonoscillatory schemes for systems of conservation laws.
# 2012. SIAM. vol. 50. No 2. pp. 544-573

struct FVTecnoScheme <: AbstractFVAlgorithm
  order :: Int
  Nflux :: Function #Entropy stable 2 point flux
  ve    :: Function #Entropy variable
end

function FVTecnoScheme(Nflux;order=2, ve = u -> u)
  FVTecnoScheme(order, Nflux, ve)
end

"""
compute_fluxes!(hh, Flux, u, mesh, dt, alg::FVTecnoScheme, nonscalar::Bool, ::Type{Val{false}})
Numerical flux of Tecno Scheme in 1D
"""
function compute_fluxes!(hh, Flux, u, mesh, dt, alg::FVTecnoScheme, nonscalar::Bool, ::Type{Val{false}})
    order = alg.order; Nflux = alg.Nflux; ve = alg.ve
    M,N = size(u)
    dx = cell_volume(mesh, 1)
    #Eno Reconstrucion
    @inbounds ul=cellval_at_left(1,u,mesh)
    @inbounds ur=cellval_at_right(1,u,mesh)
    RΛ1 = eigen(Flux.Dflux(0.5*(ul+ur)))
    MatR = Vector{typeof(RΛ1.vectors)}(undef,0)
    MatΛ = Vector{typeof(RΛ1.values)}(undef,0)
    push!(MatR,RΛ1.vectors)
    push!(MatΛ,RΛ1.values)
    for j = 2:(N+1)
      @inbounds ul=cellval_at_left(j,u,mesh)
      @inbounds ur=cellval_at_right(j,u,mesh)
      RΛj = eigen(Flux.Dflux(0.5*(ul+ur)))
      push!(MatR,RΛj.vectors); push!(MatΛ,RΛj.values)
    end
    dd = fill(0.0,M,N+1) #Extra numerical diffusion
    k = order - 1
    v = fill!(similar(u),zero(eltype(u)))
    for j in axes(v, 2)
      v[:,j] = ve(u[:,j]) #entropy variables
    end
    vminus = fill(0.0,M,N); vplus = fill(0.0,M,N)
    wminus = fill(0.0,M,N); wplus = fill(0.0,M,N)
    for j in cell_indices(mesh)
      for i in 1:M
        v_eno = get_cellvals(v,mesh,(i,j-k:j+k)...)
        vminus[i,j],vplus[i,j] = reconstruct(v_eno,dx,ENO_Reconstruction(2*order-1))
      end
      wminus[:,j] = MatR[j]*vminus[:,j]
      wplus[:,j] = MatR[j+1]*vplus[:,j]
    end
    wdiff = fill(0.0,M,N+1)
    for j = 2:N
      wdiff[:,j] = wminus[:,j] - wplus[:,j-1]
    end

    if mesh.left_boundary isa Periodic; wdiff[:,1] = wminus[:,1] - wplus[:,N];end
    if mesh.right_boundary isa Periodic; wdiff[:,N+1] = wdiff[:,1];end

    for j in node_indices(mesh)
      dd[:,j] = MatR[j]*[abs(MatΛ[j][i])*wdiff[i,j] for i in 1:M]
    end

    ff = fill(0.0,M,N+1)
    if order == 2
      for j in node_indices(mesh)
          @inbounds ul=cellval_at_left(j,u,mesh)
          @inbounds ur=cellval_at_right(j,u,mesh)
        ff[:,j] = Nflux(ul,ur)
      end
    elseif order == 3 || order == 4
      for j in node_indices(mesh)
        @inbounds ull=cellval_at_left(j-1,u,mesh)
        @inbounds ul=cellval_at_left(j,u,mesh)
        @inbounds ur=cellval_at_right(j,u,mesh)
        @inbounds urr=cellval_at_right(j+1,u,mesh)
        ff[:,j] = 4.0/3.0*Nflux(ul,ur)-1.0/6.0*(Nflux(ull,ur)+Nflux(ul,urr))
      end
    elseif order == 5
      for j in node_indices(mesh)
        @inbounds ulll=cellval_at_left(j-2,u,mesh)
        @inbounds ull=cellval_at_left(j-1,u,mesh)
        @inbounds ul=cellval_at_left(j,u,mesh)
        @inbounds ur=cellval_at_right(j,u,mesh)
        @inbounds urr=cellval_at_right(j+1,u,mesh)
        @inbounds urrr=cellval_at_right(j+2,u,mesh)
        ff[:,j] = 3.0/2.0*Nflux(ul,ur)-3.0/10.0*(Nflux(ull,ur)+Nflux(ul,urr))+
            1.0/30.0*(Nflux(ulll,ur)+Nflux(ull,urr)+Nflux(ul,urrr))
      end
    end
    hh[:,:] = ff - dd
end


raw = open(f->read(f, String), "input.txt")
shipLocation = [0; 0]
shipOrientation = [1; 0]

N = [0; 1];
S = [0; -1];
E = [1; 0];
W = [-1; 0];
L = [0 -1;
     1 0];
R = [0 1;
    -1 0];

function f((shipL, shipO), s)
    action = s[1]
    n = parse(Int64, s[2:end])

    if action == 'N'
        (shipL + n * N, shipO)
    elseif action == 'S'
        (shipL + n * S, shipO)
    elseif action == 'E'
        (shipL + n * E, shipO)
    elseif action == 'W'
        (shipL + n * W, shipO)
    elseif action == 'F'
        (shipL + n * shipO, shipO)
    elseif action == 'L'
        k = div(n, 90)
        (shipL, L^k * shipO)
    elseif action == 'R'
        k = div(n, 90)
        (shipL, R^k * shipO)
    end
end

manhattanNorm(x, y) = abs.(x - y)' * ones(size(x))

println(foldl(f, split(raw); init=(shipLocation, shipOrientation)) |> (ship -> manhattanNorm(ship[1], [0; 0])))


@testset "Hermite 4D" begin

    f(x, y, z, v) = cos(x^2 + y^2 + z^2 + v^2)

    xmin, xmax, nx = -1, 1, 32
    ymin, ymax, ny = -1, 1, 32
    zmin, zmax, nz = -1, 1, 32
    vmin, vmax, nv = -1, 1, 32

    ϵ = 0.1
    γ = 3.0

    hermite_x = Hermite(Chebyshev( xmin, xmax, nx ), ϵ, γ)
    hermite_y = Hermite(Chebyshev( ymin, ymax, ny ), ϵ, γ)
    hermite_z = Hermite(Chebyshev( zmin, zmax, nz ), ϵ, γ)
    hermite_v = Hermite(Chebyshev( vmin, vmax, nv ), ϵ, γ)

    xk = hermite_x.nodes
    yk = hermite_y.nodes
    zk = hermite_z.nodes
    vk = hermite_v.nodes

    nxe, nye, nze, nve = 32, 16, 16, 32

    xe = collect(range(xmin, stop=xmax, length=nxe))
    ye = collect(range(ymin, stop=ymax, length=nye))
    ze = collect(range(zmin, stop=zmax, length=nze))
    ve = collect(range(vmin, stop=vmax, length=nve))
    dx = abs(xe[2] - xe[1])
    dy = abs(ye[2] - ye[1])
    dz = abs(ze[2] - ze[1])
    dv = abs(ve[2] - ve[1])

    fk = [f(x, y, z, v) for x in xk, y in yk, z in zk, v in vk]
    fe = [f(x, y, z, v) for x in xe, y in ye, z in ze, v in ve]

    @time s  = interpolate(hermite_x, hermite_y, hermite_z, hermite_v, fk, xe, ye, ze, ve)

    max_error = maximum(abs.(s .- fe))

    l2_error  = sqrt(trapz((s - fe).^2, dx, dy, dz, dv))

    println(" Hermite 4d max error : $max_error ")
    println(" Hermite 4d L2  error : $l2_error  ")

    @test true

end


# Smallest multiple

using Primes

LIMIT = 20

function solve()
    return prod(n^floor(log(n, LIMIT)) for n in primes(LIMIT))
end

print(Int(solve()))