using LinearAlgebra
using JuMP
using Ipopt
using Parameters

## Setting parameters

@with_kw struct Par
    burn::Int64=0 ;
    nsim::Int64=500000 ;
    nfast::Int64=20000 ;
    nboost::Int64=20000;
end

Param=Par()

## Road map

# 1) Warm start
# 2) Chain filling
# 3) Optimization

## Warm start
# min ||g(x,e)|| st. Inequalities

function warm_start(p,q)
    T,N=size(q)
    wmst=Model(Ipopt.Optimizer)
    set_optimizer_attribute(wmst, "print_level", 0)

    # Declaring variables
    @variable(wmst, d[1:T,1:N] >= 0) # δ_it Convex cost function c'(q)
    @variable(wmst, 1.0e-16 <= qs[1:T,1:N] <= 1.0) # unobserved true q_it

    # Non-negative markups
    # @constraint(wmst,mup[t=1:T,j=1:N],d[t,j]<=p[t])
    for j=1:N
        for t=1:T
            set_upper_bound(d[t,j],p[t])
        end
    end

    # Common ratio constraings (p_t-δ_it)/q_it=(p_t-δ_jt)/q_jt
    @NLconstraint(wmst,eq[t=1:T,j=2:N],(p[t]-d[t,1])/qs[t,1]-(p[t]-d[t,j])/qs[t,j]==0)

    # Co-monotone constraint (δ_it'-δ_it)(q_it'-q_it)>=0
    for j=1:N
        @constraint(wmst,[t=1:T,s=1:T],(d[t,j]-d[s,j])*(qs[t,j]-qs[s,j])>=0)
    end

    # Setting objective function
    @NLexpression(wmst, obj, sum((p[t]*(qs[t,j] - q[t,j]))^2 for t=1:T, j=1:N))
    @NLobjective(wmst,Min,obj)

    ## Solving the Model
    JuMP.optimize!(wmst)

    # Checking convergence
    # JuMP.primal_status(wmst)==MOI.FEASIBLE_POINT || error("Unfeasible point in warm start")
    status=JuMP.primal_status(wmst)==MOI.FEASIBLE_POINT

    # Saving variable values
    dsim=value.(d)
    qsim=value.(qs)

    # Memory cleaning
    wmst=nothing
    println("Warm star done.")
    return dsim, qsim, status
end

## Build chain

# Getting random direction vector from uniform distribution on the unit sphere

# Compute interval Α to draw uniformly to get α≥0
# Interval Α has to satisfy all constraints
# 1) Sign constraints
# 2) Markup constraints
# 3) Common ratio property
# 4) Co-monotone property

## jump function

function jump(p, q, eδ, eq; n::Int64=0)
    n < 5 || return q.-eq, eδ, eq
    T, N = size(eδ)
    # Getting random direction vector from uniform distribution on the unit sphere
    v=rand(2,T,N)
    v=v.*2 .-1 # comes from unif*(1-(-1))+(-1)=unif*(ub-lb)+lb
    ξ=v./norm(v) # ξ( δ ,t, i) ξ(q*,t,i)
    unif=rand(N)

    # Initializing matrices
    αmax=ones(N).*1.0e6
    αmin=ones(N).*-1.0e6
    α=zeros(N)

    # Sign constraints for δit
    for i=1:N, t=1:T
        if ξ[1,t,i]<0
            αmax[i]=αmax[i] < -(eδ[t,i]/ξ[1,t,i]) ? αmax[i] : -(eδ[t,i]/ξ[1,t,i])
            αmin[i]=αmin[i] > (p[t]-eδ[t,i]/ξ[1,t,i]) ? αmin[i] : (p[t]-eδ[t,i]/ξ[1,t,i])
        else
            αmax[i]=αmax[i] < (p[t]-eδ[t,i]/ξ[1,t,i]) ? αmax[i] : (p[t]-eδ[t,i]/ξ[1,t,i])
            αmin[i]=αmin[i] > -(eδ[t,i]/ξ[1,t,i]) ? αmin[i] : -(eδ[t,i]/ξ[1,t,i])
        end
    end

    # Sign constraints for q^*it
    for i=1:N, t=1:T
        if ξ[2,t,i]<0
            αmax[i]=αmax[i] < -(eq[t,i]/ξ[2,t,i]) ? αmax[i] : -(eq[t,i]/ξ[2,t,i])
        else
            αmin[i]=αmin[i] > -(eq[t,i]/ξ[2,t,i]) ? αmin[i] : -(eq[t,i]/ξ[2,t,i])
        end
    end

    # Co-monotone constraints
    for i=1:N, t=1:T, s=1:T
        t==s && continue
        num=eq[s,i]-eq[t,i]
        den=ξ[2,s,i]-ξ[2,t,i]

        if  den > 0
            # αmin[i]=αmin[i] > -(num/den) ? αmin[i] : -(num/den)
            if num > 0
                αmin[i]=αmin[i] > -(num/den) ? αmin[i] : -(num/den)
            else
                αmax[i]=αmax[i] < -(num/den) ? αmax[i] : -(num/den) # If negative
            end
        else
            # αmax[i]=αmax[i] < -(num/den) ? αmax[i] : -(num/den)
            if num < 0
                αmin[i]=αmin[i] > -(num/den) ? αmin[i] : -(num/den) # if negative
            else
                αmax[i]=αmax[i] < -(num/den) ? αmax[i] : -(num/den)
            end
        end

        num=eδ[s,i]-eδ[t,i]
        den=ξ[1,s,i]-ξ[1,t,i]

        if  den > 0
            # αmin[i]=αmin[i] > -(num/den) ? αmin[i] : -(num/den)
            if num > 0
                αmin[i]=αmin[i] > -(num/den) ? αmin[i] : -(num/den)
            else
                αmax[i]=αmax[i] < -(num/den) ? αmax[i] : -(num/den) # if negative
            end
        else
            # αmax[i]=αmax[i] < -(num/den) ? αmax[i] : -(num/den)
            if num < 0
                αmin[i]=αmin[i] > -(num/den) ? αmin[i] : -(num/den) #if negative
            else
                αmax[i]=αmax[i] < -(num/den) ? αmax[i] : -(num/den)
            end
        end
    end

    # Draw uniformily from Α
    for i=1:N
        α[i]=αmax[i]>αmin[i] ? unif[i]*(αmax[i]-αmin[i])+αmin[i] : 0.0
    end

    # Generate random δt ∀i in t=0
    δsim=eδ.+α'.*ξ[1,:,:]

    # Common ratio constraint
    qtemp=eq.+α'.*ξ[2,:,:]
    # Pick a country i at random for every t
    pick=rand(collect(1:N),T)
    # estimate ratio for every t
    r=[(p[t]-δsim[t,i])/qtemp[t,i] for (t,i) ∈ zip(collect(1:T), pick)]
    # Use Common ratio property to estimate q^* for every i and t
    qsim=(p.-δsim)./r

    # Estimate W
    w=q.-qsim

    ## Check inequalities
    check=[(δsim[t,i]-δsim[s,i])*(qsim[t,i]-qsim[s,i]) for t=1:T, s=1:T, i=1:N]
    # minimum(check)>=0 || error("Co-monotone inequalities are not satisfied!")
    minimum(check)>=0 || jump(p,q,eδ,eq,n=n+1)

    return w, δsim, qsim
end

function jump_algo(p, q, eδ, eq; n::Int64=0)
    n < 5 || return q.-eq, eδ, eq
    T, N = size(eδ)
    # Getting random direction vector from uniform distribution on the unit sphere
    v=rand(2,T,N)
    v=v.*2 .-1 # comes from unif*(1-(-1))+(-1)=unif*(ub-lb)+lb
    ξ=v./norm(v) # ξ( δ ,t, i) ξ(q*,t,i)
    unif=rand(N)

    # Initializing matrices
    αmax=ones(N).*1.0e6
    αmin=ones(N).*-1.0e6
    βmax=ones(N).*1.0e6
    βmin=ones(N).*-1.0e6
    α=zeros(N)
    β=zeros(N)

    # Sign constraints for δit
    for i=1:N, t=1:T
        if ξ[1,t,i]<0
            αmax[i]=αmax[i] < -(eδ[t,i]/ξ[1,t,i]) ? αmax[i] : -(eδ[t,i]/ξ[1,t,i])
            αmin[i]=αmin[i] > (p[t]-eδ[t,i]/ξ[1,t,i]) ? αmin[i] : (p[t]-eδ[t,i]/ξ[1,t,i])
        else
            αmax[i]=αmax[i] < (p[t]-eδ[t,i]/ξ[1,t,i]) ? αmax[i] : (p[t]-eδ[t,i]/ξ[1,t,i])
            αmin[i]=αmin[i] > -(eδ[t,i]/ξ[1,t,i]) ? αmin[i] : -(eδ[t,i]/ξ[1,t,i])
        end
    end

    # Sign constraints for q^*it
    for i=1:N, t=1:T
        if ξ[2,t,i]<0
            βmax[i]=βmax[i] < -(eq[t,i]/ξ[2,t,i]) ? βmax[i] : -(eq[t,i]/ξ[2,t,i])
        else
            βmin[i]=βmin[i] > -(eq[t,i]/ξ[2,t,i]) ? βmin[i] : -(eq[t,i]/ξ[2,t,i])
        end
    end

    # Co-monotone constraints
    for i=1:N, t=1:T, s=1:T
        t==s && continue
        # For q
        num=eq[s,i]-eq[t,i]
        den=ξ[2,s,i]-ξ[2,t,i]

        if  den > 0
            # αmin[i]=αmin[i] > -(num/den) ? αmin[i] : -(num/den)
            if num > 0
                βmin[i]=βmin[i] > -(num/den) ? βmin[i] : -(num/den)
            else
                βmax[i]=βmax[i] < -(num/den) ? βmax[i] : -(num/den) # If negative
            end
        else
            # βmax[i]=βmax[i] < -(num/den) ? βmax[i] : -(num/den)
            if num < 0
                βmin[i]=βmin[i] > -(num/den) ? βmin[i] : -(num/den) # if negative
            else
                βmax[i]=βmax[i] < -(num/den) ? βmax[i] : -(num/den)
            end
        end

        num=eδ[s,i]-eδ[t,i]
        den=ξ[1,s,i]-ξ[1,t,i]

        if  den > 0
            # αmin[i]=αmin[i] > -(num/den) ? αmin[i] : -(num/den)
            if num > 0
                αmin[i]=αmin[i] > -(num/den) ? αmin[i] : -(num/den)
            else
                αmax[i]=αmax[i] < -(num/den) ? αmax[i] : -(num/den) # if negative
            end
        else
            # αmax[i]=αmax[i] < -(num/den) ? αmax[i] : -(num/den)
            if num < 0
                αmin[i]=αmin[i] > -(num/den) ? αmin[i] : -(num/den) #if negative
            else
                αmax[i]=αmax[i] < -(num/den) ? αmax[i] : -(num/den)
            end
        end
    end

    # Draw uniformily from Α
    for i=1:N
        α[i]=αmax[i]>αmin[i] ? unif[i]*(αmax[i]-αmin[i])+αmin[i] : 0.0
    end

    # Generate random δt ∀i in t=0
    δsim=eδ.+α'.*ξ[1,:,:]

    # Common ratio constraint
    # qsim=zeros(size(eq))
    # for t=1:T, s=1:T
    #     t==s && continue
    #     pick = rand(1:N) # pick a country at random
    #
    #     if δsim[s,pick]-δsim[t,pick] >= 0
    #         # t>=2 & (minimum(qsim[s,:]-qsim[t,:]) >= 0) && continue
    #         diff = 1.0-eq[t,pick]
    #         qs= diff > 0 ? rand()*(diff)+eq[t,pick] : eq[t,pick]
    #         r=(p[s]-δsim[s,pick])/qs
    #         qsim[s,:]=(p[s].-δsim[s,:])./r
    #     else
    #         # t>=2 & (maximum(qsim[s,:]-qsim[t,:]) <= 0) && continue
    #         diff = eq[t,pick]-1e-8
    #         qs = diff > 0 ? rand()*diff+0.0 : eq[t,pick]
    #         r=(p[s]-δsim[s,pick])/qs
    #         qsim[s,:]=(p[s].-δsim[s,:])./r
    #     end
    # end

    # Draw uniformily from B
    for i=1:N
        β[i]=βmax[i]>βmin[i] ? rand()*(βmax[i]-βmin[i])+βmin[i] : 0.0
    end

    # Generate random q^*it ∀i in t=0
    qtemp=eq.+β'.*ξ[2,:,:]
    # Pick a country i at random for every t
    pick=rand(collect(1:N),T)

    # estimate ratio for every t
    r=[(p[t]-δsim[t,i])/qtemp[t,i] for (t,i) ∈ zip(collect(1:T), pick)]
    # Use Common ratio property to estimate q^* for every i and t
    qsim=(p.-δsim)./r

    # Estimate W
    w=q.-qsim

    ## Check inequalities
    check=[(δsim[t,i]-δsim[s,i])*(qsim[t,i]-qsim[s,i]) for t=1:T, s=1:T, i=1:N]
    # minimum(check)>=0 || error("Co-monotone inequalities are not satisfied!")
    minimum(check)>=0 || jump(p,q,eδ,eq,n=n+1)

    return w, δsim, qsim
end

## Fill in MCMC Markov Chain Monte Carlo integration

function gchain(p, q; Par::Par=Param)
    @unpack burn, nsim = Par
    T, N = size(q)
    eδ, eq, status = warm_start(p,q)
    ew = eq-q
    mchain=zeros(N,(nsim-burn))
    status || return mchain, status # If Unfeasible solution in warmstart don't do loop
    for r=burn+1:nsim
        W, δsim, qsim = jump(p,q,eδ,eq)
        logtrydens = norm(p.*W)-norm(p.*ew)

        if logtrydens.>log.(rand())
            ew=W
            eδ=δsim
            eq=qsim
        end

        mchain[:,r-burn]=gM(p,ew)
    end
    println("Chain is ready.")
    return mchain, status
end

function gchain_algo(p, q; Par::Par=Param)
    @unpack burn, nsim = Par
    T, N = size(q)
    eδ, eq, status = warm_start(p,q)
    ew = eq-q
    mchain=zeros(N,(nsim-burn))
    status || return mchain, status # If Unfeasible solution in warmstart don't do loop
    for r=burn+1:nsim
        W, δsim, qsim = jump_algo(p,q,eδ,eq)
        logtrydens = norm(p.*W)-norm(p.*ew)

        if logtrydens.>log.(rand())
            ew=W
            eδ=δsim
            eq=qsim
        end

        mchain[:,r-burn]=gM(p,ew)
    end
    println("Chain is ready.")
    return mchain, status
end

function gchain_alt(p, q; Par::Par=Param)
    @unpack burn, nsim = Par
    T, N = size(q)
    eδ, eq, status = warm_start(p,q)
    ew = eq-q
    mchain=zeros(N,(nsim-burn))
    status || return mchain, status # If Unfeasible solution in warmstart don't do loop
    for r=burn+1:nsim
        W, δsim, qsim = jump(p,q,eδ,eq)
        logtrydens = norm(p.*W)-norm(p.*ew)

        if rand() < 1.4^(logtrydens[1])
            ew=W
            eδ=δsim
            eq=qsim
        end

        mchain[:,r-burn]=gM(p,ew)
    end
    println("Chain is ready.")
    return mchain, status
end


function surf_exch(ebm::EBM, cn::Constants, Ta, Ua)

    zTs = ebm.zT
    if (ebm.zvar)
        zTs = ebm.zT - sum(ebm.Ds)
        zTs = max(zTs, 1.0)
    end

    # Neutral exchange coefficients
    z0h = 0.1 * ebm.z0
    CD = (cn.vkman / log(ebm.zU / ebm.z0))^2.0
    ebm.CH = cn.vkman^2.0 / (log(ebm.zU / ebm.z0) * log(zTs / z0h))

    # Stability correction (Louis et al. 1982, quoted by Beljaars 1992)
    if (ebm.em == 1)
        RiB = cn.g * (Ta - ebm.Tsurf) * ebm.zU^2.0 / (zTs * Ta * Ua^2.0)
        if (RiB > 0.0)
            fh = 1.0 / (1.0 + 3.0 * ebm.bstb * RiB * sqrt(1.0 + ebm.bstb * RiB))
        else
            fh = 1.0 - 3.0 * ebm.bstb * RiB / (1.0 + 3.0 * ebm.bstb^2.0 * CD * sqrt(-RiB * ebm.zU / ebm.z0))
        end
        ebm.CH = fh * ebm.CH
    end

    return nothing

end


function fasthist!(h, v, edg)
    n = length(edg)-1
    for x in v
        i = searchsortedfirst(edg, x)-1
        if 1 <= i <= n
            h[i] = h[i] + 1.0
        end
    end
end

function mi(probabilites)
    a = 0.
    @inbounds @fastmath for bin_i = 1:size(probabilites,2)
        p_value = sum(probabilites[:,bin_i]); # response probability
        for stim_i = 1:size(probabilites,1)
            p_stim = sum(probabilites[stim_i,:]); # stimulus probability
            if probabilites[stim_i,bin_i]>0
                PrGs = probabilites[stim_i,bin_i]; # response probability given stim
                inc = PrGs*log2(PrGs/(p_value*p_stim)); # MI
                a += inc
            end
        end
    end
    return a
end

function mutual_info(response, class_id)
    stims = sort(unique(class_id))
    nstim = length(stims)
    nfet = size(response,2)
    axis_MI= zeros(Float64,nfet);
    nbins_dist = 10::Int
    histcounts = StatsBase.histrange(extrema(response)..., nbins_dist,
        :left)::StepRangeLen
    probabilites = zeros(Float64,nstim,length(histcounts)-1);
    for axis_i in 1:nfet
        resp = view(response, :, axis_i)
        for (stim_i,stim) in enumerate(stims)
            fasthist!(view(probabilites,stim_i,:),resp[class_id.==stim],
                histcounts);
        end
        probabilites ./= sum(probabilites);
        axis_MI[axis_i]=mi(probabilites)
        fill!(probabilites,0.0)
    end
    return axis_MI
end

function surrogate_MI_coefs(wvmatrix, class_id, nsurr)
    MI_coefs_surrogate = zeros(nsurr,size(wvmatrix,2))
    for surr_i = 1:nsurr
        class_id_surrogate = StatsBase.shuffle(class_id);
        MI_coefs_surrogate[surr_i,:] = mutual_info(wvmatrix,class_id_surrogate);
    end
    return MI_coefs_surrogate
end

function level_per_thresh(MI_coefs_surrogate,  percentile, L)
    MI_thresholds = zeros(size(MI_coefs_surrogate,2))

	levelbounds = cumsum([0; L[1:end-1]]);
    levelthres = zeros(1,length(L)-1);
    for level_i in 1:length(L)-1
        level_coefs = levelbounds[level_i]+1:levelbounds[level_i+1];
        level_MI_nulldist = MI_coefs_surrogate[:,level_coefs];
        level_MI_nulldist = level_MI_nulldist[:];
        levelthres[level_i] = StatsBase.percentile(level_MI_nulldist,percentile);
        MI_thresholds[level_coefs] .= levelthres[level_i]
    end
    return MI_thresholds
end

function mutual_info_thresh(wvmatrix, class_id, nsurr, percentile, nscales)
    L = [1; [2^n for n in 1:nscales]];
    MI_coefs_surrogate = surrogate_MI_coefs(wvmatrix, class_id, nsurr)
    MI_thresholds = level_per_thresh(MI_coefs_surrogate,  percentile, L)
	return MI_thresholds
end

function mi_select(wvmatrix, class_id, opts=WIOpts())
    opts.nscales = opts.nscales == :max ?
        floor(Int,log2(size(wvmatrix,2))) : opts.nscales

    MI = mutual_info(wvmatrix, class_id)
    MI_bias = mutual_info_thresh(wvmatrix, class_id,
                             opts.nsurr, opts.percentile, opts.nscales)

    unbiased_MI = MI .- MI_bias
    selected_wvcoefs = findall(unbiased_MI.>0)

    if length(selected_wvcoefs) > opts.maxwvcoefs
        selected_wvcoefs = sortperm(unbiased_MI, rev=true)[1:opts.maxwvcoefs]
    elseif length(selected_wvcoefs) < opts.minwvcoefs
        selected_wvcoefs = sortperm(unbiased_MI, rev=true)[1:opts.minwvcoefs]
    end

    return selected_wvcoefs,(MI,unbiased_MI)
end


using PyPlot
using LsqFit
using DelimitedFiles

# Load Config File in /FemtoLabScripts/Data
up_power = readdlm(joinpath(@__DIR__,"../Data/uc_power_20201118.config"), header=true)

# Load Values for max_power, power_value and sample_power
m = up_power[1][:,1]
v = up_power[1][:,2]
s = up_power[1][:,3]

# Define 2D-Model s = p1 + p2*m + p3*m^2 + p4*v + p5*v^2
x = [m v]
@. model(x,p) = p[1] + p[2]*x[:,1] + p[3]*x[:,1]^2 + p[4]*x[:,2] + p[5]*x[:,2]^2

# Set Starting Values
p0 = ones(5)

# Fit the data
fit = curve_fit(model, x, s, p0)

# Generate Data Points for Calculated Values
L = 11
mcalc = range(12, 30, length=L)
vcalc = range(0, 30, length=L)
mvcalc = Array{Float64,2}(undef, 0, 2)
for mm in mcalc, vv in vcalc
    global mvcalc = vcat(mvcalc, [mm vv])
end
scalc = model(mvcalc, fit.param)
scalc[scalc .< 0] .= NaN

# Plot the data
println("Plotting data...")
pygui(true)
figure()
scatter3D(m,v,s, label="real Sample power", color="C1")
mesh(mcalc, vcalc, reshape(scalc, (L,L)))
# scatter3D(model(x,fit.param),m,v, label="calculated Sample Power")
xlabel("Max Power")
ylabel("Power Value")
zlabel("Sample Power")
legend()

println("...done!")

# Print calibration equation
param = round.(fit.param,digits=5)
println("------------------------------------------------------------------")
println("Equation to calculate Sample Power with Max Power and Power Value:")
println("s = $(param[1]) + ($(param[2]))*m + ($(param[3]))*m**2 + ($(param[4]))*v + ($(param[5]))*v**2;")
println("------------------------------------------------------------------")


export STCG

using LinearAlgebra
using CUTEst
using NLPModels, LinearOperators, Krylov, SolverTools, SolverBenchmark,JuMP, Ipopt

function STCG(nlp;x :: AbstractVector=copy(nlp.meta.x0),
    atol :: Real=√eps(eltype(x)), rtol :: Real=√eps(eltype(x)),max_iter = 1_000_000, max_time = 30.0)
    t₀ = time()
    Δt = time() - t₀

    f(x) = obj(nlp,x)
    ∇f(x) = grad(nlp,x)
    gx = ∇f(x)
    H(x) = hess(nlp,x)
    Δ = 1.0
    η = 1e-3
    iter = 0
    ϵ = atol + rtol * norm(gx)

    status =:unknown

    p= x .* 0
    z = zero(∇f(x))
    while norm(∇f(x)) > ϵ

        r = ∇f(x)
        d = -r
        n = length(∇f(x))
        fx = f(x)
        gx = ∇f(x)
        Hx = H(x)

        if norm(r) < ϵ
            p = z
        end

        for j = 1:n

            if dot(d, Hx*d) ≤ 0
                model = Model(with_optimizer(Ipopt.Optimizer, print_level=0))
                @variable(model, t)
                @objective(model, Min, fx + dot(gx, z + t*d) + dot((z + t*d), Hx*(z + t*d))/2)
                for i = 1:length(z)
                    @constraint(model, (z[i] + t*d[i])^2 == Δ^2)
                end
                @constraint(model, t ≥ 0)
                optimize!(model)
                τ = value.(t)
                p = z + τ*d
            end
            α = dot(r, r)/dot(d, Hx*d)
            znew = z + α*d

            #end 1st test

            if norm((znew)) ≥ Δ
                model = Model(with_optimizer(Ipopt.Optimizer, print_level=0))
                @variable(model, t)
                for i = 1:length(z)
                    @objective(model, Min, z[i] + t*d[i])
                    @constraint(model, (z[i] + t*d[i])^2 == Δ^2)
                end
                @constraint(model, t ≥ 0)
                optimize!(model)
                τ = value.(t)
                p = z + τ*d
            end
            rnew = r + α*Hx*d

            #end 2nd test

            if norm((rnew)) < ϵ
                p = znew
            end

            βnew = dot(rnew, rnew)/dot(r, r)
            dnew = -rnew + βnew*d

            #end 3rd test
            if Δt > max_time
                status = :max_time
                break
            end

            if iter > max_iter
                status = :max_iter
                break
            end
            z, r, d = znew, rnew, dnew
        end
        Δt = time() - t₀

        ared = f(x) - f(x + p)
        pred = f(x) - (fx + dot(gx, p) + dot(p, Hx*p)/2)

        ρ = ared/pred

        if norm(ρ) < 0.25
            Δ = 0.25*Δ
        elseif norm(ρ) > 0.75 && norm(ρ) == Δ
            Δ = min(2*Δ, ϵ)
        else
             Δ = Δ
        end

        if norm(ρ) > η
            x = x + p
        else
            x = x
        end

        Δt = time() - t₀
        iter += 1

        if iter > max_iter
            status = :max_iter
            break
        end

        if Δt > max_time
            status = :max_time
            break
        end

    end

    if norm(∇f(x)) <= ϵ
        status = :first_order
    elseif (Δt > max_time || iter > max_iter)
        if Δt > max_time
            status = :max_time
        elseif iter > max_iter
            status = :max_iter
        end
    end
    return GenericExecutionStats(status,nlp,objective=f(x),solution=x,dual_feas=norm(∇f(x)),iter = iter, elapsed_time = Δt)
end


using JuMP, Mosek, SCS
import MathProgComplex

function run_SDPpb(cst, optsense)
    # m = Model(solver=MosekSolver(LOG=0))
    m = Model(solver=SCSSolver(verbose=0))

    @variable(m, Xs1[1:3,1:3], SDP)
    @variable(m, Xs2[1:1,1:1], SDP)
    @variable(m, Xs3[1:1,1:1], SDP)

    @constraint(m, Xs1[1,1] == 1)
    @constraint(m, Xs2[1,1] + Xs1[2,2] + Xs1[3,3] == 4)
    @constraint(m, Xs3[1,1] - Xs1[2,1] - Xs1[3,1] == 0)

    # Find upper bound
    @objective(m, optsense, -Xs1[1,3]+cst)
    solve(m)
    return getobjectivevalue(m)
end

@show run_SDPpb(0., :Min)
@show run_SDPpb(0.5, :Min)

@show run_SDPpb(0., :Max)
@show run_SDPpb(0.5, :Max)


module Model

using Random

# model solution
c_star(w::Real, e::Real, θ::NamedTuple) = θ.γ*(1 - θ.τ)*w + θ.γ*e

l_star(w::Real, e::Real, θ::NamedTuple) = 1 - θ.γ + (1 - θ.γ)/(1 - θ.τ)*(e/w)

# objective function
function obj_fun(θ::NamedTuple,
                 w::AbstractVector,
                 mom_data::Tuple{Float64,Float64,Float64},
                 moments_fun::Function,
                 rng::AbstractRNG=MersenneTwister(893245) # Allow possing in an RNG to use when drawing random numbers
                 )

    S = 100 # number of simulation draws for each observed individual
    n = length(w)    # number of individuals
    w = repeat(w, S) # stack observed wages S times
    e = θ.σ*randn(rng, n*S)

    con = c_star.(w, e, Ref(θ))
    lab = l_star.(w, e, Ref(θ))

    # Calculate moments based on simulated data for the value of θ
    mom_sim = moments_fun(w, con, lab)

    # Calculate objective function as squared difference
    Q = sum(abs2(data - sim) for (data, sim) in zip(mom_data, mom_sim))

    return Q
end

end


using JuMP
using Ipopt

# Define sets
# Streams
S = 2:7

# Components (chemical species)
C = ["A", "B"]

# Units
U = ["U1", "U2"]

# Define stream/unit connectivity using dictionaries
inlets = Dict{String,Integer}("U1"=>2, "U2"=>5)
outletV = Dict{String,Integer}("U1"=>3, "U2"=>6)
outletL = Dict{String,Integer}("U1"=>4, "U2"=>7)

# steams
feeds = [2, 5]

# Equilibrium coefficients

K = Dict{Tuple{String, String}, Float64}(
	("U1","A")=>1.008,		("U1","B")=>0.9,
	("U2","A")=>1.099,		("U2","B")=>0.9 )

# Feed flowrate specification
# (for components, same for all feed streams)
feedflow = Dict{String, Float64}("A"=>0.55, "B"=>0.45)

# Define model
m = Model(solver=IpoptSolver())

##### General flowsheet unit model #####

# Total molar flowrate
@variable(m, f[S] >= 0)

# Component molar flowrate
@variable(m, fc[S,C] >= 0)

# Component mole fraction
@variable(m, 0 <= x[S,C] <= 1)

# Unit model
for u in U

	i = inlets[u]
	v = outletV[u]
	l = outletL[u]

	# Overall mass balance
	@constraint(m, f[i] == f[v] + f[l])

	for c in C
		# Component mass balance
		@constraint(m, fc[i,c] == fc[v,c] + fc[l,c])

		# vapor-liquid equilibrium
		@constraint(m, x[v, c] == K[(u,c)]*x[l,c])

	end

	# Unit summation (Rachford-Rice equation)
	@constraint(m, sum(x[v,c] - x[l,c] for c in C) == 0)

end

# Stream model: mole fraction specification
for s in S
	for c in C
		@constraint(m, fc[s,c] == f[s]*x[s,c])
	end
end

##### Economics and recovery/purity specifications

@variable(m, 0.6 <= purityA <= 1.0)
@variable(m, 0.551 <= recoveryA <= 1.0)

# Equipment cost
ecost = Dict{String,Float64}("U1"=>1.5, "U2"=>1.0)

# Set total feed to one
@constraint(m, sum(f[s] for s in feeds) == 1)

# Calculate purity of A
@constraint(m, purityA*sum(sum(f[s] for s in outletV[u]) for u in U) == sum(sum(fc[s,"A"] for s in outletV[u]) for u in U))

# Calculate recovery of A
@constraint(m, recoveryA == sum(sum(fc[s,"A"] for s in outletV[u]) for u in U) / feedflow["A"])

# Objective
@objective(m, Min, sum(sum(f[s] for s in inlets[u])*ecost[u] for u in U) - 100*purityA)

for s in feeds
	for c in C
		setupperbound(x[s,c], feedflow[c])
		setlowerbound(x[s,c], feedflow[c])
	end
end

##### Initialization

# Guess all molar flowrates are one
# And molar fractions match feed
for s in S

	setvalue(f[s], 1.0)

	for c in C
		setvalue(fc[s,c], feedflow[c])
		setvalue(x[s,c], feedflow[c])
	end
end

print(m)

solve(m)


function formulation(
    i::UniformMatroidInstance{Float64, Maximise},
    f::DefaultLinearFormulation
)
    dim = dimension(i)

    model = Model(f.solver)
    x = @variable(model, [1:dim], Bin)
    @objective(model, Max, sum(x[j] * i.rewards[j] for j in 1:dim))
    @constraint(model, sum(x) <= i.m)

    set_silent(model)

    return model, x
end

function solve(
    i::UniformMatroidInstance{Float64, Maximise},
    f::DefaultLinearFormulation
)
    m, x = formulation(i, f)
    optimize!(m)
    return UniformMatroidSolution(i, findall(JuMP.value.(x) .>= 0.5))
end


using Oceananigans
using Oceanostics

grid = RegularRectilinearGrid(size=(4,4,4), extent=(1,1,1))
model = IncompressibleModel(grid=grid)
u, v, w = model.velocities

ke = Oceanostics.KineticEnergy(model, u, v, w)

SPx = Oceanostics.XShearProduction(model, u, v, w, 0, 0, 0)
SPy = Oceanostics.YShearProduction(model, u, v, w, 0, 0, 0)
SPz = Oceanostics.ZShearProduction(model, u, v, w, 0, 0, 0)

#ε_iso = Oceanostics.IsotropicViscousDissipation(model, ν, u, v, w)
#ε_ani = Oceanostics.AnisotropicViscousDissipation(model, ν, ν, ν, u, v, w)


function compute_grad_ratio(grads)
    nsamps = length(grads)
    return grads[2:nsamps-1,:]./grads[1:nsamps-2,:]
end


function vec_to_vech{S<:Int}(n::S)

  x = zeros(Int,round(Int,(n*(n+1)/2)),n*n)

  c = 1
  r = 1
  counter = 1

  for i = 1:n*n

    if r >= c
      x[counter,i] = 1
      counter += 1
    end

    r += 1
    if r > n
      c += 1
      r = 1
    end

  end

  return x

end


using GeometricIntegrators.Config
using GeometricIntegrators.Solvers
using Test

include("linear_solvers_tests.jl")
include("jacobian_tests.jl")
include("nonlinear_solvers_tests.jl")


"""
    Circuit(n_nodes::Int)

A Hamiltonian circuit. If the vector `x` is constrained within a `Circuit(n)`,
each `x[i]` denotes the next node in the graph, for `i ∈ [1, n]`.

The considered graph is an undirected complete graph with `n` nodes.

Also called `cycle` or `atour`.
"""
struct Circuit <: MOI.AbstractVectorSet
    n_nodes::Int
end

MOI.dimension(set::Circuit) = set.n_nodes

"""
    CircuitPath(n_nodes::Int)

A Hamiltonian circuit. If the vectors `x` and `y` are constrained within a
`CircuitPath(n)`, each `x[i]` denotes the next node in the graph, for
`i ∈ [1, n]`. The last `n` variables denote the order in which the nodes are
visited, i.e. `y[1]` is the first visited node (1 by convention), `y[2]` is
the next node in the path, etc.

The considered graph is an undirected complete graph with `n` nodes.
"""
struct CircuitPath <: MOI.AbstractVectorSet
    n_nodes::Int
end

MOI.dimension(set::CircuitPath) = 2 * set.n_nodes

"""
    WeightedCircuit{T <: Real}(n_nodes::Int, cost_matrix::AbstractMatrix{T})

A Hamiltonian circuit. If the vector `x` and the scalar `c` are constrained
within a `WeightedCircuit(n, cost_matrix)`, each `x[i]` denotes the next node
in the graph, for `i ∈ [1, n]`. `c` is the total cost of the circuit, defined as:

``c = \\sum_{i=1}^n \\mathtt{cost\\_matrix}_{i, x[i]}``

The considered graph is an undirected complete graph with `n` nodes.
"""
struct WeightedCircuit{T <: Real} <: MOI.AbstractVectorSet
    n_nodes::Int
    cost_matrix::AbstractMatrix{T}
end

MOI.dimension(set::WeightedCircuit{T}) where {T} = set.n_nodes + 1
function copy(set::WeightedCircuit{T}) where {T}
    return WeightedCircuit(set.n_nodes, copy(set.cost_matrix))
end
function Base.:(==)(x::WeightedCircuit{T}, y::WeightedCircuit{T}) where {T}
    return x.n_nodes == y.n_nodes && x.cost_matrix == y.cost_matrix
end

"""
    WeightedCircuitPath(n_nodes::Int, cost_matrix::AbstractMatrix{T})

A Hamiltonian circuit. If the vectors `x` and `y` and the scalar `c` are
constrained within a `CircuitPath(n)`, each `x[i]` denotes the next node in the graph, for
`i ∈ [1, n]`. The next `n` variables denote the order in which the nodes are
visited, i.e. `y[1]` is the first visited node (1 by convention), `y[2]` is
the next node in the path, etc. `c` is the total cost of the circuit, defined as:

``c = \\sum_{i=1}^n \\mathtt{cost\\_matrix}_{i, x[i]}``

The considered graph is an undirected complete graph with `n` nodes.
"""
struct WeightedCircuitPath{T <: Real} <: MOI.AbstractVectorSet
    n_nodes::Int
    cost_matrix::AbstractMatrix{T}
end

MOI.dimension(set::WeightedCircuitPath{T}) where {T} = 2 * set.n_nodes + 1
function copy(set::WeightedCircuitPath{T}) where {T}
    return WeightedCircuitPath(set.n_nodes, copy(set.cost_matrix))
end
function Base.:(==)(
    x::WeightedCircuitPath{T},
    y::WeightedCircuitPath{T},
) where {T}
    return x.n_nodes == y.n_nodes && x.cost_matrix == y.cost_matrix
end

# isbits types, nothing to copy
function copy(set::Union{Circuit, CircuitPath})
    return set
end


__precompile__()
module Metaheuristics

export WOA, GOA, GSA, CGSA, SA, CMAES_AEP
export CFO, DE, ABC, PSO

# v2 items
export optimize, ECA, Options, State, Information, Engine, Problem
export MOEAD_DE

# v2.1 items
export convergence, minimizer, minimum, positions, fvals, nfes, get_position, fval, NSGA2
export hval, gval, hvals, gvals

export NSGA3, gen_ref_dirs

export SMS_EMOA, SPEA2, MCCGA

export PerformanceIndicators, pareto_front, nadir, ideal

include("externals.jl")

include("core/abstracts.jl")
include("core/information.jl")
include("core/options.jl")
include("core/state.jl")
include("core/structures.jl")

include("solutions/individual.jl")
include("solutions/constrained.jl")
include("solutions/display.jl")

include("common/multi-objective-functions.jl")
include("common/repair.jl")

include("common/stop.jl")
include("common/compare.jl")
include("common/crossover.jl")
include("common/mutation.jl")
include("common/selection.jl")
include("common/generic-reproduction.jl")
include("common/non-dominated-sorting.jl")

include("TestProblems/TestProblems.jl")
include("PerformanceIndicators/PerformanceIndicators.jl")

include("algorithms/algorithm.jl")

include("optimize.jl")

#######################################################
#                                                     #
#           A L G O R I T H M S                       #
#                                                     #
#######################################################

# template file
include("algorithms/template.jl")

# ECA algorithm
include("algorithms/ECA/ECA.jl")
include("algorithms/ECA/CECA.jl")

# Differential algorithm
include("algorithms/DE/DE.jl")

# PSO algorithm
include("algorithms/PSO/PSO.jl")

# The whale optimization algorithm
# S Mirjalili, A Lewis - Advances in Engineering Software, 2016
include("algorithms/WOA/WOA.jl")

# Mirjalili, Seyedali, and Amir H. Gandomi.
# "Chaotic gravitational constants for the gravitational search algorithm."
# Applied Soft Computing 53 (2017): 407-419.
include("algorithms/CGSA/CGSA.jl")

# SA: Simulated Annealing
# Kirkpatrick, S., Gelatt, C.D., & Vecchi, M.P. (1983). Optimization by
# Simulated Annealing. _Science, 220_, 671-680.
include("algorithms/SA/SA.jl")

# Aritifical Bee colony
include("algorithms/ABC/ABC.jl")

# MOEA decomposition based using Differential Evolution
include("algorithms/MOEAD_DE/MOEAD_DE.jl")

# Non-dominate sorting Genetic Algorithm
include("algorithms/NSGA2/NSGA2.jl")
include("algorithms/NSGA3/NSGA3.jl")
include("algorithms/SMS_EMOA/SMS_EMOA.jl")
include("algorithms/SPEA2/SPEA2.jl")

# genetic algorithm
include("algorithms/MCCGA/MCCGA.jl")

include("common/reproduction.jl")

#######################################################
#                                                     #
#           D E P R E C A T E D                       #
#                                                     #
#######################################################
# nothing to deprecate yet

end # module


module nt_signal

using PyPlot

export imageplot, resize_image, load_image, cconvol, upsampling, subsampling, plot_wavelet, snr, perform_wavortho_transf

#######################################
function imageplot(g, str="", a=-1, b=-1, c=-1)
	if size(g,3)==1
		# special for b&w images.
		g = g[:,:,1];
	end
	if c>0
		subplot(a,b,c);
	end
	imshow(g, interpolation="nearest")
	set_cmap("gray")
	axis("off")
	if str!=""
		title(str);
	end
end

#######################################
function load_image(name,N)
	g = imread(name);
	g = resize_image(g, N);
	return g;
end

#######################################
function resize_image(f, N)
	P = size(f,1);
	# add 1 pixel boundary
	g = f;
	g = cat(2, g, g[:,1,:]);
	g = cat(1, g, g[1,:,:]);
	# interpolate
	t = range(1, stop=P, length=N);
	ti = floor(t); tj = ceil(t);
	fi = t-floor(t); fj = 1-fi;
	h = zeros(N,N,size(f,3));
	for s=1:size(f,3)
	    h[:,:,s] = g[ti,ti,s] .* (fj*fj') + g[tj,tj,s] .* (fi*fi') + g[ti,tj,s] .* (fj*fi') + g[tj,ti,s] .* (fi*fj');
	end
	return h;
end

###################################################
function trim_dim(x)
	# remove empty trailing dimensions
	if size(x,3)==1
		x = x[:,:,1];
		if size(x,2)==1
			x = x[:,1];
		end
	end
	return x;
end

###################################################
function subsampling(x,d=1,p=2)

	# downsampling - subsampling along dimension d
	#
	#   y = subsampling(x,d,p);
	#
	#   default is p==2, d==1
	#
	#   Copyright (c) 2009 Gabriel Peyre

	if d==1
	        y = x[1:p:end,:,:];
	elseif d==2
	        y = x[:,1:p:end,:];
	elseif d==3
	        y = x[:,:,1:p:end];
	else
	        error("Not implemented");
	end
	y = trim_dim(y);

	return y;

end # subsampling

###################################################
function upsampling(x,d=1,p=2)

	# upsampling - add p zeros between samples along dimension d
	#
	#   y = upsampling(x,d,p);
	#
	#   default is p==2, d==1
	#
	#   Copyright (c) 2009 Gabriel Peyre

	if d==1
	        y = zeros(p*size(x,1),size(x,2),size(x,3));
	        y[1:p:end,:,:] = x;
	elseif d==2
	        y = zeros(size(x,1),p*size(x,2),size(x,3));
	        y[:,1:p:end,:] = x;
	elseif d==3
	        y = zeros(size(x,1),size(x,2),p*size(x,3));
	        y[:,:,1:p:end] = x;
	else
	        error("Not implemented");
	end
	y = trim_dim(y);

	return y;
end # upsampling

###################################################
function cconvol(x,h,d=1)

	# cconvol - spatial domain circular convolution
	#
	#   y = cconv(x,h,d);
	#
	#   Circular convolution along dimension |d|.
	#
	#   If p=length(h) is odd then h((p+1)/2) corresponds to the zero position.
	#   If p is even, h(p/2) corresponds to the zero position.
	#
	#   Copyright (c) 2009 Gabriel Peyre

	if d==2
	    return ( cconvol( x',h, 1 )' );
	elseif d==3
		# permute do not exists in python
	    return permute( cconvol( permute(x,[3, 2, 1]),h), [3 2 1]);
	end

	p = length(h);
	if mod(p,2)==0
	    pc = p/2;
	else
	    pc = (p+1)/2;
	end

	y = zeros(size(x));
	for i=1:length(h)
	    y = y + h[i]*circshift(x,i-pc);
	end
	return y;

end # cconvol

#####################################################
function plot_wavelet(fW, Jmin=0)
    #    plot_wavelet - plot wavelets coefficients.
    #    U = plot_wavelet(fW, Jmin):
    #    Copyright (c) 2014 Gabriel Peyre

    function rescaleWav(A)
        v = maximum( abs(A[:]) );
        B = copy(A)
        if v > 0
            B = .5 + .5 * A / v;
        end
        return B;
    end
    function rescale(x,a=0,b=1)
		m = minimum(x[:]);
		M = maximum(x[:]);
		if M-m<1e-10
		    y = x;
		else
		    y = (b-a) * (x-m)/(M-m) + a;
		end
		return y;
	end ## end of rescale
    ##
    n = size(fW,1);
    Jmax = log2(n) - 1;
    U = copy(fW);
    for j = Jmax:-1:Jmin
    	L = 1:2^j; H = (2^j)+1:2^(j+1);
        U[L,H] = rescaleWav( U[L,H] );
        U[H,L] = rescaleWav( U[H,L] );
        U[H,H] = rescaleWav( U[H,H] );
    end
    # coarse scale
    L = 1:2^Jmin;
    U[L,L] = rescale(U[L,L]);
    # plot underlying image
    imageplot(U);
    # display crosses
    for j=Jmax:-1:Jmin
        plot([0, 2^(j+1)], [2^j, 2^j], "r");
        plot([2^j, 2^j], [0, 2^(j + 1)], "r");
    end
    # display box
    plot([0, n], [0, 0], "r");
    plot([0, n], [n, n], "r");
    plot([0, 0], [0, n], "r");
    plot([n, n], [0, n], "r");
    return U;

end # plot_wavelet

#########################################
function subselectdim(f,sel,d)
	g = [];
	if d==1
	    g = f[sel,:,:];
	elseif d==2
	    g = f[:,sel,:];
	elseif d==3
	    g = f[:,:,sel];
	end
	return trim_dim(g);
end

#########################################
function subassign(f,sel,g)
	d = ndims(f);
	if d==1
	    f[sel] = g;
	elseif d==2
	    f[sel,sel] = g;
	elseif d==3
	    f[sel,sel,sel] = g;
	end
	return f;
end # subassign

#########################################
function subselect(f,sel)
	d = ndims(f)
    if d==1
        return f[sel];
    elseif d==2
        return f[sel,sel];
    elseif d==3
        return f[sel,sel,sel];
	end
	return [];
end

##############################################################
function perform_wavortho_transf(f0,Jmin,dir,h)

	# perform_wavortho_transf - compute orthogonal wavelet transform
	#
	#   fw = perform_wavortho_transf[f,Jmin,dir,options);
	#
	#   You can give the filter in options.h.
	#
	#   Works in arbitrary dimension.
	#
	#   Copyright (c) 2009 Gabriel Peyre

	n = size(f0,1);
	Jmax = log2(n)-1;

	g = [0, h[length(h):-1:2]] .* (-1).^(1:length(h));

	f = copy(f0)
	if dir==1
	    ### FORWARD ###
	    for j=Jmax:-1:Jmin
	        sel = 1:2^(j+1);
	        a = subselect(f,sel);
	        for d=1:ndims(f)
	            a = cat(d, subsampling(cconvol(a,h,d),d), subsampling(cconvol(a,g,d),d) );
	        end
	        f = subassign(f,sel,a);
	    end
	else
	    ### FORWARD ###
	    for j=Jmin:Jmax
	        sel = 1:2^(j+1);
	        a = subselect(f,sel);
	        for d=1:ndims(f)
	            w = subselectdim(a,2^j+1:2^(j+1),d);
	            a = subselectdim(a,1:2^j,d);
	            a = cconvol(upsampling(a,d),reverse(h),d) + cconvol(upsampling(w,d),reverse(g),d);
	        end
	        f = subassign(f,sel,a);
	    end
	end
	return f;
end # perform_wavortho_transf

#####################################################
function snr(x,y)

	# snr - signal to noise ratio
	#
	#   v = snr(x,y);
	#
	# v = 20*log10( norm(x[:]) / norm(x[:]-y[:]) )
	#
	#   x is the original clean signal (reference).
	#   y is the denoised signal.
	#
	#   Copyright (c) 2008 Gabriel Peyre

	return 20*log10(norm(x[:])/norm(x[:]-y[:]));

end # snr

end ## module nt_signal


mutable struct ScalarKalmanState{Fl<:AbstractFloat}
    v::Fl
    F::Fl
    att::Fl
    a::Fl
    Ptt::Fl
    P::Fl
    llk::Fl
    steady_state::Bool
    # Auxiliary matrices
    P_to_check_steady_state::Fl
    function ScalarKalmanState(a1::Fl, P1::Fl) where Fl
        P_to_check_steady_state = zero(Fl)
        return new{Fl}(
            zero(Fl),
            zero(Fl),
            zero(Fl),
            a1,
            zero(Fl),
            P1,
            zero(Fl),
            false,
            P_to_check_steady_state,
        )
    end
end

function save_a1_P1_in_filter_output!(
    filter_output::FilterOutput{Fl}, kalman_state::ScalarKalmanState{Fl}
) where Fl
    filter_output.a[1] = copy(fill(kalman_state.a, 1))
    filter_output.P[1] = copy(fill(kalman_state.P, 1, 1))
    return filter_output
end

function save_kalman_state_in_filter_output!(
    filter_output::FilterOutput{Fl}, kalman_state::ScalarKalmanState{Fl}, t::Int
) where Fl
    filter_output.v[t] = copy(fill(kalman_state.v, 1))
    filter_output.F[t] = copy(fill(kalman_state.F, 1, 1))
    filter_output.a[t + 1] = copy(fill(kalman_state.a, 1))
    filter_output.att[t] = copy(fill(kalman_state.att, 1))
    filter_output.P[t + 1] = copy(fill(kalman_state.P, 1, 1))
    filter_output.Ptt[t] = copy(fill(kalman_state.Ptt, 1, 1))
    # There is no diffuse part in UnivariateKalmanState
    filter_output.Pinf[t] = copy(fill(zero(Fl), size(filter_output.Ptt[t])))
    return filter_output
end

"""
    ScalarKalmanFilter{Fl <: Real} <: KalmanFilter

Similar to the univariate Kalman filter but exploits the fact that the dimension of the
state is equal to 1.
"""
mutable struct ScalarKalmanFilter{Fl<:Real} <: KalmanFilter
    steadystate_tol::Fl
    a1::Fl
    P1::Fl
    skip_llk_instants::Int
    kalman_state::ScalarKalmanState

    function ScalarKalmanFilter(
        a1::Fl, P1::Fl, skip_llk_instants::Int=1, steadystate_tol::Fl=Fl(1e-5)
    ) where Fl
        kalman_state = ScalarKalmanState(a1, P1)
        return new{Fl}(steadystate_tol, a1, P1, skip_llk_instants, kalman_state)
    end
end

function reset_filter!(kf::ScalarKalmanFilter{Fl}) where Fl
    reset_filter!(kf.kalman_state, kf.a1, kf.P1)
    return kf
end

function reset_filter!(kalman_state::ScalarKalmanState, a1::Fl, P1::Fl) where Fl
    kalman_state.a = a1
    kalman_state.P = P1
    kalman_state.llk = zero(Fl)
    kalman_state.P_to_check_steady_state = zero(Fl)
    kalman_state.steady_state = false
    return kalman_state
end

function scalar_check_steady_state(kalman_state::ScalarKalmanState{Fl}, tol::Fl) where Fl
    if abs((kalman_state.P - kalman_state.P_to_check_steady_state) / kalman_state.P) > tol
        kalman_state.P_to_check_steady_state = kalman_state.P
        return kalman_state
    end
    kalman_state.steady_state = true
    return kalman_state
end

function scalar_update_v!(
    kalman_state::ScalarKalmanState{Fl}, y::Fl, Z::Fl, d::Fl
) where Fl
    kalman_state.v = y - Z * kalman_state.a - d
    return kalman_state
end

function scalar_update_F!(kalman_state::ScalarKalmanState{Fl}, Z::Fl, H::Fl) where Fl
    kalman_state.F = Z * kalman_state.P * Z + H
    return kalman_state
end

function scalar_update_att!(kalman_state::ScalarKalmanState{Fl}, Z::Fl) where Fl
    kalman_state.att = kalman_state.a + Z * kalman_state.P * kalman_state.v / kalman_state.F
    return kalman_state
end

function scalar_update_a!(kalman_state::ScalarKalmanState{Fl}, T::Fl, c::Fl) where Fl
    kalman_state.a = T * kalman_state.att + c
    return kalman_state
end

function scalar_update_Ptt!(kalman_state::ScalarKalmanState{Fl}, Z::Fl) where Fl
    kalman_state.Ptt =
        kalman_state.P - kalman_state.P * Z * Z * kalman_state.P / kalman_state.F
    return kalman_state
end

function scalar_update_P!(kalman_state::ScalarKalmanState{Fl}, T::Fl, RQR::Fl) where Fl
    kalman_state.P = T * kalman_state.Ptt * T + RQR
    return kalman_state
end

function update_llk!(kalman_state::ScalarKalmanState{Fl}) where Fl
    kalman_state.llk -= (
        HALF_LOG_2_PI + 0.5 * (log(kalman_state.F) + kalman_state.v^2 / kalman_state.F)
    )
    return kalman_state
end

function scalar_update_kalman_state!(
    kalman_state, y, Z, T, H, RQR, d, c, skip_llk_instants, tol, t
)
    if isnan(y)
        kalman_state.v = NaN
        scalar_update_F!(kalman_state, Z, H)
        kalman_state.att = kalman_state.a
        scalar_update_a!(kalman_state, T, c)
        kalman_state.Ptt = kalman_state.P
        scalar_update_P!(kalman_state, T, RQR)
        kalman_state.steady_state = false # Not on steadystate anymore
    elseif kalman_state.steady_state
        scalar_update_v!(kalman_state, y, Z, d)
        scalar_update_att!(kalman_state, Z)
        scalar_update_a!(kalman_state, T, c)
        if t > skip_llk_instants
            update_llk!(kalman_state)
        end
    else
        scalar_update_v!(kalman_state, y, Z, d)
        scalar_update_F!(kalman_state, Z, H)
        scalar_update_att!(kalman_state, Z)
        scalar_update_a!(kalman_state, T, c)
        scalar_update_Ptt!(kalman_state, Z)
        scalar_update_P!(kalman_state, T, RQR)
        scalar_check_steady_state(kalman_state, tol)
        if t > skip_llk_instants
            update_llk!(kalman_state)
        end
    end
    return kalman_state
end

function optim_kalman_filter(
    sys::LinearUnivariateTimeInvariant{Fl}, filter::ScalarKalmanFilter{Fl}
) where Fl
    return filter_recursions!(
        filter.kalman_state, sys, filter.steadystate_tol, filter.skip_llk_instants
    )
end

function kalman_filter!(
    filter_output::FilterOutput, sys::StateSpaceSystem, filter::ScalarKalmanFilter{Fl}
) where Fl
    filter_recursions!(
        filter_output,
        filter.kalman_state,
        sys,
        filter.steadystate_tol,
        filter.skip_llk_instants,
    )
    return filter_output
end

function filter_recursions!(
    kalman_state::ScalarKalmanState{Fl},
    sys::LinearUnivariateTimeInvariant{Fl},
    steadystate_tol::Fl,
    skip_llk_instants::Int,
) where Fl
    RQR = sys.R[1] * sys.Q[1] * sys.R[1]
    @inbounds for t in eachindex(sys.y)
        scalar_update_kalman_state!(
            kalman_state,
            sys.y[t],
            sys.Z[1],
            sys.T[1],
            sys.H,
            RQR,
            sys.d,
            sys.c[1],
            skip_llk_instants,
            steadystate_tol,
            t,
        )
    end
    return kalman_state.llk
end

function filter_recursions!(
    filter_output::FilterOutput,
    kalman_state::ScalarKalmanState{Fl},
    sys::LinearUnivariateTimeInvariant,
    steadystate_tol::Fl,
    skip_llk_instants::Int,
) where Fl
    RQR = sys.R[1] * sys.Q[1] * sys.R[1]
    save_a1_P1_in_filter_output!(filter_output, kalman_state)
    @inbounds for t in eachindex(sys.y)
        scalar_update_kalman_state!(
            kalman_state,
            sys.y[t],
            sys.Z[1],
            sys.T[1],
            sys.H,
            RQR,
            sys.d,
            sys.c[1],
            skip_llk_instants,
            steadystate_tol,
            t,
        )
        save_kalman_state_in_filter_output!(filter_output, kalman_state, t)
    end
    return filter_output
end


"""
    VectorFold{T,V <: AbstractVector{T}}
A folded vector structure that extends the methods of AbstractVector to a folded structure.
"""
struct VectorFold{T,V <: AbstractVector{T}} <: PatternFold{T,V}
    pattern::V
    gap::T
    folds::Int
end

"""
    pattern(vf, index)
Return the element at `index` in the original pattern.
"""
pattern(vf, index) = pattern(vf)[index]

# REVIEW - how to optimize assignation
# TODO - can unfold be made almost generic?
"""
    unfold(vf::VectorFold; from=1, to=folds(vf))
Construct the unfolded version of `vf` (with the same type as `pattern(vf)`) based. Please note that using an iterator on `vf` avoid memory allocation, which is not the case of `unfold`.
"""
function unfold(vf::VectorFold; from=1, to=folds(vf))
    pl = pattern_length(vf)
    ul = (to - from + 1) * pl
    v = typeof(pattern(vf))(undef, ul)

    count = 0
    for fold in from:to
        for i in 1:pl
            v[count * pl + i] = pattern(vf, i) + (fold - 1) * gap(vf)
        end
        count += 1
    end
    return v
end


struct EmbeddedMap{T<:Integer}
    P::PlanarMap{T}
    locs::Array{<:Tuple{<:Real,<:Real}}
end

function EmbeddedMap(RNG::Random.AbstractRNG,
                     P::PlanarMap;
                     outeredge=(1,neighbors(P,1)[1]),
                     corners=nothing)
    T = triangulation(RNG,P,outeredge=outeredge,corners=corners)
    outface = corners ≠ nothing ? Face(corners...) : face(T,outeredge...)
    WT = schnyderwood(RNG,T,outface=outface)
    n = length(P)
    map((a,b)->ϕ(a,b,n),EmbeddedMap(P,schnyder_coords(WT)))
end

EmbeddedMap(P::PlanarMap;kwargs...) =
        EmbeddedMap(Random.MersenneTwister(0),P;kwargs...)

length(E::EmbeddedMap) = length(E.P)

import Base.map
function map(f::Function,E::EmbeddedMap)
    EmbeddedMap(E.P,[f(a,b) for (a,b) in E.locs])
end

ϕ(a::Real,b::Real,n::Real) = tuple(([1 1/2; 0 sqrt(3)/2]*[a; b]/n)...)
ϕ(a::Real,b::Real) = ϕ(a,b,1)
ϕ(t::NTuple{2}) = ϕ(t...)

function bbox_size(E::EmbeddedMap)
    My = maximum(y for (x,y) in E.locs)
    my = minimum(y for (x,y) in E.locs)
    Mx = maximum(x for (x,y) in E.locs)
    mx = minimum(x for (x,y) in E.locs)
    max(Mx-mx,My-my)
end

struct GraphPenSet
    pointpen
    diskpen
    labelpen
    edgepen
    facepen
    pointpens
    diskpens
    labelpens
    edgepens
    facepens
end

function GraphPenSet(;kwargs...)
    args = DataStructures.OrderedDict(
            :pointpen=>AsyPlots.Pen(color="DarkGreen"),
            :diskpen=>AsyPlots.Pen(color="Black"),
            :labelpen=>AsyPlots.Pen(fontsize=12,color="White"),
            :edgepen=>AsyPlots.Pen(color="Navy",linewidth=1.2),
            :facepen=>AsyPlots.Pen(color="LightBlue"),
            :pointpens=>nothing,
            :diskpens=>nothing,
            :labelpens=>nothing,
            :edgepens=>nothing,
            :facepens=>nothing)

    merge!(args,Dict(kwargs))
    GraphPenSet(values(args)...)
end

for name in (:point,:disk,:label,:edge,:face)
    namepen = Symbol(string(name)*"pen")
    namepens = Symbol(string(name)*"pens")
    @eval function $namepen(G::GraphPenSet,k)
        if hasmethod(getindex,map(typeof,(G.$namepens,k)))
            G.$namepens[k]
        elseif !isempty(methods(G.$namepens))
            G.$namepens(k)
        else
            G.$namepen
        end
    end
end

function draw(E::EmbeddedMap;
              outeredge=(1,neighbors(E.P,1)[1]),
              drawpoints=true,
              drawedges=true,
              fillfaces=false,
              drawlabels=true,
              graphpenset=GraphPenSet(),
              kwargs...)

    if length(kwargs) > 0
        G = GraphPenSet(;kwargs...)
    else
        G = graphpenset
    end
    s = bbox_size(E) # for sizing the disk that the label goes in
    grlist = AsyPlots.GraphicElement2D[]
    if fillfaces
        append!(grlist,[AsyPlots.Polygon2D([E.locs[i] for i in fc],
                            pen=AsyPlots.NoPen(),
                            fillpen=facepen(G,fc)) for fc in interiorfaces(
                                faces(E.P;outeredge=outeredge))])
    end
    if drawedges
        append!(grlist,[AsyPlots.Path([E.locs[i],E.locs[j]],pen=edgepen(G,(i,j)))
                                                    for (i,j) in edges(E.P)])
    end
    if drawlabels
        append!(grlist,vcat([[AsyPlots.Circle(E.locs[i],
                                              s*labelpen(G,i).fontsize/500,
                                     pen=diskpen(G,i),
                                     fillpen=pointpen(G,i)),
                              AsyPlots.Label(string(i),E.locs[i];
                                     pen=labelpen(G,i))] for i in 1:length(E.P)]...))
    elseif drawpoints
        append!(grlist,[AsyPlots.Point(E.locs[i],
                                  pen=pointpen(G,i)) for i in 1:length(E.P)])
    end
    AsyPlots.Plot(grlist)
end

"""
    draw(P,outeredge,corners)

Draw the planar map P with outer face either:
    - the face to the left of the `outeredge`
    - the face containing the sequence of vertices `corners`

- `outeredge` is an ordered pair
- `corners` is either `nothing` (to indicate deference to
    specification via `outeredge`) or an ordered triple
"""
function draw(P::PlanarMap;
              outeredge=(1,neighbors(P,1)[1]),
              corners=nothing,
              kwargs...)
    draw(EmbeddedMap(P;outeredge=outeredge,corners=corners);kwargs...)
end

function draw(WT::WoodedTriangulation;
              rotation=0,
              linewidth=10/length(WT),
              pointsize=3,
              pointcolor=AsyPlots.NamedColor(0.65,0.65,0.65),
              linecolor="default",
              labelcolor="black",
              labelsize=max(1,floor(log10(length(WT))))*min(12,max(144/length(WT))),
              kwargs...)
    ASYCOLORS =     Dict(zip((:blue, :red, :green,     :gray),
        AsyPlots.NamedColor.(("Navy","Red","DarkGreen","Gray"))))
    edgedict = Dict((e.tail,e.head)=>e.color for e in edges(WT))
    edgepens(t) = linecolor == "default" ?
                        AsyPlots.Pen(linewidth=linewidth,
                                     color=ASYCOLORS[edgedict[t]]) :
                                AsyPlots.Pen(linewidth=linewidth,
                                             color=linecolor)
    function facepens(F::Face)
        AsyPlots.Pen(color=sum(1/3 * AsyPlots.NamedColor(
                            string(edgedict[e])) for e in edges(F)))
    end
    GPS = GraphPenSet(;edgepens=edgepens,
                       facepens=facepens,
                       diskpen=AsyPlots.Pen(linewidth=10/length(WT)),
                       pointpen=AsyPlots.Pen(linewidth=pointsize,
                                             color=pointcolor),
                       labelpen=AsyPlots.Pen(fontsize=labelsize,color=labelcolor))
    draw(EmbeddedMap(WT.P,
         map((t->reim(cis(rotation)*(t[1]+im*t[2])))∘ϕ,schnyder_coords(WT))),
         outeredge=(blueroot(WT),greenroot(WT));
         graphpenset=GPS,kwargs...)
end


using Distributed
using DistributedArrays
using DistributedArrays.SPMD
#@everywhere using SharedArrays
@everywhere using Distributed
@everywhere using DistributedArrays
@everywhere using DistributedArrays.SPMD

"""
The following function evaluates the electric field on a uniform grid from the electric potential.

    // Calculate electric field from potential
"""
function get_E_from_ϕ!(ϕ, E, dx)
      J = length(E)
      for j in 2:J-1
        E[j] = (ϕ[j-1] - ϕ[j+1]) / 2. / dx
      end
      E[1] = (ϕ[J] - ϕ[2]) / 2. / dx;
      E[J] = (ϕ[J-1] - ϕ[1]) / 2. / dx;
end

""" The following routine solves Poisson's equation in 1-D to find the instantaneous electric potential on a uniform grid.

// Solves 1-d Poisson equation:
//    d^u / dx^2 = v   for  0 <= x <= L
// Periodic boundary conditions:
//    u(x + L) = u(x),  v(x + L) = v(x)
// Arrays u and v assumed to be of length J.
// Now, jth grid point corresponds to
//    x_j = j dx  for j = 0,J-1
// where dx = L / J. L / (J-1) in Julia
// Also,
//    kappa = 2 pi / L
"""
function get_ϕ!(ϕ, ρ, κ)
  #V = fill(0.0+im*0.,J)
  #U = fill(0.0+im*0.,J÷2+1)
  J = length(ρ)
  # Fourier transform source term
  V = rfft(ρ)

  # Calculate Fourier transform of u

  V[1] =  0.
  for j in  2:(J÷2+1)
    V[j] = - V[j] / (j-1)^2 / κ^2
  end

  # Inverse Fourier transform to obtain u
  ϕ[:] = irfft(V,J)
end

"""
Takes out the mass of the grid function so that the sum is now null
"""
function filter_constant!(E)
  J = length(E)
  V = rfft(E)
  V[1] = 0.0 #extract the first component
  E[:] = irfft(V,J)
end

"""The routine below evaluates the electron number density on an evenly spaced mesh given the instantaneous electron coordinates.

// Evaluates electron number density n(0:J-1) from
// array r(0:N-1) of electron coordinates.
"""
function get_density_old_old!(r, n, dx)
  # Initialize
  N = length(r)
  J = length(n)
  fill!(n,0.0)
  # Evaluate number density.
  for i in 1:N
    j, y = get_index_and_distance(r[i],dx,L)
    if j < 1
        error("j = $j")
    end
    n[j] += (1. - y) / dx;
    if (j == J)
        n[1] += y / dx
    else
        n[j+1] += y / dx
    end
  end
end

function get_density_old!(u, n, p)
  L, N, J, κ, dx, order = p
  r = view(u,1:N)
  fill!(n,0.0)
  # Evaluate number density.
  for i in 1:N
    j, y = get_index_and_y(r[i],J,L)
    #if j < 1
    #    error("j = $j")
    #end
    if order == 1
      n[j] += (1. - y) / dx;
      if (j == J)
          n[1] += y / dx
      else
        n[j+1] += y / dx
      end
    end
    if order == 2
      if y <= 1/2
        n[j] += (3/4 - y^2) / dx
        if j == 1
          n[J] += (1-2y)^2/8 / dx
          n[2] += (1+2y)^2/8 / dx
        elseif j==J
          n[1] += (1+2y)^2/8 / dx
          n[J-1] += (1-2y)^2/8 / dx
        else
          n[j+1] += (1+2y)^2/8 / dx
          n[j-1] += (1-2y)^2/8 / dx
        end
      elseif y <= 3/2
        n[j] += (3 - 2y)^2/8 / dx
        if j == J-1
          n[J] += (3/4 - (1-y)^2) / dx
          n[1] += (1-2y)^2/8 / dx
        elseif j==J
          n[1] += (3/4 - (1-y)^2) / dx
          n[2] += (1-2y)^2/8 / dx
        else
          n[j+1] += (3/4 - (1-y)^2) / dx
          n[j+2] += (1-2y)^2/8 / dx
        end
      end
    end
  end
    #n .= n/n0 - 1.0 # return rho directly
end

function get_density!(u, n, p)
  L, N, J, κ, dx, order = p
  r = view(u,1:N)
  fill!(n,0.0)
  # Evaluate number density.
  for i in 1:N
    @inbounds j, y = get_index_and_y(r[i],J,L)
    for l in (-order):order
      @inbounds n[mod1(j + l, J)] += W(order, -y + l) / dx;
    end
  end
    #n .= n/n0 - 1.0 # return rho directly
end

function get_density_ro!(u, n, p)
  L, N, J, κ, dx, order = p
  r = view(u,1:2:2N-1)
  fill!(n,0.0)
  # Evaluate number density.
  for i in 1:N
    j, y = get_index_and_y(r[i],J,L)
    for l in (-order):order
      n[mod1(j + l, J)] += W(order, -y + l) / dx;
    end
  end
    #n .= n/n0 - 1.0 # return rho directly
end

"""The routine below evaluates the electron current on an evenly spaced mesh given the instantaneous electron coordinates.

// Evaluates electron number density S(0:J-1) from
// array r(0:N-1) of electron coordinates.
"""
function get_current_old!(u, S, p)
  L, N, J, κ, dx, order = p
  r = view(u,1:N)
  v = view(u,N+1:2N)
  fill!(S,0.0)
  # Evaluate number density.
  for i in 1:N
    #j, y = get_index_and_distance(r[i],dx,L)
    j, y = get_index_and_y(r[i],J,L)
    #if j < 1 || j > J
    #    error("j = $j")
    #end
    if order == 1
      S[j] += (1. - y)*v[i] / dx;
      if (j == J)
        S[1] += y*v[i] / dx
      else
        S[j+1] += y*v[i] / dx
      end
    end
    if order == 2
      if y <= 1/2
        S[j] += (3/4 - y^2) *v[i] / dx
        if j == 1
          S[J] += (1-2y)^2/8 *v[i] / dx
          S[2] += (1+2y)^2/8 *v[i] / dx
        elseif j==J
          S[1] += (1+2y)^2/8 *v[i] / dx
          S[J-1] += (1-2y)^2/8 *v[i] / dx
        else
          S[j+1] += (1+2y)^2/8 *v[i] / dx
          S[j-1] += (1-2y)^2/8 *v[i] / dx
        end
      elseif y <= 3/2
        S[j] += (3 - 2y)^2/8 *v[i] / dx
        if j == J-1
          S[J] += (3/4 - (1-y)^2) *v[i] / dx
          S[1] += (1-2y)^2/8 *v[i] / dx
        elseif j==J
          S[1] += (3/4 - (1-y)^2) *v[i] / dx
          S[2] += (1-2y)^2/8 *v[i] / dx
        else
          S[j+1] += (3/4 - (1-y)^2) *v[i] / dx
          S[j+2] += (1-2y)^2/8 *v[i] / dx
        end
      end
    end
  end
end

function get_current!(u, S, p)
  L, N, J, κ, dx, order = p
  r = view(u,1:N)
  v = view(u,N+1:2N)
  fill!(S,0.0)

  for i in 1:N
    @inbounds j, y = get_index_and_y(r[i],J,L)
    for l in (-order):order
      @inbounds S[mod1(j + l, J)] += W(order, -y + l) * v[i] / dx;
      #S[mod1(j + l, J)] += W_alt(order, -y + l) * v[i] / dx;
    end
  end
end

function get_current_ro!(u, S, p)
  L, N, J, κ, dx, order = p
  r = view(u,1:2:2N-1)
  v = view(u,2:2:2N)
  fill!(S,0.0)

  for i in 1:N
    @inbounds j, y = get_index_and_y(r[i],J,L)
    for l in (-order):order
      @inbounds S[mod1(j + l, J)] += W(order, -y + l) * v[i] / dx;
    end
  end
end

function get_current(u, S, p)
  L, N, J, κ, dx, order = p
  r = view(u,1:N)
  v = view(u,N+1:2N)
  fill!(S,0.0)

  for i in 1:N
    @inbounds j, y = get_index_and_y(r[i],J,L)
    for l in (-order):order
      @inbounds  S[mod1(j + l, J)] += W(order, -y + l) * v[i] / dx;
    end
  end
  return S[:]
end

function get_mini_current(pp, S, p)
  L, N, J, κ, dx, order = p
  fill!(S,0.0)
    j, y = get_index_and_y(pp.r,J,L)
    for l in (-order):order
      S[mod1(j + l, J)] = W(order, -y + l) * pp.v / dx;
    end
  return S[:]
end

function get_current_threads!(u, S, p)
  L, N, J, κ, dx, order, TS = p
  j = fill(Int64(1),nthreads())
  y = fill(Float64(1.0),nthreads())
  TS .= zeros(Float64)
  @threads for i in 1:N
    @inbounds j[threadid()], y[threadid()] = get_index_and_y(u[i], J, L)
    for l in (-order):-j[threadid()]
      @inbounds TS[J + j[threadid()] + l, threadid()] += W(order, -y[threadid()] + l) * u[N+i] / dx;
    end
    for l in max(-order,-j[threadid()]+1):min(order,J-j[threadid()])
      @inbounds TS[j[threadid()] + l, threadid()] += W(order, -y[threadid()] + l) * u[N+i] / dx;
    end
    for l in J-j[threadid()]+1:order
      @inbounds TS[j[threadid()] - J + l, threadid()] += W(order, -y[threadid()] + l) * u[N+i] / dx;
    end
    # for l in (-order):order
    #   @inbounds TS[mod1(j + l, J), threadid()] += W(order, -y + l) * v[i] / dx;
    # end
  end

  S .= zeros(Float64)
  @threads for i in 1:J
    for t in 1:nthreads()
      @inbounds S[i] += TS[i, t]
    end
  end
  S
end

function get_current_ro_par(Dpars::DArray, DS::DArray, p)
  L, N, J, κ, dx, order = p
  ran_pars = first(localindices(Dpars))
  #assuming ran_pars[1] is odd
  odd_range = first(ran_pars):2:last(ran_pars)
  LS = DS[:L]
  fill!(LS,0.0)
  for i in odd_range
    j, y = get_index_and_y(Dpars[i],J,L)
    for l in (-order):order
      LS[mod1(j + l, J),:] .+= W(order, -y + l) * Dpars[i+1] / dx;
    end
  end
end

"""
Calculates the RHS of the evolution equation.
Returns du
uses several functions which are passed as parameters
p = N, J, L, dx, Density!, Electric!, Poisson1D!
"""
function RHS(u,t,p_RHC)
    N, J, L, dx, n, E, ϕ, du, Density!, Electric!, Poisson1D! = p_RHC
    p = L, N, J, κ, dx
    get_density!(u[1:N], n, p)
    n0 = N/L
    get_ϕ!(ϕ, n/n0 .-1.0,κ)
    get_E_from_ϕ!(ϕ,E,dx)

    for i in 1:N
        #j, y = get_index_and_distance(u[i],dx,L)
        j, y = get_index_and_y(u[i],J,L)
        if (j == J)
            Efield = E[j] * (1. - y) + E[1] * y;
        else
            Efield = E[j] * (1. - y) + E[j+1] * y;
        end

        du[i] = u[N+i]
        du[N+i] = - Efield
    end
    return du[:]
end

"""
Calculates the RHS of the evolution equation.
Returns du
uses several functions which are passed as parameters
p = N, J, L, dx, n, S, du, get_current!
"""
function RHSC_old(u,t,p_RHSC)
  N, J, L, dx, order, n, S, du, get_density!, get_current! = p_RHSC
    p = L, N, J, κ, dx, order
    #get_density!(u, n, p)
    get_current!(u, S, p)
    E = view(u,2N+1:2N+J)
    n0 = N/L

    for i in 1:N
        #j, y = get_index_and_distance(u[i],dx,L)
        j, y = get_index_and_y(u[i],J,L)

        if order == 1
          if (j == J)
              Efield = E[j] * (1. - y) + E[1] * y;
          else
              Efield = E[j] * (1. - y) + E[j+1] * y;
          end
        elseif order == 2
          if y <= 1/2
            if j == 1
              Efield = E[j] * (3/4 - y^2) + E[2] * (1+2y)^2/8 + E[J] * (1-2y)^2/8
            elseif j==J
              Efield = E[j] * (3/4 - y^2) + E[1] * (1+2y)^2/8 + E[J-1] * (1-2y)^2/8
            else
              Efield = E[j] * (3/4 - y^2) + E[j+1] * (1+2y)^2/8 + E[j-1] * (1-2y)^2/8
            end
          elseif y <= 3/2
            if j == J-1
              Efield = E[j] * (3 - 2y)^2/8 + E[J] * (3/4 - (1-y)^2) + E[1] * (1-2y)^2/8
            elseif j == J
              Efield = E[j] * (3 - 2y)^2/8 + E[1] * (3/4 - (1-y)^2) + E[2] * (1-2y)^2/8
            else
              Efield = E[j] * (3 - 2y)^2/8 + E[j+1] * (3/4 - (1-y)^2) + E[j+2] * (1-2y)^2/8
            end
          end
        end

        du[i] = u[N+i]
        du[N+i] = - Efield
    end

      for j in 1:J
        du[2N+j] =  S[j]/n0 # particles have negative sign!
      end

    return du[:]
end

function RHSC(u,t,p_RHSC)
  if nthreads() == 1
    N, J, L, dx, order, n, S, du, get_density!, get_current!, Interpolate = p_RHSC
    p = L, N, J, κ, dx, order
    get_current!(u, S, p)
  else
    N, J, L, dx, order, n, S, du, get_density!, get_current!, Interpolate, TS = p_RHSC
    p = L, N, J, κ, dx, order, TS
    get_current_threads!(u, S, p)
  end

    #get_density!(u, n, p)

    E = view(u,2N+1:2N+J)
    n0 = N/L

    #@threads for i in 1:N
    for i in 1:N
        #j, y = get_index_and_y(u[i],J,L)
        #Efield = 0.0
        #for l in (-order):order
        #  Efield += E[mod1(j+l,J)] * W(order, -y + l)
        #end

        @inbounds du[i] = u[N+i]
        @inbounds du[N+i] = - Interpolate(order, E, u[i], J, L)
    end

    #@threads for j in 1:J
    for j in 1:J
        @inbounds du[2N+j] =  S[j]/n0 # particles have negative sign!
    end
    return du[:]
end

function Coordinate_test(r,L)
    if minimum(r) < 0.0
        error("negative coordinates")
    end
    if maximum(r) > L
        error("coordintates extend beyond L")
    end
end

function make_periodic_old!(r,L)
    N = length(r)
    for i in 1:N
      if (r[i] < 0.)
        r[i] = r[i] + L;
      end
      if (r[i] > L)
        r[i] = r[i] - L;
      end
    end
    return r[:]
end

function make_periodic!(r,L)
  return mod1.(r,L)
end

function RK4_Step!(f,y0,t0,h,p)
    k1 = h*f(y0,t0,p)
    k2 = h*f(y0+0.5*k1, t0+0.5*h,p)
    k3 = h*f(y0+0.5*k2, t0+0.5*h,p)
    k4 = h*f(y0+k3, t0+h,p)
    y0 .= y0 + (k1 + 2k2 + 2k3 + k4)/6
end

function get_index_and_distance(s,dx,L)
    #if s < 0
    #    s = s + L
    #end
    #if s > L
    #    s = s - L
    #end
    s = mod1(s,L)
    j = floor(Int64, s ÷ dx) + 1 #find the grid space where it is.
    if j > J || j < 1
      error("j = $j")
    end
    y = (s % dx)/dx #how far is there
    return j, y
end

"""
given a number s in between x_j and x_{j+1} computes y = (s - x_j)/dx and return j and y.
get_index_and_y(0.4,2,1)
dx = 0.5, j = 1, y = 0.4/0.5 = 0.8
get_index_and_y(0.7,2,1) = j = 2, 0.2/0.5= 0.4
"""
function get_index_and_y(s,J,L)
  s = (s/L*J + J)%J
  j = floor(Int,s) + 1
  #j = convert(Int64,s) + 1
  y = (s%1)
  return j, y
end

function get_energy(u,p)
  L, N, J = p
  dx = L/J
  n0 = N/L
  energy_K = 0.0
  energy_E = 0.0
  for i in 1:N
    energy_K = energy_K + u[N+i]^2
  end
  for j in 1:J
    energy_E = energy_E + u[2N+j]^2
  end

  return energy_K/2,  dx*energy_E /2 * n0
end

"""
Derivatives of shape functions
"""
function W(order::Int,y::Float64)
  y = abs(y)
  if order == 0
    return  (y <= 1/2) ? 1 : 0
  elseif order ==1
    return  (y <= 1) ? 1 - y : 0
  elseif order == 2
    return (y <= 1/2) ? 3/4 - y^2  : (((y > 1/2) && (y <= 3/2)) ? (3 - 2*y)^2 / 8 : 0)
  elseif order == 3
    return (y <= 1) ? 2/3 - y^2 + y^3 / 2 : (((y > 1) && (y <= 2)) ? (2 - y)^3 / 6 : 0)
  elseif order == 4
    return (y <= 1/2) ? 115/192 - 5y^2/8 + y^4/4 : (((y > 1/2) && (y <= 3/2)) ? (55 + 20y -120y^2 + 80y^3 - 16y^4)/96 : (((y > 3/2) && (y < 5/2)) ? (5 - 2y)^4/384 : 0))
  elseif order == 5
    return (y <= 1) ? 11/20 - y^2/2 + y^4/4 - y^5/12 : (((y > 1) && (y <= 2)) ? 17/40 + 5y/8 - 7y^2/4 + 5y^3/4 - 3y^4/8 + y^5/24 : (((y > 2) && (y < 3)) ? (3 - y)^5/120 : 0))
  else
    error("order = $order not yet implemented ")
  end
end

"""
Alternative definition
"""
function W_alt(order::Int,y::Float64)
  y = abs(y)
  if order == 0
    return  (y > 1/2) ? 0 : 1
  elseif order ==1
    return  (y > 1) ? 0 : 1 - y
  elseif order == 2
    #return (y <= 1/2) ? 3/4 - y^2  : (((y > 1/2) && (y <= 3/2)) ? (3 - 2*y)^2 / 8 : 0)
    return (y > 3/2) ? 0 : (((y > 1/2) && (y <= 3/2)) ? (3 - 2*y)^2 / 8 : 3/4 - y^2)
  elseif order == 3
    #return (y <= 1) ? 2/3 - y^2 + y^3 / 2 : (((y > 1) && (y <= 2)) ? (2 - y)^3 / 6 : 0)
    return (y > 2) ? 0 : (((y > 1) && (y <= 2)) ? (2 - y)^3 / 6 : 2/3 - y^2 + y^3 / 2)
  elseif order == 4
    #return (y <= 1/2) ? 115/192 - 5y^2/8 + y^4/4 : (((y > 1/2) && (y <= 3/2)) ? (55 + 20y -120y^2 + 80y^3 - 16y^4)/96 : (((y > 3/2) && (y < 5/2)) ? (5 - 2y)^4/384 : 0))
    return (y > 5/2) ? 0 : (((y > 3/2) && (y < 5/2)) ? (5 - 2y)^4/384 : (((y > 1/2) && (y <= 3/2)) ? (55 + 20y -120y^2 + 80y^3 - 16y^4)/96 : 115/192 - 5y^2/8 + y^4/4))
  elseif order == 5
    #return (y <= 1) ? 11/20 - y^2/2 + y^4/4 - y^5/12 : (((y > 1) && (y <= 2)) ? 17/40 + 5y/8 - 7y^2/4 + 5y^3/4 - 3y^4/8 + y^5/24 : (((y > 2) && (y < 3)) ? (3 - y)^5/120 : 0))
    return (y > 3) ? 0 : (((y > 2) && (y < 3)) ? (3 - y)^5/120 : (((y > 1) && (y <= 2)) ? 17/40 + 5y/8 - 7y^2/4 + 5y^3/4 - 3y^4/8 + y^5/24 : 11/20 - y^2/2 + y^4/4 - y^5/12))
  else
    error("order = $order not yet implemented ")
  end
end
"""
This are interpolation functions for getting the Electric field correct.
According the SHARP the second is better. Since it keeps momentum conservation.
"""
function Interpolate_1(order, vector, x, J, L)
  j, y = get_index_and_y(x,J,L)
  vi = 0.0
    for l in (-order+1):order
      vi += vector[mod1(j+l,J)] * W(order, -y + l)
    end
  return vi
end

function Interpolate_2(order, vector, x, J, L)
  j, y = get_index_and_y(x,J,L)
  vi = 0.0
    for l in (-order):order
      vi += (vector[mod1(j+l,J)] + vector[mod1(j+l+1,J)])/ 2 * W(order, -y + 1/2 + l)
    end
  return vi
end
"""
Terminado, pero sin probar.
"""
function Interpolate_per(order, vector, x, J, L)
  j, y = get_index_and_y(x,J,L)
  vi = 0.0
    for l in (-order):-j
      vi += vector[J+j+l] * W(order, -y + 1/2 + l)
    end
    for l in  max(-order,-j+1):min(order,J-j)
      vi += vector[j+l] * W(order, -y + 1/2 + l)
    end
    for l in J-j+1:order
      vi += vector[j+l] * W(order, -y + 1/2 + l)
    end
    # now we increase j by 1
    j += 1
    for l in (-order):-j
      vi += vector[J+j+l] * W(order, -y + 1/2 + l)
    end
    for l in  max(-order,-j+1):min(order,J-j)
      vi += vector[j+l] * W(order, -y + 1/2 + l)
    end
    for l in J-j+1:order
      vi += vector[j+l] * W(order, -y + 1/2 + l)
    end
  return vi / 2
end

"""
Structure and functions to work with particles
This is probably much faster for the position and
velocity are adjacent in memory.
"""

mutable struct Particles
  r ::Float64
  v ::Float64
end

#pars = Vector{Particles}(undef, N)
#par1 = par(0.0,0.0)

#pp = fill(par1,N)
"""
Makes particles out of a vector with positions first and then
velocities
"""

function make_particles!(par_dis, pars)
  N = length(par_dis)÷2
  for i in 1:N
      pars[i] = Particles(par_dis[i], par_dis[N+i])
  end
end

"""
reorder particle vector so that position and velocity are contiguous
"""
function reorder_particles!(u,uro)
  N = length(u)÷2
  for i in 1:N
    uro[2i - 1] = u[i]
    uro[2i] = u[N+i]
  end
end


# wait for ReverseDiffSource to be updated to v0.5
# import ReverseDiffSource
# import ReverseDiffSource:rdiff

using Calculus: differentiate

export AnalyticPotential

import FunctionWrappers
import FunctionWrappers: FunctionWrapper
typealias F64fun FunctionWrapper{Float64, Tuple{Float64}}

# ===========================================================================
#     macro that takes an expression, differentiates it
#     and returns anonymous functions for the derivatives
# ===========================================================================

"""
`diff2(ex::Expr)` :

takes an expression, differentiates it twice,
     and returns anonymous functions for the derivatives
"""
function diff2(ex::Expr, sym=:r)
   ex_d = differentiate(ex, sym)
   ex_dd = differentiate(ex_d, sym)
   return eval( :( (r->$ex, r->$ex_d, r->$ex_dd)) )
end

# ================== Analytical Potentials ==========================
#
# TODO: this construction should not be restricted to pair potentials
#       but need a better model

# documentation attached below
@pot type AnalyticPotential <: PairPotential
   f::F64fun
   f_d::F64fun
   f_dd::F64fun
   id::AbstractString
   cutoff::Float64
end

"""
`type AnalyticPotential <: PairPotential`

### Usage:
```julia
lj = AnalyticPotential(:(r^(-12) - 2.0*r^(-6)), "LennardJones")
println(lj)   # will output `LennardJones`
A = 4.0; r0 = 1.234
morse = AnalyticPotential("exp(-2.0*\$A*(r/\$r0-1.0)) - 2.0*exp(-\$A*(r/\$r0-1.0))",
                           "Morse(A=\$A,r0=\$r0)")
```

use kwarg `cutoff` to set a cut-off, default is `Inf`
"""
AnalyticPotential

evaluate(p::AnalyticPotential, r) = p.f(r)
evaluate_d(p::AnalyticPotential, r) = p.f_d(r)
evaluate_dd(p::AnalyticPotential, r) = p.f_dd(r)
Base.print(io::Base.IO, p::AnalyticPotential) = print(io, p.id)
cutoff(p::AnalyticPotential) = p.cutoff

# construct from string or expression
AnalyticPotential(s::AbstractString; id = s, cutoff=Inf) =
                        AnalyticPotential(parse(s), id=id, cutoff=cutoff)

function AnalyticPotential(ex::Expr; id = string(ex), cutoff=Inf)
   @assert isa(id, AbstractString)
   f, f_d, f_dd = diff2(ex)
   return AnalyticPotential(F64fun(f), F64fun(f_d), F64fun(f_dd), id, cutoff)
end

# this is a hack to make tight-binding work; but it should be reconsidered
# right now I am thinking it is actually ok as is!
evaluate(p::AnalyticPotential, r, R) = evaluate(p, r)
evaluate_d(p::AnalyticPotential, r, R) = evaluate_d(p, r)


#! /usr/bin/julia

# Rosetta Code, Order two numerical lists

function isallreal{T<:AbstractArray}(a::T)
    all(map(x->isa(x, Real), a))
end

function islexfirst{T<:AbstractArray,U<:AbstractArray}(a::T, b::U)
    isallreal(a) && isallreal(b) || throw(DomainError())
    for i in 1:min(length(a), length(b))
        x = a[i]
        y = b[i]
        x != y || continue
        return x < y
    end
    return length(a) < length(b)
end

tests = {[1, 2, 3],
         primes(10),
         0:2:6,
         [-Inf, 0.0, Inf],
         [π, e, φ, catalan],
         [2015, 5],
         [-sqrt(50.0), 50.0^2],
         }

println("Testing islexfirst:")
for (a, b) in combinations(tests, 2)
    tres = islexfirst(a, b) ? " is " : " is not "
    tres *= "lexically prior to\n    "
    println("\n    ", a, tres, b)
end


"""
$(TYPEDEF)

An equality relationship between two expressions.

# Fields
$(FIELDS)
"""
struct Equation
    """The expression on the left-hand side of the equation."""
    lhs
    """The expression on the right-hand side of the equation."""
    rhs
end
Base.:(==)(a::Equation, b::Equation) = all(isequal.((a.lhs, a.rhs), (b.lhs, b.rhs)))
Base.hash(a::Equation, salt::UInt) = hash(a.lhs, hash(a.rhs, salt))

Base.show(io::IO, eq::Equation) = print(io, eq.lhs, " ~ ", eq.rhs)

SymbolicUtils.simplify(x::Equation; kw...) = simplify(x.lhs; kw...) ~ simplify(x.rhs; kw...)

lhss(xs) = map(x->x.lhs, xs)
rhss(xs) = map(x->x.rhs, xs)

"""
$(TYPEDSIGNATURES)

Create an [`Equation`](@ref) out of two [`Num`](@ref) instances, or an
`Num` and a `Number`.

# Examples

```jldoctest
julia> using Symbolics

julia> @variables x y;

julia> @variables A[1:3, 1:3] B[1:3, 1:3];

julia> x ~ y
x ~ y

julia> x - y ~ 0
x - y ~ 0

julia> A .~ B
3×3 Array{Equation,2}:
 A₁ˏ₁ ~ B₁ˏ₁  A₁ˏ₂ ~ B₁ˏ₂  A₁ˏ₃ ~ B₁ˏ₃
 A₂ˏ₁ ~ B₂ˏ₁  A₂ˏ₂ ~ B₂ˏ₂  A₂ˏ₃ ~ B₂ˏ₃
 A₃ˏ₁ ~ B₃ˏ₁  A₃ˏ₂ ~ B₃ˏ₂  A₃ˏ₃ ~ B₃ˏ₃

julia> A .~ 3x
3×3 Array{Equation,2}:
 A₁ˏ₁ ~ 3x  A₁ˏ₂ ~ 3x  A₁ˏ₃ ~ 3x
 A₂ˏ₁ ~ 3x  A₂ˏ₂ ~ 3x  A₂ˏ₃ ~ 3x
 A₃ˏ₁ ~ 3x  A₃ˏ₂ ~ 3x  A₃ˏ₃ ~ 3x
```
"""
Base.:~(lhs::Num, rhs::Num) = Equation(value(lhs), value(rhs))
Base.:~(lhs::Num, rhs::Number    ) = Equation(value(lhs), value(rhs))
Base.:~(lhs::Number    , rhs::Num) = Equation(value(lhs), value(rhs))
Base.:~(lhs::Symbolic, rhs::Symbolic) = Equation(value(lhs), value(rhs))
Base.:~(lhs::Symbolic, rhs::Any    ) = Equation(value(lhs), value(rhs))
Base.:~(lhs::Any, rhs::Symbolic    ) = Equation(value(lhs), value(rhs))

struct ConstrainedEquation
  constraints
  eq
end

function expand_derivatives(eq::Equation, simplify=false)
    return Equation(expand_derivatives(eq.lhs, simplify), expand_derivatives(eq.rhs, simplify))
end


export vertices, bonds, distance, unit, isconnected
export viz

"""
    AbstractSites

Abstrat type for all lattice or non-lattice atom collections.
The interface includes

* `vertices`
* `bonds`
* `Base.getindex`
"""
abstract type AbstractSites end
abstract type AbstractLattice <: AbstractSites end

"""
    vertices(sites)

Get the vertices for the site collection.
"""
function vertices end

"""
    isconnected(lattice, i, j; kwargs...)

return true if sites `i` and `j` are connected.
"""
function bonds end

"""
    bond(sites, i, j)

Get the bond (a pair of tuples) for the site `i` and `j`.
One can also access it with `sites[i; j]`.
"""
function bond(lt::AbstractSites, loc1::Tuple, loc2::Tuple)
    lt[loc1...], lt[loc2...]
end
function bond(lt::AbstractSites, loc1, loc2)
    lt[loc1], lt[loc2]
end
function bond(lt::AbstractSites, loc1::CartesianIndex, loc2::CartesianIndex)
    lt[loc1.I...], lt[loc2.I...]
end

Base.typed_vcat(lt::AbstractSites, loc1, loc2) = bond(lt, loc1, loc2)

distance(i::Tuple{T,T}, j::Tuple{T,T}) where T<:Real = sqrt((i[1] - j[1])^2 + (i[2] - j[2])^2)
Base.length(lt::AbstractSites) = length(vertices(lt))
Base.CartesianIndices(lt::AbstractLattice) = CartesianIndices(size(lt))
Base.LinearIndices(lt::AbstractLattice) = LinearIndices(size(lt))

"""
    bonds(atoms)

Get the bonds for the atom collection.
"""
function bonds(ud::AbstractSites)
    edges = Tuple{Int,Int}[]
    n = length(ud)
    for i = 1:n
        for j = i+1:n
            if isconnected(ud, i, j)
                push!(edges, (i,j))
            end
        end
    end
    return edges
end

function viz(lt::AbstractSites; line_style=bondstyle(:default, stroke("black"), linewidth(3*unit(lt)*mm)),
             node_style=nodestyle(:default, r=unit(lt)*0.2, stroke("black"), fill("white"), linewidth(3*unit(lt)*mm)),
             text_style=textstyle(:default, fontsize(unit(lt)*100pt)), labels=vertices(lt))
    empty_cache!()
    for (i,node) in enumerate(vertices(lt))
        node_style >> lt[node]
        text_style >> (lt[node], "$(labels[i])")
    end
    for bond in bonds(lt)
        line_style >> lt[bond[1]; bond[2]]
    end
    flush!()
end

function Base.show(io::IO, mime::MIME"text/html", lt::AbstractSites)
    Base.show(io, mime, viz(lt))
end

include("unitdisk.jl")
include("parallelogram.jl")
include("squarelattice.jl")
include("chimera.jl")


#!/usr/bin/env julia
# Python and Advanced Data Structures
# https://teaching.healthtech.dtu.dk/22110/index.php/Python_and_Advanced_Data_Structures

# Ex 1
function read_experiment!(outdict, f)
    for line = eachline(f)
        line_split = split(line, "\t")
        key = string(line_split[1])
        nums = [parse(Float64, x) for x in line_split[2:end]]
        outdict[key] = append!(get(outdict, key, Float64[]), nums)
    end
end

function read_experiments(out, files...)
    expdict = Dict{String,Array{Float64,1}}()
    for file in files
        open(f->read_experiment!(expdict, f), file)
    end
    for (key, value) in sort(collect(expdict))
        print(out, key, "\t")
        println(out, join(value, "\t"))
    end
end

open("data/test.dat", "w") do f
    read_experiments(f, "data/test1.dat", "data/test2.dat", "data/test3.dat")
end

# Ex 2

matrix = readdlm("data/matrix.dat", '\t', Int, '\n')
#matrix1 = readdlm("data/matrix1", '\t', Int, '\n')
#matrix2 = readdlm("data/matrix2", '\t', Int, '\n')
#dot = matrix1*matrix2
transpose(matrix)


using DiffEqBase: DEProblem, ODEProblem
using ProgressMeter

macro showprogress_if(pred, args...)
    quote
        if $(esc(pred))
            @showprogress $(map(esc, args)...)
        else
            $(map(esc, args)...)
        end
    end
end

objname(f) = rsplit(string(f), "."; limit=2)[end]

function is_semi_unitary(U, error=1e-7)
    n, m = size(U)
    I = n > m ? U' * U : U * U'
    diff = abs.(I .- eye(I))
    sum(diff) / length(diff) <= error
end

ceil_if(prop, x) = prop ? ceil(Int, x) : x

function default_Q0(T::DataType, dim_phase, dim_lyap)
    Q0 = zeros(T, dim_phase, dim_lyap)
    for i in 1:dim_lyap
        Q0[1:dim_phase - i + 1, i] = 1
    end
    return qr(Q0)[1]
end

default_Q0(array::AbstractArray, dim_phase, dim_lyap) =
    default_Q0(eltype(array), dim_phase, dim_lyap)

default_Q0(prob::DEProblem, dim_phase, dim_lyap) =
    default_Q0(prob.u0, dim_phase, dim_lyap)

default_Q0(x, dim) = default_Q0(x, dim, dim)


using Tmatrix
using Zygote
using JuMP
using Ipopt

function some_target_to_minimize_for_input_Tmatrix(r_array)
    # println(r_array)
    # println("I am <<some_target_to_minimize_for_input_Tmatrix>>, and the type of r_array is $(typeof(r_array))")
    # println("r_array = $r_array")

    T = Tmatrix.T_matrix_SeparateRealImag_arbitrary_mesh(
        n_max, wl_or_freq_input, input_unit, Eps_r_r_1, Eps_r_i_1, Mu_r_r_1, Mu_r_i_1, Eps_r_r_2, Eps_r_i_2, Mu_r_r_2, Mu_r_i_2,
        (collect(r_array)), θ_array, ϕ_array,
        rotationally_symmetric, symmetric_about_plane_perpendicular_z, BigFloat_precision
    )
    k1_complex = Tmatrix.get_WaveVector(
        wl_or_freq_input;
        input_unit=input_unit,
        Eps_r=Complex(Eps_r_r_1, Eps_r_i_1),
        Mu_r=Complex(Mu_r_r_1, Mu_r_i_1)
    )
    # return abs(T[1]) # if we need to minimize the first element of T-matrix

    # return sum(abs.(T)) # if we need to minimize the sum of all real and imag

    return Tmatrix.get_OrentationAv_scattering_CrossSection_from_Tmatrix(T, k1_complex) # if we need to minimize scattering cross section

    """
    return -1 * Tmatrix.get_OrentationAv_emissivity_from_Tmatrix(
        T,
        Complex(k1_r, k1_i),
        Tmatrix.calculate_surface_area_of_axisymmetric_particle(r_array, θ_array)
    ) # if we need to minimize emissivity
    """
end

function ∂some_target_to_minimize_for_input_Tmatrix(r_array)
    # println("I am <<∂some_target_to_minimize_for_input_Tmatrix>>, and the type of r_array is $(typeof(r_array))")
    # println("r_array = $r_array")
    return Zygote.gradient(some_target_to_minimize_for_input_Tmatrix, (collect(r_array)))[1]
end

rx, rz = 1e-6, 1e-6
wl_or_freq_input = 1e-6; input_unit = "m"
n_max = 1
Eps_r_r_1 = 1.0; Eps_r_i_1 = 0.0
Eps_r_r_2 = 1.5; Eps_r_i_2 = 0.01
Mu_r_r_1 = 1.0; Mu_r_i_1 = 0.0
Mu_r_r_2 = 1.0; Mu_r_i_2 = 0.0

####################################################################
# trying JuMP with some_target_to_minimize_for_input_Tmatrix
function some_target_to_minimize_for_input_Tmatrix__(r_array...)
    # println("this is <<some_target_to_minimize_for_input_Tmatrix__>>, collect(r_array) = $(collect(r_array))")
    # println("r_array = $r_array")

    return some_target_to_minimize_for_input_Tmatrix(collect((r_array)))
end
function ∂some_target_to_minimize_for_input_Tmatrix__(r_array...)
    # println("this is <<∂some_target_to_minimize_for_input_Tmatrix__>>, collect(r_array) = $(collect(r_array))")
    # println("r_array = $r_array")
    # println("r_array[1] = $(r_array[1])")
    # println("r_array[2:end] = $(r_array[2:end])")

    return ∂some_target_to_minimize_for_input_Tmatrix(collect((r_array[2:end])))
end

n_θ_points = 10
θ_array = collect(LinRange(1e-6, pi, n_θ_points))
r_array = rx * ones(size(θ_array))
ϕ_array = zeros(size(θ_array))
rotationally_symmetric = true
symmetric_about_plane_perpendicular_z = false
BigFloat_precision = nothing

model = Model(Ipopt.Optimizer)
set_optimizer_attributes(model, "tol" => 1e-16, "max_iter" => 10)
set_time_limit_sec(model, 3000)
@variable(model, r[1:n_θ_points])
for r_here in r
    JuMP.set_lower_bound(r_here, 0.1e-7)
    JuMP.set_upper_bound(r_here, 5e-6)
    JuMP.set_start_value(r_here, 4e-6)
end
# set_start_value.(r, 0.5e-7*ones(n_θ_points).+rand(n_θ_points)*1e-8)
JuMP.register(model, :some_target_to_minimize_for_input_Tmatrix__, n_θ_points, some_target_to_minimize_for_input_Tmatrix__, ∂some_target_to_minimize_for_input_Tmatrix__)
@NLobjective(model, Max, some_target_to_minimize_for_input_Tmatrix__(r...))
optimize!(model)
println("optimum r is $(value.(r))")


using Base.Test
using QuantumOptics

T = [0.:0.1:0.5;]

# Define Spin 1/2 operators
spinbasis = SpinBasis(1//2)
I = dense_identity(spinbasis)
sigmax = full(spin.sigmax(spinbasis))
sigmay = full(spin.sigmay(spinbasis))
sigmaz = full(spin.sigmaz(spinbasis))
sigmap = full(spin.sigmap(spinbasis))
sigmam = full(spin.sigmam(spinbasis))
I_spin = identity(spinbasis)

N = 2
b = tensor([spinbasis for i=1:N]...)

# psi0 = 1./sqrt(2)*(spinup(spinbasis) + spindown(spinbasis))
psi0 = normalize(spinup(spinbasis) + 0.5*spindown(spinbasis))
rho0 = cumulantexpansion.ProductDensityOperator([psi0 for i=1:N]...)

Ω = [1. 2. 3.;
     2. 1. 4.;
     3. 4. 1.]
# Ω = zeros(Float64, N, N)
γ = 1.
δ = 0.2

H = LazyTensor[LazyTensor(b, i, sigmaz, 0.5*δ) for i=1:N]
for i=1:N, j=1:N
    if i==j
        continue
    end
    h = LazyTensor(b, [i, j], [sigmap, sigmam], Ω[i, j])
    push!(H, h)
end
H = LazySum(H...)
J = LazyTensor[LazyTensor(b, i, sigmam, γ) for i=1:N]
# J = LazyTensor[]

tout, rho_t = cumulantexpansion.master(T, rho0, H, J)
tout, rho_t_full = timeevolution.master(T, full(rho0), full(H), map(full, J))

type ProductState
    N::Int
    data::Vector{Float64}
end

ProductState(N::Int) = ProductState(N, zeros(Float64, 3*N))

function ProductState(rho::DenseOperator)
    state = ProductState(N)
    sx, sy, sz = splitstate(state)
    f(ind, op) = real(expect(embed(b, ind, op), rho))
    for k=1:N
        sx[k] = f(k, sigmax)
        sy[k] = f(k, sigmay)
        sz[k] = f(k, sigmaz)
    end
    combinestate(sx, sy, sz, state.data)
    return state
end

splitstate(N::Int, data::Vector{Float64}) = vec(data[0*N+1:1*N]), vec(data[1*N+1:2*N]), vec(data[2*N+1:3*N])
splitstate(state::ProductState) = splitstate(state.N, state.data)
function combinestate(sx::Vector{Float64}, sy::Vector{Float64}, sz::Vector{Float64}, state::Vector{Float64})
    state[0*N+1:1*N] = sx
    state[1*N+1:2*N] = sy
    state[2*N+1:3*N] = sz
    state
end

function f(t, y::Vector{Float64}, dy::Vector{Float64})
    sx, sy, sz = splitstate(N, y)
    dsx, dsy, dsz = splitstate(N, dy)
    @inbounds for k=1:N
        dsx[k] = -δ*sy[k] - 0.5*γ*sx[k]
        dsy[k] = δ*sx[k] - 0.5*γ*sy[k]
        dsz[k] = -γ*(1+sz[k])
        for j=1:N
            if j==k
                continue
            end
            dsx[k] += Ω[k,j]*sy[j]*sz[k]
            dsy[k] += -Ω[k,j]*sx[j]*sz[k]
            dsz[k] += Ω[k,j]*(sx[j]*sy[k] - sy[j]*sx[k])
        end
    end
    combinestate(dsx, dsy, dsz, dy)
end

t_out = Float64[]
state_out = ProductState[]

function fout_(t, y::Vector{Float64})
    push!(t_out, t)
    push!(state_out, ProductState(N, deepcopy(y)))
end

function densityoperator(sx::Real, sy::Real, sz::Real)
    return 0.5*(I + sx*sigmax + sy*sigmay + sz*sigmaz)
end
function densityoperator(state::ProductState)
    sx, sy, sz = splitstate(state)
    rho = densityoperator(sx[1], sy[1], sz[1])
    for i=2:state.N
        rho = tensor(rho, densityoperator(sx[i], sy[i], sz[i]))
    end
    return rho
end

state0 = ProductState(full(rho0))
QuantumOptics.ode_dopri.ode(f, T, state0.data, fout_)

@test tracedistance(densityoperator(state_out[end]), full(rho_t[end])) < 1e-6
# println(tracedistance(densityoperator(state_out[end]), full(rho_t_full[end])))
@test tracedistance(full(rho_t[end]), full(rho_t[1])) > 0.1


#
# Minimize the acceleration of a composite Bézier curve on the Sphere S2 with approximation
#
# This example appeared in Sec. 5.2, second example, of
#
# > Bergmann, R. and Gousenbourger, P.-Y.: _A variational model for data fitting on manifolds
# > by minimizing the acceleration of a Bézier curve_.
# > Frontiers in Applied Mathematics and Statistics, 2018.
# > doi: [10.3389/fams.2018.00059](https://dx.doi.org/10.3389/fams.2018.00059),
# > arXiv: [1807.10090](https://arxiv.org/abs/1807.10090)
#
using Manopt, Manifolds, Colors, ColorSchemes
experiment_name = "Bezier_Approximation"
results_folder = joinpath(@__DIR__, "Minimize_Acceleration_Bezier")
!isdir(results_folder) && mkdir(results_folder)
asy_export = true #export data and results to asyExport
λ = 10.0

curve_samples = [range(0, 3; length=101)...] # sample curve for the gradient
curve_samples_plot = [range(0, 3; length=201)...] # sample curve for asy exports
camera_position = (-1.0, -0.7, 0.3)
curveColor = RGBA{Float64}(colorant"#000000")
sColor = RGBA{Float64}(colorant"#BBBBBB")
dColor = RGBA{Float64}(colorant"#EE7733") # data Color: Tol Vibrant Orange
pColor = RGBA{Float64}(colorant"#0077BB") # control point data color: Tol Virbant Blue
ξColor = RGBA{Float64}(colorant"#33BBEE") # tangent vector: Tol Vibrant blue
bColor = RGBA{Float64}(colorant"#009988") # inner control points: Tol Vibrant teal
#
# Data
#
M = Sphere(2)
B = artificial_S2_composite_bezier_curve()
cP = de_casteljau(M, B, curve_samples_plot)
dataP = get_bezier_junctions(M, B)
# export original data.
if asy_export
    asymptote_export_S2_signals(
        joinpath(results_folder, experiment_name * "-orig.asy");
        curves=[cP],
        points=[get_bezier_junctions(M, B), get_bezier_inner_points(M, B)],
        tangent_vectors=[[
            Tuple(a) for a in
            zip(get_bezier_junctions(M, B, true), get_bezier_junction_tangent_vectors(M, B))
        ]],
        colors=Dict(
            :curves => [curveColor], :points => [dColor, bColor], :tvectors => [ξColor]
        ),
        camera_position=camera_position,
        arrow_head_size=10.0,
        line_widths=[1.5, 1.5],
        dot_size=4.0,
    )
    render_asymptote(joinpath(results_folder, experiment_name * "-orig.asy"); render=4)
end
pB = get_bezier_points(M, B, :differentiable)
N = PowerManifold(M, NestedPowerRepresentation(), length(pB))
function F(pB)
    return cost_L2_acceleration_bezier(
        M, pB, get_bezier_degrees(M, B), curve_samples, λ, dataP
    )
end
∇F(pB) = ∇L2_acceleration_bezier(M, pB, get_bezier_degrees(M, B), curve_samples, λ, dataP)
x0 = pB
pB_opt = gradient_descent(
    N,
    F,
    ∇F,
    x0;
    stepsize=ArmijoLinesearch(1.0, ExponentialRetraction(), 0.5, 0.001), # use Armijo lineSearch
    stopping_criterion=StopWhenAny(
        StopWhenChangeLess(10.0^(-5)),
        StopWhenGradientNormLess(10.0^-7),
        StopAfterIteration(300),
    ),
    debug=[
        :Stop,
        :Iteration,
        " | ",
        :Cost,
        " | ",
        DebugGradientNorm(),
        " | ",
        DebugStepsize(),
        " | ",
        :Change,
        "\n",
        25,
    ],
)
B_opt = get_bezier_segments(M, pB_opt, get_bezier_degrees(M, B), :differentiable)
res_curve = de_casteljau(M, B_opt, curve_samples_plot)
#res_curve ./= norm.(res_curve)
if asy_export
    asymptote_export_S2_signals(
        joinpath(results_folder, experiment_name * "-result.asy");
        curves=[res_curve, cP],
        points=[get_bezier_junctions(M, B_opt), get_bezier_inner_points(M, B_opt)],
        tangent_vectors=[[
            Tuple(a) for a in zip(
                get_bezier_junctions(M, B_opt, true),
                get_bezier_junction_tangent_vectors(M, B_opt),
            )
        ]],
        colors=Dict(
            :curves => [curveColor, pColor],
            :points => [dColor, bColor],
            :tvectors => [ξColor],
        ),
        camera_position=camera_position,
        arrow_head_size=10.0,
        line_widths=[1.5, 0.75, 1.5],
        dot_size=4.0,
    )
    render_asymptote(joinpath(results_folder, experiment_name * "-result.asy"); render=4)
end


using Ballistics

function Δrange(v0::Float64, θ::Float64, target::Types.Target2D, trajectory::Function)

vx = v0*cosd(θ)
vy = v0*sind(θ)
p = scenario.createProjectile(0.03, 0.04,[vx,vy], [0.0,0.0])
trajectory(p,target)
Δrange = target.position[2]-p.position[2]

return Δrange
end

function bissection(target::Types.Target2D; θmin = 0.0, θmax = 45.0, ϵ = 0.001 )

#t2D = scenario.createTarget(0.2,[200.0,50.0])

#global θmin = 0.0
#global θmax = 45.0
v0 = 900.0
#ϵ = 0.001
#global fmin = Δrange(inputs.v0, θmin,t2D, ExternalBallistics.PmmZeroDrag.timeOfFlight!)
#global fmax = Δrange(inputs.v0, θmax,t2D, ExternalBallistics.PmmZeroDrag.timeOfFlight!)
#global error = min(abs(fmin),abs(fmax))
fmin = Δrange(inputs.v0, θmin,t2D, ExternalBallistics.PmmZeroDrag.timeOfFlight!)
fmax = Δrange(inputs.v0, θmax,t2D, ExternalBallistics.PmmZeroDrag.timeOfFlight!)
error = min(abs(fmin),abs(fmax))

if sign(fmin)==sign(fmax)
  println("Bisection method fails.")
else

  while error>ϵ

    #global θmin
    #global θmax
    #global fmin
    #global fmax
    #global error
    #global θ = 1/2*(θmin+θmax)
    θ = 1/2*(θmin+θmax)
    #println(error)

    f = Δrange(inputs.v0, θ,t2D, ExternalBallistics.PmmZeroDrag.timeOfFlight!)
    error = abs(f)
    if f*fmin < 0
            θmax = θ
        else f*fmax < 0
            θmin = θ
      end
    end

  end
  return θ

end


using Test #, Aiyagari
using QuantEcon, Parameters, Interpolations

function marginal_distribution(exo, var)
  size_exo = size(exo)
  p_reshaped = reshape(exo.mc.p, (size_exo..., size_exo...))

  names = keys(exo)
  integrate_dim_bool = names .!= var

  n_dim = length(names)

  int_dim = findall([integrate_dim_bool...])

  int_col = int_dim .+ n_dim

  sum_columns = dropdims(sum(p_reshaped, dims=int_col), dims=Tuple(int_col))
  #sum_columns[:,:,:,1]
  dropdims(mean(sum_columns, dims=int_dim), dims=Tuple(int_dim))
end

## (Conditional) expectations of value functions
abstract type Expectation end
struct Unconditional <: Expectation end
struct Conditional <: Expectation
  var::Symbol
end

function extrapolated_𝔼V(endo, itp_scheme, value, exo, i_exo, ::Unconditional)
  𝔼V0 = value * exo.mc.p[i_exo,:]

  𝔼V_itp = interpolate(reshape(𝔼V0, size(endo)), itp_scheme)

  𝔼V = extrapolate(
          scale(𝔼V_itp, Tuple(endo.grids)...),
          Interpolations.Line()
          )
end

function extrapolated_𝔼V(endo, itp_scheme, value, exo, i_exo, cond::Conditional)
  var = cond.var
  n = size(exo)[findfirst(keys(exo) .== var)]

  𝔼V0_vec = map(1:n) do i
    get_cond_𝔼V(value, exo, i_exo, var => i)
  end

  𝔼V_itp = interpolate.(𝔼V0_vec, Ref(itp_scheme))

  𝔼V_vec = extrapolate.(
          scale.(𝔼V_itp, Ref(Tuple(endo.grids))),
          Ref(Interpolations.Line())
        )

  i_cond_var = exo.indices[i_exo][var]

  π = marginal_distribution(exo, var)[i_cond_var, :]

  function 𝔼V(vec::AbstractVector)
    mapreduce(+, 1:n) do i
      π[i] * 𝔼V_vec[i](vec[i])
    end
  end
  function 𝔼V(x::Number)
    mapreduce(+, 1:n) do i
      π[i] * 𝔼V_vec[i](x)
    end
  end
  𝔼V
end

function get_cond_𝔼V(value, exo, i_exo, condition)
  len_endo = size(value,1)

  value_reshaped = reshape(value, (len_endo, size(exo)...))

  v = value_reshaped[:, [k == condition[1] ? condition[2] : Colon() for k in keys(exo)]...]

  oth_dim = keys(exo) .!= condition[1]

  len_exo_other = prod(size(exo)[[oth_dim...]])

  p = exo.mc.p
  π_ = reshape(p[i_exo,:], size(exo))[[k == condition[1] ? condition[2] : Colon() for k in keys(exo)]...]

  ∑π = sum(π_)
  π_ = ∑π == 0 ? π_ : π_ / ∑π

  reshape(v, (len_endo, len_exo_other)) * vec(π_)
end

export extrapolated_𝔼V

@testset "exogenous states" begin
  x1_grid = [0.5; 1.0; 1.5]
  x1_prob = [0.7 0.15 0.15;
             0.2 0.6 0.2;
             0.15 0.15 0.7]
  x1_MC = MarkovChain(x1_prob, x1_grid, :x1)

  x2_grid = [0.15; 1.1; 1.15; 3.0]
  x2_prob = [0.15 0.0 0.7  0.15;
             0.6  0.2 0.0  0.2;
             0.15 0.15 0.7 0.0;
             0.6  0.2 0.0  0.2]
  x2_MC = MarkovChain(x2_prob, x2_grid, :x2)

  x3_grid = [0.3; 0.6]
  x3_prob = [0.7 0.3;
             1.0 0.0]
  x3_MC = MarkovChain(x3_prob, x3_grid, :x3)

  exo = ExogenousStateSpace([x1_MC, x2_MC, x3_MC])

  @testset "marginal distribution" begin
    @test all(marginal_distribution(exo, :x1) .≈ x1_prob)
    @test all(marginal_distribution(exo, :x2) .≈ x2_prob)
    @test all(marginal_distribution(exo, :x3) .≈ x3_prob)
  end

  a_grid = LinRange(5, 10, 50)

  my_val = (a, exo) -> begin
    c = a + exo.x1 - (exo.x2 == 0.6)
    #c > 0 ? log(c) : 1 * c - 1
  end

  value = my_val.(a_grid, permutedims(exo.grid))

  @testset "conditional expectation" begin
    for i_exo in [1; 7; 14; 20; 24]
      for (i_dim, cond_dim) in enumerate([:x1, :x2, :x3])
        oth_dim = findall(keys(exo) .!= cond_dim)
        π_sub = dropdims(sum(reshape(exo.mc.p[i_exo,:], size(exo)), dims=oth_dim), dims=Tuple(oth_dim))

        𝔼V = mapreduce(+, 1:size(exo)[i_dim]) do x
         get_cond_𝔼V(value, exo, i_exo, cond_dim => x) * π_sub[x]
        end

        @test all(𝔼V .≈ value * exo.mc.p[i_exo,:])
      end
    end
  end
end


using Espresso: @simple_rule

struct Rule
    left
    right
    condition
end

# These symbols must always correspond to numbers
const NUM_PHS = [:a, :n]
integralTable = []

addMatch!(x::Symbol) = Expr(:ref, :match, QuoteNode(x))
function addMatch!(ex::Expr)
    for (i, x) ∈ enumerate(ex.args)
        if i == 1 && ex.head == :call
            continue
        elseif x isa Symbolic
            ex.args[i] = addMatch!(x)
        end
    end
    ex
end

addMatch(ex::Expr) = Expr(:->, :match, addMatch!(ex))

macro integration_rule(pat, rpat, condition = nothing)
    conditions = Any[m -> m[p] isa Number for p ∈ NUM_PHS if contains(p, pat)]
    if condition ≠ nothing
        condition = condition |> addMatch |> eval
        push!(conditions, condition)
    end

    fullCondition = _ -> true
    if !isempty(conditions)
        fullCondition = match -> all(f -> f(match), conditions)
    end

    rule = Rule(pat, rpat, fullCondition)
    push!(integralTable, rule)
    nothing
end

@simple_rule    (+)(x)     x
@simple_rule    (*)(x)     x
@simple_rule    x - -y     x + y
@simple_rule    x + -y     x - y

@integration_rule   x       x^2 / 2
@integration_rule   1/x     log(x)
@integration_rule   x^n     x^(n+1) / (n+1)       n ≠ -1
@integration_rule   a^x     a^x / log(a)
@integration_rule   exp(x)  e^x
@integration_rule   e^x     e^x
@integration_rule   exp(-x) -e^(-x)
@integration_rule   e^(-x)  -e^(-x)

@integration_rule   sin(x)  -cos(x)
@integration_rule   cos(x)  sin(x)
@integration_rule   1 / sin(x)^2      -cot(x)
@integration_rule   1 / cos(x)^2      tan(x)

@integration_rule   log(x)            x*log(x) - x
@integration_rule   1/(x*log(x))      log(log(x))
@integration_rule   x * log(x)        x^2 * (log(x)/2 - 1/4)
@integration_rule   x^n * log(x)      x^(n+1) * (log(x)/(n+1) - 1/(n+1)^2)


function logistic(r,x)
    r*x*(1.0-x)
end

x0 = 0.2
r = 3.7
results = [(x0,logistic(r,x0))];
# We've already seen arrays: [1,2,3,4]
# Here I'm using tuples: (1,2,3,4)
# Results is an array filled with tuples!
N = 100; # or any big number
for i in 1:(N-1)
  xold = results[end][2]
  xnew = logistic(r,xold)
  append!(results,[(xold,xnew)])
end

print(results)

function logisticmap(x0=0.2,r=4,n=10)
    results = [(x0,logistic(r,x0))];
    N = 100;
    for i in 1:(N-1)
      xold = results[end][2]
      xnew = logistic(r,xold)
      append!(results,[(xold,xnew)])
      print(results)
  end
  results
end

using PlotlyJS
using Distributions

labs=["1","2","3","4","5"]
N=100;
x=1:N;
y = for i in 1:(N-1)
  xold = results[end][2]
  xnew = logistic(r,xold)
  append!(results,[(xold,xnew)])
end

layout = layout(
    title = "Logistical Map"
    xaxis_title="Xn"
    yaxis_title="Xn+1"
    font_family = "Arial",
    font_size = 12
)

plot()

labs=["1"]
function logisticmap(x0,r=4,n=10)
    results = [(x0,logistic(r,x0))];
    N = 100;
    for i in 1:(N-1)
      xold = results[end][2]
      xnew = logistic(r,xold)
      append!(results,[(xold,xnew)])
      print(results)
  end
  results
end
x = xold
y = xnew

layout = layout(
    title = "Logistical Map"
    xaxis_title="Xn"
    yaxis_title="Xn+1"
    font_family ="Arial",
    font_size = 12
)

plot()

function logisticmaping()
    trace1 = scatter(;x=xold, y=xnew, mode="lines+markers")
    plot([trace1])
end

function logisticmaping()
    function logisticmap(x0=0.2,r=4,n=10)
        results = [(x0,logistic(r,x0))];
        N = 100;
        for i in 1:(N-1)
          xold = results[end][2]
          xnew = logistic(r,xold)
          append!(results,[(xold,xnew)])
          print(results)
      end
      results
    end
    trace1 = scatter(;x=xold, y=xnew, mode="lines+markers")
    plot([trace1])
end