text:
# copied from DSP.jl & simplified + multithreaded

# resample_filter output for 1//n, with attenuation = 20
const DECIM_FILTER = Dict{Int,Vector{Float32}}(
    2 => Float32[0.034286804, 0.0, -0.044083036, 0.0, 0.06171625, 0.0, -0.10286041, 0.0, 0.30858126, 0.4847183, 0.30858126, 0.0, -0.10286041, 0.0, 0.06171625, 0.0, -0.044083036, 0.0, 0.034286804],
    3 => Float32[0.021267435, 0.0, -0.025134241, -0.027647667, 0.0, 0.03455958, 0.039496664, 0.0, -0.055295333, -0.06911916, 0.0, 0.13823833, 0.27647665, 0.33431545, 0.27647665, 0.13823833, 0.0, -0.06911916, -0.055295333, 0.0, 0.039496664, 0.03455958, 0.0, -0.027647667, -0.025134241, 0.0, 0.021267435],
    4 => Float32[0.013523265, 0.0, -0.015326367, -0.023222953, -0.01768427, 0.0, 0.020899592, 0.032512136, 0.025543945, 0.0, -0.032842215, -0.05418689, -0.0459791, 0.0, 0.07663184, 0.16256067, 0.2298955, 0.2553497, 0.2298955, 0.16256067, 0.07663184, 0.0, -0.0459791, -0.05418689, -0.032842215, 0.0, 0.025543945, 0.032512136, 0.020899592, 0.0, -0.01768427, -0.023222953, -0.015326367, 0.0, 0.013523265],
    5 => Float32[0.009190319, 0.0, -0.010157721, -0.017348625, -0.01836913, -0.012062294, 0.0, 0.013785479, 0.024021171, 0.026022935, 0.017545154, 0.0, -0.021444079, -0.039034404, -0.044610746, -0.032166116, 0.0, 0.048249178, 0.10409174, 0.15613762, 0.19299671, 0.20630562, 0.19299671, 0.15613762, 0.10409174, 0.048249178, 0.0, -0.032166116, -0.044610746, -0.039034404, -0.021444079, 0.0, 0.017545154, 0.026022935, 0.024021171, 0.013785479, 0.0, -0.012062294, -0.01836913, -0.017348625, -0.010157721, 0.0, 0.009190319],
)

# Decimator FIR kernel
struct FIRDecimator{T}
    h::Vector{T}
    decimation::Int
    input_delay::Int
end

function FIRDecimator(decimation::Integer)
    # Select filter coefficients
    h = DECIM_FILTER[decimation]
    # Calculate the delay caused by the FIR filter in # of samples at the input sample rate
    input_delay = round(Int, (length(h) - 1)/2) + 1
    return FIRDecimator(h, decimation, input_delay)
end

function decimate(kernel::FIRDecimator{Th}, x::AbstractVector{Tx}) where {Th,Tx}
    buf_len = outputlength(kernel, length(x))
    buf = Vector{promote_type(Th,Tx)}(undef, buf_len)
    return decimate!(buf, kernel, x)
end

function decimate!(buf::AbstractVector, kernel::FIRDecimator, x::AbstractVector)
    buf_len = length(buf)
    x_idx = kernel.input_delay
    decim = kernel.decimation
    checkbounds(x, buf_len*decim+x_idx)
    Threads.@threads for i in 1:buf_len
        @inbounds buf[i] = dot_b(kernel.h, x, (i-1)*decim+x_idx)
    end
    return buf
end

# Calculate the input length of a multirate filtering operation given the output length
inputlength(kernel::FIRDecimator, outlen::Integer) = outlen * kernel.decimation + kernel.input_delay

# Calculate the resulting length of a multirate filtering operation given an input length
outputlength(kernel::FIRDecimator, inlen::Integer) = cld(inlen - kernel.input_delay, kernel.decimation)

# dot up to a specified index for b
function dot_b(a::AbstractArray{T}, b::AbstractArray{T}, b_last::Integer) where T<:Real
    a_len = length(a)
    dot_len = min(b_last, a_len)
    r = zero(T)*zero(T)
    @simd for i in 1:dot_len
        @inbounds r += a[a_len - dot_len + i] * b[b_last - dot_len + i]
    end
    return r
end


text:
## CurveSpace

Space{S<:Fourier}(d::PeriodicCurve{S})=Fourier(d)
Space{S<:Laurent}(d::PeriodicCurve{S})=Laurent(d)

#TODO: Make type stable
Base.convert(::Type{Curve},f::Fun)=isa(domain(f),IntervalDomain)?IntervalCurve(f):PeriodicCurve(f)

# function evaluate{C<:Curve,TT}(f::AbstractVector,S::Space{TT,C},x::Number)
#     rts=roots(domain(S).curve-x)
#     @assert length(rts)==1
#     evaluate(Fun(f,setdomain(S,canonicaldomain(S))),first(rts))
# end

identity_fun{C<:Curve,TT}(d::Space{TT,C})=Fun(domain(d).curve.coefficients,
                                              setdomain(space(domain(d).curve),domain(d)))

# Bernstein polynomials are given by:
#
# b_{ν,n}(x) = binomial(n,ν)/2^n*(1+x)^ν(1-x)^{n-v},  for x ∈ [-1,1], ν = 0,…,n.
#
export Bernstein, Bézier

immutable Bernstein{order,T} <: RealUnivariateSpace{T}
    domain::Interval{T}
    Bernstein(d) = new(d)
    Bernstein() = new(Interval{T}())
end

const Bézier = Bernstein # option+e e gives é

@compat (::Type{Bernstein{O}}){O}() = Bernstein{O,Float64}()
@compat (::Type{Bernstein{O}}){O}(d::Domain) = Bernstein{O,eltype(d)}(d)
@compat (::Type{Bernstein{O}}){O}(d::Vector) = Bernstein{O}(Interval(d))

order{O}(::Bernstein{O}) = O
order{O,T}(::Type{Bernstein{O,T}}) = O
dimension{O}(::Bernstein{O}) = O+1
dimension{O,T}(::Type{Bernstein{O,T}}) = O+1

canonicalspace(B::Bernstein) = Chebyshev(domain(B))
canonicaldomain{O,T}(B::Bernstein{O,T}) = Interval{T}()

spacescompatible{O,T}(a::Bernstein{O,T},b::Bernstein{O,T})=domainscompatible(a,b)

setdomain{O}(S::Bernstein{O},d::Domain)=Bernstein{O}(d)

identity_fun{order,T}(B::Bernstein{order,T})=Fun(collect(-one(T):2one(T)/order:one(T)),B)

evaluate(f::AbstractVector,S::Bernstein,z) = decasteljau(f,S,tocanonical(S,z))

# de Casteljau's numerically stable evaluation of Bernstein polynomials

function decasteljau(f::AbstractVector,S::Bernstein,z)
    β,omz,opz = copy(f),one(z)-z,one(z)+z
    for i=order(S):-1:1
        for j=1:i
            β[j] = (β[j]*omz+β[j+1]*opz)/2
        end
    end
    β[1]
end

function decasteljau(f::AbstractVector,S::Bernstein,z::AbstractVector)
    βinit = copy(f)
    β = copy(βinit)
    T = promote_type(eltype(f),eltype(z))
    ret = zeros(T,length(z))
    for k=1:length(z)
        omzk,opzk = one(z[k])-z[k],one(z[k])+z[k]
        for i=order(S):-1:1
            for j=1:i
                β[j] = (β[j]*omzk+β[j+1]*opzk)/2
            end
        end
        ret[k] = β[1]
        copy!(β,βinit)
    end
    ret
end

splitbernstein{B<:Bernstein,T}(f::Fun{B,T},z) = splitbernstein(coefficients(f),space(f),z)

function splitbernstein(f::AbstractVector,S::Bernstein,z)
    β,omz,opz = copy(f),one(z)-z,one(z)+z
    β1,β2 = copy(β),copy(β)
    for i=order(S):-1:1
        for j=1:i
            β[j] = (β[j]*omz+β[j+1]*opz)/2
        end
        β1[order(S)+2-i] = β[1]
        β2[i] = β[i]
    end
    Fun(interlace(β1,β2),PiecewiseSpace(Bernstein{order(S)}(Interval(first(domain(S)),z)),Bernstein{order(S)}(Interval(z,last(domain(S))))))
end

Fun(f::Function,S::Bernstein) = Fun(Fun(f,canonicalspace(S),dimension(S)),S)

# Connection coefficients with Chebyshev polynomials are derived in
#
#    A. Rababah, "Transformation of Chebyshev–Bernstein polynomial basis", Comp. Meth. Appl. Math. 3:608–622, 2003.
#
function coefficients(f::Vector,a::Chebyshev,b::Bernstein)
    if domain(a) == domain(b)
        n = length(f)-1
        @assert n == order(b)
        M = zeros(eltype(f),n+1,n+1)
        for j=0:n
            for k=0:n
                temp = zero(eltype(f))
                for i=max(0,j+k-n):min(j,k)
                    temp += (-1)^(k-i)*binomial(2k,2i)*binomial(n-k,j-i)
                end
                M[j+1,k+1] = temp/binomial(n,j)
            end
        end
        M*f
    else
        defaultcoefficients(f,a,b)
    end
end

function coefficients(f::Vector,a::Bernstein,b::Chebyshev)
    if domain(a) == domain(b)
        n = length(f)-1
        @assert n == order(a)
        Minv = zeros(eltype(f),n+1,n+1)
        for j=0:n
            for k=0:n
                temp = zero(eltype(f))
                for i=0:j
                    temp += (-1)^(j-i)*binomial(2j,2i)*binomial(2k+2i,k+i)*binomial(2n+2j-2k-2i,n+j-k-i)/binomial(n+j,k+i)
                end
                Minv[j+1,k+1] = (2-FastTransforms.δ(j,0))/4^(n+j)*binomial(n,k)*temp
            end
        end
        Minv*f
    else
        defaultcoefficients(f,a,b)
    end
end

ext: jl


text:
"""
Validate the minimization function - `PotentialMap.action`.

Reference
---------
- `run.m`
"""

# Find Steady States
steadies1, jac_ms1, stab_modes1 = fixedpoint_gen(sp.ODE!, sp.u0, sp.p, [-10.:1.0:10.,-10.:1.0:10.])

# Generate Gradient function
TotalTime = 2.
TotalPoint = 50 # 350 in original paper
gradfunc = gradient_gen(sp.ODE!, sp.p, length(sp.u0), TotalPoint)

# Test
point_start = [0.,0.]
point_end = [1.,1.]
dt = 1.0

initpath = LeastAction.linearpath(point_start, point_end, TotalPoint)

# Integration
LeastAction.integral(initpath, sp.ODE!, LeastAction.DiffusionMatrix, 0.1, sp.p )

# Gradient
g! = LeastAction.grad!(gradfunc, dt, length(point_start), TotalPoint)

# Least action
ref_i = 5
ops = []
@info steadies1[ref_i]
for i in 1:length(steadies1)
    op = action(TotalPoint, TotalTime, steadies1[i], steadies1[ref_i], sp.ODE!, sp.p, gradfunc, LeastAction.DiffusionMatrix)
    push!(ops, op)

    @show op.lam
end

# Plot

fig, ax = plt.subplots()

ax.scatter([i[1] for i in steadies1], [i[2] for i in steadies1], color="black")
ax.scatter( steadies1[ref_i]..., color="red")

for op in ops
    xs = op.path[1,:]
    ys = op.path[2,:]
    ax.plot(xs, ys)
end

plt.display_figs()


text:
"""
    College

College object.
"""
Base.@kwdef mutable struct College
    # College type (index into how data are presented)
    collType :: CollInt
    # Education level for dropouts and graduates
    edDrop :: Symbol = :SC
    edGrad :: Symbol = :CG
    # Grid for courses taken
    nGrid :: CourseGrid
    # H production function
    hcProd :: AbstractHcProdFct
    # H shocks
    hShock :: HcShock
    learnS :: AbstractLearning
    # Admissible work times (in model time units)
    workTimeV :: Vector{Double}
    sTimeGrid :: StudyTimeGrid
    # # Admissible study times
    # studyTimeV :: Vector{Double}
    # # Are these study times per course or total?
    # # If per course: the grid points INCLUDE fixed time per course.
    # sTimeGridPerCourse :: Bool = false
    gradRule :: GradRule
    dropRule :: DropoutRule
    # Wage earned by students (per model time unit)
    wage :: Double
    # Tuition function
    tuitionFct :: AbstractTuitionFunction
    # Minimum net cost; including cost shifters. Model dollars
    minNetCost :: Double
end

## ------------  Show

"""
	$(SIGNATURES)

Table with settings. Columns are explanation and values.
All formatted into strings.
"""
function settings_table(coll :: College)
    nV = Int.(n_tried_grid(coll, 1));
    workTimeV = round.(coll.workTimeV, digits = 2);
    # studyTimeV = round.(coll.studyTimeV, digits = 2);
    # perCourseStr = stime_grid_per_course(c) ? " per course" : " total";
    tuit = format_number(median_tuition(coll; modelUnits = false));
    return [
        "College $(coll.collType)"  " ";
        "No of courses"  "$nV";
        "Work times"  "$workTimeV";
        # "Study times"  "$studyTimeV" * perCourseStr;
        "Tuition"  tuit;
        "Wage"  "$(coll.wage)"
    ]
end

StructLH.describe(coll :: College) = settings_table(coll);

function Base.show(io :: IO, coll :: College)
    # show_text_table(settings_table(coll), io = io);
    tuitionStr = round.(Int, median_tuition(coll; modelUnits = false));
    wageStr = round.(Int, dollars_model_to_data(college_wage(coll), :perYear));
    print(io,  "College $(coll.collType):  ");
    print(io,  "  Tuition (median): $tuitionStr");
    println(io,  "  Wage: $wageStr");
    show(io, coll.hcProd);
    return nothing
end

# ---------  Convenience methods: College

# Model units
min_net_cost(c :: College) = c.minNetCost;

ed_drop(c :: College) = c.edDrop;
ed_grad(c :: College) = c.edGrad;

Lazy.@forward College.sTimeGrid (
    study_time_total, n_study_times, study_time_grid, max_study_time
)

Lazy.@forward College.hcProd (
    hprime
)

Lazy.@forward College.dropRule (
    college_duration, drop_prob_grid, drop_prob
);

Lazy.@forward College.nGrid (
    n_tried_grid, n_n_tried, n_completed_grid, n_n_completed, update_ncourses
);

Lazy.@forward College.gradRule (
    can_graduate, t_first_grad, grad_prob_grid, grad_prob
)

college_wage(c :: College) = c.wage;

work_time_grid(c :: College) = c.workTimeV;
n_work_times(c :: College) = length(c.workTimeV);

max_n_tried(c :: College, t :: Integer) = n_tried_grid(c, t)[end];

## ----------  Substantive calculations

# Worker work start ages.
# `edLevel` is a separate argument because in some cases dropouts work as HSG.
# Empty if grad not possible.
function work_start_ages(c :: College, edLevel :: Symbol)
    Tmax = college_duration(c) + one(TimeInt);
    if edLevel ∈ (:HSG, :SC)
        ageV = TimeInt.(2 : Tmax);
    elseif edLevel == :CG
        if can_graduate(c)
            ageV = (t_first_grad(c) + one(TimeInt)) : Tmax;
        else
            ageV = Vector{TimeInt}();
        end
    else
        error("Invalid $edLevel");
    end
    return ageV
end

# Study time PER COURSE for all choices of course loads and study times
# function study_time_per_course_ns(c :: College, t :: Integer)
#     # Iterate over rows first.
#     nn = n_n_tried(c, t);
#     ns = n_study_times(c);
#     studyTime_nsM = zeros(nn, ns);
#     for (i_n, nTried) in enumerate(n_tried_grid(c, t))
#         studyTime_nsM[i_n, :] = stime_per_course_grid(c.studyTimeGrid, nTried))
#     end
#     return studyTime_nsM
# end

"""
	$(SIGNATURES)

Study times (total) on the (nTried, study times) grid.
"""
function study_time_ns(c :: College, t :: Integer)
    nn = n_n_tried(c, t);
    ns = n_study_times(c);
    studyTime_nsM = zeros(nn, ns);
    for (i_n, nTried) in enumerate(n_tried_grid(c, t))
        studyTime_nsM[i_n, :] = study_time_grid(c.sTimeGrid, nTried);
    end
    return studyTime_nsM
end

# Tuition for median students. For giving college tuition a level that can be displayed.
median_tuition(c :: College; modelUnits :: Bool = false) =
    CollegeStratCollege.tuition(c.tuitionFct, c.collType, 0.5, 0.5, 1; modelUnits = modelUnits);

## -------------  Testing

function make_test_college(;
    nc = 4,
    twoYear :: Bool = false,
    sTimeGridPerCourse :: Bool = false)

    twoYear  ?  (T = 2)  :  (T = 5);
    nGrid = make_test_course_grid(T + 1);
    hcProd = make_test_hprod();
    hcShock = make_test_hshock();
    learnS = make_test_learning();
    workTimeV = collect(range(0.2, 0.4, length = T));
    # studyTimeV = collect(range(0.1, 0.5, length = T));
    sTimeGrid = make_test_study_time_grid(sTimeGridPerCourse);
    gRule = make_test_grad_rule(GradRuleSwitchesLinear(), nc - 1);
    # Must enforce consistency with max time in college
    # switches = DropoutRuleSwitchesSimple();
    dRule = DropoutRuleSimple(T, 0.03);
    wage = 2.5;
    tuition = make_test_tuition_by_qual(nc);
    minNetCost = -1.0;
    c = College(1, :SC, :CG,
        nGrid, hcProd, hcShock, learnS, workTimeV,
        sTimeGrid, gRule, dRule,
        wage, tuition, minNetCost);
    @assert validate_college(c)
    return c
end

"""
	$(SIGNATURES)

Validate a college.
"""
function validate_college(c :: College)
	isValid = true
    T = college_duration(c);
    if c.nGrid.T != (T + 1)
    	@warn "Wrong length course grid"
    	isValid = false;
    end

    sTimeGridV = study_time_grid(c, 1);
    minTime = sTimeGridV[1] + c.workTimeV[1];
    if minTime > 0.8
        @warn "Minimum time requirement is high"
        isValid = false;
    end
    wage = college_wage(c);
    tuit = median_tuition(c; modelUnits = true);
    if (tuit > 10.0 * wage)
        @warn "Tuition too high: $tuit  Wage: $wage"
        isValid = false;
    end
    # This no longer makes sense with calibrated tuition
    # if (wage > 50.0 * tuit)
    #     @warn "Wage too high:  $wage  Tuition: $tuit"
    #     isValid = false;
    # end
    if dollars_model_to_data(wage, :perYear) > 200_000.0
        @warn "Wage not in model dollars?  $wage"
        isValid = false;
    end
    if !isValid
        show(c);
    end
    return isValid
end

# -----------------


text:
using QuadGK

using BenchmarkTools

function integrand(x)
    return exp(10.0 * x * cos(x))
end

@btime quadgk(integrand, 0, 10, rtol=1.49e-08, atol=1.49e-08)


text:
# Test 1 x 1 convolution module using Householder matrices
# Author: Philipp Witte, pwitte3@gatech.edu
# Date: January 2020

using InvertibleNetworks, LinearAlgebra, Test

###################################################################################################
# Initialize parameters

# Dimensions
nx = 28
ny = 28
k = 4
batchsize = 2

# Variables
v1 = randn(Float32, k)
v10 = randn(Float32, k)
dv1 = v1 - v10

v2 = randn(Float32, k)
v20 = randn(Float32, k)
dv2 = v2 - v20

v3 = randn(Float32, k)
v30 = randn(Float32, k)
dv3 = v3 - v30

# Input
X = randn(Float32, nx, ny, k, batchsize)
X0 = randn(Float32, nx, ny, k, batchsize)
dX = X - X0

# Operators
C = Conv1x1(v1, v2, v3)
C0 = Conv1x1(v10, v20, v30)

###################################################################################################
# Test invertibility

X_ = C.inverse(C.forward(X))
err1 = norm(X - X_)/norm(X)

@test isapprox(err1, 0f0; atol=1f-6)

X_ = C.forward(C.inverse(X))
err2 = norm(X - X_)/norm(X)

@test isapprox(err2, 0f0; atol=1f-6)

Y = C.forward(X)
ΔY = randn(Float32, nx, ny, k, batchsize)
ΔX_, X_ = C.inverse((ΔY, Y))
err3 = norm(X - X_)/norm(X)

@test isapprox(err3, 0f0; atol=1f-6)

###################################################################################################
# Test gradients are set in inverse pass

# Predicted data and misfit
C0.v1.grad = nothing
Y_ = C0.forward(X)
@test isnothing(C0.v1.grad)

ΔY = Y_ - Y

# Compute gradients w.r.t. v
ΔX, X_ = C0.inverse((ΔY, Y_))
@test ~isnothing(C0.v1.grad)

###################################################################################################
# Gradient test

loss(ΔY) = .5f0*norm(ΔY)^2

function objective(C, X, Y)
    Y0 = C.forward(X)
    ΔY = Y0 - Y
    f = loss(ΔY)
    ΔX, X_ = C.inverse((ΔY, Y0))
    @test isapprox(norm(X - X_)/norm(X), 0f0, atol=1f-6)
    return f, ΔX, C.v1.grad, C.v2.grad, C.v3.grad
end

# Observed data
Y = C.forward(X)

# Gradient test for X
maxiter = 5
print("Gradient test ΔX\n")
clear_grad!(C)
f0, ΔX = objective(C, X0, Y)[1:2]
h = .01f0
err1 = zeros(Float32, maxiter)
err2 = zeros(Float32, maxiter)
for j=1:maxiter
    f = loss(C.forward(X0 + h*dX) - Y)
    err1[j] = abs(f - f0)
    err2[j] = abs(f - f0 - h*dot(dX, ΔX))
    print(err1[j], "; ", err2[j], "\n")
    global h = h/2f0
end

@test isapprox(err1[end] / (err1[1]/2^(maxiter-1)), 1f0; atol=1f0)
@test isapprox(err2[end] / (err2[1]/4^(maxiter-1)), 1f0; atol=1f0)

# Gradient test for v
print("\nGradient test Δv1\n")
clear_grad!(C0)
f0, ΔX, Δv1, Δv2, Δv3 = objective(C0, X, Y)
h = .01f0
err3 = zeros(Float32, maxiter)
err4 = zeros(Float32, maxiter)
for j=1:maxiter
    C0.v1.data = v10 + h*dv1
    C0.v2.data = v20 + h*dv2
    C0.v3.data = v30 + h*dv3
    f = loss(C0.forward(X) - Y)
    err3[j] = abs(f - f0)
    err4[j] = abs(f - f0 - h*dot(dv1, Δv1) - h*dot(dv2, Δv2) - h*dot(dv3, Δv3))
    print(err3[j], "; ", err4[j], "\n")
    global h = h/2f0
end

@test isapprox(err3[end] / (err3[1]/2^(maxiter-1)), 1f0; atol=1f1)
@test isapprox(err4[end] / (err4[1]/4^(maxiter-1)), 1f0; atol=1f1)

###################################################################################################
# Gradient test: forward-inverse in reverse order

loss(ΔY) = .5f0*norm(ΔY)^2

function objectiveT(C, X, Y)
    Y0 = C.inverse(X)
    ΔY = Y0 - Y
    f = loss(ΔY)
    ΔX, X_ = C.forward((ΔY, Y0))
    @test isapprox(norm(X - X_)/norm(X), 0f0, atol=1f-6)
    return f, ΔX, C.v1.grad, C.v2.grad, C.v3.grad
end

# Observed data
Y = C.forward(X)

# Gradient test for X
maxiter = 5
print("\nGradient test ΔX\n")
C = Conv1x1(v1, v2, v3)
f0, ΔX = objectiveT(C, X0, Y)[1:2]
h = .01f0
err5 = zeros(Float32, maxiter)
err6 = zeros(Float32, maxiter)
for j=1:maxiter
    f = loss(C.inverse(X0 + h*dX) - Y)
    err5[j] = abs(f - f0)
    err6[j] = abs(f - f0 - h*dot(dX, ΔX))
    print(err5[j], "; ", err6[j], "\n")
    global h = h/2f0
end

@test isapprox(err5[end] / (err5[1]/2^(maxiter-1)), 1f0; atol=1f0)
@test isapprox(err6[end] / (err6[1]/4^(maxiter-1)), 1f0; atol=1f0)

# Gradient test for v
print("\nGradient test Δv1\n")
C0 = Conv1x1(v10, v20, v30)
f0, ΔX, Δv1, Δv2, Δv3 = objectiveT(C0, X, Y)
h = .01f0
err7 = zeros(Float32, maxiter)
err8 = zeros(Float32, maxiter)
for j=1:maxiter
    C0.v1.data = v10 + h*dv1
    C0.v2.data = v20 + h*dv2
    C0.v3.data = v30 + h*dv3
    f = loss(C0.inverse(X) - Y)
    err7[j] = abs(f - f0)
    err8[j] = abs(f - f0 - h*dot(dv1, Δv1) - h*dot(dv2, Δv2) - h*dot(dv3, Δv3))
    print(err7[j], "; ", err8[j], "\n")
    global h = h/2f0
end

@test isapprox(err7[end] / (err7[1]/2^(maxiter-1)), 1f0; atol=1f1)
@test isapprox(err8[end] / (err8[1]/4^(maxiter-1)), 1f0; atol=1f1)


text:
using Plots

function bracket_minimum(f, x=0; s=1e-2, k=2.0)
    a, ya = x, f(x)
    b, yb = a + s, f(a + s)
    if yb > ya
        a, b = b, a
        ya, yb = yb, ya
        s = -s
    end
    while true
        c, yc = b + s, f(b + s)
        if yc > yb
            return a < c ? (a, c) : (c, a)
        end
        a, ya, b, yb = b, yb, c, yc
        s *= k
    end
end

φ = MathConstants.φ
function golden_section_search(f, a, b, n)
    ρ = φ-1
    d = ρ * b + (1 - ρ)*a
    yd = f(d)
    for i = 1 : n-1
        c = ρ*a + (1 - ρ)*b
        yc = f(c)
        if yc < yd
            b, d, yd = d, c, yc
        else
            a, b = b, c
        end
    end
    return a < b ? (a, b) : (b, a)
end

f = x->sin(10x)+cos(3x)
x0 = 3.4
#busca do intervalo
a,c = bracket_minimum(f,x0)

#numero de iteracoes
n=(c-a)/(10^-3*log(φ))

am,bm = golden_section_search(f,a,c,n)
(am,f(am))
am,bm

plot(f,0.,4.,lw=2,draw_arrow="true")
scatter!([a,c],[f(a),f(c)])
scatter!([am,bm],[f(am),f(bm)])


text:
abstract type NBodySystem
end

abstract type BasicPotentialSystem <: NBodySystem
end

struct ChargedParticles{bType <: ChargedParticle,kType <: Real} <: BasicPotentialSystem
    bodies::Vector{bType}
    k::kType
end

struct GravitationalSystem{bType <: MassBody,gType <: Real} <: BasicPotentialSystem
    bodies::Vector{bType}
    G::gType
end

struct CustomAccelerationSystem{bType <: Body} <: NBodySystem
    bodies::Vector{bType}
    acceleration # f(u, v, i, system)
    parameters::Vector{<:Number}
end

struct PotentialNBodySystem{bType <: Body} <: NBodySystem
    bodies::Vector{bType}
    potentials::Dict{Symbol,<:PotentialParameters}
end

function PotentialNBodySystem(bodies::Vector{<:Body}; potentials::Vector{Symbol}=[])

    paramteres = Dict{Symbol,PotentialParameters}()

    if :lennard_jones ∈ potentials
        paramteres[:lennard_jones] = LennardJonesParameters()
    end

    if :electrostatic ∈ potentials
        paramteres[:electrostatic] = ElectrostaticParameters()
    end

    if :gravitational ∈ potentials
        paramteres[:gravitational] = GravitationalParameters()
    end

    if :magnetostatic ∈ potentials
        paramteres[:magnetostatic] = MagnetostaticParameters()
    end

    PotentialNBodySystem(bodies, paramteres)
end

function Base.show(stream::IO, s::PotentialNBodySystem)
    println(stream, "Potentials: ")

    ordered_list = [:lennard_jones, :electrostatic, :magnetostatic,:gravitational]
    for potential in ordered_list
        if potential ∈ keys(s.potentials)
            show(stream, s.potentials[potential])
        end
    end
end

function PotentialNBodySystem(system::PotentialNBodySystem)
    return system
end

function PotentialNBodySystem(system::ChargedParticles)
    pp = ElectrostaticParameters(system.k)
    potential = Dict{Symbol,PotentialParameters}(:electrostatic => pp)
    PotentialNBodySystem(system.bodies, potential)
end

function PotentialNBodySystem(system::GravitationalSystem)
    pp = GravitationalParameters(system.G)
    potential =  Dict{Symbol,PotentialParameters}(:gravitational => pp)
    PotentialNBodySystem(system.bodies, potential)
end

struct WaterSPCFw{bType <: Body,pType <: Real} <: NBodySystem
    bodies::Vector{bType}
    mH::pType
    mO::pType
    qH::pType
    qO::pType
    lj_parameters::LennardJonesParameters{pType}
    e_parameters::ElectrostaticParameters{pType}
    scpfw_parameters::SPCFwParameters{pType}
end


text:
export TRARC2_HO_4

function TRARC2_HO_4(nlp 	    :: AbstractNLPModel,
                     nlp_stop   :: NLPStopping;
                	 TR 	    :: TrustRegion = TrustRegion(eltype(nlp.meta.x0)(10.0)),
			    	 c 		    :: Combi =  Combi{eltype(nlp.meta.x0)}(hessian_dense, PDataLDLt{eltype(nlp.meta.x0)}, solve_modelTRDiag_HO_3, preprocessLDLt, decreaseFact, Tparam{eltype(nlp.meta.x0)}()),
					 correction :: Symbol = :Chebyshev,
			    	 robust     :: Bool = true,
                	 verbose    :: Bool = false
                	 )

	T = eltype(nlp.meta.x0)

	nlp_at_x = nlp_stop.current_state
    hessian_rep, PData, solve_model, pre_process, decrease, params = extract(c)

    α = TR.α₀  # initial Trust Region size
    xt, xtnext, d, Df = copy(nlp.meta.x0), copy(nlp.meta.x0), copy(nlp.meta.x0), 0.0
    xopt = xt
    λ = 1.0

    n = length(xt)
    ∇f = Array{T}(undef, n)
    ∇fnext = Array{T}(undef, n)

    ft = obj(nlp, xt)
    fopt = ft
    grad!(nlp, xt, ∇f)
	OK = update_and_start!(nlp_stop, x = xt, fx = ft, gx = ∇f, g0 = ∇f)

	norm_∇f = norm(nlp_at_x.gx)
    norm_∇f0 = norm_∇f
    ∇fopt = ∇f
    norm_∇fopt = norm_∇f
	!OK && update!(nlp_at_x, Hx = hessian_rep(nlp, xt))

    ftnext = ft
    iter = 0

    verbose && display_header_iterations()
    verbose && display_success(iter, ftnext, norm_∇f0, 0.0, α)

    succ, unsucc, verysucc, unsuccinarow = 0, 0, 0, 0

    max_unsuccinarow = TR.max_unsuccinarow

    calls = [0, 0, 0, 0]

	global xdemi = NaN * rand(n)  ## How to add in a globalized version?

    while !OK
        PData = pre_process(nlp_at_x.Hx, ∇f, params, calls, nlp_stop.meta.max_eval)

        if ~PData.OK
			@warn("Something wrong with PData")
			return nlp_at_x, nlp_stop.meta.optimal
		end

        success = false
		Ht = nothing

        while !success & !OK & (unsuccinarow < TR.max_unsuccinarow)
            try
                d, xdemi, λ = solve_model(nlp_stop, PData, α) ## xdemi...
            catch
                println(" Problem in solve_model")
                return nlp_at_x, nlp_stop.meta.optimal
            end

			# Doesn't this go against the stopping philosophy?
			# Shouldn't we have a stopping call here and not an if?
            Δq = -(∇f + 0.5 * nlp_at_x.Hx * d)⋅d

			if Δq < 0.0
				println("*******   Ascent direction in SolveModel: Δq = $Δq")
                println("  g⋅d = $(∇f⋅d), 0.5 d'Hd = $(0.5*(nlp_at_x.Hx*d)⋅d)  α = $α  λ = $λ")
				#@bp
                #try println(" cond(H) = $(cond(full(H)))") catch println("sparse hessian, no cond") end
                #D, Q = eig(full(H))
                #lm = findmin(D); lM = findmax(D)
                #println("λ min (H) = $lm  λ max (H) = $lM")
                #try printtln(" cond(L) = $(cond(PData.L))") catch println("Spectral") end
                #try println(" ||H - reconstructH|| = $(norm(H-reconstructH(PData)))")
                #catch
                #    println(" reconstruct H no available ")
                #end
                println("  calls  $calls   iters $verysucc; $succ; $unsucc")

				return nlp_at_x, nlp_stop.meta.optimal
            end
            slope = ∇f ⋅ d
			# again: how to put in a general idea?
			if !(true in isnan.(xdemi))
				xtnext = xdemi + d
			else
				xtnext = xt + d
			end

            iter += 1

            try
                ftnext = obj(nlp, xtnext)
            catch
                ftnext = Inf;
            end

            if isnan(ftnext)
                ftnext = Inf
            end

            Δf = ft - ftnext

            r, good_grad, ∇fnext = compute_r(nlp, ft, Δf, Δq, slope, d, xtnext, ∇fnext, robust)

            if r < TR.acceptance_threshold
                verbose && display_failure(iter, ftnext, λ, α)
	        	unsucc = unsucc + 1
	        	unsuccinarow = unsuccinarow + 1
	        	α = decrease(PData, α, TR)
	    	else
	        	success = true

	        	unsuccinarow = 0
	        	xt = copy(xtnext)
	        	ft = ftnext

	        	if good_grad
					∇f = copy(∇fnext)
            	else
					grad!(nlp, xt, ∇f);
                end

                norm_∇f = stop_norm(∇f)
				verysucc += 1
                Ht = hessian_rep(nlp, xt)
		        if r > TR.increase_threshold
		            α = increase(PData, α, TR)
		            verbose && display_v_success(iter, ft, norm_∇f, λ, α)
		        else
	                if r < TR.reduce_threshold
	                    α = decrease(PData, α, TR)
	                end
		            verbose && display_success(iter, ft, norm_∇f, λ, α)
		            succ += 1
		        end
	    	end
        end # while !succes

		OK = update_and_stop!(nlp_stop, x = xt, fx = ft, gx = ∇f, Hx = Ht)
        calls = [nlp.counters.neval_obj,  nlp.counters.neval_grad, nlp.counters.neval_hess, nlp.counters.neval_hprod]
    end # while !OK

    xopt = xt
    fopt = ft
    ∇fopt = ∇f
    norm_∇fopt = norm_∇f

    niter = verysucc + succ + unsucc;
    verbose && @printf(" %10i unsuccessful iterations \n", unsucc)
    verbose && @printf(" %10i successful iterations \n", succ)
    verbose && @printf(" %10i very successful iterations \n", verysucc)
    verbose && @printf(" %10i total successful iterations \n", verysucc + succ)
    verbose && @printf(" %10i total iterations \n", niter)

    calls = [nlp.counters.neval_obj, nlp.counters.neval_grad, nlp.counters.neval_hess, nlp.counters.neval_hprod]

    total_calls = nlp.counters.neval_obj + nlp.counters.neval_grad + n * nlp.counters.neval_hess + nlp.counters.neval_hprod
     verbose && @printf(" %10i total f %10i g %10i H calls  %10i Hv calls  \n", nlp.counters.neval_obj, nlp.counters.neval_grad, nlp.counters.neval_hess, nlp.counters.neval_hprod)

    if OK && verbose  @printf("\n     Final cost(x) = %f  ||g|| = %9.2e  nb of f/g/H evals = %i  converged\n", fopt, norm_∇fopt, total_calls)
    elseif verbose
        @printf("\n     Final cost(x) = %f  ||g|| = %9.2e  nb of f/g/H evals  = %i  NOT converged\n\n", fopt, norm_∇fopt, total_calls)
    end;

    return nlp_at_x, nlp_stop.meta.optimal

end


text:
## tests of derivative free methods (orders 1,2,5,8,16)
using Roots
using Compat.Test

orders = [1,2,5,8,16]

fn, x0, alpha = x -> cos(x) - x , 1.0, 0.7390851332151607
for order in orders
    @test fzero(fn, x0, order=order)  ≈ alpha
    @test fzero(fn, big(x0), order=order)  ≈ alpha
end

fn = x -> x^5 - x + 1 # initial steffensen step is a problem, so not secant, but others
x0 = -1.0
alpha = -1.1673039782614187

for order in orders
    @test fzero(fn, x0, order=order)  ≈ alpha
end

## failures
fn = x -> x^20 - 1
x0, alpha = 0.5, 1.0
for order in orders[3:end]
    @test_throws Roots.ConvergenceFailed fzero(fn, x0, order=order)
end

## Issue near 0 that does not cross
fn = x -> cos(x) - 1
x0, alpha = 0.1, 0.0
for order in orders[end]
    ## test that these do not throw
    fn(fzero(fn, x0, order=order, ftol=1e-10))
end

#@test_throws Roots.ConvergenceFailed fzero(fn, x0, order=1)
@test fn(fzero(fn, x0, order=1, maxsteps = 300))+1 ≈ 1.0

## trivial
for order in orders
    @test fzero(x -> 0.0, 1, order=order) ≈ 1.0
    @test fzero(x -> x, 0.0, order=order) ≈ 0.0
end


text:
immutable SIMDVector{M, N, R, T <: Number} <: AbstractVector{T}
    simd_vecs::NTuple{M, VecRegister{N, T}}
    rest::NTuple{R, T}
end

Base.size{M, N, R}(::SIMDVector{M, N, R}) = (R + M * N,)

@inline function Base.call{M, N, R, T}(::Type{SIMDVector{M, N, R}}, a::Tuple{}, b::NTuple{R, T})
    SIMDVector{M, N, R, T}(a, b)
end

@inline function Base.call{M, N, R, T}(::Type{SIMDVector{M, N, R}}, a::NTuple{M, VecRegister{N, T}}, b::NTuple{R, T})
    SIMDVector{M, N, R, T}(a, b)
end

@inline function Base.call{M, N, R, T}(::Type{SIMDVector{M, N, R}}, a::NTuple{M, VecRegister{N, T}}, b::Tuple{})
    SIMDVector{M, N, R, T}(a, b)
end

# TODO: Try make this more efficient
function Base.getindex{M, N, R}(v::SIMDVector{M, N, R}, i::Int)
    if i > M * N
        @inbounds val = v.rest[i - M*N]
        return val
    else
        # TODO, while loop instead of div
        bucket = div(i-1, N) + 1
        @inbounds val = v.simd_vecs[bucket][i - (bucket-1)*N]
        return val
    end
end

function compute_lengths(N, T)
    if T in VECTOR_DATATYPES
        simd_len = div(VEC_REGISTER_SIZE_BITS , sizeof(T) * 8)
    else # Default to store all other types in the rest field which is a normal tuple
        simd_len = 0
    end

    if simd_len == 0
        rest = N
        buckets = 0
    else
        rest = Int(N % simd_len)
        buckets = div(N - rest, simd_len)
    end

    return simd_len, rest, buckets
end

@generated function load{N}(::Type{SIMDVector{N}}, data, offset::Int = 0)
    T = eltype(data)
    simd_len, rest, buckets = compute_lengths(N, T)

    simd_array_create_expr = Expr(:tuple)
    if simd_len != 0
        for i in 1:simd_len:N-rest
            exp_simd_ele = Expr(:call, :VecRegister)
            push!(exp_simd_ele.args, Expr(:tuple, [:(VecElement(data[$j + offset])) for j in i:simd_len+i-1]...))
            push!(simd_array_create_expr.args, exp_simd_ele)
        end
    end

    rest_array_create_expr = Expr(:tuple, [:(data[$j + offset]) for j in (N-rest+1):N]...)

    return quote
        @assert $N + offset <= length(data)
        @inbounds simd_tup =  $simd_array_create_expr
        @inbounds rest = $rest_array_create_expr
        SIMDVector{$buckets, $simd_len, $rest, $T}(simd_tup, rest)
    end
end

function store!{M, N, R}(data, v::SIMDVector{M,N,R}, offset::Int = 0)
    @assert length(data) + offset >= M*N + R
    c = 1 + offset
    #@inbounds
    for i in 1:M
        simd_element = v.simd_vecs[i]
        @simd for j in 1:N
            data[c] = simd_element[j]
            c += 1
        end
    end

    @simd for i in 1:R
        @inbounds data[c] = v.rest[i]
        c += 1
    end
    return data
end

# Elementwise unary functions
for f in UNARY_FUNCS
    tuple_f_string = symbol(string(f) * "_tuple")
    @eval begin
        @generated function Base.$(f){M, N, R, T}(a::SIMDVector{M, N, R, T})
            ex_simd = vectupexpr(i -> :(($($f))(a.simd_vecs[$i])), M)
            return quote
                SIMDVector{M, N, R}($ex_simd, $($(tuple_f_string))(a.rest))
            end
        end
    end
end

# Binary functions between two vectors and vector, number.
for f in BINARY_FUNCS
    tuple_f_string = symbol(string(f) * "_tuple")
    @eval begin
        @generated function Base.$(f){M, N, R, T <: Number}(a::SIMDVector{M, N, R, T}, b::SIMDVector{M, N, R, T})
            ex_simd = vectupexpr(i -> :(($($f))(a.simd_vecs[$i], b.simd_vecs[$i])), M)
            return quote
                SIMDVector{M, N, R}($ex_simd, $($(tuple_f_string))(a.rest, b.rest))
            end
        end

        @generated function Base.$(f){M, N, R, T <: Number}(a::SIMDVector{M, N, R, T}, b::T)
            ex_simd = vectupexpr(i -> :(($($f))(a.simd_vecs[$i], b)), M)
            return quote
                SIMDVector{M, N, R}($ex_simd, $($(tuple_f_string))(a.rest, b))
            end
        end

        @generated function Base.$(f){M, N, R, T <: Number}(b::T, a::SIMDVector{M, N, R, T})
            ex_simd = vectupexpr(i -> :(($($f))(b, a.simd_vecs[$i])), M)
            return quote
                SIMDVector{M, N, R}($ex_simd, $($(tuple_f_string))(b, a.rest))
            end
        end

        function Base.$(f){M, N, R, T1 <: Number, T2 <: Number}(b::T1, a::SIMDVector{M, N, R, T2})
            $(f)(promote_eltype(b, a)...)
        end

        function Base.$(f){M, N, R, T1 <: Number, T2 <: Number}(a::SIMDVector{M, N, R, T1}, b::T2)
            $(f)(promote_eltype(a, b)...)
        end

        function $(f){M1, N1, R1, T1, M2, N2, R2, T2}(a::SIMDVector{M1, N1, R1, T1},
                                                      b::SIMDVector{M2, N2, R2, T2})
            $(f)(promote(a,b)...)
        end
    end
end

# Reductions
for (f_vec, f_scal) in REDUCTION_FUNCS
    @eval begin
        function $f_vec{M, N, R, T}(v::SIMDVector{M, N, R, T})
            if M == 0
                return $f_vec(v.rest)
            end
            @inbounds v1 = v.simd_vecs[1]
            @inbounds for i in 2:M
                v1 = $f_scal(v1, v.simd_vecs[i])
            end
            if R == 0
                return $f_vec(v1)
            else
                return $f_scal($f_vec(v1), $f_vec(v.rest))
            end
        end
    end
end

@generated function Base.rand{M, N, R, T}(a::Type{SIMDVector{M, N, R, T}})
    ex_simd = SIMDVectors.vectupexpr(i -> :(rand(VecRegister{N, T})), M)
    return quote
        $(Expr(:meta, :inline))
        SIMDVector($ex_simd, rand_tuple(NTuple{R, T}))
    end
end

@generated function Base.zero{M, N, R, T}(a::Type{SIMDVector{M, N, R, T}})
    ex_simd = SIMDVectors.vectupexpr(i -> :z, M)
    return quote
        $(Expr(:meta, :inline))
        z = zero(VecRegister{N, T})
        SIMDVector($ex_simd, zero_tuple(NTuple{R, T}))
    end
end

@generated function Base.one{M, N, R, T}(a::Type{SIMDVector{M, N, R, T}})
    ex_simd = SIMDVectors.vectupexpr(i -> :z, M)
    return quote
        $(Expr(:meta, :inline))
        z = one(VecRegister{N, T})
        SIMDVector($ex_simd, one_tuple(NTuple{R, T}))
    end
end


text:
function set!(msg::vector_3d_t, trans::AbstractVector)
    @boundscheck checkbounds(trans, 1:3)
    @inbounds begin
        msg.x = trans[1]
        msg.y = trans[2]
        msg.z = trans[3]
    end
    msg
end

function set!(msg::quaternion_t, rot::Rotation{3})
    quat = Quat(rot)
    msg.w = quat.w
    msg.x = quat.x
    msg.y = quat.y
    msg.z = quat.z
    msg
end

function set!(msg::position_3d_t, tf::Transform3D)
    set!(msg.translation, translation(tf))
    set!(msg.rotation, rotation(tf))
    msg
end

function set!(msg::twist_t, twist::Twist)
    set!(msg.linear_velocity, linear(twist))
    set!(msg.angular_velocity, angular(twist))
    msg
end

function set!(msg::force_torque_t,
        left_foot_wrench::Wrench, right_foot_wrench::Wrench,
        left_hand_wrench::Wrench, right_hand_wrench::Wrench)
    msg.l_foot_force_z = linear(left_foot_wrench)[3]
    msg.l_foot_torque_x = angular(left_foot_wrench)[1]
    msg.l_foot_torque_y = angular(left_foot_wrench)[2]
    msg.r_foot_force_z = linear(right_foot_wrench)[3]
    msg.r_foot_torque_x = angular(right_foot_wrench)[1]
    msg.r_foot_torque_y = angular(right_foot_wrench)[2]
    msg.l_hand_force = linear(left_hand_wrench)
    msg.l_hand_torque = angular(left_hand_wrench)
    msg.r_hand_force = linear(right_hand_wrench)
    msg.r_hand_torque = angular(right_hand_wrench)
    msg
end

# Type piracy, but whatever:
StaticArrays.SVector(msg::vector_3d_t) = SVector(msg.x, msg.y, msg.z)
Rotations.Quat(msg::quaternion_t) = Quat(msg.w, msg.x, msg.y, msg.z)

function contact_wrench_in_body_frame(state::MechanismState, result::DynamicsResult, body::RigidBody)
    transform(RigidBodyDynamics.contact_wrench(result, body), inv(transform_to_root(state, body)))
end

function set!(msg::robot_state_t, result::DynamicsResult, robot_info::HumanoidRobotInfo,
        τprev::AbstractVector, t::Number, state::MechanismState)
    # time
    msg.utime = floor(Int, t * 1e6)

    # pose
    floatingbody = robot_info.floatingbody
    pose = transform_to_root(state, floatingbody)
    set!(msg.pose, pose)

    # twist
    # twist in message should be expressed in world-aligned body frame (for some reason)
    twist = twist_wrt_world(state, robot_info.floatingbody)
    to_world_aligned_floating_body = Transform3D(pose.to, robot_info.world_aligned_floating_body_frame, -translation(pose))
    twist = transform(twist, to_world_aligned_floating_body)
    set!(msg.twist, twist)

    # state info for actuated joints
    q = configuration(state)
    v = velocity(state)
    for i = 1 : msg.num_joints
        actuator = robot_info.actuators[i]
        position_ind = range_to_ind(configuration_range(state, actuator.jointid))
        velocity_ind = range_to_ind(velocity_range(state, actuator.jointid))
        msg.joint_position[i] = q[position_ind]
        msg.joint_velocity[i] = v[velocity_ind]
        msg.joint_effort[i] = τprev[velocity_ind]
        i += 1
    end

    # contact wrenches
    RigidBodyDynamics.contact_dynamics!(result, state)
    set!(msg.force_torque,
        contact_wrench_in_body_frame(state, result, robot_info.feet[Sides.left]),
        contact_wrench_in_body_frame(state, result, robot_info.feet[Sides.right]),
        contact_wrench_in_body_frame(state, result, robot_info.hands[Sides.left]),
        contact_wrench_in_body_frame(state, result, robot_info.hands[Sides.right]))

    msg
end

function set!(τ::AbstractVector, Δt::Number, state::MechanismState, τprev::AbstractVector,
        msg::atlas_command_t, robot_info::HumanoidRobotInfo)
    τ .= 0
    q = configuration(state)
    v = velocity(state)
    for i = 1 : msg.num_joints
        actuator = findactuator(robot_info, msg.joint_names[i])
        position_ind = range_to_ind(configuration_range(state, actuator.jointid))
        velocity_ind = range_to_ind(velocity_range(state, actuator.jointid))
        gains = LowLevelJointGains(
            msg.k_q_p[i],
            msg.k_q_i[i],
            msg.k_qd_p[i],
            msg.k_f_p[i],
            msg.ff_qd[i],
            msg.ff_qd_d[i],
            msg.ff_f_d[i],
            msg.ff_const[i])
        joint_state = JointState(q[position_ind], v[velocity_ind], τprev[velocity_ind])
        joint_state_des = JointState(msg.position[i], msg.velocity[i], msg.effort[i])
        torque_scale = msg.k_effort[i] / typemax(UInt8)
        τ[velocity_ind] = torque_scale * command_effort(gains, joint_state, joint_state_des, Δt)
    end
    τ
end

function set!(state::MechanismState, msg::robot_state_t, robot_info::HumanoidRobotInfo)
    floatingjoint = robot_info.floatingjoint
    world_aligned_floating_body_frame = robot_info.world_aligned_floating_body_frame

    trans = SVector(msg.pose.translation)
    rot = Quat(msg.pose.rotation)
    pose = Transform3D(frame_after(floatingjoint), frame_before(floatingjoint), rot, trans)
    set_configuration!(state, floatingjoint, pose)

    # twist in message is expressed in world-aligned body frame (for some reason)
    to_body = Transform3D(world_aligned_floating_body_frame, frame_after(floatingjoint), inv(rot))
    angular = SVector(msg.twist.angular_velocity)
    linear = SVector(msg.twist.linear_velocity)
    twist = Twist(frame_after(floatingjoint), frame_before(floatingjoint), world_aligned_floating_body_frame,
        angular, linear)
    twist = transform(twist, to_body)
    set_velocity!(state, floatingjoint, twist)

    msg.num_joints == length(robot_info.revolutejoints) || throw(ArgumentError("Number of joints incorrect."))
    for i = 1 : msg.num_joints
        joint = robot_info.revolutejoints[i]
        @boundscheck msg.joint_name[i] == joint.name || throw(ArgumentError("Joint name mismatch."))
        set_configuration!(state, joint, msg.joint_position[i])
        set_velocity!(state, joint, msg.joint_velocity[i])
    end
    state
end

function set!(msg::atlas_command_t, τ::AbstractVector, t::Float64,
        state_des::MechanismState, robot_info::HumanoidRobotInfo, desired_controller_period_ms::Integer)
        msg.utime = floor(Int, t * 1e6)
    q = configuration(state_des)
    v = velocity(state_des)
    for i = 1 : msg.num_joints
        # simple torque control for now; really, this method should take a Dict{Actuator, LowLevelJointGains}
        actuator = findactuator(robot_info, msg.joint_names[i])
        position_ind = range_to_ind(configuration_range(state_des, actuator.jointid))
        velocity_ind = range_to_ind(velocity_range(state_des, actuator.jointid))
        gains = LowLevelJointGains(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0)

        msg.position[i] = q[position_ind]
        msg.velocity[i] = v[velocity_ind]
        msg.effort[i] = τ[velocity_ind]
        msg.k_q_p[i] = gains.k_q_p
        msg.k_q_i[i] = gains.k_q_i
        msg.k_qd_p[i] = gains.k_qd_p
        msg.k_f_p[i] = gains.k_f_p
        msg.ff_qd[i] = gains.ff_qd
        msg.ff_qd_d[i] = gains.ff_qd_d
        msg.ff_f_d[i] = gains.ff_f_d
        msg.ff_const[i] = gains.ff_const
        msg.k_effort[i] = typemax(UInt8) # TODO: ?
    end
    msg.desired_controller_period_ms = desired_controller_period_ms
    msg
end

# type piracy to avoid allocations when decoding robot_state_t
@generated function LCMCore.decodefield!(x::robot_state_t, io::IO)
    field_assignments = Vector{Expr}(undef, fieldcount(x))
    for (i, fieldname) in enumerate(fieldnames(x))
        F = fieldtype(x, fieldname)
        field_assignments[i] = if fieldname == :joint_name
            quote
                if isempty(x.joint_name)
                    # decode as normal the first time
                    LCMCore.resizearrayfield!(x, $(Val(fieldname)), $F)
                    LCMCore.decodefield!(x.joint_name, io)
                else
                    # check that bytes match joint names in robot_state_t
                    @inbounds for name in x.$fieldname
                        len = ntoh(read(io, UInt32))
                        length(name) == len - 1 || error("Mismatch in joint_name field.")
                        for j = 1 : len - 1
                            name[j] === Char(read(io, UInt8)) || error("Mismatch in joint_name field.")
                        end
                        read(io, UInt8) # strip off null
                    end
                end
            end
        else
            # just do the normal thing
            quote
                $F <: Array && LCMCore.resizearrayfield!(x, $(Val(fieldname)), $F)
                if LCMCore.decode_in_place($F)
                    LCMCore.decodefield!(x.$fieldname, io)
                else
                    x.$fieldname = LCMCore.decodefield(io, $F)
                end
            end
        end
    end
    return quote
        $(field_assignments...)
        return x
    end
end


text:
"""
Utilities.jl contains conversion methods, purification schemes,
and other miscelanious utilities
"""

"""
Convert from decibelic loss to metric form
"""
function dB_to_P(dB::Float64)::Float64
    P = 10.0^(-dB/10)
    return P
end

"""
Convert from metric form to decibelic loss
"""
function P_to_dB(P::Float64)::Float64
    dB = -10.0*log(10,P)
    return dB
end

"""
Convert from dephasing probability to decibelic form
"""
function Z_to_dB(Z::Float64)::Float64
    dB = -10.0*log(10, 2*Z-1)
    return dB
end

"""
Convert from decibelic dephasing to metric form
"""
function dB_to_Z(dB::Float64)::Float64
    Z = (10^(-dB/10) + 1)/2
    return Z
end

function purify_PBS(F1::Float64,F2::Float64)::(Float64,Float64)
    F = F1*F1 / (F1*F2 + (1-F1)(1-F2))
    P = 1
    return (F,P)
end

function purify_CNOT(F1::Float64,F2::Float64)::(Float64,Float64)
    F = F1*F1 / (F1*F2 + (1-F1)(1-F2))
    P = 1
    return (F,P)
end

"""
Probabilistic purification scheme used in greedy_multi_path!
Takes a list of cost vectors (dictionaries) as input and returns a cost vector
"""
function purify(cost_vectors::Vector{Dict{Any,Any}}, return_as_dB::Bool=true)
    @assert keys(zero_costvector()) == keys(cost_vectors[1]) "Incompatible keys"
    p_arr = [dB_to_P(i["loss"]) for i in cost_vectors]
    z_arr = [dB_to_Z(i["Z"]) for i in cost_vectors]

    p = prod(p_arr) * (prod(z_arr) + prod(1 .- z_arr))
    z = prod(z_arr) / ((prod(z_arr) + prod(1 .- z_arr)))

    if return_as_dB == true
        return Dict("loss"=>P_to_dB(p), "Z"=>Z_to_dB(z))
    else
        return Dict("loss"=>p, "Z"=>z)
    end
end

function purify(paths::Vector{<:QChannel}, return_as_dB::Bool=true)
    cost_vectors = Vector{Dict}()
    for path in paths
        cost = get_pathcv(path)
        push!(cost_vectors, cost)
    end
    purify(cost_vectors, return_as_dB)
end

max_stars_repo_name: drpeterrohde/QuNet

max_issues_repo_name: SamuelMarks/QuNet

max_forks_repo_name: SamuelMarks/QuNet


text:
# This file was generated by the Julia Swagger Code Generator
# Do not modify this file directly. Modify the swagger specification instead.

mutable struct ApplicationGatewayHeaderConfiguration <: SwaggerModel
    headerName::Any # spec type: Union{ Nothing, String } # spec name: headerName
    headerValue::Any # spec type: Union{ Nothing, String } # spec name: headerValue

    function ApplicationGatewayHeaderConfiguration(;headerName=nothing, headerValue=nothing)
        o = new()
        validate_property(ApplicationGatewayHeaderConfiguration, Symbol("headerName"), headerName)
        setfield!(o, Symbol("headerName"), headerName)
        validate_property(ApplicationGatewayHeaderConfiguration, Symbol("headerValue"), headerValue)
        setfield!(o, Symbol("headerValue"), headerValue)
        o
    end
end # type ApplicationGatewayHeaderConfiguration

const _property_map_ApplicationGatewayHeaderConfiguration = Dict{Symbol,Symbol}(Symbol("headerName")=>Symbol("headerName"), Symbol("headerValue")=>Symbol("headerValue"))
const _property_types_ApplicationGatewayHeaderConfiguration = Dict{Symbol,String}(Symbol("headerName")=>"String", Symbol("headerValue")=>"String")
Base.propertynames(::Type{ ApplicationGatewayHeaderConfiguration }) = collect(keys(_property_map_ApplicationGatewayHeaderConfiguration))
Swagger.property_type(::Type{ ApplicationGatewayHeaderConfiguration }, name::Symbol) = Union{Nothing,eval(Base.Meta.parse(_property_types_ApplicationGatewayHeaderConfiguration[name]))}
Swagger.field_name(::Type{ ApplicationGatewayHeaderConfiguration }, property_name::Symbol) =  _property_map_ApplicationGatewayHeaderConfiguration[property_name]

function check_required(o::ApplicationGatewayHeaderConfiguration)
    true
end

function validate_property(::Type{ ApplicationGatewayHeaderConfiguration }, name::Symbol, val)
end


text:
struct AdjointDiffCache{UF,PF,G,TJ,PJT,uType,JC,GC,PJC,PJNC,rateType,DG,DI,AI,FM}
  uf::UF
  pf::PF
  g::G
  J::TJ
  pJ::PJT
  dg_val::uType
  jac_config::JC
  g_grad_config::GC
  paramjac_config::PJC
  paramjac_noise_config::PJNC
  f_cache::rateType
  dg::DG
  diffvar_idxs::DI
  algevar_idxs::AI
  factorized_mass_matrix::FM
  issemiexplicitdae::Bool
end

"""
    adjointdiffcache(g,sensealg,discrete,sol,dg;quad=false)

return (AdjointDiffCache, y)
"""
function adjointdiffcache(g,sensealg,discrete,sol,dg,f;quad=false,noiseterm=false)
  prob = sol.prob
  if prob isa DiffEqBase.SteadyStateProblem
    @unpack u0, p = prob
    tspan = (nothing, nothing)
  #elseif prob isa SDEProblem
  #  @unpack tspan, u0, p = prob
  else
    @unpack u0, p, tspan = prob
  end
  numparams = length(p)
  numindvar = length(u0)
  isautojacvec = get_jacvec(sensealg)

  issemiexplicitdae = false
  mass_matrix = sol.prob.f.mass_matrix
  if mass_matrix isa UniformScaling
    factorized_mass_matrix = mass_matrix'
  elseif mass_matrix isa Tuple{UniformScaling,UniformScaling}
    factorized_mass_matrix = (I',I')
  else
    mass_matrix = mass_matrix'
    diffvar_idxs = findall(x->any(!iszero, @view(mass_matrix[:, x])), axes(mass_matrix, 2))
    algevar_idxs = setdiff(eachindex(u0), diffvar_idxs)
    # TODO: operator
    M̃ = @view mass_matrix[diffvar_idxs, diffvar_idxs]
    factorized_mass_matrix = lu(M̃, check=false)
    issuccess(factorized_mass_matrix) || error("The submatrix corresponding to the differential variables of the mass matrix must be nonsingular!")
    isempty(algevar_idxs) || (issemiexplicitdae = true)
  end
  if !issemiexplicitdae
    diffvar_idxs = eachindex(u0)
    algevar_idxs = 1:0
  end

  J = (issemiexplicitdae || !isautojacvec || prob isa DiffEqBase.SteadyStateProblem) ? similar(u0, numindvar, numindvar) : nothing

  if !discrete
    if dg != nothing
      pg = nothing
      pg_config = nothing
    else
      pg = UGradientWrapper(g,tspan[2],p)
      pg_config = build_grad_config(sensealg,pg,u0,p)
    end
  else
    pg = nothing
    pg_config = nothing
  end

  if DiffEqBase.has_jac(f) || (J === nothing)
    jac_config = nothing
    uf = nothing
  else
    if DiffEqBase.isinplace(prob)
      uf = DiffEqBase.UJacobianWrapper(f,tspan[2],p)
      jac_config = build_jac_config(sensealg,uf,u0)
    else
      uf = DiffEqBase.UDerivativeWrapper(f,tspan[2],p)
      jac_config = nothing
    end
  end

  if prob isa DiffEqBase.SteadyStateProblem
    y = copy(sol.u)
  else
    y = copy(sol.u[end])
  end

  if sensealg.autojacvec isa ReverseDiffVJP ||
    (sensealg.autojacvec isa Bool && sensealg.autojacvec && DiffEqBase.isinplace(prob))

    if prob isa DiffEqBase.SteadyStateProblem
       if DiffEqBase.isinplace(prob)
        tape = ReverseDiff.GradientTape((y, prob.p)) do u,p
          du1 = similar(p, size(u))
          f(du1,u,p,nothing)
          return vec(du1)
        end
      else
        tape = ReverseDiff.GradientTape((y, prob.p)) do u,p
          vec(f(u,p,nothing))
        end
      end
    else
      if DiffEqBase.isinplace(prob)
        tape = ReverseDiff.GradientTape((y, prob.p, [tspan[2]])) do u,p,t
          du1 = similar(p, size(u))
          f(du1,u,p,first(t))
          return vec(du1)
       end
     else
       tape = ReverseDiff.GradientTape((y, prob.p, [tspan[2]])) do u,p,t
          vec(f(u,p,first(t)))
       end
     end
    end
    if compile_tape(sensealg.autojacvec)
      paramjac_config = ReverseDiff.compile(tape)
    else
      paramjac_config = tape
    end

    pf = nothing
  elseif (DiffEqBase.has_paramjac(f) || isautojacvec || quad)
    paramjac_config = nothing
    pf = nothing
  else
    if DiffEqBase.isinplace(prob)
      pf = DiffEqBase.ParamJacobianWrapper(f,tspan[1],y)
      paramjac_config = build_param_jac_config(sensealg,pf,y,p)
    else
      pf = ParamGradientWrapper(f,tspan[2],y)
      paramjac_config = nothing
    end
  end

  pJ = (quad || isautojacvec) ? nothing : similar(u0, numindvar, numparams)

  dg_val = similar(u0, numindvar) # number of funcs size
  dg_val .= false
  f_cache = DiffEqBase.isinplace(prob) ? deepcopy(u0) : nothing

  if noiseterm
    if (sensealg.noise isa ReverseDiffNoise ||
      (sensealg.noise isa Bool && sensealg.noise && DiffEqBase.isinplace(prob)))

      paramjac_noise_config = []

      if DiffEqBase.isinplace(prob)
        for i in 1:numindvar
          function noisetape(indx)
            ReverseDiff.GradientTape((y, prob.p, [tspan[2]])) do u,p,t
              du1 = similar(p, size(u))
              f(du1, u,p,first(t))
              return du1[indx]
            end
          end
          tapei = noisetape(i)
          if compile_tape(sensealg.noise)
            push!(paramjac_noise_config, ReverseDiff.compile(tapei))
          else
            push!(paramjac_noise_config, tapei)
          end
        end
      else
        for i in 1:numindvar
          function noisetapeoop(indx)
            ReverseDiff.GradientTape((y, prob.p, [tspan[2]])) do u,p,t
              f(u,p,first(t))[indx]
            end
          end
          tapei = noisetapeoop(i)
          if compile_tape(sensealg.noise)
            push!(paramjac_noise_config, ReverseDiff.compile(tapei))
          else
            push!(paramjac_noise_config, tapei)
          end
        end
      end
    elseif (sensealg.noise isa Bool && !sensealg.noise)
      if DiffEqBase.isinplace(prob)
        pf = DiffEqBase.ParamJacobianWrapper(f,tspan[1],y)
        paramjac_noise_config = build_param_jac_config(sensealg,pf,y,p)
      else
        pf = ParamGradientWrapper(f,tspan[2],y)
        paramjac_noise_config = nothing
      end
      pJ = similar(u0, numindvar, numparams)
    else
      paramjac_noise_config = nothing
    end
  else
    paramjac_noise_config = nothing
  end

  adjoint_cache = AdjointDiffCache(uf,pf,pg,J,pJ,dg_val,
                          jac_config,pg_config,paramjac_config,paramjac_noise_config,
                          f_cache,dg,diffvar_idxs,algevar_idxs,
                          factorized_mass_matrix,issemiexplicitdae)

  return adjoint_cache, y
end

getprob(S::SensitivityFunction) = (S isa ODEBacksolveSensitivityFunction) ? S.prob : S.sol.prob

function generate_callbacks(sensefun, g, λ, t, callback, init_cb)
  sensefun.discrete || return callback

  @unpack sensealg, y = sensefun
  @unpack diffvar_idxs, algevar_idxs, factorized_mass_matrix, issemiexplicitdae, J, uf, f_cache, jac_config = sensefun.diffcache
  prob = getprob(sensefun)
  cur_time = Ref(length(t))
  time_choice = let cur_time=cur_time, t=t
    integrator -> cur_time[] > 0 ? t[cur_time[]] : nothing
  end
  affect! = let isq = (sensealg isa QuadratureAdjoint), λ=λ, t=t, y=y, cur_time=cur_time, idx=length(prob.u0), F=factorized_mass_matrix
    function (integrator)
      p, u = integrator.p, integrator.u
      # Warning: alias here! Be careful with λ
      gᵤ = isq ? λ : @view(λ[1:idx])
      g(gᵤ,y,p,t[cur_time[]],cur_time[])
      if issemiexplicitdae
        jacobian!(J, uf, y, f_cache, sensealg, jac_config)
        dhdd = J[algevar_idxs, diffvar_idxs]
        dhda = J[algevar_idxs, algevar_idxs]
        # TODO: maybe need a `conj`
        Δλa = -dhda'\gᵤ[algevar_idxs]
        Δλd = dhdd'Δλa + gᵤ[diffvar_idxs]
      else
        Δλd = gᵤ
      end

      if factorized_mass_matrix !== nothing
        F !== I && F !== (I,I) && ldiv!(F, Δλd)
      end
      u[diffvar_idxs] .+= Δλd
      u_modified!(integrator,true)
      cur_time[] -= 1
      return nothing
    end
  end
  cb = IterativeCallback(time_choice,affect!,eltype(prob.tspan);initial_affect=init_cb)
  return CallbackSet(cb,callback)
end


text:
module Mdp
include("VI.jl")
include("FH.jl")
include("PricingProblem.jl")
include("PricingAlgorithmFH.jl")
end


text:
"""
Module for running CoupledCluster computations in Fermi.

"""
module CoupledCluster

using Fermi
using Fermi.Integrals: IntegralHelper
using Fermi.HartreeFock: RHF
using Fermi.Output
using TensorOperations
using LinearAlgebra

function print_header()
    banner =
raw"""
================================================================================
//    _____                   _          _   _____ _           _              \\
//   /  __ \                 | |        | | /  __ \ |         | |             \\
//   | /  \/ ___  _   _ _ __ | | ___  __| | | /  \/ |_   _ ___| |_ ___ _ __   \\
//   | |    / _ \| | | | '_ \| |/ _ \/ _` | | |   | | | | / __| __/ _ \ '__|  \\
//   | \__/\ (_) | |_| | |_) | |  __/ (_| | | \__/\ | |_| \__ \ ||  __/ |     \\
//    \____/\___/ \__,_| .__/|_|\___|\__,_|  \____/_|\__,_|___/\__\___|_|     \\
//                    | |                                                     \\
//                    |_|                                                     \\
//                                                                            \\
//                 Module by M.M. Davis and G.J.R. Aroeira                    \\
================================================================================
"""
    @output "\n{}\n" banner
end

"""
    Fermi.CoupledCluster.AbstractCCWavefunction

Fermi abstract type common to all Coupled Cluster wavefunctions

_struct tree:_

**AbstractCCWavefunction** <: AbstractCorrelatedWavefunction <: AbstractWavefunction
"""
abstract type AbstractCCWavefunction <: Fermi.AbstractCorrelatedWavefunction end

# Structures symbolizing the type of implementation for each CC method
abstract type CCAlgorithm end
struct DPD   <: CCAlgorithm end
struct CTF   <: CCAlgorithm end
struct DFCTF <: CCAlgorithm end

function select_precision(A::String)
    implemented = Dict{Any,Any}("single" => Float32,
                                "double" => Float64)
    try
        return implemented[A]
    catch KeyError
        throw(Fermi.InvalidFermiOptions("Invalid precision: $(A)"))
    end
end

function select_algorithm(A::String)
    implemented = Dict{Any,Any}("DPD" => DPD(),
                                "CTF" => CTF(),
                                "DFCTF" => DFCTF())
    try
        return implemented[A]
    catch KeyError
        throw(Fermi.InvalidFermiOptions("Invalid CC algorithm: $(A)"))
    end
end

#include("RCCD/RCCD.jl")
include("RCCSD/RCCSD.jl")
include("pT/PerturbativeTriples.jl")
include("ecRCCSD/ecRCCSD.jl")
include("ecpT/ecPerturbativeTriples.jl")
include("BCC/BD.jl")

end #module CC


text:
module CPortGraphDynam

using Catlab.WiringDiagrams
using Catlab.WiringDiagrams.CPortGraphs
using  Catlab.CategoricalAlgebra
using  Catlab.CategoricalAlgebra.FinSets
using Catlab.Graphs
import Catlab.Graphs: Graph
using Catlab.Graphics.GraphvizGraphs: to_graphviz

import Catlab.WiringDiagrams: oapply

using ..DWDDynam
using ...DWDDynam: AbstractInterface, destruct, unit
import ..UWDDynam: nstates, nports, eval_dynamics, euler_approx
import ..DWDDynam: AbstractMachine, ContinuousMachine, DiscreteMachine, DelayMachine,
ninputs, noutputs
import Catlab.CategoricalAlgebra: migrate!

using Base.Iterators
import Base: show, eltype

ContinuousMachine{T, I}(nports::Int, nstates::Int, d::Function, r::Function) where {T,I} =
    ContinuousMachine{T, I}(nports, nstates, nports, d, r)
ContinuousMachine{T}(nports::Int, nstates::Int, d::Function, r::Function) where T =
    ContinuousMachine{T}(nports, nstates, nports, d, r)
DiscreteMachine{T, I}(nports::Int, nstates::Int, d::Function, r::Function) where {T,I} =
    DiscreteMachine{T, I}(nports, nstates, nports, d, r)
DiscreteMachine{T}(nports::Int, nstates::Int, d::Function, r::Function) where T =
    DiscreteMachine{T}(nports, nstates, nports, d, r)
DelayMachine{T, I}(nports::Int, nstates::Int, d::Function, r::Function) where {T,I} =
    DelayMachine{T, I}(nports, nstates, nports, d, r)
DelayMachine{T}(nports::Int, nstates::Int, d::Function, r::Function) where T =
    DelayMachine{T}(nports, nstates, nports, d, r)

migrate!(g::Graph, pg::OpenCPortGraph) = migrate!(g, migrate!(CPortGraph(), pg))

draw(g::Graph) = to_graphviz(g, prog="neato", edge_labels=true, node_labels=true)
draw(pg::OpenCPortGraph) = draw(migrate!(Graph(), pg))

concat(xs::Vector) = (collect ∘ Iterators.flatten)(xs)

nports(d::OpenCPortGraph, b::Int) = incident(d, b, :box) |> length
nports(d::OpenCPortGraph, b) = map(length, incident(d, b, :box))
nports(d::OpenCPortGraph, b::Colon) = map(length, incident(d, :, :box))

function fills(m::AbstractMachine, d::OpenCPortGraph, b::Int)
    nports = length(incident(d, b, :box))
    return (nports == ninputs(m)) && (nports == noutputs(m))
end

"""    oapply(d::OpenCPortGraph, ms::Vector)

Implements the operad algebras for directed composition of dynamical systems given a
composition pattern (implemented by an open circular port graph `d`)
and primitive systems (implemented by a collection of
machines `ms`).

Each box of the composition pattern `d` is filled by a machine with the
appropriate type signature. Returns the composite machine.
"""
function oapply(d::OpenCPortGraph, ms::Vector{M}) where {M<:AbstractMachine}
    @assert nparts(d, :Box) == length(ms)
    for b in 1:nparts(d, :Box)
        @assert fills(ms[b], d, b)
    end

    S = coproduct((FinSet∘nstates).(ms))

    return M(
        nparts(d, :OuterPort),
        length(apex(S)),
        induced_dynamics(d, ms, S),
        induced_readout(d, ms, S)
    )
end

"""    oapply(d::OpenCPortGraph, m::AbstractMachine)

A version of `oapply` where each box of `d` is filled with the machine `m`.
"""
function oapply(d::OpenCPortGraph, x::AbstractMachine)
    oapply(d, collect(repeated(x, nparts(d, :Box))))
end

function induced_dynamics(d::OpenCPortGraph, ms::Vector{M}, S) where {T, I, M<:AbstractMachine{T, I}}
    function v(u::AbstractVector, xs::AbstractVector, p, t::Real)
        states = destruct(S, u)
        readins = unit(I, nparts(d, :Port)) # in port order

        for (b,m) in enumerate(ms)
            readouts = readout(m, states[b], p, t)
            for (i, port) in enumerate(incident(d, b, :box))
                for w in incident(d, port, :src)
                    readins[subpart(d, w, :tgt)] += readouts[i]
                end
            end
        end

        for (i,x) in enumerate(xs)
            readins[subpart(d, i, :con)] += x
        end

        return reduce(vcat, map(enumerate(ms)) do (b,m)
            eval_dynamics(m, collect(states[b]), view(readins, incident(d, b, :box)), p, t)
        end)
    end
end

function induced_dynamics(d::OpenCPortGraph, ms::Vector{M}, S) where {T, I, M<:DelayMachine{T, I}}
    function v(u::AbstractVector, xs::AbstractVector, h::Function, p, t::Real)
        states = destruct(S, u)
        hists = destruct(S, h)
        readins = unit(I, nparts(d, :Port)) # in port order

        for (b,m) in enumerate(ms)
            readouts = readout(m, states[b], hists[b], p, t)
            for (i, port) in enumerate(incident(d, b, :box))
                for w in incident(d, port, :src)
                    readins[subpart(d, w, :tgt)] += readouts[i]
                end
            end
        end

        for (i,x) in enumerate(xs)
            readins[subpart(d, i, :con)] += x
        end

        return reduce(vcat, map(enumerate(ms)) do (b,m)
            eval_dynamics(m, collect(states[b]), view(readins, incident(d, b, :box)), hists[b], p, t)
        end)
    end
end

function induced_readout(d::OpenCPortGraph, ms::Vector{M}, S) where {T, I, M<:AbstractMachine{T, I}}
    function r(u::AbstractVector, p, t::Real)
        states = destruct(S, u)
        port_readout = unit(I, nparts(d, :Port))

        for (b,m) in enumerate(ms)
            readouts = readout(m, states[b], p, t)
            for (i, port) in enumerate(incident(d, b, :box))
                port_readout[port] = readouts[i]
            end
        end

        return collect(view(port_readout, subpart(d, :con)))
    end
end

function induced_readout(d::OpenCPortGraph, ms::Vector{M}, S) where {T, I, M<:DelayMachine{T, I}}
    function r(u::AbstractVector, h::Function, p, t::Real)
        states = destruct(S, u)
        hists = destruct(S, h)
        port_readout = unit(I, nparts(d, :Port))

        for (b,m) in enumerate(ms)
            readouts = readout(m, states[b], hists[b], p, t)
            for (i, port) in enumerate(incident(d, b, :box))
                port_readout[port] = readouts[i]
            end
        end

        return collect(view(port_readout, subpart(d, :con)))
    end
end

"""    barbell(n::Int)

Constructs an open CPG with two boxes each with `n` ports. The ``i``th ports on each box are connected.
"""
barbell(k::Int) = begin
  g = OpenCPortGraph()
  add_parts!(g, :Box, 2)
  add_parts!(g, :Port, 2k; box=[fill(1,k); fill(2,k)])
  add_parts!(g, :Wire, k; src=1:k, tgt=k+1:2k)
  add_parts!(g, :Wire, k; tgt=1:k, src=k+1:2k)
  return g
end

meshpath(n::Int) = begin
    gt = @acset OpenCPortGraph begin
        Box = 1
        Port = 3
        Wire = 0
        OuterPort = 2
        box= ones(Int, 3)
        con= [3,2]
    end
    gm = @acset OpenCPortGraph begin
        Box = 1
        Port = 4
        Wire = 0
        OuterPort = 2
        box= ones(Int, 4)
        con= [4,2]
    end
    subs = [gt]
    for i in 2:n-1
        push!(subs, gm)
    end
    push!(subs, gt)
    X = coproduct(subs)
    for i in 1:n-1
        xi = subs[i]
        xj = subs[i+1]
        p = legs(X)[i][:Port](nparts(xi, :Port)-1)
        q = legs(X)[i+1][:Port](1)
        add_parts!(apex(X), :Wire, 2, src=[p,q], tgt=[q,p])
    end
    c₁ = apex(X)[1:2:nparts(apex(X),:OuterPort) ,:con]
    c₂ = apex(X)[2:2:nparts(apex(X),:OuterPort) ,:con]
    apex(X)[:,:con] = vcat(c₁,c₂)
    return X
end

function gridpath(n::Int, width::Int)
    node = @acset OpenCPortGraph begin
        Box = 1
        Port = 0
        Wire = 0
        box = 1
    end
    add_parts!(node, :Port, 2width, box=1)
    X = coproduct(collect(repeated(node, n)))
    L = legs(X)
    A = apex(X)
    for i in 1:n-1
        for j in 1:width
            s = L[i][:Port](j)
            t = L[i+1][:Port](j+width)
            add_part!(A, :Wire, src=s, tgt=t)
            add_part!(A, :Wire, src=t, tgt=s)
        end
    end
    upstream = L[1][:Port](width+1:2width)
    add_parts!(A, :OuterPort, width, con=upstream)
    downstream = L[end][:Port](1:width)
    add_parts!(A, :OuterPort, width, con=downstream)
    return X
end

grid(n::Int, m::Int) = ocompose(apex(gridpath(n,m)), collect(repeated(apex(meshpath(m)), n)))

end


text:
module SfLaser

using Unitful

export diffraction_limited_element_size, diffraction_limited_spot_diameter, diffraction_limited_range, diffraction_limited_wavelength,
	attenuated_beam_power, approximate_breakdown_threshold, beam_intensity

"""
    diffraction_limited_spot_diameter(r::Unitful.Length, d_element::Unitful.Length, λ::Unitful.Length)

Approximate minimum spot size for a diffraction-limited gaussian laser beam of wavelength `λ` with
initial diameter `d_element` at range `r`.
"""
function diffraction_limited_spot_diameter(r::Unitful.Length, d_element::Unitful.Length, λ::Unitful.Length, m2 = 1)
	if r <= 0u"m"
		throw(DomainError(r, "Range r must be greater than zero"))
	elseif d_element <= 0u"m"
		throw(DomainError(d_element, "Element diameter must be greater than zero"))
	elseif λ <= 0u"m"
		throw(DomainError(λ, "Wavelength λ must be greater than zero"))
	elseif m2 < 1
		throw(DomainError(m2, "M² cannot be less than 1"))
	end

	if r < d_element
		@warn "Range r less than element size, result will be inaccurate" r element_size
	end

	return 2m2 * λ / (π * atan(d_element, 2r)) |> unit(d_element)
end

function diffraction_limited_element_size(r::Unitful.Length, d_spot::Unitful.Length, λ::Unitful.Length, m2 = 1)
	if r <= 0u"m"
		throw(DomainError(r, "Range r must be greater than zero"))
	elseif d_spot <= 0u"m"
		throw(DomainError(d_spot, "Spot diameter must be greater than zero"))
	elseif λ <= 0u"m"
		throw(DomainError(λ, "Wavelength λ must be greater than zero"))
	elseif m2 < 1
		throw(DomainError(m2, "M² cannot be less than 1"))
	end

	d_element =  4r * tan(m2 * λ / (π * d_spot))

	if r < d_element
		@warn "Range r less than element size, result will be inaccurate" r d_element
	end

	return d_element |> unit(d_spot)
end

function diffraction_limited_range(d_element::Unitful.Length, d_spot::Unitful.Length, λ::Unitful.Length, m2 = 1)
	if d_element <= 0u"m"
		throw(DomainError(d_element, "Element diameter must be greater than zero"))
	elseif d_spot <= 0u"m"
		throw(DomainError(d_spot, "Spot diameter must be greater than zero"))
	elseif λ <= 0u"m"
		throw(DomainError(λ, "Wavelength λ must be greater than zero"))
	elseif m2 < 1
		throw(DomainError(m2, "M² cannot be less than 1"))
	end

	r = d_element / 4tan(m2 * λ / (π * d_spot))

	if r < d_element
		@warn "Range r less than element size, result will be inaccurate" r d_element
	end

	return upreferred(r)
end

function diffraction_limited_wavelength(r::Unitful.Length, d_element::Unitful.Length, d_spot::Unitful.Length, m2 = 1)
	if d_element <= 0u"m"
		throw(DomainError(d_element, "Element diameter must be greater than zero"))
	elseif d_spot <= 0u"m"
		throw(DomainError(d_spot, "Spot diameter must be greater than zero"))
	elseif r <= 0u"m"
		throw(DomainError(r, "Range r must be greater than zero"))
	elseif m2 < 1
		throw(DomainError(m2, "M² cannot be less than 1"))
	end

	if r < d_element
		@warn "Range r less than element size, result will be inaccurate" r d_element
	end

	return tan(d_element /2r) * π * d_spot / 2m2 |> u"nm"
end

attenuated_beam_power(r::Unitful.Length, power::Unitful.Power, attenuation_length::Unitful.Length) = power * exp(-r / attenuation_length)

# http://panoptesv.com/SciFi/LaserDeathRay/Breakdown.html
function approximate_breakdown_threshold(λ::Unitful.Length, ambient_pressure::Unitful.Pressure)
	p = ustrip(ambient_pressure |> u"atm")
	l = ustrip(λ |> u"m")

	return ((0.31 / (l * p^0.6)^2) * u"W*cm^-2") |> u"W/m^2"
end

beam_intensity(power::Unitful.Power, d_spot::Unitful.Length) = power / (π * d_spot^2 / 4)

end

max_stars_repo_name: icantbelieveitsnotphysics/SfPhysics

max_issues_repo_name: icantbelieveitsnotphysics/SfPhysics

max_forks_repo_name: icantbelieveitsnotphysics/SfPhysics


text:
function add_group!(cells::Cells, file::AbstractString)
    func = file[end-2:end] * "_group!"
    eval(Symbol(func))(cells, file)
end

function su2_group!(cells::Cells, file::AbstractString)
    lines = readlines(file)
    ids = Int[]
    for i = 1:length(lines)-1
        if lines[i][1:4] * lines[i+1][1:4] == "MARKMARK"
            push!(ids, i)
        end
    end

    tags = [[lines[id]] for id in ids]
    for idx in eachindex(tags)
        for i = ids[idx]+2:length(lines)
            lines[i][1:4] == "MARK" && break
            push!(tags[idx], lines[i])
        end
    end

    for idx in eachindex(tags)
        key = split(tags[idx][1], "= ")[2] |> String

        value = zeros(Int, length(tags[idx]) - 1, length(split(tags[idx][2], "\t")) - 1)
        for i in axes(value, 1)
            value[i, :] = parse.(Int, split(tags[idx][i+1], "\t")[2:end]) .+ 1
        end

        push!(cells.type, key)
        push!(cells.index, value)
    end
end


text:
# Data used for curve construction from curvature function
mutable struct CurveData
    κ_formula::String
    # Initial position
    x0::Float64
    y0::Float64
    # Initial orientation
    θ0::Float64
    # arc-length range
    smin::Float64
    smax::Float64
end

# Constructor for default values of initial position and orientation
CurveData(κ_formula::String, smin::Float64,smax::Float64) = CurveData(κ_formula,0.0,0.0,0.0,smin,smax)

# evaluate curvature function from string formula
function kappa(s::Float64, formula::String)
    ex = quote
        s = $s
        "formula here"
    end
    ex.args[4] = Meta.parse(formula::String)
    eval(ex)
end

# Second order ODE (as first order ODE) to construct curve from curvature
function kappa_field_from_formula(κ_formula)
    function f(q,s)
        x = q[1]
        y = q[2]
        tx = q[3]
        ty = q[4]
        κ_val = kappa(s,κ_formula)
        [tx,ty,-κ_val*ty,κ_val*tx]
    end
    f
end

# Obtain curve by integrating differential equation using RK4
function curve_from_curvature(cd::CurveData; n = 250)
    f = kappa_field_from_formula(cd.κ_formula)
    h = (cd.smax-cd.smin)/n # length step
    k1 = zeros(4)
    k2 = zeros(4)
    k3 = zeros(4)
    k4 = zeros(4)
    # Setting Initial conditions
    qs = [zeros(4) for i = 1:(n+1)]
    qs[1] = [cd.x0,cd.y0,cos(cd.θ0),sin(cd.θ0)]
    s = cd.smin
    for i = 2:(n+1)
        q = qs[i-1]
        k1 = f(q,s)
        k2 = f(q + (h/2.0)*k1,s)
        k3 = f(q + (h/2.0)*k2,s)
        k4 = f(q + h*k3,s)
        qs[i] = q + (h/6.0)*(k1 + 2*k2 +2*k3 + k4)
        s = s + h
    end
    qs
end


text:
### A Pluto.jl notebook ###
# v0.18.0

using Markdown
using InteractiveUtils

# ╔═╡ bd814b22-c126-40eb-94ff-02eeac4a2965
begin
	using Pkg, Revise
	Pkg.activate(Base.current_project())
    # Pkg.add([
    #     Pkg.PackageSpec(name="Catalyst", version="10.1"),
    # ])
    # using Catalyst

	Pkg.instantiate()
	#Pkg.develop(path="..")
	#Pkg.add(url="https://github.com/kris-brown/Catlab.jl", rev="looseacset_csp")
	#Pkg.add(url="https://github.com/AlgebraicJulia/AlgebraicPetri.jl", rev="Catalyst_v10")
	using Catlab.CategoricalAlgebra
	using AlgebraicPetri
	using AlgebraicPetri: Graph
    using ModelExploration
	using Plots
	using Test
end

# ╔═╡ 28a03065-8d42-402d-b140-13b7f87001b6
md"""Define typing system for models"""

# ╔═╡ 36849bbb-b1e0-4fca-b633-fe04ac3377ac
begin
	infectious_type = LabelledPetriNet([:Pop],
	  :interaction=>((:Pop, :Pop)=>(:Pop, :Pop)),
	  :t_infection=>(:Pop=>:Pop),
	  :t_strata=>(:Pop=>:Pop))
	Graph(infectious_type)
end

# ╔═╡ 5c946c97-7484-4a58-89fa-eba5f903faac
md"""Define petri nets for epidemiology models dimension"""

# ╔═╡ 43112b41-b539-481e-9a29-fea386439544
begin
	IO_help(i::Int) = let d = Dict([j=>j%2+1 for j in 1:2*i]); (I=d,O=d) end

	SIR = LabelledPetriNet([:S, :I, :R],
	  :inf => ((:S, :I)=>(:I, :I)),
	  :rec => (:I=>:R),
	  :id => (:S => :S),
	  :id => (:I => :I),
	  :id => (:R => :R)
	)
	SIR_type = merge((T=[1,2,3,3,3],), IO_help(1))

	SIR_travel_ban = LabelledPetriNet([:S, :I, :R],
	  :inf => ((:S, :I)=>(:I, :I)),
	  :rec => (:I=>:R),
	  :id => (:S => :S),
	  :id => (:R => :R)
	)

	SIR_tb_type = merge((T=[1,2,3,3],), IO_help(1))

	SIS = LabelledPetriNet([:S, :I],
	  :inf => ((:S, :I)=>(:I, :I)),
	  :rec => (:I=>:S),
	  :id => (:S => :S),
	  :id => (:I => :I),
	)

	SIS_type = merge((T=[1,2,3,3],),IO_help(1))

	SVIIvR = LabelledPetriNet([:S, :I, :R, :Iv, :V],
	  :inf => ((:S, :I)=>(:I, :I)),
	  :inf => ((:V,:I)=>(:I,:I)),
	  :inf => (:S, :Iv) => (:Iv,:Iv),
	  :inf => ((:V, :Iv) => (:Iv,:Iv)),
	  :rec => (:I=>:R),
	  :rec => (:Iv=>:R),
	  :vax => (:S=>:V),
	  :id => (:S => :S),
	  :id => (:I => :I),
	  :id => (:V => :V),
	  :id => (:Iv => :Iv),
	  :id => (:R => :R)
	)

	SVIIvR_type = merge((T=[1,1,1,1,2,2,2,3,3,3,3,3],),IO_help(4))

end;

# ╔═╡ 148087ee-ed18-4e1e-aecb-a3736614c4de
	epi_models = SliceLit(infectious_type, [
		SIR=>SIR_type, SIS=>SIS_type,
		SIR_travel_ban=>SIR_tb_type, SVIIvR=>SVIIvR_type]);

# ╔═╡ 8eac847e-d590-47d4-8b20-b407b620c559
md"""Define petri nets for stratification dimension"""

# ╔═╡ c34e9ad3-e43a-4d6b-981d-4dbcb82d7c0e
begin
	quarantine = LabelledPetriNet([:Q, :not_Q],
	    :interaction => ((:not_Q, :not_Q) => (:not_Q, :not_Q)),
	    :enter_quarantine => (:not_Q => :Q),
	    :exit_quarantine => (:Q => :not_Q),
	    :id => (:Q => :Q),
	    :id => (:not_Q => :not_Q),
	)
	quarantine_type = (T=[1, 3, 3, 2, 2], I=Dict(1=>1,2=>2), O=Dict(1=>1,2=>2))

	age_stratification = LabelledPetriNet([:Child, :Adult],
	    :interaction => ((:Child, :Child) => (:Child, :Child)),
	    :interaction => ((:Adult, :Adult) => (:Adult, :Adult)),
	    :interaction => ((:Child, :Adult) => (:Child, :Adult)),
	    :id => (:Child => :Child),
	    :id => (:Adult => :Adult),
	)
	age_s_type = merge((T=[1,1,1,2,2],),IO_help(3))

	flux_metapopulation = LabelledPetriNet([:Patch1, :Patch2],
	    :interaction => ((:Patch1, :Patch1) => (:Patch1, :Patch1)),
	    :interaction => ((:Patch2, :Patch2) => (:Patch2, :Patch2)),
		:travel => (:Patch1 => :Patch2),
	    :travel => (:Patch2 => :Patch1),
	    :id => (:Patch1 => :Patch1),
	    :id => (:Patch2 => :Patch2),
	)
	flux_m_type = merge((T=[1,1,3,3,2,2],), IO_help(2))

	simple_trip =  LabelledPetriNet([:P11, :P21, :P12, :P22],
	    :interaction11_11 => ((:P11, :P11) => (:P11, :P11)),
	    :interaction12_12 => ((:P12, :P12) => (:P12, :P12)),
	    :interaction21_21 => ((:P21, :P21) => (:P21, :P21)),
	    :interaction22_22 => ((:P22, :P22) => (:P22, :P22)),
	    :interaction11_21 => ((:P11, :P21) => (:P11, :P21)),
	    :interaction22_12 => ((:P22, :P12) => (:P22, :P12)),
	    :interaction12_22 => ((:P12, :P22) => (:P12, :P22)),
	    :interaction21_11 => ((:P21, :P11) => (:P21, :P11)),
		:travel11_12 => (:P11 => :P12),
		:travel12_11 => (:P12 => :P11),
		:travel21_22 => (:P21 => :P22),
		:travel22_21 => (:P22 => :P21),
	    :id => (:P11 => :P11),
	    :id => (:P21 => :P21),
	    :id => (:P12 => :P12),
	    :id => (:P22 => :P22),
	)

	simple_t_type = merge((T=[1,1,1,1,1,1,1,1,3,3,3,3,2,2,2,2],), IO_help(8))
end;

# ╔═╡ 35a823ec-260b-4a15-876b-30d3b6a090a6
	strat_models = SliceLit(infectious_type, [quarantine => quarantine_type, age_stratification => age_s_type, flux_metapopulation => flux_m_type, simple_trip => simple_t_type ]);

# ╔═╡ dc439087-d393-416f-b0b4-57a3c2c8d69c
md"""Construct the model space as Product([dim1, dim2], slice)"""

# ╔═╡ 634c732d-6c68-4a2d-a570-11e149358973
prodSpace= ModelExploration.Product(epi_models, strat_models, infectious_type);

# ╔═╡ b6a6d27d-cae4-4fad-9f59-e1feccad5cea
begin
	true_model = unfold(prodSpace)[2]
	Graph(true_model)
end

# ╔═╡ f3092427-d6b8-4241-a319-b17e968ca735
true_model

# ╔═╡ 7664c213-a569-4069-9b0e-45db9d35344f
begin
	# inf: (-8,-5), rec: (-4,-1), else (-10,0)
	true_rates = Float64[5e-6,5e-8,5e-7, 1e-2, 2e-2]
	# I: (2,6), R: (-5 0), else (2,8)
	true_initial_pop = Float64[1e7,1e3,1e-1,2e7,1e4,1e-1]
end;

# ╔═╡ 520ec980-861b-4f7a-b7b2-9450cb3c3e60
begin
	d = generate_data(true_model, true_rates, true_initial_pop)
	plot(d)
end

# ╔═╡ d029518b-456e-4520-8df1-7e4320893822
begin
    l2, ps = train(true_model,d)
	println("Loss2 = $l2 PS = $ps")
	plot(generate_data(true_model, ps[1:5], ps[6:11]))

end

# ╔═╡ f6750c70-0a9a-4a8b-b505-ab462314b6a7
ms = unfold(prodSpace);

# ╔═╡ 50e151e5-417c-4d5a-bad2-f0b54c0b32a5
begin
    ls4, ps4 = train(ms[4],d)
	println("Loss4 = $ls4 PS4 = $ps4")
	plot(generate_data(ms[4], ps4[1:nt(ms[4])], ps4[nt(ms[4])+1:end]))

end

# ╔═╡ 7c12d16c-c395-47b9-bee1-c674d525af6d
loss_fun = eval_petri_fn(true_model, true_rates, true_initial_pop);

# ╔═╡ 21f0b44f-6868-4b2c-b235-35ea91fd3046
md"""Define the loss function
We've already computed the simulated data from the desired Petri net (load from file)
"""

# ╔═╡ 33ae7c95-ded4-45d6-a600-4d8b409b16a0
md"""Run selection, should return the Petri net that generated the data"""

# ╔═╡ 2ec8c472-d3de-4fc9-875b-c5815b7d22f7
begin
	best_model = ModelExploration.select(prodSpace, loss_fun)
    @test best_model == true_model
	#Graph(best_model)
end

# ╔═╡ Cell order:
# ╠═bd814b22-c126-40eb-94ff-02eeac4a2965
# ╟─28a03065-8d42-402d-b140-13b7f87001b6
# ╠═36849bbb-b1e0-4fca-b633-fe04ac3377ac
# ╟─5c946c97-7484-4a58-89fa-eba5f903faac
# ╠═43112b41-b539-481e-9a29-fea386439544
# ╠═148087ee-ed18-4e1e-aecb-a3736614c4de
# ╟─8eac847e-d590-47d4-8b20-b407b620c559
# ╟─c34e9ad3-e43a-4d6b-981d-4dbcb82d7c0e
# ╠═35a823ec-260b-4a15-876b-30d3b6a090a6
# ╟─dc439087-d393-416f-b0b4-57a3c2c8d69c
# ╠═634c732d-6c68-4a2d-a570-11e149358973
# ╠═b6a6d27d-cae4-4fad-9f59-e1feccad5cea
# ╠═f3092427-d6b8-4241-a319-b17e968ca735
# ╠═7664c213-a569-4069-9b0e-45db9d35344f
# ╠═520ec980-861b-4f7a-b7b2-9450cb3c3e60
# ╠═d029518b-456e-4520-8df1-7e4320893822
# ╠═f6750c70-0a9a-4a8b-b505-ab462314b6a7
# ╠═50e151e5-417c-4d5a-bad2-f0b54c0b32a5
# ╠═7c12d16c-c395-47b9-bee1-c674d525af6d
# ╟─21f0b44f-6868-4b2c-b235-35ea91fd3046
# ╟─33ae7c95-ded4-45d6-a600-4d8b409b16a0
# ╠═2ec8c472-d3de-4fc9-875b-c5815b7d22f7


text:
# Arrays

glorot_uniform(dims...) = glorot_uniform(STD_REAL_PREC, dims...)
glorot_normal(dims...) = glorot_normal(STD_REAL_PREC, dims...)
rescaled_normal(scale::Real, dims...) = rescaled_normal(STD_REAL_PREC, scale, dims...)

glorot_uniform(T::Type, dims...) = (rand(T, dims...) .- T(0.5)) .* sqrt(T(24)/sum(dims))
glorot_normal(T::Type, dims...) = randn(T, dims...) .* sqrt(T(2)/sum(dims))
rescaled_normal(T::Type, scale::Real, dims::Integer...) = randn(T, dims...) .* T(scale) .* sqrt(T(24)/sum(dims))

# Utils

@inline ℒ2(x) = 2one(x)*cosh(x)
@inline ∂logℒ2(x) = tanh(x)
@inline logℒ2(x::T) where T<:Real =
    log(2one(x))+ (abs(x)<= T(12.0) ? log(cosh(x)) : abs(x) - log(2one(X)))
@inline logℒ2(x::Complex{T}) where T<:Real =
    log(T(2.0))+ (abs(x)<= T(12.0) ? log(cosh(x)) : abs(x) - log(2one(x)))

@inline ℒ(x) = one(x) + exp(x)
#@inline ∂logℒ(x) = one(x)/(one(x)+exp(-x))
const ∂logℒ = NNlib.σ
NNlib.σ(x::Complex) = one(x)/(one(x)+exp(-x))
#@inline logℒ(x) = log1p(exp(x))#log(one(x) + exp(x))
const logℒ = NNlib.softplus
NNlib.softplus(x::Complex) = log1p(exp(x))#log(one(x) + exp(x))


text:
using Downloads: download
using FileIO, Makie, Colors, GeoJSON, FileIO, Downloads, GeoMakie
using RadeonProRender, RPRMakie
RPR = RadeonProRender

earth_img = load(download("https://upload.wikimedia.org/wikipedia/commons/5/56/Blue_Marble_Next_Generation_%2B_topography_%2B_bathymetry.jpg"))

# data from
# https://github.com/telegeography/www.submarinecablemap.com
urlPoints = "https://raw.githubusercontent.com/telegeography/www.submarinecablemap.com/master/web/public/api/v3/landing-point/landing-point-geo.json"

landPoints = Downloads.download(urlPoints, IOBuffer())
land_geoPoints = GeoJSON.read(seekstart(landPoints))
toPoints = GeoMakie.geo2basic(land_geoPoints)

function toCartesian(lon, lat; r = 1.02, cxyz = (0,0,0) )
    lat, lon = lat*π/180, lon*π/180
    cxyz[1] + r * cos(lat) * cos(lon), cxyz[2] + r * cos(lat) * sin(lon), cxyz[3] + r *sin(lat)
end

n = 1024 ÷ 4 # 2048
θ = LinRange(0,pi,n)
φ = LinRange(-pi,pi,2*n)
xe = [cos(φ)*sin(θ) for θ in θ, φ in φ]
ye = [sin(φ)*sin(θ) for θ in θ, φ in φ]
ze = [cos(θ) for θ in θ, φ in φ]
fig = Figure(; resolution = (1500,1500))
ax = LScene(fig[1, 1], scenekw=(show_axis=false,))
surface!(ax, xe, ye, ze, color = earth_img)
toPoints3D = [Point3f([toCartesian(point[1], point[2])...]) for point in toPoints]
meshscatter!(ax, toPoints3D, color = 1:length(toPoints3D), markersize = 0.01, colormap = :plasma)

isdefined(Main, :context) && RPR.release(context)
context = RPR.Context()

RPRMakie.to_rpr_scene(context, ax.scene)

fb_size = (1500, 1500)
frame_buffer = RPR.FrameBuffer(context, RGBA, fb_size)
frame_bufferSolved = RPR.FrameBuffer(context, RGBA, fb_size)
set!(context, RPR.RPR_AOV_COLOR, frame_buffer)
set_standard_tonemapping!(context)

begin
    clear!(frame_buffer)
    RPR.rprContextSetParameterByKey1u(context, RPR.RPR_CONTEXT_ITERATIONS, 100)
    RPR.render(context)
    RPR.rprContextResolveFrameBuffer(context, frame_buffer, frame_bufferSolved, false)
    RPR.save(frame_bufferSolved, "test.png")
end


text:
using ApproxFun, OrdinaryDiffEq
using LinearAlgebra
using DiffEqOperators

function makesim(sts::Union{STS{:KuramotoSivashinsky}, STS{:ksiva}})
    @unpack ic, T, S, Δt, N = sts
    L = try
            L = sts.p.L
        catch e
            L = 22
        end
    # prepare initial condition
    if ic isa Union{Nothing, Integer}
        # here we integrate a pre-determined initial condition
        # for a random amount of time
        rng = MersenneTwister(isnothing(ic) ? 42 : ic)
        S += Δt*rand(rng, 10:1000)
        x = L*(0:N-1)/N
        u0 = @. cos(x) + 0.1*sin(x/8) + 0.01*cos((2π/L)*x)
    elseif ic isa AbstractVector{<:Real}
        u0 = ic
        @assert length(u0) == N
    else
        error(ICERROR)
    end
    saveat = S:Δt:(T+S)
    t, u = kuramoto_sivashinsky(u0, L, saveat)
    t = 0:Δt:T
    return @strdict u t sts
end

"""
    kuramoto_sivashinsky(u, L, saveat)

Solve the Kuramoto Sivashinsky system given by the PDE
```math
u_t = -u*u_x - u_{xx} - u_{xxxx}
```
with periodic boundary conditions and initial condition `u`.

`L` is the physical spatial length (with `length(u)` points sampled
equidistantly in it).

`saveat` dictates at which timepoints to save the system (which is solved
using `OrdinaryDiffEq`).
"""
function kuramoto_sivashinsky(u, L, saveat)
    n = length(u)                  # number of gridpoints
    F = Fourier(0..L)
    T = ApproxFun.plan_transform(F, n)
    Ti = ApproxFun.plan_itransform(F, n)

    #Linear Part
    D  = (Derivative(F) → F)[1:n,1:n]
    D2 = Derivative(F,2)[1:n,1:n]
    D4 = Derivative(F,4)[1:n,1:n]
    A = DiffEqArrayOperator(Diagonal(-D2-D4))

    #Nonlinear Part
    ks = KSFunctor(zeros(n))

    params = (D, T, Ti)
    prob = SplitODEProblem(A, ks, T*u, (0.0, saveat[end]), params)
    sol = solve(prob, ETDRK4(), dt=Float64(saveat.step), saveat=saveat,
                save_start = false, save_end = false)
    # @assert sol.t == saveat
    return sol.t, map(u -> Ti*u, sol.u)
end

struct KSFunctor{T}; tmp::Vector{T}; end
function (ks::KSFunctor)(du,u,p,t)
    D, T, Ti = p
    mul!(du,Ti,u)
    @. du = -1/2*du^2
    mul!(ks.tmp, T, du)
    mul!(du, D, ks.tmp)
end


text:
abstract MFOperation <: Membershipfunction

#############################
# Interection of 2 Membership Functions
immutable T1IntersectOP <: MFOperation
    t1::Membershipfunction
    t2::Membershipfunction
end
# export T1IntersectOP

import Base: getindex
getindex(op::T1IntersectOP, value::Real) = tnorm(op.t1[value], op.t2[value])
export getindex

# Union of 2 Membership Functions
immutable T1UnionOP <: MFOperation
    t1::Membershipfunction
    t2::Membershipfunction
end
# export T1UnionOP

import Base: getindex
getindex(op::T1UnionOP, value::Real) = tconorm(op.t1[value], op.t2[value])
export getindex

# Complement of
immutable T1ComplementOP <: MFOperation
    t1::Membershipfunction
end
import Base: getindex
getindex(op::T1ComplementOP, value::Real) = 1.0 - op.t1[value]
export getindex
# export T1ComplementOP

Base.complement(mf::Membershipfunction) = T1ComplementOP(mf)
export complement

Base.intersect(mf::Membershipfunction, mfs::Membershipfunction...) = intersect(T1IntersectOP(mf,mfs[1]),mfs[2:end]...)
Base.intersect(mf::Membershipfunction, mf1::Membershipfunction) = T1IntersectOP(mf,mf1)
export intersect

Base.union(mf::Membershipfunction, mfs::Membershipfunction...) = union(T1UnionOP(mf,mfs[1]),mfs[2:end]...)
Base.union(mf::Membershipfunction, mf1::Membershipfunction) = T1UnionOP(mf,mf1)
export union


text:
include("prelude.jl")

@testset "default_pty" begin
    interact(`cat`, 1) do proc
        @static if Sys.isunix()
            @test typeof(proc.out_stream) <: Expect.TTY
        else
            @test typeof(proc.out_stream) <: Base.PipeEndpoint
        end
    end
end

@testset "8bit_pty" begin
    interact(`cat`, 1) do proc
        # Ensure the transport is already 8bit safe
        buf = [UInt8(i) for i in 0:255]
        write(proc, buf)
        ret = Vector{UInt8}(undef, length(buf))
        readbytes!(proc, ret)
        @test buf == ret
    end
end

@testset "nop_raw!" begin
    interact(`cat`, 1) do proc
        # raw! is unnecessary (it's toggled during construction), but it should be a
        # no-op on windows for compatibility
        @test Expect.raw!(proc, true)
        @test process_running(proc)
    end
end

@testset "no_pty" begin
    # Test with pty=false
    interact(`cat`, 1; pty=false) do proc
        @test typeof(proc.out_stream) <: Base.PipeEndpoint

        # raw should always succeed on pipes/windows
        @test raw!(proc, true)

        # switching off raw without a pty should fail
        @test !raw!(proc, false)

        @test process_running(proc)
    end
end


text:
# optim.jl
using Flux
using JLD: save

function optim_cnn(xin,
                   density,
                   G,
                   model,
                   yi,
                   ri,
                   niter,
                   β,
                   xtrue,
                   doseGT,
                   kernel,
                   scale_factor_den,
                   scale_factor_dose;
                   nepoch = 100)

    # generate DVK dose-rate map for residual learning
    doseDVK = gen_dvk(xin, density, kernel, scale_factor_den, scale_factor_dose)
    loss_dict = -1 * ones(nepoch)

    for epoch = 1:nepoch

        # run ML-EM recon
        xout = reg_eml_em(xin, density, G, model, yi, ri, niter; β)

        # input to the CNN
        xout_gpu = cu(Flux.unsqueeze(Flux.unsqueeze(xout, 4), 5))
        # density_gpu = cu(Flux.unsqueeze(Flux.unsqueeze(density, 4), 5))

        # ground truth
        # data_GT = cu(Flux.unsqueeze(cat(xtrue, doseGT .- doseDVK, dims = 4), 5))
        data_GT = cu(Flux.unsqueeze(Flux.unsqueeze(xtrue, 4), 5))

        # put model onto GPU
        # data_input = (xout_gpu, density_gpu)
        model_gpu = fmap(cu, model)
        ps = Flux.params(model_gpu)
        opt = ADAM(0.002)

        # define loss function
        function loss(data_input, xtrue)
            pred = model_gpu(data_input)
            return Flux.Losses.mse(pred, xtrue)
            # α = 0.5
            # return α * Flux.Losses.mse(pred[:,:,:,1,1], xtrue[:,:,:,1,1])
            #        + Flux.Losses.mse(pred[:,:,:,2,1], xtrue[:,:,:,2,1])
        end

        evalcb() = @show(loss(xout_gpu, data_GT))
        data = [(xout_gpu, data_GT)]

        # train 100 epochs
        loss_dict[epoch] = loss(xout_gpu, data_GT)

        @epochs 100 Flux.train!(loss, ps, data, opt, cb = evalcb)
        # back to CPU
        model = model_gpu |> cpu
        @save "mymodel_recon_16th.bson" model
        save("loss_curve.jld", "loss_dict", loss_dict)
    end
end


text:
# Use baremodule to shave off a few KB from the serialized `.ji` file
baremodule SuiteSparse_GPU_jll
using Base
using Base: UUID
import JLLWrappers

JLLWrappers.@generate_main_file_header("SuiteSparse_GPU")
JLLWrappers.@generate_main_file("SuiteSparse_GPU", UUID("d273dfef-62f2-5f88-806b-f32b81b662f1"))
end  # module SuiteSparse_GPU_jll


text:
## Domains

show(io::IO,d::Segment) = print(io,"the segment [$(leftendpoint(d)),$(rightendpoint(d))]")
function show(io::IO,d::Line)
    if d.center == angle(d) == 0 && d.α == d.β == -1.
        print(io,"ℝ")
    elseif  d.α == d.β == -1.
        print(io,"Line($(d.center),$(angle(d)))")
    else
        print(io,"Line($(d.center),$(angle(d)),$(d.α),$(d.β))")
    end
end

function show(io::IO,d::Ray)
    if d.orientation && angle(d)==0
        print(io,"【$(d.center),∞❫")
    elseif  d.orientation && angle(d)==1.0π
        print(io,"【$(d.center),-∞❫")
    elseif  d.orientation
        print(io,"【$(d.center),exp($(angle(d))im)∞❫")
    elseif !d.orientation  && angle(d)==0
        print(io,"❪∞,$(d.center)】")
    elseif !d.orientation && angle(d)==1.0π
        print(io,"❪-∞,$(d.center)】")
    else
        print(io,"❪exp($(angle(d))im)∞,$(d.center)】")
    end
end

show(io::IO,d::PeriodicSegment) = print(io,"【$(leftendpoint(d)),$(rightendpoint(d))❫")
show(io::IO,d::Circle) =
    print(io,(d.radius==1 ? "" : string(d.radius))*
                    (d.orientation ? "🕒" : "🕞")*
                    (d.center==0 ? "" : "+$(d.center)"))
show(io::IO,d::Point) = print(io,"Point($(d.x))")

## Spaces

show(io::IO,::ConstantSpace{AnyDomain}) = print(io,"ConstantSpace")
show(io::IO,S::ConstantSpace) = print(io,"ConstantSpace($(domain(S)))")
show(io::IO,f::Fun{ConstantSpace{AnyDomain}}) =
    print(io,"$(convert(Number,f)) anywhere")

show(io::IO,f::Fun{ConstantSpace{DD,RR}}) where {DD,RR} =
    print(io,"$(convert(Number,f)) on $(domain(f))")

for typ in ("Chebyshev","Fourier","Laurent","Taylor","SinSpace","CosSpace")
    TYP = Meta.parse(typ)
    @eval function show(io::IO,S::$TYP{D,R}) where {D,R}
        print(io,$typ*"(")
        show(io,domain(S))
        print(io,")")
    end
end

function show(io::IO, S::Ultraspherical)
    print(io,"Ultraspherical($(order(S)),")
    show(io,domain(S))
    print(io,")")
end

function show(io::IO,S::Jacobi)
    S.a == S.b == 0 ? print(io,"Legendre(") : print(io,"Jacobi($(S.b),$(S.a),")
    show(io,domain(S))
    print(io,")")
end

show(io::IO, S::Chebyshev{<:ChebyshevInterval}) = print(io, "Chebyshev()")
show(io::IO, S::Ultraspherical{<:Any,<:ChebyshevInterval}) = print(io, "Ultraspherical($(order(S)))")
show(io::IO, S::Jacobi{<:ChebyshevInterval}) =
    S.a == S.b == 0 ? print(io,"Legendre()") : print(io,"Jacobi($(S.b),$(S.a))")

function show(io::IO,s::JacobiWeight)
    d=domain(s)
    #TODO: Get shift and weights right
    if s.α==s.β
        print(io,"(1-x^2)^$(s.α)[")
    elseif s.β==0
        print(io,"(1-x)^$(s.α)[")
    elseif s.α==0
        print(io,"(1+x)^$(s.β)[")
    else
        print(io,"(1+x)^$(s.β)*(1-x)^$(s.α)[")
    end

    show(io,s.space)
    print(io,"]")
end

function show(io::IO,s::LogWeight)
    d=domain(s)
    #TODO: Get shift and weights right
    if s.α==s.β
        print(io,"log((1-x^2)^$(s.α))[")
    else
        print(io,"log((1+x)^$(s.α)*(1-x)^$(s.β))[")
    end

    show(io,s.space)
    print(io,"]")
end

function show(io::IO,s::QuotientSpace)
    show(io,s.space)
    print(io," /\n")
    show(io,s.bcs;header=false)
end

function show(io::IO,ss::SumSpace)
    s = components(ss)
    show(io,s[1])
    for sp in s[2:end]
        print(io,"⊕")
        show(io,sp)
    end
end

function show(io::IO,ss::PiecewiseSpace)
    s = components(ss)
    show(io,s[1])
    for sp in s[2:end]
        print(io,"⨄")
        show(io,sp)
    end
end

summary(ss::ArraySpace) = string(Base.dims2string(length.(axes(ss))), " ArraySpace")
function show(io::IO,ss::ArraySpace;header::Bool=true)
    header && print(io,summary(ss)*":\n")
    show(io, ss.spaces)
end

function show(io::IO,s::TensorSpace)
    d = length(s.spaces)
    for i=1:d-1
        show(io,s.spaces[i])
        print(io,"⊗")
    end
    show(io,s.spaces[d])
end

function show(io::IO,s::SubSpace)
    print(io,s.space)
    print(io,"|")
    show(io,s.indexes)
end

## Fun

show(io::IO, ::MIME"text/plain", f::Fun) = show(io, f)

function show(io::IO, f::Fun)
    print(io,"Fun(")
    show(io,f.space)
    print(io,",")
    show(io,f.coefficients)
    print(io,")")
end

## MultivariateFun

function show(io::IO,L::LowRankFun)
    print(io,"LowRankFun on ",space(L)," of rank ",rank(L),".")
end

function show(io::IO,P::ProductFun)
    print(io,"ProductFun on ",space(P),".")
end

## Operator

summary(B::Operator) =
    string(typeof(B).name.name)*" : "*string(domainspace(B))*" → "*string(rangespace(B))

struct PrintShow
    str
end
Base.show(io::IO,N::PrintShow) = print(io,N.str)

function show(io::IO,B::Operator;header::Bool=true)
    header && println(io,summary(B))
    dsp=domainspace(B)

    if !isambiguous(domainspace(B)) && (eltype(B) <: Number)
        if isbanded(B) && isinf(size(B,1)) && isinf(size(B,2))
            BM=B[1:10,1:10]

            M=Matrix{Any}(undef,11,11)
            fill!(M,PrintShow("⋅"))
            for j = 1:size(BM,2),k = colrange(BM,j)
                M[k,j]=BM[k,j]
            end

            for k=max(1,11-bandwidth(B,2)):11
                M[k,end]=PrintShow("⋱")
            end
            for j=max(1,11-bandwidth(B,1)):10
                M[end,j]=PrintShow("⋱")
            end

            print_array(io, M)
        elseif isinf(size(B,1)) && isinf(size(B,2))
            BM=B[1:10,1:10]

            M=Matrix{Any}(undef,11,11)
            for k=1:10,j=1:10
                M[k,j]=BM[k,j]
            end

            M[1,end]=PrintShow("⋯")
            M[end,1]=PrintShow("⋮")

            for k=2:11
                M[k,end]=M[end,k]=PrintShow("⋱")
            end

            print_array(io, M)
        elseif isinf(size(B,1))
            BM=B[1:10,1:size(B,2)]

            M=Matrix{Any}(undef,11,size(B,2))
            for k=1:10,j=1:size(B,2)
                M[k,j]=BM[k,j]
            end
            for k=1:size(B,2)
                M[end,k]=PrintShow("⋮")
            end

            print_array(io, M)
        elseif isinf(size(B,2))
            BM=B[1:size(B,1),1:10]

            M=Matrix{Any}(undef,size(B,1),11)
            for k=1:size(B,1),j=1:10
                M[k,j]=BM[k,j]
            end
            for k=1:size(B,1)
                M[k,end]=PrintShow("⋯")
            end

            print_array(io, M)
        else
            print_array(io, AbstractMatrix(B)[1:size(B,1),1:size(B,2)])
        end
    end
end