text:
#####
##### Helper Functions
#####

# Exected transformation:
#
# @op f(a,b,c) -> Node(@ngraphcall f(a,b,c))
# @op T f(a,b,c) -> Node{T}(@ngraphcall f(a,b,c))
# @op T N f(a,b,c) -> Node(T,N)(@ngraphcall f(a,b,c))
"""
    @op [T] [N] fn(args)

Make an Op call into the nGraph library.
Automatically returns a `Node`, with optional type parameters `T` and `N`.
"""
macro op(x...)
    if length(x) > 3
        error("Expected at most 3 arguments to @op")
    end
    # Get the Optional Type and Dimensionality Parameters
    T = length(x) > 1 ? x[1] : nothing
    N = length(x) > 2 ? x[2] : nothing
    expr = x[end]

    # Make sure this is a function call.
    # wrap the `Lib` module around the op.
    if expr.head != :call
        error("@Op only support function calls.")
    end

    # Escape all but the function name.
    # Since we forward this to the `@ngraphcall` macro, we apparently need to do this.
    for i in 2:length(expr.args)
        expr.args[i] = esc(expr.args[i])
    end

    # Prefix the node constructor.
    params = []
    isnothing(T) || push!(params, T)
    isnothing(N) || push!(params, N)
    prefix = isempty(params) ? :(Node) : :(Node{$(esc.(params)...)})
    return :($prefix(@ngraphcall $expr))
end

#####
##### Shape and element type promotion
#####

promote_eltype(x::Node{T}, y::Node{T}) where {T} = (x,y)
function promote_eltype(x::Node{T1}, y::Node{T2}) where {T1,T2}
    T = promote_type(T1, T2)
    return convert_eltype.(T, (x,y))
end

function autobroadcast(x::Node, y::Node)
    # Get the common axes for this object
    shape = map(last, Base.Broadcast.combine_axes(x, y))

    # Make broadcasts if needed.
    x = (size(x) == shape) ? x : broadcast(x, shape)
    y = (size(y) == shape) ? y : broadcast(y, shape)
    return (x, y)
end

function homogenize(x::Node, y::Node)
    x, y = promote_eltype(x, y)
    return autobroadcast(x, y)
end

#####
##### Broadcasting machinery
#####

# Define _forward to allow dispatching to alternative implementations of common base
# operations.
#
# We basically shortcircuit the lazy base implementations.
_forward(f) = f
_forward(::typeof(*)) = multiply
#_forward(::typeof(NNlib.σ)) = _sigmoid
_forward(::typeof(/)) = divide

Base.broadcasted(f, x::Node, y::Node) = _forward(f)(homogenize(x,y)...)
Base.broadcasted(f, x::Node, y::AbstractArray) = _forward(f)(homogenize(x, Node(y))...)
Base.broadcasted(f, x::AbstractArray, y::Node) = _forward(f)(homogenize(Node(x), y)...)
Base.broadcasted(f, x::Node) = _forward(f)(x)

function Base.broadcasted(f, x::Node{T}, y::Number) where {T}
    return _forward(f)(homogenize(x, Node{T}(convert(T, y)))...)
end
function Base.broadcasted(f, x::Number, y::Node{T}) where {T}
    return _forward(f)(homogenize(Node{T}(convert(T, x)), y)...)
end

Base.convert(::Type{Node{T,0}}, x::S) where {T,S <: Number} = Node{T,0}(convert(T, x))

# Special case element-wise copy - this gets around an issue in Metalhead's ResNet
# implementation.
Base.broadcasted(::typeof(copy), x::Node) = x

#####
##### Parameter
#####

parameter(x::T) where {T} = @op T 0 op_parameter(T, Int64[])
function parameter(::Type{T}, dims::NTuple{N,Int}) where {T,N}
    _shape = shape(dims)
    GC.@preserve _shape begin
        node = @op T N op_parameter(T, _shape)
    end
    return node
end

parameter(x::AbstractArray{T,N}) where {T,N} = parameter(T, size(x))
parameter(::Type{T}, dims...) where {T} = parameter(T, dims)
parameter(x::Node) = x

############################################################################################

#####
##### Add
#####

add(a::U, b::U) where {T,N,U <: Node{T,N}} = @op T N op_add(a, b)
Base.:+(a::Node, b::Node) = add(a,b)

#####
##### AvgPool
#####

function avgpool(x::Node{T,N}, kernel; pad = 0, stride = kernel) where {T,N}
    return @op T N op_avgpool(
        x,
        strides(N-2, stride),
        shape(N-2, pad),
        shape(N-2, pad),
        shape(N-2, kernel),
        false,
    )
end
#Flux.meanpool(x::Node, args...; kw...) = avgpool(x, args...; kw...)

# #####
# ##### BatchNorm
# #####
#
# function batchnorm_training(input::Node, γ::Node, β::Node, ϵ)
#     return Node(Lib.op_batchnorm_training(
#         getpointer(input),
#         getpointer(γ),
#         getpointer(β),
#         convert(Float64, ϵ)
#     ))
# end

#####
##### Broadcast
#####

function Base.broadcast(a::Node{T,M}, like::Node{Int64,1}) where {T,M,N}
    return @op T op_broadcast(a, like)
end

function Base.broadcast(a::Node{T}, dims::NTuple{N,Int64}) where {T,N}
    return Base.broadcast(a, constant(shape(dims)))
end

#####
##### Concat
#####

function concat(nodes::Vector{Node{T,N}}; dims::Integer = 1) where {T,N}
    # Flip dims for column -> row
    return @op T N op_concat(nodes, N - dims)
end
Base.cat(x::Node...; kw...) = concat(collect(x); kw...)

#####
##### Constants
#####

function constant(x::T) where {T}
    A = Array{T,0}(undef)
    A[] = x
    return constant(A)
end

function constant(x::AbstractArray{T,N}) where {T,N}
    # Reinterpret the `x` as a byte array
    _x = collect(reinterpret(UInt8, reshape(x,:)))
    GC.@preserve _x begin
        node = @op T N op_constant(Element(T)[], shape(size(x)), _x)
    end
    return node
end

#####
##### Convert
#####

convert_eltype(::Type{T}, x::Node{T}) where {T} = x
function convert_eltype(::Type{T}, x::Node{U,N}) where {T,U,N}
    @op T N op_convert(x, T)
end

#####
##### Divide
#####

divide(a::Node{T,N}, b::Node{T,N}) where {T,N} = @op T N op_divide(a, b)
Base.:/(a::Node{T,0}, b::Node{T,0}) where {T} = divide(a, b)
Base.://(a::Node{T,0}, b::Node{T,0}) where {T} = divide(a, b)

#####
##### Dot
#####

# Reverse the order in the call to `Lib.op_dot` to account for row major/col major
# differences
dot(a::Node{T}, b::Node{T}, n) where {T} = @op T op_dot(b, a, UInt(n))
Base.:*(x::Node, y::Node) = dot(x, y, 1)

# #####
# ##### Indexing
# #####
#
# _lb(i) = i
# _lb(::Colon) = 1
#
# _ub(bound, i) = i
# _ub(bound, ::Colon) = bound
# function Base.getindex(n::Node{T,N}, args...) where {T,N}
#     sz = size(n)
#     # Subtract 1 from the lower bound since the lower bounds are inclusive in ngraph.
#     # Leave the upper bounds as is since the upper-bounds are exclusive.
#     lb = Coordinate(map(_lb, args) .- 1)
#     ub = Coordinate(ntuple(i -> _ub(sz[i], args[i]), length(args)))
#
#     return Node(Lib.op_slice(getpointer.((n, lb, ub))... ))
# end

#####
##### GetOutput
#####

goe(x::Node, n) = @op op_goe(x, convert(UInt64, n-1))

#####
##### Log
#####

Base.log(a::Node{T,N}) where {T,N} = @op T N op_log(a)

#####
##### Max
#####

# The semantics between max and maximum are flipped around beween Julia and nGraph
Base.max(a::U, b::U) where {T,N, U <: Node{T,N}} = @op T N op_maximum(a, b)

# #####
# ##### MaxPool
# #####
#
# function Flux.maxpool(x::Node{T,N}, shape::Tuple; pad = 0, stride = shape) where {T,N}
#     # Convert to nGraph types
#     window_shape = Shape(shape)
#     strides = Strides(expand(N-2, stride))
#     padding_below = Shape(expand(N-2, pad))
#     padding_above = Shape(expand(N-2, pad))
#
#     ptr = Lib.op_maxpool(getpointer(x), window_shape, strides, padding_below, padding_above)
#     return Node{T,N}(ptr)
# end
#
# stride_size(c::NNlib.PoolDims{N,K,S,P,D}) where {N,K,S,P,D} = S
# pad_size(c::NNlib.PoolDims{N,K,S,P,D}) where {N,K,S,P,D} = P
# function NNlib.maxpool(x::Node{T,N}, dims::NNlib.PoolDims) where {T,N}
#     ptr = Lib.op_maxpool(
#         getpointer(x),
#         Shape(NNlib.kernel_size(dims)),             # window_shape
#         Strides(stride_size(dims)),                 # strides (same as window_shape)
#         Shape(pad_size(dims)[1:div(N,2)]),          # padding_below
#         Shape(pad_size(dims)[(div(N,2) + 1):N]),    # padding_above
#     )
#     return Node{T,N}(ptr)
# end

#####
##### Multiply
#####

multiply(a::Node{T,N}, b::Node{T,N}) where {T,N} = @op T N op_mul(a, b)

#####
##### Minimum
#####

# The `min` and `minimum` semantics are swapped between Julia and nGraph.
Base.min(a::Node{T,N}, b::Node{T,N}) where {T,N} = @op T N op_minimum(a, b)

#####
##### Negative
#####

negative(a::Node{T,N}) where {T,N} = @op T N op_negative(a)
Base.:-(a::Node) = negative(a)

# #####
# ##### One Hot
# #####
#
# function onehot(x::Node{T,N}, max_index, onehot_index) where {T,N}
#     # Create the output size from `max_index` and `onehot_index`
#     sz = size(x)
#     output_sz = collect(splicein(sz, max_index, onehot_index))
#
#     return Node{T,N+1}(Lib.op_onehot(
#         getpointer(x .- one(T)),
#         Shape(output_sz),
#         convert(UInt, N + 1 - onehot_index)
#     ))
# end

# #####
# ##### permutedims
# #####
#
# function Base.permutedims(x::N, perm) where {N <: Node}
#     av = AxisVector(perm, length(perm))
#     shape = Shape([size(x)[i] for i in perm])
#     return N(Lib.op_reshape(getpointer(x), av, shape))
# end
#
# #####
# ##### Power
# #####
#
# power(a::N, b::N) where {N <: Node} = N(Lib.op_parameter(getpointer(a), getpointer(b)))
# Base.:^(a::N, b::N) where {N <: Node} = power(a, b)
#
# #####
# ##### Relu
# #####
#
# Flux.relu(a::Node{T,N}) where {T,N} = Node{T,N}(Lib.op_relu(getpointer(a)))
#
# #####
# ##### Reshape
# #####
#
# # NOTE:We're hijacking an internal Base function here to do all of the `Base.Colon`
# # preprocessing for us
# function Base._reshape(x::Node{T,N}, dims::NTuple{M,Int}) where {T,N,M}
#     av = AxisVector(1:N, N)
#     shape = Shape(dims)
#     node = Lib.op_reshape(getpointer(x), av, shape)
#     return Node{T,M}(node)
# end

# #####
# ##### Sigmoid
# #####
#
# _sigmoid(x::N) where {N <: Node} = N(Lib.op_sigmoid(getpointer(x)))
#
# #####
# ##### Softmax
# #####
#
# function Flux.softmax(x::Node{T,N}; axes = 1) where {T,N}
#     av = AxisSet(axes, N)
#     node = Lib.op_softmax(getpointer(x), av)
#     return Node{T,N}(node)
# end
#
# #####
# ##### Sqrt
# #####
#
# Base.sqrt(x::N) where {N <: Node} = N(Lib.op_sqrt(getpointer(x)))
#
# #####
# ##### Subtract
# #####
#
# subtract(a::N, b::N) where {N <: Node} = N(Lib.op_subtract(getpointer(a), getpointer(b)))
# Base.:-(a::N, b::N) where {N <: Node} = subtract(a, b)
#
# #####
# ##### Sum
# #####
#
# # Default to reducing along all dimensions
# function Base.sum(x::Node{T,N}; axes = 1:N ) where {T,N}
#     as = AxisSet(axes, N)
#     node = Lib.op_sum(getpointer(x), as)
#     return Node{T, N - length(axes)}(node)
# end
#
# #####
# ##### Tanh
# #####
#
# Base.tanh(a::N) where {N <: Node} = N(Lib.op_tanh(getpointer(a)))
#
# #####
# ##### Transpose
# #####
#
# function Base.transpose(a::Node{T,N}) where {T,N}
#     av = AxisVector(N:-1:1, N)
#     shape = Shape(reverse(size(a)))
#     node = Lib.op_reshape(getpointer(a), av, shape)
#     return Node{T,N}(node)
# end


text:
rosenbrock2d(x) = 100.0*abs2(x[2] - x[1]^2) + abs2(x[1] - 1.0)

function rosenbrock(x)
    @inbounds res = sum(i -> 100.0*abs2(x[i+1] - x[i]^2) + abs2(x[i] - 1.0), 1:length(x)-1)
    return res
end

@testset "bboptimize" begin
    @testset "README examples" begin
        @testset "example #1" begin
            res = bboptimize(rosenbrock2d; SearchRange = (-5.0, 5.0), NumDimensions = 2, TraceMode = :silent)
            @test best_fitness(res) < 0.001
            @test length(best_candidate(res)) == 2
        end

        @testset "example #2" begin
            res = bboptimize(rosenbrock2d; SearchRange = [(-5.0, 5.0), (-2.0, 2.0)], TraceMode = :silent)
            @test best_fitness(res) < 0.001
        end

        @testset "example #3" begin
            res = bboptimize(rosenbrock2d; SearchRange = (-5.0, 5.0), NumDimensions = 2, Method = :de_rand_1_bin, TraceMode = :silent)
            @test best_fitness(res) < 0.001
        end

        @testset "example #4" begin
            res = bboptimize(rosenbrock2d; SearchRange = (-5.0, 5.0), NumDimensions = 2,
                            Method = :random_search, MaxTime = 10.0, TraceMode = :silent)
            @test best_fitness(res) < 0.2
        end

        @testset "example #5" begin
            res = BlackBoxOptim.compare_optimizers(rosenbrock; SearchRange = (-5.0, 5.0), NumDimensions = 3,
                            MaxTime = 2.0, TraceMode = :compact)

            # We at least expect the DE optimizers and DX-NES to come out better than random search
            idx_adaptive_de = findfirst(t -> t[1] == :adaptive_de_rand_1_bin, res)
            idx_random = findfirst(t -> t[1] == :random_search, res)
            @test idx_adaptive_de < idx_random

            idx_dxnes = findfirst(t -> t[1] == :dxnes, res)
            @test idx_dxnes < idx_random
        end
    end

    # run one longer example in case there is problem with the reporting in long runs
    @testset "long runs reporting" begin
        res = bboptimize(rosenbrock2d; SearchRange = (-5.0, 5.0), NumDimensions = 2,
            Method = :de_rand_1_bin, TraceMode = :silent, MaxSteps = 25001)
        @test best_fitness(res) < 0.001
    end

    @testset "Fixed-dimensional problem takes precedence over search range and related params" begin
        prob = BlackBoxOptim.minimization_problem((x) -> sum(x), "no name", (10.0, 20.0), 3)
        res = bboptimize(prob; SearchRange = (0.0, 2.0), NumDimensions = 2, TraceMode = :silent)
        xbest = best_candidate(res)
        @test length(xbest) == 3
        @test xbest[1] >= 10.0
        @test xbest[2] >= 10.0
        @test xbest[3] >= 10.0
    end

    @testset "Fault Handling" begin
        #@testset "anydimensional problem" begin
        #    @test_throws ArgumentError bboptimize(BlackBoxOptim.anydim_problem("dummy", (x) -> sum(x), 0.0:1.0))
        #end

        @testset "func & SearchRange given but no NumDimensions specified" begin
            @test_throws KeyError BlackBoxOptim.setup_problem(sum, ParamsDict(:SearchRange => (0.0, 1.0)))
        end
    end

#  @testset "restarting an optimizer again and again should gradually improve" begin
#    optimizer, problem, params = BlackBoxOptim.setup_bboptimize(rosenbrock2d,
#      {:SearchRange => (-5.0, 5.0), :NumDimensions => 2,
#        :MaxSteps => 10, :TraceMode => :silent})
#    best10, fitness10, termination_reason10, elapsed_time10, params, num_evals10 = #BlackBoxOptim.run_optimizer(optimizer, problem, params);
#    best20, fitness20, termination_reason20, elapsed_time20, params, num_evals20 = #BlackBoxOptim.run_optimizer(optimizer, problem, params);
#    params[:MaxSteps] = 980
#    best1000, fitness1000, termination_reason1000, elapsed_time1000, params, num_evals1000 = #BlackBoxOptim.run_optimizer(optimizer, problem, params);
#    params[:MaxSteps] = 1000
#    fitness10000 = best10000 = elapsed_time1000b = 1 # Just so saved outside of loop body...
#    for i in 1:9
#      best10000, fitness10000, termination_reason10000, elapsed_time1000b, params, num_evals10000 = #BlackBoxOptim.run_optimizer(optimizer, problem, params);
#    end
#
#    # Fitness is not worse in sub-sequent runs
#    @test fitness10 >= fitness20 >= fitness1000 >= fitness10000
#
#    # and it should (almost) always be better after 10000 steps than after 10:
#    @test fitness10 > fitness10000
#  end
end

ext: jl


text:
function gimbal_conntroller(;
                            full=true,
                            J = 0.029,
                            k_s = 0.2,
                            k_v = 0.545,
                            ω_brk = 0.074,
                            T_c = 0.106,
                            T_brk = 0.177,
                            K_dc = 6878.7,
                            Z1 = 0.01,
                            Z2 = 0.029,
                            P2 = 0.16,
                            Z3 = 0.06,
                            P3 = 0.009,
                            Z4 = 0.01,
                            bw = 465.0)
    @parameters t
    @named pid1 = PID_factory(kp=Z1,ki=1.0,kd=0.0)
    @named lag = lead_lag_factory(k=1.0, zero=Z2, pole=P2)
    @named lead = lead_lag_factory(k=1.0, zero=Z3, pole=P3)
    @named pid2 = PID_factory(kp=Z4,ki=1.0,kd=0.0)
    @named amp = amp_factory(a=K_dc)
    @named notch1 = notch(ω=100.0,gain=0.1,width=0.3)
    @named notch2 = notch(ω=330.0,gain=0.1,width=0.3)
    @named notch3 = notch(ω=380.0,gain=0.1,width=0.3)
    @named notch4 = notch(ω=475.0,gain=0.1,width=0.3)
    @named p = gimbal_plant(;J,k_s,k_v,ω_brk,T_c,T_brk)
    @named gyro = sensor(bw=bw)
    tspan = (0.0,5.0)
    if full
        connections = [
            p.ω_body ~ 16*π^2/180*sin(2π*2t)
            gyro.input ~ p.ω
            pid1.e ~ -(180/π)*gyro.output
            lead.input ~ pid1.u
            lag.input ~ lead.output
            pid2.e ~ lag.output
            amp.input ~ pid2.u
            notch1.input ~ amp.output
            notch2.input ~ notch1.output
            notch3.input ~ notch2.output
            notch4.input ~ notch3.output
            p.T ~ 0.339*min(max(notch4.output,-2.5),2.5)]
        @named _connected = ODESystem(connections, t)
        @named connected = compose(_connected,
                                   [pid1, pid2, lead, lag, amp,
                                    notch1, notch2, notch3, notch4,
                                    p, gyro])
        sys = alias_elimination(connected)
        prob = ODEProblem(sys,[],tspan,jac=true)
    else
        connections = [
            p.ω_body ~ 16*π^2/180*sin(2π*2t)
            pid1.e ~ -(180/π)*p.ω
            lead.input ~ pid1.u
            lag.input ~ lead.output
            pid2.e ~ lag.output
            amp.input ~ pid2.u
            p.T ~ 0.339*min(max(amp.output,-2.5),2.5)]
        @named _connected = ODESystem(connections, t)
        @named connected = compose(_connected,
                                   [pid1, pid2, lead,
                                    lag,  amp, p])
        #sys = structural_simplify(dae_index_lowering(connected))
        sys = dae_index_lowering(connected)
        prob = ODEProblem(sys,[],tspan,jac=true)
    end

    return  prob, p, sys
end

export gimbal_conntroller


text:
############################################################################################
"""
$(TYPEDEF)

Abstract super type if update has to be applied for given function.

# Fields
$(TYPEDFIELDS)
"""
abstract type UpdateBool end
struct UpdateTrue <: UpdateBool end
struct UpdateFalse <: UpdateBool end

############################################################################################
"""
$(TYPEDEF)

Mutable struct to store update information that can be stored and updated in immutable structs. Contains boolean if updated is needed.
Note that a UpdateBool field cannot be stored and updated as a concrete type here.

# Fields
$(TYPEDFIELDS)
"""
mutable struct Updater
    "Boolean of whether update has to be applied"
    current::Bool
    function Updater(start::Bool)
        return new(start)
    end
end
function update!(updater::Updater)
    updater.current = updater.current ? false : true
    return nothing
end
function init!(updater::Updater, bool::Bool)
    updater.current = bool
    return nothing
end

############################################################################################
"""
$(TYPEDEF)

Mutable struct to hold a single scalar value that can be stored and updated in immutable structs.

# Fields
$(TYPEDFIELDS)
"""
mutable struct ValueHolder{T<:Real}
    "Scalar value"
    current::T
    function ValueHolder(val::T) where {T<:Real}
        return new{T}(val)
    end
end
function update!(value::ValueHolder{T}, val::T) where {T<:Real}
    value.current = val
    return nothing
end
function init!(value::ValueHolder{T}, val::T) where {T<:Real}
    value.current = val
    return nothing
end

############################################################################################
"""
$(TYPEDEF)

Mutable struct that can be stored and updated in immutable structs. Contains current iteration value.

# Fields
$(TYPEDFIELDS)
"""
mutable struct Iterator
    "Current iteration number."
    current::Int64
    function Iterator(start::Int64)
        return new(start)
    end
end
function update!(iterator::Iterator)
    iterator.current += 1
    return nothing
end
function init!(iterator::Iterator, val::I) where {I<:Integer}
    iterator.current = val
    return nothing
end

############################################################################################
"""
$(TYPEDEF)

Mutable iterator struct that can be stored and updated in immutable structs. Contains cumulative and current value of input.

# Fields
$(TYPEDFIELDS)
"""
mutable struct Accumulator
    "Cumulative value of all inputs."
    cumulative::Float64
    "Current input value."
    current::Float64
    function Accumulator()
        return new(0.0, 0.0)
    end
end
function update!(accumulator::Accumulator, val::T) where {T<:AbstractFloat}
    accumulator.current = val
    accumulator.cumulative += accumulator.current
    return nothing
end
function init!(accumulator::Accumulator)
    accumulator.current = 0.0
    accumulator.cumulative = 0.0
    return nothing
end

############################################################################################
# Export
export UpdateBool, UpdateTrue, UpdateFalse, Updater, Iterator, Accumulator, ValueHolder


text:
diffsym(s::Symbol) = Symbol(s, "##BAR##")
diffsym(v::Variable) = diffsym(name(v))

function differentiate(m::Model)
    differentiate!(∂Model(m))
end

function differentiate!(∂m::∂Model)
    @unpack mold = ∂m
    reset_varinitialized!(mold)
    reset_funclowered!(mold)

    # diffvars = Vector{DiffRuleVariable}(undef, length(mold.funcs))
    for (n,func) ∈ enumerate(mold.funcs) # is
        islowered(func) || add_func_diff!(∂m, func)
    end
    ReverseDiffExpressions.propagate_var_tracked!(∂m.m)
    ∂m.m
end


text:
const TPDistribution = SparseCat{Vector{Int64},Vector{Float64}}
is_inf_hor(tp::TPDistribution) = (tp.vals[1] == typemax(Int64))

"""
The CMSSP state space is a cartesian product of a discrete enumeration set and
a continuous space.

Attributes:
    - `mode::D` A discrete value for the mode of the system.
    - `continuous::C` A continuous value for the intra-modal state of the system.
"""
struct CMSSPState{D,C}
    mode::D
    continuous::C
end

"""
A CMSSP action is either a discrete mode-switching action or a continuous control action.

Attributes:
    - `action::Union{AD,AC}` Either a discrete mode switching action or a continuous control action.
"""
struct CMSSPAction{AD,AC}
    action::Union{AD,AC}
    action_idx::Int64
end

# TODO : Require discrete stuff as a Tabular MDP and map from d to integer and ad to integer (actionindex)

"""
Contains the information to represent and track the CMSSP problem.

Attributes:
    - `actions::Vector{CMSSPAction{AD,AC}}` The set of actions that the agent can take, both mode switching and control
    - `modes::Vector{D}` The set of discrete modes of the problem
    - `mode_actions::Vector{AD}` The set of mode-switching actions in the same relative order as the full action set
    - `modeswitch_mdp::TabularMDP` The tabular MDP that represents the mode-switching dynamics. The integer indices for
    the modes and actions follow the ordering in modes::Vector{D} and mode_actions::Vector{AD} respectively
    - `control_actions::Vector{AC}` The set of control actions in the same relative order as the full action set
"""
mutable struct CMSSP{D,C,AD,AC,P,CS} <: POMDPs.MDP{CMSSPState{D,C}, CMSSPAction{AD,AC}}
    actions::Vector{CMSSPAction{AD,AC}}
    modes::Vector{D}
    mode_actions::Vector{AD}
    modeswitch_mdp::TabularMDP
    control_actions::Vector{AC}
    goal_state::CMSSPState{D,C}
    params::P
    curr_context_set::CS
end

function CMSSP{D,C,AD,AC,P,CS}(actions::Vector{CMSSPAction{AD,AC}}, modes::Vector{D},
                          switch_mdp::TabularMDP, goal_state::CMSSPState{D,C}, params::P,
                          init_context_cs::CS) where {D,C,AD,AC,P,CS}
    return CMSSP{D,C,AD,AC,P,CS}(actions, modes,
                 get_modeswitch_actions(actions), switch_mdp,
                 get_control_actions(actions), goal_state, params, init_context_cs)
end

# POMDPs overrides
POMDPs.actions(cmssp::CMSSP) = cmssp.actions
POMDPs.n_actions(cmssp::CMSSP) = length(cmssp.actions)
POMDPs.discount(cmssp::CMSSP) = 1.0 # SSP - Undiscounted
POMDPs.actionindex(cmssp::CMSSP, a::CMSSPAction) = a.action_idx

modetype(::CMSSP{D,C,AD,AC}) where {D,C,AD,AC} = D
continuoustype(::CMSSP{D,C,AD,AC}) where {D,C,AD,AC} = C
modeactiontype(::CMSSP{D,C,AD,AC}) where {D,C,AD,AC} = AD
controlactiontype(::CMSSP{D,C,AD,AC}) where {D,C,AD,AC} = AC

"""
Returns a vector of the mode-switch CMSSP actions
"""
function get_modeswitch_actions(actions::Vector{CMSSPAction{AD,AC}}) where {AD,AC}
    mode_actions = Vector{AD}(undef,0)
    for a in actions
        if typeof(a.action) <: AD
            push!(mode_actions,a.action)
        end
    end
    return mode_actions
end

"""
Returns a vector of only the control actions
"""
function get_control_actions(actions::Vector{CMSSPAction{AD,AC}}) where {AD,AC}
    control_actions = Vector{AC}(undef,0)
    for a in actions
        if typeof(a.action) <: AC
            push!(control_actions,a.action)
        end
    end
    return control_actions
end

"""
Returns the integer index of the argument mode from the modes member of the cmssp.
This is typically required to refer into the mode-switching MDP
"""
function mode_index(cmssp::CMSSP, mode::D) where D
    idx = findfirst(isequal(mode), cmssp.modes)
    @assert idx != nothing "Mode not present in set of modes"
    return idx
end

"""
Return the integer index of the mode-switching action relaty
"""
function mode_actionindex(cmssp::CMSSP, mode_action::AD) where {AD}
    idx = findfirst(isequal(mode_action), cmssp.mode_actions)
    @assert idx != nothing @show mode_action, cmssp.mode_actions
    return idx
end

"""
Return type for bridge sample points.

Attributes:
    - `pre_bridge_state::C` The continuous state sample prior to the transition (represents pre-conditions)
    - `post_bridge_state::C` The continuous state sample after the transition (represents post-conditions)
    - `tp::TPDistribution` The distribution over time horizons for the bridge sample to be reached
"""
struct BridgeSample{C,M}
    pre_bridge_state::C
    post_bridge_state::C
    tp::TPDistribution
    metadata::M
end

"""
Vertex type for open-loop layer.

Attributes:
    -`state::CMSSPState{D,C}` The actual state encoded by the vertex (typically after a bridge transition)
    - `pre_bridge_state::CMSSPState{D,C}` The state before the bridge transition
    - `bridging_action::AD` The chosen mode-switching action by the bridge sampler
    - `tp::TPDistribution` The distribution over time horizons for the bridge sample to be reached
"""
mutable struct OpenLoopVertex{D,C,AD,M}
    state::CMSSPState{D,C}
    pre_bridge_state::CMSSPState{D,C}
    bridging_action::AD
    tp::TPDistribution
    metadata::M
end

function OpenLoopVertex(state::CMSSPState{D,C}, action::AD, metadata::M=zero(M)) where {D,C,AD,M}
    return OpenLoopVertex(state, state, action, TPDistribution([typemax(Int64)],[1.0], metadata))
end

function OpenLoopVertex{D,C,AD,M}(state::CMSSPState{D,C}, action::AD, metadata::M=zero(M)) where {D,C,AD,M}
    return OpenLoopVertex{D,C,AD,M}(state, state, action, TPDistribution([typemax(Int64)],[1.0]), metadata)
end

function OpenLoopVertex(state::CMSSPState{D,C}, action::AD, tp_dist::TPDistribution, metadata::M=zero(M)) where {D,C,AD,M}
    return OpenLoopVertex(state, state, action, tp_dist, metadata)
end

function OpenLoopVertex{D,C,AD,M}(state::CMSSPState{D,C}, action::AD, tp_dist::TPDistribution, metadata::M=zero(M)) where {D,C,AD,M}
    return OpenLoopVertex{D,C,AD,M}(state, state, action, tp_dist, metadata)
end

function OpenLoopVertex(pre_mode::D, post_mode::D, bridge_sample::BridgeSample{C,M}, action::AD) where {D,C,AD,M}
    state = CMSSPState(post_mode, bridge_sample.post_bridge_state)
    pre_bridge_state = CMSSPState(pre_mode, bridge_sample.pre_bridge_state)
    return OpenLoopVertex(state, pre_bridge_state, action, bridge_sample.tp, bridge_sample.metadata)
end

function OpenLoopVertex{D,C,AD,M}(pre_mode::D, post_mode::D, bridge_sample::BridgeSample{C,M}, action::AD) where {D,C,AD,M}
    state = CMSSPState(post_mode, bridge_sample.post_bridge_state)
    pre_bridge_state = CMSSPState(pre_mode, bridge_sample.pre_bridge_state)
    return OpenLoopVertex{D,C,AD,M}(state, pre_bridge_state, action, bridge_sample.tp, bridge_sample.metadata)
end


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

mutable struct VirtualNetworkGatewayConnectionPropertiesFormat <: SwaggerModel
    authorizationKey::Any # spec type: Union{ Nothing, String } # spec name: authorizationKey
    virtualNetworkGateway1::Any # spec type: Union{ Nothing, VirtualNetworkGateway } # spec name: virtualNetworkGateway1
    virtualNetworkGateway2::Any # spec type: Union{ Nothing, VirtualNetworkGateway } # spec name: virtualNetworkGateway2
    localNetworkGateway2::Any # spec type: Union{ Nothing, LocalNetworkGateway } # spec name: localNetworkGateway2
    connectionType::Any # spec type: Union{ Nothing, VirtualNetworkGatewayConnectionType } # spec name: connectionType
    connectionProtocol::Any # spec type: Union{ Nothing, ConnectionProtocol } # spec name: connectionProtocol
    routingWeight::Any # spec type: Union{ Nothing, Int32 } # spec name: routingWeight
    dpdTimeoutSeconds::Any # spec type: Union{ Nothing, Int32 } # spec name: dpdTimeoutSeconds
    sharedKey::Any # spec type: Union{ Nothing, String } # spec name: sharedKey
    connectionStatus::Any # spec type: Union{ Nothing, VirtualNetworkGatewayConnectionStatus } # spec name: connectionStatus
    tunnelConnectionStatus::Any # spec type: Union{ Nothing, Vector{TunnelConnectionHealth} } # spec name: tunnelConnectionStatus
    egressBytesTransferred::Any # spec type: Union{ Nothing, Int64 } # spec name: egressBytesTransferred
    ingressBytesTransferred::Any # spec type: Union{ Nothing, Int64 } # spec name: ingressBytesTransferred
    peer::Any # spec type: Union{ Nothing, SubResource } # spec name: peer
    enableBgp::Any # spec type: Union{ Nothing, Bool } # spec name: enableBgp
    useLocalAzureIpAddress::Any # spec type: Union{ Nothing, Bool } # spec name: useLocalAzureIpAddress
    usePolicyBasedTrafficSelectors::Any # spec type: Union{ Nothing, Bool } # spec name: usePolicyBasedTrafficSelectors
    ipsecPolicies::Any # spec type: Union{ Nothing, Vector{IpsecPolicy} } # spec name: ipsecPolicies
    trafficSelectorPolicies::Any # spec type: Union{ Nothing, Vector{TrafficSelectorPolicy} } # spec name: trafficSelectorPolicies
    resourceGuid::Any # spec type: Union{ Nothing, String } # spec name: resourceGuid
    provisioningState::Any # spec type: Union{ Nothing, ProvisioningState } # spec name: provisioningState
    expressRouteGatewayBypass::Any # spec type: Union{ Nothing, Bool } # spec name: expressRouteGatewayBypass

    function VirtualNetworkGatewayConnectionPropertiesFormat(;authorizationKey=nothing, virtualNetworkGateway1=nothing, virtualNetworkGateway2=nothing, localNetworkGateway2=nothing, connectionType=nothing, connectionProtocol=nothing, routingWeight=nothing, dpdTimeoutSeconds=nothing, sharedKey=nothing, connectionStatus=nothing, tunnelConnectionStatus=nothing, egressBytesTransferred=nothing, ingressBytesTransferred=nothing, peer=nothing, enableBgp=nothing, useLocalAzureIpAddress=nothing, usePolicyBasedTrafficSelectors=nothing, ipsecPolicies=nothing, trafficSelectorPolicies=nothing, resourceGuid=nothing, provisioningState=nothing, expressRouteGatewayBypass=nothing)
        o = new()
        validate_property(VirtualNetworkGatewayConnectionPropertiesFormat, Symbol("authorizationKey"), authorizationKey)
        setfield!(o, Symbol("authorizationKey"), authorizationKey)
        validate_property(VirtualNetworkGatewayConnectionPropertiesFormat, Symbol("virtualNetworkGateway1"), virtualNetworkGateway1)
        setfield!(o, Symbol("virtualNetworkGateway1"), virtualNetworkGateway1)
        validate_property(VirtualNetworkGatewayConnectionPropertiesFormat, Symbol("virtualNetworkGateway2"), virtualNetworkGateway2)
        setfield!(o, Symbol("virtualNetworkGateway2"), virtualNetworkGateway2)
        validate_property(VirtualNetworkGatewayConnectionPropertiesFormat, Symbol("localNetworkGateway2"), localNetworkGateway2)
        setfield!(o, Symbol("localNetworkGateway2"), localNetworkGateway2)
        validate_property(VirtualNetworkGatewayConnectionPropertiesFormat, Symbol("connectionType"), connectionType)
        setfield!(o, Symbol("connectionType"), connectionType)
        validate_property(VirtualNetworkGatewayConnectionPropertiesFormat, Symbol("connectionProtocol"), connectionProtocol)
        setfield!(o, Symbol("connectionProtocol"), connectionProtocol)
        validate_property(VirtualNetworkGatewayConnectionPropertiesFormat, Symbol("routingWeight"), routingWeight)
        setfield!(o, Symbol("routingWeight"), routingWeight)
        validate_property(VirtualNetworkGatewayConnectionPropertiesFormat, Symbol("dpdTimeoutSeconds"), dpdTimeoutSeconds)
        setfield!(o, Symbol("dpdTimeoutSeconds"), dpdTimeoutSeconds)
        validate_property(VirtualNetworkGatewayConnectionPropertiesFormat, Symbol("sharedKey"), sharedKey)
        setfield!(o, Symbol("sharedKey"), sharedKey)
        validate_property(VirtualNetworkGatewayConnectionPropertiesFormat, Symbol("connectionStatus"), connectionStatus)
        setfield!(o, Symbol("connectionStatus"), connectionStatus)
        validate_property(VirtualNetworkGatewayConnectionPropertiesFormat, Symbol("tunnelConnectionStatus"), tunnelConnectionStatus)
        setfield!(o, Symbol("tunnelConnectionStatus"), tunnelConnectionStatus)
        validate_property(VirtualNetworkGatewayConnectionPropertiesFormat, Symbol("egressBytesTransferred"), egressBytesTransferred)
        setfield!(o, Symbol("egressBytesTransferred"), egressBytesTransferred)
        validate_property(VirtualNetworkGatewayConnectionPropertiesFormat, Symbol("ingressBytesTransferred"), ingressBytesTransferred)
        setfield!(o, Symbol("ingressBytesTransferred"), ingressBytesTransferred)
        validate_property(VirtualNetworkGatewayConnectionPropertiesFormat, Symbol("peer"), peer)
        setfield!(o, Symbol("peer"), peer)
        validate_property(VirtualNetworkGatewayConnectionPropertiesFormat, Symbol("enableBgp"), enableBgp)
        setfield!(o, Symbol("enableBgp"), enableBgp)
        validate_property(VirtualNetworkGatewayConnectionPropertiesFormat, Symbol("useLocalAzureIpAddress"), useLocalAzureIpAddress)
        setfield!(o, Symbol("useLocalAzureIpAddress"), useLocalAzureIpAddress)
        validate_property(VirtualNetworkGatewayConnectionPropertiesFormat, Symbol("usePolicyBasedTrafficSelectors"), usePolicyBasedTrafficSelectors)
        setfield!(o, Symbol("usePolicyBasedTrafficSelectors"), usePolicyBasedTrafficSelectors)
        validate_property(VirtualNetworkGatewayConnectionPropertiesFormat, Symbol("ipsecPolicies"), ipsecPolicies)
        setfield!(o, Symbol("ipsecPolicies"), ipsecPolicies)
        validate_property(VirtualNetworkGatewayConnectionPropertiesFormat, Symbol("trafficSelectorPolicies"), trafficSelectorPolicies)
        setfield!(o, Symbol("trafficSelectorPolicies"), trafficSelectorPolicies)
        validate_property(VirtualNetworkGatewayConnectionPropertiesFormat, Symbol("resourceGuid"), resourceGuid)
        setfield!(o, Symbol("resourceGuid"), resourceGuid)
        validate_property(VirtualNetworkGatewayConnectionPropertiesFormat, Symbol("provisioningState"), provisioningState)
        setfield!(o, Symbol("provisioningState"), provisioningState)
        validate_property(VirtualNetworkGatewayConnectionPropertiesFormat, Symbol("expressRouteGatewayBypass"), expressRouteGatewayBypass)
        setfield!(o, Symbol("expressRouteGatewayBypass"), expressRouteGatewayBypass)
        o
    end
end # type VirtualNetworkGatewayConnectionPropertiesFormat

const _property_map_VirtualNetworkGatewayConnectionPropertiesFormat = Dict{Symbol,Symbol}(Symbol("authorizationKey")=>Symbol("authorizationKey"), Symbol("virtualNetworkGateway1")=>Symbol("virtualNetworkGateway1"), Symbol("virtualNetworkGateway2")=>Symbol("virtualNetworkGateway2"), Symbol("localNetworkGateway2")=>Symbol("localNetworkGateway2"), Symbol("connectionType")=>Symbol("connectionType"), Symbol("connectionProtocol")=>Symbol("connectionProtocol"), Symbol("routingWeight")=>Symbol("routingWeight"), Symbol("dpdTimeoutSeconds")=>Symbol("dpdTimeoutSeconds"), Symbol("sharedKey")=>Symbol("sharedKey"), Symbol("connectionStatus")=>Symbol("connectionStatus"), Symbol("tunnelConnectionStatus")=>Symbol("tunnelConnectionStatus"), Symbol("egressBytesTransferred")=>Symbol("egressBytesTransferred"), Symbol("ingressBytesTransferred")=>Symbol("ingressBytesTransferred"), Symbol("peer")=>Symbol("peer"), Symbol("enableBgp")=>Symbol("enableBgp"), Symbol("useLocalAzureIpAddress")=>Symbol("useLocalAzureIpAddress"), Symbol("usePolicyBasedTrafficSelectors")=>Symbol("usePolicyBasedTrafficSelectors"), Symbol("ipsecPolicies")=>Symbol("ipsecPolicies"), Symbol("trafficSelectorPolicies")=>Symbol("trafficSelectorPolicies"), Symbol("resourceGuid")=>Symbol("resourceGuid"), Symbol("provisioningState")=>Symbol("provisioningState"), Symbol("expressRouteGatewayBypass")=>Symbol("expressRouteGatewayBypass"))
const _property_types_VirtualNetworkGatewayConnectionPropertiesFormat = Dict{Symbol,String}(Symbol("authorizationKey")=>"String", Symbol("virtualNetworkGateway1")=>"VirtualNetworkGateway", Symbol("virtualNetworkGateway2")=>"VirtualNetworkGateway", Symbol("localNetworkGateway2")=>"LocalNetworkGateway", Symbol("connectionType")=>"VirtualNetworkGatewayConnectionType", Symbol("connectionProtocol")=>"ConnectionProtocol", Symbol("routingWeight")=>"Int32", Symbol("dpdTimeoutSeconds")=>"Int32", Symbol("sharedKey")=>"String", Symbol("connectionStatus")=>"VirtualNetworkGatewayConnectionStatus", Symbol("tunnelConnectionStatus")=>"Vector{TunnelConnectionHealth}", Symbol("egressBytesTransferred")=>"Int64", Symbol("ingressBytesTransferred")=>"Int64", Symbol("peer")=>"SubResource", Symbol("enableBgp")=>"Bool", Symbol("useLocalAzureIpAddress")=>"Bool", Symbol("usePolicyBasedTrafficSelectors")=>"Bool", Symbol("ipsecPolicies")=>"Vector{IpsecPolicy}", Symbol("trafficSelectorPolicies")=>"Vector{TrafficSelectorPolicy}", Symbol("resourceGuid")=>"String", Symbol("provisioningState")=>"ProvisioningState", Symbol("expressRouteGatewayBypass")=>"Bool")
Base.propertynames(::Type{ VirtualNetworkGatewayConnectionPropertiesFormat }) = collect(keys(_property_map_VirtualNetworkGatewayConnectionPropertiesFormat))
Swagger.property_type(::Type{ VirtualNetworkGatewayConnectionPropertiesFormat }, name::Symbol) = Union{Nothing,eval(Base.Meta.parse(_property_types_VirtualNetworkGatewayConnectionPropertiesFormat[name]))}
Swagger.field_name(::Type{ VirtualNetworkGatewayConnectionPropertiesFormat }, property_name::Symbol) =  _property_map_VirtualNetworkGatewayConnectionPropertiesFormat[property_name]

function check_required(o::VirtualNetworkGatewayConnectionPropertiesFormat)
    (getproperty(o, Symbol("virtualNetworkGateway1")) === nothing) && (return false)
    (getproperty(o, Symbol("connectionType")) === nothing) && (return false)
    true
end

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


text:
using RDatasets, DataFrames, Lasso, LaTeXStrings, Plots, Measures; pyplot()

df = dataset("MASS", "cpus")
df.Freq = map( x->10^9/x , df.CycT)
df = df[:, [:Perf, :Freq, :MMin, :MMax, :Cach, :ChMin, :ChMax]]
X = [df.Freq df.MMin df.MMax df.Cach df.ChMin df.ChMax]
Y = df.Perf

targetNumVars = 3

lambdaStep = 0.2
lamGrid = collect(0:lambdaStep:150)
lassoFit = fit(LassoPath,X, Y, λ = lamGrid);
dd = Array(lassoFit.coefs)'
nV = sum(dd .!= 0.0 ,dims=2)

goodLambda = lamGrid[findfirst((n)->n==targetNumVars,nV)]
newFit = fit(LassoPath,X, Y, λ = [goodLambda - lambdaStep, goodLambda])
println(newFit)
println("Coefficients: ", Array(newFit.coefs)'[2,:])

p1 = plot(lassoFit.λ, dd, label = ["Freq" "MMin" "MMax" "Cach" "ChMin" "ChMax"],
    ylabel = "Coefficient Value")
plot!([goodLambda,goodLambda],[-1,1.5],c=:black, lw=2, label = "Model cut-off")

p2 = plot(lassoFit.λ,nV, ylabel = "Number of Variables",legend = false)
plot!([goodLambda,goodLambda],[0,6],c=:black, lw=2, label = "Model cut-off")

plot(p1,p2,xlabel= L"\lambda", margin = 5mm, size = (800,400))

max_stars_repo_name: Yoshinobu-Ishizaki/StatsWithJuliaBook

max_issues_repo_name: Yoshinobu-Ishizaki/StatsWithJuliaBook

max_forks_repo_name: Yoshinobu-Ishizaki/StatsWithJuliaBook


text:
using Lazy

curry(f, x) = (xs...) -> f(x, xs...)

fibs = @lazy 0:1:(fibs + tail(fibs))

under = curry(takewhile, (x -> x <= 4e6))
even = curry(filter, iseven)
solve = (sum ∘ even ∘ under)

println(solve(fibs))

max_stars_repo_name: alexandru-dinu/project-euler

max_issues_repo_name: alexandru-dinu/project-euler

max_forks_repo_name: alexandru-dinu/project-euler


text:
using Plots
using Random
Random.seed!(1)

include_implicit_dynamics()
include_ddp()
include(joinpath(pwd(), "examples/implicit_dynamics/models/ball_in_cup/model.jl"))

include(joinpath(pwd(), "models/visualize.jl"))
include(joinpath(pwd(), "examples/implicit_dynamics/models/ball_in_cup/visuals.jl"))

vis = Visualizer()
mvis = MechanismVisualizer(kuka, kuka_visuals, vis[:base])
render(vis)
default_background!(vis)

# Implicit dynamics
h = 0.1
data = dynamics_data(model, h,
        r_func, rz_func, rθ_func, rz_array, rθ_array;
        idx_ineq = idx_ineq,
		z_subset_init = z_subset_init,
        dyn_opts =  InteriorPointOptions{Float64}(
						r_tol = 1.0e-8,
						κ_tol = 1.0e-4,
						κ_init = 0.1,
						diff_sol = true),
		jac_opts =  InteriorPointOptions{Float64}(
						r_tol = 1.0e-8,
						κ_tol = 1.0e-2,
						κ_init = 0.1,
						diff_sol = true))

model_implicit = ImplicitDynamics{Midpoint, FixedTime}(2 * model.dim.q, model.dim.u, 0, data)

n = model_implicit.n
nq = model.dim.q
m = model.dim.u

# problem setup
T = 21

q0 = zeros(nq)
q0[1] = 0
q0[3] = 0
q0[4] = -pi/2
q0[5] = 0.0
p_pos0 = Array(kinematics_ee(model, q0))
p_pos0[1] += 0.0
p_pos0[3] -= 0.5
q0[8:10] = p_pos0

q1 = copy(q0)

qN = zeros(nq)
qN[1] = 0
qN[3] = 0
qN[4] = -pi/2
qN[5] = 0.

ee_posN = Array(kinematics_ee(model, qN))
p_posN = ee_posN
p_posN[1] += 0.0
p_posN[3] += 0.1
qN[8:10] = p_posN

qD = zeros(nq)
qD[1] = 0
qD[3] = 0
qD[4] = -pi/2
qD[5] = 0.

ee_posD = Array(kinematics_ee(model, qD))
p_posD = ee_posD
# p_posD[1] += 0.5
qD[8:10] = p_posD

x1 = [q1; q1]
xT = [qN; qN]
ū = [0.0 * randn(m) for t = 1:T-1]#[t == 1 ? [0.0, 10.0, 0.0, 0.0, 0.0, -0.0, 0.0] : t == 2 ? [0.0, -10.0, 0.0, -0.0, 0.0, 0.0, 0.0] : [0.0, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0] for t = 1:T-1] #u_hist #[-1.0 * gravity_compensation(_model, h, q1, q1) for t = 1:T-1]
w = [zeros(model_implicit.d) for t = 1:T-1]

# Rollout
x̄ = rollout(model_implicit, x1, ū, w, h, T)
q̄ = state_to_configuration(x̄)

visualize!(mvis, model, q̄, Δt = h)

# Objective
V = 0.0 * Diagonal(ones(nq))
Q_velocity = 0.0 * 1.0e-3 * [V -V; -V V] ./ h^2.0
Q_track = 0.0 * 1.0 * Diagonal(ones(n))
Q_track_extend = copy(Q_track)
# Q_track_extend[8, 8] = 1.0
# Q_track_extend[nq + 8, nq + 8] = 1.0
x_track = [qD; qD]
x_track_extend = copy(x_track)
# x_track_extend[8] += 0.5
# x_track_extend[nq + 8] += 0.5
Q = [t < T ? Q_velocity + (t == 11 ? Q_track_extend : 1.0 * Q_track) : Q_velocity + 1.0 * Q_track for t = 1:T]
q = [t < T ? -2.0 * (t == 11 ? Q_track_extend * x_track_extend : 1.0 * Q_track * x_track) : -2.0 * 1.0 * Q_track * x_track for t = 1:T]
R = [Diagonal(1.0e-3 * ones(m)) for t = 1:T-1]
r = [zeros(m) for t = 1:T-1]

obj = StageCosts([QuadraticCost(Q[t], q[t],
	t < T ? R[t] : nothing, t < T ? r[t] : nothing) for t = 1:T], T)

function g(obj::StageCosts, x, u, t)
	T = obj.T
    if t < T
		Q = obj.cost[t].Q
		q = obj.cost[t].q
	    R = obj.cost[t].R
		r = obj.cost[t].r
        return x' * Q * x + q' * x + u' * R * u + r' * u
    elseif t == T
		Q = obj.cost[T].Q
		q = obj.cost[T].q
        return x' * Q * x + q' * x
    else
        return 0.0
    end
end

# Constraints
ul = -100.0 * ones(m)
uu = 100.0 * ones(m)
p = [t < T ? (t == 11 ? 2 * m + 0 * nq : 2 * m) : n for t = 1:T]
info_t = Dict(:ul => ul, :uu => uu, :inequality => (1:2 * m))
info_m = Dict(:qD => x_track_extend[1:nq], :ul => ul, :uu => uu, :inequality => (1:2 * m))
info_T = Dict(:xT => xT)
con_set = [StageConstraint(p[t], t < T ? (t == 11 ? info_m : info_t) : info_T) for t = 1:T]

function c!(c, cons::StageConstraints, x, u, t)
	T = cons.T
	p = cons.con[t].p

	if t < T
		ul = cons.con[t].info[:ul]
		uu = cons.con[t].info[:uu]
		# c[1:2 * m] .= [ul - u; u - uu]
        # if t == 11
        #     c[2 * m .+ (1:nq)] = x[1:nq] - cons.con[t].info[:qD]
        # end
	else
		c[1:n] .= x - cons.con[T].info[:xT]
	end
end

prob = problem_data(model_implicit, obj, con_set, copy(x̄), copy(ū), w, h, T,
	analytical_dynamics_derivatives = true)

ū = [t == 1 ? [0.0, 5.5, 0.0, 0.0, 0.0, -0.0, 0.0] : t == 2 ? [0.0, -5.5, 0.0, -0.0, 0.0, 0.0, 0.0] : [0.0, 0.0, 0.0, -0.0, 0.0, 0.0, 0.0] for t = 1:T-1] #u_hist #[-1.0 * gravity_compensation(model, h, q1, q1) for t = 1:T-1]
w = [zeros(model_implicit.d) for t = 1:T-1]

# Rollout
x̄ = rollout(model_implicit, x1, ū, w, h, T)
q̄ = state_to_configuration(x̄)
visualize!(mvis, model, q̄, Δt = h)

# Solve
@time constrained_ddp_solve!(prob,
	max_iter = 1000, max_al_iter = 3,
	ρ_init = 1.0, ρ_scale = 10.0,
	con_tol = 0.1)

x, u = current_trajectory(prob)
x̄, ū = nominal_trajectory(prob)

q̄ = state_to_configuration(x̄)
visualize!(mvis, model, q̄, Δt = h)

max_issues_repo_name: thowell/DirectMotionPlanning


text:
using Distributions

mutable struct Lineage
  fitness::Float64
  size::Int64
  state::Vector{Float64}
  mod::Bool
end

function average_fitness(population::Dict{Array{Float64,1}, Lineage})
#calculate average fitness of the population. also outputs population size
  popN = Int64
  popN = 0
  popW = Float64
  popW = 0
  for line in values(population)
    popN+=line.size
    popW+=line.size * line.fitness
  end
  return popW/popN
end

function pop_size(population::Dict{Array{Float64,1}, Lineage})
  popN = Int64
  popN = 0
  for line in values(population)
    popN+=line.size
  end
  return popN
end

function assay_var_freq(population::Dict{Array{Float64,1}, Lineage})
#calculates  frequency of the sign-variable lineage
  popN = Int64
  popN = 0
  modN = Int64
  modN = 0
  for line in values(population)
    popN+=line.size
    if line.mod modN+=line.size end
  end
  modF = Float64
  modF = modN/popN
  return modF
end

function assay_var_count(population::Dict{Array{Float64,1}, Lineage})
#calculates the number of sign-variable carriers
  popN = Int64
  popN = 0
  modN = Int64
  modN = 0
  for line in values(population)
    popN+=line.size
    if line.mod modN+=line.size end
  end
  modF = Float64
  modF = modN/popN
  return modN
end

function model1(population::Dict{Array{Float64,1}, Lineage}, wcost::Float64, wben::Float64)
#between-lineage sign variability
  costly_start=false
  ben_start=false
  new_population = Dict{Array{Float64,1}, Lineage}()
  #initialize new population dictionary
  for (key, line) in population
    if line.mod
        pheno_dis = rand(Multinomial(line.size, [0.01, 0.99, 0.0]))
        #generates #s of beneficial, deleterious, and neutral carriers. When N=1 picks one of the three.
        if pheno_dis[3] > 0
            if key in keys(new_population)
                new_population[key].size+=pheno_dis[3]
            else new_population[key] = Lineage(line.fitness, pheno_dis[3], key, line.mod)
            end
        end

        if pheno_dis[1] > 0
            ben_start=true
            newkey = copy(line.state)
            newkey[2] =wben
            if newkey in keys(new_population) new_population[newkey].size+=pheno_dis[1]
            else new_population[newkey] = Lineage(wben, pheno_dis[1], newkey, line.mod)
            end
        end

        if pheno_dis[2] > 0
            costly_start=true
            newkey = copy(line.state)
            newkey[2] =wcost
            if newkey in keys(new_population) new_population[newkey].size+=pheno_dis[2]
            else new_population[newkey] = Lineage(wcost, pheno_dis[2], newkey, line.mod)
            end
        end

    else
        if key in keys(new_population) new_population[key].size+=line.size
        else new_population[key] = Lineage(line.fitness, line.size, line.state, line.mod)
        end
    end
  end
return new_population, costly_start, ben_start
end

function model2(generations::Int64, wcost::Float64, wben::Float64, t::Int64)
#within-lineage time-dependent sign variability model
  if generations < t
      w_inv=wcost
  else
      w_inv=wben
  end
  w_res=exp(1.0)
  return w_inv, w_res
end

function model3(freq::Float64, wcost::Float64, wben::Float64, f::Float64)
#within-lineage frequency-dependent sign variability model
  if freq < f
      w_inv=wcost
  else
      w_inv=wben
  end
  w_res=exp(1.0)
  return w_inv, w_res
end

function re_fit(population::Dict{Array{Float64,1}, Lineage}, w_inv::Float64, w_res::Float64)
    new_population = Dict{Array{Float64,1}, Lineage}()
    for (key, line) in population
        if line.mod
            newkey = copy(key)
            newkey[2] = w_inv
            new_population[newkey] = Lineage(w_inv, line.size, line.state, line.mod)
        else
            newkey = copy(key)
            newkey[2] = w_res
            new_population[newkey] = Lineage(w_res, line.size, line.state, line.mod)
        end
    end
    return new_population
end

function wright_fisher_reproduction(population::Dict{Array{Float64,1}, Lineage}, N::Int64, popw::Float64)
  proby_list = Float64[] #array of probabilities
  lineage_list = Lineage[] #initialize lineage list
  for (key,line) in population
    push!(proby_list, line.size/N * line.fitness/popw)
    #representation of a lineage in the next generations depends on its frequency and relative fitness
    push!(lineage_list, line)
  end
  new_counts_list = rand(Multinomial(N, proby_list))
  new_population = Dict{Array{Float64,1}, Lineage}()
  for i = 1:length(new_counts_list)
    if new_counts_list[i] > 0
      new_population[lineage_list[i].state] = Lineage(lineage_list[i].fitness, new_counts_list[i], lineage_list[i].state, lineage_list[i].mod)
    end
  end
  return new_population
end

function simulate(pop_N::Int64, model::Int64, wcost::Float64, wben::Float64, t::Int64, f::Float64)
  population = Dict{Array{Float64,1}, Lineage}()
  wt_state = [0.0, exp(1.0)]
  population[wt_state] = Lineage(exp(1.0), pop_N-1, wt_state, false)
  mod_state = [1.0, exp(1.0)]
  population[mod_state] = Lineage(exp(1.0), 1, mod_state, true)
  if model == 1 population, costly_start, ben_start = model1(population, wcost, wben) end
  modf = assay_var_freq(population)
  popw = average_fitness(population)
  generations = Int64
  generations = 0
  fgen = Int64
  fgen = 0
  # survivors = Int64
  # survivors = 0
  rfcrit=false
  fcr=0.0
  #initialize generations counter

  while 0.0<modf<1.0
    if model == 2
        w_inv, w_res = model2(generations, wcost, wben, t)
        population=re_fit(population,w_inv, w_res)
    elseif model == 3
        w_inv, w_res = model3(modf, wcost, wben, f)
        population=re_fit(population,w_inv, w_res)
    end
    popw = average_fitness(population)
    population = wright_fisher_reproduction(population, pop_N, popw)
    modf = assay_var_freq(population)
    popw = average_fitness(population)
    #if generations == 50 survivors =assay_var_count(population) end
    if modf < f
    else
        if rfcrit
        else
            rfcrit=true
            fgen=generations
            fcr=modf
        end
    end
    generations+=1
  end
  return modf ==1.0, generations, rfcrit, fgen, fcr, popw
end

job_id = ARGS[1]
for N in [10, 13, 17, 23, 32, 42, 57, 76, 102, 137, 183, 245, 327, 438, 586, 784, 1049, 1404, 1877, 2511]
  time = Float64[]
  # survs = Int64[]
  ftime= Int64[]
  f_cr=Float64[]
  println(N)
  #time_to_mut list records all times of successful mutator hitchhiking
  for run = 1:10000000
    output = simulate(N, 3, exp(0.99),exp(1.1), 50, 0.15)
    #simulation of model 3, records whether threshold frequency has been reached and the realized x* for every replicate
    if output[1]
        push!(time, output[2])
    else
        push!(time, 0)
    end

    if output[3]
        push!(ftime, output[4])
        push!(f_cr, output[5])

    else
        push!(ftime, 0)
        push!(f_cr, 0.0)

    end
    # push!(survs, output[4])
  end

outfile = open(string(job_id,"time.csv"), "a")
write(outfile, join(time, ","), "\n")
close(outfile)
outfile = open(string(job_id,"ftime.csv"), "a")
write(outfile, join(ftime, ","), "\n")
close(outfile)
outfile = open(string(job_id,"fcrt.csv"), "a")
write(outfile, join(f_cr, ","), "\n")
close(outfile)

end

max_stars_repo_name: yraynes/Sign-Variability

max_issues_repo_name: yraynes/Sign-Variability

max_forks_repo_name: yraynes/Sign-Variability


text:
@testset "inverse.jl" begin
    @testset "square" begin
        x = randn()
        @test GPLikelihoods.square(x) == x^2
    end

    @testset "inverse" begin
        x = rand()
        for f in (
            exp,
            log,
            inv,
            log1pexp,
            logexpm1,
            sqrt,
            GPLikelihoods.square,
            logit,
            logistic,
            normcdf,
            norminvcdf,
        )
            g = GPLikelihoods.inverse(f)

            # check that definitions are correct
            @test g(f(x)) ≈ x
            @test f(g(x)) ≈ x

            # check that definitions are complete
            @test GPLikelihoods.inverse(g) === f
        end
    end
end


text:
function load_friends()
  buf = open("county2county.binary") do io
    read(io)
  end
  len = length(buf)
  n = Int.(sqrt(len*2))
  @assert n == 3142
  X = zeros(n,n)
  # each entry is encoded in 4 bits
  for i=1:n
    for j=1:2:n # each
      byteno = (((i-1)*n) + (j-1)) ÷ 2 + 1
      byte = buf[byteno]
      X[j,i] = (byte & 0b11110000) >> 4
      X[j+1,i] = (byte & 0b1111)
    end
  end
  return X
end
X = load_friends()

## From the Chrome Debugger

Y = [0 8
1 7
2 8
3 8
4 7
5 8
6 8
7 7
8 8
9 7
10 8
11 7
12 8
13 7
14 7
15 8
16 7
17 8
18 8
19 8
20 8
21 7
22 7
23 8
24 7
25 8
26 7
27 7
28 7
29 7
30 7
31 7
32 7
33 7
34 8
35 6
36 8
37 7
38 7
39 7
40 8
41 7
42 8
43 8
44 7
45 8
46 7
47 7
48 7
49 8
50 8
51 7
52 8
53 7
54 8
55 7
56 7
57 7
58 8
59 8
60 8
61 8
62 8
63 7
64 7
65 8
66 7
67 1
68 1
69 4
70 1
71 2
72 4
73 1
74 4
75 1
76 1
77 1
78 3
79 2
80 2
81 1
82 1
83 2
84 1
85 3
86 1
87 4
88 1
89 2
90 2
91 2
92 1
93 1
94 4
95 1
96 1
97 2
98 1
99 1]
for i=1:size(Y,1)
  @assert(Y[i,2].== X[i,1])
end
## The smallest value is X
minimum(X)

## the smallest entry
A = X .- 1

## There are 1818 non-symmetric entries
nonsym = findall(abs.(A-A') .> 0)
display(nonsym)
## Let's look at them
for i=1:50
  println("A[$(nonsym[i])] = $(A[nonsym[i]]) ≠ $(A[nonsym[i][2],nonsym[i][1]])")
end
## Is the difference always at most one?

diffs = [ abs(A[nonsym[i]] - A[nonsym[i][2],nonsym[i][1]]) for i in eachindex(nonsym)]

## Ugh, there are 8 entries where the differnce is larger than 1
count(diffs .> 1)

##
nonsym[findall(diffs .> 1)]

##
diffs[findall(diffs .> 1)]
## We are going to symmetrize this and have a list of corrections.
B = min.(A,A') # this is the strenght of both...
C = A - B # this is the correction

## From https://github.com/dgleich/DGFun.jl/blob/master/src/io.jl
using SparseArrays
function writeSMAT(filename::AbstractString, A::SparseMatrixCSC{T,Int}; values::Bool=true) where T
    open(filename, "w") do outfile
        write(outfile, join((size(A,1), size(A,2), nnz(A)), " "), "\n")

        rows = rowvals(A)
        vals = nonzeros(A)
        m, n = size(A)
        for j = 1:n
           for nzi in nzrange(A, j)
              row = rows[nzi]
              val = vals[nzi]
              if values
                write(outfile, join((row-1, j-1, val), " "), "\n")
              else
                write(outfile, join((row-1, j-1, 1), " "), "\n")
              end
           end
        end
    end
end
writeSMAT("facebook-county-friendship.smat", Int.(sparse(B)))
writeSMAT("facebook-county-friendship-nonsym.smat", Int.(sparse(C)))

##
## Let's also parse the top-10 network
using DelimitedFiles
topdata = Int.(readdlm("top.csv",',',skipstart=1))
##
T = sparse(topdata[:,1] .+ 1,topdata[:,2] .+ 1,1, 3142,3142 )
##
writeSMAT("facebook-county-friendship-top10.smat", T)

max_stars_repo_name: empet/more-graphs

max_issues_repo_name: empet/more-graphs

max_forks_repo_name: empet/more-graphs


text:
#RSDeltaSigma: designHBF() algorithm
#-------------------------------------------------------------------------------
#=IMPORTANT
designHBF6() (a version that doesn't need firpm()) no longer exists in the
source code used in this port. It actually just copies designHBF7().
=#

#==Helper functions
===============================================================================#
"""`F1 = evalF1(f1, z, phi)`

Calculate the values of the F1 filter (tranformed prototype filter) of a
Saramaki HBF at the given points.
"""
function evalF1(f1, z)
	F1 = 0.5
	for i = 1:length(f1)
		F1 = F1 .+ f1[i] * z .^ (2*i-1)
	end
	return F1
end
evalF1(f1, z, phi::Float64) = evalF1(f1, z/phi)

"""`F0 = evalF0(f1, z, phi)`

Calculate the values of the F0 (prototype) filter of a Saramaki HBF
at the given points.
"""
evalF0(f1, z, phi::Float64) =
	evalF1( f1, 0.5*(z + 1 ./ z), phi )

#==frespHBF()
===============================================================================#
function _frespHBF(f, f1, f2, fp, calcripples::Bool)
	Npts = length(f)
	w = 2*pi*f
	z = exp.(j*w)
	cos_w = real.(z)

	n2 = length(f2)
	F2 = zeros(size(w))
	for i = 1:n2
		F2 = F2 + f2[i]*cos.(w*(2*i-1))
	end
	F2 = F2*2
	mag = evalF1(f1, F2)

	pbr = sbr = nothing
	if calcripples
		passband = 1:floor(Int, 2*fp*(Npts-1)+1)
		stopband = (Npts + 1) .- passband
		pbr = maximum( abs.(abs.(mag[passband]) .- 1) )
		sbr = maximum( abs.(mag[stopband]) )
	end
	return (z, mag, F2, pbr, sbr)
end

"""`(mag, pbr, sbr) = frespHBF(f,f1,f2,phi=1.0,fp=0.2,msg="")`

Compute the frequency response, the passband ripple and the stopband ripple
of a Saramaki HBF. If msg is non-null, a plot is made.
 - `fp` is the passband edge.
 - `phi` is used by designHBF.
"""
function frespHBF(f,f1,f2; phi=1.0, fp=0.2, msg="", calcripples=true)
	if isa(f, Number) && isnan(f)
		f = range(0, stop=.5, length=1024)
	end
	if isa(f1, Vector{CoeffCSD})
		f1 = values(f1)
	end
	if isa(f2, Vector{CoeffCSD})
		f2 = values(f2)
	end
	calcripples = calcripples || isempty(msg)
	(z, mag, F2, pbr, sbr) = _frespHBF(f, f1, f2, fp, calcripples)

	if !isempty(msg)
		F1 = evalF0(f1, z, phi)
		plot = cons(:plot, linlin, nstrips=2, title="Frequency Response", legend=true,
			xaxis=set(label="Normalized Frequency", min=0, max=0.5),
			ystrip1=set(axislabel="Mag", min=0, max=1.1),
			ystrip2=set(axislabel="Mag [dB]", min=-150, max=10),
		)
		plot.title = msg
		w1 = waveform(f, abs.(F1))
		w2 = waveform(f, phi*abs.(F2))
		w3 = waveform(f, abs.(mag))
		w4 = waveform(f, dbv.(mag))
		pbr_str = @sprintf("pbr = %.1e", pbr) #orig. coords: (0.0,-10)
		sbr_str = @sprintf("sbr = %.0fdB", dbv(sbr)) #orig. coords: (0.0,dbv(sbr))

		push!(plot,
			cons(:wfrm, w1, line=set(style=:dash, color=:blue, width=2), label="F1"),
			cons(:wfrm, w2, line=set(style=:dot, color=:red, width=2), label="F2"),
			cons(:wfrm, w3, line=set(style=:solid, color=:green, width=2), label="HBF"),
			cons(:wfrm, w4, line=set(style=:solid, color=:green, width=2), label="HBF", strip=2),
			cons(:atext, pbr_str, y=-10, reloffset=set(x=0.05), align=:tl, strip=2),
			cons(:atext, sbr_str, y=dbv(sbr), reloffset=set(x=.95), align=:br, strip=2),
		)
		displaygui(plot)
	end

	return (mag,pbr,sbr)
end

#==designF1()
===============================================================================#
"""`(f1, zetap, phi) = designF1(delta, fp1)`

Design the F1 sub-filter of a Saramaki halfband filter.
This function is called by designHBF()

 - `f1`: A structure array containing the F1 filter coefficents and
   Their CSD representation.
 - `phi`: The scaling factor for the F2 filter (imbedded in the f1 coeffs.)
"""
function designF1(delta, fp1)
	passband = exp.(4π*j*range(0, stop=fp1, length=10))
	ok = false
	n1_found = 7
	local h
	for n1 in 1:2:7 #Odd values only
		if n1 == 1
			h = [0.5, 0.5]
		else
			h = remez(2*n1, [(0,4*fp1)=>1, (1,1)=>0], Hz=2)
			if !(abs(sum(h)-1) < 1e-3) #remez bug! Use firls instead
				throw("Remez bug! workaround not yet implemented")
				#h = firls(2*n1-1,[0 4*fp1 1-1e-6 1],[1 1 0 0]) #workaround
			end
		end
		fresp = abs.( polyval(h,passband) )
		if maximum( abs.(fresp .- 1) ) <= delta
			n1_found = n1
			ok = true
			break
		end
	end
	n1 = n1_found
	if !ok
		zetap = 1 #Use this as an indication that the function failed.
		return ([], zetap, 0)
	end

	#Transform h(n) to a chebyshev polynomial f1(n)
	#Sum(f1(i)*cos(w)^n)|i=1:n1 + Sum(h(n1+i))*cos(n*w))|i=1:n1, n = 2*i-1;
	w = π*rand(1,n1)
	cos_w = cos.(w)
	A = zeros(n1,length(w))
	B = zeros(1,n1)
	for i in 1:n1
		n = 2*i-1
		A[i,:] = cos_w .^ n
		B = B .+ h[n1+i] * cos.(n*w)
	end
	f1 = B/A
	f1 = f1[:] #Vectorize

	phivecb = []

	#Optimize the quantized version of f1 to maximize the stopband width
	#( = acos(zetap) )
	zetap = 1
	phi = 0
	f1_saved = nothing
	testPoints = vcat(0, 10 .^ range(-2, stop=0, length=128)) .- 1
	for nsd in 3:8
		f1a = f1; f1b = f1 #First try the unperturbed filter.
		for phia in 1 ./ vcat(1, f1)
			phia = phia / 2^nextpow(2, abs(phia)) #keep phi in (0.5,1]
			#Try a bunch of coefficients in the current neighborhood,
			#shrinking the neighborhood once 10 successive trial values show no
			#improvement.  If 2 successive shrinkages do no good, try a higher nsd.
			count = 0
			nohelp = 0
			neighborhood = .05
			while neighborhood > 1e-5
				phivec = phia .^ collect(1:2:2*n1-1)
				#Matlab Ver. 5 change:
				if isempty(phivecb); phivecb = phivec; end
				f1q = bquantize( f1a .* phivec, nsd )
				f1qval = values(f1q)
				F1 = evalF1( f1qval, testPoints, phia )
				fi = findall( abs.(F1) .> delta )
				zeta = -testPoints[ max(fi[1]-1, 1) ]
				#fprintf(2,'nsd=%d, nbhd= %f, count=%d, zeta = %f, phia=%f\n', ...
				#  nsd, neighborhood, count, zeta, phia )
				if zeta < zetap
					count = 0
					nohelp = 0
					zetap = zeta
					f1b = f1qval
					f1_saved = f1q
					phi = phia
					phivecb = phivec
				else
					count = count + 1
				end
				if count > 10
					count = 0
					neighborhood = neighborhood/2
					nohelp = nohelp + 1
					if nohelp > 2
						break
					end
				end
				f1a = f1b ./ phivecb + neighborhood*(rand(size(f1b)...) .- 0.5)
				phia = phia .+ neighborhood*(rand()-0.5)
			end #while neighborhood ...
			if zetap < 1 #Found a filter with adequate attn.
				break
			end
		end #for phia ...
		if zetap < 1 #Found a filter with adequate attn.
			break
		end
	end #for nsd ...

	return (f1_saved, zetap, phi)
end

#==designF2()
===============================================================================#
"""`f2 = designF2(fp,zetap,phi)`

Design the F2 sub-filter of a Saramaki halfband filter.
This function is called by designHBF().

# subfilter design:
   `1 - delta2' < |F2/phi| < 1`  for `f` in `[0 fp]`
   `-1 < |F2/phi| < -1 + delta2` for `f` in `[0.5-fp, 0.5]`
   `1-delta2 = (1-delta2)/(1+delta2)`
"""
function designF2(fp, zetap, phi)
	delta2 = (1-zetap)/(1+zetap)
	#delta2p = 1 - (1-delta2)/(1+delta2)

	#determine the minimum order required by the filter
	passband = exp.(j*range(0, stop=4π*fp, length=10))
	nsub_found = 17
	for nsub in 3:2:17
		h2 = remez(nsub+1, [(0,4*fp)=>1, (1,1)=>0], Hz=2)
		mag = abs.( polyval(h2, passband) )
		if maximum(abs.(mag .- 1)) < delta2
			nsub_found = nsub
			break
		end
	end
	n2min = (nsub_found+1)/2

	#Search all n2,nsd pairs, in order of the product n2*(nsd+1)
	#allowing fp to be a variable?
	success = false
	nsdmin = 3; nsdmax = 6
	for product in (nsdmin+1)*n2min:(nsdmax+1)*n2min
		for nsd in nsdmin:nsdmax
			n2 = product/(nsd+1)
			if floor(n2) != n2 #Only take integer n2,nsd pairs
				break
			end
			n2 = floor(Int, n2)
			nsub = 2*n2-1
			#Could try a bunch of fp values
			#fprintf(2,'designF2: Trying (n2,nsd2,fp)=(%2d,%2d,%6.4f)\n',n2,nsd,fp);
			h2 = remez(nsub+1, [(0,4*fp)=>1, (1,1)=>0], Hz=2)
			h2 =  h2/(phi*(1+delta2)) #Adjust the coefficients.
			f2 = bquantize( h2[n2+1:nsub+1], nsd )
			f2val = values(f2)
			h2 = (1+delta2)*phi*vcat(f2val[n2:-1:1], f2val)
			mag = abs.( polyval(h2, passband) )
			if maximum(abs.(mag .- 1)) < delta2
				success = true
				break
			end
		end
		if success
			break
		end
	end

	if !success
		f2 = []
		q2 = []
	end
	return f2
end

#==designHBF()
===============================================================================#
"""`(f1,f2,info)=designHBF(fp(=0.2),delta=1e-5,debug=false)`

Design a half-band filter which can be realized without general multipliers.
The filter is a composition of a prototype and sub- filter.

# Input
 - `fp`: The normalized cutoff frequency of the filter. Due to the
   symmetry imposed by a HBF, the stopband begins at `0.5-fp`.
 - `delta`: The absolute value of the deviation of the frequency response from
   the ideal values of 1 in the passband and 0 in the stopband.

# Output
 - `f1`,`f2`: The coefficients of the prototype and sub-filters
   and their canonical-signed digit (csd) representation.
 - `info`: A vector containing the following data (only set when debug=1):
 - -->`complexity`: The number of additions per output sample.
 - -->`n1,n2`: The length of the f1 and f2 vectors.
 - -->`sbr`: The achieved stob-band attenuation (dB).
 - -->`phi`: The scaling factor for the F2 filter.
"""
function designHBF(fp::Float64=0.2; delta::Float64=1e-5, debug::Bool=false)
	f1_saved = f2_saved = nothing #Declare variables
	phi_saved = 0.0
#=To Do:
	Clean up the code a bit more, esp. wrt the use of the struct. arrays.
	Use the phi variable to cut down on the number of adders in F2.
	Apply a simulated annealing/genetic optimization alg instead
	of the ad hoc one I have now.
=#

	#Try several different values for the fp1 parameter.
	#The best values are usually around .04
	#Surrender if 3 successive attempts yield progressively greater complexity.
	lowest_complexity = Inf; prev_complexity = Inf;
	local worse
	for fp1 in [.03, .035, .025, .040, .020, .045, .015, .05]
		failed = false
		(f1, zetap, phi) = designF1(delta, fp1)
		if zetap == 1 #designF1 failed
			failed = true
			if debug
				@warn("designF1 failed at fp1=$fp1\n")
			end
		end
		if !failed
			f2 = designF2(fp, zetap, phi)
			n1 = length(f1);	n2 = length(f2)
			if n2 == 0 #designF2 failed
				failed = true
				if debug
					@warn("designF2 failed when zetap=$zetap, phi=$phi\n")
				end
			end
		end
		if !failed
			#complexity(+ performance) = the number of two-input adders (+ sbr)
			complexity = sum(csdsize2(f1)) + (2*n1-1)*(n2+sum(csdsize2(f2))-1) #VERIFYME
			msg = ""

			if debug
				msg = @sprintf("%d adders: n1=%d, n2=%d, (fp1=%.2f, zetap=%.3f, phi=%4.2f)",
					complexity, n1, n2, fp1, zetap, phi
				)
			end
			(fresp, pbr, sbr) = frespHBF(NaN, f1, f2, phi=phi, fp=fp, msg=msg)
			if pbr <= delta && sbr <= delta
				complexity = complexity + sbr
				if complexity < prev_complexity
					worse = 0
					if complexity < lowest_complexity
						lowest_complexity = complexity
						f1_saved = f1;	f2_saved = f2
						phi_saved = phi
						if debug; println(msg); end
					end
				else
					worse = worse + 1
					if worse > 2; break; end
				end
				prev_complexity = complexity
			end #if pbr <= delta
		end #!failed
	end #for fp1

	info=nothing
	if isinf(lowest_complexity)
		@warn("designHBF: Unable to meet the design requirements.")
	else
		complexity = floor(lowest_complexity)
		msg = "Final Design: $complexity adders"
		(junk, pbr, sbr) = frespHBF(NaN, f1_saved, f2_saved, phi=phi_saved, fp=fp, msg=msg)
		n1 = length(f1_saved); n2 = length(f2_saved)
		if debug
			msg *= @sprintf(" (%d,%d,%.0fdB)", n1, n2, dbv(sbr))
			println(msg)
		end
		info = (complexity=complexity, n1=n1, n2=n2, sbr=dbv(sbr), phi=phi_saved)
	end

	return (f1_saved, f2_saved, info)
end

#Last line


text:
using LinearAlgebra

import ClimaCore:
    Domains, Fields, Geometry, Meshes, Operators, Spaces, Topologies

using OrdinaryDiffEq: ODEProblem, solve, SSPRK33

import Logging
import TerminalLoggers
Logging.global_logger(TerminalLoggers.TerminalLogger())

"""
    convergence_rate(err, Δh)

Estimate convergence rate given vectors `err` and `Δh`

    err = C Δh^p+ H.O.T
    err_k ≈ C Δh_k^p
    err_k/err_m ≈ Δh_k^p/Δh_m^p
    log(err_k/err_m) ≈ log((Δh_k/Δh_m)^p)
    log(err_k/err_m) ≈ p*log(Δh_k/Δh_m)
    log(err_k/err_m)/log(Δh_k/Δh_m) ≈ p

"""
convergence_rate(err, Δh) =
    [log(err[i] / err[i - 1]) / log(Δh[i] / Δh[i - 1]) for i in 2:length(Δh)]

# Advection problem on a sphere. The initial condition can be set via a command line
# argument. Possible test cases are: cosine_bell (default) and gaussian_bell

const R = 6.37122e6
const h0 = 1000.0
const r0 = R / 3
const u0 = 2 * pi * R / (86400 * 12)
const center = Geometry.LatLongPoint(0.0, 270.0)
const test_name = get(ARGS, 1, "cosine_bell") # default test case to run
const test_angle_name = get(ARGS, 2, "alpha0") # default test case to run
const cosine_test_name = "cosine_bell"
const gaussian_test_name = "gaussian_bell"
const alpha0_test_name = "alpha0"
const alpha45_test_name = "alpha45"

if test_angle_name == alpha45_test_name
    const α0 = 45.0
else # default test case, α0 = 0.0
    const α0 = 0.0
end

# Plot variables and auxiliary function
ENV["GKSwstype"] = "nul"
import ClimaCorePlots, Plots
Plots.GRBackend()
dirname = "cg_sphere_solidbody_$(test_name)"
dirname = "$(dirname)_$(test_angle_name)"
path = joinpath(@__DIR__, "output", dirname)
mkpath(path)

function linkfig(figpath, alt = "")
    # buildkite-agent upload figpath
    # link figure in logs if we are running on CI
    if get(ENV, "BUILDKITE", "") == "true"
        artifact_url = "artifact://$figpath"
        print("\033]1338;url='$(artifact_url)';alt='$(alt)'\a\n")
    end
end

FT = Float64
ne_seq = 2 .^ (2, 3, 4, 5)
Δh = zeros(FT, length(ne_seq))
L1err, L2err, Linferr = zeros(FT, length(ne_seq)),
zeros(FT, length(ne_seq)),
zeros(FT, length(ne_seq))
Nq = 4

# h-refinement study
for (k, ne) in enumerate(ne_seq)
    domain = Domains.SphereDomain(R)
    mesh = Meshes.EquiangularCubedSphere(domain, ne)
    grid_topology = Topologies.Topology2D(mesh)
    quad = Spaces.Quadratures.GLL{Nq}()
    space = Spaces.SpectralElementSpace2D(grid_topology, quad)

    coords = Fields.coordinate_field(space)

    Δh[k] = 2 * R / ne

    global_geom = space.global_geometry

    h_init = map(coords) do coord
        rd = Geometry.great_circle_distance(coord, center, global_geom)

        if test_name == gaussian_test_name
            h0 * exp(-(rd / r0)^2 / 2)
        else # default test case, cosine bell
            if rd < r0
                h0 / 2 * (1 + cospi(rd / r0))
            else
                0.0
            end
        end
    end

    u = map(coords) do coord
        ϕ = coord.lat
        λ = coord.long

        uu = u0 * (cosd(α0) * cosd(ϕ) + sind(α0) * cosd(λ) * sind(ϕ))
        uv = -u0 * sind(α0) * sind(λ)
        Geometry.UVVector(uu, uv)
    end

    function rhs!(dh, h, u, t)
        div = Operators.Divergence()

        @. dh = -div(h * u) # strong form of equation
        Spaces.weighted_dss!(dh)
    end

    # Set the RHS function
    rhs!(similar(h_init), h_init, u, 0.0)

    # Solve the ODE
    T = 86400 * 12
    dt = 20 * 60
    prob = ODEProblem(rhs!, h_init, (0.0, T), u)
    sol = solve(
        prob,
        SSPRK33(),
        dt = dt,
        saveat = dt,
        progress = true,
        adaptive = false,
        progress_message = (dt, u, p, t) -> t,
    )
    L1err[k] = norm(sol.u[end] .- h_init, 1)
    L2err[k] = norm(sol.u[end] .- h_init)
    Linferr[k] = norm(sol.u[end] .- h_init, Inf)

    @info "Test case: $(test_name) with α: $(test_angle_name)"
    @info "Number of elements per cube panel: $(ne) x $(ne)"
    @info "Solution norm at t = 0: ", norm(h_init)
    @info "Solution norm at t = $(T): ", norm(sol.u[end])
    @info "L₁ error at t = $(T): ", L1err[k]
    @info "L₂ error at t = $(T): ", L2err[k]
    @info "L∞ error at t = $(T): ", Linferr[k]
end

# Print convergence rate info
conv = convergence_rate(L2err, Δh)
@info "Converge rates for this test case are: ", conv

# Plot the errors
# L₁ error Vs number of elements
Plots.png(
    Plots.plot(
        collect(ne_seq),
        L1err,
        yscale = :log10,
        xlabel = "Nₑ",
        ylabel = "log₁₀(L₁ err)",
        label = "",
    ),
    joinpath(path, "L1error.png"),
)
linkfig(
    relpath(joinpath(path, "L1error.png"), joinpath(@__DIR__, "../..")),
    "L₁ error Vs Nₑ",
)

# L₂ error Vs number of elements
Plots.png(
    Plots.plot(
        collect(ne_seq),
        L2err,
        yscale = :log10,
        xlabel = "Nₑ",
        ylabel = "log₁₀(L₂ err)",
        label = "",
    ),
    joinpath(path, "L2error.png"),
)
linkfig(
    relpath(joinpath(path, "L2error.png"), joinpath(@__DIR__, "../..")),
    "L₂ error Vs Nₑ",
)

# L∞ error Vs number of elements
Plots.png(
    Plots.plot(
        collect(ne_seq),
        Linferr,
        yscale = :log10,
        xlabel = "Nₑ",
        ylabel = "log₁₀(L∞ err)",
        label = "",
    ),
    joinpath(path, "Linferror.png"),
)
linkfig(
    relpath(joinpath(path, "Linferror.png"), joinpath(@__DIR__, "../..")),
    "L∞ error Vs Nₑ",
)