text:
using TypedTables
using NullableArrays
using Compat

if VERSION >= v"0.5-"
    using Base.Test
else
    using BaseTestNext
    const Test = BaseTestNext
end

@testset "Tables tests" begin

include("test_cell.jl")
include("test_column.jl")
include("test_row.jl")
include("test_table.jl")

include("test_join.jl")
include("test_setalgorithms.jl")
include("test_IO.jl")
include("test_datamacros.jl")

end


text:
using Intan, SpikeSorting, Gtk.ShortNames, Cairo,BaslerCamera, StatsBase
import Intan

mutable struct Impedance
    pulses::BitArray{1}
    counter::Int64
    current::Float64
    impedance::Float64
    stim_voltage::Float64
    base_voltage::Float64
end

Impedance()=Impedance(falses(Intan.SAMPLES_PER_DATA_BLOCK),1,1.0,1.0,1.0,1.0)

mutable struct cam_param
    c::Gtk.GtkCanvasLeaf
    serial::String
    trigger::Int64
    frame_period::Float64
    connected::Bool
    plot_frame::Array{UInt32,2}
    h::Int32
    w::Int32
end

cam_param(h,w,s="") = cam_param(Canvas(20,20),s,1,20.0,false,zeros(UInt32,w,h),h,w)

#=
Data structure
should be a subtype of Task abstract type
=#
mutable struct Task_CameraTask <: Intan.Task
    cam::Camera
    cam2::Camera
    b::Gtk.GtkBuilder
    impedance::Impedance
    pw::Int64
    period::Int64
    stim_start_time::Float64
    in_stim::Bool
    c::Gtk.GtkCanvasLeaf #View Canvas
    c2::Gtk.GtkCanvasLeaf
    n_camera::Int32
    cam1_param::cam_param
    cam2_param::cam_param
end

#=
Constructors for data type
=#

function Task_CameraTask(config_path = "./config.jl")

    include(config_path)

    b = Builder(filename="./whiskertask.glade")

    #Camera 1
    cam=Camera(cam_1_h,cam_1_w,1,1)
    cam_param1 = cam_param(cam_1_h,cam_1_w,cam_1_s)
    push!(b["cam_1_active_box"],cam_param1.c)

    c=Canvas(-1,-1)

    push!(b["cam_1_box"], c)
    Gtk.GAccessor.hexpand(c,true)
    Gtk.GAccessor.vexpand(c,true)

    #Camera 2
    cam2=Camera(cam_2_h,cam_2_w,1,1)
    cam_param2 = cam_param(cam_2_h,cam_2_w,cam_2_s)
    push!(b["cam_2_active_box"],cam_param2.c)

    c2=Canvas(-1,-1)

    push!(b["cam_2_box"], c2)
    Gtk.GAccessor.hexpand(c2,true)
    Gtk.GAccessor.vexpand(c2,true)

    handles = Task_CameraTask(cam,cam2,b,Impedance(),250,3000,0,false,
    c,c2,num_cam,cam_param1,cam_param2)

    sleep(5.0)
    Gtk.showall(handles.b["win"])

    handles
end

function connect_cb(widget::Ptr,user_data::Tuple{Task_CameraTask})
   han, = user_data

   if !han.cam1_param.connected

       #change_camera_config(han.cam,path)
       #Open by serial number
       #connect(han.cam)
       BaslerCamera.connect_camera(han.cam.cam,han.cam1_param.serial)
       set_gtk_property!(han.b["cam_1_serial"],:label,han.cam1_param.serial)

       sleep(1.0)

       draw_circle(han.cam1_param,(0,1,0))
       #set_gtk_property!(han.b["cam1_connect_label"],:label,"Connected")

       BaslerCamera.change_resolution(han.cam.cam,han.cam1_param.h,han.cam1_param.w)

       start_acquisition(han.cam)

       han.cam1_param.connected = true
   end

    nothing
end

function view_cb(widget::Ptr,user_data::Tuple{Task_CameraTask,FPGA})

    han,fpga = user_data

    if get_gtk_property(han.b["view_button"],:active,Bool)
        Intan.manual_trigger(fpga,0,true)
    else
        #End TTL
        Intan.manual_trigger(fpga,0,false)
    end

    nothing
end

function change_pw(widget::Ptr,user_data::Tuple{Task_CameraTask,FPGA})

    han, fpga = user_data

    nothing
end

function change_period(widget::Ptr,user_data::Tuple{Task_CameraTask,FPGA})

    han, fpga  = user_data

    nothing
end

function update_stimulation_parameters(han,fpga)

    han.pw = get_gtk_property(han.b["stim_pw_adj"],:value,Int64)
    han.period = get_gtk_property(han.b["stim_period_adj"],:value,Int64)
    num_pulses = get_gtk_property(han.b["stim_pulses_adj"],:value,Int64)

    if num_pulses == 1
        fpga.d[3].pulseOrTrain = 0
        fpga.d[3].numPulses = 1

        fpga.d[3].firstPhaseDuration = han.pw * 1000
        fpga.d[3].refractoryPeriod = (han.period - han.pw) * 1000
        fpga.d[3].pulseTrainPeriod = (han.period - han.pw) * 1000
    else
        fpga.d[3].pulseOrTrain = 1
        fpga.d[3].numPulses = num_pulses

        fpga.d[3].firstPhaseDuration = han.pw * 1000
        #this effectively sets how long until next stimulation. don't want it to be too long
        fpga.d[3].refractoryPeriod = 1e6 * 10
        fpga.d[3].pulseTrainPeriod = han.period * 1000
    end

    Intan.update_digital_output(fpga,fpga.d[3])

    nothing
end

function update_pico_parameters(han,fpga)

    period = get_gtk_property(han.b["pico_period_adj"],:value,Int64)

    total_time = 1000
    pw_ds = .5 # 50% duty cycle
    pw = round(Int,pw_ds * period)
    inter_burst_duration = 2 * total_time # 2 seconds between each stim epoch
    n_p = div(total_time,period)

    fpga.d[5].pulseOrTrain = 1
    fpga.d[5].numPulses = n_p

    fpga.d[5].firstPhaseDuration = pw * 1000 #On Time
    fpga.d[5].refractoryPeriod = (inter_burst_duration - (n_p * period) + (period-pw)) * 1000
    fpga.d[5].pulseTrainPeriod = (period) * 1000

    Intan.update_digital_output(fpga,fpga.d[5])

    nothing
end

function plot_image(han,img,plot_frame)

     ctx=Gtk.getgc(han.c)

     c_w=width(ctx)
     c_h=height(ctx)

     w,h = size(img)
     Cairo.scale(ctx,c_w/w,c_h/h)

     for i=1:length(img)
        plot_frame[i] = (convert(UInt32,img[i]) << 16) | (convert(UInt32,img[i]) << 8) | img[i]
     end
     stride = Cairo.format_stride_for_width(Cairo.FORMAT_RGB24, w)
     @assert stride == 4*w
     surface_ptr = ccall((:cairo_image_surface_create_for_data,Cairo._jl_libcairo),
                 Ptr{Nothing}, (Ptr{Nothing},Int32,Int32,Int32,Int32),
                 plot_frame, Cairo.FORMAT_RGB24, w, h, stride)

     ccall((:cairo_set_source_surface,Cairo._jl_libcairo), Ptr{Nothing},
     (Ptr{Nothing},Ptr{Nothing},Float64,Float64), ctx.ptr, surface_ptr, 0, 0)

     rectangle(ctx, 0, 0, w, h)

     fill(ctx)

     SpikeSorting.identity_matrix(ctx)
     reveal(han.c)

    nothing
end

function laser_cb(widget::Ptr,user_data::Tuple{Task_CameraTask,FPGA})

    han,fpga = user_data

    if get_gtk_property(han.b["laser_button"],:active,Bool)
        Intan.manual_trigger(fpga,1,true)
    else
        Intan.manual_trigger(fpga,1,false)
    end

    nothing
end

function stim_cb(widget::Ptr,user_data::Tuple{Task_CameraTask,FPGA})

    han,fpga = user_data

    if get_gtk_property(han.b["stimulator_button"],:active,Bool)
        update_stimulation_parameters(han,fpga)
        Intan.manual_trigger(fpga,2,true)
    else
        Intan.manual_trigger(fpga,2,false)
    end

    nothing
end

function pico_cb(widget::Ptr,user_data::Tuple{Task_CameraTask,FPGA})

    han, fpga = user_data

    if get_gtk_property(han.b["pico_button"],:active,Bool)
        update_pico_parameters(han,fpga)
        Intan.manual_trigger(fpga,4,true)
    else
        Intan.manual_trigger(fpga,4,false)
    end

    nothing
end

function change_ffmpeg_cb(widget::Ptr,user_data::Tuple{Task_CameraTask,Intan.Gui_Handles})

    myt,han = user_data

    base_path = get_gtk_property(han.save_widgets.input,:text,String)

    change_ffmpeg_folder(myt.cam.cam,base_path)

    nothing
end

function recording_cb(widget::Ptr,user_data::Tuple{Task_CameraTask,Intan.Gui_Handles,FPGA,Intan.RHD2000})

    myt,han,fpga,rhd = user_data

    if get_gtk_property(han.record,:active,Bool)

        #we may want to make sure that the ttl box is checked. Perhaps this one should be on by default?

        #If camera is in view mode, we will be collecting frames already
        #Turn off briefly
        if get_gtk_property(myt.b["view_button"],:active,Bool)
            set_gtk_property!(myt.b["view_button"],:active,false)
        end

        #We wait here for everything to power down. Ideally we could flush the camera
        sleep(1.0)

        #Start ffmpeg
        start_ffmpeg(myt.cam.cam)

        #Wait for ffmpeg to get loaded up
        sleep(1.0)

        #Start TTL
        set_gtk_property!(myt.b["view_button"],:active,true)

    else #turn off

        #Turn off camera
        set_gtk_property!(myt.b["view_button"],:active,false)

        #Let ffmpeg finish
        sleep(2.0)

        end_ffmpeg(myt.cam.cam)

        check_alignment(rhd)
    end

    nothing
end

function check_alignment(rhd)

    analog_path = rhd.save.ttl

    vid_path = string(rhd.save.folder,"/output.mp4")

    xx=read(`mediainfo --Output="Video;%FrameCount%" $(vid_path)`)
    if length(xx) < 2
        video_frames = 0
    else
        video_frames=parse(Int64,String(xx[1:(end-1)]))
    end

    analog_size=length(findall(diff(Intan.parse_ttl(analog_path)[2]).>1))

    println("Video Frames: ", video_frames)
    println("Analog Data: ", analog_size)
    if (video_frames == analog_size)
        println("Yay, frames match!")
    else
        println("Boo! Frame mismatch by ", abs(video_frames-analog_size))
    end
end

function calculate_impedance(myt,fpga)

    adc_input=6
    stim_control_ttl=3

    if get_gtk_property(myt.b["calc_impedance_button"],:active,Bool)

        myt.impedance.current = get_gtk_property(myt.b["impedance_current_adj"],:value,Float64)

        for i=1:length(fpga.ttlout)
            myt.impedance.pulses[i]=false
            if (fpga.ttlout[i] & (1 << (stim_control_ttl-1))) > 0
                myt.impedance.pulses[i]=true
            end
        end

        if sum(myt.impedance.pulses) > (length(myt.impedance.pulses)*.75)
            myt.impedance.stim_voltage=mean(fpga.adc[myt.impedance.pulses,adc_input])

            dv = myt.impedance.stim_voltage - myt.impedance.base_voltage
            dv = dv * (10.24*2) / typemax(UInt16) * 1000
            myt.impedance.impedance = round(dv / myt.impedance.current,digits=2)
            set_gtk_property!(myt.b["impedance_label"],:label,string(myt.impedance.impedance))
        else
            myt.impedance.base_voltage=mean(fpga.adc[.!myt.impedance.pulses,adc_input])
            v = myt.impedance.base_voltage * (10.24*2) / typemax(UInt16) * 1000
            set_gtk_property!(myt.b["impedance_voltage"],:label,string(round(v,digits=1)))
        end

    end
end

function draw_circle(cam::cam_param,color::Tuple)
    ctx = Gtk.getgc(cam.c)

    set_source_rgb(ctx, color...)
    arc(ctx, 10, 10, 9, 0, 2pi)
    stroke_preserve(ctx)
    fill(ctx)
    reveal(cam.c)
end

function setup_pico(fpga::FPGA)

    total_time = 1000
    inter_burst_duration = 2 * total_time
    n_p = 100
    pw_ds = 0.5
    period = 10
    pw = round(Int,pw_ds * period)

    fpga.d[5].channel=4
    fpga.d[5].pulseOrTrain = 1

    fpga.d[5].triggerEnabled=1
    fpga.d[5].triggerSource=28

    fpga.d[5].numPulses = n_p
    fpga.d[5].postTriggerDelay=0
    fpga.d[5].firstPhaseDuration=pw * 1000
    fpga.d[5].refractoryPeriod = (inter_burst_duration - (n_p * period) + (period-pw)) * 1000
    fpga.d[5].pulseTrainPeriod = (period) * 1000

    Intan.update_digital_output(fpga,fpga.d[5])
    nothing
end

#=
Initialization function
This will build all of the necessary elements before anything starts running
(initializing external boards, creating GUIs etc)
=#
function Intan.init_task(myt::Task_CameraTask,rhd::Intan.RHD2000,han,fpga)

    #Yoke together change in save filename textbox with ffmpeg folder
    signal_connect(change_ffmpeg_cb,han.save_widgets.input,"activate",Nothing,(),false,(myt,han))

    #Clicking Record in Main GUI starts the Camera Recording
    signal_connect(recording_cb, han.record,"toggled",Nothing,(),false,(myt,han,fpga[1],rhd))

    signal_connect(connect_cb,myt.b["connect_button"],"clicked",Nothing,(),false,(myt,))
    signal_connect(view_cb,myt.b["view_button"],"toggled",Nothing,(),false,(myt,fpga[1]))
    signal_connect(laser_cb,myt.b["laser_button"],"toggled",Nothing,(),false,(myt,fpga[1]))

    signal_connect(change_pw,myt.b["stim_pw_sb"],"value-changed",Nothing,(),false,(myt,fpga[1]))
    signal_connect(change_period,myt.b["stim_period_sb"],"value-changed",Nothing,(),false,(myt,fpga[1]))
    signal_connect(stim_cb,myt.b["stimulator_button"],"toggled",Nothing,(),false,(myt,fpga[1]))

    signal_connect(pico_cb,myt.b["pico_button"],"toggled",Nothing,(),false,(myt,fpga[1]))

    change_ffmpeg_folder(myt.cam.cam,rhd.save.folder)

    #For this experiment, we want to be recording voltage, timestamps, and TTLs
    set_gtk_property!(han.save_widgets.ts,:active,true)
    set_gtk_property!(han.save_widgets.volt,:active,true)
    set_gtk_property!(han.save_widgets.ttlin,:active,true)

    draw_circle(myt.cam1_param,(1,0,0))

    #Stimulation Parameters
    #TTL 1 is camera at 500 fps
    fpga[1].d[1].channel=0
    fpga[1].d[1].pulseOrTrain = 0

    fpga[1].d[1].triggerEnabled=1
    fpga[1].d[1].triggerSource=24

    fpga[1].d[1].numPulses=1
    fpga[1].d[1].postTriggerDelay=0
    fpga[1].d[1].firstPhaseDuration=1000
    fpga[1].d[1].refractoryPeriod=1000
    Intan.update_digital_output(fpga[1],fpga[1].d[1])

    #TTL 2 is laser at
    fpga[1].d[2].channel=1
    fpga[1].d[2].pulseOrTrain = 0

    fpga[1].d[2].triggerEnabled=1
    fpga[1].d[2].triggerSource=25

    fpga[1].d[2].numPulses=1
    fpga[1].d[2].postTriggerDelay=0
    fpga[1].d[2].firstPhaseDuration=2000 #2 ms
    fpga[1].d[2].refractoryPeriod=48000
    Intan.update_digital_output(fpga[1],fpga[1].d[2])

    #TTL 3 is juxtacellular stimulation
    fpga[1].d[3].channel=2
    fpga[1].d[3].pulseOrTrain = 0

    fpga[1].d[3].triggerEnabled=1
    fpga[1].d[3].triggerSource=26

    fpga[1].d[3].numPulses=1
    fpga[1].d[3].postTriggerDelay=0
    fpga[1].d[3].firstPhaseDuration = 250000 #250 ms
    fpga[1].d[3].refractoryPeriod = 1000000 - 250000#3 seconds
    Intan.update_digital_output(fpga[1],fpga[1].d[3])

    setup_pico(fpga[1])

    #set TTL output #4 as high
    Intan.setTtlMode(fpga[1],[false,false,false,true,false,false,false,false])
    Intan.manual_trigger(fpga[1],3,true)

    nothing
end

#=
Experimental Control Function
This will implement the control logic of the task
such as updating GUIs, modifying the data structure, talking to external boards
=#
function Intan.do_task(myt::Task_CameraTask,rhd::Intan.RHD2000,myread,han,fpga)

    #Draw Picture
    if (myread)
        (myimage,grabbed) = BaslerCamera.get_camera_data(myt.cam.cam,myt.cam1_param.h,myt.cam1_param.w,1)

        if (grabbed)
            plot_image(myt,myimage,myt.cam1_param.plot_frame)
        end

        calculate_impedance(myt,fpga[1])
    end

    nothing
end

#=
Logging Function
This will save the appropriate elements of the data structure, as well as specifying what
analog streams from either the Intan or other external DAQs
=#
function Intan.save_task(myt::Task_CameraTask,rhd::Intan.RHD2000)
end

ext: jl

max_stars_repo_name: paulmthompson/WhiskerTask

max_issues_repo_name: paulmthompson/WhiskerTask

max_forks_repo_name: paulmthompson/WhiskerTask


text:
import ..UncertainValues.AbstractUncertainScalarKDE
import ..SamplingConstraints:
    SamplingConstraint,
    constrain

@recipe function plot_uncertainvalueKDE(uv::AbstractUncertainScalarKDE)
    @series begin
        seriestype := :path
        fα --> 0.5
        fc --> :green
        xlabel --> "Value"
        ylabel --> "Density"
        label --> ""
        uv.distribution.x, uv.distribution.density ./ sum(uv.distribution.density)
    end
end

@recipe function plot_uncertainvalueKDE(uv::AbstractUncertainScalarKDE,
        constraint::SamplingConstraint)

    cuv = constrain(uv, constraint)
    @series begin
        seriestype := :path
        fα --> 0.5
        fc --> :green
        xlabel --> "Value"
        ylabel --> "Density"
        label --> ""
        cuv.distribution.x, cuv.distribution.density ./ sum(cuv.distribution.density)
    end
end


text:
using Base.Cartesian
using LinearAlgebra
using DiscreteAxis
# This will be removed in favour of DiffEqOperators

function δ₂(F::AbstractVector{N}, axis::DAxis) where N <: Number
    δ = similar(F)
    for i in axis.i[2:end-1]
        δ[i] = 2*F[i]-F[i-1]-F[i-2]/axis
    end
    return δ
end

function δ⁻(size::Int, step::Number) # reverse difference matrix generator
    δ = zeros(size,size)
    for i in 1:size
        δ[i,i] = 1
        if i > 1
            δ[i,i-1] = -1
        end
    end
    return (1/step).*δ
end

function δ⁺(F::AbstractVector{N}, axis::DAxis) where N <: Number # forward difference matrix generator
    δ = similar(F)
    for i in axis.i[1:end-1]
        δ[i] = (F[i+1]-F[i])/axis.Δ[i]
    end
    δ[end] = -F[end]
    return δ
end

function δ⁻(F::AbstractVector{N}, axis::DAxis) where N <: Number
    δ = similar(F)
    for i in axis.i[2:end]
        δ[i] = (F[i]-F[i-1])/axis.Δ[i]
    end
    return δ
end

function δ⁺(x::LinearAxis)
    δ = zeros(x.N,x.N)
    for i in 1:x.N
        δ[i,i] = -1
        if i < x.N
            δ[i,i+1] = 1
        end
    end
    return δ
end

function δ⁺(x::Int)
    δ = zeros(x,x)
    for i in 1:x
        δ[i,i] = -1
        if i < x
            δ[i,i+1] = 1
        end
    end
    return δ
end

function δ⁻(x::LinearAxis)
    δ = zeros(x.N,x.N)
    for i in 1:x.N
        δ[i,i] = 1
        if i > 1
            δ[i,i-1] = -1
        end
    end
    return δ
end

function δ⁻(x::Int)
    δ = zeros(x,x)
    for i in 1:x
        δ[i,i] = 1
        if i > 1
            δ[i,i-1] = -1
        end
    end
    return δ
end

function δδ(size::Int, step::Number) # computes the 2nd order differentiator matrix
     δ = zeros(size,size)
     for i in 1:size
         δ[i,i] = -2.0

         if i >= 2
             δ[i-1,i] = δ[i,i-1] = 1.0
         end
     end
    # a = [203.0/45.0,-87.0/5.0,117.0/4.0, -254.0/9.0,33.0/2.0, -27.0/5.0, 137.0/180.0]
     #l = length(a)
     #δ[1,1:l] = a
     # δ[end, end-(l-1):end] = a[end:-1:1]
     return δ./step^2
  end

# this function splits the matrix B in to vectors along the specified direction, and then multiplies each vector by the matrix A
 function ⊗(A::AbstractArray,B::AbstractArray{N,2}, direction::Int) where N<:Number
     s = size(B)

    if direction == 1
        Bt = transpose(B)
        Ct = zeros(N,s[end:-1:1])
        for i in 1:s[1]
            @views Ct[:,i] = A*Bt[:,i]
        end
        C = transpose(Ct)
    elseif direction == 2
        C = zeros(N,s)
        for i in 1:s[2]
            @views C[:,i] = A*B[:,i]
        end
    else
        throw("Direction given is too large")
    end
    return C
end
# this is a test that does exactly the same thing as the function above
 function SliceProduct(A::Array{N,2},B::Array{N,2}, direction::Int) where N<:Number
     s = size(B)
     C = zeros(N,s)
     if direction == 1
         for i in 1:s[1]
            @views C[:,i] = A*B[:,i]
         end
    elseif direction == 2
        for i in 1:s[2]
            @views C[i,:] = A*B[i,:]
        end
    else
        throw("direction too large, choose 1 for through columns and 2 for through rows")
    end
    return C
end
#=
function ⊗(A::CuArray,B::CuArray, direction::Int64)
    s = size(B)
    C = cu(zeros(s))
    if direction == 1
        for i in 1:s[2]
            @inbounds C[:,i] = A*B[:,i]
        end
   elseif direction == 2
       for i in 1:s[1]
           @inbounds C[i,:] = A*B[i,:]
       end
   end
   return C
end
=#
∇(u, δxx, δyy) = ⊗(δxx,u,1).+⊗(δyy,u,2)


text:
"Parameters for right-hand side function of variational partitioned Runge-Kutta methods."
mutable struct ParametersVPRKpInternal{DT, TT, ET <: IODE{DT,TT}, D, S} <: AbstractParametersVPRK{DT,TT,ET,D,S}
    equ::ET
    tab::TableauVPRK{TT}
    Δt::TT

    t::TT
    q::Vector{DT}
    p::Vector{DT}
end

function ParametersVPRKpInternal(equ::ET, tab::TableauVPRK{TT}, Δt::TT) where {DT, TT, ET <: IODE{DT,TT}}
    D = equ.d
    S = tab.s

    q = zeros(DT,D)
    p = zeros(DT,D)

    ParametersVPRKpInternal{DT,TT,ET,D,S}(equ, tab, Δt, 0, q, p)
end

function compute_stages_vprk!(x, q̅, p̅, Q, V, Λ, P, F, R, params::ParametersVPRKpInternal)
    # copy x to V
    compute_stages_v_vprk!(x, V, params)

    # copy x to Λ
    compute_stages_λ_vprk!(x, Λ, params)

    # compute Q
    compute_stages_q_vprk!(q̅, Q, V, Λ, params)

    # compute p̅ and R
    compute_projection_vprk!(q̅, p̅, Q, V, Λ, R, params)

    # compute P and F
    compute_stages_p_vprk!(Q, V, P, F, params)
end

function compute_stages_λ_vprk!(x::Vector{ST}, Λ::Matrix{ST}, params::ParametersVPRKpInternal{DT,TT,ET,D,S}) where {ST,DT,TT,ET,D,S}
    @assert D == size(Λ,1)
    @assert S == size(Λ,2)

    # copy x to Λ
    for i in 1:S
        for k in 1:D
            Λ[k,i] = x[D*S+k]
        end
    end
end

function compute_stages_q_vprk!(q̅::Vector{ST}, Q::Matrix{ST}, V::Matrix{ST}, Λ::Matrix{ST}, params::ParametersVPRKpInternal{DT,TT,ET,D,S}) where {ST,DT,TT,ET,D,S}
    @assert D == size(Q,1) == size(V,1) == size(Λ,1) == length(q̅)
    @assert S == size(Q,2) == size(V,2) == size(Λ,2)

    local y1::ST
    local y2::ST
    local y3::ST
    local y4::ST

    # compute Q
    for i in 1:S
        for k in 1:D
            y1 = y2 = y3 = y4 = 0
            for j in 1:S
                y1 += params.tab.q.a[i,j] * V[k,j]
                y2 += params.tab.q.â[i,j] * V[k,j]
                y3 += params.tab.q.a[i,j] * Λ[k,j]
                y4 += params.tab.q.â[i,j] * Λ[k,j]
            end
            Q[k,i] = params.q[k] + params.Δt * (y1 + y2 + y3 + y4)
        end
    end

    # compute q̅
    for k in 1:D
        y1 = y2 = y3 = y4 = 0
        for j in 1:S
            y1 += params.tab.q.b[j] * V[k,j]
            y2 += params.tab.q.b̂[j] * V[k,j]
            y3 += params.tab.q.b[j] * Λ[k,j]
            y4 += params.tab.q.b̂[j] * Λ[k,j]
        end
        q̅[k] = params.q[k] + params.Δt * (y1 + y2 + y3 + y4)
    end
end

@generated function compute_projection_vprk!(q̅::Vector{ST}, p̅::Vector{ST},
                Q::Matrix{ST}, V::Matrix{ST}, Λ::Matrix{ST}, R::Matrix{ST},
                params::ParametersVPRKpInternal{DT,TT,ET,D,S}
            ) where {ST,DT,TT,ET,D,S}

    # create temporary vectors
    tQ = zeros(ST,D)
    tV = zeros(ST,D)
    tΛ = zeros(ST,D)
    tR = zeros(ST,D)

    quote
        local t₀::TT = params.t
        local t₁::TT = params.t + params.Δt
        local tᵢ::TT

        # compute p̅=ϑ(q̅)
        $tV .= 0
        params.equ.ϑ(t₁, q̅, $tV, p̅)

        for i in 1:S
            simd_copy_xy_first!($tQ, Q, i)
            simd_copy_xy_first!($tV, V, i)
            simd_copy_xy_first!($tΛ, Λ, i)

            tᵢ = t₀ + params.Δt * params.tab.p.c[i]

            params.equ.g(tᵢ, $tQ, $tΛ, $tR)

            simd_copy_yx_first!($tR, R, i)
        end
    end
end

function compute_rhs_vprk!(b::Vector{ST}, P::Matrix{ST}, F::Matrix{ST}, R::Matrix{ST},
                params::ParametersVPRKpInternal{DT,TT,ET,D,S}) where {ST,DT,TT,ET,D,S}

    local z1::ST
    local z2::ST
    local z3::ST
    local z4::ST

    # compute b = - [(P-AF-AR)]
    for i in 1:S
        for k in 1:D
            z1 = z2 = z3 = z4 = 0
            for j in 1:S
                z1 += params.tab.p.a[i,j] * F[k,j]
                z2 += params.tab.p.â[i,j] * F[k,j]
                z3 += params.tab.p.a[i,j] * R[k,j]
                z4 += params.tab.p.â[i,j] * R[k,j]
            end
            b[D*(i-1)+k] = (P[k,i] - params.p[k]) - params.Δt * (z1 + z2 + z3 + z4)
        end
    end
end

function compute_rhs_vprk_projection!(b::Vector{ST}, p̅::Vector{ST},
                F::Matrix{ST}, R::Matrix{ST}, offset::Int,
                params::ParametersVPRKpInternal{DT,TT,ET,D,S}
            ) where {ST,DT,TT,ET,D,S}

    local z1::ST
    local z2::ST
    local z3::ST
    local z4::ST

    for k in 1:D
        z1 = z2 = z3 = z4 = 0
        for j in 1:S
            z1 += params.tab.p.b[j] * F[k,j]
            z2 += params.tab.p.b̂[j] * F[k,j]
            z3 += params.tab.p.b[j] * R[k,j]
            z4 += params.tab.p.b̂[j] * R[k,j]
        end
        b[offset+k] = (p̅[k] - params.p[k]) - params.Δt * (z1 + z2 + z3 + z4)
    end
end

"Compute stages of variational partitioned Runge-Kutta methods."
@generated function function_stages!(x::Vector{ST}, b::Vector{ST},
                params::ParametersVPRKpInternal{DT,TT,ET,D,S}
            ) where {ST,DT,TT,ET,D,S}

    scache = NonlinearFunctionCacheVPRK{ST}(D,S)
    pcache = NonlinearFunctionCacheVPRKprojection{ST}(D,S)

    function_stages = quote
        compute_stages_vprk!(x, $pcache.q̅, $pcache.p̅,
                                $scache.Q, $scache.V, $pcache.Λ,
                                $scache.P, $scache.F, $pcache.R,
                                params)

        # compute b = [P-AF-AR]
        compute_rhs_vprk!(b, $scache.P, $scache.F, $pcache.R, params)

        # compute b = Φ
        compute_rhs_vprk_projection!(b, $pcache.p̅, $scache.F, $pcache.R, D*S, params)

        compute_rhs_vprk_correction!(b, $scache.V, params)
    end

    return function_stages
end

struct IntegratorVPRKpInternal{DT, TT, PT <: ParametersVPRKpInternal{DT,TT},
                                        ST <: NonlinearSolver{DT},
                                        IT <: InitialGuessPODE{DT,TT}} <: AbstractIntegratorVPRK{DT,TT}
    params::PT
    solver::ST
    iguess::IT

    scache::NonlinearFunctionCacheVPRK{DT}
    pcache::NonlinearFunctionCacheVPRKprojection{DT}

    q::Vector{Vector{TwicePrecision{DT}}}
    p::Vector{Vector{TwicePrecision{DT}}}
end

function IntegratorVPRKpInternal(equation::ET, tableau::TableauVPRK{TT}, Δt::TT) where {DT, TT, ET <: IODE{DT,TT}}
    D = equation.d
    M = equation.n
    S = tableau.s

    # create params
    params = ParametersVPRKpInternal(equation, tableau, Δt)

    # create solver
    solver = create_nonlinear_solver(DT, D*(S+1), params)

    # create initial guess
    iguess = InitialGuessPODE(get_config(:ig_interpolation), equation, Δt)

    # create cache for internal stage vectors and update vectors
    scache = NonlinearFunctionCacheVPRK{DT}(D,S)
    pcache = NonlinearFunctionCacheVPRKprojection{DT}(D,S)

    # create solution vectors
    q = create_solution_vector(DT, D, M)
    p = create_solution_vector(DT, D, M)

    # create integrator
    IntegratorVPRKpInternal{DT, TT, typeof(params), typeof(solver), typeof(iguess)}(
                params, solver, iguess, scache, pcache, q, p)
end

function initial_guess!(int::IntegratorVPRKpInternal{DT,TT}, m::Int) where {DT,TT}
    for i in 1:int.params.tab.s
        evaluate!(int.iguess, m, int.scache.y, int.scache.z, int.scache.v, int.params.tab.q.c[i], int.params.tab.p.c[i])
        for k in 1:int.params.equ.d
            int.solver.x[int.params.equ.d*(i-1)+k] = int.scache.v[k]
        end
    end
    for k in 1:int.params.equ.d
        int.solver.x[int.params.equ.d*int.params.tab.s+k] = 0
    end
end

"Integrate ODE with variational partitioned Runge-Kutta integrator."
function integrate_step!(int::IntegratorVPRKpInternal{DT,TT}, sol::SolutionPDAE{DT,TT}, m::Int, n::Int) where {DT,TT}
    # check if m and n are compatible with solution dimensions
    check_solution_dimension_asserts(sol, m, n)

    # set time and solution for nonlinear solver
    int.params.t = sol.t[0] + (n-1)*int.params.Δt
    int.params.q .= int.q[m]
    int.params.p .= int.p[m]

    # compute initial guess
    initial_guess!(int, m)

    # call nonlinear solver
    solve!(int.solver)

    # print solver status
    print_solver_status(int.solver.status, int.solver.params, n)

    # check if solution contains NaNs or error bounds are violated
    check_solver_status(int.solver.status, int.solver.params, n)

    # compute final update
    compute_stages_vprk!(int.solver.x, int.pcache.q̅, int.pcache.p̅,
                         int.scache.Q, int.scache.V, int.pcache.Λ,
                         int.scache.P, int.scache.F, int.pcache.R,
                         int.params)

    # compute unprojected solution
    update_solution!(int.q[m], int.scache.V, int.params.tab.q.b, int.params.tab.q.b̂, int.params.Δt)
    update_solution!(int.p[m], int.scache.F, int.params.tab.p.b, int.params.tab.p.b̂, int.params.Δt)

    # add projection to solution
    update_solution!(int.q[m], int.pcache.Λ, int.params.tab.q.b, int.params.tab.q.b̂, int.params.Δt)
    update_solution!(int.p[m], int.pcache.R, int.params.tab.p.b, int.params.tab.p.b̂, int.params.Δt)

    # copy solution to initial guess for next time step
    update!(int.iguess, m, sol.t[0] + n*int.params.Δt, int.q[m], int.p[m])

    # take care of periodic solutions
    cut_periodic_solution!(int.q[m], int.params.equ.periodicity)

    # copy to solution
    copy_solution!(sol, int.q[m], int.p[m], int.pcache.λ, n, m)
end


text:
# --------------------------------------------------------------------------- #
# Perform a numerical experiment (with a fake version of GRASP-SPP)

function graspSPP(fname, alpha, nbIterationGrasp)

    zconstruction = zeros(Int64,nbIterationGrasp)
    zamelioration = zeros(Int64,nbIterationGrasp)
    zbest = zeros(Int64,nbIterationGrasp)
    zbetter=0

    for i=1:nbIterationGrasp
        zconstruction[i] = rand(15:40) # # livrable du DM2
        zamelioration[i] = rand(0:10) + zconstruction[i] # livrable du DM2
        zbetter = max(zbetter, zamelioration[i])
        zbest[i] = zbetter
    end
    return zconstruction, zamelioration, zbest
end

function plotRunGrasp(iname,zinit, zls, zbest)
    figure("Examen d'un run",figsize=(6,6)) # Create a new figure
    title("GRASP-SPP | \$z_{Init}\$  \$z_{LS}\$  \$z_{Best}\$ | " * iname)
    xlabel("Itérations")
    ylabel("valeurs de z(x)")
    ylim(0, maximum(zbest)+2)

    nPoint = length(zinit)
    x=collect(1:nPoint)
    xticks([1,convert(Int64,ceil(nPoint/4)),convert(Int64,ceil(nPoint/2)), convert(Int64,ceil(nPoint/4*3)),nPoint])
    plot(x,zbest, linewidth=2.0, color="green", label="meilleures solutions")
    plot(x,zls,ls="",marker="^",ms=2,color="green",label="toutes solutions améliorées")
    plot(x,zinit,ls="",marker=".",ms=2,color="red",label="toutes solutions construites")
    vlines(x, zinit, zls, linewidth=0.5)
    legend(loc=4, fontsize ="small")
end

function plotAnalyseGrasp(iname, x, zmoy, zmin, zmax)
    figure("bilan tous runs",figsize=(6,6)) # Create a new figure
    title("GRASP-SPP | \$z_{min}\$  \$z_{moy}\$  \$z_{max}\$ | " * iname)
    xlabel("Itérations (pour nbRunGrasp)")
    ylabel("valeurs de z(x)")
    ylim(0, zmax[end]+2)

    nPoint = length(x)
    intervalle = [reshape(zmoy,(1,nPoint)) - reshape(zmin,(1,nPoint)) ; reshape(zmax,(1, nPoint))-reshape(zmoy,(1,nPoint))]
    xticks(x)
    errorbar(x,zmoy,intervalle,lw=1, color="black", label="zMin zMax")
    plot(x,zmoy,linestyle="-", marker="o", ms=4, color="green", label="zMoy")
    legend(loc=4, fontsize ="small")
end

function plotCPUt(allfinstance, tmoy)
    figure("bilan CPUt tous runs",figsize=(6,6)) # Create a new figure
    title("GRASP-SPP | tMoy")
    ylabel("CPUt moyen (s)")

    xticks(collect(1:length(allfinstance)), allfinstance, rotation=60, ha="right")
    margins(0.15)
    subplots_adjust(bottom=0.15,left=0.21)
    plot(collect(1:length(allfinstance)),tmoy,linestyle="--", lw=0.5, marker="o", ms=4, color="blue", label="tMoy")
    legend(loc=4, fontsize ="small")
end

# Simulation d'une experimentation numérique  --------------------------

#Pkg.add("PyPlot") # Mandatory before the first use of this package
using PyPlot

function simulation()
    allfinstance      =  ["didactic.txt", "fn2.txt", "fn3.txt", "fnA.txt", "fnX.txt"]
    nbInstances       =  length(allfinstance)
    nbRunGrasp        =  30   # nombre de fois que la resolution GRASP est repetee
    nbIterationGrasp  =  200  # nombre d'iteration que compte une resolution GRASP
    nbDivisionRun     =  10   # nombre de division que compte une resolution GRASP

    zinit = zeros(Int64, nbIterationGrasp) # zero
    zls   = zeros(Int64, nbIterationGrasp) # zero
    zbest = zeros(Int64, nbIterationGrasp) # zero

    x     = zeros(Int64, nbDivisionRun)
    zmax  = Matrix{Int64}(undef,nbInstances , nbDivisionRun); zmax[:] .= typemin(Int64)  # -Inf entier
    zmoy  = zeros(Float64, nbInstances, nbDivisionRun) # zero
    zmin  = Matrix{Int64}(undef,nbInstances , nbDivisionRun) ; zmin[:] .= typemax(Int64)  # +Inf entier
    tmoy  = zeros(Float64, nbInstances)  # zero

    # calcule la valeur du pas pour les divisions
    for division=1:nbDivisionRun
        x[division] = convert(Int64, ceil(nbIterationGrasp / nbDivisionRun * division))
    end

    println("Experimentation GRASP-SPP avec :")
    println("  nbInstances       = ", nbInstances)
    println("  nbRunGrasp        = ", nbRunGrasp)
    println("  nbIterationGrasp  = ", nbIterationGrasp)
    println("  nbDivisionRun     = ", nbDivisionRun)
    println(" ")
    cpt = 0

    # run non comptabilise (afin de produire le code compile)
    zinit, zls, zbest = graspSPP(allfinstance[1], 0.5, 1)

    for instance = 1:nbInstances
        # les instances sont traitees separement

        print("  ",allfinstance[instance]," : ")
        for runGrasp = 1:nbRunGrasp
            # une instance sera resolue nbrungrasp fois

            start = time() # demarre le compteur de temps
            alpha = 0.75
            zinit, zls, zbest = graspSPP(allfinstance[instance], alpha, nbIterationGrasp)
            tutilise = time()-start # arrete et releve le compteur de temps
            cpt+=1; print(cpt%10)

            # mise a jour des resultats collectes
            for division=1:nbDivisionRun
                zmax[instance,division] = max(zbest[x[division]], zmax[instance,division])
                zmin[instance,division] = min(zbest[x[division]], zmin[instance,division])
                zmoy[instance,division] =  zbest[x[division]] + zmoy[instance,division]
            end #division
            tmoy[instance] = tmoy[instance] + tutilise

        end #run
        for division=1:nbDivisionRun
             zmoy[instance,division] =  zmoy[instance,division] /  nbRunGrasp
        end #division
        tmoy[instance] = tmoy[instance] / nbRunGrasp
        println(" ")

    end #instance

    #Pkg.add("PyPlot") # Mandatory before the first use of this package
    println(" ");println("  Graphiques de synthese")
#    using PyPlot
    instancenb = 1
    plotRunGrasp(allfinstance[instancenb], zinit, zls, zbest)
    plotAnalyseGrasp(allfinstance[instancenb], x, zmoy[instancenb,:], zmin[instancenb,:], zmax[instancenb,:] )
    plotCPUt(allfinstance, tmoy)
end


text:
import Makie
import CairoMakie
using Statistics: mean, std, var, cor
using Random: randn!, bitrand
using LogExpFunctions: logsumexp
using RestrictedBoltzmannMachines: BinaryRBM, energy, free_energy, metropolis!

N = 5
M = 2
rbm = BinaryRBM(randn(N), randn(M), randn(N, M) / √N);

β = 0.5
nsteps = 10000
nchains = 100
v = bitrand(N, nchains, nsteps)
metropolis!(v, rbm; β);

counts = Dict{BitVector, Int}()
for t in 1000:nsteps, n in 1:nchains
    counts[v[:,n,t]] = get(counts, v[:,n,t], 0) + 1
end
freqs = Dict(v => c / sum(values(counts)) for (v,c) in counts);

𝒱 = [BitVector(digits(Bool, x; base=2, pad=N)) for x in 0:(2^N - 1)];
ℱ = free_energy.(Ref(rbm), 𝒱);

fig = Makie.Figure()
ax = Makie.Axis(fig[1,1], xlabel="empirical freqs.", ylabel="log(p)", xscale=log10, yscale=log10)
Makie.scatter!(ax, [get(freqs, v, 0.0) for v in 𝒱], exp.(-β * ℱ .- logsumexp(-β * ℱ)))
Makie.scatter!(ax, [get(freqs, v, 0.0) for v in 𝒱], exp.(-ℱ .- logsumexp(-ℱ)))
Makie.abline!(ax, 0, 1, color=:red)
fig

# Correlation

cor([get(freqs, v, 0.0) for v in 𝒱], exp.(-β * ℱ .- logsumexp(-β * ℱ)))


text:
"""
Implementation of Bundle-Trust-Region method based on
    Kim, Kibaek, Cosmin G. Petra, and Victor M. Zavala. "An asynchronous bundle-trust-region method for
    dual decomposition of stochastic mixed-integer programming." SIAM Journal on Optimization 29.1 (2019): 318-342.

"""

using LinearAlgebra
using Printf

mutable struct TrustRegionMethod <: AbstractMethod
    model::BundleModel

    y::Array{Float64,1}  # current iterate of dimension n
    fy::Array{Float64,1} # objective values at y for N functions
    θ::Array{Float64,1}  # θ value of trust region master problem
    g::Dict{Int,SparseVector{Float64}}  # subgradients of dimension n for N functions

    iter::Int # iteration counter

    x_lb::Array{Float64,1}  # original variable lower bound
    x_ub::Array{Float64,1}  # original variable upper bound
    Δ::Float64              # current trust region size
    x0::Array{Float64,1}	# current trust region center (at iteration k)
    fx0::Array{Float64,1}	# current best objective values
    linerr::Float64         # linearization error

    statistics::Dict{Any,Any} # arbitrary collection of statistics

    null_count::Int         # ineffective search count
    eval_time::Float64      # function evaluation time
    start_time::Float64     # start time

    params::Parameters

    # Constructor
    function TrustRegionMethod(n::Int, N::Int, func;
        init::Array{Float64,1} = zeros(n),
        params::Parameters = Parameters())

        trm = new()

        @assert length(init) == n

        trm.y = copy(init)
        trm.fy, trm.g = func(trm.y)
        trm.θ = zeros(N)

        trm.iter = 0

        trm.x_lb = zeros(n)
        trm.x_ub = zeros(n)
        trm.Δ = 10.0
        trm.x0 = copy(init)
        trm.fx0 = copy(trm.fy)
        trm.linerr = 0.0

        trm.statistics = Dict(
            "total_eval_time" => 0.0)

        trm.null_count = 0
        trm.start_time = time()

        trm.params = params

        trm.model = BundleModel(n, N, params.ncuts_per_iter, func)
        return trm
    end
end

TrustRegionMethod(n::Int, N::Int, func, init::Array{Float64,1}) = TrustRegionMethod(n, N, func, init = init)

function store_initial_variable_bounds!(method::TrustRegionMethod)
    bundle = get_model(method)
    model = get_model(bundle)
    x = model[:x]
    for i = 1:bundle.n
        if has_lower_bound(x[i])
            method.x_lb[i] = lower_bound(x[i])
        else
            method.x_lb[i] = -Inf
        end
        if has_upper_bound(x[i])
            method.x_ub[i] = upper_bound(x[i])
        else
            method.x_ub[i] = Inf
        end
    end
end

function build_bundle_model!(method::TrustRegionMethod)
    add_variables!(method)
    store_initial_variable_bounds!(method)
    add_objective_function!(method)
    add_constraints!(method)
end

# This returns BundleModel object.
get_model(method::TrustRegionMethod)::BundleModel = method.model

# This returns solution.
get_solution(method::TrustRegionMethod) = method.x0

# This returns objective value.
get_objective_value(method::TrustRegionMethod) = sum(method.fx0)

function evaluate_functions!(method::TrustRegionMethod)
    stime = time()
    method.fy, method.g = method.model.evaluate_f(method.y)
    method.statistics["total_eval_time"] += time() - stime
end

# This will specifically add trust region bounds to model
function add_constraints!(method::TrustRegionMethod)
    bundle = get_model(method)
    model = get_model(bundle)
    Δ = method.Δ
    x = model[:x]
    center = method.x0
    for i = 1:bundle.n
        set_lower_bound(x[i], max(center[i] - Δ, method.x_lb[i]))
        set_upper_bound(x[i], min(center[i] + Δ, method.x_ub[i]))
    end
end

function add_bundle_constraint!(
    method::TrustRegionMethod, y::Array{Float64,1}, fy::Float64, g::SparseVector{Float64}, θ::JuMP.VariableRef)
    bundle = get_model(method)
    model = get_model(bundle)
    x = model[:x]
    @constraint(model, fy + sum(g[i] * (x[i] - y[i]) for i = 1:bundle.n) <= θ)
end

function add_bundles!(method::TrustRegionMethod)
    y = method.y
    fy = method.fy
    g = method.g

    bundle = get_model(method)

    # compute linearization error
    method.linerr = sum(method.fx0) - sum(fy)
    for j = 1:bundle.N, i = 1:bundle.n
        method.linerr -= g[j][i] * (method.x0[i] - y[i])
    end

    # add bundles constraints to the model
    θ = bundle.model[:θ]
    for i = 1:bundle.ncuts_per_iter
        agg_fy = sum(fy[j] for j in bundle.cut_indices[i])
        agg_g = sum(g[j] for j in bundle.cut_indices[i])
        add_bundle_constraint!(method, y, agg_fy, agg_g, θ[i])
    end
end

function collect_model_solution!(method::TrustRegionMethod)
    bundle = get_model(method)
    model = get_model(bundle)
    if JuMP.termination_status(model) in [MOI.OPTIMAL, MOI.LOCALLY_SOLVED]
        x = model[:x]
        θ = model[:θ]
        for i = 1:bundle.n
            method.y[i] = JuMP.value(x[i])
        end
        for j = 1:bundle.ncuts_per_iter
            method.θ[j] = JuMP.value(θ[j])
        end
    else
        @error "Unexpected model solution status ($(JuMP.termination_status(model)))"
    end
end

function termination_test(method::TrustRegionMethod)::Bool
    model = get_jump_model(method)
    if JuMP.termination_status(model) ∉ [MOI.OPTIMAL, MOI.ALMOST_OPTIMAL, MOI.LOCALLY_SOLVED]
        return true
    end
    if sum(method.fx0) - sum(method.θ) <= method.params.ϵ_s * (1 + abs(sum(method.fx0))) #&& !is_trust_region_binding(method)
        println("TERMINATION: Optimal")
        return true
    end
    if method.iter >= method.params.maxiter
        println("TERMINATION: Maximum number of iterations reached.")
        return true
    end
    if sum(method.fx0) <= method.params.obj_limit
        println("TERMINATION: Dual objective limit reached.")
        return true
    end
    return false
end

# Update bundles and trust region constraints based on
function update_bundles!(method::TrustRegionMethod)
    predicted_decrease_ratio = (sum(method.fx0) - sum(method.fy)) / (sum(method.fx0) - sum(method.θ))
    if predicted_decrease_ratio >=  method.params.m_L

        is_binding = is_trust_region_binding(method)

        # serious step
        method.x0 = copy(method.y)
        method.fx0 = copy(method.fy)

        if is_binding && predicted_decrease_ratio >= 0.5
            update_Δ_serious_step!(method)
        end
        method.null_count = 0
    else
        # null step
        add_bundles!(method::TrustRegionMethod)

        ρ = min(1.0, method.Δ) * (-predicted_decrease_ratio)
        if ρ > 0
            method.null_count += 1
        end
        if ρ > 3 || (method.null_count >= 3 && ρ > 1)
            update_Δ_null_step!(method, ρ)
            method.null_count = 0
        end
    end
    # add new trust region bounds
    add_constraints!(method::TrustRegionMethod)
end

# This displays iteration information.
function display_info!(method::TrustRegionMethod)
    model = get_jump_model(method)
    nrows = 0
    for tp in [MOI.LessThan{Float64}, MOI.EqualTo{Float64}, MOI.GreaterThan{Float64}]
        nrows += num_constraints(model, AffExpr, tp)
    end
    @printf("Iter %4d: ncols %5d, nrows %5d, Δ %e, fx0 %+e, m %+e, fy %+e, linerr %+e, master time %6.1fs, eval time %6.1fs, time %6.1fs\n",
        method.iter, num_variables(model), nrows, method.Δ, sum(method.fx0), sum(method.θ), sum(method.fy), method.linerr,
        sum(method.model.time), method.statistics["total_eval_time"], time() - method.start_time)
end

function update_iteration!(method::TrustRegionMethod)
    method.iter += 1
end

function is_trust_region_binding(method::TrustRegionMethod)
    is_binding = false
    bundle = get_model(method)
    model = get_model(bundle)
    x = model[:x]
    for i = 1:bundle.n
        xval = JuMP.value(x[i])
        if isapprox(xval, method.x0[i] + method.Δ) || isapprox(xval, method.x0[i] - method.Δ)
            is_binding = true
            break
        end
    end
    return is_binding
end

# The following functions are specific to trust region method
function update_Δ_serious_step!(method::TrustRegionMethod)
    method.Δ = min(method.Δ * 2, method.params.Δ_ub)
end

function update_Δ_null_step!(method::TrustRegionMethod, ρ = 4.0)
    method.Δ = max(method.Δ / min(4.0, ρ), method.params.Δ_lb)
end


text:
using Pkg;
# Pkg.add(["Pluto", "PlutoUI", "Plots", "LaTeXStrings", "TikzPictures", "Javis"]);
Pkg.add(["Pluto", "PlutoUI"]);
Pkg.precompile();


text:
import FileIO:save
using Distributed
using DataFrames, DataFramesMeta, PyPlot
using Dates, Statistics

# Run gridsearch for the Categorical task (2.3.2)
# (i.e. find optimal parameter values for a categorical learner)
function run_gridsearch_multinomial(;learnertype = TLeakyIntegrator,
                            learnerparametervalues = collect(eps():0.03:0.9999999),
                            foldername = "gridsearch_multi",
                            filename = "leaky",
                            nseeds = 3,
                            # nsteps = 10^6,
                            nsteps = 10^5,
                            # changeprobabilities = [0.001, 0.0001], # With 10^6 steps
                            changeprobabilities = [0.1, 0.05, 0.01, 0.005], # With 10^5 steps
                            stochasticities = [5., 2., 1., 0.25, 0.14, 0.1, 0.01],
                            Npar = 20)

    #rand(1:typemax(UInt64)-1, nseeds)
    magicseeds = [0x04b1a7a85370a670, 0xdedefc91f311b389, 0x32cf09cbbeb603b6]
    specs = SpecsMultinomial(
                    stochasticities = stochasticities,
                    changeprobabilities = changeprobabilities,
                    # changeprobabilities = [10^i for i in range(-4, length=40, stop=-1)], # for robustness
                    nseeds = nseeds, seedenvpolicy = magicseeds,
                    seedlearner = magicseeds,
                    ns = 5, nsteps = nsteps)

    gridsearch(specs, learnertype, learnerparametervalues, foldername, filename,
                Npar = Npar)
end

function gridsearch(specs::SpecsMultinomial, learnertype, learnerparametervalues,
                        foldername::String, filename::String; Npar = 20)

    println("Starting: " * Dates.format(Dates.now(), "yyyy-mm-dd_HH-MM-SS"))
    @show learnertype
    settings = collect(Iterators.product(learnerparametervalues,
                        specs.stochasticities, specs.changeprobabilities))
    df = @distributed vcat for setting in settings
        meanterrors, stdterrors, seerrors = grid_core(specs, learnertype, setting,
                                            Npar = Npar)
        DataFrame(param = setting[1],
                stochasticity = setting[2],
                changeprobability = setting[3],
                meanterrors = meanterrors,
                stdterrors = stdterrors,
                seerrors = seerrors)
    end
    println("Finishing: " * Dates.format(Dates.now(), "yyyy-mm-dd_HH-MM-SS"))
    savepath = makehomesavepath(foldername); mkdir(savepath)
    save(joinpath(savepath, foldername * "_" * string(filename)*".jld2"),
        Dict("df" => df,  "learnertype" => string(learnertype),
        "specs" => specs,
        "seedenvpolicy" => specs.seedenvpolicy,
        "seedlearner" => specs.seedlearner))
end

function grid_core(specs::SpecsMultinomial, learnertype, setting; Npar = 20)
    terrors = []
    for i in 1:length(specs.seedenvpolicy)
        parameterset = getgridparamset(learnertype, setting, Npar = Npar)
        callback = run!(specs.envtype, learnertype, parameterset,
                        ns = specs.ns,
                        stochasticity = setting[2],
                        changeprobability = setting[3],
                        nsteps = specs.nsteps,
                        iseedenvpolicy = specs.seedenvpolicy[i],
                        iseedlearner = specs.seedlearner[i]
                        )
        terrortmp = computesquareddiff(callback.true_history,
                                        callback.estimated_history)
        push!(terrors, terrortmp)
    end
    terrors = collect(skipmissing(reduce(vcat, terrors)))
    meanterrors = mean(terrors)
    stdterrors = std(terrors)
    seerrors = stdterrors / sqrt(length(terrors))
    meanterrors, stdterrors, seerrors
end


text:
export rule

@rule AR(:γ, Marginalisation) (q_y_x::MultivariateNormalDistributionsFamily, q_θ::NormalDistributionsFamily, meta::ARMeta) = begin
    order = getorder(meta)
    F     = getvform(meta)

    myx, Vyx = mean(q_y_x), cov(q_y_x)

    mA, Vθ = as_companion_matrix(mean(q_θ)), cov(q_θ)
    my, Vy = ar_slice(F, myx, 1:order), ar_slice(F, Vyx, 1:order, 1:order)
    mx, Vx = ar_slice(F, myx, order + 1:2order), ar_slice(F, Vyx, order+1:2order, order+1:2order)
    Vyx    = ar_slice(F, Vyx, order+1:2order, 1:order)

    B = (Vy + my * my')[1, 1] - 2 * (mA * (Vyx + mx * my'))[1, 1] + (mA * (Vx + mx * mx') * mA')[1, 1] + tr(Vθ * (Vx + mx * mx'))

    return GammaShapeRate(3.0 / 2.0, B / 2.0)
end


text:
using PATHSolver
using ForwardDiff, SparseArrays, Random
using Test

include("sparse_matrix.jl")
include("mcp.jl")
include("lcp.jl")
include("custom_nnz.jl")


text:
"""
    A PETSc Mat wrapper.
"""
mutable struct PetscMat <: AbstractMatrix{PetscScalar}

    " The pointer to the PETSc Mat object "
    mat::Ref{Mat}

    " Whether or not a size has been set by calling setSize() "
    sized::Bool

    " Whether or not preallocation has been done by calling setPreallocation() "
    preallocated::Bool

    " Whether or not the matrix has ever been assembled (Note: the matrix might currently NOT be assembled) "
    assembled::Bool

    function PetscMat()
        mat = Ref{Mat}()
        ccall((:MatCreate, library), PetscErrorCode, (comm_type, Ref{Mat}), MPI.COMM_WORLD, mat)
        ccall((:MatSetType, library), PetscErrorCode, (Mat, MatType), mat[], MATAIJ)
        new(mat, false, false, false)
    end
end

"""
    Set up the size of a PetscMat

    Note: This MUST be called before setting Pre-allocation and _definitely_ must be called before setting/getting values!
"""
function setSize!(mat::PetscMat; m_local::PetscInt=PETSC_DECIDE, n_local::PetscInt=PETSC_DECIDE, m_global::PetscInt=PETSC_DETERMINE, n_global::PetscInt=PETSC_DETERMINE)
    @assert !mat.sized

    # Must provide _some_ size!
    @assert (m_local != PETSC_DECIDE && n_local != PETSC_DECIDE) || (m_global != PETSC_DETERMINE && n_global != PETSC_DETERMINE)

    ccall((:MatSetSizes, library), PetscErrorCode, (Mat, PetscInt, PetscInt, PetscInt, PetscInt), mat.mat[], m_local, n_local, m_global, n_global)

    mat.sized = true
end

"""
    Set the preallocation for storage in each row.

    Note: Should be called _after_ setSize!
"""
function setPreallocation!(mat::PetscMat, local_nonzeros_per_row::Array{PetscInt}, off_processor_nonzeros_per_row::Array{PetscInt})
    @assert mat.sized
    @assert !mat.preallocated

    ccall((:MatSeqAIJSetPreallocation, library), PetscErrorCode, (Mat, PetscInt, Ptr{PetscInt}),
          mat.mat[], PETSC_DEFAULT, local_nonzeros_per_row)

    ccall((:MatMPIAIJSetPreallocation, library), PetscErrorCode, (Mat, PetscInt, Ptr{PetscInt}, PetscInt, Ptr{PetscInt}),
          mat.mat[], PETSC_DEFAULT, local_nonzeros_per_row, PETSC_DEFAULT, off_processor_nonzeros_per_row)

    mat.preallocated = true
end

"""
    Must be called after setting/adding values to construct the matrix
"""
function assemble!(mat::PetscMat)
    @assert mat.sized
    @assert mat.preallocated

    ccall((:MatAssemblyBegin, library), PetscErrorCode, (Mat, MatAssemblyType), mat.mat[], MAT_FINAL_ASSEMBLY)
    ccall((:MatAssemblyEnd, library), PetscErrorCode, (Mat, MatAssemblyType), mat.mat[], MAT_FINAL_ASSEMBLY)

    mat.assembled = true
end

"""
    Use PETSc viewer to print out the matrix
"""
function viewMat(mat::PetscMat)
    @assert mat.sized
    @assert mat.preallocated
    @assert mat.assembled

    viewer = ccall((:PETSC_VIEWER_STDOUT_, library), PetscViewer, (comm_type,), MPI.COMM_WORLD)
    ccall((:MatView, library), PetscErrorCode, (Mat, PetscViewer), mat.mat[], viewer)
end

"""
    mat[i,j] += v

    If the values coming in aren't Floats then we need to make them so
"""
function plusEquals!(mat::PetscMat, v::Matrix{T}, i, j) where T
    plusEquals!(mat, float(v), i, j)
end

"""
    mat[i,j] += v
"""
function plusEquals!(mat::PetscMat, v::Matrix{Float64}, i, j)
    # TODO: do the transpose faster (with loops so there are less copies
    # The transpose is to go from column-major to row-major
    v_T = Vector{Float64}(reshape(v', length(v)))

    # Convert the indices to 0-based indexing
    i_ind = (PetscInt)[i_val-1 for i_val in i]
    j_ind = (PetscInt)[j_val-1 for j_val in j]

    @assert length(v_T) == length(i_ind) * length(j_ind)

    ccall((:MatSetValues, library), PetscErrorCode, (Mat, PetscInt, Ptr{PetscInt}, PetscInt, Ptr{PetscInt}, Ptr{PetscScalar}, InsertMode), mat.mat[], length(i_ind), i_ind, length(j_ind), j_ind, v_T, ADD_VALUES)
end

"""
    mat = 0
"""
function zero!(mat::PetscMat)
    ccall((:MatZeroEntries, library), PetscErrorCode, (Mat,), mat.mat[])
end

"""
    Zeros rows
"""
function zeroRows!(mat::PetscMat, i)
    rows = (PetscInt)[i_ind-1 for i_ind in i]
    ccall((:MatSetOption, library), PetscErrorCode, (Mat, MatOption, PetscBool), mat.mat[], MAT_KEEP_NONZERO_PATTERN, PETSC_TRUE)
    ccall((:MatZeroRows, library), PetscErrorCode, (Mat, PetscInt, Ptr{PetscInt}, PetscScalar, Vec, Vec), mat.mat[], length(rows), rows, 0., C_NULL, C_NULL)
end

#### AbstractArray Interface Definitions ###

"""
    PETSc Matrices are inherently 2D
"""
function linearindexing(mat::PetscMat)
    return Base.LinearSlow()
end

import Base.size

"""
    Returns the _global_ size of the matrix
"""
function size(mat::PetscMat)
    M = Ref{PetscInt}(0)
    N = Ref{PetscInt}(0)

    ccall((:MatGetSize, library), PetscErrorCode, (Mat, Ref{PetscInt}, Ref{PetscInt}), mat.mat[], M, N)

    return (M[], N[])
end

import Base.setindex!

"""
    Sets the value at i,j
"""
function setindex!(mat::PetscMat, v, i, j)
    # Copy out the values
    val = (Float64)[v_val for v_val in v]

    # Call the specialization below
    setindex!(mat, val, i, j)
end

"""
    Sets a matrix into the larger matrix
"""
function setindex!(mat::PetscMat, v::Matrix{Float64}, i, j)
    # TODO: do the transpose faster (with loops so there are less copies
    # The transpose is to go from column-major to row-major
    v_T = reshape(v', length(v))

    # Call the specialization below
    setindex!(mat, v_T, i, j)
end

"""
    Sets the value at i,j.

    Specialization for when v is already an array of Float64 (faster because we don't need to copy it)

    HUGE NOTE!!!  This assumes that `v` is already laid out _row-major_.  If that's not true call one of the above functions...
"""
function setindex!(mat::PetscMat, v::Array{Float64}, i, j)
    # Convert the indices to 0-based indexing and flip them for row-major
    i_ind = (PetscInt)[i_val-1 for i_val in i]
    j_ind = (PetscInt)[j_val-1 for j_val in j]

    @assert length(v) == length(i_ind) * length(j_ind)

    ccall((:MatSetValues, library), PetscErrorCode, (Mat, PetscInt, Ptr{PetscInt}, PetscInt, Ptr{PetscInt}, Ptr{PetscScalar}, InsertMode), mat.mat[], length(i_ind), i_ind, length(j_ind), j_ind, v, INSERT_VALUES)
end

"""
    Don't do this
"""
function setindex!(mat::PetscMat, v, I)
    error("Attempt to index PetscMatrix using a single dimension!")
end

import Base.getindex

"""
    Don't do this either
"""
function getindex(mat::PetscMat, I)
    error("Attempt to index PetscMatrix using a single dimension!")
end

"""
    Proper getter for entries from the matrix for integer indices
"""
function getindex(mat::PetscMat, i::T, j::T) where T<:Integer
    # Don't forget about 1-based indexing...
    i_ind = (PetscInt)[i-1]
    j_ind = (PetscInt)[j-1]

    get_vals = Vector{Float64}(undef, 1)

    ccall((:MatGetValues, library), PetscErrorCode, (Mat, PetscInt, Ptr{PetscInt}, PetscInt, Ptr{PetscInt}, Ref{PetscScalar}), mat.mat[], 1, i_ind, 1, j_ind, get_vals)

    return get_vals[1]
end

"""
    Proper getter for entries from the matrix
"""
function getindex(mat::PetscMat, i, j)
    i_ind = (PetscInt)[i_val-1 for i_val in i]
    j_ind = (PetscInt)[j_val-1 for j_val in j]

    get_vals = Vector{Float64}(undef, length(i_ind) * length(j_ind))

    ccall((:MatGetValues, library), PetscErrorCode, (Mat, PetscInt, Ptr{PetscInt}, PetscInt, Ptr{PetscInt}, Ref{PetscScalar}), mat.mat[], length(i_ind), i_ind, length(j_ind), j_ind, get_vals)

    return reshape(get_vals, length(j_ind), length(i_ind))'
end


text:
# simple scheduler metadata store

const SHAREMODE_KEY = "S"
const TIMEOUT_KEY = ""

"""
Metadata store using broker in-memory datastructures
"""
mutable struct SimpleExecutorMeta <: ExecutorMeta
    path::String
    brokerid::Int
    proclocal::Dict{String,Any}
    donetasks::Set{TaskIdType}
    sharemode::ShareMode
    results_channel::Union{Void,RemoteChannel{Channel{Tuple{String,Vector{UInt8}}}}}
    add_annotation::Function
    del_annotation::Function
    result_callback::Union{Function,Void}
    cachingpool::Union{Base.Distributed.CachingPool, Void}

    function SimpleExecutorMeta(path::String, sharethreshold::Int)
        new(path, myid(),
            Dict{String,Any}(),
            Set{TaskIdType}(),
            ShareMode(sharethreshold),
            nothing,
            identity, identity, nothing, nothing)
    end
end

function Base.show(io::IO, M::SimpleExecutorMeta)
    print(io, "SimpleExecutorMeta(", M.path, ")")
end

function init(M::SimpleExecutorMeta, brokerid::Int; add_annotation=identity, del_annotation=identity, result_callback=nothing)
    M.brokerid = brokerid
    M.add_annotation = add_annotation
    M.del_annotation = del_annotation
    M.result_callback = result_callback
    pid = myid()
    M.results_channel = brokercall(broker_register_for_results, M, pid)
    M.cachingpool = Base.Distributed.CachingPool([brokerid])
    donetasks = brokercall(broker_get_donetasks, M)::Set{TaskIdType}
    union!(M.donetasks, donetasks)
    nothing
end

function process_trigger(M::SimpleExecutorMeta, k::String, v::Vector{UInt8})
    if k == TIMEOUT_KEY
        # ignore
    elseif k == SHAREMODE_KEY
        ncreated,ndeleted = meta_deser(v)
        S = M.sharemode
        if ncreated > S.ncreated
            S.ncreated = ncreated
        end
        if ndeleted > S.ndeleted
            S.ndeleted = ndeleted
        end
        S.nshared = ncreated - ndeleted
    else
        val = meta_deser(v)
        M.proclocal[k] = val
        id = parse(TaskIdType, basename(k))
        push!(M.donetasks, id)
        if M.result_callback !== nothing
            M.result_callback(id, val)
        end
    end
    nothing
end

function process_triggers(M::SimpleExecutorMeta)
    while isready(M.results_channel)
        (k,v) = take!(M.results_channel)
        process_trigger(M, k, v)
    end
end

function wait_trigger(M::SimpleExecutorMeta; timeoutsec::Int=5)
    trigger = M.results_channel
    fire = true
    if !isready(trigger)
        @schedule begin
            sleep(timeoutsec)
            fire && !isready(trigger) && put!(trigger, (TIMEOUT_KEY,Vector{UInt8}()))
        end
    end

    # process results and sharemode notifications
    (k,v) = take!(M.results_channel)
    fire = false
    process_trigger(M, k, v)
    process_triggers(M)

    nothing
end

function delete!(M::SimpleExecutorMeta)
    reset(M)
    if myid() === M.brokerid
        empty!(META)
        empty!(TASKS)
        empty!(PID_RR_MAP)
        empty!(PID_TASK_MAP)
        deregister(RESULTS)
    end
    nothing
end

function reset(M::SimpleExecutorMeta)
    reset(M.sharemode)
    empty!(M.proclocal)
    empty!(M.donetasks)
    M.cachingpool = nothing
    M.results_channel = nothing
    M.add_annotation = identity
    M.del_annotation = identity
    M.result_callback = nothing

    nothing
end

function cleanup(M::SimpleExecutorMeta)
end

function detach(M::SimpleExecutorMeta, pid)
    taskmap = Dict{TaskIdType,TaskIdType}()
    if (myid() === M.brokerid) && (pid in keys(PID_RR_MAP))
        rr = PID_RR_MAP[pid]
        delete!(PID_RR_MAP, pid)
        deregister(RESULTS, rr)
        if pid in keys(PID_TASK_MAP)
            taskmap = PID_TASK_MAP[pid]
            delete!(PID_TASK_MAP, pid)
        end
    end
    taskmap
end

@timetrack function share_task(M::SimpleExecutorMeta, id::TaskIdType, allow_dup::Bool)
    annotated_task = M.add_annotation(id)
    pid = myid()
    brokercall(broker_share_task, M, id, annotated_task, allow_dup, pid)
    nothing
end

@timetrack function steal_task(M::SimpleExecutorMeta, selector=default_task_scheduler)
    pid = myid()
    taskid = remotecall_fetch(broker_steal_task, M.cachingpool, selector, pid)::TaskIdType
    ((taskid === NoTask) ? taskid : M.del_annotation(taskid))::TaskIdType
end

@timetrack function set_result(M::SimpleExecutorMeta, id::TaskIdType, val; refcount::Int=0, processlocal::Bool=true)
    k = resultpath(M, id)
    M.proclocal[k] = val
    if refcount > 1
        DagScheduler.RefCounter[].refcount(id, refcount)
    end
    if !processlocal
        val = meta_pack(val)
        serval = meta_ser(val)
        pid = myid()
        brokercall(broker_set_result, M, k, serval, id, pid)
    end
    push!(M.donetasks, id)
    nothing
end

@timetrack function get_result(M::SimpleExecutorMeta, id::TaskIdType)
    k = resultpath(M, id)
    if k in keys(M.proclocal)
        val = M.proclocal[k]
    else
        v = brokercall(broker_get_result, M, k)::Vector{UInt8}
        val = meta_deser(v)
        M.proclocal[k] = val
    end
    meta_unpack(val)
end

function has_result(M::SimpleExecutorMeta, id::TaskIdType)
    #process_triggers(M)
    id in M.donetasks
end

function decr_result_ref(M::SimpleExecutorMeta, id::TaskIdType)
    DagScheduler.RefCounter[].refcount(id, -1)
end

function export_local_result(M::SimpleExecutorMeta, id::TaskIdType, executable)
    k = resultpath(M, id)
    (k in keys(M.proclocal)) || return

    exists = brokercall(broker_has_result, M, k)::Bool
    exists && return

    val = repurpose_result_to_export(executable, M.proclocal[k])
    val = meta_pack(val)
    serval = meta_ser(val)
    pid = myid()
    brokercall(broker_set_result, M, k, serval, id, pid)
    nothing
end

# --------------------------------------------------
# methods invoked at the broker
# --------------------------------------------------
function broker_has_result(k)
    k in keys(META)
end

function broker_share_task(id::TaskIdType, annotated::TaskIdType, allow_dup::Bool, pid::Int)
    M = (DagScheduler.genv[].meta)::SimpleExecutorMeta
    s = sharepath(M, id)
    canshare = withtaskmutex() do
        if !(s in keys(META))
            META[s] = ""
            true
        else
            false
        end
    end
    canshare |= allow_dup
    if canshare
        push!(TASKS, annotated)
        M.sharemode.ncreated += 1
        M.sharemode.nshared += 1
        broker_send_sharestats(M)
        broker_remove_pending_task(id, pid)
    end
    nothing
end

function broker_steal_task(selector, executorid)
    genv = DagScheduler.genv[]
    (genv === nothing) && (return NoTask)
    M = (genv.meta)::SimpleExecutorMeta
    taskid = withtaskmutex() do
        taskid = NoTask
        if !isempty(TASKS)
            pos = selector(TASKS)
            taskid = splice!(TASKS, pos)
            M.sharemode.nshared -= 1
            M.sharemode.ndeleted += 1
        end
        taskid
    end
    if taskid !== NoTask
        broker_send_sharestats(M)
        broker_add_pending_task(M.del_annotation(taskid), taskid, executorid)
    end
    taskid
end

function broker_send_sharestats(M)
    @schedule begin
        sm = M.sharemode
        c,d = sm.ncreated,sm.ndeleted
        put!(RESULTS, (SHAREMODE_KEY, meta_ser((c,d))))
    end
    nothing
end

function broker_set_result(k::String, val::Vector{UInt8}, id, pid)
    META[k] = val
    put!(RESULTS, (k,val))
    broker_remove_pending_task(id, pid)
    nothing
end

# remove task id from pending list of broker id
broker_remove_pending_task(id, pid) = broker_remove_pending_task(TaskIdType(id), Int(pid))
function broker_remove_pending_task(id::TaskIdType, pid::Int)
    (pid in keys(PID_TASK_MAP)) && delete!(PID_TASK_MAP[pid], id)
    nothing
end

# add task id as pending from executor
broker_add_pending_task(id, annotatedid, pid) = broker_add_pending_task(TaskIdType(id), TaskIdType(annotatedid), Int(pid))
function broker_add_pending_task(id::TaskIdType, annotatedid::TaskIdType, pid::Int)
    tasks = get!(()->Dict{TaskIdType,TaskIdType}(), PID_TASK_MAP, pid)
    tasks[id] = annotatedid
    nothing
end

function broker_get_result(k::String)
    META[k]
end

function broker_register_for_results(pid)
    rr = register(RESULTS)
    PID_RR_MAP[pid] = rr
    rr
end

function broker_get_donetasks()
    genv = DagScheduler.genv[]
    (genv === nothing) && (return Set{TaskIdType}())
    M = (genv.meta)::SimpleExecutorMeta
    M.donetasks
end


text:
abstract type QuerySynthesisCVWrapperOracle <: Oracle end

function QuerySynthesisCVWrapperOracle(data::Array{T, 2}, labels::Vector{Symbol}, params::Dict{Symbol, Any}=Dict{Symbol, Any}())::Oracle where T <: Real
    :subtype in keys(params) || throw(ArgumentError("Parameter 'subtype' missing for QuerySynthesisCVWrapperOracle."))
    !(params[:subtype] <: QuerySynthesisCVWrapperOracle) || throw(ArgumentError("Subtype $(params[:subtype]) not allowed for QuerySynthesisCVWrapperOracle."))
    gamma_search_range = get(params, :gamma_search_range_oracle, 10.0.^range(-2, stop=2, length=20))
    C = get(params, :C, 1)
    num_folds = get(params, :num_folds, 5)
    metric = eval(get(params, :metric, :matthews_corr))

    data_inliers, data_outliers = SVDD.generate_binary_data_for_tuning(data)
    if isempty(data_inliers)
        data_merged = hcat(data, data_outliers)
        ground_truth = vcat(labels, fill(:outlier, size(data_outliers, 2)))
    else
        data_merged = hcat(data, data_inliers, data_outliers)
        ground_truth = vcat(labels, fill(:inlier, size(data_inliers, 2)),
                            fill(:outlier, size(data_outliers, 2)))
    end

    folds = StratifiedKfold(ground_truth, num_folds)
    best_gamma = 1.0
    best_score = -Inf
    info(getlogger(@__MODULE__), "[ORACLE] Testing $(length(gamma_search_range)) gamma values.")
    for gamma in gamma_search_range
        info(getlogger(@__MODULE__), "[ORACLE] Testing gamma = $gamma.")
        cur_scores = []
        for f in folds
            train_mask = falses(length(ground_truth))
            train_mask[f] .= true
            test_mask = .~train_mask

            params[:init_strategy] = SVDD.SimpleCombinedStrategy(SVDD.FixedGammaStrategy(MLKernels.GaussianKernel(gamma)), SVDD.FixedCStrategy(C))
            model = initialize_oracle(params[:subtype], data_merged[:, train_mask], ground_truth[train_mask], params)

            prediction = ask_oracle(model, data_merged[:, findall(test_mask)])
            cm = ConfusionMatrix(prediction, ground_truth[test_mask], pos_class=:inlier, neg_class=:outlier)
            push!(cur_scores, metric(cm))
        end
        score = mean(cur_scores)
        debug(getlogger(@__MODULE__), "[ORACLE] gamma = $gamma, score = $score.")
        if score >= best_score
            info(getlogger(@__MODULE__), "[ORACLE] New best fond with gamma = $gamma and score = $score.")
            best_gamma = gamma
            best_score = score
        end
    end
    info(getlogger(@__MODULE__), "[ORACLE] Final gamma = $best_gamma with score = $best_score.")
    params[:init_strategy] = SVDD.SimpleCombinedStrategy(SVDD.FixedGammaStrategy(MLKernels.GaussianKernel(best_gamma)), SVDD.FixedCStrategy(C))
    model = initialize_oracle(params[:subtype], data_merged, ground_truth, params)
    return model
end


text:
module CrossEntropyMethod
    using Distributions
    using Random
    using ProgressMeter
    using POMDPs
    using POMDPPolicies
    using POMDPSimulators

    export cross_entropy_method, add_categorical_entropy
    include("cross_entropy_method.jl")

    export mdp_loss, mdp_weight, continous_actions_weight, sample_playback_policy_fn
    include("mdp_helpers.jl")

end # module


text:
module ParamChk_Tests

using Compat
using Compat.Test
using IntervalArithmetic
using EAGOParametricInterval

# Tests strict inclusion procedure for interval vectors
Y = [Interval(0,5),Interval(0,5),Interval(0,5)]
X1 = [Interval(1,2),Interval(1,2),Interval(1,2)]
X2 = [Interval(1,2),Interval(-10,10),Interval(1,2)]
X3 = [Interval(1,2),Interval(1,2),Interval(0,5)]
flag1 = EAGOParametricInterval.Strict_XinY(X1,Y)
flag2 = EAGOParametricInterval.Strict_XinY(X2,Y)
flag3 = EAGOParametricInterval.Strict_XinY(X3,Y)
@test flag1 == true
@test flag2 == false
@test flag3 == false

# Checks extended division routine
A1 = Interval(0)
A2 = Interval(0,3)
A3 = Interval(-2,0)
A4 = Interval(-3,2)
ind1,B1,C1 = EAGOParametricInterval.extDivide(A1)
ind2,B2,C2 = EAGOParametricInterval.extDivide(A2)
ind3,B3,C3 = EAGOParametricInterval.extDivide(A3)
ind4,B4,C4 = EAGOParametricInterval.extDivide(A4)
@test ind1 == 0
@test ind2 == 1
@test ind3 == 2
@test ind4 == 3
@test B1 == Interval(-Inf,Inf)
@test 0.33333 - 1E-4 <= B2.lo <= 0.33333 + 1E-4
@test B2.hi == Inf
@test B3 == Interval(-Inf,-0.5)
@test B4.lo == -Inf
@test -0.33333 - 1E-4 <= B4.hi <= -0.33333 + 1E-4
@test C1 == Interval(-Inf,Inf)
@test C2 == Interval(Inf,Inf)
@test C3 == Interval(-Inf,-Inf)
@test C4 == Interval(0.5,Inf)

param = EAGOParametricInterval.Param_Bisect_Opts()
@test param.DAGflag == false

N =  Interval(-5,5)
X = Interval(-5,5)
Mii = Interval(-5,5)
S1 = Interval(-5,5)
S2 = Interval(-5,5)
B = Interval(-5,5)
rtol = 1E-4
indx1,box11,box12 = EAGOParametricInterval.extProcess(N,X,Mii,S1,S2,B,rtol)
Miib = Interval(0,5)
S1b = Interval(1,5)
S2b = Interval(1,5)
Bb = Interval(1,5)
indx2,box21,box22 = EAGOParametricInterval.extProcess(N,X,Miib,S1b,S2b,Bb,rtol)
Miic = Interval(-5,0)
S1c = Interval(1,5)
S2c = Interval(1,5)
Bc = Interval(1,5)
indx3,box31,box32 = EAGOParametricInterval.extProcess(N,X,Miic,S1c,S2c,Bc,rtol)
Miia = Interval(1,5)
S1a = Interval(1,5)
S2a = Interval(1,5)
Ba = Interval(1,5)
indx6,box61,box62 = EAGOParametricInterval.extProcess(N,X,Miia,S1a,S2a,Ba,rtol)
Miid = Interval(0,0)
S1d = Interval(1,5)
S2d = Interval(1,5)
Bd = Interval(1,5)
indx8,box81,box82 = EAGOParametricInterval.extProcess(N,X,Miid,S1d,S2d,Bd,rtol)

@test indx1 == 0
@test box11 == Interval(-Inf,Inf)
@test box12 == Interval(-5,5)

@test indx2 == 0
@test box21.hi > -Inf
@test box22 == Interval(-5,5)

@test indx3 == 0
@test box31.lo < Inf
@test box32 == Interval(-5,5)

@test indx6 == 1
@test -15.0002 - 1E-4 <= box61.lo <= -15.0002 + 1E-4
@test box62.lo == -Inf
@test box61.hi == Inf
@test -0.599979 - 1E-4 <= box62.hi <= -0.599979 + 1E-4

@test indx8 == 0
@test box81 == Interval(-5,5)
@test box82 == Interval(-5,5)

end


text:
#!/usr/bin/env python

using BenchmarkTools
using Printf
using NetCDF
@everywhere using NetCDF

@everywhere begin
    """
        calculate_pressure_levels(numlevels)

    Calculate pressure levels using data from a file for a total number of levels numlevels.
    """
    function calcPressureLevels(numlevels)
        ak = zeros(Float32, numlevels + 1)
        bk = zeros(Float32, numlevels + 1)
        pressures = zeros(Float32, numlevels)

        filename = string(string(numlevels), "-layer.p")
        fileid = open(filename);
        lines = readlines(fileid);
        linenumber = 1;
        k = 1;
        for line in lines
            linenumber += 1;
            if linenumber > 3
                line = strip(line);
                columns = split(line);
                ak[k] = float(columns[2]);
                bk[k] = float(columns[3]);
                k += 1;
            end
        end
        close(fileid);

        for k in 1:numlevels
            pressures[k] = 0.50*((ak[k] + ak[k+1]) + 0.01*1.00e+05*(bk[k] + bk[k+1]));
        end

        return pressures[numlevels:-1:1]
    end
end

@everywhere begin
    """
        getdata(file)

    Read and manipulate a data point from file.
    """
    function getdata(file)
        coefficient = 365.5
        variablename = "aoa"
        referencelatitude = -86.0

        #latitudes = ncread(file, "lat")
        #latitudeindex = findfirst(latitudes, referencelatitude)
        #startlist = [1, latitudeindex, 1, 1]
        startlist = [1, 5, 1, 1]
        countlist = [-1, 1, -1, 1]

        # Read the daily average age of air
        #println( "     --->File: ", file)
        ageofair = ncread(file, variablename, start=startlist, count=countlist) / coefficient

        # Determine the zonal mean
        zonalmean = mean(ageofair, 1)
        zonalmean = zonalmean[1 , 1, :, 1]

        return zonalmean
    end
end

@everywhere begin
    """
        getlevels(file)

    Get the array of levels data from file.
    """
    function getlevels(file)
        levels = ncread(file, "lev")
        numlevels = length(levels)
        levels = calcPressureLevels(numlevels)
        return levels
    end
end

@everywhere begin
    """
        getwholedata(startyear, endyear)

    Get and manipulate data from all of the files.
    """
    function getwholedata(startyear, endyear)
        numdays = 0
        datavalue = zeros(72)

        reference_directory = "../Data/"

        # Loop over the years
        for year in startyear:endyear
            #println( "Processing files for ", year)
            directoryY = reference_directory*"Y"*string(year)*"/"
            fileslist = filter(x -> endswith(x, "_1200z.nc4"),  readdir(directoryY))
            fileslist = [file for file in fileslist]
            numfiles = length(fileslist)
            numdays = numdays + numfiles

            # Loop over the daily files
            @sync begin
                tempvariable = @parallel (hcat) for i = 1:numfiles
                    getdata(string(directoryY, fileslist[i]));
                end;
            end

            # Stack the daily values into an existing array
            datavalue = hcat(datavalue, tempvariable)
            #println(month, "       Size of datavalue: ", size(datavalue))
        end

        @everywhere Base.flush_gc_msgs()
        @everywhere gc()

        return numdays, datavalue
    end
end

# Start time
@btime begin

startyear = 1990
endyear = 2009

numdays, datavalue = getwholedata(startyear, endyear);

    # Remove the first row
    datavalue = datavalue[setdiff(1:end, 1), :] # delete the first row

# End time
end

if numdays == 7305
   println("Successfully read ", numdays, "files")
else
   println("Only read ", numdays, "files! Check your script.")
end


text:
#=
Copyright 2021 BlackRock, Inc.

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
=#

"""
    Interval(lb::Real, ub::Real)

Represents a univariate interval.

The fields represent:
- `lb`: Lower bound of the interval
- `ub`: Upper bound of the interval

# Example
```jldoctest
julia> interval = Interval(0., 1.)
[0.00000, 1.00000]

julia> Interval()
ℝ

```
"""
mutable struct Interval
    lb::Float64
    ub::Float64
    function Interval(lb::Real, ub::Real)
        @assert !isnan(lb)
        @assert !isnan(ub)
        return new(lb, ub)
    end
end

Interval() = Interval(-Inf, Inf)

function Base.show(io::IO, A::Interval)
    if A.lb == -Inf && A.ub == Inf
        @printf(io, "ℝ\n")
    else
        @printf(io, "[%.5f, %.5f]\n", A.lb, A.ub)
    end
end

#####
##### Boolean
#####

"""
    isempty(A::Interval)

Return `true` if the interval `A` is empty (`isempty(A::Interval)`).
"""
isempty(A::Interval) = A.lb > A.ub

∈(x::Real, A::Interval) = (x ≥ A.lb) & (x ≤ A.ub)

function ⊆(A::Interval, B::Interval)
    return (A.lb ∈ B) & (A.ub ∈ B)
end

==(A::Interval, B::Interval) = (A.lb == B.lb) && (A.ub == B.ub)

≈(A::Interval, B::Interval) = (A.lb ≈ B.lb) && (A.ub ≈ B.ub)

function <(A::Interval, B::Interval)
    return A.ub < B.lb
end

function >(A::Interval, B::Interval)
    return B < A
end

∩(A::Interval, B::Interval) = Interval(max(A.lb, B.lb), min(A.ub, B.ub))


text:
# Bogumił Kamiński, 2022

# Codes for chapter 6

# Code for listing 6.1

import Downloads
Downloads.download("https://raw.githubusercontent.com/" *
                   "sidooms/MovieTweetings/" *
                   "44c525d0c766944910686c60697203cda39305d6/" *
                   "snapshots/10K/movies.dat",
                   "movies.dat")

# Code for string interpolation examples

x = 10
"I have $x apples"

"I have $(2 * x) apples"

"I have \$100."
"I have $100."

# Code for multiline strings

Downloads.download("https://raw.githubusercontent.com/\
                    sidooms/MovieTweetings/\
                    44c525d0c766944910686c60697203cda39305d6/\
                    snapshots/10K/movies.dat",
                   "movies.dat")

"a\
 b\
 c"

# Code for raw strings

"C:\my_folder\my_file.txt"

raw"C:\my_folder\my_file.txt"

# Code for listing 6.2

movies = readlines("movies.dat")

# Code for section 6.2

movie1 = first(movies)

movie1_parts = split(movie1, "::")

supertype(String)
supertype(SubString{String})

# Code for section 6.3

movie1_parts[2]

rx = r"(.*) \((\d{4})\)$"

m = match(rx, movie1_parts[2])

m[1]
m[2]

parse(Int, m[2])

# Code for listing 6.3

function parseline(line::AbstractString)
    parts = split(line, "::")
    m = match(r"(.*) \((\d{4})\)", parts[2])
    return (id=parts[1],
            name=m[1],
            year=parse(Int, m[2]),
            genres=split(parts[3], "|"))
end

# Code for parsing one line of movies data

record1 = parseline(movie1)

# Code for listing 6.4

codeunits("a")
codeunits("ε")
codeunits("∀")

# Codes for different patterns of string subsetting

word = first(record1.name, 8)

record1.name[1:8]

for i in eachindex(word)
    println(i, ": ", word[i])
end

codeunits("ô")

codeunits("Fantômas")

isascii("Hello world!")
isascii("∀ x: x≥0")

word[1]
word[5]

# Code for section 6.5

records = parseline.(movies)

genres = String[]
for record in records
    append!(genres, record.genres)
end
genres

using FreqTables
table = freqtable(genres)
sort!(table)

years = [record.year for record in records]
has_drama = ["Drama" in record.genres for record in records]
drama_prop = proptable(years, has_drama; margins=1)

# Code for listing 6.5

using Plots
plot(names(drama_prop, 1), drama_prop[:, 2]; legend=false,
     xlabel="year", ylabel="Drama probability")

# Code for section 6.6.1

s1 = Symbol("x")
s2 = Symbol("hello world!")
s3 = Symbol("x", 1)

typeof(s1)
typeof(s2)
typeof(s3)

Symbol("1")

:x
:x1

:hello world
:1

# Code for section 6.6.2

supertype(Symbol)

:x == :x
:x == :y

# Code for listing 6.6

using BenchmarkTools
str = string.("x", 1:10^6)
symb = Symbol.(str)
@btime "x" in $str;
@btime :x in $symb;

# Code for section 6.7

using InlineStrings
s1 = InlineString("x")
typeof(s1)
s2 = InlineString("∀")
typeof(s2)
sv = inlinestrings(["The", "quick", "brown", "fox", "jumps",
                    "over", "the", "lazy", "dog"])

# Code for listing 6.7

using Random
using BenchmarkTools
Random.seed!(1234);
s1 = [randstring(3) for i in 1:10^6]
s2 = inlinestrings(s1)

# Code for analyzing properties of InlineStrings.jl

Base.summarysize(s1)
Base.summarysize(s2)

@btime sort($s1);
@btime sort($s2);

# Code for listing 6.8

open("iris.txt", "w") do io
    for i in 1:10^6
        println(io, "Iris setosa")
        println(io, "Iris virginica")
        println(io, "Iris versicolor")
    end
end

# Code for section 6.8.2

uncompressed = readlines("iris.txt")

using PooledArrays
compressed = PooledArray(uncompressed)

Base.summarysize(uncompressed)
Base.summarysize(compressed)

# Code for section 6.8.3

compressed.invpool
compressed.pool

compressed[10]
compressed.pool[compressed.refs[10]]

Base.summarysize.(compressed.pool)

v1 = string.("x", 1:10^6)
v2 = PooledArray(v1)
Base.summarysize(v1)
Base.summarysize(v2)

max_stars_repo_name: Mo-Gul/JuliaForDataAnalysis

max_issues_repo_name: Mo-Gul/JuliaForDataAnalysis

max_forks_repo_name: Mo-Gul/JuliaForDataAnalysis


text:
# include("dubinsmaneuver2d.jl")
include("vertical.jl")

"""
    DubinsManeuver3D struct

This struct contains all necessary information about the maneuver.
  * qi - initial configuration (x, y, z, heading, pitch)
  * qf - final configuration (x, y, z, heading, pitch)
  * rhomin - minimum turning radius
  * pitchlims - limits of the pitch angle [pitch_min, pitch_max]
    where pitch_min < 0.0
  * path - array containing horizontal and vertical Dubins paths
  * length - total length of the 3D maneuver
"""
mutable struct DubinsManeuver3D
    qi::Vector{Float64}
    qf::Vector{Float64}

    rhomin::Float64
    pitchlims::Vector{Float64}

    path::Vector{DubinsManeuver2D}
    length::Float64
end

"""
    DubinsManeuver3D(qi, qf, rhomin, pitchlims)

Create 3D Dubins path between two configurations qi, qf
    * qi - initial configuration (x, y, z, heading, pitch)
    * qf - final configuration (x, y, z, heading, pitch)
    * rhomin - minimum turning radius
    * pitchlims - limits of the pitch angle [pitch_min, pitch_max]
"""
function DubinsManeuver3D(qi::Vector{Float64}, qf::Vector{Float64},
        rhomin::Float64, pitchlims::Vector{Float64})
    maneuver = DubinsManeuver3D(qi, qf, rhomin, pitchlims, [], -1.)

    # Delta Z (height)
    zi = maneuver.qi[3]
    zf = maneuver.qf[3]
    dz = zf - zi

    # Multiplication factor of rhomin in [1, 1000]
    a = 1.0
    b = 1.0

    fa = try_to_construct(maneuver, maneuver.rhomin * a)
    fb = try_to_construct(maneuver, maneuver.rhomin * b)

    while length(fb) < 2
        b *= 2.0
        fb = try_to_construct(maneuver, maneuver.rhomin * b)
    end

    if length(fa) > 0
        maneuver.path = fa
    else
        if length(fb) < 2
            error("No maneuver exists")
        end
    end

    # Binary searchs
    # while abs(b-a) > 10e-5:
    #     c = (a+b) / 2.0
    #     #print("Binary search ", [a, b])
    #     fc = self.try_to_construct(self.rhomin * c)

    #     if len(fc) > 0:
    #         b = c
    #         fb = fc
    #     else:
    #         a = c

    # Local optimalization between horizontal and vertical radii
    step = 0.1
    while abs(step) > 1e-10
        c = b + step
        if c < 1.0
            c = 1.0
        end
        fc = try_to_construct(maneuver, maneuver.rhomin * c)
        if length(fc) > 0
            if fc[2].maneuver.length < fb[2].maneuver.length
                b = c
                fb = fc
                step *= 2.
                continue
            end
        end
        step *= -0.1
    end

    maneuver.path = fb
    Dlat, Dlon = fb
    maneuver.length = Dlon.maneuver.length
    return maneuver
end

"""
    get_configuration(self::DubinsManeuver3D, distance::Number)

Finds vehicle configuration at the given distance from start of maneuver
    * self - maneuver on which a configuration is queried
    * distance - distance from the start of maneuver where vehicle configuration is queried
"""
function get_configuration(self::DubinsManeuver3D, distance::Number)
    Dlat, Dlon = self.path
    lena = Dlon.maneuver.length
    qSZ = getCoordinatesAt(Dlon, distance)
    qXY = getCoordinatesAt(Dlat, qSZ[1])
    return [qXY[1], qXY[2], qSZ[2], qXY[3], qSZ[3]]
end

"""
    compute_sampling(self::DubinsManeuver3D; numberOfSamples::Number = 1000)

Sample the maneuver with a given number of samples
    * self - maneuver to be sampled
    * numberOfSamples - number of samples taken along the maneuver
"""
function compute_sampling(self::DubinsManeuver3D; numberOfSamples::Number = 1000)
    Dlat, Dlon = self.path
    lena = Dlon.maneuver.length
    samplingStep = lena/(numberOfSamples-1)

    return sample_path(self, samplingStep=samplingStep, addEndpoint=false)
end

"""
    sample_path(self::DubinsManeuver3D, samplingStep::Number = 0.1; addEndpoint::Bool = true)

Sample the maneuver with a given sampling step
    * self - maneuver to be sampled
    * samplingStep - distance between two samples
    * addEndpoint - wheater the maneuver endpoint should be explicitelly added to the sampling
"""
function sample_path(self::DubinsManeuver3D; samplingStep::Number = 0.1, addEndpoint::Bool = true)
    Dlat, Dlon = self.path
    # Sample points on the final path
    points = []
    lena = Dlon.maneuver.length
    rangeLon = 0.0 : samplingStep : lena

    for ran in rangeLon
        q = get_configuration(self, ran)
        push!(points, q)
    end

    if addEndpoint
        q = get_configuration(self, lena)
        push!(points, q)
    end

    points
end

function try_to_construct(self::DubinsManeuver3D, horizontal_radius::Float64)
    qi2D = self.qi[[1,2,4]]
    qf2D = self.qf[[1,2,4]]

    #@show qi2D, qf2D

    Dlat = DubinsManeuver2D(qi2D, qf2D; rhomin = horizontal_radius)

    # After finding a long enough 2D curve, calculate the Dubins path on SZ axis
    qi3D = [0., self.qi[3], self.qi[5]]
    qf3D = [Dlat.maneuver.length, self.qf[3], self.qf[5]]

    vertical_curvature = sqrt(1. /self.rhomin/self.rhomin - 1. /horizontal_radius/horizontal_radius)
    if vertical_curvature < 1e-5
        return []
    end

    vertical_radius = 1. / vertical_curvature
    # Dlon = Vertical1D(qi3D, qf3D, vertical_radius, self.pitchlims)
    Dlon = DubinsManeuver2D(qi3D, qf3D; rhomin = vertical_radius)

    if Dlon.maneuver.case == "RLR" || Dlon.maneuver.case == "RLR"
        return []
    end

    if Dlon.maneuver.case[1] == 'R'
        if self.qi[5] - Dlon.maneuver.t < self.pitchlims[1]
            return []
        end
    else
        if self.qi[5] + Dlon.maneuver.t > self.pitchlims[2]
            return []
        end
    end

    # Final 3D path is formed by the two curves (Dlat, Dlon)
    return [Dlat, Dlon]
end

function getLowerBound(qi, qf, rhomin=1, pitchlims=[-pi/4, pi/2])
    maneuver = DubinsManeuver3D(qi, qf, rhomin, pitchlims, [], -1.)

    spiral_radius = rhomin * ( (cos(max(-pitchlims[1], pitchlims[2]))) ^ 2 )

    qi2D = [maneuver.qi[i] for i in [1,2,4]]
    qf2D = [maneuver.qf[i] for i in [1,2,4]]
    Dlat = DubinsManeuver2D(qi2D, qf2D; rhomin = spiral_radius)

    qi3D = [0, maneuver.qi[3], maneuver.qi[5]]
    qf3D = [Dlat.maneuver.length, maneuver.qf[3], maneuver.qf[5]]

    Dlon = Vertical(qi3D, qf3D, maneuver.rhomin, maneuver.pitchlims)

    if Dlon.maneuver.case == "XXX"
        # TODO - update Vertical1D such that it compute the shortest prolongation
        maneuver.length = 0.0
        return maneuver
    end

    maneuver.path = [Dlat, Dlon]
    maneuver.length = Dlon.maneuver.length
    return maneuver
end

function getUpperBound(qi, qf, rhomin=1, pitchlims=[-pi/4, pi/2])
    maneuver = DubinsManeuver3D(qi, qf, rhomin, pitchlims, [], -1.)

    safeRadius = sqrt(2) * maneuver.rhomin

    p1 = qi[1:2]
    p2 = qf[1:2]
    diff = p2 - p1
    dist = sqrt(diff[1]^2 + diff[2]^2)
    if dist < 4.0 * safeRadius
        maneuver.length = Inf
        return maneuver
    end

    qi2D = [maneuver.qi[i] for i in [1,2,4]]
    qf2D = [maneuver.qf[i] for i in [1,2,4]]
    Dlat = DubinsManeuver2D(qi2D, qf2D; rhomin = safeRadius)

    qi3D = [0, maneuver.qi[3], maneuver.qi[5]]
    qf3D = [Dlat.maneuver.length, maneuver.qf[3], maneuver.qf[5]]

    Dlon = Vertical(qi3D, qf3D, safeRadius, maneuver.pitchlims)

    if Dlon.maneuver.case == "XXX"
        # TODO - update Vertical1D such that it compute the shortest prolongation
        maneuver.length = Inf
        return maneuver
    end

    maneuver.path = [Dlat, Dlon]
    maneuver.length = Dlon.maneuver.length
    return maneuver
end


text:
"""
    contourf(xs, ys, zs; kwargs...)

Plots a filled contour of the height information in `zs` at horizontal grid positions `xs`
and vertical grid positions `ys`.

The attribute `levels` can be either
- an `Int` that produces n equally wide levels
- an `AbstractVector{<:Real}` that lists consecutive levels
- an `AbstractVector{<:Tuple{Real, Real}}` that lists levels as (low, high) tuples
- a  `Tuple{<:AbstractVector{<:Real},<:AbstractVector{<:Real}}` that lists levels as a tuple of lows and highs

## Attributes
$(ATTRIBUTES)
"""
@recipe(Contourf) do scene
    Theme(
        levels = 10,
        colormap = :viridis,
        colorrange = automatic,
    )
end

function _get_isoband_levels(levels::Int, mi, ma)
    edges = Float32.(LinRange(mi, ma, levels+1))
    (edges[1:end-1], edges[2:end])
end

function _get_isoband_levels(levels::AbstractVector{<:Real}, mi, ma)
    edges = Float32.(levels)
    (edges[1:end-1], edges[2:end])
end

function _get_isoband_levels(levels::AbstractVector{<:Tuple{Real, Real}}, mi, ma)
    (Float32.(first.(levels)), Float32.(last.(levels)))
end

function _get_isoband_levels(levels::Tuple{<:AbstractVector{<:Real},<:AbstractVector{<:Real}}, mi, ma)
    (Float32.(levels[1]), Float32.(levels[2]))
end

function AbstractPlotting.plot!(c::Contourf{<:Tuple{<:AbstractVector{<:Real}, <:AbstractVector{<:Real}, <:AbstractMatrix{<:Real}}})
    xs, ys, zs = c[1:3]

    levels = lift(zs, c.levels) do zs, levels
        _get_isoband_levels(levels, extrema_nan(zs)...)
    end

    PolyType = typeof(Polygon(Point2f0[], [Point2f0[]]))

    polys = Observable(PolyType[])
    colors = Observable(Float32[])

    function calculate_polys(xs, ys, zs, levels)
        empty!(polys[])
        empty!(colors[])
        @assert levels isa Tuple
        lows, highs = levels

        # zs needs to be transposed to match rest of abstractplotting
        isos = Isoband.isobands(xs, ys, zs', lows, highs)

        allvertices = Point2f0[]
        allfaces = NgonFace{3,OffsetInteger{-1,UInt32}}[]
        allids = Int[]
        levelcenters = (highs .+ lows) ./ 2
        for (center, group) in zip(levelcenters, isos)
            points = Point2f0.(group.x, group.y)
            polygroups = _group_polys(points, group.id)
            for polygroup in polygroups
                outline = polygroup[1]
                holes = polygroup[2:end]
                push!(polys[], GeometryBasics.Polygon(outline, holes))
                # use contour level center value as color
                push!(colors[], center)
            end
        end
        polys[] = polys[]
        return
    end

    onany(calculate_polys, xs, ys, zs, levels)
    # onany doesn't get called without a push, so we call
    # it on a first run!
    calculate_polys(xs[], ys[], zs[], levels[])

    mesh!(c,
        polys,
        colormap = c.colormap,
        colorrange = c.colorrange,
        color = colors,
        shading=false)
end

"""
    _group_polys(points, ids)

Given a vector of polygon vertices, and one vector of group indices, which
are assumed to be returned from the isoband algorithm, return
a vector of groups, where each group has one outer polygon and zero or more
inner polygons which are holes in the outer polygon. It is possible that one
group has multiple outer polygons with multiple holes each.
"""
function _group_polys(points, ids)

    polys = [points[ids .== i] for i in unique(ids)]
    npolys = length(polys)

    polys_lastdouble = [push!(p, first(p)) for p in polys]

    # this matrix stores whether poly i is contained in j
    # because the marching squares algorithm won't give us any
    # intersecting or overlapping polys, it should be enough to
    # check if a single point is contained, saving some computation time
    containment_matrix = [
        p1 != p2 &&
        PolygonOps.inpolygon(first(p1), p2) == 1
        for p1 in polys_lastdouble, p2 in polys_lastdouble]

    unclassified_polyindices = collect(1:size(containment_matrix, 1))
    # @show unclassified_polyindices

    # each group has first an outer polygon, and then its holes
    # TODO: don't specifically type this 2f0?
    groups = Vector{Vector{Point2f0}}[]

    # a dict that maps index in `polys` to index in `groups` for outer polys
    outerindex_groupdict = Dict{Int, Int}()

    # all polys have to be classified
    while !isempty(unclassified_polyindices)
        to_keep = ones(Bool, length(unclassified_polyindices))

        # go over unclassifieds and find outer polygons in the remaining containment matrix
        for (ii, i) in enumerate(unclassified_polyindices)
            # an outer polygon is not inside any other polygon of the matrix
            if sum(containment_matrix[ii, :]) == 0
                # an outer polygon
                # println(i, " is an outer polygon")
                push!(groups, [polys_lastdouble[i]])
                outerindex_groupdict[i] = length(groups)
                # delete this poly from further rounds
                to_keep[ii] = false
            end
        end

        # go over unclassifieds and find hole polygons
        for (ii, i) in enumerate(unclassified_polyindices)
            # the hole polygons can only be in one polygon from the current group
            # if they are in more than one, they are "inner outer" or inner hole polys
            # and will be handled in one of the following passes
            if sum(containment_matrix[ii, :]) == 1
                outerpolyindex_of_unclassified = findfirst(containment_matrix[ii, :])
                outerpolyindex = unclassified_polyindices[outerpolyindex_of_unclassified]
                # a hole
                # println(i, " is an inner polygon of ", outerpolyindex)
                groupindex = outerindex_groupdict[outerpolyindex]
                push!(groups[groupindex], polys_lastdouble[i])
                # delete this poly from further rounds
                to_keep[ii] = false
            end
        end

        unclassified_polyindices = unclassified_polyindices[to_keep]
        containment_matrix = containment_matrix[to_keep, to_keep]
    end
    groups
end