text:
# Structured optimizer attributes #
# ========================== #
"""
    RelativeTolerance

An optimizer attribute for specifying the relative tolerance of a structure-exploiting algorithm.
"""
struct RelativeTolerance <: AbstractStructuredOptimizerAttribute end
"""
    MasterOptimizer

An optimizer attribute for specifying the MathOptInterface optimizer used to solve master problems arising in a structure-exploiting procedure.
"""
struct MasterOptimizer <: AbstractStructuredOptimizerAttribute end

function JuMP.set_optimizer_attribute(stochasticprogram::StochasticProgram, attr::MasterOptimizer, value)
    MOI.set(stochasticprogram, attr, value)
    set_master_optimizer!(structure(stochasticprogram), value)
end
"""
    MasterOptimizerAttribute

An optimizer attribute used for setting attributes of the master optimizer.
"""
struct MasterOptimizerAttribute <: AbstractStructuredOptimizerAttribute end
"""
    RawMasterOptimizerParameter

An optimizer attribute used for raw parameters of the master optimizer. Defers to `RawParameter`.
"""
struct RawMasterOptimizerParameter <: AbstractStructuredOptimizerAttribute
    name::Any
end

function MOI.set(sp::StochasticProgram, attr::MasterOptimizerAttribute, master_attr::MOI.AbstractOptimizerAttribute, value)
    MOI.set(optimizer(sp), attr, master_attr, value)
    return nothing
end

function JuMP.set_optimizer_attribute(stochasticprogram::StochasticProgram, attr::MasterOptimizerAttribute, master_attr::MOI.AbstractOptimizerAttribute, value)
    MOI.set(stochasticprogram, attr, master_attr, value)
    set_master_optimizer_attribute!(structure(stochasticprogram), master_attr, value)
end

function JuMP.set_optimizer_attribute(stochasticprogram::StochasticProgram, param::RawMasterOptimizerParameter, value)
    MOI.set(stochasticprogram, param, value)
    set_master_optimizer_attribute!(structure(stochasticprogram), MOI.RawParameter(param.name), value)
end
"""
    get_masteroptimizer_attribute(stochasticprogram::StochasticProgram, name::String)

Return the value associated with the solver-specific attribute named `name` of the master optimizer in `stochasticprogram`.

See also: [`set_masteroptimizer_attribute`](@ref), [`set_masteroptimizer_attributes`](@ref).
"""
function get_masteroptimizer_attribute(stochasticprogram::StochasticProgram, name::String)
    return get_masteroptimizer_attribute(stochasticprogram, RawMasterOptimizerParameter(name))
end
"""
    get_masteroptimizer_attribute(stochasticprogram::StochasticProgram, attr::MOI.AbstractOptimizerAttribute)

Return the value of the solver-specific attribute `attr` of the master optimizer in `stochasticprogram`.

See also: [`set_masteroptimizer_attribute`](@ref), [`set_masteroptimizer_attributes`](@ref).
"""
function get_masteroptimizer_attribute(stochasticprogram::StochasticProgram, attr::MOI.AbstractOptimizerAttribute)
    return MOI.get(optimizer(stochasticprogram), MasterOptimizerAttribute(), attr)
end
"""
    set_masteroptimizer_attribute(stochasticprogram::StochasticProgram, attr::MOI.AbstractOptimizerAttribute, value)

Sets solver-specific attribute `attr` of the master optimizer to `value`.

"""
function set_masteroptimizer_attribute(stochasticprogram::StochasticProgram, attr::MOI.AbstractOptimizerAttribute, value)
    return set_optimizer_attribute(stochasticprogram, MasterOptimizerAttribute(), attr, value)
end
"""
    set_masteroptimizer_attribute(stochasticprogram::StochasticProgram, name::Union{Symbol, String}, value)

Sets solver-specific attribute of the master optimizer identified by `name` to `value`.

"""
function set_masteroptimizer_attribute(stochasticprogram::StochasticProgram, name::Union{Symbol, String}, value)
    return set_optimizer_attribute(stochasticprogram, RawMasterOptimizerParameter(String(name)), value)
end
"""
    set_masteroptimizer_attributes(stochasticprogram::StochasticProgram, pairs::Pair...)

Given a list of `attribute => value` pairs or a collection of keyword arguments, calls
`set_masteroptimizer_attribute(stochasticprogram, attribute, value)` for each pair.

"""
function set_masteroptimizer_attributes(stochasticprogram::StochasticProgram, pairs::Pair...)
    for (name, value) in pairs
        set_masteroptimizer_attribute(stochasticprogram, name, value)
    end
end
function set_masteroptimizer_attributes(stochasticprogram::StochasticProgram; kw...)
    for (name, value) in kw
        set_masteroptimizer_attribute(stochasticprogram, name, value)
    end
end
"""
    SubproblemOptimizer

An optimizer attribute for specifying the MathOptInterface optimizer used to solve subproblems arising in a structure-exploiting procedure.
"""
struct SubproblemOptimizer <: AbstractStructuredOptimizerAttribute end

function JuMP.set_optimizer_attribute(stochasticprogram::StochasticProgram, attr::SubproblemOptimizer, value)
    MOI.set(stochasticprogram, attr, value)
    set_subproblem_optimizer!(structure(stochasticprogram), value)
end

struct SubproblemOptimizerAttribute <: AbstractStructuredOptimizerAttribute end
"""
    RawSubproblemOptimizerParameter

An optimizer attribute used for raw parameters of the subproblem optimizer. Defers to `RawParameter`.
"""
struct RawSubproblemOptimizerParameter <: AbstractStructuredOptimizerAttribute
    name::Any
end

function MOI.set(sp::StochasticProgram, attr::SubproblemOptimizerAttribute, sub_attr::MOI.AbstractOptimizerAttribute, value)
    MOI.set(optimizer(sp), attr, sub_attr, value)
    return nothing
end

function JuMP.set_optimizer_attribute(stochasticprogram::StochasticProgram, attr::SubproblemOptimizerAttribute, sub_attr::MOI.AbstractOptimizerAttribute, value)
    MOI.set(stochasticprogram, attr, sub_attr, value)
    set_subproblem_optimizer_attribute!(structure(stochasticprogram), sub_attr, value)
end

function JuMP.set_optimizer_attribute(stochasticprogram::StochasticProgram, param::RawSubproblemOptimizerParameter, value)
    MOI.set(stochasticprogram, param, value)
    set_subproblem_optimizer_attribute!(structure(stochasticprogram), MOI.RawParameter(param.name), value)
end
"""
    get_suboptimizer_attribute(stochasticprogram::StochasticProgram, name::String)

Return the value associated with the solver-specific attribute named `name` of the subproblem optimizer in `stochasticprogram`.

See also: [`set_suboptimizer_attribute`](@ref), [`set_suboptimizer_attributes`](@ref).
"""
function get_suboptimizer_attribute(stochasticprogram::StochasticProgram, name::String)
    return get_suboptimizer_attribute(stochasticprogram, RawSubproblemOptimizerParameter(name))
end
"""
    get_suboptimizer_attribute(stochasticprogram::StochasticProgram, attr::MOI.AbstractOptimizerAttribute)

Return the value of the solver-specific attribute `attr` of the subproblem optimizer in `stochasticprogram`.

See also: [`set_suboptimizer_attribute`](@ref), [`set_suboptimizer_attributes`](@ref).
"""
function get_suboptimizer_attribute(stochasticprogram::StochasticProgram, attr::MOI.AbstractOptimizerAttribute)
    return MOI.get(optimizer(stochasticprogram), SubproblemOptimizerAttribute(), attr)
end
"""
    set_suboptimizer_attribute(stochasticprogram::StochasticProgram, attr::MOI.AbstractOptimizerAttribute, value)

Sets solver-specific attribute `attr` of the subproblem optimizer to `value`.

"""
function set_suboptimizer_attribute(stochasticprogram::StochasticProgram, attr::MOI.AbstractOptimizerAttribute, value)
    return set_optimizer_attribute(stochasticprogram, SubproblemOptimizerAttribute(), attr, value)
end
"""
    set_suboptimizer_attribute(stochasticprogram::StochasticProgram, name::Union{Symbol, String}, value)

Sets solver-specific attribute of the subproblem optimizer identified by `name` to `value`.

"""
function set_suboptimizer_attribute(stochasticprogram::StochasticProgram, name::Union{Symbol, String}, value)
    return set_optimizer_attribute(stochasticprogram, RawSubproblemOptimizerParameter(String(name)), value)
end
"""
    set_suboptimizer_attributes(stochasticprogram::StochasticProgram, pairs::Pair...)

Given a list of `attribute => value` pairs or a collection of keyword arguments, calls
`set_suboptimizer_attribute(stochasticprogram, attribute, value)` for each pair.

"""
function set_suboptimizer_attributes(stochasticprogram::StochasticProgram, pairs::Pair...)
    for (name, value) in pairs
        set_suboptimizer_attribute(stochasticprogram, name, value)
    end
end
function set_suboptimizer_attributes(stochasticprogram::StochasticProgram; kw...)
    for (name, value) in kw
        set_suboptimizer_attribute(stochasticprogram, name, value)
    end
end
"""
    Execution

An optimizer attribute for specifying an execution policy for a structure-exploiting algorithm. Options are:

- [`Serial`](@ref):  Classical L-shaped (default)
- [`Synchronous`](@ref): Classical L-shaped run in parallel
- [`Asynchronous`](@ref): Asynchronous L-shaped ?Asynchronous for parameter descriptions.
"""
struct Execution <: AbstractStructuredOptimizerAttribute end
"""
    ExecutionParameter

Abstract supertype for execution-specific attributes.
"""
abstract type ExecutionParameter <: AbstractStructuredOptimizerAttribute end

# Sampled optimizer attributes #
# ========================== #
"""
    Confidence

An optimizer attribute for specifying the confidence level of sample-based procedures.
"""
struct Confidence <: AbstractSampledOptimizerAttribute end
"""
    NumSamples

An optimizer attribute for specifying the sample size of sample-based procedures.
"""
struct NumSamples <: AbstractSampledOptimizerAttribute end
"""
    NumEvalSamples

An optimizer attribute for specifying the sample size of evaluation instances of sample-based procedures.
"""
struct NumEvalSamples <: AbstractSampledOptimizerAttribute end
"""
    NumEWSSamples

An optimizer attribute for specifying the sample size of wait-and-see instances in sample-based procedures.
"""
struct NumEWSSamples <: AbstractSampledOptimizerAttribute end
"""
    NumEEVSamples

An optimizer attribute for specifying the sample size of expected-value evaluation instances in sample-based procedures.
"""
struct NumEEVSamples <: AbstractSampledOptimizerAttribute end
"""
    NumLowerTrials

An optimizer attribute for specifying the number of trials used to compute lower bounds of confidence intervals in sample-based procedures.
"""
struct NumLowerTrials <: AbstractSampledOptimizerAttribute end
"""
    NumUpperTrials

An optimizer attribute for specifying the number of trials used to compute upper bounds of confidence intervals in sample-based procedures.
"""
struct NumUpperTrials <: AbstractSampledOptimizerAttribute end
"""
    InstanceOptimizer

An optimizer attribute for specifying the AbstractStructuredOptimizer/AbstractOptimizer used to solve sampled problems in sample-based procedures.
"""
struct InstanceOptimizer <: AbstractSampledOptimizerAttribute end

"""
    InstanceOptimizerAttribute

An optimizer attribute used for setting attributes of the instance optimizer.
"""
struct InstanceOptimizerAttribute end
"""
    RawInstanceOptimizerParameter

An optimizer attribute used for raw parameters of the instance optimizer. Defers to `RawParameter`.
"""
struct RawInstanceOptimizerParameter <: AbstractSampledOptimizerAttribute
    name::Any
end
MOI.supports(::MOI.AbstractOptimizer, ::RawInstanceOptimizerParameter) = false

function MOI.set(sm::StochasticModel, attr::InstanceOptimizerAttribute, instance_attr::MOI.AbstractOptimizerAttribute, value)
    MOI.set(optimizer(sm), attr, instance_attr, value)
    return nothing
end
"""
    get_instanceoptimizer_attribute(stochasticprogram::StochasticProgram, name::String)

Return the value associated with the solver-specific attribute named `name` of the instance optimizer in `stochasticprogram`.

See also: [`set_instanceoptimizer_attribute`](@ref), [`set_instanceoptimizer_attributes`](@ref).
"""
function get_instanceoptimizer_attribute(stochasticprogram::StochasticProgram, name::String)
    return get_instanceoptimizer_attribute(stochasticprogram, RawInstanceOptimizerParameter(name))
end
"""
    get_instanceoptimizer_attribute(stochasticmodel::StochasticModel, attr::MOI.AbstractOptimizerAttribute)

Return the value of the solver-specific attribute `attr` of the instance optimizer in `stochasticmodel`.

See also: [`set_instanceoptimizer_attribute`](@ref), [`set_instanceoptimizer_attributes`](@ref).
"""
function get_instanceoptimizer_attribute(stochasticmodel::StochasticModel, attr::MOI.AbstractOptimizerAttribute)
    return MOI.get(optimizer(stochasticmodel), InstanceOptimizerAttribute(), attr)
end
"""
    set_instanceoptimizer_attribute(stochasticmodel::StochasticModel, attr::MOI.AbstractOptimizerAttribute, value)

Sets solver-specific attribute `attr` of the instance optimizer to `value` in `stochasticmodel`.
"""
function set_instanceoptimizer_attribute(stochasticmodel::StochasticModel, attr::MOI.AbstractOptimizerAttribute, value)
    return MOI.set(optimizer(stochasticmodel), InstanceOptimizerAttribute(), attr, value)
end
"""
    set_instanceoptimizer_attribute(stochasticmodel::StochasticModel, name::Union{Symbol, String}, value)

Sets solver-specific attribute of the instance optimizer identified by `name` to `value` in `stochasticmodel`.

"""
function set_instanceoptimizer_attribute(stochasticmodel::StochasticModel, name::Union{Symbol, String}, value)
    return set_optimizer_attribute(stochasticmodel, RawInstanceOptimizerParameter(String(name)), value)
end
"""
    set_instanceoptimizer_attributes(stochasticmodel::StochasticModel, pairs::Pair...)

Given a list of `attribute => value` pairs or a collection of keyword arguments, calls
`set_instanceoptimizer_attribute(stochasticmodel, attribute, value)` for each pair.

"""
function set_instanceoptimizer_attributes(stochasticmodel::StochasticModel, pairs::Pair...)
    for (name, value) in pairs
        set_instanceoptimizer_attribute(stochasticmodel, name, value)
    end
end
function set_instanceoptimizer_attributes(stochasticmodel::StochasticModel; kw...)
    for (name, value) in kw
        set_instanceoptimizer_attribute(stochasticmodel, name, value)
    end
end


text:
# Test FMM matvec vs full matrix
push!(LOAD_PATH, string(pwd(),"/src"))
using Base.Test
using ModifiedStokesSolver
import AnalyticDomains

@testset "Matvec" begin
    # Params
    alpha = 10.0
    # Discretize
    numpanels = 20
    panelorder = 16
    curve = AnalyticDomains.starfish(n_arms = 3, amplitude=0.2)
    dcurve = discretize(curve, numpanels, panelorder)
    N = dcurve.numpoints
    # Compute
    x = rand(2*N)
    matvec = system_matvec(dcurve, alpha)
    yfmm = matvec(x)
    A = system_matrix(dcurve, alpha)
    ydir = A*x
    erel = norm(yfmm-ydir, Inf) / norm(ydir, Inf)
    @test erel < 5e-14
end

ext: jl


text:
# Autogenerated wrapper script for libplist_jll for x86_64-w64-mingw32-cxx03
export libplist, plistutil

JLLWrappers.@generate_wrapper_header("libplist")
JLLWrappers.@declare_library_product(libplist, "libplist-2.0.dll")
JLLWrappers.@declare_executable_product(plistutil)
function __init__()
    JLLWrappers.@generate_init_header()
    JLLWrappers.@init_library_product(
        libplist,
        "bin\\libplist-2.0.dll",
        RTLD_LAZY | RTLD_DEEPBIND,
    )

    JLLWrappers.@init_executable_product(
        plistutil,
        "bin\\plistutil.exe",
    )

    JLLWrappers.@generate_init_footer()
end  # __init__()


text:
@testset "Maxima" begin
  maxima = BlockMaxima(1:100, 2)
  @test collect(maxima) == collect(2:2:100)

  maxima = PeakOverThreshold(1:100, 50.)
  @test collect(maxima) == collect(51:100)
end


text:
using Faust
using Test

@testset "Faust.jl" begin
    p0 = init!(compile("process = 0, 0;"))
    p_ = init!(compile("process = _, _;"))
    posc = init!(compile("""
    import("stdfaust.lib");
    freq = hslider("freq", 110, 110, 880, 10);
    t = ba.time : ba.samp2sec;
    process = cos(2*ma.PI*freq*t);
    """))

    p_.inputs = rand(256, 2)

    @test compute!(p0) == zeros(256, 2)
    @test compute!(p_) == p_.inputs

    @test posc.ui.ranges["/score/freq"] == Faust.UIRange(110, 110, 880, 10)
    t = (0:255) / posc.samplerate
    t2 = (256:511) / posc.samplerate
    @test compute!(posc) ≈ cos.(2*pi*110*t) atol=0.001
    setparams!(posc, Dict("/score/freq" => 220.0))
    @test compute!(posc) ≈ cos.(2*pi*220*t2) atol=0.001
end

@testset "llvm-c-dsp.jl" begin
    argv = ["-vec"]
    code = """
    import("stdfaust.lib");
    f0 = hslider("[foo:bar]f0", 110, 110, 880, 1);
    n = 2;
    inst = par(i, n, os.oscs(f0 * (n+i) / n)) :> /(n);
    process = inst, inst;
    """
    factory = createCDSPFactoryFromString("score", code, argv, "", -1)
    dsp = createCDSPInstance(factory)
    @test getNumInputsCDSPInstance(dsp) == 0
    @test getNumOutputsCDSPInstance(dsp) == 2

    sr = 22050
    initCDSPInstance(dsp, sr)
    out = computeCDSPInstance(dsp, 256)
    @test out[:, 1] == out[:, 2]

    deleteCDSPInstance(dsp)
    deleteCDSPFactory(factory)
end


text:
ENV["PYCALL_JL_RUNTIME_PYTHON"] = Sys.which("python")

using Pkg
Pkg.activate(joinpath(@__DIR__, ".."))

using ArgParse
using Distributions
using LinearAlgebra
using FileIO
using JLD2
using Printf
using Random
using RiskSensitiveSAC
import RobotOS.Time

################ COMMAND LINE ARGUMENTS ################
function parse_commandline()
    s = ArgParseSettings()

    @add_arg_table s begin
        "params_file_name"
            help = "parameter file name in default_params (without .jl)"
            arg_type = String
            required = true
        "test_case_file_name_1"
            help = "a test case file name in test_cases (without .jl)"
            arg_type = String
            required = true
        "test_case_file_name_2"
            help = "a test case file name in test_cases (without .jl)"
            arg_type = String
        "test_case_file_name_3"
            help = "a test case file name in test_cases (without .jl)"
            arg_type = String
        "--sigma_risk"
            help = "σ_risk value (risk sensitivity parameter)"
            arg_type = Float64
        "--alpha_col"
            help = "α_col value (collision cost peak parameter)"
            arg_type = Float64
        "--lambda_col"
            help = "λ_col value (collision cost bandwidth parameter)"
            arg_type = Float64
        "--num_samples"
            help = "number of prediction samples (not used in bic controller or oracle predictor)"
            arg_type = Int64
        "--nominal_search_only"
            help = "use nominal search results only instead of rssac"
            action = :store_true
        "--nominal_base_only"
            help = "use nominal base only instead of rssac"
            action = :store_true
        "--deterministic_prediction"
            help = "perform deterministic prediction"
            action = :store_true
        "--future_conditional"
            help = "condition prediction on future nominal control"
            action = :store_true
        "--use_bic"
            help = "use buffered input cell controller instead of rssac"
            action = :store_true
        "--use_crowd_nav"
            help = "use crowd nav controller instead of rssac"
            action = :store_true
        "--rng_seed"
            help = "rng seed for prediction (and simulation if in synthetic scene mode)"
            arg_type = Int64
        "--run_id"
            help = "run id"
            arg_type = Int64
            default = 1
        "--verbose"
            help = "verbose (true or false)"
            arg_type = Bool
            default = true
        "--show_log"
            help = "show log after evaluation is done"
            action = :store_true
        "--randomize_goal"
            help = "perturb goal location with unit Gaussian"
            action = :store_true
    end
    localARGS = (@isdefined newARGS) ? newARGS : ARGS
    return parse_args(localARGS, s)
end

################ SIMULATION SETUP ################
parsed_args = parse_commandline()
# check for consistency
if parsed_args["nominal_search_only"]
    @assert !parsed_args["nominal_base_only"]
end
if parsed_args["nominal_base_only"]
    @assert !parsed_args["nominal_search_only"]
end
if parsed_args["use_bic"]
    @assert !parsed_args["nominal_base_only"] &&
            !parsed_args["nominal_search_only"] &&
            !parsed_args["future_conditional"]
end

# load base parameters file
params_file_name = parsed_args["params_file_name"];
include("$(@__DIR__)/default_params/$(params_file_name).jl");
# load test case file(s)
test_case_file_name_1 = parsed_args["test_case_file_name_1"];
include("$(@__DIR__)/test_cases/$(test_case_file_name_1).jl");
if !isnothing(parsed_args["test_case_file_name_2"]);
    test_case_file_name_2 = parsed_args["test_case_file_name_2"];
    include("$(@__DIR__)/test_cases/$(test_case_file_name_2).jl");
end
if !isnothing(parsed_args["test_case_file_name_3"]);
    test_case_file_name_3 = parsed_args["test_case_file_name_3"];
    include("$(@__DIR__)/test_cases/$(test_case_file_name_3).jl");
end

# manually overwrite keyword arguments
if !isnothing(parsed_args["sigma_risk"])
    σ_risk = parsed_args["sigma_risk"];
end
if !isnothing(parsed_args["alpha_col"])
    α_col = parsed_args["alpha_col"];
end
if !isnothing(parsed_args["lambda_col"])
    λ_col = parsed_args["lambda_col"];
end
if !isnothing(parsed_args["num_samples"])
    num_samples = parsed_args["num_samples"]
end
if parsed_args["nominal_search_only"]
    dtexec = [0.0]
end
if parsed_args["deterministic_prediction"]
    deterministic = true
end
if parsed_args["future_conditional"]
    incl_robot_node = true
    use_robot_future = true
end
if !isnothing(parsed_args["rng_seed"])
    prediction_rng_seed = parsed_args["rng_seed"]
end
if parsed_args["randomize_goal"]
    goal_rng = MersenneTwister(prediction_rng_seed);
    d = MvNormal(zeros(2), Diagonal([1.0, 1.0]));
    ego_pos_goal_vec .+= rand(goal_rng, d)
end

# setup parameters
if parsed_args["use_bic"]
    @assert !parsed_args["use_crowd_nav"]
    include("$(@__DIR__)/parameter_setup_bic.jl");
    if scene_mode == "synthetic"
        scene_loader, controller, w_init, measurement_schedule, target_trajectory, target_speed, predictor =
        controller_setup(scene_param,
                         cost_param=cost_param,
                         cnt_param=cnt_param,
                         dtc=dtc,
                         dto=dto,
                         prediction_steps=prediction_steps,
                         num_samples=num_samples,
                         sim_rng=sim_rng,
                         ado_pos_init_dict=ado_pos_init_dict,
                         ado_vel_dict=ado_vel_dict,
                         ego_pos_init_vec=ego_pos_init_vec,
                         ego_vel_init_vec=nothing,
                         ego_pos_goal_vec=ego_pos_goal_vec,
                         target_speed=target_speed,
                         sim_horizon=sim_horizon,
                         verbose=parsed_args["verbose"]);

    elseif scene_mode == "data"
        scene_loader, controller, w_init, measurement_schedule, target_trajectory, target_speed =
        controller_setup(scene_param,
                         cost_param=cost_param,
                         cnt_param=cnt_param,
                         dtc=dtc,
                         prediction_steps=prediction_steps,
                         ego_pos_init_vec=ego_pos_init_vec,
                         ego_pos_goal_vec=ego_pos_goal_vec,
                         target_speed=target_speed,
                         sim_horizon=sim_horizon,
                         verbose=parsed_args["verbose"]);

    else
        @error "scene_mode: $(scene_mode) is not supported!"
    end
elseif parsed_args["use_crowd_nav"]
    @assert !parsed_args["use_bic"]
    include("$(@__DIR__)/parameter_setup_crowd_nav.jl");
    if scene_mode == "data"
        scene_loader, controller, w_init, ado_inputs, measurement_schedule, target_trajectory, target_speed =
        controller_setup(scene_param, cnt_param,
                         cost_param=cost_param,
                         dtc=dtc,
                         prediction_steps=prediction_steps,
                         ego_pos_init_vec=ego_pos_init_vec,
                         ego_pos_goal_vec=ego_pos_goal_vec,
                         target_speed=target_speed,
                         sim_horizon=sim_horizon,
                         verbose=parsed_args["verbose"]);
    else
        @error "scene_mode: $(scene_mode) is not supported!"
    end
else
    include("$(@__DIR__)/parameter_setup.jl")
    if scene_mode == "synthetic" && prediction_mode == "gaussian"
        scene_loader, controller, w_init, measurement_schedule, target_trajectory, target_speed =
        controller_setup(scene_param,
                         predictor_param,
                         prediction_device=prediction_device,
                         cost_param=cost_param,
                         cnt_param=cnt_param,
                         dtc=dtc,
                         dto=dto,
                         ado_pos_init_dict=ado_pos_init_dict,
                         ado_vel_dict=ado_vel_dict,
                         ego_pos_init_vec=ego_pos_init_vec,
                         ego_vel_init_vec=nothing,
                         ego_pos_goal_vec=ego_pos_goal_vec,
                         target_speed=target_speed,
                         sim_horizon=sim_horizon,
                         verbose=parsed_args["verbose"])

    elseif scene_mode == "data" && prediction_mode == "trajectron"
        if !@isdefined ado_id_to_replace
            ado_id_to_replace = nothing;
        end
        scene_loader, controller, w_init, measurement_schedule, target_trajectory, target_speed =
        controller_setup(scene_param,
                         predictor_param,
                         prediction_device=prediction_device,
                         cost_param=cost_param,
                         cnt_param=cnt_param,
                         dtc=dtc,
                         ego_pos_init_vec=ego_pos_init_vec,
                         ego_vel_init_vec=nothing,
                         ego_pos_goal_vec=ego_pos_goal_vec,
                         target_speed=target_speed,
                         sim_horizon=sim_horizon,
                         ado_id_to_replace=ado_id_to_replace,
                         verbose=parsed_args["verbose"])

    elseif scene_mode == "data" && prediction_mode == "oracle"
        scene_loader, controller, w_init, measurement_schedule, target_trajectory, target_speed =
        controller_setup(scene_param,
                         predictor_param,
                         prediction_device=prediction_device,
                         cost_param=cost_param,
                         cnt_param=cnt_param,
                         dtc=dtc,
                         ego_pos_init_vec=ego_pos_init_vec,
                         ego_vel_init_vec=nothing,
                         ego_pos_goal_vec=ego_pos_goal_vec,
                         target_speed=target_speed,
                         sim_horizon=sim_horizon,
                         verbose=parsed_args["verbose"])
    else
        @error "scene_mode: $(scene_mode) + prediction_mode: $(prediction_mode) is not supported!"
    end
end

#=
println("σ_risk:        $(controller.sim_param.cost_param.σ_risk)");
println("α_col:         $(controller.sim_param.cost_param.α_col)");
println("λ_col:         $(controller.sim_param.cost_param.λ_col)");
println("num_samples:   $(controller.sim_param.num_samples)");
println("dtexec:        $(controller.cnt_param.dtexec)");
try
    println("deterministic: $(controller.predictor.param.deterministic)")
catch e
end
try
    println("robot future:  $(controller.predictor.param.use_robot_future)")
catch e
end
try
    println("incl robot:    $(scene_loader.param.incl_robot_node)")
catch e
end
println("prediction_rng_seed:        $(prediction_rng_seed)");
=#

################ EVALUATION START ################
if !@isdefined predictor
    predictor = nothing;
end
if typeof(controller) == BICController || typeof(controller) == CrowdNavController
    nominal_control = nothing;
else
    nominal_control = parsed_args["nominal_base_only"]
end
if !@isdefined ado_inputs
    ado_inputs = nothing;
end
if !@isdefined ado_id_removed
    ado_id_removed = nothing;
end
if isnothing(ego_pos_goal_vec)
    ego_pos_goal_vec = maximum(target_trajectory)[2]
end
println("Evaulation has started.")
result, ~, ~ = evaluate(scene_loader, controller, w_init, ego_pos_goal_vec,
                        target_speed, measurement_schedule, target_trajectory,
                        pos_error_replan, nominal_control=nominal_control,
                        ado_id_removed=ado_id_removed,
                        predictor=predictor, ado_inputs_init=ado_inputs);
if parsed_args["show_log"]
    display_log(result.log);
end
println("")
println("###### Simulation Result #######");
cnt_cost_str = @sprintf "Total Control Cost  : %10.4f" result.total_cnt_cost;
pos_cost_str = @sprintf "Total Position Cost : %10.4f" result.total_pos_cost;
col_cost_str = @sprintf "Total Collision Cost: %10.4f" result.total_col_cost;
min_dist_val = minimum([minimum(vcat([norm(get_position(w.e_state) - ap) for ap in values(w.ap_dict)], Inf))
                      for w in result.w_history]);
init_dist_goal_val = norm(get_position(result.w_history[1].e_state) - ego_pos_goal_vec);
final_dist_goal_val = norm(get_position(result.w_history[end].e_state) - ego_pos_goal_vec);
relative_goal_dist_val = final_dist_goal_val/init_dist_goal_val;
min_dist_str =      @sprintf "Minimum E-A Dist [m]: %10.4f" min_dist_val;
rel_goal_dist_str = @sprintf "Rel Goal Dist       : %10.4f" relative_goal_dist_val;
println(cnt_cost_str);
println(pos_cost_str);
println(col_cost_str);
println(min_dist_str);
println(rel_goal_dist_str);

################ SAVE ################
save_dir = normpath(joinpath(@__DIR__, "..", "simulation_data",
                             parsed_args["params_file_name"]));

if (@isdefined test_case_file_name_2) && (@isdefined test_case_file_name_3)
    test_case_name = "$(test_case_file_name_1)_$(test_case_file_name_2)_$(test_case_file_name_3)";
elseif @isdefined test_case_file_name_2
    test_case_name = "$(test_case_file_name_1)_$(test_case_file_name_2)";
else
    test_case_name = test_case_file_name_1;;
end
save_dir = joinpath(save_dir, test_case_name);

if (@isdefined incl_robot_node) && incl_robot_node &&
   (@isdefined use_robot_future) && use_robot_future
    save_dir = joinpath(save_dir, "future_conditional")
else
    save_dir = joinpath(save_dir, "non_future_conditional")
end

if typeof(controller) == BICController || typeof(controller) == CrowdNavController
    save_dir = joinpath(save_dir, "deterministic")
elseif deterministic || typeof(controller.predictor) == OraclePredictor
    save_dir = joinpath(save_dir, "deterministic")
else
    num_samples_str = @sprintf "%03d" controller.sim_param.num_samples;
    save_dir = joinpath(save_dir, "stochastic", "samples_$(num_samples_str)")
end
if !isdir(save_dir)
    mkpath(save_dir)
end

alpha_str = @sprintf "%05.1f" controller.sim_param.cost_param.α_col;
lambda_str = @sprintf "%04.2f" controller.sim_param.cost_param.λ_col;
risk_str = @sprintf "%05.2f" controller.sim_param.cost_param.σ_risk;
file_name = "alpha_$(alpha_str)_lambda_$(lambda_str)_risk_$(risk_str)_";
run_id_str = @sprintf "%03d" parsed_args["run_id"];
file_name *= "run_$(run_id_str).jld2"

save(save_dir * "/$(file_name)", "result", result, "parsed_args", parsed_args);
println("Results saved to " * save_dir * "/$(file_name)");


text:
import Base.linearindexing
import Base.similar

"Destructuring view around an arbitrary abstract array.  See destructure()"
immutable DestructuredArray{T,N,ArrayT} <: AbstractArray{T,N}
    base::ArrayT
end

function DestructuredArray{ArrayT<:AbstractArray}(A::ArrayT)
    DestructuredArray{eltype(eltype(A)), ndims(A)+ndims(eltype(A)), ArrayT}(A)
end

linearindexing(::Type{DestructuredArray}) = Base.LinearSlow()
size{T,N,ArrayT}(A::DestructuredArray{T,N,ArrayT}) = (size(eltype(ArrayT))..., size(A.base)...)

# TODO: Should define similar() but it doesn't work for sparse matrices in 0.4
# since they don't support more than two dimensions.
# similar{S,D}(A::DestructuredArray, ::Type{S}, dims::NTuple{D,Int}) = similar(A.base, S, dims...)

@generated function getindex{T,N,ArrayT}(A::DestructuredArray{T,N,ArrayT}, inds::Int...)
    eldims = ndims(eltype(ArrayT))
    # We'd like to just do
    # A.base[inds[eldims+1:end]...][inds[1:eldims]...]
    # but splatting is currently (2015-12) a performance disaster
    fixinds = [:(inds[$i]) for i in 1:eldims]
    baseinds = [:(inds[$i]) for i in eldims+1:length(inds)]
    quote
        A.base[$(baseinds...)][$(fixinds...)]
    end
end

@generated function setindex!{T,N,ArrayT}(A::DestructuredArray{T,N,ArrayT}, value, inds::Int...)
    eldims = ndims(eltype(ArrayT))
    fixinds = [:(inds[$i]) for i in 1:eldims]
    baseinds = [:(inds[$i]) for i in eldims+1:length(inds)]
    quote
        # TODO: FixedSizeArrays.setindex gives a rather severe performance
        # penalty here.
        A.base[$(baseinds...)] = setindex(A.base[$(baseinds...)], value, $(fixinds...))
    end
end

"""
Destructure the elements of an array `A` to create `M=ndims(eltype(A))`
additional dimensions prepended to the dimensions of `A`.  The returned array
is a view onto the original elements; additional dimensions occur first
for consistency with the natural memory ordering.

For example, `AbstractArray{F<:FixedArray{T,M,SIZE},N}` appears as an
`AbstractArray{T,M+N}` after destructuring.
"""
destructure(A::AbstractArray) = DestructuredArray(A)

# Faster reinterpret-based version (as of 2015-12) for plain Array
destructure{T,N}(A::Array{T,N}) = reinterpret(eltype(T), A, (size(T)..., size(A)...))


text:
using ADCME
using PyCall
using LinearAlgebra
using PyPlot
using Random
Random.seed!(233)

if Sys.islinux()
py"""
import tensorflow as tf
libSatOp = tf.load_op_library('./build/libSatOp.so')
@tf.custom_gradient
def sat_op(s0,pt,permi,poro,qw,qo,muw,muo,sref,dt,h):
    sat = libSatOp.sat_op(s0,pt,permi,poro,qw,qo,muw,muo,sref,dt,h)
    def grad(dy):
        return libSatOp.sat_op_grad(dy, sat, s0,pt,permi,poro,qw,qo,muw,muo,sref,dt,h)
    return sat, grad
"""
elseif Sys.isapple()
py"""
import tensorflow as tf
libSatOp = tf.load_op_library('./build/libSatOp.dylib')
@tf.custom_gradient
def sat_op(s0,pt,permi,poro,qw,qo,muw,muo,sref,dt,h):
    sat = libSatOp.sat_op(s0,pt,permi,poro,qw,qo,muw,muo,sref,dt,h)
    def grad(dy):
        return libSatOp.sat_op_grad(dy, sat, s0,pt,permi,poro,qw,qo,muw,muo,sref,dt,h)
    return sat, grad
"""
elseif Sys.iswindows()
py"""
import tensorflow as tf
libSatOp = tf.load_op_library('./build/libSatOp.dll')
@tf.custom_gradient
def sat_op(s0,pt,permi,poro,qw,qo,muw,muo,sref,dt,h):
    sat = libSatOp.sat_op(s0,pt,permi,poro,qw,qo,muw,muo,sref,dt,h)
    def grad(dy):
        return libSatOp.sat_op_grad(dy, sat, s0,pt,permi,poro,qw,qo,muw,muo,sref,dt,h)
    return sat, grad
"""
end

sat_op = py"sat_op"

if Sys.islinux()
py"""
import tensorflow as tf
libUpwpsOp = tf.load_op_library('../Upwps/build/libUpwpsOp.so')
@tf.custom_gradient
def upwps_op(permi,mobi,src,funcref,h,rhograv,index):
    pres = libUpwpsOp.upwps_op(permi,mobi,src,funcref,h,rhograv,index)
    def grad(dy):
        return libUpwpsOp.upwps_op_grad(dy, pres, permi,mobi,src,funcref,h,rhograv,index)
    return pres, grad
"""
elseif Sys.isapple()
    py"""
    import tensorflow as tf
    libUpwpsOp = tf.load_op_library('../Upwps/build/libUpwpsOp.dylib')
    @tf.custom_gradient
    def upwps_op(permi,mobi,src,funcref,h,rhograv,index):
        pres = libUpwpsOp.upwps_op(permi,mobi,src,funcref,h,rhograv,index)
        def grad(dy):
            return libUpwpsOp.upwps_op_grad(dy, pres, permi,mobi,src,funcref,h,rhograv,index)
        return pres, grad
    """
end
upwps_op = py"upwps_op"

if Sys.islinux()
py"""
import tensorflow as tf
libUpwlapOp = tf.load_op_library('../Upwlap/build/libUpwlapOp.so')
@tf.custom_gradient
def upwlap_op(perm,mobi,func,h,rhograv):
    out = libUpwlapOp.upwlap_op(perm,mobi,func,h,rhograv)
    def grad(dy):
        return libUpwlapOp.upwlap_op_grad(dy, out, perm,mobi,func,h,rhograv)
    return out, grad
"""
elseif Sys.isapple()
py"""
import tensorflow as tf
libUpwlapOp = tf.load_op_library('../Upwlap/build/libUpwlapOp.dylib')
@tf.custom_gradient
def upwlap_op(perm,mobi,func,h,rhograv):
    out = libUpwlapOp.upwlap_op(perm,mobi,func,h,rhograv)
    def grad(dy):
        return libUpwlapOp.upwlap_op_grad(dy, out, perm,mobi,func,h,rhograv)
    return out, grad
"""
end
upwlap_op = py"upwlap_op"

if Sys.islinux()
py"""
import tensorflow as tf
libPoissonOp = tf.load_op_library('../Poisson/build/libPoissonOp.so')
@tf.custom_gradient
def poisson_op(coef,g,h,rhograv,index):
    p = libPoissonOp.poisson_op(coef,g,h,rhograv,index)
    def grad(dy):
        return libPoissonOp.poisson_op_grad(dy, p, coef, g, h, rhograv, index)
    return p, grad
"""
elseif Sys.isapple()
py"""
import tensorflow as tf
libPoissonOp = tf.load_op_library('../Poisson/build/libPoissonOp.dylib')
@tf.custom_gradient
def poisson_op(coef,g,h,rhograv,index):
    p = libPoissonOp.poisson_op(coef,g,h,rhograv,index)
    def grad(dy):
        return libPoissonOp.poisson_op_grad(dy, p, coef, g, h, rhograv, index)
    return p, grad
"""
end
poisson_op = py"poisson_op"

function ave_normal(quantity, m, n)
    aa = sum(quantity)
    return aa/(m*n)
end

# TODO:
# const ALPHA = 0.006323996017182
const ALPHA = 1.0
const SRC_CONST = 86400.0
const K_CONST =  9.869232667160130e-16 * 86400
nz=20
nx=30
sw = constant(zeros(nz, nx))
swref = constant(zeros(nz,nx))
μw = constant(0.001)
μo = constant(0.003)
K = constant(100.0 .* ones(nz, nx))
ϕ = constant(0.25 .* ones(nz, nx))
dt = constant(30.0)
h = constant(100.0 * 0.3048)
q1 = zeros(nz,nx)
q2 = zeros(nz,nx)
q1[10,5] = 0.002 * (1/(100.0 * 0.3048)^2)/20.0/0.3048 * SRC_CONST
q2[10,25] = -0.002 * (1/(100.0 * 0.3048)^2)/20.0/0.3048 * SRC_CONST
qw = constant(q1)
qo = constant(q2)

λw = sw.*sw/μw
λo = (1-sw).*(1-sw)/μo
λ = λw + λo
f = λw/λ
q = qw + qo + λw/(λo+1e-16).*qo

# Θ = laplacian_op(K.*λo, potential_c, h, constant(0.0))
Θ = upwlap_op(K*K_CONST, λo, constant(zeros(nz,nx)), h, constant(0.0))

load_normal = (Θ+q/ALPHA) - ave_normal(Θ+q/ALPHA, nz, nx)

tf_comp_p0 = upwps_op(K*K_CONST, λ, load_normal, constant(zeros(nz,nx)), h, constant(0.0), constant(2))
sess = Session()
init(sess)
p0 = run(sess, tf_comp_p0)
tf_p0 = constant(p0)

# s = sat_op(sw,p0,K,ϕ,qw,qo,sw,dt,h)

# function step(sw)
#     λw = sw.*sw
#     λo = (1-sw).*(1-sw)
#     λ = λw + λo
#     f = λw/λ
#     q = qw + qo + λw/(λo+1e-16).*qo

#     # Θ = laplacian_op(K.*λo, constant(zeros(nz,nx)), h, constant(0.0))
#     Θ = upwlap_op(K, λo, constant(zeros(nz,nx)), h, constant(0.0))
#     # Θ = constant(zeros(nz,nx))

#     load_normal = (Θ+q/ALPHA) - ave_normal(Θ+q/ALPHA, nz, nx)

#     p = poisson_op(λ.*K, load_normal, h, constant(0.0), constant(0)) # potential p = pw - ρw*g*h
#     # p = upwps_op(K, λ, load_normal, constant(zeros(nz,nx)), h, constant(0.0), constant(0))
    # sw = sat_op(sw,p,K,ϕ,qw,qo,μw,μo,sw,dt,h)
#     return sw
# end

# NT=100
# function evolve(sw, NT, qw, qo)
#     # qw_arr = constant(qw) # qw: NT x m x n array
#     # qo_arr = constant(qo)
#     tf_sw = TensorArray(NT+1)
#     function condition(i, ta)
#         tf.less(i, NT+1)
#     end
#     function body(i, tf_sw)
#         sw_local = step(read(tf_sw, i))
#         i+1, write(tf_sw, i+1, sw_local)
#     end
#     tf_sw = write(tf_sw, 1, sw)
#     i = constant(1, dtype=Int32)
#     _, out = while_loop(condition, body, [i;tf_sw])
#     read(out, NT+1)
# end

# s = evolve(sw, NT, qw, qo)

# J = tf.nn.l2_loss(s)
# tf_grad_K = gradients(J, K)
# sess = Session()
# init(sess)
# # P = run(sess,p0)

# # error("")
# S=run(sess, s)
# imshow(S);colorbar();

# error("")

# grad_K = run(sess, tf_grad_K)
# imshow(grad_K);colorbar();
# error("")
# TODO:

# gradient check -- v
function scalar_function(m)
    # return sum(tanh(sat_op(m,tf_p0,K*K_CONST,ϕ,qw,qo,μw,μo,constant(zeros(nz,nx)),dt,h)))
    return sum(tanh(sat_op(sw,m,K*K_CONST,ϕ,qw,qo,μw,μo,constant(zeros(nz,nx)),dt,h)))
    # return sum(tanh(sat_op(sw,tf_p0,m,ϕ,qw,qo,μw,μo,constant(zeros(nz,nx)),dt,h)))
    # return sum(tanh(sat_op(sw,tf_p0,K*K_CONST,m,qw,qo,μw,μo,constant(zeros(nz,nx)),dt,h)))
end

# m_ = sw
# v_ = 0.1 * rand(nz,nx)

m_ = tf_p0
v_ = 5e5 .* rand(nz,nx)

# m_ = K*K_CONST
# v_ = 10 .* rand(nz,nx) *K_CONST

# m_ = ϕ
# v_ = 0.1 * rand(nz,nx)

y_ = scalar_function(m_)
dy_ = gradients(y_, m_)
ms_ = Array{Any}(undef, 5)
ys_ = Array{Any}(undef, 5)
s_ = Array{Any}(undef, 5)
w_ = Array{Any}(undef, 5)
gs_ =  @. 1 / 10^(1:5)

for i = 1:5
    g_ = gs_[i]
    ms_[i] = m_ + g_*v_
    ys_[i] = scalar_function(ms_[i])
    s_[i] = ys_[i] - y_
    w_[i] = s_[i] - g_*sum(v_.*dy_)
end

sess = Session()
init(sess)
sval_ = run(sess, s_)
wval_ = run(sess, w_)
close("all")
loglog(gs_, abs.(sval_), "*-", label="finite difference")
loglog(gs_, abs.(wval_), "+-", label="automatic differentiation")
loglog(gs_, gs_.^2 * 0.5*abs(wval_[1])/gs_[1]^2, "--",label="\$\\mathcal{O}(\\gamma^2)\$")
loglog(gs_, gs_ * 0.5*abs(sval_[1])/gs_[1], "--",label="\$\\mathcal{O}(\\gamma)\$")

plt.gca().invert_xaxis()
legend()
xlabel("\$\\gamma\$")
ylabel("Error")


text:
"This function computes finite difference stencil coefficients which approximate the
kth derivative of a function at a point x̄ using a linear combination of evaluations at x_i."

function fdcoeffV(k,x̄,x)
    n = length(x)
    if n <= k
        error("Too few points in the stencil")
    end
    A = zeros(n,n)
    A[1,:] = ones(n)
    for i = 2:n
        A[i,:] = @. (x - x̄)^(i-1) / factorial(i-1) # ∑ a_i * (x_i - x̄)^i/i! = b_i
    end
    b = zeros(n,1)
    b[k+1] = 1
    return vec(A\b)
end


text: abstract type AbstractKernel end


text:
using RecipesBase
using Colors

@recipe function plot(env::UnderwaterEnvironment;
    sources=[], receivers=[], transmissionloss=[], rays=[], xmargin=0.0, dynamicrange=42.0,
    colors=range(colorant"darkred", colorant"deepskyblue"; length=256)
  )
  if length(transmissionloss) > 0
    size(transmissionloss) == size(receivers) || throw(ArgumentError("Mismatched receivers and transmissionloss"))
    receivers isa AcousticReceiverGrid2D || throw(ArgumentError("Receivers must be an instance of AcousticReceiverGrid2D"))
    minloss = minimum(transmissionloss)
    clims --> (-minloss-dynamicrange, -minloss)
    colorbar --> true
    cguide --> "dB"
    @series begin
      seriestype := :heatmap
      receivers.xrange, receivers.zrange, -transmissionloss'
    end
  end
  xmin = Inf64
  xmax = -Inf64
  length(rays) > 0 && (xmin = min(xmin, minimum(minimum(r1[1] for r1 ∈ r.raypath) for r ∈ rays)))
  length(rays) > 0 && (xmax = max(xmax, maximum(maximum(r1[1] for r1 ∈ r.raypath) for r ∈ rays)))
  length(sources) > 0 && (xmin = min(xmin, minimum(p[1] for p ∈ location.(sources))))
  length(sources) > 0 && (xmax = max(xmax, maximum(p[1] for p ∈ location.(sources))))
  length(receivers) > 0 && (xmin = min(xmin, minimum(p[1] for p ∈ location.(receivers))))
  length(receivers) > 0 && (xmax = max(xmax, maximum(p[1] for p ∈ location.(receivers))))
  isinf(xmin) && (xmin = 0.0)
  isinf(xmax) && (xmax = 1000.0)
  xmin -= xmargin
  xmax += xmargin
  xrange = range(xmin, xmax; length=1000)
  ticks --> :native
  legend --> false
  xguide --> "x (m)"
  yguide --> "z (m) "
  z = altimetry(env)
  @series begin
    seriestype := :line
    linecolor := :royalblue
    xrange, [altitude(z, x, 0.0) for x ∈ xrange]
  end
  z = bathymetry(env)
  @series begin
    seriestype := :line
    linecolor := :brown
    xrange, [-depth(z, x, 0.0) for x ∈ xrange]
  end
  if length(rays) > 0
    reverse!(rays)
    ampl = [r.phasor === missing ? -(r.surface + 3*r.bottom) : amp2db(abs.(r.phasor)) for r ∈ rays]
    ampl .-= minimum(ampl)
    if maximum(ampl) > 0.0
      cndx = round.(Int, 1 .+ (length(colors)-1) .* ampl ./ maximum(ampl))
    else
      cndx = ones(Int, length(ampl)) .* length(colors)
    end
    for (j, eigenray) ∈ enumerate(rays)
      r = eigenray.raypath
      @series begin
        seriestype := :line
        linecolor := colors[cndx[j]]
        [r[i][1] for i ∈ 1:length(r)], [r[i][3] for i ∈ 1:length(r)]
      end
    end
  end
  if length(sources) > 0
    @series begin
      seriestype := :scatter
      marker := :star
      seriescolor := :red
      [p[1] for p ∈ location.(sources)], [p[3] for p ∈ location.(sources)]
    end
  end
  if length(receivers) > 0 && length(transmissionloss) == 0
    @series begin
      seriestype := :scatter
      seriescolor := :blue
      [p[1] for p ∈ location.(receivers)], [p[3] for p ∈ location.(receivers)]
    end
  end
end

@recipe function plot(ssp::SoundSpeedProfile; maxdepth=missing)
  D = 10.0
  if maxdepth === missing
    ssp isa SampledSSP && (D = maximum(-ssp.z))
  else
    D = maxdepth
  end
  d = 0.0:-0.1:-D
  c = [soundspeed(ssp, 0.0, 0.0, d1) for d1 ∈ d]
  clim = extrema(c)
  if clim[2] - clim[1] > 50.0
    clim = (round(clim[1]-5.0), round(clim[2]+5.0))
  else
    μ = (clim[2] + clim[1]) / 2.0
    clim = (round(μ - 30.0), round(μ + 30.0))
  end
  ticks --> :native
  legend --> false
  xlims --> clim
  xguide --> "soundspeed (m/s)"
  yguide --> "z (m) "
  @series begin
    c, d
  end
end


text:
"""
This package defines a `FBD` type to represent the FBD model, and provides a set of methods to access its properties.
In order to generate an instance of `FBD`, use the following command, where the description of the arguments
and keywords is given below.
```julia
pa=FBD(ntg, nt, nr, nts; dobs, gobs, sobs)
```
# Arguments
Let `(ntg,nr)=size(g)`, `(nt,nr)=size(d)` and `(nts,)=size(s)`, where

**`ntg`** is input dimension of the channel impulse responses `g`.

**`nt`** is input dimension of the channel outputs `d`.

**`nr`** is the number of receivers or channels.

**`nts`** is input dimension of the source.

# Keywords

**`dobs`** are the channel responses that will be factorized. Alternatively, the user may input the next two keywords i.e.,
`gobs` and `sobs` for synthetic experiments, where `dobs` are internally generated.

**`gobs`** (optional) are the true channel impulse responses, stored in `pa`.

**`sobs`** (optional) similarly, it is the true source.
"""
mutable struct FBD{T}
	pfibd::IBD{T}
	pfpr::FPR
	plsbd::BD{T}
	g::Matrix{T}
	gxcorr::Matrix{T}
	s::Vector{T}
	sa::Vector{T}
end

function FBD(ntg, nt, nr, nts;
	       dobs=nothing,
	       gobs=nothing,
	       sobs=nothing,
	       sxp=Sxparam(1,:positive),
	       fmin=0.0,
	       fmax=0.5,
	       size_check=false,
	       )
	pfibd=IBD(ntg, nt, nr, nts, gobs=gobs, dobs=dobs, sobs=sobs,
		  fft_threads=true, fftwflag=FFTW.MEASURE,
		  verbose=false, sxp=sxp,
		  sx_fix_zero_lag_flag=true, fmin=fmin, fmax=fmax, size_check=size_check);

	pfpr=FPR(ntg, nr, )

	plsbd=BD(ntg, nt, nr, nts, dobs=dobs, gobs=gobs, sobs=sobs,
	  sxp=sxp,
		 fft_threads=true, verbose=false, fftwflag=FFTW.MEASURE, size_check=size_check);

	return FBD(pfibd, pfpr, plsbd, plsbd.optm.cal.g, pfibd.optm.cal.g,
	      plsbd.optm.cal.s,	pfibd.optm.cal.s)

end

"""
One can use the `lsbd!` method to perform LSBD over a given instance of `FBD` i.e., `pa`.
LBSD is a least-squares fitting of `d` to jointly optimize the `g` and `s`, which can be accessed via
`pa[:g]` and `pa[:s]`, respectively.
The joint optimization is
carried out using alternating minimization: in one cycle, we fix one array and optimize the other, and then fix the other
and optimize the first.
Several cycles are expected to be performed to reach convergence.
```julia
lsbd!(pa)
heatmap(pa[:g], title="estimated impulse responses from LSBD")
```
"""
function lsbd!(pa::FBD, io=stdout; args...)
	bd!(pa.plsbd, io; args...)
	return nothing
end

function ibd!(pa::FBD, io=stdout; args...)
	ibd!(pa.pfibd, io; args...)
	return nothing
end

"""
One can use the `fibd!` method to perform FIBD over a given instance of `FBD` i.e., `pa`.
FIBD is a least-squares fitting of `xd` to jointly optimize the `xg` and `sa`, which can be accessed via
`pa[:xg]` and `pa[:sa]`, respectively.
The joint optimization is
carried out using alternating minimization.
```julia
fibd!(pa)
heatmap(pa[:xg], title="estimated interferometric impulse responses from FIBD")
```
"""
function fibd!(pa::FBD, io=stdout)
	fibd_tol=[1e-10,1e-6]
	initialize!(pa.pfibd)
	fibd!(pa.pfibd, io, α=[Inf],tol=[fibd_tol[1]])
	fibd!(pa.pfibd, io, α=[0.0],tol=[fibd_tol[2]])

	# input g from fibd to fpr
	gobs = (iszero(pa.pfibd.om.g)) ? nothing : pa.pfibd.om.g # choose gobs for nearest receiver or not?
	update_cymat!(pa.pfpr; cymat=pa.pfibd.optm.cal.g, gobs=gobs)
	return nothing
end

"""
After performing FIBD on a `FBD` instance `pa`, we can perform FPR to complete FBD.
These two code blocks should be equivalent.
```julia
fibd!(pa)
fpr!(pa)
```
```julia
fbd!(pa)
```
The result of FPR i.e, `g` can be extracted using `pa[:g]`.
The corresponding source signature is stored in `pa[:s]`.
"""
function fpr!(pa::FBD)
	# input g from fibd to fpr
	gobs = (iszero(pa.pfibd.om.g)) ? nothing : pa.pfibd.om.g # choose gobs for nearest receiver or not?
	update_cymat!(pa.pfpr; cymat=pa.pfibd.optm.cal.g, gobs=gobs)

	# perform fpr
	g=pa.plsbd.optm.cal.g
	Random.randn!(g)

	fill!(pa.pfpr.g, 0.0)
	#update_f_index_loaded!(pa.pfpr)
	fpr!(g,  pa.pfpr,
        precon=[:focus, :pr],
        #precon=[:pr],
	#index_loaded=pa.pfpr.index_loaded,
        index_loaded=1,
			show_trace=true, g_tol=1e-4)

	# update S
	update!(pa.plsbd, pa.plsbd.sx.x, S(), optS)
	return nothing
end

"""
Perform FIBD and FPR to complete FBD of an instance of `FBD` i.e., `pa`.
```julia
fbd!(pa)
plot(pa[:g], title="estimated impulse responses using FBD")
plot(pa[:s], title="estimated source using FBD")
```
"""
function fbd!(pa::FBD, io=stdout; tasks=[:restart, :fibd, :fpr])

	if(:restart ∈ tasks)
		# initialize
		initialize!(pa.pfibd)
	end

	if(:fibd ∈ tasks)
		fibd!(pa)
	end

	if(:fpr ∈ tasks)
		fpr!(pa)
	end

	#=
	if(:updateS ∈ tasks)
		# update source according to the estimated g from fpr
		if(STF_FLAG)
			update_stf!(pa.plsbd)
		else
			update!(pa.plsbd, pa.plsbd.sx.x, S(), optS)
		end
	end
	=#

	# regular lsbd: do a few more AM steps? might diverge..
	#if(:lsbd ∈ tasks)
	#	bd!(pa.plsbd, io; tol=1e-5)
	#end

	return nothing
end

function random_problem()

	ntg=3
	nr=50
	tfact=10
	gobs=randn(ntg, nr)
	nt=ntg*tfact
	nts=nt-ntg+1;
	sobs=randn(nts)
	sxp=Sxparam(1,:positive)
	if(STF_FLAG)
		sobs=abs.(sobs);
	end
	return FBD(ntg, nt, nr, nts, gobs=gobs, sobs=sobs,sxp=sxp)
end

function simple_problem()

	ntg=30
	nr=20
	tfact=20
	gobs=zeros(ntg, nr)
	toy_direct_green!(gobs, c=4.0, bfrac=0.20, afrac=1.0);
	#toy_direct_green!(gobs, c=4.0, bfrac=0.4, afrac=1.0);
	toy_reflec_green!(gobs, c=1.5, bfrac=0.35, afrac=-0.6);
	#toy_reflec_green!(gobs, c=1.5, bfrac=0.35, afrac=1.0);
	nt=ntg*tfact
	nts=nt-ntg+1;
	sobs=(randn(nts));
	return FBD(ntg, nt, nr, nts, gobs=gobs, sobs=sobs,sxp=Sxparam(1,:positive))
end

function simple_bandlimited_problem(fmin=0.1, fmax=0.4)
	pa=simple_problem()

	dobs=pa.plsbd.om.d
	responsetype = Bandpass(fmin,fmax; fs=1)
	designmethod = Butterworth(8)
	# zerophase filter please
	dobs_filt=DSP.Filters.filtfilt(digitalfilter(responsetype, designmethod), dobs)

	# filter dobs
	pom=pa.plsbd.om
	return FBD(pom.ntg, pom.nt, pom.nr, pom.nts, dobs=dobs_filt,
	    gobs=pom.g, sobs=pom.s,sxp=Sxparam(1,:positive), fmin=fmin, fmax=fmax)

end

#Source Time Functions are always positive
function simple_STF_problem()

	ntg=30
	nr=20
	tfact=20
	gobs=zeros(ntg, nr)
	toy_direct_green!(gobs, c=4.0, bfrac=0.20, afrac=1.0);
	#toy_direct_green!(gobs, c=4.0, bfrac=0.4, afrac=1.0);
	toy_reflec_green!(gobs, c=1.5, bfrac=0.35, afrac=-0.6);
	#toy_reflec_green!(gobs, c=1.5, bfrac=0.35, afrac=1.0);
	nt=ntg*tfact
	nts=nt-ntg+1;
	sobs=abs.(randn(nts));
	return FBD(ntg, nt, nr, nts, gobs=gobs, sobs=sobs, sxp=Sxparam(2,:positive))
end


text:
# AVertexMap{T} defined in newtwork_interface.jl
valtype(m::AVertexMap{T}) where {T} = T
eltype(m::AVertexMap{T}) where {T} = T

"""
    mutable struct VertexMap{G <: AGraphOrDiGraph, T, D} <: AVertexMap{T}
        g::G
        vtype::Type{T}
        data::D
    end

Type implementing a vertex map. The underlying container `data` can be a
dictionary or a vector.

    VertexMap{T}(g, ::Type{T})

Returns a map that associates values of type `T` to the vertices of  graph `g`.
The underlying storage structures is chosen accordingly.

    VertexMap(g, data)

Construct a VertexMap with `data` as underlying storage.

    VertexMap(g, f)

Construct a vertex map with value `f(u)` for `u in 1:nv(g)`.
"""
mutable struct VertexMap{G<:AGraphOrDiGraph, T, D} <: AVertexMap{T}
    g::G
    vtype::Type{T}
    data::D
end
# show{G,T,D}(io::IO, m::VertexMap{G,T,D}) = print(io, "VertexMap: $(m.data)")

VertexMap(g::AGraphOrDiGraph, d::AbstractVector{T}) where {T} = VertexMap(g, T, d)
VertexMap(g::AGraphOrDiGraph, d::Dict{V, T}) where {V<:Integer,T} = VertexMap(g, T, d)

function VertexMap(g::AGraphOrDiGraph, ::Type{T}) where T
    V = vertextype(g)
    return VertexMap(g, T, Dict{V,T}())
end

function VertexMap(g::AGraphOrDiGraph, f::Function)
    data = f.(vertices(g))
    return VertexMap(g, eltype(data), data)
end
length(m::VertexMap) = length(m.data)
getindex(m::VertexMap, i::Integer) = getindex(m.data, i)
setindex!(m::VertexMap, x, i::Integer) = setindex!(m.data, x, i)
haskey(m::VertexMap{G,T,D}, i::Integer) where {G,T,D<:AbstractVector} =
    1 <= i <= length(m)
haskey(m::VertexMap{G,T,D}, i::Integer) where {G,T,D<:Dict} =
    haskey(m.data, i)
get(m::VertexMap, i::Integer, x) = get(m.data, i, x)

==(m1::VertexMap, m2::VertexMap) = m1.data == m2.data

Base.Vector(vmap::VertexMap) = [vmap[i] for i=1:nv(vmap.g)]

"""
    struct ConstVertexMap{T} <: AVertexMap{T}
        val::T
    end

A type representing a constant vector map.
Any attempt to change the internal value, e.g. `vm[1] = 4`, will
fail silently.
"""
struct ConstVertexMap{T} <: AVertexMap{T}
    val::T
end

length(d::ConstVertexMap) = typemax(Int)
getindex(d::ConstVertexMap, i::Integer) = d.val
setindex!(d::ConstVertexMap, x, i::Integer) = nothing
get(d::ConstVertexMap, i::Integer, x) = d.val
haskey(v::ConstVertexMap, i::Integer) = true


text:
# Function that performes the calculation: takes data from the input channel and puts it on the output channel
function rcell(x::T where T<:j4pr.AbstractCell, ci::S, co::S) where S<:Union{Channel, RemoteChannel}

	while true
		# Take data and perform processing
		try
			data = take!(ci)
			put!(co, data |> x)
		catch exception
			if (exception isa InterruptException )					# Task received an interrupt
				println("InterruptException() caught, exiting.")
				break
			end

			if (exception isa InvalidStateException ) 				# Task could not write/read channels
				println("InvalidStateException() caught, exiting.") 		# probably because another task/process
				break  								# closed them.
			end

			if (exception isa RemoteException ) 					# Task could not write/read channels
				println("RemoteException() caught, exiting.") 			# probably because another task/process
				break  								# closed them.
			end

			@show exception
			println("Could not execute operation")
		end #try

		# Loop exit condition
	end #while

end

# Small interrupt function that acts on the result of the @async_cell and @remote_cell macros
function interrupt(t::Tuple{T,S1,S2} where T<:Task where S1<:Channel where S2<:Channel)

	# Map tuple to local vars
	task,ic,oc = t

	# Check that the channels are open
	@assert isopen(ic) && isopen(oc)
	try
		@schedule Base.throwto(task, InterruptException())
		return true
	catch
		println("Could not interrupt $(task).")
		return false
	end

end

function interrupt(t::Tuple{Int,S1,S2} where S1<:RemoteChannel where S2<:RemoteChannel)

	# Map tuple to local vars
	id,ic,oc = t

	try
		interrupt(id)
		try
			close(ic);
			close(oc);
		catch
			println("Could not close channels.")
			return false
		end
		return true
	catch
		println("Could not interrupt process on worker $(id).")
		return false
	end
end

# Macro for asynchronous cell execution in the current process
"""
	@async_cell x

The macro generates two unbuffered channels, for input and output, and an starts a task that
waits for data from the input channel, passes it through `x` and puts the result
on the ouput channel.
"""
macro async_cell(x)
	ex=quote begin
		local ci = Channel(0) 								# Input channel
		local co = Channel(0)								# Output channel

		local t = @async j4pr.rcell($x, ci, co)						# Call the cell server function asynchronously (e.g. waits for data)

		# Bind channels (when interrupting task, they will automatically close)
		bind(ci,t)
		bind(co,t)
		(t,ci,co)									# Return the channels and termination condition
	end # local block

	end #quote

	return esc(ex)
end

# Macro for asynchronous cell execution in the current process
# that supports specifying the input/output channels as well
"""
	@async_cell x ci co

The macro uses the channels `ci` and `co` as input and output and starts a task that
waits for data from the input channel, passes it through `x` and puts the result
on the ouput channel.
"""
macro async_cell(x, ci, co)
	ex=quote begin
		# Basic checks
		@assert $ci isa Channel && isopen($ci)
		@assert $co isa Channel && isopen($co)

		local t = @async j4pr.rcell($x, $ci, $co)					# Call the cell server function asynchronously (e.g. waits for data)

		# Bind channels (when interrupting task, they will automatically close)
		bind(ci,t)
		bind(co,t)
		(t,$ci,$co)									# Return the channels and termination condition
	end # local block
	end #quote

	return esc(ex)
end

# Macro for asynchronous cell execution on a remote process
"""
	@remote_cell id x

The macro generates two unbuffered remote channels, for input and output, and starts a task
on worker `id` that waits for data from the input channel, passes it through `x` and puts
the result on the ouput channel.
"""
macro remote_cell(id::Int, x)
	ex=quote begin
		local ci = RemoteChannel(()->Channel(0) )					# Input channel
		local co = RemoteChannel(()->Channel(0) )					# Output channel
		local worker 									# Variable that holds the id of the worker
		if !($(id) in workers()) 							# Check if the specified worker exists
			error("[@remote_cell] Worker $($(id)) not present.")
		else
			worker = $(id)
		end
		local t = @async remote_do(j4pr.rcell, worker, $x, ci, co)			# Start on a remote worker the cell server
		(worker,ci,co)									# Return the channels and termination condition
	end # local block
	end #quote

	return esc(ex)
end

# Macro for asynchronous cell execution on a remote process
# that supports specifying the input/output channels as well
"""
	@remote_cell id x ci co

The macro uses the two remote channels `ci` and `co` as input and output
and starts a task on worker `id` that waits for data from the input channel,
passes it through `x` and puts the result on the ouput channel.
"""
macro remote_cell(id::Int, x, ci, co)
	ex=quote begin
		# Basic checks
		@assert $ci isa RemoteChannel
		@assert $co isa RemoteChannel

		local worker 									# Variable that holds the id of the worker
		if !($(id) in workers()) 							# Check if the specified worker exists
			error("[@remote_cell] Worker $($(id)) not present.")
		else
			worker = $(id)
		end

		local t = @async remote_do(j4pr.rcel, worker, $x, $ci, $co)			# Start on a remote worker the cell server
		(worker,$ci,$co)								# Return the channels and termination condition
	end # local block
	end #quote

	return esc(ex)
end

# Experimental stuff
function rcell(x::T where T<:j4pr.AbstractCell, ci::Vector{S}, co::Vector{S}) where S<:Union{Channel, RemoteChannel}

	while true
		# Take data and perform processing
		try
			if length(ci) == 1
				data = take!(ci[1])
			else
				data = Any[take!(c) for c in ci]
			end
			r = data |> x
			for c in co
				@async put!(c, r)
			end
		catch exception
			if (exception isa InterruptException )					# Task received an interrupt
				println("InterruptException() caught, exiting.")
				break
			end

			if (exception isa InvalidStateException ) 				# Task could not write/read channels
				println("InvalidStateException() caught, exiting.") 		# probably because another task/process
				break  								# closed them.
			end

			if (exception isa RemoteException ) 					# Task could not write/read channels
				println("RemoteException() caught, exiting.") 			# probably because another task/process
				break  								# closed them.
			end

			@show exception
			println("Could not execute operation")
		end #try

		# Loop exit condition
	end #while
end

macro async_cell_exp(x, ci, co)
	ex=quote begin
		# Basic checks
		local t = @async j4pr.rcell($x, $ci, $co)					# Call the cell server function asynchronously (e.g. waits for data)
	end # local block
	end #quote

	return esc(ex)
end


text:
@symbol_func function omega_ce(cur_reactor::AbstractReactor, cur_rho::AbstractSymbol)
  cur_omega = 1.76e11

  cur_omega *= B_rho(cur_reactor, cur_rho, cur_reactor.wave_theta)

  cur_omega
end


text:
## Transform

abstract type TransformedObject <: Object end

export AffineTransform
struct AffineTransform <: TransformedObject
    object :: Object
    matrix :: Array{Float64,2}
    vector :: Array{Float64,1}
    function AffineTransform(object,matrix,vector)
        if object == Empty()
            Empty()
        elseif (rank(matrix) ≤ 1)
            Empty()
        else
            new(object,matrix,vector)
        end
    end
    function AffineTransform(object::Object, matrix::RealMatrix; fixedpoint=[0.0,0.0,0.0])
        vector = fixedpoint - matrix*fixedpoint
        return AffineTransform(object,matrix,vector)
    end
end

export ParallelTranslation
struct ParallelTranslation <: TransformedObject
    object :: Object
    vector :: Array{Float64,1}
    function ParallelTranslation(object,vector)
        if object == Empty()
            Empty()
        else
            new(object,vector)
        end
    end
end

export Scaling
struct Scaling <: TransformedObject
    object :: Object
    scalar :: Float64
    function Scaling(object, scalar; fixedpoint=[0.0,0.0,0.0])
        if object == Empty()
            Empty()
        elseif scalar == 0
            Empty()
        elseif norm(fixedpoint) ≠ 0
            new(ParallelTranslation(object,fixedpoint*(1-scalar)/scalar),scalar)
        else
            new(object,scalar)
        end
    end
end

function povray_script(affinetransform :: AffineTransform; preindent = 0)
    script = "object{\n" * "  "^(preindent+1)
    script *= povray_script(affinetransform.object, preindent=preindent+1) * "\n" * "  "^(preindent+1)
    script *= "matrix " * povray_script(vcat(reshape(affinetransform.matrix,9),affinetransform.vector)) * "\n" * "  "^preindent
    script *= "}"
    return script
end

function povray_script(paralleltranslation :: ParallelTranslation; preindent = 0)
    script = "object{\n" * "  "^(preindent+1)
    script *= povray_script(paralleltranslation.object, preindent=preindent+1) * "\n" * "  "^(preindent+1)
    script *= "translate "*povray_script(paralleltranslation.vector) * "\n" * "  "^preindent
    script *= "}"
    return script
end

function povray_script(scaling :: Scaling; preindent = 0)
    script = "object{\n" * "  "^(preindent+1)
    script *= povray_script(scaling.object, preindent=preindent+1) * "\n" * "  "^(preindent+1)
    script *= "scale "*povray_script(scaling.scalar) * "\n" * "  "^preindent
    script *= "}"
    return script
end

export Rotate
function Rotate(object::Object, v::RealVector, θ::Real; fixedpoint::RealVector=[0.0,0.0,0.0])
    return AffineTransform(object,rotatematrix(v,θ),fixedpoint=fixedpoint)
end

export Mirror
function Mirror(object::Object, p1::RealVector, p2::RealVector, p3::RealVector)
    e₃ = NormalVector(p1,p2,p3)
    e₁ = OrthogonalVector(e₃)
    e₂ = cross(e₃,e₁)
    A = hcat(e₁,e₂,e₃)*transpose(hcat(e₁,e₂,-e₃))
    return AffineTransform(object,A,fixedpoint=(p1+p2+p3)/3)
end

max_stars_repo_name: hyrodium/JuliRay

max_issues_repo_name: hyrodium/JuliRay

max_forks_repo_name: hyrodium/JuliRay


text:
include("solver_preamble.jl")
import SDPNAL
factory = with_optimizer(SDPNAL.Optimizer, printlevel=0, tol=1e-4)
config = MOI.Test.TestConfig(atol=5e-3, rtol=5e-3, query=false)
@testset "Linear" begin
    Tests.linear_test(factory, config, [
        # Expression: ≈(JuMP.objective_value(model), expected, atol=atol, rtol=rtol)
        # Evaluated: 142.57342982938655 ≈ 132.63 (atol=0.05, rtol=0.05)
        "dsos_options_pricing",
        # Infeasible not supported
        "dsos_horn"
    ])
end
@testset "SDP" begin
    Tests.sd_test(factory, config, [
        # MathOptInterface.NUMERICAL_ERROR == MathOptInterface.OPTIMAL
        "chebyshev",
        # SDPNAL supports LessThan on Nonnegatives variables but cannot
        # create free variables. See
        # https://github.com/JuliaOpt/MathOptInterface.jl/issues/987
        "sosdemo5_feasible",
        # Expression: ≈(JuMP.objective_value(model), expected, atol=atol, rtol=rtol)
        # Evaluated: 20.161198836088243 ≈ 17.17 (atol=0.05, rtol=0.05)
        "sos_options_pricing",
        # Infeasible not supported
        "quartic_ideal_rem",
        "quartic_ideal_2_rem",
        "sos_horn",
        "maxcut",
        "choi",
        "motzkin",
        "quadratic_infeasible_lyapunov_switched_system",
        "quadratic_infeasible_scaled_lyapunov_switched_system",
        "sosdemo5_infeasible",
        "quartic_infeasible_lyapunov_switched_system",
        "quartic_infeasible_scaled_lyapunov_switched_system"
    ])
end


text:
#
# public.jl -
#
# Implement high-level interface to Nüvü Camēras.
#

#------------------------------------------------------------------------------
# CONTROLLER LIST METHODS

ControllerList(basic::Bool = false) =
    ControllerList(NC.open(NC.CtrlList, basic))

function Base.close(lst::ControllerList)
    lst.cnt = 0
    handle = lst.handle
    if handle.ptr != C_NULL
        lst.handle = NC.CtrlList(C_NULL)
        NC.close(handle)
    end
end

Base.length(lst::ControllerList) = lst.cnt

"""
```julia
getfreeportunit(lst, i)
```

yields the board unit for the `i`-th unused port in list of controllers `lst`.

"""
getfreeportunit

for p in (:getPortUnit, :getPortChannel, :getFreePortUnit, :getFreePortChannel,
          :getFreePortReason, :getSerial, :getModel, :getPortInterface,
          :getUniqueID, :getDetectorType, :getFreePortInterface,
          :getFreePortUniqueID, :getPluginName, :getFullSizeSize,
          :getDetectorSize)
    f = Symbol(lowercase(string(p)))
    @eval function $f(lst::ControllerList, i::Integer)
        @assert 1 ≤ i ≤ lst.cnt "out of range index"
        return NC.$p(lst.handle, i - 1)
    end
end

for p in (:getFreePortCount, :getPluginCount)
    f = Symbol(lowercase(string(p)))
    @eval $f(lst::ControllerList, i::Integer) = NC.$p(lst.handle)
end

#------------------------------------------------------------------------------
# CAMERA METHODS

Camera(unit::Integer, chnl::Integer, nbufs::Integer = 4) =
    Camera(NC.open(NC.Cam, unit, chnl, nbufs))

Camera(ctrl::ControllerList, idx::Integer, nbufs::Integer = 4) =
    Camera(NC.open(NC.Cam, ctrl.handle, idx, nbufs))

function Base.close(cam::Camera)
    handle = cam.handle
    if handle.ptr != C_NULL
        cam.handle = NC.Cam(C_NULL)
        NC.close(handle)
    end
end

#------------------------------------------------------------------------------
# FRAME GRABBER METHODS

FrameGrabber(unit::Integer, chnl::Integer, nbufs::Integer) =
    FrameGrabber(NC.open(NC.Grab, unit, chnl, nbufs))

FrameGrabber(ctrl::ControllerList, index::Integer, nbufs::Integer) =
    FrameGrabber(NC.open(NC.Grab, ctrl.handle, index, nbufs))

function Base.close(grb::FrameGrabber)
    handle = grb.handle
    if handle.ptr != C_NULL
        grb.handle = NC.Grab(C_NULL)
        NC.close(handle)
    end
end


text:
"""
Fits the feature allocation model (FAM) as shown in the paper via MCMC.

# Arguments <defaults>:

- `init::State`: Inital state.

- `c::Constants`: Model constants.

- `d::Data`: Model data.

- `nmcmc::Int`<1000>: Number of mcmc iterations to do post burn-in.
   If `thins` is not the default, the number of samples obtained for each
   `monitor` may not be `nmcmc`.

- `nburn::Int`<1000>: Number of iterations for burn in.

- `monitors::Vector{Vector{Symbol}}`
   <[[:Z, :lam, :W], :v, :sig2, :delta, :alpha, :eta, :eps]]>:
   A list of monitors, where each monitor is a list of symbols.  If a symbol
   appears in a monitor, it is stored and returned at the end of the MCMC. The
   symbols have to correspond to fields in the `State` object.

- `fix::Vector{Symbol}`<[]>: A list of parameters (which should be a field in the
   `State` object) to fix at the initial value. (i.e.  if :Z appears in `fix`,
   then Z is never updated.) This is for debugging purposes.

- `thins::Vector{Int}<[1]>`: A list of integers with the same length as
   monitors. The first integer is the factor to thin the first group of
   parameters (monitor). So if `thins[1]`=2 and `nmcmc`=2000, then
   1000=`nmcmc/thins[1]` samples will be obtained for `monitors[1]`.

- `thin_dden::Int`<1>: Samples of density-estimates of the data are stored.
   This argument is to thin the samples.

- `printFreq::Int`<0>: Print frequency. Defaults to 0 => prints every 10%.
  turn off printing by setting to -1. If `printFreq` = 2, loglikelihood and
  time will be printed every 2 mcmc iterations.

- `computeDIC::Bool`<false>: Whether to compute DIC.

- `computeLPML::Bool`:<false>: Whether to compute LPML.

- `computedden::Bool`<false>: Whether to compute data density estimates.

- `sb_ibp::Bool`<false>: Whether to use the stick-break construction of IBP.
  If `false`, the regular IBP is used, and a Gibbs step is available.
  Otherwise, an anto-tuned metropolis step is used.

- `use_repulsive::Bool`<false>: Whether to use the repulsive FAM.

- `Z_marg_lamgam::Bool`<true>: Whether to marginalize over (lambda, gamma)
   when updating Z.

- `verbose::Bool`<1>: Verbosity of output. 0 for bare minimum to none.
  1 for basic output. Greater than 1 for full on debug mode.
"""
function cytof_fit(init::State, c::Constants, d::Data;
                   nmcmc::Int=1000, nburn::Int=1000,
                   monitors=[[:Z, :lam, :W, :v,
                              :sig2, :delta, :alpha,
                              :eta, :eps]],
                   fix::Vector{Symbol}=Symbol[],
                   thins::Vector{Int}=[1],
                   thin_dden::Int=1,
                   printFreq::Int=0,
                   computeDIC::Bool=false, computeLPML::Bool=false,
                   computedden::Bool=false,
                   sb_ibp::Bool=false,
                   use_repulsive::Bool=false, Z_marg_lamgam::Bool=false,
                   verbose::Int=1)

  if verbose >= 1
    fixed_vars_str = join(fix, ", ")
    if fixed_vars_str == ""
      fixed_vars_str = "nothing"
    end
    printlnflush("fixing: $fixed_vars_str")
    printlnflush("Use stick-breaking IBP: $(sb_ibp)")
  end

  @assert printFreq >= -1
  if printFreq == 0
    numPrints = 10
    printFreq = Int(ceil((nburn + nmcmc) / numPrints))
  end

  y_tuner = begin
    dict = Dict{Tuple{Int, Int, Int}, MCMC.TuningParam{Float64}}()
    for i in 1:d.I
      for n in 1:d.N[i]
        for j in 1:d.J
          if d.m[i][n, j] == 1
            dict[i, n, j] = MCMC.TuningParam(1.0)
          end
        end
      end
    end
    dict
  end

  tuners = Tuners(y_imputed=y_tuner, # yinj, for inj s.t. yinj is missing
                  Z=MCMC.TuningParam(MCMC.sigmoid(c.probFlip_Z, a=0.0, b=1.0)),
                  v=[MCMC.TuningParam(1.0) for k in 1:c.K])

  # Loglike
  loglike = Float64[]

  function printMsg(iter::Int, msg::String)
    if printFreq > 0 && iter % printFreq == 0
      printflush(msg)
    end
  end

  # Instantiate (but not initialize) CPO stream
  if computeLPML
    cpoStream = MCMC.CPOstream{Float64}()
  end

  # DIC
  if computeDIC
    local tmp = DICparam(p=deepcopy(d.y),
                         mu=deepcopy(d.y),
                         sig=[zeros(Float64, d.N[i]) for i in 1:d.I],
                         y=deepcopy(d.y))
    dicStream = MCMC.DICstream{DICparam}(tmp)

    loglikeDIC(param::DICparam) = computeLoglikeDIC(d, param)

    # FIXME: Doesn't work for c.noisyDist not Normal
    function convertStateToDicParam(s::State)::DICparam
      p = [[prob_miss(s.y_imputed[i][n, j], c.beta[:, i])
            for n in 1:d.N[i], j in 1:d.J] for i in 1:d.I]

      mu = [[s.lam[i][n] > 0 ? mus(i, n, j, s, c, d) : 0.0
             for n in 1:d.N[i], j in 1:d.J] for i in 1:d.I]

      sig = [[s.lam[i][n] > 0 ? sqrt(s.sig2[i]) : std(c.noisyDist)
              for n in 1:d.N[i]] for i in 1:d.I]

      y = deepcopy(s.y_imputed)

      return DICparam(p, mu, sig, y)
    end
  end

  dden = Matrix{Vector{Float64}}[]

  function update!(s::State, iter::Int, out)
    update_state!(s, c, d, tuners, loglike, fix, use_repulsive,
                  Z_marg_lamgam, sb_ibp)

    if computedden && iter > nburn && (iter - nburn) % thin_dden == 0
      append!(dden,
              [[datadensity(i, j, s, c, d) for i in 1:d.I, j in 1:d.J]])
    end

    if computeLPML && iter > nburn
      # Inverse likelihood for each data point
      like = [[compute_like(i, n, s, c, d) for n in 1:d.N[i]] for i in 1:d.I]

      # Update (or initialize) CPO
      MCMC.updateCPO(cpoStream, vcat(like...))

      # Add to printMsg
      printMsg(iter, " -- LPML: $(MCMC.computeLPML(cpoStream))")
    end

    if computeDIC && iter > nburn
      # Update DIC
      MCMC.updateDIC(dicStream, s, updateParams,
                     loglikeDIC, convertStateToDicParam)

      # Add to printMsg
      printMsg(iter, " -- DIC: $(MCMC.computeDIC(dicStream, loglikeDIC,
                                                 paramMeanCompute))")
    end

    printMsg(iter, "\n")
  end

  if isinf(compute_loglike(init, c, d, normalize=true))
    printlnflush("Warning: Initial state yields likelihood of zero.")
    msg = """
    It is likely the case that the initialization of missing values
    is not consistent with the provided missing mechanism. The MCMC
    will almost certainly reject the initial values and sample new
    ones in its place.
    """
    printlnflush(join(split(msg), " "))
  else
    printlnflush()
  end

  out, lastState = MCMC.gibbs(init, update!, monitors=monitors,
                              thins=thins, nmcmc=nmcmc, nburn=nburn,
                              printFreq=printFreq,
                              loglike=loglike,
                              printlnAfterMsg=!(computeDIC || computeLPML))

  # Create dictionary to store results
  results = Dict{Symbol, Any}()

  results[:out] = out  # monitors
  results[:lastState] = lastState
  results[:loglike] = loglike
  results[:c] = c
  results[:init] = init

  if computeDIC || computeLPML
    LPML = computeLPML ? MCMC.computeLPML(cpoStream) : NaN
    Dmean, pD = computeDIC ? MCMC.computeDIC(dicStream,
                                             loglikeDIC,
                                             paramMeanCompute,
                                             return_Dmean_pD=true) : (NaN, NaN)
    metrics = Dict(:LPML => LPML,
                   :DIC => Dmean + pD,
                   :Dmean => Dmean,
                   :pD => pD)
    printlnflush()
    printlnflush("metrics:")
    for (k, v) in metrics
      printlnflush("$k => $v")
    end

    results[:metrics] = metrics
  end

  if computedden
    results[:dden] = dden
  end

  return results
end


text:
using QuantumTimeSteppers
using ApproxFun
using Test

@testset "QuantumTimeSteppers.jl" begin
    H = HermiteFSE()

    v = [0.83;0.78;0.65;1.11]
    trans = QuantumTimeSteppers.plan_transform(H,v)
    itrans = QuantumTimeSteppers.plan_itransform(H,v)
    @test v ≈ itrans*(trans*v)
    @test v ≈ trans*(itrans*v)

    f = x -> (1+x^2)*exp(-x^2 / 2)
    F = Fun(f,H)
    x = 0.78
    @test F(x) ≈ f(x)

    f = x -> cos(10x)*exp(-x^2)
    F = Fun(f,H)
    x = 0.78
    @test F(x) ≈ f(x)

    H = HermiteFSE()
    f = x -> sech(x)^2
    fprime = x -> -2tanh(x)*sech(x)^2
    F = Fun(f,H)
    Fprime = differentiate(F)
    x = 0.78
    @test Fprime(x) ≈ fprime(x)
    @test (Derivative(H,2)*F)(x) ≈ (differentiate(Fprime))(x)

end


text:
using SoilMulch
using Test

@testset "SoilMulch.jl" begin
    # Write your tests here.
    # plant_transpiration(1, 0.15, 0.5, 1, 0.5)
    @test plant_transpiration(1, 0.15, 0.5, 1, 0.5) == 0.5
    # @test soil_leakage(1, 150, 14) == 150
    @test soil_leakage(1, 250, 14) == 250
    @test soil_evaporation(1, 0.15, 1, 0.5) == 0.5
end


text:
using DifferentialEquations
using DelayDiffEq
using DiffEqCallbacks
using Tables
using DataFrames
using StatsPlots
using BenchmarkTools

function sir_dde!(du,u,h,p,t)
    (S,I,R) = u
    (β,c,τ) = p
    N = S+I+R
    infection = β*c*I/N*S
    (Sd,Id,Rd) = h(p, t-τ) # Time delayed variables
    Nd = Sd+Id+Rd
    recovery = β*c*Id/Nd*Sd
    @inbounds begin
        du[1] = -infection
        du[2] = infection - recovery
        du[3] = recovery
    end
    nothing
end;

δt = 0.1
tmax = 40.0
tspan = (0.0,tmax)
t = 0.0:δt:tmax;

u0 = [990.0,10.0,0.0]; # S,I.R

p = [0.05,10.0,4.0]; # β,c,τ

function sir_history(p, t)
    [1000.0, 0.0, 0.0]
end;

function affect_initial_recovery!(integrator)
    integrator.u[2] -= u0[2]
    integrator.u[3] += u0[2]

    reset_aggregated_jumps!(integrator)
end
cb_initial_recovery = DiscreteCallback((u,t,integrator) -> t == p[3], affect_initial_recovery!);

prob_dde = DDEProblem(DDEFunction(sir_dde!),
        u0,
        sir_history,
        tspan,
        p;
        constant_lags = [p[3]]);

alg = MethodOfSteps(Tsit5());

sol_dde = solve(prob_dde,alg, callback=cb_initial_recovery);

df_dde = DataFrame(Tables.table(sol_dde(t)'))
rename!(df_dde,["S","I","R"])
df_dde[!,:t] = t;

@df df_dde plot(:t,
    [:S :I :R],
    label=["S" "I" "R"],
    xlabel="Time",
    ylabel="Number")

function sir_ode_initial!(du,u,p,t)
    (S,I,R) = u
    (β,c,γ) = p
    N = S+I+R
    @inbounds begin
        du[1] = -β*c*I/N*S
        du[2] = β*c*I/N*S
        du[3] = 0
    end
    nothing
end;

prob_ode = ODEProblem(sir_ode_initial!,u0,(0,p[3]),p);
sol_ode = solve(prob_ode);
u1 = sol_ode[end]
u1[2] -= u0[2]
u1[3] += u0[2]

function ode_history(p, t, sol)
    sol(t)
end;
sir_history1(p,t)=ode_history(p,t,sol_ode)

prob_dde1 = DDEProblem(DDEFunction(sir_dde!),
        u1,
        sir_history1,
        (p[3],tmax),
        p;
        constant_lags = [p[3]]);
alg1 = MethodOfSteps(Tsit5());
sol_dde1 = solve(prob_dde1,alg1);

@benchmark solve(prob_dde, alg, callback=cb_initial_recovery)


text:
struct Screen
    window
    renderer
    height::Int
    width::Int
    background::Union{ARGB, Ptr{SDL_Surface}}

    function Screen(name, w, h, background)
        win, renderer = makeWinRenderer(name, w, h)
        new(win, renderer, h, w, to_ARGB(background))
    end
end

#non ARGB colorant is converted to ARGB
#ARGB colorant is rerturned as is
# non colorant is returned as is (required since background is stored as an Union )
to_ARGB(c) = c
to_ARGB(c::ARGB) = c
to_ARGB(c::Colorant) = ARGB(c)

abstract type Geom end

"""
`Rect(x::Int,y::Int,w::Int,h::Int)`
`Rect(x::Tuple, y::Tuple)`

Creates an actor representing a rectangle.
"""
mutable struct Rect <: Geom
    x::Int
    y::Int
    w::Int
    h::Int
end
Rect(x::Tuple, y::Tuple) = Rect(x[1], x[2], y[1], y[2])

import Base:+
+(r::Rect, t::Tuple{T,T}) where T <: Number = Rect(Int(r.x+t[1]), Int(r.y+t[2]), r.h, r.w)

"""
`Line(x1::Int, y1::Int, x2::Int, y2::Int)`
`Line(x::Tuple, y::Tuple)`

Creates an actor representing a line.
"""
mutable struct Line <: Geom
    x1::Int
    y1::Int
    x2::Int
    y2::Int
end

Line(x::Tuple, y::Tuple) = Line(x[1], x[2], y[1], y[2])

mutable struct Triangle <: Geom
  p1::Vector{Int}
  p2::Vector{Int}
  p3::Vector{Int}

  function Triangle(p1::Vector{Int}, p2::Vector{Int}, p3::Vector{Int})
    if !(length(p1) == length(p2) == length(p3) == length(p3) == 2)
      error("Given vectors are of incompatible size.")
    end
    return new(p1, p2, p3)
  end

  function Triangle(x1::Int, y1::Int, x2::Int, y2::Int, x3::Int, y3::Int)
    new([x1; y1], [x2; y2], [x3; y3])
  end

  function Triangle(p1::Tuple, p2::Tuple, p3::Tuple)
    if !(length(p1) == length(p2) == length(p3) == length(p3) == 2)
      error("Given tuples are of incompatible size.")
    end
    return new([p1[1]; p1[2]], [p2[1]; p2[2]], [p3[1]; p3[2]])
  end
end

mutable struct Circle <: Geom
    x::Int
    y::Int
    r::Int
end

Base.convert(T::Type{SDL_Rect}, r::Rect) = SDL_Rect(Cint.((r.x, r.y, r.w, r.h))...)

function Base.setproperty!(s::Geom, p::Symbol, x)
    if hasfield(typeof(s), p)
        setfield!(s, p, Int(round(x)))
    else
        v = getPos(Val(p), s, x)
        setfield!(s, :x, Int(round(v[1])))
        setfield!(s, :y, Int(round(v[2])))
    end
end

function Base.getproperty(s::Geom, p::Symbol)
    if hasfield(typeof(s), p)
        getfield(s, p)
    else
        v = getPos(Val(p), s)
        return v
    end
end

# A rect is stored as x/y/width/height. The following functions
# then calculate other metrics dynamically. Some metrics refer to
# two dimensions (topright); they return tuples. Some other refer to
# a single dimension (top); they return scalars
# The functions are called from `getproperty` for Rect
getPos(::Val{:left}, s::Rect) = s.x
getPos(::Val{:right}, s::Rect) = s.x+s.w
getPos(::Val{:top}, s::Rect) = s.y
getPos(::Val{:bottom}, s::Rect) = s.y+s.h
getPos(::Val{:pos}, s::Rect) = getPos(Val(:topleft), s)
getPos(::Val{:topleft}, s::Rect) = (s.x, s.y)
getPos(::Val{:topright}, s::Rect) = (s.x+s.w, s.y)
getPos(::Val{:bottomleft}, s::Rect) = (s.x, s.y+s.h)
getPos(::Val{:bottomright}, s::Rect) = (s.x+s.w, s.y+s.h)
getPos(::Val{:center}, s::Rect) = (s.x+s.w/2, s.y+s.h/2)
getPos(::Val{:centerx}, s::Rect) = s.x+s.w/2
getPos(::Val{:centery}, s::Rect) =  s.y+s.h/2
getPos(::Val{:centerleft}, s::Rect) = (s.x, s.y+s.h/2)
getPos(::Val{:centerright}, s::Rect) = (s.x+s.w, s.y+s.h/2)
getPos(::Val{:bottomcenter}, s::Rect) = (s.x+s.w/2, s.y+s.h)
getPos(::Val{:topcenter}, s::Rect) = (s.x+s.w/2, s.y)

# The following functions are used to postion a rectangle using various metrics.
# They essentially return the x and y position, given other metrics (eg. bottomleft)
# These functions are called from `setproperty` for Rect
getPos(X::Val, s::Geom, v...) = nothing
getPos(X::Val, s::Geom, v::Tuple) = getPos(X, s, v[1], v[2])

getPos(::Val{:left}, s::Rect, v) = (v, s.y)
getPos(::Val{:right}, s::Rect, v) = (v-s.w, s.y)
getPos(::Val{:top}, s::Rect, v) = (s.x, v)
getPos(::Val{:bottom}, s::Rect, v) = (s.x, v-s.h)
getPos(::Val{:pos}, s::Rect, u, v) = getPos(Val(:topleft), s, u, v)
getPos(::Val{:topleft}, s::Rect, u, v) = (u, v)
getPos(::Val{:topright}, s::Rect, u, v) = (u-s.w, v)
getPos(::Val{:bottomleft}, s::Rect, u, v) = (u, v-s.h)
getPos(::Val{:bottomright}, s::Rect, u, v) = (u-s.w, v-s.h)
getPos(::Val{:center}, s::Rect, u, v) = (u-s.w/2, v-s.h/2)
getPos(::Val{:centerx}, s::Rect, u) = (u-s.w/2, s.y)
getPos(::Val{:centery}, s::Rect, v) = (s.x, v-s.h/2)
getPos(::Val{:centerleft}, s::Rect, u, v) = (u, v-s.h/2)
getPos(::Val{:centerright}, s::Rect, u, v) = (u-s.w, v-s.h/2)
getPos(::Val{:bottomcenter}, s::Rect, u, v) = (u-s.w/2, v-s.h)
getPos(::Val{:topcenter}, s::Rect, u, v) = (u-s.w/2, v)

getPos(::Val{:center}, s::Triangle) = (s.p1 + s.p2 + s.p3) / 3
getPos(::Val{:left}, s::Triangle) = min(s.p1[1], s.p2[1], s.p3[1])
getPos(::Val{:right}, s::Triangle) = max(s.p1[1], s.p2[1], s.p3[1])
getPos(::Val{:top}, s::Triangle) = max(s.p1[2], s.p2[2], s.p3[2])
getPos(::Val{:bottom}, s::Triangle) = min(s.p1[2], s.p2[2], s.p3[2])
getPos(::Val{:centerx}, s::Triangle) = (s.p1[1] + s.p2[1] + s.p3[1]) / 3
getPos(::Val{:centery}, s::Triangle) = (s.p1[2] + s.p2[2] + s.p3[2]) / 3

getPos(::Val{:center}, s::Circle, u, v) = (u, v)
getPos(::Val{:top}, s::Circle, v) = (s.x, v-s.r)
getPos(::Val{:bottom}, s::Circle, v) = (s.x, v+s.r)
getPos(::Val{:left}, s::Circle, u) = (u-s.r, s.y)
getPos(::Val{:right}, s::Circle, u) = (u+s.r, s.y)
getPos(::Val{:centerx}, s::Circle, u) = (u, s.y)
getPos(::Val{:centery}, s::Circle, v) = (s.x, v)

getPos(::Val{:center}, s::Circle) = (s.x, s.y)
getPos(::Val{:top}, s::Circle) = s.y-s.r
getPos(::Val{:bottom}, s::Circle) = s.y+s.r
getPos(::Val{:left}, s::Circle) = s.x-s.r
getPos(::Val{:right}, s::Circle) = s.x+s.r
getPos(::Val{:centerx}, s::Circle) = s.x
getPos(::Val{:centery}, s::Circle) = s.y

function clear(s::Screen)
    fill(s, s.background)
end

clear() = clear(game[].screen)

function Base.fill(s::Screen, c::Colorant)
    SDL_SetRenderDrawColor(
        s.renderer,
        sdl_colors(c)...,
    )
    SDL_RenderClear(s.renderer)
end

function Base.fill(s::Screen, sf::Ptr{SDL_Surface})
    texture = SDL_CreateTextureFromSurface(s.renderer, sf)
    SDL_RenderCopy(s.renderer, texture, C_NULL, C_NULL)
    SDL_DestroyTexture(texture)
end

draw(l::T, args...; kv...) where T <: Geom = draw(game[].screen, l, args...; kv...)

function draw(s::Screen, l::Line, c::Colorant=colorant"black")
    SDL_SetRenderDrawColor(
        s.renderer,
        sdl_colors(c)...,
    )
    SDL_RenderDrawLine(s.renderer, Cint.((l.x1, l.y1, l.x2, l.y2))...)
end

function draw(s::Screen, r::Rect, c::Colorant=colorant"black"; fill=false)
    SDL_SetRenderDrawColor(
        s.renderer,
        sdl_colors(c)...,
    )
    sr = convert(SDL_Rect, r)
    if !fill
        SDL_RenderDrawRect(s.renderer, Ref(sr))
    else
        SDL_RenderFillRect(s.renderer, Ref(sr))
    end
end

sdl_colors(c::Colorant) = sdl_colors(convert(ARGB{Colors.FixedPointNumbers.Normed{UInt8,8}}, c))
sdl_colors(c::ARGB) = Int.(reinterpret.((red(c), green(c), blue(c), alpha(c))))

function draw(s::Screen, tr::Triangle, c::Colorant=colorant"black"; fill=false)
  p1, p2, p3 = Cint.(tr.p1), Cint.(tr.p2), Cint.(tr.p3)
  SDL_SetRenderDrawColor(s.renderer, sdl_colors(c)...)
  SDL_RenderDrawLines(s.renderer, [p1; p2; p3; p1], Cint(4))

  ymax = max(p1[2], p2[2], p3[2])
  ymin = min(p1[2], p2[2], p3[2])
  if fill && ymin != ymax
    # Set q1, q2 and q3 in descending order of y-value
    q1 = (p1[2] != ymax != p2[2]) * p3 +
        (p2[2] != ymax != p3[2]) * p1 +
        (p3[2] != ymax != p1[2]) * p2 +
        (p1[2] == p2[2] == ymax) * p2 +
        (p2[2] == p3[2] == ymax) * p3 +
        (p3[2] == p1[2] == ymax) * p1
    q3 = (p1[2] != ymin != p2[2]) * p3 +
        (p2[2] != ymin != p3[2]) * p1 +
        (p3[2] != ymin != p1[2]) * p2 +
        (p1[2] == p2[2] == ymin) * p2 +
        (p2[2] == p3[2] == ymin) * p3 +
        (p3[2] == p1[2] == ymin) * p1
    q2 = ((q1 == p1 && q3 == p3) || (q1 == p3 && q3 == p1)) * p2 +
        ((q1 == p1 && q3 == p2) || (q1 == p2 && q3 == p1)) * p3 +
        ((q1 == p2 && q3 == p3) || (q1 == p3 && q3 == p2)) * p1

    n = q1[2] - q2[2]
    x0 = q1[1] + (q2[2] - q1[2]) / (q3[2] - q1[2]) * (q3[1] - q1[1])
    for j = Cint(0):n-Cint(1)
      r1 = [round(Cint, q2[1] + j / n * (q1[1] - q2[1])); q2[2] + j]
      r2 = [round(Cint, x0 + j / n * (q1[1] - x0)); q2[2] + j]
      SDL_RenderDrawLines(s.renderer, [r1; r2], Cint(2))
    end
    n = q2[2] - q3[2]
    for j = Cint(1):n-Cint(1)
      r1 = [round(Cint, q2[1] + j / n * (q3[1] - q2[1])); q2[2] - j]
      r2 = [round(Cint, x0 + j / n * (q3[1] - x0)); q2[2] - j]
      SDL_RenderDrawLines(s.renderer, [r1; r2], Cint(2))
    end
  end
end

# improved circle drawing algorithm. slower but fills completely. needs optimization
function draw(s::Screen, circle::Circle, c::Colorant=colorant"black"; fill=false)
    # define the center and needed sides of circle
    centerX = Cint(circle.x)
    centerY = Cint(circle.y)
    int_rad = Cint(circle.r)
    left = centerX - int_rad
    top = centerY - int_rad

    SDL_SetRenderDrawColor(
        s.renderer,
        sdl_colors(c)...,
    )

    # we consider a grid with sides equal to the circle's diameter
    for x in left:centerX
        for y in top:centerY

            # for each pixel in the top left quadrant of the grid we measure the distance from the center.
            dist = sqrt( (centerX - x)^2 + (centerY - y)^2 )

            # if it is close to the circle's radius it and all associated points in the other quadrants are colored in.
            if (dist <= circle.r + 0.5 && dist >= circle.r - 0.5)
                rel_x = centerX - x
                rel_y = centerY - y

                quad1 = (x              , y              )
                quad2 = (centerX + rel_x, y              )
                quad3 = (x              , centerY + rel_y)
                quad4 = (quad2[1]       , quad3[2]       )

                SDL_RenderDrawPoint(s.renderer, quad1[1], quad1[2])
                SDL_RenderDrawPoint(s.renderer, quad2[1], quad2[2])
                SDL_RenderDrawPoint(s.renderer, quad3[1], quad3[2])
                SDL_RenderDrawPoint(s.renderer, quad4[1], quad4[2])

                # if we are told to fill in the circle we draw lines between all of the quadrants to completely fill the circle
                if (fill == true)
                    SDL_RenderDrawLine(s.renderer, quad1[1], quad1[2], quad2[1], quad2[2])
                    SDL_RenderDrawLine(s.renderer, quad2[1], quad2[2], quad4[1], quad4[2])
                    SDL_RenderDrawLine(s.renderer, quad4[1], quad4[2], quad3[1], quad3[2])
                    SDL_RenderDrawLine(s.renderer, quad3[1], quad3[2], quad1[1], quad1[2])
                end
            end

        end
    end

end

rect(x::Rect) = x
rect(x::Circle) = Rect(x.left, x.top, 2*x.r, 2*x.r)


text:
using Distributions
using SpecialFunctions

export impliedVolatilityLiSORTS

function impliedVolatilitySqrtTimeRationalGuess(x::T, c::T) where {T}
    m00 = -0.00006103098165
    n00 = 1.0
    m01 = 5.33967643357688
    n01 = 22.96302109010794
    m10 = -0.40661990365427
    n10 = -0.48466536361620
    m02 = 3.25023425332360
    n02 = -0.77268824532468
    m11 = -36.19405221599028
    n11 = -1.34102279982050
    m20 = 0.08975394404851
    n20 = 0.43027619553168
    m03 = 83.84593224417796
    n03 = -5.70531500645109
    m12 = 41.21772632732834
    n12 = 2.45782574294244
    m21 = 3.83815885394565
    n21 = -0.04763802358853
    m30 = -0.21619763215668
    n30 = -0.03326944290044
    num = m00 + m10 * x + m20 * x^2 + m30 * x^3 + c * (m01 + m11 * x + m21 * x^2) + c^2 * (m02 + m12 * x) + c^3 * m03
    den = n00 + n10 * x + n20 * x^2 + n30 * x^3 + c * (n01 + n11 * x + n21 * x^2) + c^2 * (n02 + n12 * x) + c^3 * n03
    return num / den
end

function impliedVolatilityLiRationalGuess(
    price::T,
	isCall::Bool,
	strike::T,
    forward::T,
    timeToExpiry::T,
    df::T
) where {T}
    c = price / forward / df
    ex = forward / strike
    x0 = log(ex)
    x = x0
    if !isCall
        if x0 <= 0
            c = c + 1 - 1.0 / ex # put call parity
        else
			#duality + put call parity
            c = ex * c
            x = -x0
            ex = 1.0 / ex
        end
    else
        if x0 > 0
			# use c(-x0, v)
            c = (forward * (c - 1) + strike) / strike #in out duality
            x = -x0
            ex = 1.0 / ex
        end
    end
    if c >= 1 / ex || c >= 1
        error("price ", c, " higher than intrinsic value ", 1 / ex)
    elseif c <= 0
        error("price ", c, " negative")
    end
    sqrtte = sqrt(timeToExpiry)
    return impliedVolatilitySqrtTimeRationalGuess(x, c) / sqrtte
end

"Implied volatility of a vanilla option under the Black-Scholes model via Li SOR-TS algorithm"
function impliedVolatilityLiSORTS(
    price::T,
    isCall::Bool,
    strike::T,
    forward::T,
    timeToExpiry::T,
    df::T,
    impliedVolGuess::T,
    tolerance::T,
    maxIterations::Int
) where {T}
    c = price / forward / df
    ex = forward / strike
    x0 = log(ex)
    x = x0
    if !isCall
        if x0 <= 0
            c = c + 1 - 1.0 / ex # put call parity
        else
			#duality + put call parity
            c = ex * c
            x = -x0
            ex = 1.0 / ex
        end
    else
        if x0 > 0 			# use c(-x0, v)
            c = (forward * c - forward + strike) / strike # in out duality
            x = -x0
            ex = 1.0 / ex
        end
    end
    if c > 1 / ex
        return 0
    end
    sqrtte = sqrt(timeToExpiry)
    v0 = 0.0
    if impliedVolGuess == 0.0
        v0 = impliedVolatilitySqrtTimeRationalGuess(x, c)
    else
        v0 = impliedVolGuess * sqrtte
    end
    return computeLiSORTS(c, x, ex, v0, sqrtte, tolerance, maxIterations)
end

const OneOverSqrtTwo = sqrt(0.5)

function computeLiSORTS(c::T, x::T, ex::T, v0::T, sqrttte::T, tolerance::T, maxIterations::Int) where {T}
    v = v0
    if maxIterations <= 0
        return v
    end
    exinvsqrt = 1.0 / sqrt(ex)
    h = x / v
    t = v / 2

    Np = erfcx(-OneOverSqrtTwo * (h + t))
    Nm = erfcx(-OneOverSqrtTwo * (h - t))
    norm = exinvsqrt * 0.5 * exp(-0.5 * (h^2 + t^2))
    cEstimate = norm * (Np - Nm)
    if cEstimate < 0
        cEstimate = 0.0
    end

    omega = 1.0
    iterations = 0
    normal = Normal()
    while abs(c - cEstimate) > tolerance && iterations < maxIterations
        phi = (v^2 + 2 * x) / (v^2 - 2 * x)
        alpha = (1 + omega) / (1 + phi)
        F = c + norm * (Nm + omega * Np)
        Nm1 = quantile(normal, F / (1 + omega))
        G = Nm1 + sqrt(Nm1^2 - 2 * x)
        v = alpha * G + (1 - alpha) * v
        h = x / v
        t = v / 2
        Np = erfcx(-OneOverSqrtTwo * (h + t))
        Nm = erfcx(-OneOverSqrtTwo * (h - t))
        norm = exinvsqrt * 0.5 * exp(-0.5 * (h^2 + t^2))
        cEstimate = norm * (Np - Nm)
        if cEstimate < 0
            cEstimate = 0
        end
        iterations += 1
    end
    sigma = v / sqrttte
    return sigma
end


text:
"""
    timestep!(simulation)

Perform one timestep of the simulation
"""
function timestep!(simulation)
    @unpack model,timestepping_params, output_params = simulation
    update_state!(model)
    #write solution if at the first timestep (hack for https://github.com/RJArthern/WAVI.jl/issues/46 until synchronicity is fixed)
    if (output_params.output_start) && (simulation.clock.n_iter == 0)
        write_output(simulation)
    end
    if timestepping_params.step_thickness
        update_thickness!(simulation)
    end
    update_clock!(simulation)
end

"""
update_thickness!(model::AbstractModel)

Update thickness using rate of change of thickness and apply minimum thickness constraint. Includes an option for not evolving shelf thickness.
"""
function update_thickness!(simulation)
@unpack model,timestepping_params=simulation
hUpdate=zeros(model.grid.nx,model.grid.ny)
aground=zeros(model.grid.nx,model.grid.ny)
hUpdate[model.fields.gh.mask]=max.(model.params.minimum_thickness .- model.fields.gh.h[model.fields.gh.mask],timestepping_params.dt*model.fields.gh.dhdt[model.fields.gh.mask])
#Specify whether to evolve the shelves:
if !model.params.evolveShelves
    hUpdate[model.fields.gh.mask]=max.(model.params.smallHAF.-(model.params.density_ocean./model.params.density_ice).*model.fields.gh.b[model.fields.gh.mask].-model.fields.gh.h[model.fields.gh.mask],hUpdate[model.fields.gh.mask])
    aground=(model.fields.gh.haf.>=0)
    wc=[1 1 1; 1 1 1; 1 1 1]
    w=centered(wc)
    nearfloat_mask=imfilter(model.fields.gh.mask.&.!aground,reflect(w),Fill(0,w))
    nearfloat_mask=iszero.(iszero.(nearfloat_mask))
    hUpdate[nearfloat_mask].=0
  end
model.fields.gh.h[model.fields.gh.mask] = model.fields.gh.h[model.fields.gh.mask] .+ hUpdate[model.fields.gh.mask]
return simulation
end

"""
    update_clock!(simulation::AbstractSimulation)

Update the simulation clock
"""
function update_clock!(simulation)
    @unpack clock,timestepping_params=simulation
    clock.n_iter += 1
    clock.time += timestepping_params.dt
    return simulation
end

"""
    run_simulation(simulation)
Perform the simulation specified by the simulation
"""
function run_simulation!(simulation)
    @unpack model, timestepping_params, output_params = simulation
    chkpt_tag = "A"
    for i = (simulation.clock.n_iter+1):timestepping_params.n_iter_total
        timestep!(simulation)

        #check if we have hit a temporary checkpoint
        if mod(i,timestepping_params.n_iter_chkpt) == 0
            #output a temporary checkpoint
            fname = string("Chkpt",chkpt_tag, ".jld2")
            @save fname simulation
            chkpt_tag = (chkpt_tag == "A") ? "B" : "A"
            println("making temporary checkpoint at timestep number $(simulation.clock.n_iter)")
        end

        #check if we have hit a permanent checkpoint
        if mod(i,simulation.timestepping_params.n_iter_pchkpt) == 0
            #output a permanent checkpoint
            n_iter_string =  lpad(simulation.clock.n_iter, 10, "0"); #filename as a string with 10 digits
            fname = string("PChkpt_",n_iter_string, ".jld2")
            @save fname simulation
            println("making permanent checkpoint at timestep number $(simulation.clock.n_iter)")
        end

        #check if we have hit an output timestep
        if mod(i,simulation.output_params.n_iter_out) == 0
            write_output(simulation)
        end

        #check the dump velocity flag at the final timestep
        if (i == timestepping_params.n_iter_total) && output_params.dump_vel
            write_vel(simulation)
        end
    end

    #zip the simulation output (no zipping handled by zip_output)
    zip_output(simulation)

    return simulation
end

"""
    function write_vel(simulation)

Write the velocity at the the final timestep of the simulation (used in the coupled wavi-mitgcm model to communicate with streamice)
"""
function write_vel(simulation::Simulation)
    @unpack model = simulation
    uVel_file_string = string(simulation.output_params.prefix,  "_U.bin")
    vVel_file_string = string(simulation.output_params.prefix,  "_V.bin")

    u_out=model.fields.gu.u[1:end-1,:]
    v_out=model.fields.gv.v[:,1:end-1]

    u_out .= hton.(u_out)
    v_out .= hton.(v_out)

    ufileID =  open(uVel_file_string,"w")
      write(ufileID, u_out[:,:])
    close(ufileID)
    vfileID =  open(vVel_file_string,"w")
    write(vfileID, v_out[:,:])
    close(vfileID)

 end