function coordinated_turn_bank(turn_rate, alpha, beta, tas, gamma)

    g0 = GRAVITY_ACCEL
    G = turn_rate * tas / g0

    if abs(gamma) < 1e-8
        phi = G * cos(beta) / (cos(alpha) - G * sin(alpha) * sin(beta))
        phi = atan(phi)
    else
        a = 1 - G * tan(alpha) * sin(beta)
        b = sin(gamma) / cos(beta)
        c = 1 + G^2 * cos(beta)^2

        sq = sqrt(c * (1 - b^2) + G^2 * sin(beta)^2)

        num = (a - b^2) + b * tan(alpha) * sq
        den = a ^ 2 - b^2 * (1 + c * tan(alpha)^2)

        phi = atan(G * cos(beta) / cos(alpha) * num / den)
    end
    return phi
end

function climb_theta(gamma, alpha, beta, phi)
    a = cos(alpha) * cos(beta)
    b = sin(phi) * sin(beta) + cos(phi) * sin(alpha) * cos(beta)
    sq = sqrt(a^2 - sin(gamma)^2 + b^2)
    theta = (a * b + sin(gamma) * sq) / (a^2 - sin(gamma)^2)
    theta = atan(theta)
    return theta
end

function turn_rate_angular_velocity(turn_rate, theta, phi)
    # w = turn_rate * k_h
    # k_h = sin(theta) i_b + sin(phi) * cos(theta) j_b + cos(theta) * sin(phi)
    # w = p * i_b + q * j_b + r * k_b
    p = - turn_rate * sin(theta)
    q = turn_rate * sin(phi) * cos(theta)
    r = turn_rate * cos(theta) * cos(phi)
    return [p, q, r]
end

"""
    body_angular_velocity_to_euler_angles_rates(p, q, r, ψ, θ, ϕ)

Transform body angular velocity (p, q, r) [rad/s] to Euler angles rates
(ψ_dot, θ_dot, ϕ_dot) [rad/s] given the euler angles (θ, ϕ) [rad] using
kinematic angular equations.

# References

- [1] Stevens, B. L., Lewis, F. L., & Johnson, E. N. (2015). Aircraft control
 and simulation: dynamics, controls design, and autonomous systems. John Wiley
 & Sons. Equation (1.4-4) (page 20)
"""
function body_angular_velocity_to_euler_angles_rates(p, q, r, θ, ϕ)

    sθ, cθ = sin(θ), cos(θ)
    sϕ, cϕ = sin(ϕ), cos(ϕ)

    ψ_dot = (q * sϕ + r * cϕ) / cθ
    θ_dot = q * cϕ - r * sϕ
    # ϕ_dot = p + (q * sϕ + r * cϕ) * tan(θ)
    ϕ_dot = p + ψ_dot * sθ

    return [ψ_dot, θ_dot, ϕ_dot]
end

"""
    euler_angles_rates_to_body_angular_velocity(ψ_dot, θ_dot, ϕ_dot, ψ, θ, ϕ)

Transform Euler angles rates (ψ_dot, θ_dot, ϕ_dot) [rad/s] to body angular
velocity (p, q, r) [rad/s] given the euler angles (θ, ϕ) [rad] using
kinematic angular equations.

# References

- [1] Stevens, B. L., Lewis, F. L., & Johnson, E. N. (2015). Aircraft control
 and simulation: dynamics, controls design, and autonomous systems. John Wiley
 & Sons. Equation (1.4-3) (page 20)
"""
function euler_angles_rates_to_body_angular_velocity(ψ_dot, θ_dot, ϕ_dot, θ, ϕ)

    sθ, cθ = sin(θ), cos(θ)
    sϕ, cϕ = sin(ϕ), cos(ϕ)

    p = ϕ_dot - sθ * ψ_dot
    q = cϕ * θ_dot + sϕ*cθ * ψ_dot
    r = -sϕ * θ_dot + cϕ*cθ * ψ_dot

    return [p, q, r]
end

"""
    body_angular_velocity_to_quaternion_rates(p, q, r, q0, q1, q2, q3)

Transform body angular velocity (p, q, r) [rad/s] to quaternion rates [1/s].

- [1] Stevens, B. L., Lewis, F. L., & Johnson, E. N. (2015). Aircraft control
 and simulation: dynamics, controls design, and autonomous systems. John Wiley
 & Sons. Equation (1.8-15) (page 51)

"""
function body_angular_velocity_to_quaternion_rates(p, q, r, q0, q1, q2, q3)

    Ω = [
        0 -p -q -r;
        p  0  r -q;
        q -r  0  p;
        r  q -p  0;
    ]
    q = [q0; q1; q2; q3]
    q_dot = 0.5 * Ω * q

    return q_dot
end

"""
    uvw_to_tasαβ(u, v, w)

Calculate true air speed (TAS), angle of attack (α) and angle of side slip (β)
from velocity expressed in body axis.

# Notes
This function assumes that u, v, w are the body components of the aerodynamic
speed. This is not true in genreal (wind speed different from zero), as u, v, w
represent velocity with respect to an inertial reference frame.

# References

- [1] Stevens, B. L., Lewis, F. L., & Johnson, E. N. (2015). Aircraft control
 and simulation: dynamics, controls design, and autonomous systems. John Wiley
 & Sons. Equation (2.3-6b) (page 78)
"""
function uvw_to_tasαβ(u, v, w)

    tas = sqrt(u*u + v*v + w*w)
    α = atan(w, u)
    β = asin(v / tas)
    return [tas, α, β]
end

"""
    tas_α_β_dot_from_uvw_dot(u, v, w, u_dot, v_dot, w_dot)

Calculate time derivatives of velcity expressed as TAS, AOA, AOS.

# Notes
Note that tas here is not necessarily true air speed. Could also be inertial speed in the
direction of airspeed. It will concide whith TAS for no wind.

# See also
    uvw_dot_from_tas_α_β(tas, α, β, tas_dot, α_dot, β_dot)

# References
- [1] Morelli, Eugene A., and Vladislav Klein. Aircraft system identification: Theory and
 practice. Williamsburg, VA: Sunflyte Enterprises, 2016. Equation 3.33 (page 44).
- [2] Stevens, B. L., Lewis, F. L., & Johnson, E. N. (2015). Aircraft control
 and simulation: dynamics, controls design, and autonomous systems. John Wiley
 & Sons. Equation (2.3-10) (page 81)
"""
function tas_α_β_dot_from_uvw_dot(u, v, w, u_dot, v_dot, w_dot)

    # [1] 3.31
    tas = sqrt(u*u + v*v + w*w)
    # [1] 3.33a
    tas_dot = (u * u_dot + v * v_dot + w * w_dot) / tas
    # [1] 3.33b
    β_dot = ((u*u + w*w) * v_dot - v * (u * u_dot + w * w_dot)) / (tas^2 * sqrt(u*u + w*w))
    # [2] 2.3.10b
    # β_dot = (v_dot * tas - v * tas_dot) / (tas * sqrt(u*u + w*w))
    # [1] 3.33c
    α_dot = (w_dot * u - w * u_dot) / (u*u + w*w)

    return [tas_dot, α_dot, β_dot]
end

"""
    uvw_dot_from_tas_α_β(tas, α, β, tas_dot, α_dot, β_dot)

Obatain body velocity derivatives given velocity in wind axis and its derivatives.

# See also
    tas_α_β_dot_from_uvw_dot(u, v, w, u_dot, v_dot, w_dot)

# References
- [1] Morelli, Eugene A., and Vladislav Klein. Aircraft system identification: Theory and
 practice. Williamsburg, VA: Sunflyte Enterprises, 2016. Derived from equation 3.32 (page 44).
"""
function uvw_dot_from_tas_α_β(tas, α, β, tas_dot, α_dot, β_dot)
    u_dot = tas_dot * cos(α) * cos(β) - tas * (α_dot * sin(α) * cos(β) + β_dot * cos(α) * sin(β))
    v_dot = tas_dot * sin(β) + tas * β_dot * cos(β)
    w_dot = tas_dot * sin(α) * cos(β) + tas * (α_dot * cos(α) * cos(β) - β_dot * sin(α) * sin(β))
    return [u_dot, v_dot, w_dot]
end


@testset "Automatic differentiation" begin
    @testset "Real -> Real" begin
        for T in (Float32, Float64, Int)
            x = rand(T)
            @test (@inferred gradient(x -> 2x^2 - 3x + 2, x))::T == 4x - 3
            @test (@inferred gradient(x -> 2x^2 - 3x + 2, x, :all))::Tuple{T, T} == (4x - 3, 2x^2 - 3x + 2)
            @test (@inferred gradient(x -> 3, x))::Int == zero(T)
            @test (@inferred gradient(x -> 3, x, :all))::Tuple{Int, Int} == (zero(T), 3)
        end
    end
    @testset "Real -> Tensor" begin
        for RetType in (Tensor{Tuple{2,3}}, Tensor{Tuple{3, @Symmetry{3,3}}})
            for T in (Float32, Float64)
                @eval begin
                    x = rand($T)
                    @test (@inferred gradient(x -> $RetType((ij...) -> prod(ij) * x^2), x))::$RetType{$T} == $RetType((ij...) -> prod(ij) * 2x)
                    @test (@inferred gradient(x -> $RetType((ij...) -> prod(ij) * x^2), x, :all))::Tuple{$RetType{$T}, $RetType{$T}} == ($RetType((ij...) -> prod(ij) * 2x), $RetType((ij...) -> prod(ij) * x^2))
                    @test (@inferred gradient(x -> $RetType((ij...) -> prod(ij)), x))::$RetType{Int} == zero($RetType)
                    @test (@inferred gradient(x -> $RetType((ij...) -> prod(ij)), x, :all))::Tuple{$RetType{Int}, $RetType{Int}} == (zero($RetType), $RetType((ij...) -> prod(ij)))
                end
            end
        end
    end
    @testset "Tensor -> Real" begin
        I₁ = x -> tr(x)
        I₂ = x -> (tr(x)^2 - tr(x^2)) / 2
        I₃ = x -> det(x)
        for RetType in (SecondOrderTensor, SymmetricSecondOrderTensor)
            for T in (Float32, Float64), dim in 1:4
                Random.seed!(1234)
                x = rand(RetType{dim, T})
                @test (@inferred gradient(I₁, x))::RetType{dim, T} ≈ one(x)
                @test (@inferred gradient(I₂, x))::RetType{dim, T} ≈ I₁(x)*one(x) - x'
                @test (@inferred gradient(I₃, x))::RetType{dim, T} ≈ det(x)*inv(x)'
                @test (@inferred gradient(I₁, x, :all))[1]::RetType{dim, T} ≈ one(x)
                @test (@inferred gradient(I₂, x, :all))[1]::RetType{dim, T} ≈ I₁(x)*one(x) - x'
                @test (@inferred gradient(I₃, x, :all))[1]::RetType{dim, T} ≈ det(x)*inv(x)'
                @test (@inferred gradient(I₁, x, :all))[2]::T ≈ I₁(x)
                @test (@inferred gradient(I₂, x, :all))[2]::T ≈ I₂(x)
                @test (@inferred gradient(I₃, x, :all))[2]::T ≈ I₃(x)
                @test (@inferred gradient(x -> 1, x))::RetType{dim, Int} == zero(x)
                @test (@inferred gradient(x -> 1, x, :all))::Tuple{RetType{dim, Int}, Int} == (zero(x), 1)
            end
        end
    end
    @testset "Tensor -> Tensor" begin
        for T in (Float32, Float64), dim in 1:4
            x = rand(SecondOrderTensor{dim, T})
            y = rand(SymmetricSecondOrderTensor{dim, T})
            @test (@inferred gradient(identity, x))::FourthOrderTensor{dim, T} == one(FourthOrderTensor{dim, T})
            @test (@inferred gradient(identity, y))::SymmetricFourthOrderTensor{dim, T} == one(SymmetricFourthOrderTensor{dim, T})
            @eval begin
                @test (@inferred gradient(x -> one(SecondOrderTensor{$dim, Int}), $x))::FourthOrderTensor{$dim, Int} == zero(FourthOrderTensor{$dim, Int})
                @test (@inferred gradient(x -> one(SecondOrderTensor{$dim, Int}), $y))::Tensor{Tuple{$dim,$dim,@Symmetry{$dim,$dim}}, Int} == zero(FourthOrderTensor{$dim, Int})
            end
        end
    end
end


using StateSpace

println("Testing Extended Kalman Filter...")

# Define process model: Ricker growth model, with two populations
r1 = 0.03 							# 10% intrinsic growth rate for pop 1
r2 = 0.06 							# 14% intrinsic growth rate for pop 1
K1 = 20000 							# carrying capacity for pop 1
K2 = 10000 							# carrying capacity for pop 2

# process model
function f(x)
	[x[1] .* exp(r1 * (1 - (x[1] ./ K1))),
	 x[2] .* exp(r2 * (1 - (x[2] ./ K2)))]
end

V = diagm([3000.0, 5000.0]) 		# covariance of (additive) process noise

# Define observation model
g(x) = 0.01 .* x
W = diagm([100.0, 250.0])

mod = NonlinearGaussianSSM(f, V, g, W)
x0 = MvNormal([1000.0, 500.0], [100.0 0.0; 0.0 100.0])

x1 = predict(mod, x0)
# println(mean(x1))

y1 = observe(mod, x1)
u1 = update(mod, x1, mean(y1))
# println(mean(u1))

xx, yy = simulate(mod, 200, x0)
yy[1, 50] = NaN

# using PyPlot
# subplot(211)
# plot(xx')
# title("True state")
# subplot(212)
# plot(yy')
# title("Observations")
# readline()

fs = filter(mod, yy, x0)

# figure()
# for i in 1:2
# 	subplot(2, 1, i)
# 	plot(xx[i, :]')
# 	plot(mean(fs)[i, :]', "k")
# 	plot(mean(fs)[i, :]' + 1.96 * sqrt(var(fs))[i, :]', "gray")
# 	plot(mean(fs)[i, :]' - 1.96 * sqrt(var(fs))[i, :]', "gray")
# end
# readline()

y_new = fs.observations[:, end] + randn(size(mod.V(1), 1)) / 10
update!(mod, fs, y_new)

ss = smooth(mod, fs)

@assert loglikelihood(fs) < loglikelihood(ss)

println("Extended Kalman Filter passed.\n")


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

function _geo2mag(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))
    x    = r * cos_lat * cos_long
    y    = r * cos_lat * sin_long
    z    = r * sin_lat

    # Compute first rotation matrix: rotation around plane of the equator, from
    # the Greenwich meridian to the meridian containing the magnetic dipole
    # pole.
    sin_pole_long, cos_pole_long = sincos(pole_long)
    geolong2maglong = 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))

    # Second rotation: in the plane of the current meridian from geographic pole
    # to magnetic dipole pole.
    s, c = sincos(T(pi) / 2 - pole_lat)
    tomaglat = SMatrix{3,3}( c,       zero(T),       s,
                             zero(T),  one(T), zero(T),
                            -s,       zero(T),       c)
    out = tomaglat * geolong2maglong * SVector(x, y, z)

    maglat  = rad2deg(atan(out[3], hypot(out[1], out[2])))
    maglong = 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 maglat, maglong
end

"""
    geo2mag(latitude, longitude[, year]) -> geomagnetic_latitude, geomagnetic_longitude

### Purpose ###

Convert from geographic to geomagnetic coordinates.

### Explanation ###

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

### Arguments ###

* `latitude`: geographic latitude (North), in degrees.
* `longitude`: geographic 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 magnetic (latitude, longitude) coordinates, in degrees.

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

### Example ###

Kyoto has geographic coordinates 35° 00' 42'' N, 135° 46' 06'' E, find its
geomagnetic coordinates in 2016:

```jldoctest
julia> using AstroLib

julia> geo2mag(ten(35,0,42), ten(135,46,6), 2016)
(36.86579228937769, -60.184060536651614)
```

### 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).

`mag2geo` converts geomagnetical coordinates to geographic coordinates.

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

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


### A Pluto.jl notebook ###
# v0.15.1

using Markdown
using InteractiveUtils

# ╔═╡ c2d0d93c-8d12-4b58-852b-10795ff1f8db
using Pkg; Pkg.develop(path=".."); Pkg.activate("..")

# ╔═╡ c74a1698-83dd-11eb-1bd2-c145ea201500
begin
	using DataFrames
	using LogisticRegression

	using Plots
	using StatsPlots
	gr()
end;

# ╔═╡ 43f669b2-8596-11eb-2e12-efdef4ac0e29
md"
# Demo of Logistic Regression

JingYu Nng
"

# ╔═╡ 135d298a-83de-11eb-1f50-83e4261f2bdf
md"
### Generate mock data
"

# ╔═╡ f374b00a-83dd-11eb-0f93-612e3ed66e75
begin
	n = 100
    β = [1., 1.]
    df = DataFrame(
        X₁=vcat(randn(n).-5, randn(n).+10),
        X₂=vcat(randn(n).-8, randn(n).+16),
        Y=vcat(ones(n), zeros(n))
    )
end

# ╔═╡ 2b9fc1a6-83de-11eb-3863-19df8f1b6252
md"
### Initial model
"

# ╔═╡ 25ae7788-83de-11eb-27be-2d51a3c90c8c
begin
	lrm = LogisticRegressionModel(df, :Y, [:X₁, :X₂])
end;

# ╔═╡ 7a44b194-83df-11eb-2de4-7f16b19d1239
begin
	const C_GRAD = cgrad([
		RGBA(53/255, 157/255, 219/255, 1),
		RGBA(240/255, 240/255, 240/255, 1),
		RGBA(219/255, 64/255, 68/255, 1)
	])

	function plot_model(model::LogisticRegressionModel, df::DataFrame; margin=3)
		lim = maximum(abs.(lrm.xs))
		x = -lim-margin:0.1:lim+margin
		heatmap(
			x, x, (x1, x2)->predict(lrm, [x1, x2]),
			title="Logistic Regression Model",
			xlabel="X₁",
			ylabel="X₂",
			legend=:topleft,
			clim=(0, 1),
			c=C_GRAD,
		)
		@df df[df.Y .== 0, :] scatter!(:X₁ , :X₂, color=C_GRAD[1], label="False")
		@df df[df.Y .== 1, :] scatter!(:X₁ , :X₂, color=C_GRAD[3], label="True")
	end
end;

# ╔═╡ da6bb3a2-83de-11eb-3a43-55dbeaabac48
md"
### Model before training
"

# ╔═╡ e78abf10-83de-11eb-29ca-c72ce80b5877
plot_model(lrm, df)

# ╔═╡ 31343290-83df-11eb-0c9f-114c43fd1c8d
md"
### Training
"

# ╔═╡ 37c5a346-83df-11eb-06f8-9b549328b45f
fit!(lrm, η=1e-2, atol=-1e-4);

# ╔═╡ 4242583c-83df-11eb-08b7-752569c5bbe8
md"
### Model after training
"

# ╔═╡ 8a450b0c-83df-11eb-34b9-4351fb9d772c
plot_model(lrm, df)

# ╔═╡ Cell order:
# ╟─43f669b2-8596-11eb-2e12-efdef4ac0e29
# ╟─c2d0d93c-8d12-4b58-852b-10795ff1f8db
# ╠═c74a1698-83dd-11eb-1bd2-c145ea201500
# ╟─135d298a-83de-11eb-1f50-83e4261f2bdf
# ╠═f374b00a-83dd-11eb-0f93-612e3ed66e75
# ╟─2b9fc1a6-83de-11eb-3863-19df8f1b6252
# ╠═25ae7788-83de-11eb-27be-2d51a3c90c8c
# ╟─7a44b194-83df-11eb-2de4-7f16b19d1239
# ╟─da6bb3a2-83de-11eb-3a43-55dbeaabac48
# ╠═e78abf10-83de-11eb-29ca-c72ce80b5877
# ╟─31343290-83df-11eb-0c9f-114c43fd1c8d
# ╠═37c5a346-83df-11eb-06f8-9b549328b45f
# ╟─4242583c-83df-11eb-08b7-752569c5bbe8
# ╠═8a450b0c-83df-11eb-34b9-4351fb9d772c


mutable struct mcpop_data
    n # number of all variables
    nb # number of binary variables
    m # number of all constraints
    numeq # number of equality constraints
    supp # support data
    coe # coefficient data
    basis # monomial bases
    rlorder # relaxation order
    ksupp # extending support at the k-th step
    cql # number of cliques
    cliques # cliques of variables
    cliquesize # numbers of cliques
    J # constraints associated to each clique
    ncc # constraints associated to no clique
    sb # sizes of different blocks
    numb # numbers of different blocks
    blocks # block structure
    cl # numbers of blocks
    blocksize # sizes of blocks
    Mmatrix # Moment matrix
    solver # SDP solver
    tol # tolerance to certify global optimality
    flag # 0 if global optimality is certified; 1 otherwise
end

"""
    opt,sol,data = cs_tssos_first(pop, x, d; nb=0, numeq=0, foc=100, CS="MF", minimize=false,
    assign="first", TS="block", merge=false, md=3, solver="Mosek", QUIET=false, solve=true, solution=false,
    MomentOne=true, tol=1e-4)

Compute the first step of the CS-TSSOS hierarchy for constrained polynomial optimization with
relaxation order `d`. Return the optimum, the (near) optimal solution (if `solution=true`) and
other auxiliary data.

# Arguments
- `pop`: the vector of the objective function, inequality constraints, and equality constraints.
- `x`: the set of variables.
- `d`: the relaxation order of the moment-SOS hierarchy.
- `nb`: the number of binary variables in `x`.
- `md`: the tunable parameter for merging blocks.
- `numeq`: the number of equality constraints.
"""
function cs_tssos_first(pop, x, d; nb=0, numeq=0, foc=100, CS="MF", minimize=false, assign="first", TS="block",
    merge=false, md=3, solver="Mosek", tune=false, QUIET=false, solve=true, solution=false, MomentOne=true, Mommat=false, tol=1e-4)
    n,supp,coe = polys_info(pop, x)
    opt,sol,data = cs_tssos_first(supp, coe, n, d, numeq=numeq, nb=nb, foc=foc, CS=CS, minimize=minimize, assign=assign, TS=TS,
    merge=merge, md=md, QUIET=QUIET, solver=solver, tune=tune, solve=solve, solution=solution, MomentOne=MomentOne, Mommat=Mommat, tol=tol)
    return opt,sol,data
end

function polys_info(pop, x)
    n = length(x)
    m = length(pop)-1
    coe = Vector{Vector{Float64}}(undef, m+1)
    supp = Vector{Vector{Vector{UInt16}}}(undef, m+1)
    for k = 1:m+1
        mon = monomials(pop[k])
        coe[k] = coefficients(pop[k])
        lm = length(mon)
        supp[k] = [UInt16[] for i=1:lm]
        for i = 1:lm
            ind = mon[i].z .> 0
            vars = mon[i].vars[ind]
            exp = mon[i].z[ind]
            for j = 1:length(vars)
                l = ncbfind(x, n, vars[j])
                append!(supp[k][i], l*ones(UInt16, exp[j]))
            end
        end
    end
    return n,supp,coe
end

"""
    opt,sol,data = cs_tssos_first(supp::Vector{Vector{Vector{UInt16}}}, coe, n, d; numeq=0,
    nb=0, foc=100, CS="MF", minimize=false, assign="first", TS="block", merge=false, md=3,
    QUIET=false, solver="Mosek", solve=true, solution=false, MomentOne=true, tol=1e-4)

Compute the first step of the CS-TSSOS hierarchy for constrained polynomial optimization with
relaxation order `d`. Here the polynomial optimization problem is defined by `supp` and `coe`,
corresponding to the supports and coeffients of `pop` respectively.
Return the optimum, the (near) optimal solution (if `solution=true`) and other auxiliary data.

# Arguments
- `supp`: the supports of the polynomial optimization problem.
- `coe`: the coeffients of the polynomial optimization problem.
- `d`: the relaxation order of the moment-SOS hierarchy.
- `numeq`: the number of equality constraints.
"""
function cs_tssos_first(supp::Vector{Vector{Vector{UInt16}}}, coe, n, d; numeq=0, nb=0,
    foc=100, CS="MF", minimize=false, assign="first", TS="block", merge=false, md=3, QUIET=false,
    solver="Mosek", tune=false, solve=true, solution=false, MomentOne=true, Mommat=false, tol=1e-4)
    println("************************TSSOS************************")
    println("TSSOS is launching...")
    m = length(supp)-1
    supp[1],coe[1] = resort(supp[1], coe[1])
    dg = [maximum(length.(supp[i])) for i=2:m+1]
    time = @elapsed begin
    cliques,cql,cliquesize = clique_decomp(n, m, dg, supp, order=d, alg=CS, minimize=minimize)
    end
    if CS != false && QUIET == false
        mc = maximum(cliquesize)
        println("Obtained the variable cliques in $time seconds. The maximal size of cliques is $mc.")
    end
    J,ncc = assign_constraint(m, supp, cliques, cql, cliquesize, assign=assign)
    rlorder = init_order(dg, J, cliquesize, cql, foc=foc, order=d)
    if TS != false && QUIET == false
        println("Starting to compute the block structure...")
    end
    time = @elapsed begin
    blocks,cl,blocksize,sb,numb,basis,status = get_cblocks_mix(dg, J, rlorder, m, supp, cliques, cql, cliquesize, nb=nb, TS=TS, merge=merge, md=md)
    end
    if TS != false && QUIET == false
        mb = maximum(maximum.(sb))
        println("Obtained the block structure in $time seconds. The maximal size of blocks is $mb.")
    end
    opt,ksupp,moment = blockcpop_mix(n, m, supp, coe, basis, cliques, cql, cliquesize, J, ncc, blocks, cl, blocksize, numeq=numeq, nb=nb, QUIET=QUIET, TS=TS, solver=solver, tune=tune, solve=solve, solution=solution, MomentOne=MomentOne, Mommat=Mommat)
    data = mcpop_data(n, nb, m, numeq, supp, coe, basis, rlorder, ksupp, cql, cliques, cliquesize, J, ncc, sb, numb, blocks, cl, blocksize, moment, solver, tol, 1)
    sol = nothing
    if solution == true
        sol,data.flag = approx_sol(opt, moment, n, cliques, cql, cliquesize, supp, coe, numeq=numeq, tol=tol)
        if data.flag == 1
            sol,ub,gap = refine_sol(opt, sol, data, QUIET=true)
            if gap != nothing
                if gap < tol
                    data.flag = 0
                else
                    println("Found a local optimal solution giving an upper bound: $ub and a relative optimality gap: $gap.")
                end
            end
        end
    end
    return opt,sol,data
end

"""
    opt,sol,data = cs_tssos_higher!(data; TS="block", merge=false, md=3, QUIET=false, solve=true,
    solution=false, MomentOne=false)

Compute higher steps of the CS-TSSOS hierarchy.
Return the optimum, the (near) optimal solution (if `solution=true`) and other auxiliary data.
"""
function cs_tssos_higher!(data; TS="block", merge=false, md=3, QUIET=false, solve=true, tune=false, solution=false, ipart=true, MomentOne=false, Mommat=false)
    n = data.n
    nb = data.nb
    m = data.m
    numeq = data.numeq
    supp = data.supp
    coe = data.coe
    basis = data.basis
    rlorder = data.rlorder
    ksupp = data.ksupp
    cql = data.cql
    cliques = data.cliques
    cliquesize = data.cliquesize
    J = data.J
    ncc = data.ncc
    blocks = data.blocks
    cl = data.cl
    blocksize = data.blocksize
    sb = data.sb
    numb = data.numb
    solver = data.solver
    tol = data.tol
    if TS != false && QUIET == false
        println("Starting to compute the block structure...")
    end
    time = @elapsed begin
    blocks,cl,blocksize,sb,numb,basis,status = get_cblocks_mix([], J, rlorder, m, supp, cliques, cql, cliquesize, tsupp=ksupp, basis=basis, blocks=blocks, cl=cl, blocksize=blocksize, sb=sb, numb=numb, nb=nb, TS=TS, merge=merge, md=md)
    end
    opt = nothing
    sol = nothing
    if status == 1
        if TS != false && QUIET == false
            mb = maximum(maximum.(sb))
            println("Obtained the block structure in $time seconds. The maximal size of blocks is $mb.")
        end
        opt,ksupp,moment = blockcpop_mix(n, m, supp, coe, basis, cliques, cql, cliquesize, J, ncc, blocks, cl, blocksize, numeq=numeq, nb=nb, QUIET=QUIET, solver=solver, solve=solve, tune=tune, solution=solution, ipart=ipart, MomentOne=MomentOne, Mommat=Mommat)
        if solution == true
            sol,data.flag = approx_sol(opt, moment, n, cliques, cql, cliquesize, supp, coe, numeq=numeq, tol=tol)
            if data.flag == 1
                sol,ub,gap = refine_sol(opt, sol, data, QUIET=true)
                if gap != nothing
                    if gap < tol
                        data.flag = 0
                    else
                        println("Found a local optimal solution giving an upper bound: $ub and a relative optimality gap: $gap.")
                    end
                end
            end
        end
        data.ksupp = ksupp
        data.blocks = blocks
        data.cl = cl
        data.blocksize = blocksize
        data.Mmatrix = moment
        data.sb = sb
        data.numb = numb
    else
        println("No higher CS-TSSOS hierarchy!")
    end
    return opt,sol,data
end

function blockcpop_mix(n, m, supp::Vector{Vector{Vector{UInt16}}}, coe, basis, cliques, cql, cliquesize,
    J, ncc, blocks, cl, blocksize; numeq=0, nb=0, QUIET=false, TS="block", solver="Mosek", tune=false,
    solve=true, solution=false, ipart=false, MomentOne=false, Mommat=false)
    tsupp = Vector{UInt16}[]
    for i = 1:cql, j = 1:cl[i][1], k = 1:blocksize[i][1][j], r = k:blocksize[i][1][j]
        @inbounds bi = sadd(basis[i][1][blocks[i][1][j][k]], basis[i][1][blocks[i][1][j][r]], nb=nb)
        push!(tsupp, bi)
    end
    if (MomentOne == true || solution == true) && TS != false
        ksupp = copy(tsupp)
    end
    for i = 1:cql, (j, w) in enumerate(J[i])
        for l = 1:cl[i][j+1], t = 1:blocksize[i][j+1][l], r = t:blocksize[i][j+1][l], s = 1:length(supp[w+1])
            ind1 = blocks[i][j+1][l][t]
            ind2 = blocks[i][j+1][l][r]
            @inbounds bi = sadd(sadd(basis[i][j+1][ind1], supp[w+1][s], nb=nb), basis[i][j+1][ind2], nb=nb)
            push!(tsupp, bi)
        end
    end
    for i ∈ ncc, j = 1:length(supp[i+1])
        push!(tsupp, supp[i+1][j])
    end
    if (MomentOne == true || solution == true) && TS != false
        for i = 1:cql, j = 1:cliquesize[i]
            push!(tsupp, [cliques[i][j]])
            for k = j+1:cliquesize[i]
                push!(tsupp, [cliques[i][j];cliques[i][k]])
            end
        end
    end
    sort!(tsupp)
    unique!(tsupp)
    if (MomentOne == true || solution == true) && TS != false
        sort!(ksupp)
        unique!(ksupp)
    else
        ksupp = tsupp
    end
    objv = nothing
    moment = nothing
    if solve == true
        ltsupp = length(tsupp)
        if QUIET == false
            println("Assembling the SDP...")
            println("There are $ltsupp affine constraints.")
        end
        if solver == "Mosek"
            model = Model(optimizer_with_attributes(Mosek.Optimizer))
            if tune == true
                set_optimizer_attributes(model,
                "MSK_DPAR_INTPNT_CO_TOL_MU_RED" => 1e-7,
                "MSK_DPAR_INTPNT_CO_TOL_INFEAS" => 1e-7,
                "MSK_DPAR_INTPNT_CO_TOL_REL_GAP" => 1e-7,
                "MSK_DPAR_INTPNT_CO_TOL_DFEAS" => 1e-7,
                "MSK_DPAR_INTPNT_CO_TOL_PFEAS" => 1e-7,
                "MSK_DPAR_INTPNT_CO_TOL_NEAR_REL" => 1e6,
                "MSK_IPAR_BI_IGNORE_NUM_ERROR" => 1,
                "MSK_DPAR_BASIS_TOL_X" => 1e-3,
                "MSK_DPAR_BASIS_TOL_S" => 1e-3,
                "MSK_DPAR_BASIS_REL_TOL_S" => 1e-5)
            end
        elseif solver == "COSMO"
            model = Model(optimizer_with_attributes(COSMO.Optimizer, "max_iter" => 10000))
        elseif solver == "SDPT3"
            model = Model(optimizer_with_attributes(SDPT3.Optimizer))
        else
            @error "The solver is currently not supported!"
            return nothing,nothing,nothing
        end
        set_optimizer_attribute(model, MOI.Silent(), QUIET)
        time = @elapsed begin
        cons = [AffExpr(0) for i=1:ltsupp]
        for i = 1:cql
            if (MomentOne == true || solution == true) && TS != false
                bs = cliquesize[i]+1
                pos = @variable(model, [1:bs, 1:bs], PSD)
                for t = 1:bs, r = t:bs
                    if t == 1 && r == 1
                        bi = UInt16[]
                    elseif t == 1 && r > 1
                        bi = [cliques[i][r-1]]
                    else
                        bi = sadd(cliques[i][t-1], cliques[i][r-1], nb=nb)
                    end
                    Locb = bfind(tsupp, ltsupp, bi)
                    if t == r
                        @inbounds add_to_expression!(cons[Locb], pos[t,r])
                    else
                        @inbounds add_to_expression!(cons[Locb], 2, pos[t,r])
                    end
                end
            end
            for l = 1:cl[i][1]
                @inbounds bs = blocksize[i][1][l]
                if bs == 1
                    pos = @variable(model, lower_bound=0)
                    @inbounds bi = sadd(basis[i][1][blocks[i][1][l][1]], basis[i][1][blocks[i][1][l][1]], nb=nb)
                    Locb = bfind(tsupp, ltsupp, bi)
                    @inbounds add_to_expression!(cons[Locb], pos)
                else
                    pos = @variable(model, [1:bs, 1:bs], PSD)
                    for t = 1:bs, r = t:bs
                        @inbounds ind1 = blocks[i][1][l][t]
                        @inbounds ind2 = blocks[i][1][l][r]
                        @inbounds bi = sadd(basis[i][1][ind1], basis[i][1][ind2], nb=nb)
                        Locb = bfind(tsupp, ltsupp, bi)
                        if t == r
                            @inbounds add_to_expression!(cons[Locb], pos[t,r])
                        else
                            @inbounds add_to_expression!(cons[Locb], 2, pos[t,r])
                        end
                    end
                end
            end
        end
        for i in ncc
            if i <= m-numeq
                pos = @variable(model, lower_bound=0)
            else
                pos = @variable(model)
            end
            for j = 1:length(supp[i+1])
                Locb = bfind(tsupp, ltsupp, supp[i+1][j])
                @inbounds add_to_expression!(cons[Locb], coe[i+1][j], pos)
            end
        end
        for i = 1:cql, (j, w) in enumerate(J[i]), l = 1:cl[i][j+1]
            bs = blocksize[i][j+1][l]
            if bs == 1
                if w <= m-numeq
                    pos = @variable(model, lower_bound=0)
                else
                    pos = @variable(model)
                end
                ind = blocks[i][j+1][l][1]
                for s = 1:length(supp[w+1])
                    @inbounds bi = sadd(sadd(basis[i][j+1][ind], supp[w+1][s], nb=nb), basis[i][j+1][ind], nb=nb)
                    Locb = bfind(tsupp, ltsupp, bi)
                    @inbounds add_to_expression!(cons[Locb], coe[w+1][s], pos)
                end
            else
                if w <= m-numeq
                    pos = @variable(model, [1:bs, 1:bs], PSD)
                else
                    pos = @variable(model, [1:bs, 1:bs], Symmetric)
                end
                for t = 1:bs, r = t:bs
                    ind1 = blocks[i][j+1][l][t]
                    ind2 = blocks[i][j+1][l][r]
                    for s = 1:length(supp[w+1])
                        @inbounds bi = sadd(sadd(basis[i][j+1][ind1], supp[w+1][s], nb=nb), basis[i][j+1][ind2], nb=nb)
                        Locb = bfind(tsupp, ltsupp, bi)
                        if t == r
                            @inbounds add_to_expression!(cons[Locb], coe[w+1][s], pos[t,r])
                        else
                            @inbounds add_to_expression!(cons[Locb], 2*coe[w+1][s], pos[t,r])
                        end
                    end
                end
            end
        end
        bc = zeros(ltsupp)
        for i = 1:length(supp[1])
            Locb = bfind(tsupp, ltsupp, supp[1][i])
            if Locb == 0
               @error "The monomial basis is not enough!"
               return nothing,nothing,nothing
            else
               bc[Locb] = coe[1][i]
            end
        end
        @variable(model, lower)
        if solution == true || Mommat == true
            cons[1] += lower
            @constraint(model, con[i=1:ltsupp], cons[i]==bc[i])
        else
            @constraint(model, cons[2:end].==bc[2:end])
            @constraint(model, cons[1]+lower==bc[1])
        end
        @objective(model, Max, lower)
        end
        if QUIET == false
            println("SDP assembling time: $time seconds.")
            println("Solving the SDP...")
        end
        time = @elapsed begin
        optimize!(model)
        end
        if QUIET == false
            println("SDP solving time: $time seconds.")
        end
        status = termination_status(model)
        objv = objective_value(model)
        if status != MOI.OPTIMAL
           println("termination status: $status")
           status = primal_status(model)
           println("solution status: $status")
        end
        println("optimum = $objv")
        if solution == true
            measure = -dual.(con)
            moment = get_moment(measure, tsupp, cliques, cql, cliquesize, nb=nb)
        end
        if Mommat == true
            measure = -dual.(con)
            moment = get_moment(measure, tsupp, cliques, cql, cliquesize, basis=basis, nb=nb)
        end
    end
    return objv,ksupp,moment
end

function init_order(dg, J, cliquesize, cql; foc=100, order="min")
    rlorder = ones(Int, cql)
    if order == "min"
        for i = 1:cql
            if !isempty(J[i])
                rlorder[i] = ceil(Int, maximum(dg[J[i]])/2)
            end
        end
    else
        for i = 1:cql
            if cliquesize[i] <= foc
                rlorder[i] = order
            end
        end
    end
    return rlorder
end

function get_cblocks_mix(dg, J, rlorder, m, supp::Vector{Vector{Vector{UInt16}}}, cliques, cql,
    cliquesize; tsupp=[], basis=[], blocks=[], cl=[], blocksize=[], sb=[], numb=[], TS="block",
    nb=0, merge=false, md=3)
    if isempty(tsupp)
        blocks = Vector{Vector{Vector{Vector{UInt16}}}}(undef, cql)
        cl = Vector{Vector{UInt16}}(undef, cql)
        blocksize = Vector{Vector{Vector{UInt16}}}(undef, cql)
        sb = Vector{Vector{UInt16}}(undef, cql)
        numb = Vector{Vector{UInt16}}(undef, cql)
        basis = Vector{Vector{Vector{Vector{UInt16}}}}(undef, cql)
        tsupp = copy(supp[1])
        for i = 2:m+1, j = 1:length(supp[i])
            push!(tsupp, supp[i][j])
        end
        sort!(tsupp)
        unique!(tsupp)
        flag = 1
    else
        flag = 0
    end
    status = ones(Int, cql)
    for i = 1:cql
        lc = length(J[i])
        nvar = cliquesize[i]
        ind = [issubset(tsupp[j], cliques[i]) for j=1:length(tsupp)]
        fsupp = copy(tsupp[ind])
        if flag == 1
            basis[i] = Vector{Vector{Vector{UInt16}}}(undef, lc+1)
            basis[i][1] = get_sbasis(cliques[i], rlorder[i], nb=nb)
            for j = 1:length(basis[i][1])
                push!(fsupp, sadd(basis[i][1][j], basis[i][1][j], nb=nb))
            end
            sort!(fsupp)
            unique!(fsupp)
            for s = 1:lc
                basis[i][s+1] = get_sbasis(cliques[i], rlorder[i]-ceil(Int, dg[J[i][s]]/2), nb=nb)
            end
            blocks[i] = Vector{Vector{Vector{UInt16}}}(undef, lc+1)
            cl[i] = Vector{UInt16}(undef, lc+1)
            blocksize[i] = Vector{Vector{UInt16}}(undef, lc+1)
            sb[i] = Vector{UInt16}(undef, lc+1)
            numb[i] = Vector{UInt16}(undef, lc+1)
            blocks[i],cl[i],blocksize[i],sb[i],numb[i],status[i] = get_cblocks(lc, fsupp, supp[J[i].+1], basis[i], TS=TS, nb=nb, QUIET=true, merge=merge, md=md)
        else
            blocks[i],cl[i],blocksize[i],sb[i],numb[i],status[i] = get_cblocks(lc, fsupp, supp[J[i].+1], basis[i], blocks=blocks[i], cl=cl[i], blocksize=blocksize[i], sb=sb[i], numb=numb[i], TS=TS, nb=nb, QUIET=true, merge=merge, md=md)
        end
    end
    return blocks,cl,blocksize,sb,numb,basis,maximum(status)
end

function assign_constraint(m, supp::Vector{Vector{Vector{UInt16}}}, cliques, cql, cliquesize; assign="first")
    J = [UInt32[] for i=1:cql]
    ncc = UInt32[]
    for i = 2:m+1
        rind = copy(supp[i][1])
        for j = 2:length(supp[i])
            append!(rind, supp[i][j])
        end
        unique!(rind)
        if assign == "first"
            ind = findfirst(k->issubset(rind, cliques[k]), 1:cql)
            if ind != nothing
                push!(J[ind], i-1)
            else
                push!(ncc, i-1)
            end
        else
            temp = UInt32[]
            for j = 1:cql
                if issubset(rind, cliques[j])
                    push!(temp, j)
                end
            end
            if !isempty(temp)
                if assign == "min"
                    push!(J[temp[argmin(cliquesize[temp])]], i-1)
                else
                    push!(J[temp[argmax(cliquesize[temp])]], i-1)
                end
            else
                push!(ncc, i-1)
            end
        end
    end
    return J,ncc
end

function get_sbasis(var, d; nb=0)
    n = length(var)
    lb = binomial(n+d, d)
    basis = Vector{Vector{UInt16}}(undef, lb)
    basis[1] = UInt16[]
    i = 0
    t = 1
    while i < d+1
        t += 1
        if sum(basis[t-1]) == var[n]*i
           if i < d
               basis[t] = var[1]*ones(UInt16, i+1)
           end
           i += 1
        else
            j = bfind(var, n, basis[t-1][1])
            basis[t] = copy(basis[t-1])
            ind = findfirst(x->basis[t][x]!=var[j], 1:length(basis[t]))
            if ind == nothing
                ind = length(basis[t])+1
            end
            if j != 1
                basis[t][1:ind-2] = var[1]*ones(UInt16, ind-2)
            end
            basis[t][ind-1] = var[j+1]
        end
    end
    if nb > 0
        ind = [!any([basis[i][j]==basis[i][j+1]&&basis[i][j]<=nb for j=1:length(basis[i])-1]) for i=1:lb]
        basis = basis[ind]
    end
    return basis
end

function get_graph(tsupp::Vector{Vector{UInt16}}, basis::Vector{Vector{UInt16}}; nb=0)
    lb = length(basis)
    G = SimpleGraph(lb)
    ltsupp = length(tsupp)
    for i = 1:lb, j = i+1:lb
        bi = sadd(basis[i], basis[j], nb=nb)
        if bfind(tsupp, ltsupp, bi)!=0
            add_edge!(G, i, j)
        end
    end
    return G
end

function get_cgraph(tsupp::Vector{Vector{UInt16}}, supp::Vector{Vector{UInt16}}, basis::Vector{Vector{UInt16}}; nb=0)
    lb = length(basis)
    ltsupp = length(tsupp)
    G = SimpleGraph(lb)
    for i = 1:lb, j = i+1:lb
        r = 1
        while r <= length(supp)
            bi = sadd(sadd(basis[i], supp[r], nb=nb), basis[j], nb=nb)
            if bfind(tsupp, ltsupp, bi)!=0
               break
            else
                r += 1
            end
        end
        if r <= length(supp)
            add_edge!(G, i, j)
        end
    end
    return G
end

function clique_decomp(n, m, dg, supp::Vector{Vector{Vector{UInt16}}}; order="min", alg="MF", minimize=false)
    if alg == false
        cliques = [UInt16[i for i=1:n]]
        cql = 1
        cliquesize = [n]
    else
        G = SimpleGraph(n)
        for i = 1:m+1
            if order == "min" || i == 1 || order == ceil(Int, dg[i-1]/2)
                for j = 1:length(supp[i])
                    add_clique!(G, unique(supp[i][j]))
                end
            else
                temp = copy(supp[i][1])
                for j = 2:length(supp[i])
                    append!(temp, supp[i][j])
                end
                add_clique!(G, unique(temp))
            end
        end
        if alg == "NC"
            cliques,cql,cliquesize = max_cliques(G)
        else
            cliques,cql,cliquesize = chordal_cliques!(G, method=alg, minimize=minimize)
        end
    end
    uc = unique(cliquesize)
    sizes=[sum(cliquesize.== i) for i in uc]
    println("------------------------------------------------------")
    println("The clique sizes of varibles:\n$uc\n$sizes")
    println("------------------------------------------------------")
    return cliques,cql,cliquesize
end

function sadd(a, b; nb=0)
    c = [a;b]
    sort!(c)
    if nb > 0
        i = 1
        while i < length(c)
            if c[i] <= nb
                if c[i] == c[i+1]
                    deleteat!(c, i:i+1)
                else
                    i += 1
                end
            else
                break
            end
        end
    end
    return c
end

function approx_sol(opt, moment, n, cliques, cql, cliquesize, supp, coe; numeq=0, tol=1e-4)
    qsol = Float64[]
    lcq = sum(cliquesize)
    A = zeros(lcq,n)
    q = 1
    for k = 1:cql
        cqs = cliquesize[k]
        F = eigen(moment[k], cqs+1:cqs+1)
        temp = sqrt(F.values[1])*F.vectors[:,1]
        if temp[1] == 0
            temp = zeros(cqs)
        else
            temp = temp[2:cqs+1]./temp[1]
        end
        append!(qsol, temp)
        for j = 1:cqs
            A[q,cliques[k][j]] = 1
            q += 1
        end
    end
    sol = (A'*A)\(A'*qsol)
    flag = 0
    upper_bound = seval(supp[1], coe[1], sol)
    gap = abs(upper_bound)>1 ? abs((opt-upper_bound)/upper_bound) : abs(opt-upper_bound)
    if gap >= tol
        flag = 1
    end
    m = length(supp)-1
    for i = 1:m-numeq
        if seval(supp[i+1], coe[i+1], sol) <= -tol
            flag = 1
        end
    end
    for i = m-numeq+1:m
        if abs(seval(supp[i+1], coe[i+1], sol)) >= tol
            flag = 1
        end
    end
    if flag == 0
        println("Global optimality certified!")
    end
    return sol,flag
end

function get_moment(measure, tsupp, cliques, cql, cliquesize; basis=[], nb=0)
    moment = Vector{Union{Float64, Symmetric{Float64}, Array{Float64,2}}}(undef, cql)
    ltsupp = length(tsupp)
    for i = 1:cql
        if basis == []
            lb = cliquesize[i]+1
        else
            lb = length(basis[i][1])
        end
        moment[i] = zeros(Float64, lb, lb)
        if basis == []
            for j = 1:lb, k = j:lb
                if j == 1
                    if k == 1
                        bi = UInt16[]
                    else
                        bi = [cliques[i][k-1]]
                    end
                else
                    bi = sadd(cliques[i][j-1], cliques[i][k-1], nb=nb)
                end
                Locb = bfind(tsupp, ltsupp, bi)
                moment[i][j,k] = measure[Locb]
            end
        else
            for j = 1:lb, k = j:lb
                bi = sadd(basis[i][1][j], basis[i][1][k], nb=nb)
                Locb = bfind(tsupp, ltsupp, bi)
                if Locb != 0
                    moment[i][j,k] = measure[Locb]
                end
            end
        end
        moment[i] = Symmetric(moment[i],:U)
    end
    return moment
end

function ncbfind(A, l, a)
    low = 1
    high = l
    while low <= high
        mid = Int(ceil(1/2*(low+high)))
        if A[mid] == a
           return mid
        elseif A[mid] < a
            high = mid-1
        else
            low = mid+1
        end
    end
    return 0
end

function seval(supp, coe, x)
    val = 0
    for i = 1:length(supp)
        if isempty(supp[i])
            temp = 1
        else
            temp = prod(x[supp[i][j]] for j=1:length(supp[i]))
        end
        val += coe[i]*temp
    end
    return val
end


# ---
# title: 1237. Find Positive Integer Solution for a Given Equation
# id: problem1237
# author: Tian Jun
# date: 2020-10-31
# difficulty: Easy
# categories: Math, Binary Search
# link: <https://leetcode.com/problems/find-positive-integer-solution-for-a-given-equation/description/>
# hidden: true
# ---
#
# Given a function  `f(x, y)` and a value `z`, return all positive integer pairs
# `x` and `y` where `f(x,y) == z`.
#
# The function is constantly increasing, i.e.:
#
#   * `f(x, y) < f(x + 1, y)`
#   * `f(x, y) < f(x, y + 1)`
#
# The function interface is defined like this:
#
#
#
#     interface CustomFunction {
#     public:
#       // Returns positive integer f(x, y) for any given positive integer x and y.
#       int f(int x, int y);
#     };
#
#
# For custom testing purposes you're given an integer `function_id` and a target
# `z` as input, where `function_id` represent one function from an secret
# internal list, on the examples you'll know only two functions from the list.
#
# You may return the solutions in any order.
#
#
#
# **Example 1:**
#
#
#
#     Input: function_id = 1, z = 5
#     Output: [[1,4],[2,3],[3,2],[4,1]]
#     Explanation:  function_id = 1 means that f(x, y) = x + y
#
# **Example 2:**
#
#
#
#     Input: function_id = 2, z = 5
#     Output: [[1,5],[5,1]]
#     Explanation:  function_id = 2 means that f(x, y) = x * y
#
#
#
#
# **Constraints:**
#
#   * `1 <= function_id <= 9`
#   * `1 <= z <= 100`
#   * It's guaranteed that the solutions of `f(x, y) == z` will be on the range `1 <= x, y <= 1000`
#   * It's also guaranteed that `f(x, y)` will fit in 32 bit signed integer if `1 <= x, y <= 1000`
#
#
## @lc code=start
using LeetCode

## add your code here:
## @lc code=end


##################################################################
#                                             twalkexample.jl
#
#  Example for the twalk implementation in Julia,
#  Created by J Andres Christen, jac at cimat.mx .
#  Julia implementation by Nicolás Kuschinski
#
#  See http://www.cimat.mx/~jac/twalk/ for more details.
#  and for the Python, R, Matlab and C implementations.
#
##################################################################

### Differences between jtwalk and pytwalk versions:
## The Julia version does not perform output analysis
## Does not estimate the remaining time
## and does not include specialized plotting functions.

## Plotting is easy in julia! The following example
## includes sample plots using the Plots.jl plotting library

using JTwalk

####################################################################
######### Product of Independent Exponentilas #########

###   In the jtwalk object, we define the objective function with an
###   energy function U.
###   The support is defined in a separate function, and is defined in
###   jtwalk as Supp.

###   NOTE: The objective function is defined as -log of the target
###   density function

lambdas = [ 1., 2., 3., 4., 5.]

function ExpU(x)
    # -log of a product of exponentials
    return sum(x .* lambdas)
end

function ExpSupp(x)
	return all(0. .< x)
end

###   The following initializes the TWalk object
###   The dimension of the parameter space is n
Exp = jtwalk( n=5, U=ExpU, Supp=ExpSupp)

#### This runs the T-Walk

Run!(Exp, T=50000, x0=30*ones(5), xp0=40*ones(5))

##### The trajectory of the chain is stored into Exp.Output & Exp.Output_u
##### These are (n+1) by T matrices, with one row per iteration.
##### Each row has the output followed by the energy function evaluated
##### at the corresponding location. Both matrices are valid MCMC chains
##### but THEY ARE NOT INDEPENDENT.

### The following shows how to use the Plots.jl plotting library
### to make relevant plots. In julia even plotting complex thinning can
### be done with simple one line instructions.

using Plots

### The following produces a trace plot:

plot(Exp.Output[:,end])

### The following shows a scatter plot of the first and third variables with
### thinning of 1000 iteration burn-in and 250 iteration autocorrelation time:

scatter(Exp.Output[1000:250:end,1], Exp.Output[1000:250:end,3])

### The following shows a histogram of the 4th variable with the same thinning:

histogram(Exp.Output[1000:250:end,4])


module SweepOperator
export sweep!

typealias AMat{T} AbstractMatrix{T}
typealias AVec{T} AbstractVector{T}

"""
# Symmetric sweep operator
Symmetric sweep operator of the matrix `A` on element `k`.  `A` is overwritten.
`inv = true` will perform the inverse sweep.  Only the upper triangle is read and swept.

`sweep!(A, k, inv = false)`

Providing a Range, rather than an Integer, sweeps on each element in the range.

`sweep!(A, first:last, inv = false)`

### Example:

```julia
x = randn(100, 10)
xtx = x'x
sweep!(xtx, 1)
sweep!(xtx, 1, true)
```
"""
function sweep!{T<:LinAlg.BlasFloat}(A::AMat{T}, k::Integer, inv::Bool = false)
    n, p = size(A)
    # ensure @inbounds is safe
    @assert n == p "A must be square"
    @assert 1 <= k <= p "pivot element not within range"
    @inbounds d = one(T) / A[k, k]  # pivot
    # get column A[:, k] (hack because only upper triangle is available)
    akk = zeros(T, p)
    for j in 1:k
        @inbounds akk[j] = A[j, k]
    end
    for j in k+1:p
        @inbounds akk[j] = A[k, j]
    end
    BLAS.syrk!('U', 'N', -d, akk, one(T), A)  # everything not in col/row k
    scale!(akk, d * (-one(T)) ^ inv)
    for i in 1:k-1  # col k
        @inbounds A[i, k] = akk[i]
    end
    for j in k+1:p  # row k
        @inbounds A[k, j] = akk[j]
    end
    A[k, k] = -d  # pivot element
    A
end

function sweep!{T<:LinAlg.BlasFloat, I<:Integer}(A::AMat{T}, ks::AVec{I}, inv::Bool = false)
    for k in ks
        sweep!(A, k, inv)
    end
    A
end

end # module


function BlackScholesDP(CallPutFlag, S, X, t1, T2, T1, r1, r2, b, v)

#=
considers later payment especially for OTC derivatives
Usage:

t1 is the time elapsing from t to payment, 2 days is 2/365
T2 is the time to expiration plus t1
r1 is the shorter term rate
r2 is the longer term rate
BlackScholesDP("c", 130, 120, 2/365, 0.5 + 2/365, 0.5, 0.5, 0.1, 0.1, 0.12)

=#

  d1 = (log(S / X) + (b + v^2 / 2) * T1) / (v * sqrt(T1))
  d2 = d1 - v * sqrt(T1)

  if CallPutFlag == "c"
    BS = exp(r1 * t1) * exp(-r2* T2) * (S * exp(b * T1) * CND(d1) - X * CND(d2))
  elseif CallPutFlag == "p"
    BS = exp(r1 * t1) * exp(-r2* T2) * (X * CND(-d2) - S* exp(-r * T) * CND(-d1))
  end

end


# deslant.jl (translated from python implementation by Thomas Breuel [UniKL] )
#
# author: Wade Shen
# swade@ll.mit.edu
# Copyright © 2009 Massachusetts Institute of Technology, Lincoln Laboratory
# version 0.1
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
using MNIST, Stage, PyCall
@pyimport scipy.ndimage.interpolation as interpolation

# Helpers
log = Log(STDERR)

# ----------------------------------------------------------------------------------------------------------------
# setup MNIST task
# ----------------------------------------------------------------------------------------------------------------
const c0 = [ i for i = 0:27, j = 0:27 ]
const c1 = [ j for i = 0:27, j = 0:27 ]

function moments(image)
  si  = sum(image)
  m0  = sum(c0 .* image)/si
  m1  = sum(c1 .* image)/si
  m00 = sum((c0 - m0) .* (c0 - m0) .* image)/si
  m11 = sum((c1 - m1) .* (c1 - m1) .* image)/si
  m01 = sum((c0 - m0) .* (c1 - m1) .* image)/si
  return [ m0, m1 ], [ m00 m01; m01 m11 ]
end

function deskew(matrix)
  output = zeros(size(matrix))
  for j = 1:size(matrix, 2)
    image   = reshape(matrix[:, j], (28, 28))
    c, v    = moments(image)
    alpha   = v[1, 2] / v[1, 1]
    affine  = [1.0 0.0; alpha 1.0]
    ocenter = [ size(image, 1)/2.0, size(image, 2)/2.0 ]
    offset  = c - (affine * ocenter)
    mat     = interpolation.affine_transform(image, affine, offset = offset, order = 1)
    for i = 1:size(output, 1)
      output[i, j] = mat[i]
    end
  end

  return output
end

@info log "reading training data"
const train_raw, train_truth = traindata()
const test_raw, test_truth   = testdata()
const classes                = Dict{Float64, Int32}()
const train                  = deskew(train_raw)
const test                   = deskew(test_raw)

f = open("mnist-deslant.jd", "w")
serialize(f, train)
serialize(f, train_truth)
serialize(f, test)
serialize(f, test_truth)
close(f)


### A Pluto.jl notebook ###
# v0.12.4

using Markdown
using InteractiveUtils

# ╔═╡ bb6cae46-4108-11eb-2309-3d11f13a19fa
begin
	using Pkg
	Pkg.activate(mktempdir())
end

# ╔═╡ d8b334ae-4108-11eb-2546-99e75efc18d6
begin
	Pkg.add(["Plots", "PlutoUI","StatsBase", "DataStructures", "Unitful", "PhysicalConstants", "UnitfulEquivalences"])

	using Plots
	plotly()
	using PlutoUI
	using StatsBase
	using DataStructures
	using Unitful
	using PhysicalConstants.CODATA2018: c_0, h, ħ
	using UnitfulEquivalences
end

# ╔═╡ 56127994-4129-11eb-1b74-e92215af5b82
energy1 = 6.2u"keV"

# ╔═╡ 54d9f070-4129-11eb-13f6-2b138f584c42
wavelength1 = uconvert(u"pm", energy1, Spectral())

# ╔═╡ 83bc9488-4129-11eb-3c64-d71221b14aeb
freq1 = uconvert(u"Hz", energy1, Spectral())

# ╔═╡ 5990a14a-410b-11eb-086a-f129b7fdf82c
refracted_speed2 = uconvert(u"Mm/s", c_0 / 1.33)

# ╔═╡ 6209ea56-410c-11eb-0bb3-ff80947cdcb0
wavelength2 = 1.5u"μm"

# ╔═╡ a78b9c3a-4109-11eb-26ca-1556cfeb2b8c
quantum_energy2 = uconvert(u"eV", wavelength2, Spectral())

# ╔═╡ 84c21a6c-4109-11eb-166e-151c460d28c8
md"""
## E2

Light, which possesses in vacuum the frequency of 500 THz and quantum energy of $quantum_energy2, passes through
sheet of glass with the refractive index of 1.5. The speed of such light in glass is $refracted_speed2, the frequency is
500 THz, the wavelength is $wavelength2 and quantum energy is 2.07 eV.
"""

# ╔═╡ 1fd66092-412a-11eb-3468-bf3c5c496686
freq2 = uconvert(u"THz", wavelength2, Spectral())

# ╔═╡ fdae38fe-410c-11eb-18dd-9bae51fb4e00
freq3 = 350u"THz"

# ╔═╡ 220ec448-410d-11eb-17c3-d1682a1588c9
refractive3 = 1.5

# ╔═╡ fdbe0894-410c-11eb-0ace-3d269b2942aa
n = refractive3 ^2

# ╔═╡ 74fa7748-410c-11eb-0688-a37608618c96
md"""
## E3.1

The electric field strength that oscillates with the frequency of **$freq3** is reduced **$n** times in some substance
with respect to the vacuum. The refractive index of this substance is **$refractive3**
"""

# ╔═╡ 31bce1b8-410d-11eb-008b-bf04b66bf555
slit_length4 = 0.03u"mm"

# ╔═╡ 2e0551ea-410d-11eb-176c-650b9c498d25
distance_from_slit = 2u"m"

# ╔═╡ 0aaf6748-410e-11eb-035a-8be0950091d6
minima_distance = 8u"cm"

# ╔═╡ 0aa8bb82-410e-11eb-163c-6de6eda9e616
minima_angle = uconvert(u"rad", atan(minima_distance/2, distance_from_slit))

# ╔═╡ 70a82a76-410e-11eb-2f0e-295b06d5d8fb
wavelength4 = uconvert(u"nm", sin(minima_angle) * slit_length4)

# ╔═╡ 3bb15c6c-410d-11eb-1b1b-4b13708ceb70
md"""
## E4.1

The monochromatic light is penetrating a single **$slit_length4** wide vertical slit. On the screen positioned perpendicular to the light propagating direction at the distance of **$distance_from_slit** behind the slit, a diffraction pattern appears in which the two first minima are located at the distance of **$minima_distance** from each other. The first minimum is located at the angle of **$minima_angle** with respect to the diffraction pattern centre. The wavelength of the light used was **$wavelength4**
"""

# ╔═╡ 0d5b2ca4-412f-11eb-2dec-d1d78f201ff5
wavelength42 = 600u"nm"

# ╔═╡ 0d6c3466-412f-11eb-2ed7-6311848972ed
distance_from_grate = 4u"m"

# ╔═╡ 564061be-412f-11eb-13e1-894885ffb2be
maxima_distance = 24u"cm"

# ╔═╡ 0c352744-412f-11eb-37ae-ff4b3812f6c2
md"""
## E4.2
Red light, which possesses in vacuum the wavelength of **$wavelength42** is penetrating a diffraction grating. On the screen positioned perpendicular to the light propagating direction at the distance of **$distance_from_grate** from the grating, the two first order maxima in the diffraction pattern are located at the distance of **$maxima_distance** from each other.

"""

# ╔═╡ 97321f46-412f-11eb-2cda-b7e915c51a5b
maxima_angle = uconvert(u"rad", atan(maxima_distance/2, distance_from_grate))

# ╔═╡ cb110218-4130-11eb-1284-0fce3810c11f
grating_period = uconvert(u"mm", wavelength42/sin(maxima_angle))

# ╔═╡ c08c3f10-4117-11eb-059e-1fa7c75e4240
absorption_coef_blue = 0.02u"cm^-1"

# ╔═╡ ffd6d630-4117-11eb-2200-a147d74af92f
absorbtion_coef_green = 4u"cm^-1"

# ╔═╡ 38b97e80-4118-11eb-34c5-65de18666aed
absorption_coef_red = 7u"cm^-1"

# ╔═╡ 45bb6c38-4118-11eb-0ba5-f55cde355ccd
green_layer_thickness = uconvert(u"cm", 1/absorbtion_coef_green)

# ╔═╡ 40d8a62c-4118-11eb-15c3-e9e0c5d87609
n_intensity_reduced = 7

# ╔═╡ 793b338a-4119-11eb-1916-6513f6eed696
ℯ

# ╔═╡ afcc4710-4119-11eb-28db-fdf5aa042d8e
substance_color1 = "blue"

# ╔═╡ d0340d2e-4119-11eb-209d-7dcf7fa4c2e7
substance_color2 = "black"

# ╔═╡ c094cb3a-4117-11eb-168a-df762ad5739e
md"""
## E5
For some substance, the absorption coefficient for the blue light is **$absorption_coef_blue**. For the green light, it is **$absorbtion_coef_green** and for the red light **$absorption_coef_red**. This means that the intensity of the green light will be reduced Euler number (e = 2.7183..) times by passing through the **$green_layer_thickness** thick layer of the substance. The layer with the thickness of 1 cm reduces the intensity of green light e **$n_intensity_reduced** times. If we illuminate this substance with the white light, the colour of the substance will be **$substance_color1**. If we illuminate the substance with the blue light, the colour of the substance will be **$substance_color2**.

"""

# ╔═╡ 80572be0-411b-11eb-3f15-fbf4e6ed7cf3
electron_speed = 364.7u"km/s"

# ╔═╡ 5788010a-411e-11eb-2c69-bd73c5cd6d3f
ground_speed = 2188u"km/s"

# ╔═╡ 7fe77a98-411b-11eb-1835-75b0687dedfd
n6 = round(ground_speed / electron_speed)

# ╔═╡ 9f78c474-412b-11eb-1cf0-d7f1a152ffcf
reduced_planck = 1.05E-34u"kg*m^2*s^-1"

# ╔═╡ 7fc27112-411b-11eb-3b4b-97599735cd31
angular_momentum = uconvert(u"kg*m^2*s^-1", reduced_planck * n6)

# ╔═╡ 224bba9e-411f-11eb-2901-87f766e95a56
ground_frequency = 6580u"THz"

# ╔═╡ 3ed8da6c-411e-11eb-3a35-4164dfd2aa54
imagined_rotation_frequency = uconvert(u"THz", ground_frequency / n6 ^3)

# ╔═╡ 91b1a86c-411f-11eb-0700-a30281bd1f40
ground_energy = -13.60u"eV"

# ╔═╡ 8a2e2e76-411f-11eb-0da9-ed7c9382599f
net_energy = ground_energy / n6^2

# ╔═╡ 0396e5f2-4120-11eb-1502-f358ec34acd9
ground_radius = 0.529u"Å"

# ╔═╡ 03d6d638-4120-11eb-20d0-0fa5cffc6d3d
mean_distance = ground_radius * n6^2

# ╔═╡ 296b84ce-4121-11eb-2c07-1b1ff4cac46c
emitted_photon_wavelength = 410.6u"nm"

# ╔═╡ f74d6ff0-4120-11eb-2de1-9d7c40c9354c
quantum_energy6 = uconvert(u"eV", emitted_photon_wavelength, Spectral())

# ╔═╡ 66942310-4121-11eb-063c-37022902216f
energy_after_photon = net_energy - quantum_energy6

# ╔═╡ 81bc1422-4121-11eb-0cb5-215e6fb5c308
qn = round(sqrt(ground_energy/energy_after_photon))

# ╔═╡ 8238f47e-4121-11eb-06a7-a3f1bf8887cb
imag_rot_freq2 = uconvert(u"THz", ground_frequency / qn ^3)

# ╔═╡ 0e7c4e68-4122-11eb-3b1a-8f9808df6829
mean_distance2 = ground_radius * qn^2

# ╔═╡ 7fec90b4-411b-11eb-04db-a913940a9f22
md"""
## E6
In the atom of hydrogen, the electron moves according to the Bohr model. Its speed is **$electron_speed**. The angular momentum of this electron is **$angular_momentum**, the frequency of imagined rotation is **$imagined_rotation_frequency**, the net energy is **$net_energy** and the mean distance from the nucleus is **$mean_distance**. The electron comes closer to the nucleus and a photon with the wavelength of **$emitted_photon_wavelength** is emitted. The quantum energy of this radiation is **$quantum_energy6**. At the end of the transition, the frequency of the imagined rotation of the electron is **$imag_rot_freq2** and its mean distance from the nucleus is **$mean_distance2**.
"""

# ╔═╡ 46652b7e-4122-11eb-1576-29e34caf1b03
mass = 20u"mg"

# ╔═╡ 78e0e354-4122-11eb-1a11-0b1d36a38b70
diam = 5u"mm"

# ╔═╡ 46798970-4122-11eb-31a8-ab967162b1ae
h_bar = 1E-34u"J*s"

# ╔═╡ 4691ff34-4122-11eb-1614-b5df5b328525
speed_increase = uconvert(u"m/s", h_bar / (mass * diam))

# ╔═╡ c9767010-4125-11eb-3529-43e03fc0ed7b
time_for_diam = uconvert(u"s", diam / speed_increase)

# ╔═╡ 63ef8d68-412e-11eb-15fd-79cc263180f7
seconds_in_year = (365 * 24 * 60 * 60)u"s"

# ╔═╡ c9680bce-4125-11eb-1a30-93e346ab5ef8
time2 = uconvert(u"s", time_for_diam)

# ╔═╡ 451edba2-4122-11eb-0ea9-b3e5a7021bd1
md"""
# E7

The uranium atom (a microscopic object) with the mass of **$mass** and diameter of **$diam** moves in a certain direction. The location of the atom is fixed by its passing through the slit possessing the width almost equal to the atom diameter. The screen containing this slit is positioned perpendicular to the velocity of the atom. As a result of penetrating this slit, the speed of the atom receives some unpredictable increase perpendicular to the direction of previous motion. Using the reduced Planck constant 10–34 J*s as a typical value of impact uncertainty, we can conclude that the increase of the speed is **$speed_increase**. After **$time_for_diam** seconds, the atom deviates from its classical straight trajectory by its own diameter. This result means that we can predict the motion of uranium atom in a classical way for maximally **$time2** nanoseconds.

"""

# ╔═╡ 571de63e-412e-11eb-3b48-d9192483a1de
time2_years = time2 / seconds_in_year

# ╔═╡ aa066c66-4126-11eb-1909-713aeda74edc
mass8 = 9E-31u"kg"

# ╔═╡ c4375a50-4126-11eb-2667-7b693682e9a8
well_width = 2u"Å"

# ╔═╡ aa323b5c-4126-11eb-18fa-a5aa100d0678
qn8 = 5

# ╔═╡ aa487444-4126-11eb-306c-1d98f391cfc6
planck8 = 6.6E-34u"J*s"

# ╔═╡ aa708326-4126-11eb-2129-df2f5666aa93
wavelength = 2*well_width / qn8

# ╔═╡ aa9ab420-4126-11eb-206b-2bd429f38b13
lin_momentum = uconvert(u"kg*m/s", planck8 / wavelength)

# ╔═╡ aa805792-4126-11eb-20eb-b127d6ae2898
energy8 = uconvert(u"eV", lin_momentum^2/(2*mass8))

# ╔═╡ c98691d4-4125-11eb-3e71-d1ffa4fc0f3a
md"""
## E8

An electron with the mass of **$mass8** is located in a single-dimensional potential well with the width of **$well_width** (approximate diameter of the atom). Let us describe this electron according to the laws of quantum mechanics as a standing wave in the string, whereas the length of the string is equal to the width of the potential well. In the state described by the quantum number **$qn8**, the wavelength of the electron is **$wavelength**, its
linear momentum is **$lin_momentum** and its energy is **$energy8**.

"""

# ╔═╡ aa5f27d4-4126-11eb-19db-9b02f89cf0f0
md"""

## E9

An incandescent lamp has the nominal power of 60 W and the nominal voltage of 220 V. The cylindrical
filament of this lamp operating at the absolute temperature of 2650 K is made from tungsten (W). The
resistivity of tungsten at 2650 K is 0.64 μΩ*m. The electric field strength inside the filament is 500 V/m.
The resistance of the filament is 807 ohms, its length is 44 cm, the cross-sectional area of the filament is
3.49 *10–10 m2 and its radius is 10.5 micrometers. The cylinder surface area of the filament is 29.0 mm2.
The surface radiance of the filament is 2.07 MW/m2. If we describe the filament as a blackbody, its radiance
will be 2.80 MW/m2. The maximum of the spectral radiance of the lamp will be located in the spectrum at
the wavelength of 1094 nm.

"""

# ╔═╡ aa1d3ab8-4126-11eb-11d9-ab8a72c7e68f
power = 21u"W"

# ╔═╡ 244d7438-4131-11eb-1b84-c37c9c688b08
voltage = 12u"V"

# ╔═╡ 2d25a12a-4131-11eb-0cf6-7f77bfcf2071
temp = 2750u"K"

# ╔═╡ 3860b1f6-4131-11eb-198e-f3f101300a20
resistivity = 0.67u"μΩ*m"

# ╔═╡ 53aad108-4131-11eb-393a-3b7c4707017a
efs = 350u"V/m"

# ╔═╡ 624d2ab2-4131-11eb-0d4d-1f55c87e0abb
resistance = uconvert(u"Ω", voltage^2/power)

# ╔═╡ 7be2c68c-4131-11eb-3e4f-41b7e535bd69
length = uconvert(u"cm", voltage / efs)

# ╔═╡ b418c7fc-4131-11eb-36ad-0bcaf71c41b3
cross_section = uconvert(u"m^2", (resistivity * length) /  resistance)

# ╔═╡ 038ff9c2-4132-11eb-3ccb-eb745f9d9827
radius = uconvert(u"μm", sqrt(cross_section/pi))

# ╔═╡ 3c07c514-4132-11eb-0a3e-8b13abce99bb
silinder_surface = uconvert(u"mm^2", 2*pi*radius*length)

# ╔═╡ 9cbd7b60-4132-11eb-0692-67048ff2a347
wien_displacement = 2896u"μm*K"

# ╔═╡ 103478d8-4132-11eb-116e-6137e6dde875
radiant_exitance = uconvert(u"MW/m^2", power / silinder_surface)

# ╔═╡ Cell order:
# ╠═bb6cae46-4108-11eb-2309-3d11f13a19fa
# ╠═d8b334ae-4108-11eb-2546-99e75efc18d6
# ╠═56127994-4129-11eb-1b74-e92215af5b82
# ╠═54d9f070-4129-11eb-13f6-2b138f584c42
# ╠═83bc9488-4129-11eb-3c64-d71221b14aeb
# ╟─84c21a6c-4109-11eb-166e-151c460d28c8
# ╠═a78b9c3a-4109-11eb-26ca-1556cfeb2b8c
# ╠═5990a14a-410b-11eb-086a-f129b7fdf82c
# ╠═6209ea56-410c-11eb-0bb3-ff80947cdcb0
# ╠═1fd66092-412a-11eb-3468-bf3c5c496686
# ╟─74fa7748-410c-11eb-0688-a37608618c96
# ╠═fdae38fe-410c-11eb-18dd-9bae51fb4e00
# ╠═fdbe0894-410c-11eb-0ace-3d269b2942aa
# ╠═220ec448-410d-11eb-17c3-d1682a1588c9
# ╠═3bb15c6c-410d-11eb-1b1b-4b13708ceb70
# ╠═31bce1b8-410d-11eb-008b-bf04b66bf555
# ╠═2e0551ea-410d-11eb-176c-650b9c498d25
# ╠═0aaf6748-410e-11eb-035a-8be0950091d6
# ╠═0aa8bb82-410e-11eb-163c-6de6eda9e616
# ╠═70a82a76-410e-11eb-2f0e-295b06d5d8fb
# ╠═0c352744-412f-11eb-37ae-ff4b3812f6c2
# ╠═0d5b2ca4-412f-11eb-2dec-d1d78f201ff5
# ╠═0d6c3466-412f-11eb-2ed7-6311848972ed
# ╠═564061be-412f-11eb-13e1-894885ffb2be
# ╠═97321f46-412f-11eb-2cda-b7e915c51a5b
# ╠═cb110218-4130-11eb-1284-0fce3810c11f
# ╠═c094cb3a-4117-11eb-168a-df762ad5739e
# ╠═c08c3f10-4117-11eb-059e-1fa7c75e4240
# ╠═ffd6d630-4117-11eb-2200-a147d74af92f
# ╠═38b97e80-4118-11eb-34c5-65de18666aed
# ╠═45bb6c38-4118-11eb-0ba5-f55cde355ccd
# ╠═40d8a62c-4118-11eb-15c3-e9e0c5d87609
# ╠═793b338a-4119-11eb-1916-6513f6eed696
# ╠═afcc4710-4119-11eb-28db-fdf5aa042d8e
# ╠═d0340d2e-4119-11eb-209d-7dcf7fa4c2e7
# ╟─7fec90b4-411b-11eb-04db-a913940a9f22
# ╠═80572be0-411b-11eb-3f15-fbf4e6ed7cf3
# ╠═5788010a-411e-11eb-2c69-bd73c5cd6d3f
# ╠═7fe77a98-411b-11eb-1835-75b0687dedfd
# ╠═9f78c474-412b-11eb-1cf0-d7f1a152ffcf
# ╠═7fc27112-411b-11eb-3b4b-97599735cd31
# ╠═224bba9e-411f-11eb-2901-87f766e95a56
# ╠═3ed8da6c-411e-11eb-3a35-4164dfd2aa54
# ╠═91b1a86c-411f-11eb-0700-a30281bd1f40
# ╠═8a2e2e76-411f-11eb-0da9-ed7c9382599f
# ╠═0396e5f2-4120-11eb-1502-f358ec34acd9
# ╠═03d6d638-4120-11eb-20d0-0fa5cffc6d3d
# ╠═296b84ce-4121-11eb-2c07-1b1ff4cac46c
# ╠═f74d6ff0-4120-11eb-2de1-9d7c40c9354c
# ╠═66942310-4121-11eb-063c-37022902216f
# ╠═81bc1422-4121-11eb-0cb5-215e6fb5c308
# ╠═8238f47e-4121-11eb-06a7-a3f1bf8887cb
# ╠═0e7c4e68-4122-11eb-3b1a-8f9808df6829
# ╟─451edba2-4122-11eb-0ea9-b3e5a7021bd1
# ╠═46652b7e-4122-11eb-1576-29e34caf1b03
# ╠═78e0e354-4122-11eb-1a11-0b1d36a38b70
# ╠═46798970-4122-11eb-31a8-ab967162b1ae
# ╠═4691ff34-4122-11eb-1614-b5df5b328525
# ╠═c9767010-4125-11eb-3529-43e03fc0ed7b
# ╠═571de63e-412e-11eb-3b48-d9192483a1de
# ╠═63ef8d68-412e-11eb-15fd-79cc263180f7
# ╠═c9680bce-4125-11eb-1a30-93e346ab5ef8
# ╠═c98691d4-4125-11eb-3e71-d1ffa4fc0f3a
# ╠═aa066c66-4126-11eb-1909-713aeda74edc
# ╠═c4375a50-4126-11eb-2667-7b693682e9a8
# ╠═aa323b5c-4126-11eb-18fa-a5aa100d0678
# ╠═aa487444-4126-11eb-306c-1d98f391cfc6
# ╠═aa708326-4126-11eb-2129-df2f5666aa93
# ╠═aa9ab420-4126-11eb-206b-2bd429f38b13
# ╠═aa805792-4126-11eb-20eb-b127d6ae2898
# ╠═aa5f27d4-4126-11eb-19db-9b02f89cf0f0
# ╠═aa1d3ab8-4126-11eb-11d9-ab8a72c7e68f
# ╠═244d7438-4131-11eb-1b84-c37c9c688b08
# ╠═2d25a12a-4131-11eb-0cf6-7f77bfcf2071
# ╠═3860b1f6-4131-11eb-198e-f3f101300a20
# ╠═53aad108-4131-11eb-393a-3b7c4707017a
# ╠═624d2ab2-4131-11eb-0d4d-1f55c87e0abb
# ╠═7be2c68c-4131-11eb-3e4f-41b7e535bd69
# ╠═b418c7fc-4131-11eb-36ad-0bcaf71c41b3
# ╠═038ff9c2-4132-11eb-3ccb-eb745f9d9827
# ╠═3c07c514-4132-11eb-0a3e-8b13abce99bb
# ╠═9cbd7b60-4132-11eb-0692-67048ff2a347
# ╠═103478d8-4132-11eb-116e-6137e6dde875


"""
    galerkin_matrix(
        B::AbstractBSplineBasis,
        [deriv = (Derivative(0), Derivative(0))],
        [MatrixType = BandedMatrix{Float64}],
    )

Compute Galerkin mass or stiffness matrix, as well as more general variants
of these.

# Extended help

The Galerkin mass matrix is defined as

```math
M_{ij} = ⟨ ϕ_i, ϕ_j ⟩ \\quad \\text{for} \\quad
i ∈ [1, N] \\text{ and } j ∈ [1, N],
```

where ``ϕ_i(x)`` is the ``i``-th basis function and `N = length(B)` is the
number of functions in the basis `B`.
Here, ``⟨f, g⟩`` is the [``L^2`` inner
product](https://en.wikipedia.org/wiki/Square-integrable_function#Properties)
between functions ``f`` and ``g``.

Since products of B-splines are themselves piecewise polynomials, integrals can
be computed exactly using [Gaussian quadrature
rules](https://en.wikipedia.org/wiki/Gaussian_quadrature).
To do this, we use Gauss--Legendre quadratures via the
[FastGaussQuadrature](https://github.com/JuliaApproximation/FastGaussQuadrature.jl)
package.

## Matrix layout and types

The mass matrix is banded with ``2k - 1`` bands.
Moreover, the matrix is symmetric and positive definite, and only ``k`` bands are
needed to fully describe the matrix.
Hence, a
[`Hermitian`](https://docs.julialang.org/en/v1/stdlib/LinearAlgebra/index.html#LinearAlgebra.Hermitian)
view of an underlying matrix is returned.

By default, the underlying matrix holding the data is a `BandedMatrix` that
defines the upper part of the symmetric matrix.
Other types of container are also supported, including regular sparse matrices
(`SparseMatrixCSC`) and dense arrays (`Matrix`).
See [`collocation_matrix`](@ref) for a discussion on matrix types.

## Derivatives of basis functions

Galerkin matrices associated to the derivatives of basis functions may be
constructed using the optional `deriv` parameter.
For instance, if `deriv = (Derivative(0), Derivative(2))`, the matrix
``⟨ ϕ_i, ϕ_j'' ⟩`` is constructed, where primes denote derivatives.
Note that, if the derivative orders are different, the resulting matrix is not
symmetric, and a `Hermitian` view is not returned in those cases.

## Combining different bases

More generally, it is possible to compute matrices of the form
``⟨ ψ_i^{(n)}, ϕ_j^{(m)} ⟩``, where `n` and `m` are derivative orders, and ``ψ_i``
and ``ϕ_j`` belong to two *different* (but related) bases `B₁` and `B₂`.
For this, instead of the `B` parameter, one must pass a tuple of bases
`(B₁, B₂)`.
The restriction is that the bases must have the same parent B-spline basis.
That is, they must share the same set of B-spline knots and be of equal
polynomial order.

Note that, if both bases are different, the matrix will not be symmetric, and
will not even be square if the bases have different lengths.

In practice, this feature may be used to combine a B-spline basis `B`, with a
recombined basis `R` generated from `B` (see [Basis recombination](@ref basis-recombination-api)).
"""
function galerkin_matrix(
        Bs::NTuple{2,AbstractBSplineBasis},
        deriv::DerivativeCombination{2} = Derivative.((0, 0)),
        ::Type{M} = BandedMatrix{Float64},
    ) where {M <: AbstractMatrix}
    B1, B2 = Bs
    symmetry = M <: Hermitian
    if symmetry && (deriv[1] !== deriv[2] || B1 !== B2)
        throw(ArgumentError(
            "input matrix type incorrectly assumes symmetry"))
    end
    A = allocate_galerkin_matrix(M, Bs)
    galerkin_matrix!(A, Bs, deriv)
end

function galerkin_matrix(
        B::AbstractBSplineBasis,
        deriv::DerivativeCombination{2} = Derivative.((0, 0)),
        ::Type{Min} = BandedMatrix{Float64},
    ) where {Min <: AbstractMatrix}
    symmetry = deriv[1] === deriv[2]
    T = eltype(Min)
    M = symmetry ? Hermitian{T,Min} : Min
    galerkin_matrix((B, B), deriv, M)
end

galerkin_matrix(B, ::Type{M}) where {M <: AbstractMatrix} =
    galerkin_matrix(B, Derivative.((0, 0)), M)

function _check_bases(Bs::Tuple{Vararg{AbstractBSplineBasis}})
    ps = parent.(Bs)
    if any(ps .!== ps[1])
        throw(ArgumentError(
            "all bases must share the same parent B-spline basis"
        ))
    end
    nothing
end

allocate_galerkin_matrix(::Type{Hermitian{T,M}},
                         Bs) where {T, M <: AbstractMatrix} =
    Hermitian(allocate_galerkin_matrix(M, Bs, true))

allocate_galerkin_matrix(::Type{M}, Bs) where {M} =
    allocate_galerkin_matrix(M, Bs, false)

allocate_galerkin_matrix(::Type{M}, Bs, _symmetry) where {M <: AbstractMatrix} =
    M(undef, length.(Bs)...)

allocate_galerkin_matrix(::Type{SparseMatrixCSC{T}}, Bs, _symmetry) where {T} =
    spzeros(T, length.(Bs)...)

function allocate_galerkin_matrix(::Type{M}, Bs, symmetry) where {M <: BandedMatrix}
    _check_bases(Bs)
    B1, B2 = Bs
    k = order(B1)
    @assert k == order(B2)  # verified in _check_bases
    # When the bases are the same, the upper/lower bandwidths are Nb = k - 1
    # (total = 2k - 1 bands).
    # If the matrix is symmetric, we only store the upper band.
    Nb = k - 1
    N1, N2 = length.(Bs)
    bands = if symmetry
        (0, Nb)
    else
        # If the bases have different number of constraints (BCs) on the left
        # boundary (⇔ δl ≠ 0), then the bands are shifted.
        δl, δr = num_constraints(B2) .- num_constraints(B1)
        @assert N1 == N2 + δl + δr
        (Nb + δl, Nb - δl)
    end
    M(undef, (N1, N2), bands)
end

"""
    galerkin_matrix!(A::AbstractMatrix, B::AbstractBSplineBasis,
                     deriv = (Derivative(0), Derivative(0)))

Fill preallocated Galerkin matrix.

The matrix may be a `Hermitian` view, in which case only one half of the matrix
will be filled. Note that, for the matrix to be symmetric, both derivative orders
in `deriv` must be the same.

More generally, it is possible to combine different functional bases by passing
a tuple of `AbstractBSplineBasis` as `B`.

See [`galerkin_matrix`](@ref) for details.
"""
function galerkin_matrix! end

galerkin_matrix!(M, B::AbstractBSplineBasis, args...) =
    galerkin_matrix!(M, (B, B), args...)

function galerkin_matrix!(
        S::AbstractMatrix, Bs::Tuple{Vararg{<:AbstractBSplineBasis,2}},
        deriv = Derivative.((0, 0)),
    )
    _check_bases(Bs)
    B1, B2 = Bs
    same_ij = B1 == B2 && deriv[1] == deriv[2]
    T = eltype(S)

    if size(S) != length.(Bs)
        throw(DimensionMismatch("wrong dimensions of Galerkin matrix"))
    end

    # Orders and knots are assumed to be the same (see _check_bases).
    k = order(B1)
    ts = knots(B1)
    @assert k == order(B2) && ts === knots(B2)

    # Quadrature information (nodes, weights).
    quadx, quadw = _quadrature_prod(Val(2k - 2))
    @assert length(quadx) == k  # we need k quadrature points per knot segment

    A = if S isa Hermitian
        deriv[1] === deriv[2] ||
            throw(ArgumentError("matrix will not be symmetric with deriv = $deriv"))
        B1 === B2 ||
            throw(ArgumentError("matrix will not be symmetric if bases are different"))
        fill_upper = S.uplo === 'U'
        fill_lower = S.uplo === 'L'
        parent(S)
    else
        fill_upper = true
        fill_lower = true
        S
    end

    fill!(S, 0)
    nlast = last(eachindex(ts))
    ioff = first(num_constraints(B1))
    joff = first(num_constraints(B2))

    # We loop over all knot segments Ω[n] = (ts[n], ts[n + 1]).
    # We integrate all supported B-spline products B1[i] * B2[j] over this
    # segment, adding the result to A[i, j].
    @inbounds for n in eachindex(ts)
        n == nlast && break
        tn, tn1 = ts[n], ts[n + 1]
        tn1 == tn && continue  # interval of length = 0

        metric = QuadratureMetric(tn, tn1)

        # Unnormalise quadrature nodes, such that xs ∈ [tn, tn1]
        xs = metric .* quadx
        # @assert all(x -> tn ≤ x ≤ tn1, xs)

        for (x, w) ∈ zip(xs, quadw)
            ilast = n - ioff
            jlast = n - joff
            _, bis = evaluate_all(B1, x, deriv[1], T; ileft = ilast)
            _, bjs = same_ij ?
                (ilast, bis) : evaluate_all(B2, x, deriv[2], T; ileft = jlast)
            y = metric.α * w
            for (δj, bj) ∈ pairs(bjs), (δi, bi) ∈ pairs(bis)
                i = ilast + 1 - δi
                j = jlast + 1 - δj
                if !fill_upper && i < j
                    continue
                elseif !fill_lower && i > j
                    continue
                elseif i ∉ axes(A, 1)  # this can be true for recombined bases
                    @assert iszero(bi)
                    continue
                elseif j ∉ axes(A, 2)
                    @assert iszero(bj)
                    continue
                end
                @inbounds A[i, j] += y * bi * bj
            end
        end
    end

    S
end


# Direct Search with Probabilistic Descent as described in Gratton2014:
#
#  S. Gratton, C. W. Royer, L. N. Vicente, and Z. Zhang, Direct search
#  based on probabilistic descent, preprint 14-11, Dept. Mathematics, Univ. Coimbra.
#  http://www.mat.uc.pt/~lnv/papers/ds-random.pdf
#

# Generate num random vectors on the n-dimensional, unit (hyper)sphere.
# This is the Muller-Marsaglia method as described on the page:
#   http://mathworld.wolfram.com/HyperspherePointPicking.html
function sample_unit_hypersphere(n, num = 1)
  X = randn(n, num)
  sqrootsums = 1 ./ sqrt(sum( X.^2, 1 ))
  broadcast(*, sqrootsums, X)
end

type RandomDirectionGen <: DirectionGenerator
  numDimensions::Int64
  numDirections::Int64
end

function directions_for_k(rdg::RandomDirectionGen, k)
  sample_unit_hypersphere(rdg.numDimensions, rdg.numDirections)
end

# A MirroredRandomDirectionGen generates half of the directions randomly and then
# mirrors by negating them.
type MirroredRandomDirectionGen <: DirectionGenerator
  numDimensions::Int64
  numDirections::Int64

  MirroredRandomDirectionGen(numDims, numDirections) = begin
    if numDirections % 2 == 1
      throw("the number of directions must be even")
    end
    new(numDims, numDirections)
  end
end

function directions_for_k(rdg::MirroredRandomDirectionGen, k)
  r = sample_unit_hypersphere(rdg.numDimensions, int(rdg.numDirections/2))
  [r -r]
end

DirectSearchProbabilisticDescentDefaultParameters = {
  :NumDirections => 2, # This should be a function of Gamma and Phi for the GSS but 2 is often enough
}

function direct_search_probabilistic_descent(parameters)
  params = Parameters(parameters, DirectSearchProbabilisticDescentDefaultParameters)
  params[:DirectionGenerator] = MirroredRandomDirectionGen(numdims(params[:Evaluator]), params[:NumDirections])
  GeneratingSetSearcher(params)
end


using ImageTracking
using Test

include("farneback.jl")
include("lucas_kanade.jl")
include("haar.jl")
include("visualization.jl")
include("error_evaluation.jl")


using LinearAlgebra
using SparseArrays

export expmv

"""
    expmv!(f, t, A, b; <keyword arguments>)

Returns the matrix-vector product ``exp(t A) b`` where `A` is a ``n × n`` sparse
real or complex matrix or matrix-free object, `b` is a vector of ``n`` real or
complex elements, and `t` is a time step parameter (or a `StepRangeLen` object
representing a range of values).

# Arguments
* `f`: an `n`-vector for preallocating the output
* `t`: `Number` or `StepRangeLen` object
* `A`: a `n × n` real or complex sparse matrix
* `b`: an `n`-vector
* `M = []`: manually set the degree of the Taylor expansion
* `precision = :double`: can be `:double`, `:single` or `:half`.
* `shift = $DEFAULT_SHIFT`: set to `true` to apply a shift in order to reduce the norm of A
        (see Sec. 3.1 of the paper)
* `full_term = false`: set to `true` to evaluate the full Taylor expansion instead
        of truncating when reaching the required precision
* `check_positive = false`: set to `true` to check if `A` has strictly positive entries
        for a faster evaluation of the 1-norm
"""
function expmv!(
        f::AbstractVecOrMat, t::Number, A, b::AbstractVecOrMat,
        M = nothing, norm = LinearAlgebra.norm, opnorm = LinearAlgebra.opnorm,
        b1 = similar(b), b2 = similar(b);
        precision = :double, shift = DEFAULT_SHIFT, full_term = false, check_positive = false
    )

    if shift == true && !hasmethod(tr, Tuple{typeof(A)})
        shift = false
        @warn "Shift set to false as $(typeof(A)) doesn't support tr"
    end

    n = size(A, 2)
    Tr = real(eltype(b))

    mu = zero(Tr)
    if shift
        mu = tr(A)/n
        A = diagshift!(A, mu) #A = A - mu * I
    end

    if M == nothing
        tt = 1
        (M,alpha,unA) = select_taylor_degree(t*A, size(b,2); precision=precision, shift=shift, check_positive=check_positive)
    else
        tt = t
    end

    tol =
      if precision == :double
          Tr(2.0^(-53))
      elseif precision == :single
          Tr(2.0^(-24))
      elseif precision == :half
          Tr(2.0^(-10))
      end

    s = 1

    if t == 0
        m = 0
    else
        (m_max,p) = size(M)
        U = diagm(0 => 1:m_max)
        C = ((ceil.(abs.(tt)*M))'*U )

        C[C .== 0] .= Inf

        cost, idx = findmin(C')
        # idx is a CarthesianIndex if C' is a Matrix, or a scalar if C' is a row
        # vector. idx[1] extract the first index, i.e. row, of the CarthesianIndex
        m = idx[1]

        if cost == Inf
            cost = 0
        end

        s = max(cost/m,1)
    end

    eta = 1

    if shift
        eta = exp(t*mu/s)
    end

    f = copyto!(f, b)
    b1 = copyto!(b1, b)
    b2 = copyto!(b2, b)
    c1 = c2 = Tr(Inf)

    @inbounds for i = 1:s
        if !full_term
            c1 = norm(b1, Inf) # only need to update if !full_term
        end

        for k = 1:m
            b2 = mul!(b2, A, b1)
            b2 .*= (t / (s * k)) # b = (t/(s*k))*(A*b)
            b1, b2 = b2, b1
            f .+= b1 # f = axpy!(1, b1, f)

            if !full_term
                c2 = norm(b1, Inf) # only need to update if !full_term
                if c1 + c2 <= tol * norm(f, Inf)
                    break
                end
                c1 = c2
            end

        end

        if shift
            f .*= eta
        end

        b1 = copyto!(b1, f)
    end

    return f
end

function expmv(
        t::Number, A, b::AbstractVecOrMat,
        M = nothing, norm = LinearAlgebra.norm, opnorm = LinearAlgebra.opnorm,
        b1 = copy(b), b2 = similar(b);
        precision = :double, shift = DEFAULT_SHIFT, full_term = false, check_positive = false
    )

    return expmv!(
        similar(b), t, A, b, M, norm, opnorm, b1, b2;
        precision = precision, shift = shift, full_term = full_term, check_positive = check_positive
    )
end


export project_cardinality_parallel

function project_cardinality_parallel(x::Vector,
                             k ::Int64,
                             n_proc::Int64
                             )
"""
Project the vector x onto the set of vectors with cardinality (l0 'norm') less then or equal to k.
project m onto {m | card(m)<= k} : x = argmin_x 1/2 ||x-m||2^2 s.t. card(x)<=k
A parallel version
"""

#sort_ind = sortperm( a, by=abs, rev=true)
#val_max=x[sort_ind[1:k]]
#x=zeros(length(x))
#x[sort_ind[1:k]]=val_max

ns = convert(Int64,floor((length(x)/n_proc)))
sort_ind=Vector{Vector{Int64}}(n_proc)
sort_ind[1] = sortperm( x[1:ns], by=abs, rev=true)
sort_ind[2] = sortperm( x[ns+1:end], by=abs, rev=true)

val_max=Vector{Float64}(2*k)
val_max[1:k]=x[sort_ind[1][1:k]]
val_max[1+k:k*2]=x[sort_ind[2][1:k]]

sort_final_ind = sortperm(val_max,by=abs,rev=true  )
#sort_ind[1]=sort_ind[1][1:k]
#sort_ind[2]=sort_ind[2][1:k]
#sort_ind_total=[sort_ind[1];sort_ind[2]]
x[sort_ind[1][k+1:end]]=0.0;
x[sort_ind[2][k+1:end]+ns]=0;.0

for i=(k+1):length(sort_final_ind)
  print(i)
  if sort_final_ind[i]<=k
    x[sort_ind[1][sort_final_ind[i]]]=0.0
  else
    x[sort_ind[2][sort_final_ind[i]-k]]=0.0
  end
end

  return x
end


function test_softmax_layer(backend::Backend, tensor_dim, n_input, T, eps)
  println("-- Testing SoftmaxLayer on $(typeof(backend)){$T}...")

  norm_dim = max(1, abs(rand(Int)) % tensor_dim)
  println("    > $tensor_dim-dimensional input, normalize along dimension $norm_dim")

  dims = [abs(rand(Int,tensor_dim)) % 6 + 6 for i = 1:n_input]
  input = [rand(T, dims[i]...) for i = 1:n_input]
  input_blob = Blob[make_blob(backend, x) for x in input]
  diff_blob = Blob[make_blob(backend, x) for x in input]

  layer = SoftmaxLayer(tops=Array(Symbol,n_input), bottoms=Array(Symbol,n_input),
      dim=norm_dim-tensor_dim-1)
  state = setup(backend, layer, input_blob, diff_blob)

  println("    > Forward")
  forward(backend, state, input_blob)

  for i = 1:n_input
    my_dims  = size(input[i])
    dim_pre  = prod(my_dims[1:norm_dim-1])
    dim_prob = my_dims[norm_dim]
    dim_post = prod(my_dims[norm_dim+1:end])

    canonical_input = reshape(input[i], (dim_pre, dim_prob, dim_post))
    output = similar(canonical_input)

    for x = 1:dim_pre
      for y = 1:dim_post
        preds = canonical_input[x,:,y]
        preds -= maximum(preds)
        preds = exp(preds)
        preds /= sum(preds)
        output[x,:,y] = preds
      end
    end
    output = reshape(output, my_dims)

    got_output = zeros(T, size(output))
    copy!(got_output, state.blobs[i])

    @test all(-eps .< output[:] - got_output[:] .< eps)
  end

  println("    > Backward")
  for i = 1:n_input
    copy!(state.blobs_diff[i], rand(T, size(input[i])))
  end
  backward(backend, state, input_blob, diff_blob)

  for i = 1:n_input
    my_dims  = size(input[i])
    dim_pre  = prod(my_dims[1:norm_dim-1])
    dim_prob = my_dims[norm_dim]
    dim_post = prod(my_dims[norm_dim+1:end])

    canonical_output = reshape(to_array(state.blobs[i]), (dim_pre, dim_prob, dim_post))
    canonical_topdiff = reshape(to_array(state.blobs_diff[i]), (dim_pre, dim_prob, dim_post))
    grad = similar(canonical_output)

    for x = 1:dim_pre
      for y = 1:dim_post
        topdiff0 = canonical_topdiff[x,:,y]
        output0 = canonical_output[x,:,y]
        grad[x,:,y] = topdiff0.*output0 - dot(vec(topdiff0), vec(output0))*output0
      end
    end

    got_grad = to_array(diff_blob[i])
    @test all(-eps .< vec(got_grad)-vec(grad) .< eps)
  end

  shutdown(backend, state)
end
function test_softmax_layer(backend::Backend, n_input, T, eps)
  for td in [2,4,5]
    test_softmax_layer(backend, td, n_input, T, eps)
  end
end
function test_softmax_layer(backend::Backend)
  test_softmax_layer(backend, 3, Float64, 1e-10)
  test_softmax_layer(backend, 3, Float32, 1e-5)
end

if test_gpu
  test_softmax_layer(backend_gpu)
end
if test_cpu
  test_softmax_layer(backend_cpu)
end


using OrdinaryDiffEq, Test

jac_called = Ref(false)
tgrad_called = Ref(false)

function Lotka(du,u,p,t)
  du[1] = u[1] - u[1] * u[2] # REPL[7], line 3:
  du[2] = -3 * u[2] + 1 * u[1] * u[2]
  nothing
end

function Lotka_jac(J,u,p,t)
  jac_called.x = true
  J[1,1] = 1.0 - u[2]
  J[1,2] = -u[1]
  J[2,1] = 1 * u[2]
  J[2,2] = -3 + u[1]
  nothing
end

function Lotka_tgrad(grad,u,p,t)
  tgrad_called.x = true
  grad[1] = 1 * 0
  grad[2] = 1 * 0
  nothing
end

Lotka_f = ODEFunction(Lotka,jac=Lotka_jac,tgrad=Lotka_tgrad)
prob = ODEProblem(Lotka_f,ones(2),(0.0,10.0))

good_sol = solve(prob,Rosenbrock23())

@test jac_called[]
@test tgrad_called[]

prob2 = ODEProblem(Lotka,ones(2),(0.0,10.0))

sol = solve(prob2,Rosenbrock23(autodiff=true))
@test ≈(good_sol[end],sol[end],rtol=1e-2)

sol = solve(prob2,Rosenbrock23(autodiff=true,chunk_size=1))
@test ≈(good_sol[end],sol[end],rtol=1e-2)

sol = solve(prob2,Rosenbrock23(autodiff=false))
@test ≈(good_sol[end],sol[end],rtol=1e-2)

invW_called = Ref(false)

function Lotka_invW(iW,u,p,γ,t)
  invW_called.x = true
  iW[1,1] = (1 - (1 * u[1] * u[2] * γ ^ 2) / (((1 - (-3 + u[1]) * γ) + (1 * u[1] * u[2] * γ ^ 2) / (1 - (1 - 1 * u[2]) * γ)) * (1 - (1 - 1 * u[2]) * γ))) / (1 - (1 - 1 * u[2]) * γ)
  iW[1,2] = (-(1) * u[1] * γ) / (((1 - (-3 + u[1]) * γ) + (1 * u[1] * u[2] * γ ^ 2) / (1 - (1 - 1 * u[2]) * γ)) * (1 - (1 - 1 * u[2]) * γ))
  iW[2,1] = (u[2] * γ) / (((1 - (-3 + u[1]) * γ) + (1 * u[1] * u[2] * γ ^ 2) / (1 - (1 - 1 * u[2]) * γ)) * (1 - (1 - 1 * u[2]) * γ))
  iW[2,2] = ((1 - (-3 + u[1]) * γ) + (1 * u[1] * u[2] * γ ^ 2) / (1 - (1 - 1 * u[2]) * γ)) ^ -1
  nothing
end

Lotka_f2 = ODEFunction(Lotka,jac=Lotka_jac,tgrad=Lotka_tgrad,invW = Lotka_invW)
prob3 = ODEProblem(Lotka_f2,ones(2),(0.0,10.0))
inv_sol = solve(prob3,Rosenbrock23())

@test invW_called[]
@test ≈(good_sol[end],inv_sol[end],rtol=1e-2)


#' ---
#' title: Benders Decomposition
#' ---

#' **Originally Contributed by**: Mathieu Besançon

#' This notebook describes how to implement the Benders decomposition in JuMP using lazy constraints.
#' We keep the same notation and problem form as the first notebook *Benders decomposition*.

#' \begin{align*}
#' & \text{maximize} \quad &&c_1^T x+c_2^T v \\
#' & \text{subject to} \quad &&A_1 x+ A_2 v \preceq b \\
#' & &&x \succeq 0, x \in \mathbb{Z}^n \\
#' & &&v \succeq 0, v \in \mathbb{R}^p \\
#' \end{align*}

#' where $b \in \mathbb{R}^m$, $A_1 \in \mathbb{R}^{m \times n}$, $A_2 \in \mathbb{R}^{m \times p}$ and
#' \mathbb{Z} is the set of integers.
#' Here the symbol $\succeq$ ($\preceq$) stands for element-wise greater (less) than or equal to.
#' Any mixed integer programming problem can be written in the form above.

#' For a detailed explanation on the Benders decomposition algorithm, see the introduction notebook.

#' ### Lazy constraints

#' Some optimization solvers allow users to interact with them during the solution process
#' by providing user-defined functions which are triggered under certain conditions.
#' The generic term for these functions is **callback**.
#' In integer optimization, the main callback types are lazy constraints, user cuts and heuristic solutions.
#' See the [JuMP documentation](https://www.juliaopt.org/JuMP.jl/stable/callbacks/) for an introduction on how to use them.
#' Some callbacks define a new constraint which is only activated when necessary, i.e.
#' when a current solution does not respect them. It can avoid building an optimization model
#' with too many constraints up-front.
#' This is the case for Benders decomposition, since the sub-problem defines an exponential number of primal vertices
#' and therefore dual cuts.
#'
#' A detailed explanation on the distinction between user cuts and lazy constraints is also available on
#' [Paul Rubin's blog](https://orinanobworld.blogspot.com/2012/08/user-cuts-versus-lazy-constraints.html).

#' We use the data from the original notebook and change the solution algorithm to leverage lazy constraints:

#' **Step 1 (Initialization)**
#' **Step 2 (defining the subproblem model)**
#' **Step 3 (registering the lazy constraint of the subproblem)**

#' ### Data for the problem

#' The input data is from page 139, Integer programming by Garfinkel and Nemhauser[[1]](#c1).

c1 = [-1; -4]
c2 = [-2; -3]

dim_x = length(c1)
dim_u = length(c2)

b = [-2; -3]

A1 = [1 -3;
     -1 -3]
A2 = [1 -2;
     -1 -1]

M = 1000;

#' [Paul Rubin's blog on Benders Decomposition](http://orinanobworld.blogspot.ca/2011/10/benders-decomposition-then-and-now.html).

# Loading the necessary packages
#-------------------------------
using JuMP
using GLPK
using LinearAlgebra
using Test

#' Subproblem creation

function build_subproblem()
    sub_problem_model = Model(GLPK.Optimizer)
    @variable(sub_problem_model, u[1:dim_u] >= 0)
    @constraint(sub_problem_model, constr_ref_subproblem[j = 1:size(A2, 2)], dot(A2[:, j], u) >= c2[j])
    return (sub_problem_model, u)
end

# Master Problem Description
# --------------------------

master_problem_model = Model(GLPK.Optimizer)
(sub_problem_model, u) = build_subproblem()

# Variable Definition
# ----------------------------------------------------------------
@variable(master_problem_model, 0 <= x[1:dim_x] <= 1e6, Int)
@variable(master_problem_model, t <= 1e6)

# Objective Setting
# -----------------
@objective(master_problem_model, Max, t)

print(master_problem_model)

#' Track the calls to the callback

iter_num = 0

#' Define lazy constraints

function benders_lazy_constraint_callback(cb_data)
    global iter_num
    iter_num += 1
    println("Iteration number = ", iter_num)

    x_current = [callback_value(cb_data, x[j]) for j in eachindex(x)]
    fm_current = callback_value(cb_data, t)

    c_sub = b - A1 * x_current
    @objective(sub_problem_model, Min, dot(c1, x_current) + dot(c_sub, u))
    optimize!(sub_problem_model)

    print("\nThe current subproblem model is \n", sub_problem_model)

    t_status_sub = termination_status(sub_problem_model)
    p_status_sub = primal_status(sub_problem_model)

    fs_x_current = objective_value(sub_problem_model)

    u_current = value.(u)

    γ = dot(b, u_current)

    if p_status_sub == MOI.FEASIBLE_POINT && fs_x_current  ≈  fm_current # we are done
        @info("No additional constraint from the subproblem")
        # println("Optimal solution of the original problem found")
        # println("The optimal objective value t is ", fm_current)
        # println("The optimal x is ", x_current)
        # println("The optimal v is ", dual.(constr_ref_subproblem))
    end

    if p_status_sub == MOI.FEASIBLE_POINT && fs_x_current < fm_current
        println("\nThere is a suboptimal vertex, add the corresponding constraint")
        cv = A1' * u_current - c1
        new_optimality_cons = @build_constraint(t + dot(cv, x) <= γ)
        MOI.submit(master_problem_model, MOI.LazyConstraint(cb_data), new_optimality_cons)
    end

    if t_status_sub == MOI.INFEASIBLE_OR_UNBOUNDED
        println("\nThere is an  extreme ray, adding the corresponding constraint")
        ce = A1' * u_current
        new_feasibility_cons = @build_constraint(dot(ce, x) <= γ)
        MOI.submit(master_problem_model, MOI.LazyConstraint(cb_data), new_feasibility_cons)
    end
end

MOI.set(master_problem_model, MOI.LazyConstraintCallback(), benders_lazy_constraint_callback)

optimize!(master_problem_model)

t_status = termination_status(master_problem_model)
p_status = primal_status(master_problem_model)

if p_status == MOI.INFEASIBLE_POINT
    println("The problem is infeasible :-(")
end

if t_status == MOI.INFEASIBLE_OR_UNBOUNDED
    fm_current = M
    x_current = M * ones(dim_x)
end

if p_status == MOI.FEASIBLE_POINT
    fm_current = value(t)
    x_current = Float64[]
        for i in 1:dim_x
        push!(x_current, value(x[i]))
    end
end

println("Status of the master problem is ", t_status,
        "\nwith fm_current = ", fm_current,
        "\nx_current = ", x_current)

@test value(t) ≈ -4

#' ### References
#'
#' 1. Garfinkel, R. & Nemhauser, G. L. Integer programming. (Wiley, 1972).


"""
TopDistance(args...)

Estimate the distance between points given. Plot either distance between each point or distance with respect to specific point

``D=\\sqrt { { \\left( { X }_{ 1 }-{ X }_{ 2 } \\right)  }^{ 2 }+{ \\left( { Y }_{ 1 }-{ Y }_{ 2 } \\right)  }^{ 2 }{ +\\left( { Z }_{ 1 }-{ Z }_{ 2 } \\right)  }^{ 2 } } ``

The next table show the list of variables allowed:

|PropertyName|Args|Default|Input|Description
|---|---|---|---|---|
|x|Mandatory|--|Array{number,1}|X coordinate of points|
|y|Mandatory|--|Array{number,1}|Y coordinate of points|
|z|Optional|z=false|Array{number,1}|z coordinate of points. If not set, a zeros array is established|
|Names|Optional|Names=false|Array{String,1}|Name of each point. If not set a numbered list is created as a name|
|Show|Optional|Show=false|Show=Number|Refers to a numbered point which distance between other points and this are shown <br> if not set, all distances are shown|
"""
@userplot topdistance

@recipe function f(h::topdistance;
            Names=false,Show=false)

    if length(h.args)==2
        x, y = h.args
    elseif length(h.args)==3
        x, y, z = h.args
    end

    n=size(x,1)  #Number of inputs
    list=1:n    #Numbered list of elements

    if length(h.args)==2      #If height not provided, zero vector is created
        z=zeros(n)
    end

    if Names==false  #If names are not provided, it is created a numbered list
        Names=1:n
    end

        WellSym=map(x->Symbol(x),Names)     #DataFrame COlumns Name
        Distance=DataFrame()

        for i=1:n
            Distance[WellSym[i]]=zeros(n)
            for j=1:n
              Distance[WellSym[i]][j]=sqrt( (x[i]-x[j])^2  + (y[i]-y[j])^2 + (z[i]-z[j])^2)
            end
        end

    if Show==false                         # If Show==false plot each distance of each well, otherwise plot distance Well choosed
        m=binomial(n,2)
        AvrD=DataFrame(X=zeros(m),Y=zeros(m),Dist=zeros(m))
        k=0
           for i=1:n-1
               for j=i+1:n
                k=k+1
                AvrD.X[k]=mean([x[i],x[j]])
                AvrD.Y[k]=mean([y[i],y[j]])
                AvrD.Dist[k]=round(Distance[i,j],digits=2)
                end
            end

    else
        m=n-1
        AvrD=DataFrame(X=zeros(m),Y=zeros(m),Dist=zeros(m))
        c=list[list.!=Show]
        k=0

        for i in c
            k=k+1
            AvrD.X[k]=mean([x[Show],x[i]])
            AvrD.Y[k]=mean([y[Show],y[i]])
            AvrD.Dist[k]=round(Distance[Show,i],digits=2)
        end
    end

# Plot the Distances

@series begin
    seriestype := :scatter
    seriescolor := :black
    series_annotations := map(x->Plots.text(x,:left,:top,8),Names)
    x, y
end
#
@series begin
        seriestype := :scatter
        seriescolor := :white
        series_annotations := map(x->Plots.text("$x m" ,:right,:top,8),AvrD.Dist)
        AvrD.X,AvrD.Y
end

    ## Plot properties for union lines
    seriestype := :path
    linestyle := :dot
    arrow := Plots.arrow(:open, :both )
    seriescolor := :black
    legend --> false

if Show==false  # If Show==false plot each distance of each well, otherwise plot distance Well choosed
    for i=1:n-1
        for j=i+1:n

            @series begin
                        x[[i,j]],y[[i,j]]
                    end
        end
    end
else
    for i in c
                     @series begin
                        x[[Show,i]],y[[Show,i]]
                    end
    end

end

end