module Loess

include("loess1d.jl")

mutable struct MultivariateLoessModel{T <: AbstractFloat}

    xs::AbstractMatrix{T}       # An n by m predictor matrix containing n observations from m predictors
    ys::AbstractMatrix{T}       # A length n response vector
    bs::Matrix{T}               # Least squares coefficients
    verts::Dict{Vector{T}, Int} # kd-tree vertexes mapped to indexes
    kdtree::KDTree{T}

end

"""
    loess(xs, ys, normalize=true, span=0.75, degreee=2)

Fit a loess model.

Args:
  `xs`: A `n` by `m` matrix with `n` observations from `m` independent predictors
  `ys`: A length `n` response vector.
  `normalize`: Normalize the scale of each predicitor. (default true when `m > 1`)
  `span`: The degree of smoothing, typically in [0,1]. Smaller values result in smaller
      local context in fitting.
  `degree`: Polynomial degree.

Returns:
  A fit `LoessModel`.

"""
function loess(xs::AbstractMatrix{T}, ys::AbstractMatrix{T};
               normalize::Bool=true,
               span::AbstractFloat=0.75,
               degree::Integer=2) where T<:AbstractFloat

    if size(xs, 1) != size(ys, 1)
        throw(DimensionMismatch("Predictor and response arrays must of the same length"))
    end

    n, m = size(xs)
    q = ceil(Int, (span * n))

    # TODO: We need to keep track of how we are normalizing so we can
    # correctly apply predict to unnormalized data. We should have a normalize
    # function that just returns a vector of scaling factors.
    if normalize && m > 1
        xs = tnormalize!(copy(xs))
    end

    kdtree = KDTree(xs, 0.05 * span)
    verts = Array{T}(undef, length(kdtree.verts), m)

    # map verticies to their index in the bs coefficient matrix
    verts = Dict{Vector{T}, Int}()
    for (k, vert) in enumerate(kdtree.verts)
        verts[vert] = k
    end

    # Fit each vertex
    ds = Array{T}(undef, n) # distances
    perm = collect(1:n)
    bs = Array{T}(undef, length(kdtree.verts), 1 + degree * m)

    # TODO: higher degree fitting
    us = Array{T}(undef, q, 1 + degree * m)
    vs = Array{T}(undef, q)
    for (vert, k) in verts
        # reset perm
        for i in 1:n
            perm[i] = i
        end

        # distance to each point
        for i in 1:n
            ds[i] = euclidean(vec(vert), vec(xs[i,:]))
        end

        # copy the q nearest points to vert into X
        partialsort!(perm, q, by=i -> ds[i])
        dmax = maximum([ds[perm[i]] for i = 1:q])

        for i in 1:q
            pi = perm[i]
            w = tricubic(ds[pi] / dmax)
            us[i,1] = w
            for j in 1:m
                x = xs[pi, j]
                wxl = w
                for l in 1:degree
                    wxl *= x
                    us[i, 1 + (j-1)*degree + l] = wxl # w*x^l
                end
            end
            vs[i] = ys[pi] * w
        end
        bs[k,:] = us \ vs
    end

    MultivariateLoessModel{T}(xs, ys, bs, verts, kdtree)
end

loess(xs::AbstractVector{T}, ys::AbstractMatrix{T}; kwargs...) where {T<:AbstractFloat} =
    loess(reshape(xs, (length(xs), 1)), ys; kwargs...)

# Predict response values from a trained loess model and predictor observations.
#
# Loess (or at least most implementations, including this one), does not perform,
# extrapolation, so none of the predictor observations can exceed the range of
# values in the data used for training.
#
# Args:
#   zs/z: An n' by m matrix of n' observations from m predictors. Where m is the
#       same as the data used in training.
#
# Returns:
#   A length n' vector of predicted response values.
#
function predict(model::MultivariateLoessModel{T}, z::T) where T <: AbstractFloat
    predict(model, T[z])
end

function predict(model::MultivariateLoessModel{T}, zs::AbstractVector{T}) where T <: AbstractFloat
    m = size(model.xs, 2)

    # in the univariate case, interpret a non-singleton zs as vector of
    # ponits, not one point
    if m == 1 && length(zs) > 1
        return predict(model, reshape(zs, (length(zs), 1)))
    end

    if length(zs) != m
        error("$(m)-dimensional model applied to length $(length(zs)) vector")
    end

    adjacent_verts = traverse(model.kdtree, zs)

    if m == 1
        @assert(length(adjacent_verts) == 2)
        z = zs[1]
        u = (z - adjacent_verts[1][1]) /
        (adjacent_verts[2][1] - adjacent_verts[1][1])

        y1 = evalpoly(zs, model.bs[model.verts[[adjacent_verts[1][1]]],:])
        y2 = evalpoly(zs, model.bs[model.verts[[adjacent_verts[2][1]]],:])
        return (1.0 - u) * y1 + u * y2
    else
        error("Multivariate blending not yet implemented")
        # TODO:
        #   1. Univariate linear interpolation between adjacent verticies.
        #   2. Blend these estimates. (I'm not sure how this is done.)
    end
end

function predict(model::MultivariateLoessModel{T}, zs::AbstractMatrix{T}) where T <: AbstractFloat
    ys = Array{T}(undef, size(zs, 1))
    for i in 1:size(zs, 1)
        # the vec() here is not necessary on 0.5 anymore
        ys[i] = predict(model, vec(zs[i,:]))
    end
    ys
end

end


# import Unreal: unreal

"""
    Eisenstein{T <: Real} <: Number

An immutable pair of real numbers describing an Eisenstein number.
"""
struct Eisenstein{T <: Real} <: Number
    l::T
    r::T

    Eisenstein{T}(l::T, r::T) where {T <: Real} = new(l, r)
end

const Eisenstein16 = Eisenstein{Int8}
const Eisenstein32 = Eisenstein{Int16}
const Eisenstein64 = Eisenstein{Int32}
const Eisenstein128 = Eisenstein{Int64}
const Eisenstein256 = Eisenstein{Int128}
const EisensteinBigInt = Eisenstein{BigInt}

# Constructors
function Eisenstein(a::T, b::T) where T <: Real
    Eisenstein{T}(a, b)
end

function Eisenstein(a::T) where T <: Real
    Eisenstein{T}(a, zero(T))
end

function Eisenstein(a::Real, b::Real)
    Eisenstein(promote(a, b)...)
end

# Eisenstein unit
const ω = Eisenstein(false, true)

# Real part
function real(z::Eisenstein)
    z.l - (z.r / 2)
end

# Imaginary part
function imag(z::Eisenstein)
    z.r * halfsqrt3
end

function Complex(z::Eisenstein)
    Complex(real(z), imag(z))
end

function complex(z::Eisenstein)
    Complex(z)
end

# Unreal part
function unreal(z::Eisenstein)
    z.r
end

function isreal(z::Eisenstein)
    iszero(z.r)
end

function asarray(z::Eisenstein)
    vcat(z.l, z.r)
end

function iszero(z::Eisenstein)
    iszero(asarray(z))
end

function zero(::Type{Eisenstein{T}}) where T <: Real
    Eisenstein{T}(zero(T), zero(T))
end

function zero(::Eisenstein{T}) where T <: Real
    zero(Eisenstein{T})
end

function one(::Type{Eisenstein{T}}) where T <: Real
    Eisenstein{T}(one(T), zero(T))
end

function one(::Eisenstein{T}) where T <: Real
    one(Eisenstein{T})
end

function omega(::Type{Eisenstein{T}}) where T <: Real
    Eisenstein{T}(zero(T), one(T))
end

function omega(::Eisenstein{T}) where T <: Real
    omega(Eisenstein{T})
end

function show(io::IO, z::Eisenstein)
    print(io, "[1: ")
    print(io, z.l)
    print(io, ", ω: ")
    print(io, z.r)
    print(io, "]")
end

function conj(z::Eisenstein)
    Eisenstein(z.l - z.r, -(z.r))
end

function (+)(z::Eisenstein)
    z
end

function (+)(x::Eisenstein, y::Eisenstein)
    Eisenstein(x.l + y.l, x.r + y.r)
end

function (+)(a::Real, z::Eisenstein)
    Eisenstein(a + z.l, z.r)
end

function (+)(z::Eisenstein, a::Real)
    Eisenstein(z.l + a, z.r)
end

function (-)(z::Eisenstein)
    Eisenstein(-(z.l), -(z.r))
end

function (-)(x::Eisenstein, y::Eisenstein)
    Eisenstein(x.l - y.l, x.r - y.r)
end

function (-)(a::Real, z::Eisenstein)
    Eisenstein(a - z.l, -(z.r))
end

function (-)(z::Eisenstein, a::Real)
    Eisenstein(z.l - a, z.r)
end

function (*)(x::Eisenstein, y::Eisenstein)
    Eisenstein(
        (x.l * y.l) - (x.r * y.r),
        (x.l * y.r) + (x.r * y.l) - (x.r * y.r),
    )
end

function (*)(a::Real, z::Eisenstein)
    Eisenstein(a * z.l, a * z.r)
end

function (*)(z::Eisenstein, a::Real)
    Eisenstein(z.l * a, z.r * a)
end

function abs2(z::Eisenstein)
    (z.l)^(2) + (z.r)^(2) - (z.l * z.r)
end

function inv(z::Eisenstein)
    if iszero(z)
        error(ZeroInverse)
    end

    conj(z) / abs2(z)
end

function (/)(x::Eisenstein, y::Eisenstein)
    x * inv(y)
end

function (\)(y::Eisenstein, x::Eisenstein)
    inv(y) * x
end

function (/)(a::Real, z::Eisenstein)
    a * inv(z)
end

function (\)(z::Eisenstein, a::Real)
    inv(z) * a
end

function (/)(z::Eisenstein, a::Real)
    if iszero(a)
        error(ZeroDenominator)
    end

    Eisenstein(z.l / a, z.r / a)
end

function (\)(a::Real, z::Eisenstein)
    if iszero(a)
        error(ZeroDenominator)
    end

    Eisenstein(a \ z.l, a \ z.r)
end

function associates(z::Eisenstein{T}) where T <: Real
    a = z
    b = -(z)
    u = omega(Eisenstein{T})
    c = z * u
    u = -(u)
    d = z * u
    u = u * u
    f = z * u
    u = -(u)
    g = z * u
    a, b, c, d, f, g
end

function random(::Type{Eisenstein{T}}) where T <: Real
    Eisenstein{T}(rand(T), rand(T))
end


#!/usr/bin/env julia --startup-file=no

"""
Linear-Stability-Calculators: Quasi-Hydrostatic Model
=====================================================

Linear Stability Calculation of an inertially unstable jet using spectral methods

The basic state is that of a barotropic Bickley jet (to be generalized).
The perturbation is assumed to be periodic in both the zonal (x) and vertical (z) directions.

Calculations are done in parallel.  See inertial_instability.py for serial execution.
"""

# put this somewhere else?
"""
    split_count(N::Integer, n::Integer)
Return a vector of `n` integers which are approximately equally sized and sum to `N`.
"""
function split_count(N::Integer, n::Integer)
    q,r = divrem(N, n)
    return [i <= r ? q+1 : q for i = 1:n]
end

using MPI
using Printf

MPI.Init()

comm = MPI.COMM_WORLD
rank = MPI.Comm_rank(comm)
comm_size = MPI.Comm_size(comm)

include("src/quasihydrostatic_setup.jl")
include("src/parameters.jl")
include("src/mpi_stuff.jl")
include("src/Compute_Eigenvalues.jl")
include("src/Plotting_Functions.jl")

files   = Files()
grid    = Grid(Ly = 1000e3, Lz = 3e3, Ny = 200, θ₀ = π/32, method=Cheb())
physics = Physics(N = 1e-4, ν = 0.26, θ₀ = grid.θ₀, NT = 1)
jet     = Jet(grid, physics)

rank == 0 ? Output_Parameters(grid, physics, jet, files.json) : nothing

Neigs  = 10
dk, Nk = 1e-6, 41
dm, Nm = 1e-4, 150

ms = collect(dm:dm:Nm*dm)

if rank == 0
    ks = Float64[(ik-1)*dk for ik in 1:Nk]
    Nk_counts = split_count(Nk, comm_size)
    ks_vbuf   = VBuffer(ks, Nk_counts)        # VBuffer for scatter

else
    Nk_counts = nothing
    ks_vbuf   = nothing
end

Nk_local, = MPI.Scatter!(Nk_counts, zeros(Int, 1), 0, comm)
ks_local = MPI.Scatterv!(ks_vbuf, zeros(Float64, Nk_local), 0, comm)

#@printf("rank = %s, Nk_local = %s, ks_local = %s\n", rank, Nk_local, ks_local)

ωs_local    = zeros(Complex{Float64}, (Neigs, Nm, length(ks_local)))
modes_local = zeros(Complex{Float64}, (3*grid.Ny+1, Neigs, Nm, length(ks_local)))

for (ik, k) in enumerate(ks_local)
    for (im, m) in enumerate(ms)
        A = build_matrix(k, m, grid, physics, jet)

        ωs_local[:, im, ik], modes_local[:, :, im, ik] = compute_spectrum(A, Neigs)

        #print("k = ", ks_local[ik], " m = ", ms[im], " growth = ", imag(ωs_local[1, im, ik]), "\n")
    end
end

MPI.Barrier(comm)

if rank == 0
    ωs    = zeros(Complex{Float64}, (Neigs, Nm, Nk))
    modes = zeros(Complex{Float64}, (3*grid.Ny+1, Neigs, Nm, Nk))

    Neigs_counts = [Neigs for k = 1:comm_size]
    Nm_counts    = [Nm    for j = 1:comm_size]

    ωs_sizes  = vcat(Neigs_counts', Nm_counts', Nk_counts')
    ωs_counts = vec(prod(ωs_sizes, dims=1))
    ωs_vbuf   = VBuffer(ωs, ωs_counts)

    Ny_counts = [3*grid.Ny+1 for k = 1:comm_size]

    modes_sizes  = vcat(Ny_counts', Neigs_counts', Nm_counts', Nk_counts')
    modes_counts = vec(prod(modes_sizes, dims=1))
    modes_vbuf   = VBuffer(modes, modes_counts)

else
    ωs_sizes = nothing
    ωs_vbuf  = nothing

    modes_sizes = nothing
    modes_vbuf  = nothing
end

MPI.Gatherv!(ωs_local,    ωs_vbuf,    0, comm)
MPI.Gatherv!(modes_local, modes_vbuf, 0, comm)

MPI.Barrier(comm)

if rank == 0
    save_spectrum(ωs, modes, ks, ms, Neigs, grid.y, grid.Ny, files.nc)

    plot_growth_slice(files.nc, files.json, files.plotslicem)
    plot_growth(files.nc, files.json, files.plotgrowth)

    plot_modes_1D(files.nc, files.json, files.plotmodes1D, Neigs)
    plot_modes_2D(files.nc, files.json, files.plotmodes2D, Neigs)
end

MPI.Finalize()


loss1 = RobustModels.L2Loss()
loss2 = RobustModels.TukeyLoss()

@testset "linear: Expectile estimators" begin
    m  = fit(RobustLinearModel, form, data, MEstimator(loss1); method=:cg, maxiter=50, verbose=false, initial_scale=:mad)
    m1 = fit(RobustLinearModel, form, data, GeneralizedQuantileEstimator(loss1, 0.5); method=:cg, maxiter=50, verbose=false, initial_scale=:mad)

    # refit
    # cannot change type to GeneralQuantileEstimator
    @test_throws TypeError refit!(m; quantile=0.5)

    @testset "$(τ) $(name)" for τ in range(0.1, 0.9, step=0.1), name in ("Expectile", "Quantile")
        println("\n\t\u25CF Estimator: $name($τ)")
        est = getproperty(RobustModels, Symbol(name * "Estimator"))(τ)
        m2 = fit(RobustLinearModel, form, data, est; method=:cg, maxiter=50, rtol=1e-4, initial_scale=:mad)
        m3 = fit(RobustLinearModel, form, data, est; method=:chol, maxiter=50, rtol=1e-4, initial_scale=:mad)
        println("rlm(cg)  : ", coef(m2))
        println("rlm(chol): ", coef(m3))
        println(m2)
        if name == "Expectile"
            @test isapprox(coef(m2), coef(m3); rtol=1e-4)
            if τ==0.5
                @test isapprox(coef(m), coef(m2); rtol=1e-4)
            end

            # refit with new quantile
            refit!(m1; quantile=τ)
            @test isapprox(coef(m1), coef(m2); rtol=1e-4)
        else
            @test isapprox(coef(m2), coef(m3); rtol=1e-2)
        end
    end
end

@testset "linear: M-Quantile estimators" begin
    @testset "$(name) estimator" for name in ("Huber", "L1L2", "Fair", "Logcosh", "Arctan") #, "Cauchy", "Geman", "Welsch", "Tukey", "YohaiZamar")
        println("\n\t\u25CF M-Quantile Estimator: $name")
        loss = getproperty(RobustModels, Symbol(name * "Loss"))()
        est = MEstimator(loss)
        m1 = fit(RobustLinearModel, form, data, est; method=:cg, maxiter=50, verbose=false, initial_scale=:mad)
        println("no quant, rlm(cg)  : ", coef(m1))
        est1 = GeneralizedQuantileEstimator(loss)
        @testset "$(τ) quantile" for τ in range(0.1, 0.9, step=0.1)
            est = GeneralizedQuantileEstimator(loss, τ)
            m2 = fit(RobustLinearModel, form, data, est; method=:cg, maxiter=50, verbose=false, initial_scale=:mad)
            m3 = fit(RobustLinearModel, form, data, est; method=:chol, maxiter=50, initial_scale=:mad)
            m4 = fit(RobustLinearModel, form, data, est1; quantile=τ, method=:chol, maxiter=50, initial_scale=:mad)
            println("quant $(τ) rlm(cg)   : ", coef(m2))
            println("quant $(τ) rlm(chol) : ", coef(m3))
            println("quant $(τ) rlm2(chol): ", coef(m4))
            if τ==0.5
                @test isapprox(coef(m1), coef(m2); rtol=1e-5)
            end
            if name != "L1"
                @test isapprox(coef(m2), coef(m3); rtol=1e-2)
            end
            @test isapprox(coef(m3), coef(m4); rtol=1e-5)
        end
    end
end


using Test
using IncrementalInference

@testset "test multihypo chain example (see #462)..." begin

l1 = -10.0
l2 = +10.0
lnoise = 1.0
x1 = 0
x2 = 0
Onoise = 0.1

fg = initfg()

addVariable!(fg, :x1, ContinuousScalar)
addVariable!(fg, :x2, ContinuousScalar)
addVariable!(fg, :l1, ContinuousScalar)
addVariable!(fg, :l1_0, ContinuousScalar)
addVariable!(fg, :l2, ContinuousScalar)
addVariable!(fg, :l2_0, ContinuousScalar)

# priors on two landmarks only
addFactor!(fg, [:l1], Prior(Normal(l1, lnoise)))
addFactor!(fg, [:l2], Prior(Normal(l2, lnoise)))

# relative constraints
addFactor!(fg, [:x1;:l1;:l1_0], LinearRelative(Normal(l1-x1, lnoise)), multihypo=[1;1/2;1/2])
addFactor!(fg, [:x2;:l2;:l2_0], LinearRelative(Normal(l2-x2, lnoise)), multihypo=[1;1/2;1/2])
addFactor!(fg, [:x1;:x2], LinearRelative(Normal(0, Onoise)))

tree, smt, hist = solveTree!(fg)

# drawTree(tree, show=true)

# expect x1 x2 to have at least one mode at 0

@test getPPE(fg, :x1).suggested[1] - x1 |> abs < 1
@test getPPE(fg, :x2).suggested[1] - x2 |> abs < 1

@test getPPE(fg, :l1).suggested[1] - l1 |> abs < 1
@test getPPE(fg, :l2).suggested[1] - l2 |> abs < 1

# l1_0, l2_0 should be nearby around l1 and l2, but cannot confirm 100%
@test getPPE(fg, :l1_0).suggested[1] - l1 |> abs < 10
@test getPPE(fg, :l2_0).suggested[1] - l2 |> abs < 10

end

# using RoMEPlotting
# Gadfly.set_default_plot_size(35cm,25cm)
#
# plotKDE(fg, [:l1;:l2])
# plotKDE(fg, [:l1_0;:l2_0])
# plotKDE(fg, [:x1;:x2])

#


modularity{V}(graph::AbstractGraph{V}, membership::Vector{Int}) = modularity(graph, membership, Float64[])

function modularity{V,T<:Real}(graph::AbstractGraph{V}, membership::Vector{Int}, weights::Vector{T})

    types = maximum(membership)
    no_of_edges = num_edges(graph)
    m = 0.0

    if length(membership) < num_vertices(graph)
        error("cannot calculate modularity, membership vector too short")
    end

    e = zeros(types)
    a = zeros(types)

    if !isempty(weights)
        if length(weights) < no_of_edges
            error("cannot caculate modularity, weight vector too short")
        end
        m = sum(weights)
        for i in edges(graph)
            w = weights[edge_index(i, graph)]
            w >= 0 || error("negative weight in weight vector")
            c1 = membership[vertex_index(source(graph, i), graph)]
            c2 = membership[vertex_index(target(graph, i), graph)]
            if c1 == c2
                e[c1] += 2w
            end
            a[c1] += w
            a[c2] += w
        end
    else
        m = no_of_edges
        for i in edges(graph)
            c1 = membership[vertex_index(source(i, graph), graph)]
            c2 = membership[vertex_index(target(i, graph), graph)]
            if c1 == c2
                e[c1] += 2
            end
            a[c1] += 1
            a[c2] += 1
        end
    end

    modularity_value = 0.0
    if m > 0
        for i=1:types
            tmp = a[i]/2/m
            modularity_value += e[i]/2/m
            modularity_value -= tmp*tmp
        end
    end
    modularity_value
end


using Distributions, LinearAlgebra, Manifolds, Random, Statistics, StatsBase, Test

@testset "DiracDelta" begin
    M = Euclidean(3)
    @testset "constructor" begin
        p = randn(3)
        d = DiracDelta(M, p)
        @test d isa DiracDelta
        @test d isa Manifolds.MPointDistribution{typeof(M)}
        @test d.point == p
        @test d.manifold === M
    end

    @testset "eltype" begin
        p = randn(3)
        d = DiracDelta(M, p)
        @test eltype(d) === typeof(p)
    end

    @testset "rand!" begin
        p = randn(3)
        q = Vector{Float64}(undef, 3)
        d = DiracDelta(M, p)
        @test rand!(d, q) === q
        @test q == p
    end

    @testset "rand" begin
        p = randn(3)
        d = DiracDelta(M, p)
        @test rand(d) == p

        ps = rand(d, 2)
        @test typeof(ps) == Vector{typeof(p)}
        @test length(ps) == 2
        @test ps[1] == p
        @test ps[2] == p
    end

    @testset "insupport" begin
        p = randn(3)
        d = DiracDelta(M, p)
        @test insupport(d, p)
        @test !insupport(d, randn(3))
    end

    @testset "$f" for f in (mean, median, mode)
        p = randn(3)
        d = DiracDelta(M, p)
        @test f(d) == p
    end

    @testset "$f" for f in (var, std)
        p = randn(3)
        d = DiracDelta(M, p)
        @inferred f(d)
        @test isreal(f(d))
        @test iszero(f(d))

        M2 = Euclidean(3; field=ℂ)
        p2 = randn(ComplexF64, 3)
        d2 = DiracDelta(M2, p2)
        @inferred f(d2)
        @test isreal(f(d2))
        @test iszero(f(d2))
    end

    @testset "convolve" begin
        G = SpecialOrthogonal(3)
        p1, p2 = ntuple(_ -> group_exp(G, hat(G, Matrix(Diagonal(ones(3))), randn(3))), 2)
        d1 = DiracDelta(G, p1)
        d2 = DiracDelta(G, p2)
        d12 = convolve(d1, d2)
        @test d12 isa typeof(d1)
        @test isapprox(G, d12.point, compose(G, p1, p2))
    end

    @testset "inversion" begin
        p = randn(3)
        d = DiracDelta(M, p)
        @test inversion(d) === d
    end
end


struct SpectralParameters{T}
    # plan::Base.DFT.FFTW.cFFTWPlan{Complex{T},-1,true,1}
    # iplan::Base.DFT.ScaledPlan{Complex{T},Base.DFT.FFTW.cFFTWPlan{Complex{T},1,true,1},T}
    plan
    iplan
    A::T
    B::Vector{T}
    force::T
    vv::Vector{Complex{T}}
    T0::T
end

"""
    conv(p,ip,u,v)

Convolution of two vectors. Uses a planned FFT algorithm.

This function directly borrows from the conv in DSP.jl:
https://github.com/JuliaDSP/DSP.jl/blob/master/src/dspbase.jl
"""
function DSP.conv(p,ip,u,v)
    T = eltype(u)
    nu = length(u)
    nv = length(v)
    n = nu+nv-1
    np2 = n>1024 ? nextprod([2,3,5],n) : nextpow(2,n)
    upad = [u;zeros(T,np2-nu)]
    vpad = [v;zeros(T,np2-nv)]

    y = ip*((p*upad).*(p*vpad))
    return y[1:n]
end

"""
    conv_mat(u)
Calculate a matrix `A` such that `A*v = conv(u,v)`.
"""
function conv_mat(u)
    nn = length(u)
    nd2 = round(Int,nn/2)
    inds = (-nd2:nd2-1).+nn.+1

    δ = zeros(eltype(u),nn)
    δ[1] = one(eltype(u))
    v1 = conv(δ,u)[inds][1:end]

    δ *= zero(eltype(u))
    δ[end] = one(eltype(u))
    v2 = conv(δ,u)[inds][end:-1:1]

    v1[1] = v2[1]

    ToeplitzMatrices.Toeplitz(v1,v2)
end

"""
    conv_mat(p,ip,u)
Same as `conv_mat(u)` except it reqires a forward plan `p` and an inverse plan `ip`.
Both plans need to be appropriately sized so that `p*conv(u,u)` will work.
"""
function conv_mat(p,ip,u)
    nn = length(u)
    nd2 = round(Int,nn/2)
    inds = (-nd2:nd2-1).+nn.+1

    δ = zeros(eltype(u),nn)
    δ[1] = one(eltype(u))
    v1 = conv(p,ip,δ,u)[inds][1:end]

    δ *= zero(eltype(u))
    δ[end] = one(eltype(u))
    v2 = conv(p,ip,δ,u)[inds][end:-1:1]

    v1[1] = v2[1]

    ToeplitzMatrices.Toeplitz(v1,v2)
end

function spectral_rhs!(::Type{Val{:jac}},jac,ϕ,params,t)
    nn = length(params.vv)
    p = view(ϕ,1:nn)
    tt = view(ϕ,nn+1:2nn)

    density_pjac!(view(jac,1:nn,1:nn),p,tt,params)
    density_ttjac!(view(jac,1:nn,nn+1:2nn),p,tt,params)
    temperature_pjac!(view(jac,nn+1:2nn,1:nn),tt,p,params)
    temperature_ttjac!(view(jac,nn+1:2nn,nn+1:2nn),tt,p,params)
    jac
end

function density!(dp,p,tt,params)
    @unpack plan,iplan,A,B,force,vv,T0 = params
    nn = length(axes(p)[1])
    nd2 = round(Int,nn/2)
    kk = -nd2:nd2-1
    dp[:] .= (-im*2π*force*kk.*p
              .-4π^2*( conv(plan,iplan,p.*kk.^2,tt)[kk.+nn.+1]
                      .+conv(plan,iplan,p.*kk,tt.*kk)[kk.+nn.+1])
              .-4π^2*( conv(plan,iplan,p,vv.*kk.^2)[kk.+nn.+1]
                      .+conv(plan,iplan,p.*kk,vv.*kk)[kk.+nn.+1]))
end

function density_pjac!(pjac,p,tt,params)
    @unpack plan,iplan,A,B,force,vv,T0 = params
    nn = length(axes(p)[1])
    nd2 = round(Int,nn/2)
    kk = (-nd2:nd2-1)*one(eltype(p)) |> collect

    K = Diagonal(kk)
    pjac[:,:] .= -im*2π*force*K.-4π^2*(  conv_mat(plan,iplan,tt)*K.^2 .+conv_mat(plan,iplan,tt.*kk)*K
                                       .+conv_mat(plan,iplan,vv.*kk.^2).+conv_mat(plan,iplan,vv.*kk)*K)
end

function density_ttjac!(ttjac,p,tt,params)
    @unpack plan,iplan,A,B,force,vv,T0 = params
    nn = length(axes(p)[1])
    nd2 = round(Int,nn/2)
    kk = (-nd2:nd2-1)*one(eltype(p)) |> collect

    K = Diagonal(kk)
    ttjac[:,:] .= -4π^2*(conv_mat(plan,iplan,p.*kk.^2).+conv_mat(plan,iplan,p.*kk)*K)
end

function temperature!(dtt,tt,p,params::SpectralParameters{T}) where T <: Number
    @unpack plan,iplan,A,B,force,vv,T0 = params
    nn = length(axes(p)[1])
    nd2 = round(Int,nn/2)
    kk = -nd2:nd2-1
    inds = (-nd2:nd2-1).+nn.+1
    delta_k_0 = zeros(nn)
    delta_k_0[nd2+1] = T0
    dtt[:] .= ( A*force^2*p-2π*im*A*force*conv(plan,iplan,kk.*p,tt)[kk.+nn.+1]
               -4π*im*A*force*conv(plan,iplan,p,kk.*vv)[kk.+nn.+1]
               -4π^2*A*conv(plan,iplan,tt,conv(plan,iplan,kk.*p,kk.*vv)[inds])[inds]
               -4π^2*A*conv(plan,iplan,p,conv(plan,iplan,vv.*kk,vv.*kk)[inds])[inds]
               -4π^2*B[1]*tt.*(kk.^2)
               +B[2]*(delta_k_0-tt))
end

function temperature_pjac!(pjac,tt,p,params)
    @unpack plan,iplan,A,B,force,vv,T0 = params
    nn = length(axes(p)[1])
    nd2 = round(Int,nn/2)
    kk = (-nd2:nd2-1)*one(eltype(p)) |> collect
    inds = (-nd2:nd2-1).+nn.+1

    K = Diagonal(kk)
    pjac[:,:] .= (A*force^2*Diagonal(ones(nn)).-2π*im*A*force*conv_mat(plan,iplan,tt)*K
                  .-4π*im*A*force*conv_mat(plan,iplan,vv.*kk)
                  .-4π^2*A*(  (conv_mat(plan,iplan,conv(plan,iplan,tt,kk.*vv)[inds]))*K
                            .+conv_mat(plan,iplan,conv(plan,iplan,vv.*kk,vv.*kk)[inds]))
    )
end

function temperature_ttjac!(ttjac,tt,p,params)
    @unpack plan,iplan,A,B,force,vv,T0 = params
    nn = length(axes(p)[1])
    nd2 = round(Int,nn/2)
    kk = (-nd2:nd2-1)*one(eltype(p)) |> collect
    inds = (-nd2:nd2-1).+nn.+1

    K = Diagonal(kk)
    ttjac[:,:] .= (-2π*im*A*force*conv_mat(kk.*p)
                   .-4π^2*A*conv_mat(plan,iplan,conv(plan,iplan,kk.*p,kk.*vv)[inds])
                   .-4π^2*B[1]*K.^2
                   .-B[2]*Diagonal(ones(nn)))
end

function spectral_rhs!(dϕ,ϕ,params,t)
    @unpack plan,iplan,A,B,force,vv,T0 = params
    nn = round(Int,length(ϕ)/2)
    dϕ[1:nn] .= density!(dϕ[1:nn],ϕ[1:nn],ϕ[nn+1:2nn],params)
    dϕ[nn+1:2nn] .= temperature!(dϕ[nn+1:2nn],ϕ[nn+1:2nn],ϕ[1:nn],params)
    dϕ
end

function real_current(ϕ,params)
    nn = round(Int,length(ϕ)/2)
    nd2 = round(Int,nn/2)
    kk = -nd2:nd2-1
    p = ϕ[1:nn]
    tt = ϕ[nn+1:2nn]

    ∇p = -im*2π*kk.*p
    ∇tt = -im*2π*kk.*tt
    ∇vv = -im*2π*kk.*params.vv

    P = fft(fftshift(p))
    T = fft(fftshift(tt))
    ∇P = fft(fftshift(∇p))
    ∇T = fft(fftshift(∇tt))
    ∇V = fft(fftshift(∇vv))

    J = -(P.*(∇V.+params.force).+T.*∇P)
end


#######################
# Convergence Metrics #
#######################

# Utilities

abschange(curr, prev) = Float64(abs(curr - prev))
relchange(curr, prev) = Float64(abs(curr - prev)/abs(curr))

maxdiff(x::AbstractArray, y::AbstractArray) = mapreduce((a, b) -> abs(a - b), max, x, y)
abschange(curr::T, prev) where {T<:AbstractArray} = maxdiff(curr, prev)
relchange(curr::T, prev) where {T<:AbstractArray} = maxdiff(curr, prev)/maximum(abs, curr)

# Single value

"""
Absolute difference convergence for single objective optimization.

This convergence metric allows to estimate an absolute difference between consecutive
states of the optimization algorithm, and triggers convergence when,

- `|f(x) - f(x')| < ε`

where `ε` is a tolerance value, `x` and `x'` are previous and current minimizers
found by the optimization algorithm.
"""
mutable struct AbsDiff{T} <: ConvergenceMetric
    tol::T
    Δ::Float64
    value::T
end
AbsDiff(tol::T) where {T<:AbstractFloat} = AbsDiff(tol, Inf, zero(T))
AbsDiff() = AbsDiff(1e-12)
description(m::AbsDiff) = "|f(x) - f(x')|"
function assess!(m::AbsDiff, state::AbstractOptimizerState)
    val = value(state)
    m.Δ = abschange(val, m.value)
    m.value = val
    converged(m)
end

"""
Relative difference convergence metric for single objective optimization.

This convergence metric allows to estimate a relative difference between consecutive
states of the optimization algorithm, and triggers convergence when,

- `|f(x) - f(x')|/|f(x')| < ε`

where `ε` is a tolerance value, `x` and `x'` are previous and current minimizers
found by the optimization algorithm.
"""
mutable struct RelDiff{T} <: ConvergenceMetric
    tol::T
    Δ::Float64
    value::T
end
RelDiff(tol::T) where {T<:AbstractFloat} = RelDiff(tol, Inf, zero(T))
RelDiff() = RelDiff(1e-12)
description(m::RelDiff) = "|f(x) - f(x')|/|f(x')|"
function assess!(m::RelDiff, state::AbstractOptimizerState)
    val = value(state)
    m.Δ = relchange(val, m.value)
    m.value = val
    converged(m)
end

"""
    gd(A,R)

Calculate a generational distance between set `A` and the reference set `R`.
This metric measures the convergence, i.e. closeness of the non-dominated solutions
to the Pareto front, of a population.

*Note:* Parameters are column-major matrices.
"""
function gd(A::AbstractMatrix, R::AbstractMatrix)
    da, na = size(A)
    dr, nr = size(R)
    (na == 0 || nr == 0) && return Inf
    sum = 0
    for a in eachcol(A)
        sum += minimum(norm(a-r) for r in eachcol(R))
    end
    sum/na
end

"""
    igd(S,R)

Calculate an inverted generational distance, [`gd`](@ref), between set `S` and the reference set `R`.
Parameters are column-major matrices.
"""
igd(S,R) = gd(R,S)

"""
(Inverse) generational distance convergence metric for multi-objective optimization.

This convergence metric allows to estimate a (inverse) generational distance between
consecutive states of the multi-objective optimization algorithm, and triggers convergence when,

- ``GD(S,R) = \\frac{1}{|S|}\\left(\\sum_{s \\in S} \\min_{r \\in R}||f(s) - f(r)||^2\\right)^{1/2} < \\varepsilon``

where `ε` is a tolerance value, `S` and `R` are discrete Pareto front approximations
for previous and current state of the optimization algorithm.

The inverse generational distance calculated as `IGD(S,R) = GR(R,S)`.
To use, `IGD` metric pass `true` value to the metric constructor argument, e.g.

- `GD()` create a generational distance metric
- `GD(true)` create an inverse generational metric
"""
mutable struct GD{T} <: ConvergenceMetric
    tol::T
    Δ::Float64
    value::Matrix{T}
    inverted::Bool
end
GD(tol::T, inv=false) where {T<:AbstractFloat} = GD(tol, Inf, zeros(T,0,0), inv)
GD(inv=false) = GD(1e-5, inv)
function description(m::GD)
    prefix = m.inverted ? "I" : ""
    "|$(prefix)GD(P) - $(prefix)GD(P')|"
end
function assess!(m::GD, state::AbstractOptimizerState)
    val = value(state)
    m.Δ = m.inverted ? igd(val, m.value) : gd(val, m.value)
    m.value = val
    converged(m)
end

"""
    spread(S,R)

Returns a diversity metric of a population of set `S` to the reference set `R`.
"""
function spread(S::AbstractMatrix, R::AbstractMatrix)
    n = size(S,2)
    m = size(R,2)
    Δₖ = [ minimum(norm(view(S,:,i)-view(R,:,j)) for i in 1:n if view(S,:,i) != view(R,:,j)) for j in 1:m ]
    Δ = mean(Δₖ)
    sum(abs.(Δₖ.-Δ))/m*Δ
end

function spread(S::AbstractMatrix)
    n = size(S,2)
    n == 1 && return NaN
    Δₖ = [ minimum(norm(view(S,:,i)-view(S,:,j)) for j in 1:n if view(S,:,i) != view(S,:,j)) for i in 1:n ]
    Δ = mean(Δₖ)
    sum(abs.(Δₖ.-Δ))/n*Δ
end

##########################
# Convergence Assessment #
##########################

assess_convergence(state::AbstractOptimizerState, method) =
    any(assess!(cm, state) for cm in metrics(method))


module LagrangeFunction

export GvectorsT
include("LagrangeFunction/Gvectors.jl")

export LF1dGrid
export init_LF1d_p
export init_LF1d_c
export init_LF1d_sinc
include("LagrangeFunction/LF1d.jl")

export LF3dGrid
export init_LF3d_p
export init_LF3d_c
export init_LF3d_sinc
include("LagrangeFunction/LF3d.jl")

export ortho_gram_schmidt
include("LagrangeFunction/ortho_gram_schmidt.jl")

export orthonormalize
include("LagrangeFunction/orthonormalize.jl")

export print_Energies
include("LagrangeFunction/EnergiesT.jl")
include("LagrangeFunction/PotentialsT.jl")

include("LagrangeFunction/op_nabla2.jl")
include("LagrangeFunction/op_H.jl")

export calc_evals, calc_rho
include("LagrangeFunction/calc_Energies.jl")
include("LagrangeFunction/calc_evals.jl")
include("LagrangeFunction/calc_rho.jl")
include("LagrangeFunction/calc_grad.jl")

export KS_solve_scf, KS_solve_Emin_cg, KS_solve_Emin_pcg
include("LagrangeFunction/KS_solve_Emin_cg.jl")
include("LagrangeFunction/KS_solve_Emin_pcg.jl")
include("LagrangeFunction/KS_solve_scf.jl")

export prec_mkl_ilu0, get_Laplacian3d_kron
include("LagrangeFunction/sparse_LF3d.jl")
include("LagrangeFunction/prec_mkl_ilu0.jl")
include("LagrangeFunction/apply_prec_ilu0.jl")

include("LagrangeFunction/solve_poisson_FFT.jl")

include("LagrangeFunction/LDA_VWN.jl")
include("LagrangeFunction/diag_lobpcg.jl")

export build_nabla2
include("LagrangeFunction/build_nabla2.jl")

end


using SpectroscopicUnits
using Unitful
using Test

@testset "SpectroscopicUnits.jl" begin
    @testset "convert Hz to length" begin
        @test ustrip(convert_unit(m, 1e15Hz)) ≈ 2.9979e-7 atol = 1e-11
        @test ustrip(convert_unit(cm, 1e15Hz)) ≈ 2.9979e-5 atol = 1e-9
        @test ustrip(convert_unit(mm, 1e15Hz)) ≈ 0.00029979 atol = 1e-8
        @test ustrip(convert_unit(μm, 1e15Hz)) ≈ 0.29979 atol = 1e-5
        @test ustrip(convert_unit(nm, 1e15Hz)) ≈ 299.79246 atol = 1e-5
    end

    @testset "convert radHz to length" begin
        @test ustrip(convert_unit(m, 2.0*pi*1e15radHz)) ≈ 2.9979e-7 atol = 1e-11
        @test ustrip(convert_unit(cm, 2.0*pi*1e15radHz)) ≈ 2.9979e-5 atol = 1e-9
        @test ustrip(convert_unit(mm, 2.0*pi*1e15radHz)) ≈ 0.00029979 atol = 1e-8
        @test ustrip(convert_unit(μm, 2.0*pi*1e15radHz)) ≈ 0.29979 atol = 1e-5
        @test ustrip(convert_unit(nm, 2.0*pi*1e15radHz)) ≈ 299.79246 atol = 1e-5
    end

    @testset "convert eV to length" begin
        @test ustrip(convert_unit(nm, 3.2eV)) ≈ 387.451 atol = 1e-3
        @test ustrip(convert_unit(μm, 3.2eV)) ≈ 0.38745 atol = 1e-5
    end
end


using LinearAlgebra
#import Gaugefields:Verbose_level,Verbose_3,Verbose_2,Verbose_1,println_verbose_level3
using InteractiveUtils
import Gaugefields.Verboseprint_mpi:Verbose_print,println_verbose_level1,println_verbose_level2,println_verbose_level3

#export bicg,bicgstag,shiftedcg,bicgstab_evenodd,reducedshiftedcg,cg

function add!(b,Y,a,X) #b*Y + a*X -> Y
    LinearAlgebra.axpby!(a,X,b,Y) #X*a + Y*b -> Y
end

function add!(Y,a,X) #Y + a*X -> Y
    LinearAlgebra.axpby!(a,X,1,Y) #X*a + Y -> Y
end

function add!(b,Y,a,X,iseven::Bool) #b*Y + a*X -> Y
    LinearAlgebra.axpby!(a,X,b,Y,iseven) #X*a + Y*b -> Y
end

function add!(Y,a,X,iseven::Bool) #Y + a*X -> Y
    LinearAlgebra.axpby!(a,X,1,Y,iseven) #X*a + Y -> Y
end

function bicg(x,A,b;eps=1e-10,maxsteps = 1000,verbose = Verbose_print(2)) #Ax=b
    #println_verbose_level3()
    println_verbose_level3(verbose,"--------------------------------------")
    println_verbose_level3(verbose,"bicg method")
    #=
    res = deepcopy(b)
    temp1 = similar(x)
    p = similar(x)
    q = similar(x)
    s = similar(x)

    mul!(temp1,A,x)
    add!(res,-1,temp1)
    =#

    temps = get_temporaryvectors_forCG(A)
    res = temps[1]
    substitute_fermion!(res,b)
    temp1 = temps[2]
    mul!(temp1,A,x)
    add!(res,-1,temp1)
    p = temps[4]
    q = temps[5]
    s = temps[6]

    rnorm = real(res⋅res)
    if rnorm < eps
        return
    end
    #println(rnorm)

    mul!(p,A',res)
    c1 = p ⋅ p

    for i=1:maxsteps
        mul!(q,A,p)
        #! ...  c2 = < q | q >
        c2 = q ⋅ q

        alpha = c1 / c2
        #! ...  x   = x   + alpha * p
        add!(x,alpha,p)
        #...  res = res - alpha * q
        add!(res,-alpha,q)
        rnorm = real(res ⋅ res)
        println_verbose_level3(verbose,"$i-th eps: $rnorm")

        if rnorm < eps
            #println("Converged at $i-th step. eps: $rnorm")
            println_verbose_level3(verbose,"Converged at $i-th step. eps: $rnorm")
            println_verbose_level3(verbose,"--------------------------------------")
            return
        end

        mul!(s,A',res)

        #c3 = s * s
        c3 = s ⋅ s

        beta = c3 / c1
        c1 = c3

        add!(beta,p,1,s) #p = beta*p + s

    end

    error("""
    The BICG is not converged! with maxsteps = $(maxsteps)
    residual is $rnorm
    maxsteps should be larger.""")

end

function bicgstab(x,A,b;eps=1e-10,maxsteps = 1000,verbose = Verbose_print(2)) #Ax=b
    println_verbose_level3(verbose,"--------------------------------------")
    println_verbose_level3(verbose,"bicg-stab method")

    #=
    r = deepcopy(b)
    temp1 = similar(x)
    mul!(temp1,A,x)
    add!(r,-1,temp1)

    rs = deepcopy(r)
    p = deepcopy(r)
    Ap = similar(r)
    s = similar(r)
    t = similar(r)
    =#

    temps = get_temporaryvectors_forCG(A)
    r = temps[1]
    substitute_fermion!(r,b)
    temp1 = temps[2]
    mul!(temp1,A,x)
    add!(r,-1,temp1)

    rs = temps[3]
    substitute_fermion!(rs,r)
    p = temps[4]
    substitute_fermion!(p,r)
    Ap = temps[5]
    s = temps[6]
    t = temps[7]

    rnorm = real(r⋅r)

    if rnorm < eps
        return
    end

    for i=1:maxsteps
        c1 = dot(rs,r)
        mul!(Ap,A,p)
        c2 = dot(rs,Ap)
        α = c1/c2
        #s = r - α*A*p
        add!(0,s,1,r)
        add!(s,-α,Ap)
        mul!(t,A,s)
        d1 = dot(t,s)
        d2 = dot(t,t)
        ω = d1/d2

        #r = (1-ω A)s
        add!(0,r,1,s)
        add!(r,-ω,t)

        #x = x + ωs+ αp
        add!(x,ω,s)
        add!(x,α,p)

        β = (dot(rs,r)/c1)*(α/ω)

        #p = r + β*(1-ωA)*p
        add!(β,p,1,r)
        add!(p,-ω*β,Ap)

        rnorm = real(r ⋅ r)
        println_verbose_level3(verbose,"$i-th eps: $rnorm")

        if rnorm < eps
            println_verbose_level3(verbose,"Converged at $i-th step. eps: $rnorm")
            println_verbose_level3(verbose,"--------------------------------------")
            return
        end

    end

    error("""
    The BICGstab is not converged! with maxsteps = $(maxsteps)
    residual is $rnorm
    maxsteps should be larger.""")

end

function bicgstab_3(x,A,b;eps=1e-10,maxsteps = 1000,verbose = Verbose_print(2)) #Ax=b
    println_verbose_level3(verbose,"--------------------------------------")
    println_verbose_level3(verbose,"bicg-stab method")
    r = deepcopy(b)
    temp1 = similar(x)
    mul!(temp1,A,x)
    add!(r,-1,temp1)

    rnorm = real(r⋅r)

    if rnorm < eps
        return
    end

    #function test()

    rs = deepcopy(r)
    #rs = deepcopy(b)
    p = deepcopy(r)
    s = similar(r)
    t = similar(r)
    α = 0
    Ap = similar(p)
    As = similar(p)
    ω = 0

    for i=0:maxsteps
        mul!(Ap,A,p)
        c1 = dot(r,rs)
        α = c1/dot(Ap,rs)
        #s = r - alpha*v
        add!(0,s,1,r)
        add!(s,-α,Ap)

        mul!(As,A,s)
        ω = dot(As,s)/dot(As,As)
        add!(x,α,p)
        add!(x,ω,s)

        add!(0,r,1,s) # r = a*r + b*s (a = 0,b = 1)
        add!(r,-ω,t)

        β = (dot(r,rs)/c1)*α/ω

        #p = r + β*(p - ωm*vm)
        add!(β,p,1,r)
        add!(p,-ω*β,Ap)

        rnorm = real(r ⋅ r)
        println_verbose_level3(verbose,"$i-th eps: $rnorm")

        if rnorm < eps
            println_verbose_level3(verbose,"Converged at $i-th step. eps: $rnorm")
            println_verbose_level3(verbose,"--------------------------------------")
            return
        end
    end
    #end

    #= other
    rs = deepcopy(r)
    ρm = 1
    α = 1
    ωm = 1
    vm = similar(x)
    p = similar(x)
    s = similar(x)
    t = similar(x)

    println("rr ",dot(r,r))
    println("rsrs ",dot(rs,rs))
    println("rsr ",dot(rs,r))

    for i=1:maxsteps
        ρ = dot(rs,r)
        println(ρ    )
        β =(ρ/ρm)*(α/ωm)
        #p = r + β*(p - ωm*vm)
        add!(β,p,1,r)
        add!(p,-ωm*β,vm)

        mul!(vm,A,p)
        α = ρ/dot(rs,vm)

        #s = r - alpha*v
        add!(0,s,1,r)
        add!(s,-α,vm)

        mul!(t,A,s)
        ωm = dot(t,s)/dot(r,r)

        add!(x,α,p)
        add!(x,ωm,s)

        add!(0,r,1,s) # r = a*r + b*s (a = 0,b = 1)
        add!(r,-ωm,t)

        rnorm = real(r ⋅ r)
        println_verbose_level3(verbose,"$i-th eps: $rnorm")

        if rnorm < eps
            println_verbose_level3(verbose,"Converged at $i-th step. eps: $rnorm")
            println_verbose_level3(verbose,"--------------------------------------")
            return
        end
    end

    =#

    #= text book
    p = deepcopy(r)
    s = similar(r)
    v = similar(x)
    t = similar(x)
    β = 0.0im
    ω = 0.0im
    ρ2 = 0.0im
    α = 0.0im

    for i=0:maxsteps
        ρ = dot(rs,r)
        if i != 0
            β = α*ρ/(ω*ρ2)

            add!(β,p,1,r)
            add!(p,-ω*β,v) #p = r + beta*(p -omega*v)
        end
        mul!(v,A,p)
        c2 = dot(rs,v)
        α = ρ/c2

        #s = r - α*v
        add!(0,s,1,r)
        add!(s,-α,v)

        rnorm = real(dot(s,s))
        if rnorm < eps
            add!(x,α,p)
            println_verbose_level3(verbose,"Converged at $i-th step. eps: $rnorm")
            println_verbose_level3(verbose,"--------------------------------------")

            return
        end
        mul!(t,A,s)

        ω = dot(t,s)/dot(t,t)
        #r = s - ω*t
        add!(0,r,1,s) # r = a*r + b*s (a = 0,b = 1)
        add!(s,-ω,t)

        add!(x,α,p)
        add!(x,ω,s)
        ρ2 = ρ

    end

    =#
    error("""
    The BICGstab is not converged! with maxsteps = $(maxsteps)
    residual is $rnorm
    maxsteps should be larger.""")

end

function bicgstab_YN(x,A,b;eps=1e-10,maxsteps = 1000,verbose = Verbose_print(2)) #Ax=b
    println_verbose_level3(verbose,"--------------------------------------")
    println_verbose_level3(verbose,"bicg-stab method")
    r = deepcopy(b)
    temp1 = similar(x)
    mul!(temp1,A,x)
    add!(r,-1,temp1)

    rnorm = real(r⋅r)

    if rnorm < eps
        return
    end
    rs = deepcopy(r)
    p = deepcopy(r)
    s = similar(r)
    Ap = similar(x)
    As = similar(x)
    β = 0.0im
    ω = 0.0im

    for i=0:maxsteps
        if i != 0
            add!(β,p,1,r)
            add!(p,-ω*β,Ap)
        end
        c1 = dot(rs,r)
        mul!(Ap,A,p)
        c2 = dot(rs,Ap)
        α = c1/c2
        add!(0,s,1,r)
        add!(s,-α,Ap)
        mul!(As,A,s)
        s1 = dot(As,s)
        s2 = dot(As,As)

        ω = s1/s2
        add!(x,α,p)
        add!(x,ω,s)
        add!(0,r,1,s)
        add!(r,-ω,As)

        rnorm = real(r ⋅ r)
        println_verbose_level3(verbose,"$i-th eps: $rnorm")

        if rnorm < eps
            println_verbose_level3(verbose,"Converged at $i-th step. eps: $rnorm")
            println_verbose_level3(verbose,"--------------------------------------")
            return
        end

        β = (α/ω)*dot(rs,r)/c1

    end

    error("""
    The BICG is not converged! with maxsteps = $(maxsteps)
    residual is $rnorm
    maxsteps should be larger.""")

end

function bicgstab_evenodd(x,A,b,iseven;eps=1e-10,maxsteps = 1000,verbose = Verbose_print(2)) #Ax=b
    println_verbose_level3(verbose,"--------------------------------------")
    println_verbose_level3(verbose,"bicg-stab even-odd method")
    r = deepcopy(b)
    temp1 = similar(x)
    mul!(temp1,A,x)
    #println(dot(temp1,temp1,iseven))
    add!(r,-1,temp1,iseven)

    rnorm = real(dot(r,r,iseven))

    if rnorm < eps
        return
    end

    rs = deepcopy(r)
    p = deepcopy(r)
    Ap = similar(r)
    s = similar(r)
    t = similar(r)

    for i=1:maxsteps
        c1 = dot(rs,r,iseven)
        mul!(Ap,A,p)
        c2 = dot(rs,Ap,iseven)
        α = c1/c2
        #s = r - α*A*p
        add!(0,s,1,r,iseven)
        add!(s,-α,Ap,iseven)
        mul!(t,A,s)
        d1 = dot(t,s,iseven)
        d2 = dot(t,t,iseven)
        ω = d1/d2

        #r = (1-ω A)s
        add!(0,r,1,s,iseven)
        add!(r,-ω,t,iseven)

        #x = x + ωs+ αp
        add!(x,ω,s,iseven)
        add!(x,α,p,iseven)

        β = (dot(rs,r,iseven)/c1)*(α/ω)

        #p = r + β*(1-ωA)*p
        add!(β,p,1,r,iseven)
        add!(p,-ω*β,Ap,iseven)

        rnorm = real(dot(r,r,iseven))
        println_verbose_level3(verbose,"$i-th eps: $rnorm")

        if rnorm < eps
            println_verbose_level3(verbose,"Converged at $i-th step. eps: $rnorm")
            println_verbose_level3(verbose,"--------------------------------------")
            return
        end

    end

    error("""
    The BICGstab is not converged! with maxsteps = $(maxsteps)
    residual is $rnorm
    maxsteps should be larger.""")

end

function bicgstab_evenodd_YN(x,A,b,iseven;eps=1e-10,maxsteps = 1000,verbose = Verbose_print(2)) #Ax=b
    println_verbose_level3(verbose,"--------------------------------------")
    println_verbose_level3(verbose,"bicg-stab even-odd method")

    r = deepcopy(b)
    temp1 = similar(x)
    mul!(temp1,A,x)
    println(dot(temp1,temp1,iseven))
    add!(r,-1,temp1,iseven)
    rs = deepcopy(r)
    p = deepcopy(r)
    s = similar(r)
    Ap = similar(x)
    As = similar(x)
    β = 0.0im
    ω = 0.0im

    rnorm = real(dot(r,r,iseven))
    if rnorm < eps
        return
    end

    for i=0:maxsteps

        #=
        if i != 0
            add!(β,p,1,r,iseven)
            add!(p,-ω*β,Ap,iseven)
        end
        =#
        c1 = dot(rs,r,iseven)
        mul!(Ap,A,p)
        c2 = dot(rs,Ap,iseven)
        α = c1/c2
        add!(0,s,1,r,iseven)
        add!(s,-α,Ap,iseven)
        mul!(As,A,s)
        s1 = dot(As,s,iseven)
        s2 = dot(As,As,iseven)

        ω = s1/s2
        add!(x,α,p,iseven)
        add!(x,ω,s,iseven)
        add!(0,r,1,s,iseven)
        add!(r,-ω,As,iseven)

        rnorm = real(dot(r,r,iseven))
        println_verbose_level3(verbose,"$i-th eps: $rnorm")

        if rnorm < eps
            println_verbose_level3(verbose,"Converged at $i-th step. eps: $rnorm")
            println_verbose_level3(verbose,"--------------------------------------")
            return
        end

        β = (α/ω)*dot(rs,r,iseven)/c1

        #if i != 0
            add!(β,p,1,r,iseven)
            add!(p,-ω*β,Ap,iseven)
        #end
    end

    error("""
    The BICG is not converged! with maxsteps = $(maxsteps)
    residual is $rnorm
    maxsteps should be larger.""")

end

function cg(x,A,b;eps=1e-10,maxsteps = 1000,verbose = Verbose_print(2)) #Ax=b
    temps = get_temporaryvectors_forCG(A)

    println_verbose_level3(verbose,"--------------------------------------")
    println_verbose_level3(verbose,"cg method")

    #=
    res = deepcopy(b)
    temp1 = similar(x)
    mul!(temp1,A,x)
    add!(res,-1,temp1)
    q = similar(x)
    p = deepcopy(res)
    =#

    res = temps[1]
    substitute_fermion!(res,b)
    temp1 = temps[2]
    mul!(temp1,A,x)
    add!(res,-1,temp1)
    q = temps[3]
    p = temps[4]
    substitute_fermion!(p,res)

    #p = deepcopy(res)

    rnorm = real(res⋅res)
    #println(rnorm)

    if rnorm < eps
        return
    end

    c1 = p ⋅ p

    for i=1:maxsteps
        mul!(q,A,p)
        c2 = dot(p,q)
        #c2 = p ⋅ q

        α = c1 / c2
        #! ...  x   = x   + alpha * p
        #println("add2")
        add!(x,α,p)
        #...  res = res - alpha * q
        #println("add1")
        add!(res,-α,q)
        c3 = res ⋅ res
        rnorm = real(c3)
        println_verbose_level3(verbose,"$i-th eps: $rnorm")

        if rnorm < eps
            #println("$i eps: $eps rnorm $rnorm")
            println_verbose_level3(verbose,"Converged at $i-th step. eps: $rnorm")
            println_verbose_level3(verbose,"--------------------------------------")
            return
        end

        β = c3 / c1
        c1 = c3

        #println("add3")
        add!(β,p,1,res) #p = beta*p + s

    end

    error("""
    The CG is not converged! with maxsteps = $(maxsteps)
    residual is $rnorm
    maxsteps should be larger.""")

end

function Base.:*(x::Array{T,1},y::Array{T,1}) where T <: Number
    return x'*y
end

function shiftedcg(vec_x,vec_β,x,A,b;eps=1e-10,maxsteps = 1000,verbose = Verbose_print(2)) #Ax=b

    println_verbose_level3(verbose,"--------------------------------------")
    println_verbose_level3(verbose,"shifted cg method")
    N = length(vec_β)
    temp1 = similar(b)
    r = deepcopy(b)
    p = deepcopy(b)
    q = similar(b)

    vec_r = Array{typeof(r),1}(undef,N)
    vec_p = Array{typeof(p),1}(undef,N)
    for j=1:N
        vec_r[j] = deepcopy(b)
        vec_p[j] = deepcopy(b)
    end

    αm = 1.0
    βm = 0.0

    ρm = ones(ComplexF64,N)
    ρ0 = ones(ComplexF64,N)
    ρp = ones(ComplexF64,N)
    residual = 0

    for i=1:maxsteps
        mul!(q,A,p)

        pAp = p ⋅ q

        rr = dot(r,r)
        αk = rr / pAp

        #! ...  x   = x   + alpha * p
        add!(x,αk,p)

        #...  r = r - alpha * q
        add!(r,-αk,q)

        βk = dot(r,r)/ rr
        add!(βk,p,1,r) #p = beta*p + r

        for j=1:N
            ρkj = ρ0[j]
            if abs(ρkj) < eps
                continue
            end
            ρkmj =ρm[j]
            ρp[j] = ρkj*ρkmj*αm/(ρkmj*αm*(1.0+αk*vec_β[j])+αk*βm*(ρkmj-ρkj))
            αkj = (ρp[j]/ρkj)*αk

            add!(vec_x[j],αkj,vec_p[j])
            βkj = (ρp[j]/ρkj)^2*βk
            add!(βkj,vec_p[j],ρp[j],r)

        end

        ρm[:] = ρ0[:]
        ρ0[:] = ρp[:]
        αm = αk
        βm = βk

        ρMAX = maximum(abs.(ρp))^2
        residual = abs(rr*ρMAX)
        println_verbose_level3(verbose,"$i-th eps: $residual")

        if abs(residual) < eps
            println_verbose_level3(verbose,"Converged at $i-th step. eps: $residual")
            println_verbose_level3(verbose,"--------------------------------------")
            return
        end

    end

    error("""
    The shifted CG is not converged! with maxsteps = $(maxsteps)
    residual is $residual
    maxsteps should be larger.""")

end

function shiftedbicg(vec_x,vec_β,x,A,b;eps=1e-10,maxsteps = 1000,verbose = Verbose_print(2)) #Ax=b

    println_verbose_level3(verbose,"--------------------------------------")
    println_verbose_level3(verbose,"shifted cg method")
    N = length(vec_β)
    temp1 = similar(b)
    r = deepcopy(b)
    p = deepcopy(b)
    q = similar(b)
    s = similar(b)

    vec_r = Array{typeof(r),1}(undef,N)
    vec_p = Array{typeof(p),1}(undef,N)
    for j=1:N
        vec_r[j] = deepcopy(b)
        vec_p[j] = deepcopy(b)
    end

    αm = 1.0
    βm = 0.0

    ρm = ones(ComplexF64,N)
    ρ0 = ones(ComplexF64,N)
    ρp = ones(ComplexF64,N)
    residual = 0

    mul!(p,A',r)
    c1 = p ⋅ p

    for i=1:maxsteps
        mul!(q,A,p)
        c2 = q ⋅ q
        alpha = c1 / c2
        rr = dot(r,r)
        #! ...  x   = x   + alpha * p
        add!(x,alpha,p)
        #...  res = res - alpha * q
        add!(r,-alpha,q)

        mul!(s,A',r)

        #c3 = s * s
        c3 = s ⋅ s

        beta = c3 / c1
        c1 = c3

        add!(beta,p,1,s) #p = beta*p + s

        αk = alpha
        βk = beta

        for j=1:N
            ρkj = ρ0[j]
            if abs(ρkj) < eps
                continue
            end
            ρkmj =ρm[j]
            ρp[j] = ρkj*ρkmj*αm/(ρkmj*αm*(1.0+αk*vec_β[j])+αk*βm*(ρkmj-ρkj))
            αkj = (ρp[j]/ρkj)*αk

            add!(vec_x[j],αkj,vec_p[j])
            βkj = (ρp[j]/ρkj)^2*βk
            #add!(βkj,vec_p[j],ρp[j],r)
            add!(βkj,vec_p[j],ρp[j],s)

        end

        ρm[:] = ρ0[:]
        ρ0[:] = ρp[:]
        αm = αk
        βm = βk

        ρMAX = maximum(abs.(ρp))^2
        residual = abs(rr*ρMAX)
        println_verbose_level3(verbose,"$i-th eps: $residual")

        if abs(residual) < eps
            println_verbose_level3(verbose,"Converged at $i-th step. eps: $residual")
            println_verbose_level3(verbose,"--------------------------------------")
            return
        end

    end

    error("""
    The shifted CG is not converged! with maxsteps = $(maxsteps)
    residual is $residual
    maxsteps should be larger.""")

end

function reducedshiftedcg(leftvec,vec_β,x,A,b;eps=1e-10,maxsteps = 1000,verbose = Verbose_print(2)) #Ax=b
    println_verbose_level3(verbose,"--------------------------------------")
    println_verbose_level3(verbose,"shifted cg method")
    N = length(vec_β)
    temp1 = similar(b)
    r = deepcopy(b)
    p = deepcopy(b)
    q = similar(b)

    #=
    vec_r = Array{typeof(r),1}(undef,N)
    vec_p = Array{typeof(p),1}(undef,N)
    for j=1:N
        vec_r[j] = deepcopy(b)
        vec_p[j] = deepcopy(b)
    end
    =#

    Σ = leftvec ⋅ b

    θ = zeros(ComplexF64,N)
    Π = ones(ComplexF64,N) .* Σ

    αm = 1.0
    βm = 0.0

    ρm = ones(ComplexF64,N)
    ρ0 = ones(ComplexF64,N)
    ρp = ones(ComplexF64,N)
    residual = 0

    for i=1:maxsteps
        mul!(q,A,p)

        pAp = p ⋅ q
        rr = r*r
        αk = rr / pAp

        #! ...  x   = x   + alpha * p
        add!(x,αk,p)

        #...  r = r - alpha * q
        add!(r,-αk,q)

        βk = (r*r)/ rr
        add!(βk,p,1,r) #p = beta*p + r

        Σ = leftvec ⋅ r

        for j=1:N
            ρkj = ρ0[j]
            if abs(ρkj) < eps
                continue
            end
            ρkmj =ρm[j]
            ρp[j] = ρkj*ρkmj*αm/(ρkmj*αm*(1.0+αk*vec_β[j])+αk*βm*(ρkmj-ρkj))
            αkj = (ρp[j]/ρkj)*αk
            θ[j] += αkj*Π[j]

            #add!(vec_x[j],αkj,vec_p[j])
            βkj = (ρp[j]/ρkj)^2*βk
            Π[j] = βkj*Π[j] + ρp[j]*Σ

            #mul!(Π,ρp[j],Σ,1,βkj)
            #Π[:] =
            #add!(βkj,vec_p[j],ρp[j],r)

        end

        ρm[:] = ρ0[:]
        ρ0[:] = ρp[:]
        αm = αk
        βm = βk

        ρMAX = maximum(abs.(ρp))^2
        residual = abs(rr*ρMAX)
        println_verbose_level3(verbose,"$i-th eps: $residual")

        if abs(residual) < eps
            println_verbose_level3(verbose,"Converged at $i-th step. eps: $residual")
            println_verbose_level3(verbose,"--------------------------------------")
            return θ
        end

    end

    error("""
    The shifted CG is not converged! with maxsteps = $(maxsteps)
    residual is $residual
    maxsteps should be larger.""")

end

function shiftedbicg_2003(σ,A,b;maxsteps=3000,eps = 1e-15,verboselevel=2)
    N = length(σ)
    btype = typeof(b)
    q = zero(b)
    x = zero(b)
    r = deepcopy(b)
    rt = deepcopy(b)
    u = zero(b)
    ut = zero(b)
    vec_u = Vector{btype}(undef,N)
    vec_x = Vector{btype}(undef,N)
    for j=1:N
        vec_u[j] = zero(b)
        vec_x[j] = zero(b)
    end
    ρkold = ones(ComplexF64,N)
    ρk = ones(ComplexF64,N)
    πkold = ones(ComplexF64,N)
    πk = ones(ComplexF64,N)
    πknew = ones(ComplexF64,N)
    ρold = 1
    αold = 1
    residual = 0

    for i=1:maxsteps
        ρ = dot(rt,r)
        β = -ρ/ρold
        axpby!(1,r,-β,u)
        axpby!(1,rt,-β',ut)
        mul!(q,A,u)
        qrt = dot(rt,q)
        α = ρ/qrt
        axpy!(α,u,x)

        for k=1:N
            πknew[k] = (1+α*σ[k])*πk[k]+ (α*β/αold)*(πkold[k]- πk[k])
            βk = (πkold[k]/πk[k])^2*β
            αk =  (πk[k]/πknew[k])*α
            axpby!(1/πk[k],r,-βk,vec_u[k])
            axpy!(αk,vec_u[k],vec_x[k])
        end

        axpy!(-α,q,r)
        mul!(q,A',ut)
        axpy!(-α',q,rt)
        αold = α
        ρold  = ρ

        residual = dot(r,r)
        if verboselevel == 3
            println("$i-th step: ",residual)
        end

        if abs(residual) < eps
            if verboselevel >= 2
                println("Converged at $i-th step. eps: $residual")
                println("--------------------------------------")
            end
            return vec_x
        end

        for k=1:N
            πkold[k] = πk[k]
            πk[k] = πknew[k]
        end

    end

    error("bicg is not converged. The residual is $residual")

end


using PyCall
@pyimport qutip as qt
@pyimport qutip.qip.algorithms.qft as qft
@pyimport qutip.control.pulseoptim as cpo

function grape_qutip(Ut,Hd,Hc,N,t,n)
    # start point for the gate evolution
    U0 = qt.identity(N)
    res = cpo.optimize_pulse_unitary(Hd,Hc,U0,Ut,n,t,init_pulse_type="ZERO",fid_err_targ=1E-16,min_grad=1E-16,max_iter=1000)
    uf = res[:final_amps]
    Uf = res[:evo_full_final][:full]()
    return uf, Uf, res
end

function opt_hadamard_qutip(n)
    Hd = qt.sigmaz()
    Hc = [qt.sigmax()]
    t = 10.0
    Ut = qt.hadamard_transform(1)
    grape_qutip(Ut,Hd,Hc,2,t,n)
end

function opt_2Q_QFT_qutip(n)
    Sx = qt.sigmax()
    Sy = qt.sigmay()
    Sz = qt.sigmaz()
    Si = qt.identity(2)
    # Drift Hamiltonian and the (four) control Hamiltonians
    H_d = (qt.tensor(Sx, Sx) + qt.tensor(Sy, Sy) + qt.tensor(Sz, Sz))
    H_c = [qt.tensor(Sx, Si), qt.tensor(Sy, Si), qt.tensor(Si, Sx), qt.tensor(Si, Sy)]
    # Target for the gate evolution - Quantum Fourier Transform gate
    U_targ = qft.qft(2)
    # Evolution time
    t = 6.0
    grape_qutip(U_targ,H_d,H_c,4,t,n)
end

#"import qutip as qt; import qutip.qip.algorithms.qft as qft; import qutip.control.pulseoptim as cpo; Sx = qt.sigmax();Sy = qt.sigmay();Sz = qt.sigmaz();Si = qt.identity(2);H_d = (qt.tensor(Sx, Sx) + qt.tensor(Sy, Sy) + qt.tensor(Sz, Sz));H_c = [qt.tensor(Sx, Si), qt.tensor(Sy, Si), qt.tensor(Si, Sx), qt.tensor(Si, Sy)];U_0 = qt.identity(4);U_targ = qft.qft(2);" "cpo.optimize_pulse_unitary(H_d, H_c, U_0, U_targ, 100, 6, init_pulse_type='ZERO')"


#############################################################################
# diagm.jl
# Converts a vector of size n into an n x n diagonal
# All expressions and atoms are subtpyes of AbstractExpr.
# Please read expressions.jl first.
#############################################################################

import LinearAlgebra.diagm, LinearAlgebra.Diagonal
export diagm, Diagonal

struct DiagMatrixAtom <: AbstractExpr
    head::Symbol
    id_hash::UInt64
    children::Tuple{AbstractExpr}
    size::Tuple{Int, Int}

    function DiagMatrixAtom(x::AbstractExpr)
        (num_rows, num_cols) = x.size

        if num_rows == 1
            sz = num_cols
        elseif num_cols == 1
            sz = num_rows
        else
            throw(ArgumentError("Only vectors are allowed for diagm/Diagonal. Did you mean to use diag?"))
        end

        children = (x, )
        return new(:diagm, hash(children), children, (sz, sz))
    end
end

function sign(x::DiagMatrixAtom)
    return sign(x.children[1])
end

# The monotonicity
function monotonicity(x::DiagMatrixAtom)
    return (Nondecreasing(),)
end

# If we have h(x) = f o g(x), the chain rule says h''(x) = g'(x)^T f''(g(x))g'(x) + f'(g(x))g''(x);
# this represents the first term
function curvature(x::DiagMatrixAtom)
    return ConstVexity()
end

function evaluate(x::DiagMatrixAtom)
    return Diagonal(vec(evaluate(x.children[1])))
end

function diagm((d, x)::Pair{<:Integer, <:AbstractExpr})
    d == 0 || throw(ArgumentError("only the main diagonal is supported"))
    return DiagMatrixAtom(x)
end
Diagonal(x::AbstractExpr) = DiagMatrixAtom(x)

function conic_form!(x::DiagMatrixAtom, unique_conic_forms::UniqueConicForms=UniqueConicForms())
    if !has_conic_form(unique_conic_forms, x)
        sz = x.size[1]

        I = 1:sz+1:sz*sz
        J = 1:sz
        coeff = sparse(I, J, 1.0, sz * sz, sz)
        objective = conic_form!(x.children[1], unique_conic_forms)
        new_obj = coeff * objective
        cache_conic_form!(unique_conic_forms, x, new_obj)
    end
    return get_conic_form(unique_conic_forms, x)
end


"""
    `LineSegment(r1,r2)`

# ARGUMENTS
* `r1::Array{Float64}`        : Starting point of panel
* `r2::Array{Float64}`        : Ending point of panel

# PROPERTIES
* `r1::Array{Float64}`        : Starting point of segment
* `r2::Array{Float64}`        : Ending point of segment
* `L::Float64`                : Length of segment
* `theta::Float64`            : Angle of panel relative to global X-axis (2D ONLY)
* `n_hat::Array{Float64}`     : Normal vector (2D ONLY)
* `t_hat::Array{Float64}`     : Tangent vector
"""
mutable struct LineSegment

    # -- Properties --
    R1::Array{Float64,1}
    R2::Array{Float64,1}
    L::Float64
    theta::Float64
    n_hat::Array{Float64,1}
    t_hat::Array{Float64,1}

    # -- Constructor --
    function LineSegment(r1::Array{Float64,1},r2::Array{Float64,1})

        if length(r1) == 2
            x1, y1 = r1[1], r1[2]
            x2, y2 = r2[1], r2[2]

            L = sqrt((y2 - y1)^2 + (x2 - x1)^2)
            theta = atan((y2 - y1),(x2 - x1))

            t_hat = (r2 - r1)/norm(r2 - r1)
            n_hat = [t_hat[2],-t_hat[1]]

        elseif length(r1) == 3
            x1, y1, z1 = r1[1], r1[2], r1[3]
            x2, y2, z2 = r2[1], r2[2], r2[3]

            L = sqrt((z2 - z1)^2 + (y2 - y1)^2 + (x2 - x1)^2)
            theta = 0.0

            t_hat = (r2 - r1)/norm(r2 - r1)
            n_hat = [0.0, 0.0, 0.0]

        end

        new(r1,r2,L,theta,n_hat,t_hat)
    end
end


function simMats(newfmat, fmat)
    tol = 1e-8
    evA = eig(full(newfmat))
    evB = eig(full(fmat))
    aNz = find(abs(evA) > tol)
    bNz = find(abs(evB) > tol)
    diffLen = length(aNz) - length(bNz)

    if diffLen < 0
        res = norm(sort(real(evB(bNz))) - (sort(real([evA(aNz);zeros(-diffLen,1)]))))
    elseif diffLen == 0
        res = norm(sort(real(evB(bNz))) - (sort(real(evA(aNz)))));
    elseif diffLen > 0
        res = norm(sort(real([evB(bNz);zeros(diffLen,1)]))-sort(real(evA(aNz))))
    end

    return res
end


using CompScienceMeshes
using BEAST

meshfile = Pkg.dir("BEAST","test","sphere.in")
Γ = readmesh(meshfile)
X = raviartthomas(Γ)

els, ad = BEAST.assemblydata(X)

els2 = elements(geometry(X))
ad2 = assemblydata(X)

@assert ad2.data == ad.data
@assert length(els) == numcells(Γ)

resize!(X.fns, 1)

els, ad = BEAST.assemblydata(X)
@assert length(els) == 2
@assert size(ad.data) == (1,3,2)


using OptimPack
using Base.Test

include("rosenbrock.jl")


"This script implement a threads version for the Laguerre-Kravchuk function, so we can distribute the calculation over a multithreaded processor"

using PixelsGT # Call the parent module

# Splits the calculations on lower [0, 2j] and upper [2j, 4j] Laguerre-Kravchuk modes

"`rd(Q₁, Q₂, q₁, q₂, n, m, θ, j)` gives a tetradimensional array of lower modes"
rd(Q₁, Q₂, q₁, q₂, n, m, θ, j) = real(conj(LKd(Q₁, Q₂, n, m, j)) * exp(-1im * θ * m) * LKd(q₁, q₂, n, m, j))

"`ru(Q₁, Q₂, q₁, q₂, n, m, θ, j)` gives a tetradimensional array of upper modes"
ru(Q₁, Q₂, q₁, q₂, n, m, θ, j) = real(conj(LKu(Q₁, Q₂, n, m, j)) * exp(-1im * θ * m) * LKu(q₁, q₂, n, m, j))

export RdM, RuM

"`RdM(θ, j)` gives the twodimensional array of lower modes at angle θ for a j-dimensional representation"
function RdM(θ, j)
    m = zeros((Integer(2 * j + 1), Integer(2 * j + 1), Integer(2 * j + 1), Integer(2 * j + 1)))
    Threads.@threads  for Q₁ in 1:Integer(2*j+1)
        for Q₂ in 1:Integer(2*j+1)
            for q₁ in 1:Integer(2*j+1)
                for q₂ in 1:Integer(2*j+1)
                    m[Q₁, Q₂, q₁, q₂] = sum([rd(Q₁-j-1, Q₂-j-1, q₁-j-1, q₂-j-1, n, m, θ, j) for n in 0:Integer(2*j) for m in -Integer(n):2:Integer(n)])
                end
            end
        end
    end
    return m
end

"`RuM(θ, j)` gives the twodimensional array of lower modes at angle θ for a j-dimensional representation"
function RuM(θ, j)
    m = zeros((Integer(2 * j + 1), Integer(2 * j + 1), Integer(2 * j + 1), Integer(2 * j + 1)))
    Threads.@threads  for Q₁ in 1:Integer(2*j+1)
        for Q₂ in 1:Integer(2*j+1)
            for q₁ in 1:Integer(2*j+1)
                for q₂ in 1:Integer(2*j+1)
                    m[Q₁, Q₂, q₁, q₂] = sum([ru(Q₁-j-1, Q₂-j-1, q₁-j-1, q₂-j-1, n, m, θ, j) for n in Integer(2*j + 1):Integer(4*j) for m in -Integer(4 * j - n):2:Integer(4 * j - n)])
                end
            end
        end
    end
    return m
end


########## Fish ##########

mutable struct Fish
    cycle::UInt8
end

cycle(f::Fish) = f.cycle

function next_day!(fish::Fish)
    (spawn = cycle(fish) == 0x00) ? fish.cycle = 0x06 : fish.cycle -= 0x01
    return spawn
end

function next_day!(fishes::Vector{Fish})
    n_spawns = sum(next_day!.(fishes))
    append!(fishes, Fish.(0x08 * ones(UInt8, n_spawns)))
    return n_spawns
end

########## Script ##########

function main()

    # open file and store content into an input string
    input = open("inputs//day6.txt", "r") do io
        read(io, String)
    end

    # split input into a data vector
    data = String.(split(input, ","))

    # parse data as a vector of fishes
    fishes = Fish.(parse.(UInt8, data))

    # part 1
    count::BigInt = length(fishes)
    for _ = 1:80
        count += next_day!(fishes)
    end
    println("part 1: ", count)

    # part 2
    for _ = 81:256
        count += next_day!(fishes)
    end
    println("part 2: ", count)

end

main()


using PositiveFactorizations
using LinearAlgebra, Test

@testset "PositiveFactorizations" begin
for pivot in (Val{false}, Val{true})
    A = [1 0; 0 1]
    F = cholesky(Positive, A, pivot)
    @test Matrix(F) ≈ A
    F, d = ldlt(Positive, A, pivot)
    @test Matrix(F) ≈ A
    @test d == [1,1]

    A = [1 0; 0 -1]
    F = cholesky(Positive, A, pivot)
    @test Matrix(F) ≈ Matrix(1.0I,2,2)
    F, d = ldlt(Positive, A, pivot)
    @test Matrix(F) ≈ Matrix(1.0I,2,2)
    @test d == [1,-1]

    A = [-1 0.5; 0.5 4]
    target = pivot == Val{false} ? [1 -0.5; -0.5 4.5] : [1.125 0.5; 0.5 4]
    dtarget = pivot == Val{false} ? [-1,1] : [1,-1]
    F = cholesky(Positive, A, pivot)
    @test Matrix(F) ≈ target
    F = cholesky(Positive, 10*A, pivot)
    @test Matrix(F) ≈ 10*target
    F, d = ldlt(Positive, A, pivot)
    @test Matrix(F) ≈ target
    @test d == dtarget

    A = [0 1; 1 0]
    F = cholesky(Positive, A, pivot)
    @test Matrix(F) ≈ Matrix(1.0I,2,2)
    F, d = ldlt(Positive, A, pivot)
    @test Matrix(F) ≈ Matrix(1.0I,2,2)
    @test d == [0,0]

    A = rand(201,200); A = A'*A
    F = cholesky(Positive, A, pivot)
    @test Matrix(F) ≈ A
    F, d = ldlt(Positive, A, pivot)
    @test Matrix(F) ≈ A
    @test all(d .== 1)

    # factorization of (not too small) BigFloat matrices passes
    a = BigFloat.(1:15); A = a * a'
    F = cholesky(Positive, A, pivot)
    F, d = ldlt(Positive, A, pivot)
end

A = [1 0; 0 -2]
F = eigen(Positive, A)

# TODO: Use this when we drop v0.4 support
# @test Matrix(F) ≈ abs.(A)
absA = abs.(A)
absA = convert(Array{Int}, absA) # v0.4 fix
@test Matrix(F) ≈ absA

A = [1 0; 0 0]
F = eigen(Positive, A)
@test Matrix(F) ≈ Matrix(1.0I,2,2)

# Test whether necessary matrix operations are supported for SubArrays
n = PositiveFactorizations.default_blocksize(Float64)
B = rand(n+3,n+4); C = rand(size(B)...); A = B'*B - C'*C
ldlt!(Positive, A)
end   # @testset


# Copyright (c) 2018: Matthew Wilhelm & Matthew Stuber.
# This code is licensed under MIT license (see LICENSE.md for full details)
#############################################################################
# McCormick.jl
# A McCormick operator library in Julia
# See https://github.com/PSORLab/McCormick.jl
#############################################################################
# src/forward_operators/concave_increasing.jl
# Contains definitions of commonly used activation functions:
# relu, parametric relu, leaky relu, maxsig, elu, selu, gelu, sigmoid,
# bisigmoid, swish, maxtanh, pentanh, softsign, softplus.
#############################################################################

# RELU DEFINITION
#=
relu

The Rectified Linear Unit (ReLU) activation function `relu(x) = max(x, 0.0)`.
=#
relu_deriv(x) = x > 0.0 ? 1.0 : 0.0
relu_deriv2(x) =  0.0
function relu_kernel(x::MC{N,T}, z::Interval{Float64}) where {N, T<:Union{NS,MV}}
    max_kernel(x, 0.0, z)
end
function relu(x::MC{N,T}) where {N, T<:Union{NS,MV}}
    relu_kernel(x, max(x.Intv, 0.0))
end

# PARAMETRIC RELU DEFINITION
"""
param_relu

The parametric Rectified Linear Unit activation function `param_relu(x, α) = (max(x, αx)` with α in [0,1].
"""
param_relu(x, α) = x > 0.0 ? x : α*x
function param_relu(x::Interval{Float64}, α::Float64)
    xL = x.lo
    xU = x.hi
    (xL < 0.0) && (xL *= α)
    (xU < 0.0) && (xU *= α)
    return Interval{Float64}(xL, xU)
end
param_relu(x::Float64, α::Float64) = x > 0.0 ? x : α*x
param_relu_deriv(x::Float64, α::Float64) = x > 0.0 ? 1.0 : α

function param_relu_grad(g, x::Float64, α::Float64)
    if x > 0.0
        g[1] = 1.0
        g[2] = 0.0
    end
    g[1] = α
    g[2] = x
    nothing
end
function param_relu_kernel(x::MC{N,T}, α::Float64, z::Interval{Float64}) where {N, T<:Union{NS,MV}}
    @assert α >= 0.0
    xLc = z.lo
    xUc = z.hi
    xL = x.Intv.lo
    xU = x.Intv.hi
    midcv, cv_id = mid3(x.cc, x.cv, xL)
    midcc, cc_id = mid3(x.cc, x.cv, xU)
    dcc = (xUc > xLc) ? (xUc - xLc)/(xU - xL) : 0.0
    convex = param_relu(midcv, α)
    concave = dcc*(midcc - xL) + xLc
    concave_grad = mid_grad(x.cc_grad, x.cv_grad, cc_id)*dcc
    convex_grad = mid_grad(x.cc_grad, x.cv_grad, cv_id)*param_relu_deriv(midcv, α)
    convex, concave, convex_grad, concave_grad = cut(xLc, xUc, convex, concave, convex_grad, concave_grad)
    return MC{N, T}(convex, concave, z, convex_grad, concave_grad, x.cnst)
end
@inline param_relu(x::MC, α::Float64) = param_relu_kernel(x, α, param_relu(x.Intv, α))

# LEAKY RELU DEFINITION
"""
leaky_relu

The leaky Rectified Linear Unit activation function `leaky_relu(x) = max(x, 0.01x)`.
"""
@inline leaky_relu(x) = param_relu(x, 0.01)
@inline leaky_relu_kernel(x::MC, z::Interval{Float64}) = param_relu_kernel(x, 0.01, z)
@inline leaky_relu(x::MC) = leaky_relu_kernel(x, param_relu(x.Intv, 0.01))
@inline leaky_relu_deriv(x) = x > 0.0 ? 1.0 : 0.01
@inline leaky_relu_deriv2(x) = 0.0

# DEFINE MAXSIG
"""
maxsig

The `maxsig` activation function  `maxsig(x) = max(x, 1.0/(1.0 + exp(-x)))`.
"""
@inline maxsig(x) = max(x, 1.0/(1.0 + exp(-x)))
@inline maxsig(x::Float64) = max(x, 1.0/(1.0 + exp(-x)))
@inline maxsig(x::Interval{Float64}) = max(x, 1.0/(1.0 + exp(-x)))
@inline function maxsig_deriv(x::Float64)
    if x > 1.0/(exp(-x) + 1.0)
        return 1.0
    end
    return exp(-x)/(exp(-x) + 1.0)^2
end
@inline function maxsig_deriv2(x::Float64)
    if x > 1.0/(exp(-x) + 1.0)
        return 0.0
    end
    return exp(x)*(1.0 - exp(x))/(exp(x) + 1.0)^3 #
end
function maxsig_kernel(x::MC{N,T}, z::Interval{Float64}) where {N, T<:Union{NS,MV}}
    xLc = z.lo
    xUc = z.hi
    xL = x.Intv.lo
    xU = x.Intv.hi
    midcv, cv_id = mid3(x.cc, x.cv, xL)
    midcc, cc_id = mid3(x.cc, x.cv, xU)
    dcc = (xUc > xLc) ? (xUc - xLc)/(xU - xL) : 0.0
    convex = maxsig(midcv)
    concave = dcc*(midcc - xL) + xLc
    concave_grad = mid_grad(x.cc_grad, x.cv_grad, cc_id)*dcc
    convex_grad = mid_grad(x.cc_grad, x.cv_grad, cv_id)*maxsig_deriv(midcv)
    convex, concave, convex_grad, concave_grad = cut(xLc, xUc, convex, concave, convex_grad, concave_grad)
    return MC{N, T}(convex, concave, z, convex_grad, concave_grad, x.cnst)
end
maxsig(x::MC{N,T}) where {N, T<:Union{NS,MV}} = maxsig_kernel(x, maxsig(x.Intv))

# DEFINE ELU
#=
elu

The Exponential Linear Unit (ELU) activation function  `elu(x, α) = x > 0 ? x : α*(exp(x) - 1.0)`.
=#
function elu(x::Interval{Float64}, α::Float64)
    xL = x.lo
    xU = x.hi
    if xU < 0.0
        return α*(exp(x) - 1.0)
    elseif xL > 0.0
        return x
    end
    xLIntv = α*(exp(x) - 1.0)
    Interval(xLIntv.lo, x.hi)
end
@inline elu_deriv(x::Float64, α::Float64) = x > 0.0 ? 1.0 : α*exp(x)
function elu_grad(g, x::Float64, α::Float64)
    g[1] = elu_deriv(x, α)
    g[2] = x > 0.0 ? 0.0 : exp(x)
    nothing
end
function elu_kernel(x::MC{N,T}, α::Float64, z::Interval{Float64}) where {N, T<:Union{NS,MV}}
    xLc = z.lo
    xUc = z.hi
    xL = x.Intv.lo
    xU = x.Intv.hi
    midcv, cv_id = mid3(x.cc, x.cv, xL)
    midcc, cc_id = mid3(x.cc, x.cv, xU)
    dcc = (xUc > xLc) ? (xUc - xLc)/(xU - xL) : 0.0
    convex = elu(midcv, α)
    concave = dcc*(midcc - xL) + xLc
    concave_grad = mid_grad(x.cc_grad, x.cv_grad, cc_id)*dcc
    convex_grad = mid_grad(x.cc_grad, x.cv_grad, cv_id)*elu_deriv(midcv, α)
    convex, concave, convex_grad, concave_grad = cut(xLc, xUc, convex, concave, convex_grad, concave_grad)
    return MC{N,T}(convex, concave, z, convex_grad, concave_grad, x.cnst)
end
elu(x::MC{N,T}, α::Float64) where {N, T<:Union{NS,MV}} = elu_kernel(x, α, elu(x.Intv, α))

"""
selu

The Scaled Exponential Linear Unit (SELU) activation function  `selu(x, α, λ) = λ*elu(x, α)`.
"""
selu(x, α, λ) = λ*elu(x, α)
function selu_grad(g, x::Float64, α::Float64, λ::Float64)
    g[1] = λ*elu_deriv(x, α)
    g[2] = λ*(x > 0.0 ? 0.0 : exp(x))
    g[3] = elu(x, α)
    nothing
end

"""
maxtanh

The `maxtanh` activation function  `maxtanh(x) = max(x, tanh(x))`.
"""
@inline maxtanh(x) = max(x, tanh(x))
@inline maxtanh(x::Float64) = max(x, tanh(x))
@inline function maxtanh(x::Interval{Float64})
    xLintv = Interval(x.lo)
    xUintv = Interval(x.hi)
    xLc = max(xLintv, tanh(xLintv))
    xUc = max(xUintv, tanh(xUintv))
    Interval(xLc.lo, xUc.hi)
end
@inline function maxtanh_deriv(x::Float64)
    if x > tanh(x)
        return 1.0
    end
    return sech(x)^2
end
@inline function maxtanh_deriv2(x::Float64)
    if x > tanh(x)
        return 0.0
    end
    return -2.0*tanh(x)*sech(x)^2
end
function maxtanh_kernel(x::MC{N,T}, z::Interval{Float64}) where {N, T<:Union{NS,MV}}
    xLc = z.lo
    xUc = z.hi
    xL = x.Intv.lo
    xU = x.Intv.hi
    midcv, cv_id = mid3(x.cc, x.cv, xL)
    midcc, cc_id = mid3(x.cc, x.cv, xU)
    dcc = (xUc > xLc) ? (xUc - xLc)/(xU - xL) : 0.0
    convex = maxtanh(midcv)
    concave = dcc*(midcc - xL) + xLc
    concave_grad = mid_grad(x.cc_grad, x.cv_grad, cc_id)*dcc
    convex_grad = mid_grad(x.cc_grad, x.cv_grad, cv_id)*maxtanh_deriv(midcv)
    convex, concave, convex_grad, concave_grad = cut(xLc, xUc, convex, concave, convex_grad, concave_grad)
    return MC{N, T}(convex, concave, z, convex_grad, concave_grad, x.cnst)
end
maxtanh(x::MC{N,T}) where {N, T<:Union{NS,MV}} = maxtanh_kernel(x, maxtanh(x.Intv))

#=
softplus

The `softplus` activation function  `softplus(x) = log(1.0 + exp(x))`.
=#
@inline softplus(x::Interval{Float64}) = log(1.0 + exp(x))
@inline softplus_deriv(x::Float64) = 1.0/(exp(-x) + 1.0)
@inline softplus_deriv2(x::Float64) = exp(-x)/(exp(-x) + 1.0)^2
function softplus_kernel(x::MC{N,T}, z::Interval{Float64}) where {N, T<:Union{NS,MV}}
    xLc = z.lo
    xUc = z.hi
    xL = x.Intv.lo
    xU = x.Intv.hi
    midcv, cv_id = mid3(x.cc, x.cv, xL)
    midcc, cc_id = mid3(x.cc, x.cv, xU)
    dcc = (xUc > xLc) ? (xUc - xLc)/(xU - xL) : 0.0
    convex = softplus(midcv)
    concave = dcc*(midcc - xL) + xLc
    concave_grad = mid_grad(x.cc_grad, x.cv_grad, cc_id)*dcc
    convex_grad = mid_grad(x.cc_grad, x.cv_grad, cv_id)*softplus_deriv(midcv)
    convex, concave, convex_grad, concave_grad = cut(xLc, xUc, convex, concave, convex_grad, concave_grad)
    return MC{N, T}(convex, concave, z, convex_grad, concave_grad, x.cnst)
end
softplus(x::MC{N,T}) where {N, T<:Union{NS,MV}} = softplus_kernel(x, softplus(x.Intv))

"""
pentanh

The `pentanh` activation function `pentanh(x) = x > 0.0 ? tanh(x) : tanh(0.25*x)`.
"""
@inline pentanh(x) = x > 0.0 ? tanh(x) : tanh(0.25*x)
@inline pentanh(x::Float64) = x > 0.0 ? tanh(x) : tanh(0.25*x)
function pentanh(x::Interval{Float64})
    (x.lo >= 0.0) && return tanh(x)
    (x.hi <= 0.0) && return tanh(0.25*x)
    lo_part = tanh(0.25*x)
    hi_part = tanh(x)
    Interval(lo_part.lo, hi_part.hi)
end
function pentanh_deriv(x::Float64)
    if x > 0.0
        return 1.0 - tanh(x)^2
    end
    0.25 - 0.25*tanh(0.25*x)^2
end
function pentanh_deriv2(x::Float64)
    if x > 0.0
        return -2.0*tanh(x)*sech(2)^2
    end
    -0.125*tanh(0.25*x)*sech(0.25*x)^2
end
@inline function pentanh_env(x::Float64, y::Float64, z::Float64)
    (x - y) - (pentanh(x) - pentanh(y))/pentanh_deriv(x)
end
@inline function cv_pentanh(x::Float64, xL::Float64, xU::Float64, p::Float64)
    (xL >= 0.0) && (return dline_seg(pentanh, pentanh_deriv, x, xL, xU)..., p)
    (xU <= 0.0) && (return pentanh(x), pentanh_deriv(x), p)
    if p == Inf
        p, flag = secant(xL, 0.0, xL, 0.0, pentanh_env, xU, 0.0)
        flag && (p = golden_section(xL, 0.0, pentanh_env, xU, 0.0))
    end
    (x <= p) && (return pentanh(x), pentanh_deriv(x), p)
    return dline_seg(pentanh, pentanh_deriv, x, p, xU)..., p
end
@inline function cc_pentanh(x::Float64, xL::Float64, xU::Float64, p::Float64)
    (xL >= 0.0) && (return pentanh(x), pentanh_deriv(x), p)
    (xU <= 0.0) && (return dline_seg(pentanh, pentanh_deriv, x, xL, xU)..., p)
    if p == Inf
        p, flag = secant(0.0, xU, 0.0, xU, pentanh_env, xL, 0.0)
        flag && (p = golden_section(0.0, xU, pentanh_env, xL, 0.0))
    end
    (x <= p) && (return dline_seg(pentanh, pentanh_deriv, x, xL, p)..., p)
    return pentanh(x), pentanh_deriv(x), p
end

#=
sigmoid

The `sigmoid` activation function `sigmoid(x) = 1.0/(1.0 + exp(-x))`.
=#
@inline sigmoid(x::Interval{Float64}) = 1.0/(1.0 + exp(-x))
@inline sigmoid_deriv(x::Float64) = exp(-x)/(1.0 + exp(-x))^2
@inline sigmoid_deriv2(x::Float64) = -exp(x)*(exp(x) - 1.0)/(exp(x) + 1.0)^3
@inline function sigmoid_env(x::Float64, y::Float64, z::Float64)
    (x - y)*sigmoid_deriv(x) - (sigmoid(x) - sigmoid(y))
end
@inline function cv_sigmoid(x::Float64, xL::Float64, xU::Float64, p::Float64)
    (xL >= 0.0) && (return dline_seg(sigmoid, sigmoid_deriv, x, xL, xU)..., p)
    (xU <= 0.0) && (return sigmoid(x), sigmoid_deriv(x), p)
    if p == Inf
        p, flag = secant(xL, 0.0, xL, 0.0, sigmoid_env, xU, 0.0)
        flag && (p = golden_section(xL, 0.0, sigmoid_env, xU, 0.0))
    end
    (x <= p) && (return sigmoid(x), sigmoid_deriv(x), p)
    return dline_seg(sigmoid, sigmoid_deriv, x, p, xU)..., p
end
@inline function cc_sigmoid(x::Float64, xL::Float64, xU::Float64, p::Float64)
    (xL >= 0.0) && (return sigmoid(x), sigmoid_deriv(x), p)
    (xU <= 0.0) && (return dline_seg(sigmoid, sigmoid_deriv, x, xL, xU)..., p)
    if p == Inf
        p, flag = secant(0.0, xU, 0.0, xU, sigmoid_env, xL, 0.0)
        flag && (p = golden_section(0.0, xU, sigmoid_env, xL, 0.0))
    end
    (x <= p) && (return dline_seg(sigmoid, sigmoid_deriv, x, xL, p)..., p)
    return sigmoid(x), sigmoid_deriv(x), p
end

"""
bisigmoid

The `bisigmoid` activation function `bisigmoid(x) = (1.0 - exp(-x))/(1.0 + exp(-x))`.
"""
@inline bisigmoid(x) = (1.0 - exp(-x))/(1.0 + exp(-x))
@inline bisigmoid(x::Float64) = (1.0 - exp(-x))/(1.0 + exp(-x))
@inline function bisigmoid(x::Interval{Float64})
    xLintv = Interval(x.lo)
    xUintv = Interval(x.hi)
    xLc = (1.0 - exp(-xLintv))/(1.0 + exp(-xLintv))
    xUc = (1.0 - exp(-xUintv))/(1.0 + exp(-xUintv))
    return Interval(xLc.hi, xUc.hi)
end
@inline bisigmoid_deriv(x::Float64) = 2.0*exp(x)/(exp(x) + 1.0)^2
@inline function bisigmoid_deriv2(x::Float64)
    term1 = exp(-x)/(exp(-x) + 1.0)
    term2 = 2.0*exp(-2.0*x)/(exp(-x) + 1.0)^2
    term3 = (1.0 - exp(-x))*(term2 - term1)/(exp(-x) + 1.0)
    return term3 - term1 + term2
end
@inline function bisigmoid_env(x::Float64, y::Float64, z::Float64)
    bisigmoid(y) - bisigmoid(x) - bisigmoid_deriv(x)*(y - x)
end
@inline function cv_bisigmoid(x::Float64, xL::Float64, xU::Float64, p::Float64)
    (xL >= 0.0) && (return dline_seg(bisigmoid, bisigmoid_deriv, x, xL, xU)..., p)
    (xU <= 0.0) && (return bisigmoid(x), bisigmoid_deriv(x), p)
    if p === Inf
        p, flag = secant(xL, 0.0, xL, 0.0, bisigmoid_env, xU, 0.0)
        flag && (p = golden_section(xL, 0.0, bisigmoid_env, xU, 0.0))
    end
    (x <= p) && (return bisigmoid(x), bisigmoid_deriv(x), p)
    return dline_seg(bisigmoid, bisigmoid_deriv, x, p, xU)..., p
end
@inline function cc_bisigmoid(x::Float64, xL::Float64, xU::Float64, p::Float64)
    (xL >= 0.0) && (return bisigmoid(x), bisigmoid_deriv(x), p)
    (xU <= 0.0) && (return dline_seg(bisigmoid, bisigmoid_deriv, x, xL, xU)..., p)
    if p === Inf
        p, flag = secant(0.0, xU, 0.0, xU, bisigmoid_env, xL, 0.0)
        flag && (p = golden_section(0.0, xU, bisigmoid_env, xL, 0.0))
    end
    (x <= p) && (return dline_seg(bisigmoid, bisigmoid_deriv, x, xL, p)..., p)
    return bisigmoid(x), bisigmoid_deriv(x), p
end

#=
softsign

The `softsign` activation function `softsign(x) = x/(1.0 + abs(x))`.
=#
@inline function softsign(x::Interval{Float64})
    xLintv = Interval(x.lo)
    xUintv = Interval(x.hi)
    xLc = xLintv/(1.0 + abs(xLintv))
    xUc = xUintv/(1.0 + abs(xUintv))
    return Interval(xLc.hi, xUc.hi)
end
@inline softsign_deriv(x::Float64) = 1.0/(1.0 + abs(x))^2
@inline softsign_deriv2(x::Float64) = -2.0*x/(abs(x)*(1.0 + abs(x))^(3))
@inline function softsign_env(x::Float64, y::Float64, z::Float64)
    (x - y)*softsign_deriv(x) - (softsign(x) - softsign(y))
end
@inline function cv_softsign(x::Float64, xL::Float64, xU::Float64, p::Float64)
    (xL >= 0.0) && (return dline_seg(softsign, softsign_deriv, x, xL, xU)..., p)
    (xU <= 0.0) && (return softsign(x), softsign_deriv(x), p)
    if p === Inf
        p, flag = secant(xL, 0.0, xL, 0.0, softsign_env, xU, 0.0)
        flag && (p = golden_section(xL, 0.0, softsign_env, xU, 0.0))
    end
    (x <= p) && (return softsign(x), softsign_deriv(x), p)
    return dline_seg(softsign, softsign_deriv, x, p, xU)..., p
end
@inline function cc_softsign(x::Float64, xL::Float64, xU::Float64, p::Float64)
    (xL >= 0.0) && (return softsign(x), softsign_deriv(x), p)
    (xU <= 0.0) && (return dline_seg(softsign, softsign_deriv, x, xL, xU)..., p)
    if p === Inf
        p, flag = secant(0.0, xU, 0.0, xU, softsign_env, xL, 0.0)
        flag && (p = golden_section(0.0, xU, softsign_env, xL, 0.0))
    end
    (x <= p) && (return dline_seg(softsign, softsign_deriv, x, xL, p)..., p)
    return softsign(x), softsign_deriv(x), p
end

const GELU_MIN = -0.751791524693564457457904946779522
const GELU_2D_ROOT1 = -sqrt(2)
const GELU_2D_ROOT2 = sqrt(2)

const SWISH1_MIN = -1.27846454276107379510935873902298015543947748
const SWISH1_2D_ROOT1 = -2.399357280515467667832739697282283888523
const SWISH1_2D_ROOT2 = 2.399357280515467667832739697282283888523

"""
gelu

The Gaussian Error Linear Unit `gelu` activation function `gelu(x) = x/(1.0 + abs(x))`.
"""
@inline function gelu(x::Interval{Float64})
    xLintv = Interval(x.lo)
    xUintv = Interval(x.hi)
    xLc = xLintv*(1.0 + erf(xLintv/sqrt(2)))/2.0
    xUc = xUintv*(1.0 + erf(xUintv/sqrt(2)))/2.0
    if x.hi < GELU_MIN
        xLcv = xUc.lo
        xUcv = xLc.hi
    elseif GELU_MIN < x.lo
        xLcv = xLc.lo
        xUcv = xUc.hi
    else
        xLcv = GELU_MIN
        xUcv = max(xLc.hi, xUc.hi)
    end
    return Interval(xLcv, xUcv)
end
function gelu_deriv(x::Float64)
    0.5*(1.0 + erf(x/sqrt(2))) + (x/sqrt(2*pi))*exp((-x^2)/2.0)
end
function gelu_deriv2(x::Float64)
    (0.797885 - 0.398942*x^2)*exp((-x^2)/2.0)
end
function gelu_env(x::Float64, y::Float64, z::Float64)
    (y - x)*gelu_deriv(x) + gelu(x) - gelu(y)
end
function gelu_denv(x::Float64, y::Float64, z::Float64)
    (y - x)*gelu_deriv2(x)
end

@inline function gelu_envm(x::Float64, y::Float64, z::Float64)
    gelu_deriv(y)*(x - y) - (gelu(x) - gelu(y))
end

@inline function gelu_rt1(x::Float64, y::Float64, z::Float64)
    gelu_deriv(y)*(x - y) + gelu(y) -gelu(x)
end

@inline function gelu_rt1_deriv(x::Float64, y::Float64, z::Float64)
    gelu_deriv(y) - gelu_deriv(x)
end

@inline function cc_gelu(x::Float64, xL::Float64, xU::Float64, p1::Float64, p2::Float64)
    # Single convexity regions
    if xL >= GELU_2D_ROOT2
        return gelu(x), gelu_deriv(x), p1, p2
    elseif xU <= GELU_2D_ROOT1
        return gelu(x), gelu_deriv(x), p1, p2
    elseif (GELU_2D_ROOT1 <= xL) && (xU <= GELU_2D_ROOT2)
        return dline_seg(gelu, gelu_deriv, x, xL, xU)..., p1, p2
    end

    if xL < GELU_2D_ROOT1
        p2, flag = newton(0.0, GELU_MIN, 0.0, gelu_rt1, gelu_rt1_deriv, xL, 0.0)
        flag && (p2 = golden_section(GELU_2D_ROOT1, 0.0, gelu_rt1, xL, 0.0))
        if p2 > xU
            if p1 === Inf
                p1, flag = secant(xL, GELU_2D_ROOT1, xL, GELU_2D_ROOT1, gelu_env, xU, 0.0)
                flag && (p1 = golden_section(xL, GELU_2D_ROOT1, gelu_env, xU, 0.0))
            end
            (x >= p1) && (return dline_seg(gelu, gelu_deriv, x, p1, xU)..., p1, p2)
            return gelu(x), gelu_deriv(x), p1, p2
        else
            return dline_seg(gelu, gelu_deriv, x, xL, xU)..., p1, p2
        end
    end

    if xL > GELU_2D_ROOT1
        p2, flag = newton(0.5*(GELU_MIN + GELU_2D_ROOT2), GELU_MIN, GELU_2D_ROOT2, gelu_rt1, gelu_rt1_deriv, xU, 0.0)
        flag && (p2 = golden_section(GELU_MIN, GELU_2D_ROOT2, gelu_rt1, xU, 0.0))
        if p2 < xL
            if p1 === Inf
                p1, flag = secant(GELU_2D_ROOT2, xU, GELU_2D_ROOT2, xU, gelu_env, xL, 0.0)
                flag && (p1 = golden_section(GELU_2D_ROOT2, xU, gelu_env, xL, 0.0))
            end
            (x >= p1) && (return dline_seg(gelu, gelu_deriv, x, p1, xU)..., p1, p2)
            return gelu(x), gelu_deriv(x), p1, p2
        else
            return dline_seg(gelu, gelu_deriv, x, xL, xU)..., p1, p2
        end
    end
end
@inline function cv_gelu(x::Float64, xL::Float64, xU::Float64, p1::Float64, p2::Float64)

    # Single convexity regions
    if xL >= GELU_2D_ROOT2
        return dline_seg(gelu, gelu_deriv, x, xL, xU)..., p1, p2
    elseif xU <= GELU_2D_ROOT1
        return dline_seg(gelu, gelu_deriv, x, xL, xU)..., p1, p2
    elseif (GELU_2D_ROOT1 <= xL) && (xU <= GELU_2D_ROOT2)
        return gelu(x), gelu_deriv(x), p1, p2
    end

    if xL < GELU_2D_ROOT1
        if p1 === Inf
            p1, flag = newton(0.5*(GELU_2D_ROOT1 + GELU_MIN), GELU_2D_ROOT1, GELU_MIN, gelu_env, gelu_denv, xL, 0.0)
            flag && (p1 = golden_section(GELU_2D_ROOT1, GELU_MIN, gelu_env, xL, 0.0))
        end
    else
        p1 = -Inf
    end

    if xU > GELU_2D_ROOT2
        if p2 === Inf
            p2, flag = newton(0.5*(GELU_MIN + GELU_2D_ROOT2), GELU_MIN, GELU_2D_ROOT2, gelu_env, gelu_denv, xU, 0.0)
            flag && (p2 = golden_section(GELU_MIN, GELU_2D_ROOT2, gelu_env, xU, 0.0))
        end
    else
        p2 = Inf
    end

    if x < p1
        return dline_seg(gelu, gelu_deriv, x, xL, p1)..., p1, p2
    elseif x > p2
        return dline_seg(gelu, gelu_deriv, x, p2, xU)..., p1, p2
    end
    return gelu(x), gelu_deriv(x), p1, p2
end

"""
swish

The Swish-1 activation function `swish(x) = x/(1.0 + exp(-x))`.
"""
@inline function swish(x::Interval{Float64})
    xLintv = Interval(x.lo)
    xUintv = Interval(x.hi)
    xLc = xLintv/(1.0 + exp(-xLintv))
    xUc = xUintv/(1.0 + exp(-xUintv))
    if x.hi < SWISH1_MIN
        xLcv = xUc.lo
        xUcv = xLc.hi
    elseif SWISH1_MIN < x.lo
        xLcv = xLc.lo
        xUcv = xUc.hi
    else
        xmin = Interval(SWISH1_MIN)
        xminI = xmin/(1.0 + exp(-xmin))
        xLcv = xminI.lo
        xUcv = max(xLc.hi, xUc.hi)
    end
    return Interval(xLcv, xUcv)
end
@inline function swish_deriv(x::Float64)
    z = 1.0/(1.0 + exp(-x))
    swish(x) + z*(1-swish(x))
end
@inline function swish_deriv2(x::Float64)
    z = 1.0/(1.0 + exp(-x))
    dz = exp(-x)/(1.0 + exp(-x))^2
    swish_deriv(x) + dz*(1.0 - swish(x)) - z*swish_deriv(x)
end
@inline function swish_env(x::Float64, y::Float64, z::Float64)
    swish_deriv(x)*(y - x) + swish(x) - swish(y)
end
@inline function swish_denv(x::Float64, y::Float64, z::Float64)
    swish_deriv2(x)*(y - x)
end

@inline function swish_envm(x::Float64, y::Float64, z::Float64)
    swish_deriv(y)*(x - y) - (swish(x) - swish(y))
end

@inline function swish_rt1(x::Float64, y::Float64, z::Float64)
    swish_deriv(y)*(x - y) + swish(y) - swish(x)
end
@inline function swish_rt1_deriv(x::Float64, y::Float64, z::Float64)
    swish_deriv(y) - swish_deriv(x)
end

@inline function cc_swish(x::Float64, xL::Float64, xU::Float64, p1::Float64, p2::Float64)
    # Single convexity regions
    if xL >= SWISH1_2D_ROOT2
        return swish(x), swish_deriv(x), p1, p2
    elseif xU <= SWISH1_2D_ROOT1
        return swish(x), swish_deriv(x), p1, p2
    elseif (SWISH1_2D_ROOT1 <= xL) && (xU <= SWISH1_2D_ROOT2)
        return dline_seg(swish, swish_deriv, x, xL, xU)..., p1, p2
    end

    if xL < SWISH1_2D_ROOT1
        p2, flag = newton(0.0, SWISH1_MIN, 0.0, swish_rt1, swish_rt1_deriv, xL, 0.0)
        flag && (p2 = golden_section(SWISH1_2D_ROOT1, 0.0, swish_rt1, xL, 0.0))
        if p2 > xU
            if p1 === Inf
                p1, flag = secant(xL, SWISH1_2D_ROOT1, xL, SWISH1_2D_ROOT1, swish_env, xU, 0.0)
                flag && (p1 = golden_section(xL, SWISH1_2D_ROOT1, swish_env, xU, 0.0))
            end
            (x >= p1) && (return dline_seg(swish, swish_deriv, x, p1, xU)..., p1, p2)
            return swish(x), swish_deriv(x), p1, p2
        else
            return dline_seg(swish, swish_deriv, x, xL, xU)..., p1, p2
        end
    end

    if xL > SWISH1_2D_ROOT1
        p2, flag = newton(0.5*(SWISH1_MIN + SWISH1_2D_ROOT2), SWISH1_MIN, SWISH1_2D_ROOT2, swish_rt1, swish_rt1_deriv, xU, 0.0)
        flag && (p2 = golden_section(SWISH1_MIN, SWISH1_2D_ROOT2, swish_rt1, xU, 0.0))
        if p2 < xL
            if p1 === Inf
                p1, flag = secant(SWISH1_2D_ROOT2, xU, SWISH1_2D_ROOT2, xU, swish_env, xL, 0.0)
                flag && (p1 = golden_section(SWISH1_2D_ROOT2, xU, swish_env, xL, 0.0))
            end
            (x >= p1) && (return dline_seg(swish, swish_deriv, x, p1, xU)..., p1, p2)
            return swish(x), swish_deriv(x), p1, p2
        else
            return dline_seg(swish, swish_deriv, x, xL, xU)..., p1, p2
        end
    end
end
@inline function cv_swish(x::Float64, xL::Float64, xU::Float64, p1::Float64, p2::Float64)

    # Single convexity regions
    if xL >= SWISH1_2D_ROOT2
        return dline_seg(swish, swish_deriv, x, xL, xU)..., p1, p2
    elseif xU <= SWISH1_2D_ROOT1
        return dline_seg(swish, swish_deriv, x, xL, xU)..., p1, p2
    elseif (SWISH1_2D_ROOT1 <= xL) && (xU <= SWISH1_2D_ROOT2)
        return swish(x), swish_deriv(x), p1, p2
    end

    if xL < SWISH1_2D_ROOT1
        if p1 === Inf
            p1, flag = newton(0.5*(SWISH1_2D_ROOT1 + SWISH1_MIN), SWISH1_2D_ROOT1, SWISH1_MIN, swish_env, swish_denv, xL, 0.0)
            flag && (p1 = golden_section(SWISH1_2D_ROOT1, SWISH1_MIN, swish_env, xL, 0.0))
        end
    else
        p1 = -Inf
    end

    if xU > SWISH1_2D_ROOT2
        if p2 === Inf
            p2, flag = newton(0.5*(SWISH1_MIN + SWISH1_2D_ROOT2), SWISH1_MIN, SWISH1_2D_ROOT2, swish_env, swish_denv, xU, 0.0)
            flag && (p2 = golden_section(SWISH1_MIN, SWISH1_2D_ROOT2, swish_env, xU, 0.0))
        end
    else
        p2 = Inf
    end

    if x < p1
        return dline_seg(swish, swish_deriv, x, xL, p1)..., p1, p2
    elseif x > p2
        return dline_seg(swish, swish_deriv, x, p2, xU)..., p1, p2
    end
    return swish(x), swish_deriv(x), p1, p2
end

# define kernel and operator for sigmoid, bisigmoid, softsign, gelu
for expri in (:pentanh, :sigmoid, :bisigmoid, :softsign)
    expri_cv = Symbol("cv_"*String(expri))
    expri_cc = Symbol("cc_"*String(expri))
    expri_kernel = Symbol(String(expri)*"_kernel")
    eps_min = :xL
    eps_max = :xU
    @eval @inline function ($expri_kernel)(x::MC{N, T}, y::Interval{Float64},
                            cv_p::Float64, cc_p::Float64) where {N,T<:Union{NS,MV}}
        xL = x.Intv.lo
        xU = x.Intv.hi
        midcv, cv_id = mid3(x.cc, x.cv, $eps_min)
        midcc, cc_id = mid3(x.cc, x.cv, $eps_max)
        cv, dcv, cv_p = $(expri_cv)(midcv, xL, xU, cv_p)
        cc, dcc, cc_p = $(expri_cc)(midcc, xL, xU, cc_p)
        cv_grad = mid_grad(x.cc_grad, x.cv_grad, cv_id)*dcv
        cc_grad = mid_grad(x.cc_grad, x.cv_grad, cc_id)*dcc
        cv, cc, cv_grad, cc_grad = cut(y.lo, y.hi, cv, cc, cv_grad, cc_grad)
        return MC{N, T}(cv, cc, y, cv_grad, cc_grad, x.cnst), cv_p, cc_p
    end
    @eval @inline function ($expri)(x::MC{N,T}) where {N, T<:RelaxTag}
        z, tp1, tp2 = ($expri_kernel)(x, ($expri)(x.Intv), Inf, Inf)
        return z
    end
end

for expri in (:swish, :gelu)
    expri_cv = Symbol("cv_"*String(expri))
    expri_cc = Symbol("cc_"*String(expri))
    expri_kernel = Symbol(String(expri)*"_kernel")
    if expri == swish
        eps_min = :(SWISH1_MIN < xL ? xL : (SWISH1_MIN > xU ? xU : SWISH1_MIN))
        eps_max = :(swish(xL) < swish(xU) ? xU : xL)
    else
        eps_min = :(GELU_MIN > xU ? xU : (GELU_MIN < xL ? xL : GELU_MIN))
        eps_max = :(gelu(xL) < gelu(xU) ? xU : xL)
    end
    @eval @inline function ($expri_kernel)(x::MC{N, T}, y::Interval{Float64},
                            cv_p::Float64, cc_p::Float64, cv_p2::Float64,
                            cc_p2::Float64) where {N,T<:Union{NS,MV}}
        xL = x.Intv.lo
        xU = x.Intv.hi
        midcv, cv_id = mid3(x.cc, x.cv, $eps_min)
        midcc, cc_id = mid3(x.cc, x.cv, $eps_max)
        cv, dcv, cv_p, cv_p2 = $(expri_cv)(midcv, xL, xU, cv_p, cv_p2)
        cc, dcc, cc_p, cc_p2 = $(expri_cc)(midcc, xL, xU, cc_p, cc_p2)
        cv_grad = mid_grad(x.cc_grad, x.cv_grad, cv_id)*dcv
        cc_grad = mid_grad(x.cc_grad, x.cv_grad, cc_id)*dcc
        cv, cc, cv_grad, cc_grad = cut(y.lo, y.hi, cv, cc, cv_grad, cc_grad)
        return MC{N, T}(cv, cc, y, cv_grad, cc_grad, x.cnst), cv_p, cc_p, cv_p2, cc_p2
    end
    @eval @inline function ($expri)(x::MC{N,T}) where {N, T<:RelaxTag}
        z, tp1, tp2, tp3, tp4 = ($expri_kernel)(x, ($expri)(x.Intv), Inf, Inf, Inf, Inf)
        return z
    end
end

"""
logcosh

The function `logcosh` is defined as `logcosh(x) = log(cosh(x))`.
"""
logcosh_deriv(x::Float64) = tanh(x)
logcosh_deriv2(x::Float64) = sech(x)^2
cc_logcosh(x::Float64, xL::Float64, xU::Float64) = dline_seg(logcosh, logcosh_deriv, x, xL, xU)
cv_logcosh(x::Float64, xL::Float64, xU::Float64) = logcosh(x), logcosh_deriv(x)
function logcosh(x::Interval{Float64})
    (abs(x.lo) < abs(x.hi)) ? (logcosh_xU = log(cosh(Interval(x.hi)))) : (logcosh_xU = log(cosh(Interval(x.lo))))
    logcosh_xL = Interval(0.0)
    (x.lo >= 0.0) && (logcosh_xL = log(cosh(Interval(x.lo))))
    (x.hi <= 0.0) && (logcosh_xL = log(cosh(Interval(x.hi))))
    Interval{Float64}(logcosh_xL.lo, logcosh_xU.hi)
end
@inline function logcosh_kernel(x::MC{N, T}, y::Interval{Float64}) where {N,T<:Union{NS,MV}}
    xL = x.Intv.lo
    xU = x.Intv.hi
    eps_min = in(0.0, x) ? 0.0 : (xU <= 0.0 ? xU : xL)
    eps_max = (abs(xL) < abs(xU)) ? xU : xL
    midcv, cv_id = mid3(x.cc, x.cv, eps_min)
    midcc, cc_id = mid3(x.cc, x.cv, eps_max)
    cv, dcv = cv_logcosh(midcv, xL, xU)
    cc, dcc = cc_logcosh(midcc, xL, xU)
    cv_grad = mid_grad(x.cc_grad, x.cv_grad, cv_id)*dcv
    cc_grad = mid_grad(x.cc_grad, x.cv_grad, cc_id)*dcc
    cv, cc, cv_grad, cc_grad = cut(y.lo, y.hi, cv, cc, cv_grad, cc_grad)
    return MC{N,T}(cv, cc, y, cv_grad, cc_grad, x.cnst)
end
@inline function logcosh_kernel(x::MC{N, T}, y::Interval{Float64}) where {N,T<:Diff}
    xL = x.Intv.lo
    xU = x.Intv.hi
    eps_min = in(0.0, x) ? 0.0 : (xU <= 0.0 ? xU : xL)
    eps_max = (abs(xL) < abs(xU)) ? xU : xL
    midcv, cv_id = mid3(x.cc, x.cv, eps_min)
    midcc, cc_id = mid3(x.cc, x.cv, eps_max)
    cv, dcv = cv_logcosh(midcv, xL, xU)
    cc, dcc = cc_logcosh(midcc, xL, xU)
    cv_grad = mid_grad(x.cc_grad, x.cv_grad, cv_id)*dcv
    cc_grad = mid_grad(x.cc_grad, x.cv_grad, cc_id)*dcc
    return MC{N,T}(cv, cc, y, cv_grad, cc_grad, x.cnst)
end
function logcosh(x::MC{N,T}) where {N, T <: RelaxTag}
	logcosh_kernel(x, logcosh(x.Intv))
end


import Base: +, -, *

"""
    +(x1::Var, x2::Var)
"""
function +(x1::Var, x2::Var)
    Var(x1.data + x2.data, ∇plus!, (x1,x2))
end
+(x1::Node, x2) = Node(+, (x1,x2))
+(x1, x2::Node) = Node(+, (x1,x2))

function ∇plus!(y::Var, x1::Var, x2::Var)
    T = eltype(y)
    isnothing(x1.grad) || addto!(x1.grad,y.grad)
    isnothing(x2.grad) || addto!(x2.grad,y.grad)
end

"""
    -(x1, x2)
"""
function -(x1::Var, x2::Var)
    Var(x1.data - x2.data, ∇minus!, (x1,x2))
end
-(x1::Node, x2) = Node(-, (x1,x2))
-(x1, x2::Node) = Node(-, (x1,x2))

function ∇minus!(y::Var, x1::Var, x2::Var)
    T = eltype(y)
    isnothing(x1.grad) || addto!(x1.grad, y.grad)
    isnothing(x2.grad) || axpy!(T(-1), y.grad, x2.grad)
end

"""
    *(A::Var, B::Var)
"""
function *(A::Var, B::Var)
    Var(A.data * B.data, ∇times!, (A,B))
end
*(x1::Node, x2) = Node(*, (x1,x2))
*(x1, x2::Node) = Node(*, (x1,x2))

function ∇times!(C::Var, A::Var, B::Var)
    T = eltype(C)
    isnothing(A.grad) || gemm!('N', 'T', T(1), C.grad, B.data, T(1), A.grad)
    isnothing(B.grad) || gemm!('T', 'N', T(1), A.data, C.grad, T(1), B.grad)
end

#=
"""
    exp(x)
"""
function exp(x::Var)
    configure!(x)
    Var(exp(x.data), (exp,x))
end
exp(x::Array) = exp.(x)

function addgrad!(y::Var, ::typeof(exp), x::Var)
    isvoid(x.grad) && return
    ∇exp!(y.data, y.grad, x.grad)
end

function ∇exp!(y::Array{T}, gy::Array{T}, gx::Array{T}) where T
    @inbounds for i = 1:length(gx)
        gx[i] += gy[i] * y[i]
    end
end

"""
    log(x)
"""
function log(x::Var)
    configure!(x)
    Var(log(x.data), (log,x))
end
log(x::Array) = log.(x)

function addgrad!(y::Var, ::typeof(log), x::Var)
    isvoid(x.grad) && return
    ∇log!(y.grad, x.data, x.grad)
end

function ∇log!(gy::Array{T}, x::Array{T}, gx::Array{T}) where T
    @inbounds for i = 1:length(gx)
        gx[i] += gy[i] / x[i]
    end
end

@generated function ∇exp!(y::CuArray{T}, gy::CuArray{T}, gx::CuArray{T}) where T
    f = CuFunction("""
    __global__ void f($T *y, $T *gy, $T *gx, int length) {
        int idx = blockIdx.x * blockDim.x + threadIdx.x;
        if (idx < length) {
            gx[idx] += gy[idx] * y[idx];
        }
    }""")
    quote
        $f(y.ptr, gy.ptr, gx.ptr, length(y), dx=length(y))
    end
end

@generated function ∇log!(gy::CuArray{T}, x::CuArray{T}, gx::CuArray{T}) where T
    f = CuFunction("""
    __global__ void f($T *gy, $T *x, $T *gx, int length) {
        int idx = blockIdx.x * blockDim.x + threadIdx.x;
        if (idx < length) {
            gx[idx] += gy[idx] / x[idx];
        }
    }""")
    quote
        $f(gy.ptr, x.ptr, gx.ptr, length(gy), dx=length(gy))
    end
end

function ∇elemtimes!{T}(gy::Array{T}, x2::Array{T}, gx1::Array{T})
    ind_x2 = CartesianIndex(size(x2))
    ind_gx1 = CartesianIndex(size(gx1))
    @inbounds @simd for I in CartesianRange(size(gy))
        gx1[min(ind_gx1,I)] += gy[I] * x2[min(ind_x2,I)]
    end
end
=#


using Reachability, MathematicalSystems, LazySets, TaylorIntegration
using Reachability: solve

# Paremeters
const μ = 3.986e14 * 60^2
const r = 42164.0e3
const r2 = r^2
const mc = 500.0
const n2 = μ / r^3
const n = sqrt(n2)

const two_n = 2*n
const μ_r2 = μ/r2

const K₁ = [-28.8287 0.1005 -1449.9754 0.0046;
            -0.087 -33.2562 0.00462 -1451.5013]
const K₂ = [-288.0288 0.1312 -9614.9898 0.0;
            -0.1312 -288.0 0.0 -9614.9883]

const K₁mc = K₁/mc
const K₂mc = K₂/mc

const zK = zero(K₁)

function switch_controller!(t, x)
    bool_time = constant_term(t) < 120.0
    x_coord = constant_term(constant_term(x[1]))
    bool_approach = -100.0 ≥ x_coord ≥ -1000.0

    if bool_time

        # Approaching mode
        if bool_approach
            return K₁mc * x
        end

        # Rendezvous attempt
        return K₂mc * x
    end

    # Aborting
    return zK * x
end

# dynamics in the 'approaching' mode
@taylorize function space_rendezvous!(t, x, dx)
    x_1 = r + x[1]
    x_12 = x_1^2
    x_22 = x[2]^2
    rc = sqrt(x_12 + x_22)
    rc3 = rc^3
    μ_rc3 = μ / rc3

    uxy = switch_controller!(t, x)

    # x' = vx
    dx[1] = x[3]
    # y' = vy
    dx[2] = x[4]
    # vx' = n²x + 2n*vy + μ/(r^2) - μ/(rc^3)*(r+x) + ux/mc
    dx[3] = (n2*x[1] + two_n*x[4]) + ((μ_r2 - μ_rc3*x_1) + uxy[1])
    # vy' = n²y - 2n*vx - μ/(rc^3)y + uy/mc
    dx[4] = (n2*x[2] - two_n*x[3]) - (μ_rc3*x[2] - uxy[2])
    # t' = 1
    # dx[5] = one(t)

    return dx
end

𝑂 = Options(:T=>200., :plot_vars=>[1, 2], :project_reachset=>true, :mode=>"reach")
X0 = Hyperrectangle([-900., -400., 0., 0.], [25., 25., 0., 0.])
𝑃 = IVP(BlackBoxContinuousSystem(space_rendezvous!, 4), X0)
𝑂jets = Options(:orderT=>10, :orderQ=>2, :abs_tol=>1e-28, :max_steps=>11000)

# solve (long time: 569.110821 seconds (7.69 G allocations: 475.990 GiB, 20.96% gc time)
# @time sol = solve(𝑃, 𝑂, op=TMJets(𝑂jets))